Skip to content

Commit

Permalink
Merge pull request MIT-SPARK#116 from SPARK/fix/tesse_cam_params
Browse files Browse the repository at this point in the history 
Fix/tesse cam params
  • Loading branch information
@ToniRV
ToniRV authored and GitHub Enterprise committed Sep 1, 2020
2 parents afeb7ec + ddb6ac3 commit 63856e1
Show file tree
Hide file tree
Showing 7 changed files with 90 additions and 32 deletions.
7 changes: 7 additions & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions launch/cam_info_yamlizer.launch
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<launch>
<arg name="dataset_name" default="test"/>
<arg name="dataset_name" default="uHumans2"/>

<!-- Frame IDs -->
<arg name="base_link_frame_id" value="base_link_gt"/>
Expand All @@ -19,6 +19,6 @@
<param name="left_cam_info_topic" value="$(arg left_cam_info_topic)"/>
<param name="right_cam_info_topic" value="$(arg right_cam_info_topic)"/>
<param name="output_dir"
value="$(find kimera_vio_ros)/param/$(arg dataset_name)"/>
value="$(find kimera_vio)/params/$(arg dataset_name)"/>
</node>
</launch>
22 changes: 11 additions & 11 deletions launch/kimera_vio_ros_tesse.launch → launch/kimera_vio_ros_uhumans1.launch
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<launch>
<arg name="dataset_name" value="Tesse"/>
<arg name="dataset_name" default="uHumans1"/>
<arg name="verbosity" default="0"/>
<arg name="online" default="true"/>
<arg name="use_lcd" default="true"/>
Expand All @@ -17,24 +17,24 @@

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

<!-- Frame IDs -->
<arg name="base_link_frame_id" value="base_link_kimera"/>
<arg name="left_cam_frame_id" value="left_cam_kimera"/>
<arg name="right_cam_frame_id" value="right_cam_kimera"/>
<arg name="base_link_frame_id" default="base_link_kimera"/>
<arg name="left_cam_frame_id" default="left_cam_kimera"/>
<arg name="right_cam_frame_id" default="right_cam_kimera"/>

<!-- Subscriber Topics -->
<arg name="left_cam_topic" value="/tesse/left_cam/image_raw"/>
<arg name="left_cam_info_topic" value="/tesse/left_cam/camera_info"/>
<arg name="right_cam_topic" value="/tesse/right_cam/image_raw"/>
<arg name="right_cam_info_topic" value="/tesse/right_cam/camera_info"/>
<arg name="imu_topic" value="/tesse/imu"/>
<arg name="left_cam_topic" default="/tesse/left_cam/image_raw"/>
<arg name="left_cam_info_topic" default="/tesse/left_cam/camera_info"/>
<arg name="right_cam_topic" default="/tesse/right_cam/image_raw"/>
<arg name="right_cam_info_topic" default="/tesse/right_cam/camera_info"/>
<arg name="imu_topic" default="/tesse/imu"/>

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

<!-- Perform stereo dense reconstruction? -->
<arg name="run_stereo_dense" default="false"/>
Expand Down
42 changes: 42 additions & 0 deletions launch/kimera_vio_ros_uhumans2.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
<launch>
<arg name="dataset_name" default="uHumans2"/>
<arg name="verbosity" default="0"/>
<arg name="online" default="true"/>
<arg name="use_lcd" default="true"/>

<param name="use_sim_time" value="true"/>

<arg name="log_output" default="true"/>
<arg name="log_output_path"
default="$(find kimera_vio_ros)/output_logs/$(arg dataset_name)"
if="$(arg log_output)"/>
<arg name="log_gt_data" default="true"/>
<arg name="gt_topic" default="/tesse/odom"/>

<arg name="use_online_cam_params" default="false"/>

<!-- Change rosbag path if online argument is false -->
<arg name="rosbag_path"
default=""
unless="$(arg online)"/>

<!-- Frame IDs -->
<arg name="base_link_frame_id" default="base_link_kimera"/>
<arg name="left_cam_frame_id" default="left_cam_kimera"/>
<arg name="right_cam_frame_id" default="right_cam_kimera"/>

<!-- Subscriber Topics -->
<arg name="left_cam_topic" default="/tesse/left_cam/mono/image_raw"/>
<arg name="left_cam_info_topic" default="/tesse/left_cam/camera_info"/>
<arg name="right_cam_topic" default="/tesse/right_cam/mono/image_raw"/>
<arg name="right_cam_info_topic" default="/tesse/right_cam/camera_info"/>
<arg name="imu_topic" default="/tesse/imu/clean/imu"/>

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

<!-- Launch actual pipeline -->
<include file="$(find kimera_vio_ros)/launch/kimera_vio_ros_uhumans1.launch"
pass_all_args="true"/>
</launch>
0 output_logs/.gitignore → output_logs/uHumans1/.gitignore
View file
File renamed without changes.
0 output_logs/Tesse/.gitignore → output_logs/uHumans2/.gitignore
View file
File renamed without changes.
47 changes: 28 additions & 19 deletions src/RosBagDataProvider.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,15 +38,21 @@ 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_));
CHECK(nh_private_.getParam("imu_rosbag_topic", imu_topic_));
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());
Expand All @@ -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_));

Expand Down Expand Up @@ -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... "
Expand All @@ -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.
Expand All @@ -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);
Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit 63856e1

Please sign in to comment.