need to clean up

ut-amrl · Dec 27, 2023 · 9411a19 · 9411a19
1 parent 9da73eb
commit 9411a19
Show file tree

Hide file tree

Showing 15 changed files with 1,150 additions and 106 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -182,6 +182,9 @@ target_link_libraries(format_tum_gt ut_vslam ${LIBS})
 ROSBUILD_ADD_EXECUTABLE(get_visualized_gt_trajectory src/evaluation/videos/get_visualized_gt_trajectory.cpp)
 target_link_libraries(get_visualized_gt_trajectory ut_vslam ${LIBS})
 
+ROSBUILD_ADD_EXECUTABLE(gt_transformer src/evaluation/videos/gt_transformer.cpp)
+target_link_libraries(gt_transformer ut_vslam ${LIBS})
+
 
 IF (GENERATE_UNITTESTS)
     MESSAGE("Adding test executable")

diff --git a/config/tum_fr2_desk_obj_fast.json b/config/tum_fr2_desk_obj_fast.json
diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_gt_format.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_gt_format.sh
@@ -1,10 +1,14 @@
+configname=tum_fr2_desk_obj_fast
+
 ./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 \
-    --time_filepath /home/tiejean/Documents/mnt/oslam/orb_post_process/sparsified_ut_vslam_in/tum_fr2_desk/freiburg2_desk/timestamps/timestamps_only.txt \
-    --gt_postprocessed_filepath_out /home/tiejean/Documents/mnt/oslam/ut_vslam_results/tum_fr2_desk/tum_fr2_desk/0_freiburg2_desk/postprocessing/interpolated_lego_loam_poses.csv 
+    --time_filepath /home/tiejean/Documents/mnt/oslam/orb_post_process/sparsified_ut_vslam_in/$configname/freiburg2_desk/timestamps/timestamps_only.txt \
+    --gt_postprocessed_filepath_out /home/tiejean/Documents/mnt/oslam/ut_vslam_results/tum_fr2_desk/$configname/0_freiburg2_desk/postprocessing/interpolated_lego_loam_poses.csv 
 
 # python src/evaluation/tum/interpolate_tum_gt.py \
 #     --inpath /home/tiejean/Documents/mnt/oslam/lego_loam_out/freiburg2_desk/poses/lego_loam_poses.csv \
 #     --tpath /home/tiejean/Documents/mnt/oslam/orb_post_process/sparsified_ut_vslam_in/tum_fr2_desk/freiburg2_desk/timestamps/timestamps_only.txt \
-#     --outpath /home/tiejean/Documents/mnt/oslam/ut_vslam_results/tum_fr2_desk/tum_fr2_desk/0_freiburg2_desk/postprocessing/interpolated_lego_loam_poses.csv
+#     --outpath /home/tiejean/Documents/mnt/oslam/ut_vslam_results/tum_fr2_desk/tum_fr2_desk/0_freiburg2_desk/postprocessing/interpolated_lego_loam_poses.csv
+
+python src/evaluation/tum/debug_transform.py 
diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh
@@ -46,7 +46,7 @@ sequence_file_base_name="tum_fr2_desk"
 
 cd $SLAM_DIR
 
-config_file_base_name="tum_fr2_desk"
+config_file_base_name="tum_fr2_desk_obj_fast"
 python3 src/evaluation/ltm_trajectory_sequence_executor.py \
     --bounding_box_post_process_base_directory ${bounding_box_post_process_base_directory} \
     --config_file_directory ${config_file_directory} \

diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_preprocess.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_preprocess.sh
@@ -4,9 +4,9 @@ ORB_OUT=$DARADIR/orb_out
 ORB_POST_PROCESS=$DARADIR/orb_post_process
 
 bagname=freiburg2_desk
-configname=base7a_2_fallback_a_2
+configname=tum_fr2_desk_obj_fast
 
-configfile=$SLAMDIR/config/${base7a_2_fallback_a_2}.json
+configfile=$SLAMDIR/config/${configname}.json
 calibration_dir=$DARADIR/calibration_tum/
 orb_out_dir=$ORB_OUT/$bagname/
 unsparsified_orb_out=$ORB_POST_PROCESS/unsparsified_ut_vslam_in/$bagname/

diff --git a/include/refactoring/optimization/object_pose_graph_optimizer.h b/include/refactoring/optimization/object_pose_graph_optimizer.h
@@ -661,7 +661,7 @@ class ObjectPoseGraphOptimizer {
       options.update_state_every_iteration = true;
     }
     options.max_num_iterations = solver_params.max_num_iterations_;
-    options.num_threads = 20;
+    options.num_threads = 8;
     options.linear_solver_type = ceres::SPARSE_SCHUR;
     options.use_nonmonotonic_steps = solver_params.allow_non_monotonic_steps_;
     options.function_tolerance = solver_params.function_tolerance_;

