debugging interpolator for TUM

CMakeLists.txt

@@ -176,6 +176,9 @@ target_link_libraries(write_bounding_boxes_for_rosbag_to_file ut_vslam ${LIBS})
 ROSBUILD_ADD_EXECUTABLE(convert_object_detections_from_yaml src/evaluation/objects/convert_object_detections_from_yaml.cpp)
 target_link_libraries(convert_object_detections_from_yaml ut_vslam ${LIBS} yaml-cpp)
 
+ROSBUILD_ADD_EXECUTABLE(format_tum_gt src/evaluation/tum/format_tum_gt.cpp)
+target_link_libraries(format_tum_gt ut_vslam ${LIBS})
+
 IF (GENERATE_UNITTESTS)
     MESSAGE("Adding test executable")
     SET(UT_VSLAM_UNITTEST_NAME "ut_vslam_tests"

convenience_scripts/tum/taijing/tum_freiburg2_desk_gt_format.sh

@@ -0,0 +1,3 @@
+./bin/format_tum_gt \
+    --gt_filepath_in /home/tiejean/Documents/mnt/oslam/TUM/freiburg2_desk/groundtruth.txt \
+    --gt_filepath_out /home/tiejean/Documents/mnt/oslam/lego_loam_out/freiburg2_desk/poses/lego_loam_poses.csv

convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh

@@ -39,6 +39,7 @@ rosbag_file_directory=${root_data_dir}TUM/
 orb_slam_out_directory=${root_data_dir}orb_out/
 orb_post_process_base_directory=${root_data_dir}orb_post_process/
 results_root_directory=${root_data_dir}ut_vslam_results/
+lego_loam_out_root_dir=${root_data_dir}lego_loam_out/
 bounding_box_post_process_base_directory=$root_data_dir/bounding_box_data/
 
 sequence_file_base_name="tum_fr2_desk"
@@ -57,5 +58,8 @@ python3 src/evaluation/ltm_trajectory_sequence_executor.py \
     --results_root_directory ${results_root_directory} \
     --config_file_base_name ${config_file_base_name} \
     --sequence_file_base_name ${sequence_file_base_name} \
-    --output_bb_assoc --record_viz_rosbag --log_to_file 
+    --output_bb_assoc --record_viz_rosbag --log_to_file --run_rviz 
+    # --lego_loam_out_root_dir ${lego_loam_out_root_dir} \
+
+
 

include/base_lib/pose_reps.h

@@ -125,9 +125,9 @@ inline Pose2d toPose2d(const Pose3d &pose_3d) {
   Eigen::Vector3d euler_angles = toEulerAngles(pose_3d.second);
   double yaw = euler_angles[2];
   if ((abs(euler_angles[0]) > 0.2) || (abs(euler_angles[1]) > 0.2)) {
-    LOG(INFO) << "Roll " << euler_angles[0];
-    LOG(INFO) << "Pitch " << euler_angles[1];
-    LOG(INFO) << "Yaw " << euler_angles[2];
+  //   LOG(INFO) << "Roll " << euler_angles[0];
+  //   LOG(INFO) << "Pitch " << euler_angles[1];
+  //   LOG(INFO) << "Yaw " << euler_angles[2];
   }
   pose::Pose2d pose =
       pose::createPose2d(pose_3d.first.x(), pose_3d.first.y(), yaw);

include/evaluation/trajectory_interpolation_utils.h

@@ -24,6 +24,10 @@ struct RelativePoseFactorInfo {
   pose::Timestamp after_pose_timestamp_;
 };
 
+void getOdomPoseEstsFromFile(
+    const std::string &rosbag_file_name, 
+    std::vector<std::pair<pose::Timestamp, pose::Pose2d>> &odom_poses) ;
+
 void getOdomPoseEsts(
     const std::string &rosbag_file_name,
     const std::string &odom_topic_name,

src/evaluation/interpolate_poses_with_required_nodes.cpp

@@ -183,8 +183,8 @@ int main(int argc, char **argv) {
 
   LOG(INFO) << "Done reading extrinsics";
   std::vector<std::pair<Timestamp, Pose2d>> odom_poses;
-  getOdomPoseEsts(FLAGS_rosbag_file, FLAGS_odometry_topic, odom_poses);
-  LOG(INFO) << "Done reading odometry poses";
+  // getOdomPoseEsts(FLAGS_rosbag_file, FLAGS_odometry_topic, odom_poses);
+  getOdomPoseEstsFromFile(FLAGS_rosbag_file, odom_poses);
 
   std::vector<file_io::Pose3DWithDoubleTimestamp> coarse_fixed_poses_raw;
   readPose3DWithDoubleTimestampFromFile(FLAGS_coarse_trajectory_file,

src/evaluation/trajectory_interpolation_utils.cpp

@@ -15,6 +15,42 @@
 
 namespace vslam_types_refactor {
 
+void getOdomPoseEstsFromFile(
+    const std::string &rosbag_file_name, 
+    std::vector<std::pair<pose::Timestamp, pose::Pose2d>> &odom_poses) {
+
+  std::map<double, Pose3D<double>> poses;
+  std::string filepath = 
+      rosbag_file_name.substr(0, rosbag_file_name.size()-4) + "/groundtruth.txt";
+  std::ifstream infile;
+  infile.open(filepath, std::ios::in);
+  if (!infile.is_open()) {
+    LOG(FATAL) << "Failed to open file " << filepath;
+  }
+  std::string line;
+  double timestamp, tx, ty, tz, qx, qy, qz, qw;
+  while (std::getline(infile, line)) {
+    if (line.at(0) == '#') { continue; }
+    std::stringstream ss(line);
+    ss >> timestamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
+    poses[timestamp] = 
+        Pose3D<double>(Eigen::Vector3d(tx, ty, tz), 
+            Orientation3D<double>(Eigen::Quaterniond(qw, qx, qy, qz)));
+  }
+  infile.close();
+
+  for (const auto &stamped_pose : poses) {
+    uint32_t sec = std::floor(stamped_pose.first);
+    uint32_t nsec = (stamped_pose.first-sec) * 1e9;
+    pose::Timestamp timestamp = std::make_pair(sec, nsec);
+    auto pose_3d = std::make_pair(
+        stamped_pose.second.transl_, 
+        Eigen::Quaterniond(stamped_pose.second.orientation_));
+    auto pose_2d = pose::toPose2d(pose_3d);
+    odom_poses.emplace_back(timestamp, pose_2d);
+  }
+}
+
 void getOdomPoseEsts(
     const std::string &rosbag_file_name,
     const std::string &odom_topic_name,
@@ -459,6 +495,10 @@ void interpolate3dPosesUsingOdom(
                                    required_timestamps,
                                    approx_odom_adjustment,
                                    closest_odom_stamps);
+  LOG(INFO) << "pose_factor_infos size: " << pose_factor_infos.size();
+  for (const auto &factor : pose_factor_infos) {
+    LOG(INFO) << factor.pose_deviation_cov_.transpose();
+  }
 
   LOG(INFO) << "Adjusting odom poses to approximately match lidar frame";
   for (const auto &odom_pose : odom_poses) {

src/evaluation/tum/format_tum_gt.cpp

@@ -0,0 +1,118 @@
+#include <analysis/cumulative_timer_constants.h>
+#include <analysis/cumulative_timer_factory.h>
+#include <base_lib/basic_utils.h>
+#include <base_lib/pose_utils.h>
+#include <debugging/ground_truth_utils.h>
+#include <file_io/bounding_box_by_node_id_io.h>
+#include <file_io/bounding_box_by_timestamp_io.h>
+#include <file_io/camera_extrinsics_with_id_io.h>
+#include <file_io/camera_info_io_utils.h>
+#include <file_io/camera_intrinsics_with_id_io.h>
+#include <file_io/cv_file_storage/config_file_storage_io.h>
+#include <file_io/cv_file_storage/long_term_object_map_file_storage_io.h>
+#include <file_io/cv_file_storage/object_and_reprojection_feature_pose_graph_file_storage_io.h>
+#include <file_io/cv_file_storage/output_problem_data_file_storage_io.h>
+#include <file_io/cv_file_storage/vslam_basic_types_file_storage_io.h>
+#include <file_io/node_id_and_timestamp_io.h>
+#include <file_io/pose_3d_with_timestamp_io.h>
+#include <file_io/pose_io_utils.h>
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <refactoring/bounding_box_frontend/bounding_box_retriever.h>
+#include <refactoring/bounding_box_frontend/feature_based_bounding_box_front_end.h>
+#include <refactoring/configuration/full_ov_slam_config.h>
+#include <refactoring/image_processing/image_processing_utils.h>
+#include <refactoring/long_term_map/long_term_map_factor_creator.h>
+#include <refactoring/offline/offline_problem_data.h>
+#include <refactoring/offline/offline_problem_runner.h>
+#include <refactoring/output_problem_data.h>
+#include <refactoring/output_problem_data_extraction.h>
+#include <refactoring/visual_feature_frontend/visual_feature_front_end.h>
+#include <refactoring/visual_feature_processing/orb_output_low_level_feature_reader.h>
+#include <refactoring/visualization/ros_visualization.h>
+#include <refactoring/visualization/save_to_file_visualizer.h>
+#include <ros/ros.h>
+#include <run_optimization_utils/optimization_runner.h>
+#include <sensor_msgs/Image.h>
+
+namespace vtr = vslam_types_refactor;
+
+DEFINE_string(gt_filepath_in,  "", "input groundtruth filepath (in TUM format)");
+DEFINE_string(gt_filepath_out, "", "output groundtruth filepath (in ObVi-SLAM format)");
+
+void LoadPoses(const std::string &filepath, std::map<double, vtr::Pose3D<double>> &poses) {
+  std::ifstream infile;
+  infile.open(filepath, std::ios::in);
+  if (!infile.is_open()) {
+    LOG(FATAL) << "Failed to open file " << filepath;
+  }
+  std::string line;
+  double timestamp, tx, ty, tz, qx, qy, qz, qw;
+  while (std::getline(infile, line)) {
+    if (line.at(0) == '#') { continue; }
+    std::stringstream ss(line);
+    ss >> timestamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
+    poses[timestamp] = 
+        vtr::Pose3D<double>(Eigen::Vector3d(tx, ty, tz), 
+            vtr::Orientation3D<double>(Eigen::Quaterniond(qw, qx, qy, qz)));
+  }
+  infile.close();
+}
+
+void SavePoses(const std::string &filepath, const std::map<double, vtr::Pose3D<double>> &timestamped_poses) {
+  std::ofstream ofile;
+  ofile.open(filepath, std::ios::out);
+  if (!ofile.is_open()) {
+    LOG(FATAL) << "Failed to open file " << filepath;
+  }
+  const char kDelim = ' ';
+  for (const auto &stamped_pose : timestamped_poses) {
+    Eigen::Quaterniond quat(stamped_pose.second.orientation_);
+    ofile << std::setprecision(20) << stamped_pose.first << kDelim
+          << stamped_pose.second.transl_.x() << kDelim
+          << stamped_pose.second.transl_.y() << kDelim
+          << stamped_pose.second.transl_.z() << kDelim
+          << quat.w() << kDelim
+          << quat.x() << kDelim
+          << quat.y() << kDelim
+          << quat.z() << std::endl;
+  }
+  ofile.close();
+}
+
+int main(int argc, char **argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  FLAGS_logtostderr = true;
+
+  if (FLAGS_gt_filepath_in.empty()) {
+    LOG(FATAL) << "gt_filepath_in cannot be empty!";
+  }
+
+  if (FLAGS_gt_filepath_out.empty()) {
+    LOG(FATAL) << "gt_filepath_out cannot be empty!";
+  }
+
+  std::map<double, vtr::Pose3D<double>> timestamped_poses;  
+  LoadPoses(FLAGS_gt_filepath_in, timestamped_poses);
+  LOG(INFO) << "timestamped_poses size: " << timestamped_poses.size();
+
+  std::vector<vtr::Pose3D<double>> poses;
+  for (const auto &timestamped_pose : timestamped_poses) {
+    poses.emplace_back(timestamped_pose.second);
+  }
+  poses = adjustTrajectoryToStartAtOrigin(poses);
+  if (poses.size() != timestamped_poses.size()) {
+    LOG(FATAL) << "size mismatches: " << poses.size() << " vs. " 
+        << timestamped_poses.size();
+  }
+  size_t poseIdx = 0;
+  for (auto timestamped_pose : timestamped_poses) {
+    timestamped_pose.second = poses.at(poseIdx);
+    ++poseIdx;
+  }
+  LOG(INFO) << "timestamped_poses size: " << timestamped_poses.size();
+  SavePoses(FLAGS_gt_filepath_out, timestamped_poses);
+
+  return 0;
+}