Skip to content

Commit 63856e1

Browse files
ToniRVGitHub Enterprise
ToniRV
authored and
GitHub Enterprise
committed
Merge pull request MIT-SPARK#116 from SPARK/fix/tesse_cam_params
Fix/tesse cam params
2 parents afeb7ec + ddb6ac3 commit 63856e1

7 files changed

Lines changed: 90 additions & 32 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,13 @@ endif()
1616
catkin_simple()
1717

1818
cs_add_library(${PROJECT_NAME}
19+
# Include files are just here for IDEs
20+
include/kimera_vio_ros/KimeraVioRos.h
21+
include/kimera_vio_ros/RosBagDataProvider.h
22+
include/kimera_vio_ros/RosDisplay.h
23+
include/kimera_vio_ros/RosOnlineDataProvider.h
24+
include/kimera_vio_ros/RosPublishers.h
25+
include/kimera_vio_ros/RosVisualizer.h
1926
src/KimeraVioRos.cpp
2027
src/RosDataProviderInterface.cpp
2128
src/RosBagDataProvider.cpp

‎launch/cam_info_yamlizer.launch‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
<launch>
2-
<arg name="dataset_name" default="test"/>
2+
<arg name="dataset_name" default="uHumans2"/>
33

44
<!-- Frame IDs -->
55
<arg name="base_link_frame_id" value="base_link_gt"/>
@@ -19,6 +19,6 @@
1919
<param name="left_cam_info_topic" value="$(arg left_cam_info_topic)"/>
2020
<param name="right_cam_info_topic" value="$(arg right_cam_info_topic)"/>
2121
<param name="output_dir"
22-
value="$(find kimera_vio_ros)/param/$(arg dataset_name)"/>
22+
value="$(find kimera_vio)/params/$(arg dataset_name)"/>
2323
</node>
2424
</launch>

launch/kimera_vio_ros_tesse.launch renamed to launch/kimera_vio_ros_uhumans1.launch

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
<launch>
2-
<arg name="dataset_name" value="Tesse"/>
2+
<arg name="dataset_name" default="uHumans1"/>
33
<arg name="verbosity" default="0"/>
44
<arg name="online" default="true"/>
55
<arg name="use_lcd" default="true"/>
@@ -17,24 +17,24 @@
1717

1818
<!-- Change rosbag path if online argument is false -->
1919
<arg name="rosbag_path"
20-
default="/home/tonirv/Code/ROS/tess_ws/src/TESSE_interface/ROS/tesse_ros_bridge/scripts/2019-08-28-16-57-09.bag"
20+
default=""
2121
unless="$(arg online)"/>
2222

2323
<!-- Frame IDs -->
24-
<arg name="base_link_frame_id" value="base_link_kimera"/>
25-
<arg name="left_cam_frame_id" value="left_cam_kimera"/>
26-
<arg name="right_cam_frame_id" value="right_cam_kimera"/>
24+
<arg name="base_link_frame_id" default="base_link_kimera"/>
25+
<arg name="left_cam_frame_id" default="left_cam_kimera"/>
26+
<arg name="right_cam_frame_id" default="right_cam_kimera"/>
2727

2828
<!-- Subscriber Topics -->
29-
<arg name="left_cam_topic" value="/tesse/left_cam/image_raw"/>
30-
<arg name="left_cam_info_topic" value="/tesse/left_cam/camera_info"/>
31-
<arg name="right_cam_topic" value="/tesse/right_cam/image_raw"/>
32-
<arg name="right_cam_info_topic" value="/tesse/right_cam/camera_info"/>
33-
<arg name="imu_topic" value="/tesse/imu"/>
29+
<arg name="left_cam_topic" default="/tesse/left_cam/image_raw"/>
30+
<arg name="left_cam_info_topic" default="/tesse/left_cam/camera_info"/>
31+
<arg name="right_cam_topic" default="/tesse/right_cam/image_raw"/>
32+
<arg name="right_cam_info_topic" default="/tesse/right_cam/camera_info"/>
33+
<arg name="imu_topic" default="/tesse/imu"/>
3434

3535
<!-- Empty string ("") means no ground-truth available. Used for init if
3636
requested to do ground-truth initialization. -->
37-
<arg name="odometry_ground_truth_topic" value="$(arg gt_topic)"/>
37+
<arg name="odometry_ground_truth_topic" default="$(arg gt_topic)"/>
3838

3939
<!-- Perform stereo dense reconstruction? -->
4040
<arg name="run_stereo_dense" default="false"/>

‎launch/kimera_vio_ros_uhumans2.launch‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
<launch>
2+
<arg name="dataset_name" default="uHumans2"/>
3+
<arg name="verbosity" default="0"/>
4+
<arg name="online" default="true"/>
5+
<arg name="use_lcd" default="true"/>
6+
7+
<param name="use_sim_time" value="true"/>
8+
9+
<arg name="log_output" default="true"/>
10+
<arg name="log_output_path"
11+
default="$(find kimera_vio_ros)/output_logs/$(arg dataset_name)"
12+
if="$(arg log_output)"/>
13+
<arg name="log_gt_data" default="true"/>
14+
<arg name="gt_topic" default="/tesse/odom"/>
15+
16+
<arg name="use_online_cam_params" default="false"/>
17+
18+
<!-- Change rosbag path if online argument is false -->
19+
<arg name="rosbag_path"
20+
default=""
21+
unless="$(arg online)"/>
22+
23+
<!-- Frame IDs -->
24+
<arg name="base_link_frame_id" default="base_link_kimera"/>
25+
<arg name="left_cam_frame_id" default="left_cam_kimera"/>
26+
<arg name="right_cam_frame_id" default="right_cam_kimera"/>
27+
28+
<!-- Subscriber Topics -->
29+
<arg name="left_cam_topic" default="/tesse/left_cam/mono/image_raw"/>
30+
<arg name="left_cam_info_topic" default="/tesse/left_cam/camera_info"/>
31+
<arg name="right_cam_topic" default="/tesse/right_cam/mono/image_raw"/>
32+
<arg name="right_cam_info_topic" default="/tesse/right_cam/camera_info"/>
33+
<arg name="imu_topic" default="/tesse/imu/clean/imu"/>
34+
35+
<!-- Empty string ("") means no ground-truth available. Used for init if
36+
requested to do ground-truth initialization. -->
37+
<arg name="odometry_ground_truth_topic" default="$(arg gt_topic)"/>
38+
39+
<!-- Launch actual pipeline -->
40+
<include file="$(find kimera_vio_ros)/launch/kimera_vio_ros_uhumans1.launch"
41+
pass_all_args="true"/>
42+
</launch>

output_logs/.gitignore renamed to output_logs/uHumans1/.gitignore

File renamed without changes.

output_logs/Tesse/.gitignore renamed to output_logs/uHumans2/.gitignore

File renamed without changes.

‎src/RosBagDataProvider.cpp‎

Lines changed: 28 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -38,15 +38,21 @@ RosbagDataProvider::RosbagDataProvider(const VioParams& vio_params)
3838
k_last_kf_(0u),
3939
k_last_imu_(0u),
4040
k_last_gt_(0u) {
41-
LOG(INFO) << "Starting Kimera-VIO wrapper offline mode.";
42-
4341
CHECK(nh_private_.getParam("rosbag_path", rosbag_path_));
4442
CHECK(nh_private_.getParam("left_cam_rosbag_topic", left_imgs_topic_));
4543
CHECK(nh_private_.getParam("right_cam_rosbag_topic", right_imgs_topic_));
4644
CHECK(nh_private_.getParam("imu_rosbag_topic", imu_topic_));
4745
CHECK(nh_private_.getParam("ground_truth_odometry_rosbag_topic",
4846
gt_odom_topic_));
4947

48+
LOG(INFO) << "Constructing RosbagDataProvider from path: \n"
49+
<< " - Rosbag Path: " << rosbag_path_.c_str() << '\n'
50+
<< "With ROS topics: \n"
51+
<< " - Left cam: " << left_imgs_topic_.c_str() << '\n'
52+
<< " - Right cam: " << right_imgs_topic_.c_str() << '\n'
53+
<< " - IMU: " << imu_topic_.c_str() << '\n'
54+
<< " - GT odom: " << gt_odom_topic_.c_str();
55+
5056
CHECK(!rosbag_path_.empty());
5157
CHECK(!left_imgs_topic_.empty());
5258
CHECK(!right_imgs_topic_.empty());
@@ -71,6 +77,7 @@ RosbagDataProvider::RosbagDataProvider(const VioParams& vio_params)
7177
bool RosbagDataProvider::spin() {
7278
if (k_ == 0) {
7379
// Initialize!
80+
LOG(INFO) << "Initialize Rosbag Data Provider.";
7481
// Parse data from rosbag first thing:
7582
CHECK(parseRosbag(rosbag_path_, &rosbag_data_));
7683

@@ -174,7 +181,7 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
174181
topics.push_back(right_imgs_topic_);
175182
topics.push_back(imu_topic_);
176183
if (!gt_odom_topic_.empty()) {
177-
CHECK(vio_params_.backend_params_->autoInitialize_ == 0)
184+
LOG_IF(WARNING, vio_params_.backend_params_->autoInitialize_ == 0)
178185
<< "Provided a gt_odom_topic; but autoInitialize "
179186
"(BackendParameters.yaml) is not set to 0,"
180187
" meaning no ground-truth initialization will be done... "
@@ -191,6 +198,7 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
191198
// Query rosbag for given topics
192199
rosbag::View view(bag, rosbag::TopicQuery(topics));
193200

201+
int imu_msg_count = 0;
194202
// Keep track of this since we expect IMU data before an image.
195203
bool start_parsing_stereo = false;
196204
// For some datasets, we have duplicated measurements for the same time.
@@ -213,6 +221,7 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
213221
// Send IMU data directly to VIO at parse level for speed boost:
214222
CHECK(imu_single_callback_)
215223
<< "Did you forget to register the IMU callback?";
224+
VLOG(10) << "IMU msg count: " << imu_msg_count++;
216225
imu_single_callback_(ImuMeasurement(imu_data_timestamp, imu_accgyr));
217226

218227
rosbag_data->imu_msgs_.push_back(imu_msg);
@@ -269,22 +278,22 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
269278
bag.close();
270279

271280
// Sanity check:
272-
ROS_ERROR_COND(rosbag_data->left_imgs_.size() == 0 ||
273-
rosbag_data->right_imgs_.size() == 0,
274-
"No images parsed from rosbag.");
275-
ROS_ERROR_COND(
276-
rosbag_data->left_imgs_.size() != rosbag_data->right_imgs_.size(),
277-
"Unequal number of images from left and right cmaeras.");
278-
ROS_ERROR_COND(
279-
rosbag_data->imu_msgs_.size() <= rosbag_data->left_imgs_.size(),
280-
"Less than or equal number of imu data as image data.");
281-
ROS_ERROR_COND(
282-
!gt_odom_topic_.empty() && rosbag_data->gt_odometry_.size() == 0,
283-
"Requested to parse ground-truth odometry, but parsed 0 msgs.");
284-
ROS_ERROR_COND(
285-
!gt_odom_topic_.empty() &&
286-
rosbag_data->gt_odometry_.size() < rosbag_data->left_imgs_.size(),
287-
"Fewer ground_truth data than image data.");
281+
LOG_IF(FATAL,
282+
rosbag_data->left_imgs_.size() == 0 ||
283+
rosbag_data->right_imgs_.size() == 0)
284+
<< "No images parsed from rosbag.";
285+
LOG_IF(FATAL,
286+
rosbag_data->left_imgs_.size() != rosbag_data->right_imgs_.size())
287+
<< "Unequal number of images from left and right cameras.";
288+
LOG_IF(FATAL, rosbag_data->imu_msgs_.size() <= rosbag_data->left_imgs_.size())
289+
<< "Less than or equal number of imu data as image data.";
290+
LOG_IF(FATAL,
291+
!gt_odom_topic_.empty() && rosbag_data->gt_odometry_.size() == 0)
292+
<< "Requested to parse ground-truth odometry, but parsed 0 msgs.";
293+
LOG_IF(WARNING,
294+
!gt_odom_topic_.empty() &&
295+
rosbag_data->gt_odometry_.size() < rosbag_data->left_imgs_.size())
296+
<< "Fewer ground_truth data than image data.";
288297
LOG(INFO) << "Finished parsing rosbag data.";
289298
return true;
290299
}

0 commit comments

Comments
 (0)