diff --git a/launch/ovslam.rviz b/launch/ovslam.rviz
@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 78
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -11,11 +11,9 @@ Panels:
         - /EstBbs11
         - /EstBbs21
         - /AllBbs21
-        - /Marker1
-        - /Marker2
         - /GtPose1
       Splitter Ratio: 0.5
-    Tree Height: 151
+    Tree Height: 176
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -31,10 +29,9 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: EstFeats2
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -62,6 +59,8 @@ Visualization Manager:
       Value: true
     - Class: rviz/TF
       Enabled: false
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: true
@@ -100,7 +99,8 @@ Visualization Manager:
         InitFeats2: true
         InitFeatureCloud: true
         InitPose: true
-        Marker: true
+        LongTermMap: true
+        PendingEllipsoids: true
         TF: true
         Value: true
       Zoom Factor: 1
@@ -129,7 +129,8 @@ Visualization Manager:
         InitFeats1: true
         InitFeatureCloud: true
         InitPose: true
-        Marker: true
+        LongTermMap: true
+        PendingEllipsoids: true
         TF: true
         Value: true
       Zoom Factor: 1
@@ -166,7 +167,8 @@ Visualization Manager:
         InitFeats2: true
         InitFeatureCloud: true
         InitPose: true
-        Marker: true
+        LongTermMap: true
+        PendingEllipsoids: true
         TF: true
         Value: true
       Zoom Factor: 1
@@ -195,7 +197,8 @@ Visualization Manager:
         InitFeats2: true
         InitFeatureCloud: true
         InitPose: true
-        Marker: true
+        LongTermMap: true
+        PendingEllipsoids: true
         TF: true
         Value: true
       Zoom Factor: 1
@@ -260,17 +263,17 @@ Visualization Manager:
       Marker Topic: /est_pose
       Name: EstPose
       Namespaces:
-        {}
+        ut_vslam: true
       Queue Size: 1000
       Value: true
     - Class: rviz/Marker
-      Enabled: true
+      Enabled: false
       Marker Topic: /init_pose
       Name: InitPose
       Namespaces:
         {}
       Queue Size: 1000
-      Value: true
+      Value: false
     - Class: rviz/Camera
       Enabled: true
       Image Rendering: background and overlay
@@ -296,7 +299,8 @@ Visualization Manager:
         InitFeats2: true
         InitFeatureCloud: true
         InitPose: true
-        Marker: true
+        LongTermMap: true
+        PendingEllipsoids: true
         TF: true
         Value: true
       Zoom Factor: 1
@@ -325,7 +329,8 @@ Visualization Manager:
         InitFeats2: true
         InitFeatureCloud: true
         InitPose: true
-        Marker: true
+        LongTermMap: true
+        PendingEllipsoids: true
         TF: true
         Value: true
       Zoom Factor: 1
@@ -354,7 +359,8 @@ Visualization Manager:
         InitFeats2: true
         InitFeatureCloud: true
         InitPose: true
-        Marker: true
+        LongTermMap: true
+        PendingEllipsoids: true
         TF: true
         Value: true
       Zoom Factor: 1
@@ -383,7 +389,8 @@ Visualization Manager:
         InitFeats2: true
         InitFeatureCloud: true
         InitPose: true
-        Marker: true
+        LongTermMap: true
+        PendingEllipsoids: true
         TF: true
         Value: true
       Zoom Factor: 1
@@ -408,7 +415,7 @@ Visualization Manager:
       Marker Topic: /gt_pose
       Name: GtPose
       Namespaces:
-        {}
+        ut_vslam: true
       Queue Size: 1000
       Value: true
   Enabled: true
@@ -439,25 +446,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 21.59466552734375
+      Distance: 4.657345771789551
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -4.303472995758057
-        Y: 4.018791198730469
-        Z: 0.7308717966079712
+        X: -0.6345020532608032
+        Y: 0.44121313095092773
+        Z: 3.377772331237793
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.23479849100112915
+      Pitch: 0.8947979211807251
       Target Frame: est_slam_base_link
-      Yaw: 0.2741045653820038
+      Yaw: 2.9341049194335938
     Saved: ~
 Window Geometry:
   AllBbs1:
@@ -474,14 +481,14 @@ Window Geometry:
     collapsed: false
   EstFeats2:
     collapsed: false
-  Height: 1043
+  Height: 983
   Hide Left Dock: false
   Hide Right Dock: false
   InitFeats1:
     collapsed: false
   InitFeats2:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:

diff --git a/src/evaluation/oa_slam/oa_slam_data_generator.cpp b/src/evaluation/oa_slam/oa_slam_data_generator.cpp
@@ -128,7 +128,7 @@ int main(int argc, char **argv) {
   }
   std::sort(ordered_frame_ids.begin(), ordered_frame_ids.end());
 
