diff --git a/CMakeLists.txt b/CMakeLists.txt
index 295bcb6..f18f080 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,13 @@ endif()
catkin_simple()
cs_add_library(${PROJECT_NAME}
+ # Include files are just here for IDEs
+ include/kimera_vio_ros/KimeraVioRos.h
+ include/kimera_vio_ros/RosBagDataProvider.h
+ include/kimera_vio_ros/RosDisplay.h
+ include/kimera_vio_ros/RosOnlineDataProvider.h
+ include/kimera_vio_ros/RosPublishers.h
+ include/kimera_vio_ros/RosVisualizer.h
src/KimeraVioRos.cpp
src/RosDataProviderInterface.cpp
src/RosBagDataProvider.cpp
diff --git a/launch/cam_info_yamlizer.launch b/launch/cam_info_yamlizer.launch
index 4336cb2..2de2e4b 100644
--- a/launch/cam_info_yamlizer.launch
+++ b/launch/cam_info_yamlizer.launch
@@ -1,5 +1,5 @@
-
+
@@ -19,6 +19,6 @@
+ value="$(find kimera_vio)/params/$(arg dataset_name)"/>
diff --git a/launch/kimera_vio_ros_tesse.launch b/launch/kimera_vio_ros_uhumans1.launch
similarity index 70%
rename from launch/kimera_vio_ros_tesse.launch
rename to launch/kimera_vio_ros_uhumans1.launch
index a376142..77f970a 100644
--- a/launch/kimera_vio_ros_tesse.launch
+++ b/launch/kimera_vio_ros_uhumans1.launch
@@ -1,5 +1,5 @@
-
+
@@ -17,24 +17,24 @@
-
-
-
+
+
+
-
-
-
-
-
+
+
+
+
+
-
+
diff --git a/launch/kimera_vio_ros_uhumans2.launch b/launch/kimera_vio_ros_uhumans2.launch
new file mode 100644
index 0000000..2e30860
--- /dev/null
+++ b/launch/kimera_vio_ros_uhumans2.launch
@@ -0,0 +1,42 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/output_logs/.gitignore b/output_logs/uHumans1/.gitignore
similarity index 100%
rename from output_logs/.gitignore
rename to output_logs/uHumans1/.gitignore
diff --git a/output_logs/Tesse/.gitignore b/output_logs/uHumans2/.gitignore
similarity index 100%
rename from output_logs/Tesse/.gitignore
rename to output_logs/uHumans2/.gitignore
diff --git a/src/RosBagDataProvider.cpp b/src/RosBagDataProvider.cpp
index d7a7bff..3a3429f 100644
--- a/src/RosBagDataProvider.cpp
+++ b/src/RosBagDataProvider.cpp
@@ -38,8 +38,6 @@ RosbagDataProvider::RosbagDataProvider(const VioParams& vio_params)
k_last_kf_(0u),
k_last_imu_(0u),
k_last_gt_(0u) {
- LOG(INFO) << "Starting Kimera-VIO wrapper offline mode.";
-
CHECK(nh_private_.getParam("rosbag_path", rosbag_path_));
CHECK(nh_private_.getParam("left_cam_rosbag_topic", left_imgs_topic_));
CHECK(nh_private_.getParam("right_cam_rosbag_topic", right_imgs_topic_));
@@ -47,6 +45,14 @@ RosbagDataProvider::RosbagDataProvider(const VioParams& vio_params)
CHECK(nh_private_.getParam("ground_truth_odometry_rosbag_topic",
gt_odom_topic_));
+ LOG(INFO) << "Constructing RosbagDataProvider from path: \n"
+ << " - Rosbag Path: " << rosbag_path_.c_str() << '\n'
+ << "With ROS topics: \n"
+ << " - Left cam: " << left_imgs_topic_.c_str() << '\n'
+ << " - Right cam: " << right_imgs_topic_.c_str() << '\n'
+ << " - IMU: " << imu_topic_.c_str() << '\n'
+ << " - GT odom: " << gt_odom_topic_.c_str();
+
CHECK(!rosbag_path_.empty());
CHECK(!left_imgs_topic_.empty());
CHECK(!right_imgs_topic_.empty());
@@ -71,6 +77,7 @@ RosbagDataProvider::RosbagDataProvider(const VioParams& vio_params)
bool RosbagDataProvider::spin() {
if (k_ == 0) {
// Initialize!
+ LOG(INFO) << "Initialize Rosbag Data Provider.";
// Parse data from rosbag first thing:
CHECK(parseRosbag(rosbag_path_, &rosbag_data_));
@@ -174,7 +181,7 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
topics.push_back(right_imgs_topic_);
topics.push_back(imu_topic_);
if (!gt_odom_topic_.empty()) {
- CHECK(vio_params_.backend_params_->autoInitialize_ == 0)
+ LOG_IF(WARNING, vio_params_.backend_params_->autoInitialize_ == 0)
<< "Provided a gt_odom_topic; but autoInitialize "
"(BackendParameters.yaml) is not set to 0,"
" meaning no ground-truth initialization will be done... "
@@ -191,6 +198,7 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
// Query rosbag for given topics
rosbag::View view(bag, rosbag::TopicQuery(topics));
+ int imu_msg_count = 0;
// Keep track of this since we expect IMU data before an image.
bool start_parsing_stereo = false;
// For some datasets, we have duplicated measurements for the same time.
@@ -213,6 +221,7 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
// Send IMU data directly to VIO at parse level for speed boost:
CHECK(imu_single_callback_)
<< "Did you forget to register the IMU callback?";
+ VLOG(10) << "IMU msg count: " << imu_msg_count++;
imu_single_callback_(ImuMeasurement(imu_data_timestamp, imu_accgyr));
rosbag_data->imu_msgs_.push_back(imu_msg);
@@ -269,22 +278,22 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
bag.close();
// Sanity check:
- ROS_ERROR_COND(rosbag_data->left_imgs_.size() == 0 ||
- rosbag_data->right_imgs_.size() == 0,
- "No images parsed from rosbag.");
- ROS_ERROR_COND(
- rosbag_data->left_imgs_.size() != rosbag_data->right_imgs_.size(),
- "Unequal number of images from left and right cmaeras.");
- ROS_ERROR_COND(
- rosbag_data->imu_msgs_.size() <= rosbag_data->left_imgs_.size(),
- "Less than or equal number of imu data as image data.");
- ROS_ERROR_COND(
- !gt_odom_topic_.empty() && rosbag_data->gt_odometry_.size() == 0,
- "Requested to parse ground-truth odometry, but parsed 0 msgs.");
- ROS_ERROR_COND(
- !gt_odom_topic_.empty() &&
- rosbag_data->gt_odometry_.size() < rosbag_data->left_imgs_.size(),
- "Fewer ground_truth data than image data.");
+ LOG_IF(FATAL,
+ rosbag_data->left_imgs_.size() == 0 ||
+ rosbag_data->right_imgs_.size() == 0)
+ << "No images parsed from rosbag.";
+ LOG_IF(FATAL,
+ rosbag_data->left_imgs_.size() != rosbag_data->right_imgs_.size())
+ << "Unequal number of images from left and right cameras.";
+ LOG_IF(FATAL, rosbag_data->imu_msgs_.size() <= rosbag_data->left_imgs_.size())
+ << "Less than or equal number of imu data as image data.";
+ LOG_IF(FATAL,
+ !gt_odom_topic_.empty() && rosbag_data->gt_odometry_.size() == 0)
+ << "Requested to parse ground-truth odometry, but parsed 0 msgs.";
+ LOG_IF(WARNING,
+ !gt_odom_topic_.empty() &&
+ rosbag_data->gt_odometry_.size() < rosbag_data->left_imgs_.size())
+ << "Fewer ground_truth data than image data.";
LOG(INFO) << "Finished parsing rosbag data.";
return true;
}