From 78e0038c1a255c66af941af89423f6e69be8c55e Mon Sep 17 00:00:00 2001 From: TieJean Date: Wed, 20 Dec 2023 13:13:23 -0600 Subject: [PATCH] debugging interpolator for TUM --- CMakeLists.txt | 3 + .../taijing/tum_freiburg2_desk_gt_format.sh | 3 + .../taijing/tum_freiburg2_desk_obvislam.sh | 6 +- include/base_lib/pose_reps.h | 6 +- .../trajectory_interpolation_utils.h | 4 + .../interpolate_poses_with_required_nodes.cpp | 4 +- .../trajectory_interpolation_utils.cpp | 40 ++++++ src/evaluation/tum/format_tum_gt.cpp | 118 ++++++++++++++++++ 8 files changed, 178 insertions(+), 6 deletions(-) create mode 100644 convenience_scripts/tum/taijing/tum_freiburg2_desk_gt_format.sh create mode 100644 src/evaluation/tum/format_tum_gt.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a06ebd38..33b931ff 100644 --- a/CMakeLists.txt +++ b/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" diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_gt_format.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_gt_format.sh new file mode 100644 index 00000000..a6759ed8 --- /dev/null +++ b/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 diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh index d2c49628..a2ff5fe9 100644 --- a/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh +++ b/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} \ + + diff --git a/include/base_lib/pose_reps.h b/include/base_lib/pose_reps.h index d2127b8e..f00db91b 100644 --- a/include/base_lib/pose_reps.h +++ b/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); diff --git a/include/evaluation/trajectory_interpolation_utils.h b/include/evaluation/trajectory_interpolation_utils.h index dc477d82..75e5b475 100644 --- a/include/evaluation/trajectory_interpolation_utils.h +++ b/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> &odom_poses) ; + void getOdomPoseEsts( const std::string &rosbag_file_name, const std::string &odom_topic_name, diff --git a/src/evaluation/interpolate_poses_with_required_nodes.cpp b/src/evaluation/interpolate_poses_with_required_nodes.cpp index ed89e3e6..d36d0283 100644 --- a/src/evaluation/interpolate_poses_with_required_nodes.cpp +++ b/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> 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 coarse_fixed_poses_raw; readPose3DWithDoubleTimestampFromFile(FLAGS_coarse_trajectory_file, diff --git a/src/evaluation/trajectory_interpolation_utils.cpp b/src/evaluation/trajectory_interpolation_utils.cpp index 8af091dc..24cff765 100644 --- a/src/evaluation/trajectory_interpolation_utils.cpp +++ b/src/evaluation/trajectory_interpolation_utils.cpp @@ -15,6 +15,42 @@ namespace vslam_types_refactor { +void getOdomPoseEstsFromFile( + const std::string &rosbag_file_name, + std::vector> &odom_poses) { + + std::map> 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(Eigen::Vector3d(tx, ty, tz), + Orientation3D(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) { diff --git a/src/evaluation/tum/format_tum_gt.cpp b/src/evaluation/tum/format_tum_gt.cpp new file mode 100644 index 00000000..4d02144c --- /dev/null +++ b/src/evaluation/tum/format_tum_gt.cpp @@ -0,0 +1,118 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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> &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(Eigen::Vector3d(tx, ty, tz), + vtr::Orientation3D(Eigen::Quaterniond(qw, qx, qy, qz))); + } + infile.close(); +} + +void SavePoses(const std::string &filepath, const std::map> ×tamped_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> timestamped_poses; + LoadPoses(FLAGS_gt_filepath_in, timestamped_poses); + LOG(INFO) << "timestamped_poses size: " << timestamped_poses.size(); + + std::vector> poses; + for (const auto ×tamped_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; +} \ No newline at end of file