-  vtr::YoloBoundingBoxQuerier bb_querier(node_handle);
+  // vtr::YoloBoundingBoxQuerier bb_querier(node_handle);
   for (const auto frame_id : ordered_frame_ids) {
     const auto &cam_ids_and_images = images[frame_id];
     const auto &node_and_timestamp = nodes_by_timestamps_vec.at(frame_id);
@@ -152,43 +152,43 @@ int main(int argc, char **argv) {
           << rel_image_path.string() << std::endl;
     }
 
-    std::unordered_map<vtr::CameraId, std::vector<vtr::RawBoundingBox>>
-        cam_ids_and_bboxes;
-    bb_querier.retrieveBoundingBoxesFromCamIdsAndImages(
-        frame_id, cam_ids_and_images, cam_ids_and_bboxes);
-    for (const auto &cam_id_and_bboxes : cam_ids_and_bboxes) {
-      fs::path rel_image_path =
-          fs::path(std::to_string(cam_id_and_bboxes.first)) /
-          (std::to_string(frame_id) + ".png");
-      std::ofstream &ofile =
-          cam_ids_and_detection_ofiles.at(cam_id_and_bboxes.first);
-      if (node_and_timestamp.node_id_ != frame_id) {
-        LOG(ERROR) << "Something went wrong with node id and frame id! "
-                   << "node id = " << node_and_timestamp.node_id_
-                   << "; frame id = " << frame_id;
-        exit(1);
-      }
-      ofile << "file_name" << std::endl;
-      // ofile << node_and_timestamp.seconds_ << "_" <<
-      // node_and_timestamp.nano_seconds_ << ".png" << std::endl; ofile <<
-      // std::to_string(frame_id) << ".png" << std::endl;
-      ofile << rel_image_path.string() << std::endl;
-      ofile << "detections" << std::endl;
-      for (const auto &bbox : cam_id_and_bboxes.second) {
-        ofile << "category_id" << std::endl;
-        ofile << class_names_and_class_ids.at(bbox.semantic_class_)
-              << std::endl;
-        ofile << "detection_score" << std::endl;
-        ofile << bbox.detection_confidence_ << std::endl;
-        ofile << "bbox" << std::endl;
-        // min_x, min_y, max_x, max_y
-        ofile << bbox.pixel_corner_locations_.first.x() << ","
-              << bbox.pixel_corner_locations_.first.y() << ","
-              << bbox.pixel_corner_locations_.second.x() << ","
-              << bbox.pixel_corner_locations_.second.y() << std::endl;
-      }
-      ofile << std::endl;
-    }
+    // std::unordered_map<vtr::CameraId, std::vector<vtr::RawBoundingBox>>
+    //     cam_ids_and_bboxes;
+    // bb_querier.retrieveBoundingBoxesFromCamIdsAndImages(
+    //     frame_id, cam_ids_and_images, cam_ids_and_bboxes);
+    // for (const auto &cam_id_and_bboxes : cam_ids_and_bboxes) {
+    //   fs::path rel_image_path =
+    //       fs::path(std::to_string(cam_id_and_bboxes.first)) /
+    //       (std::to_string(frame_id) + ".png");
+    //   std::ofstream &ofile =
+    //       cam_ids_and_detection_ofiles.at(cam_id_and_bboxes.first);
+    //   if (node_and_timestamp.node_id_ != frame_id) {
+    //     LOG(ERROR) << "Something went wrong with node id and frame id! "
+    //                << "node id = " << node_and_timestamp.node_id_
+    //                << "; frame id = " << frame_id;
+    //     exit(1);
+    //   }
+    //   ofile << "file_name" << std::endl;
+    //   // ofile << node_and_timestamp.seconds_ << "_" <<
+    //   // node_and_timestamp.nano_seconds_ << ".png" << std::endl; ofile <<
+    //   // std::to_string(frame_id) << ".png" << std::endl;
+    //   ofile << rel_image_path.string() << std::endl;
+    //   ofile << "detections" << std::endl;
+    //   for (const auto &bbox : cam_id_and_bboxes.second) {
+    //     ofile << "category_id" << std::endl;
+    //     ofile << class_names_and_class_ids.at(bbox.semantic_class_)
+    //           << std::endl;
+    //     ofile << "detection_score" << std::endl;
+    //     ofile << bbox.detection_confidence_ << std::endl;
+    //     ofile << "bbox" << std::endl;
+    //     // min_x, min_y, max_x, max_y
+    //     ofile << bbox.pixel_corner_locations_.first.x() << ","
+    //           << bbox.pixel_corner_locations_.first.y() << ","
+    //           << bbox.pixel_corner_locations_.second.x() << ","
+    //           << bbox.pixel_corner_locations_.second.y() << std::endl;
+    //   }
+    //   ofile << std::endl;
+    // }
   }
 
   for (auto &cam_id_and_ofile : cam_ids_and_ofiles) {

diff --git a/src/evaluation/tum/debug.py → src/evaluation/tum/debug_transform.py b/src/evaluation/tum/debug.py → src/evaluation/tum/debug_transform.py