From 9411a19d9fd2219ef94356ad7f93ee64e9d35962 Mon Sep 17 00:00:00 2001 From: TieJean Date: Wed, 27 Dec 2023 10:24:32 -0600 Subject: [PATCH] need to clean up --- CMakeLists.txt | 3 + config/tum_fr2_desk_obj_fast.json | 683 ++++++++++++++++++ .../taijing/tum_freiburg2_desk_gt_format.sh | 10 +- .../taijing/tum_freiburg2_desk_obvislam.sh | 2 +- .../taijing/tum_freiburg2_desk_preprocess.sh | 4 +- .../object_pose_graph_optimizer.h | 2 +- launch/ovslam.rviz | 59 +- .../oa_slam/oa_slam_data_generator.cpp | 76 +- .../tum/{debug.py => debug_transform.py} | 0 src/evaluation/tum/plot_trajectory.py | 75 ++ src/evaluation/videos/gt_transformer.cpp | 265 +++++++ src/evaluation/videos/postprocess_markers.py | 56 +- src/evaluation/videos/publish_map.py | 14 +- .../offline_object_visual_slam_main.cpp | 5 +- src/shared | 2 +- 15 files changed, 1150 insertions(+), 106 deletions(-) create mode 100644 config/tum_fr2_desk_obj_fast.json rename src/evaluation/tum/{debug.py => debug_transform.py} (100%) create mode 100644 src/evaluation/tum/plot_trajectory.py create mode 100644 src/evaluation/videos/gt_transformer.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9dee794e..e896118e 100644 --- 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 new file mode 100644 index 00000000..24e88193 --- /dev/null +++ b/config/tum_fr2_desk_obj_fast.json @@ -0,0 +1,683 @@ +{ + "config": { + "config_schema_version": 13, + "config_version_id": "base7a_1_fallback_a_2_tum", + "visual_feature_params": { + "reprojection_error_std_dev": 50, + "min_visual_feature_parallax_pixel_requirement": 5.0, + "min_visual_feature_parallax_robot_transl_requirement": 1.0000000000000001e-01, + "min_visual_feature_parallax_robot_orient_requirement": 5.0000000000000003e-02, + "enforce_min_pixel_parallax_requirement": 1, + "enforce_min_robot_pose_parallax_requirement": 1, + "inlier_epipolar_err_thresh": 5.0, + "check_past_n_frames_for_epipolar_err": 5, + "enforce_epipolar_error_requirement_": 1, + "early_votes_return": 1, + "visual_feature_inlier_majority_percentage": 5.0000000000000000e-01 + }, + "local_ba_iteration_params": { + "allow_reversion_after_detecting_jumps": 1, + "consecutive_pose_transl_tol": 0.5, + "consecutive_pose_orient_tol": 1.8, + "feature_outlier_percentage": 1.0000000000000001e-01, + "phase_one_opt_params": { + "max_num_iterations": 0, + "allow_non_monotonic_steps": 1, + "function_tolerance": 0.001, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + }, + "phase_two_opt_params": { + "max_num_iterations": 15, + "allow_non_monotonic_steps": 1, + "function_tolerance": 1e-5, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + } + }, + "global_ba_iteration_params": { + "allow_reversion_after_detecting_jumps": 1, + "consecutive_pose_transl_tol": 1.0, + "consecutive_pose_orient_tol": 3.1415926535897931e+00, + "feature_outlier_percentage": 1.0000000000000001e-01, + "phase_one_opt_params": { + "max_num_iterations": 250, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + }, + "phase_two_opt_params": { + "max_num_iterations": 250, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + } + }, + "final_ba_iteration_params": { + "allow_reversion_after_detecting_jumps": 1, + "consecutive_pose_transl_tol": 1.0, + "consecutive_pose_orient_tol": 3.1415926535897931e+00, + "feature_outlier_percentage": 1.0000000000000001e-01, + "phase_one_opt_params": { + "max_num_iterations": 300, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + }, + "phase_two_opt_params": { + "max_num_iterations": 300, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + } + }, + "pgo_solver_params": { + "relative_pose_factor_huber_loss": 5.0, + "enable_visual_feats_only_opt_post_pgo": 0, + "enable_visual_non_opt_feature_adjustment_post_pgo": 0, + "relative_pose_cov_params": { + "transl_error_mult_for_transl_error": 5.0000000000000001e-01, + "transl_error_mult_for_rot_error": 5.0000000000000001e-01, + "rot_error_mult_for_transl_error": 5.0000000000000001e-01, + "rot_error_mult_for_rot_error": 5.0000000000000001e-01 + }, + "pgo_optimization_solver_params": { + "max_num_iterations": 250, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + }, + "final_pgo_optimization_solver_params": { + "max_num_iterations": 300, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + } + }, + "ltm_tunable_params": { + "far_feature_threshold": 75.0, + "min_col_norm": 5.0000000000000001e-09, + "fallback_to_prev_for_failed_extraction": 1 + }, + "ltm_solver_residual_params": { + "object_residual_params": { + "object_observation_huber_loss_param": 5.0000000000000000e-01, + "shape_dim_prior_factor_huber_loss_param": 10.0, + "invalid_ellipsoid_error_val": 1000.0 + }, + "visual_residual_params": { + "reprojection_error_huber_loss_param": 1.0 + }, + "long_term_map_params": { + "pair_huber_loss_param": 1.0 + }, + "relative_pose_factor_huber_loss": 1.0, + "relative_pose_cov_params": { + "transl_error_mult_for_transl_error": 0.0, + "transl_error_mult_for_rot_error": 0.0, + "rot_error_mult_for_transl_error": 0.0, + "rot_error_mult_for_rot_error": 0.0 + } + }, + "ltm_solver_params": { + "max_num_iterations": 300, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 10000.0, + "max_trust_region_radius": 1.0000000000000000e+16 + }, + "shape_dimension_priors": { + "dimension_prior_label": [ + { + "semantic_class": "roadblock", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 2.8999999999999998e-01, + 2.8999999999999998e-01, + 4.7999999999999998e-01 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 9.9999999999999995e-07, + 0.0, + 0.0, + 0.0, + 9.9999999999999995e-07, + 0.0, + 0.0, + 0.0, + 1.0000000000000000e-04 + ] + } + }, + { + "semantic_class": "trashcan", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 1.0, + 1.0, + 1.5000000000000000e+00 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 2.2500000000000000e+00 + ] + } + }, + { + "semantic_class": "bottle", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 5.0000000000000003e-02, + 5.0000000000000003e-02, + 1.2000000000000000e-01 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 2.5000000000000005e-03, + 0.0, + 0.0, + 0.0, + 2.5000000000000005e-03, + 0.0, + 0.0, + 0.0, + 2.5000000000000005e-03 + ] + } + }, + { + "semantic_class": "cup", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 8.9999999999999997e-02, + 8.9999999999999997e-02, + 1.1000000000000000e-01 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 1.0000000000000002e-02, + 0.0, + 0.0, + 0.0, + 1.0000000000000002e-02, + 0.0, + 0.0, + 0.0, + 1.0000000000000002e-02 + ] + } + }, + { + "semantic_class": "lamppost", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 2.9999999999999999e-01, + 2.9999999999999999e-01, + 4.0 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 2.2499999999999999e-02, + 0.0, + 0.0, + 0.0, + 2.2499999999999999e-02, + 0.0, + 0.0, + 0.0, + 9.0 + ] + } + }, + { + "semantic_class": "keyboard", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 4.2999999999999999e-01, + 1.3000000000000000e-01, + 2.5000000000000001e-02 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 2.5000000000000005e-03, + 0.0, + 0.0, + 0.0, + 2.5000000000000005e-03, + 0.0, + 0.0, + 0.0, + 2.5000000000000005e-03 + ] + } + }, + { + "semantic_class": "tv", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 2.0000000000000001e-01, + 1.4999999999999999e-01, + 2.9999999999999999e-01 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 1.0000000000000002e-02, + 0.0, + 0.0, + 0.0, + 1.0000000000000002e-02, + 0.0, + 0.0, + 0.0, + 1.0000000000000002e-02 + ] + } + }, + { + "semantic_class": "treetrunk", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 4.0000000000000002e-01, + 4.0000000000000002e-01, + 2.0 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 4.0000000000000008e-02, + 0.0, + 0.0, + 0.0, + 4.0000000000000008e-02, + 0.0, + 0.0, + 0.0, + 9.0 + ] + } + }, + { + "semantic_class": "mouse", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 5.0000000000000003e-02, + 1.0000000000000001e-01, + 2.0000000000000000e-02 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 2.5000000000000000e-01, + 0.0, + 0.0, + 0.0, + 2.5000000000000000e-01, + 0.0, + 0.0, + 0.0, + 2.5000000000000000e-01 + ] + } + }, + { + "semantic_class": "book", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 2.0000000000000001e-01, + 2.0000000000000001e-01, + 5.0000000000000003e-02 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + } + }, + { + "semantic_class": "potted_plant", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 1.4999999999999999e-01, + 1.4999999999999999e-01, + 1.0 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 9.0 + ] + } + }, + { + "semantic_class": "teddy_bear", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 5.0000000000000000e-01, + 2.9999999999999999e-01, + 1.0 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 6.4000000000000012e-01, + 0.0, + 0.0, + 0.0, + 6.4000000000000012e-01, + 0.0, + 0.0, + 0.0, + 6.4000000000000012e-01 + ] + } + }, + { + "semantic_class": "chair", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 4.6999999999999997e-01, + 4.6999999999999997e-01, + 8.0000000000000004e-01 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 6.2500000000000000e-02, + 0.0, + 0.0, + 0.0, + 6.2500000000000000e-02, + 0.0, + 0.0, + 0.0, + 6.2500000000000000e-02 + ] + } + }, + { + "semantic_class": "bench", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 1.0, + 2.5000000000000000e+00, + 1.5000000000000000e+00 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 2.2500000000000000e+00, + 0.0, + 0.0, + 0.0, + 4.0, + 0.0, + 0.0, + 0.0, + 2.2500000000000000e+00 + ] + } + } + ] + }, + "camera_info": { + "camera_topic_to_camera_id": [ + { + "camera_topic": "/zed2i/zed_node/right/image_rect_color", + "camera_id": "2" + }, + { + "camera_topic": "/zed2i/zed_node/left/image_rect_color", + "camera_id": "1" + }, + { + "camera_topic": "/zed2i/zed_node/left/image_rect_color/compressed", + "camera_id": "1" + }, + { + "camera_topic": "/zed2i/zed_node/right/image_rect_color/compressed", + "camera_id": "2" + }, + { + "camera_topic": "/zed/zed_node/right/image_rect_color", + "camera_id": "2" + }, + { + "camera_topic": "/zed/zed_node/right/image_rect_color/compressed", + "camera_id": "2" + }, + { + "camera_topic": "/zed/zed_node/left/image_rect_color", + "camera_id": "1" + }, + { + "camera_topic": "/zed/zed_node/left/image_rect_color/compressed", + "camera_id": "1" + } + ] + }, + "bounding_box_front_end_params": { + "geometric_similarity_scorer_params": { + "max_merge_distance": 10.0, + "x_y_only_merge": 1 + }, + "feature_based_bb_association_params": { + "min_observations_for_local_est": 3, + "min_observations": 10, + "discard_candidate_after_num_frames": "40", + "min_bb_confidence": 2.0000000000000001e-01, + "required_min_conf_for_initialization": 0.0, + "min_overlapping_features_for_match": 3.0, + "feature_validity_window": "20", + "pending_obj_estimator_params": { + "object_residual_params": { + "object_observation_huber_loss_param": 5.0000000000000000e-01, + "shape_dim_prior_factor_huber_loss_param": 10.0, + "invalid_ellipsoid_error_val": 1000.0 + }, + "solver_params": { + "max_num_iterations": 500, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 10000.0, + "max_trust_region_radius": 1.0000000000000000e+16 + } + }, + "bounding_box_inflation_size": 10.0 + }, + "post_session_object_merge_params": { + "max_merge_distance": -1.0, + "x_y_only_merge": 1 + } + }, + "bounding_box_covariance_generator_params": { + "bounding_box_cov": { + "Rows": 4, + "Cols": 4, + "Data": [ + 1, + 0.0, + 0.0, + 0.0, + 0.0, + 1, + 0.0, + 0.0, + 0.0, + 0.0, + 1, + 0.0, + 0.0, + 0.0, + 0.0, + 1 + ] + }, + "near_edge_threshold": 25.0, + "image_boundary_variance": 40000.0 + }, + "sliding_window_params": { + "global_ba_frequency": "30", + "local_ba_window_size": "50" + }, + "optimization_factors_enabled_params": { + "min_low_level_feature_observations_per_frame": 50, + "include_object_factors": 1, + "include_visual_factors": 1, + "fix_poses": 0, + "fix_objects": 0, + "fix_visual_features": 0, + "fix_ltm_objects": 0, + "use_pom": 0, + "poses_prior_to_window_to_keep_constant": 5, + "min_object_observations": 10, + "min_low_level_feature_observations": 5, + "use_pose_graph_on_global_ba": 1, + "use_visual_features_on_global_ba": 0, + "use_pose_graph_on_final_global_ba": 1, + "use_visual_features_on_final_global_ba": 1 + }, + "object_visual_pose_graph_residual_params": { + "object_residual_params": { + "object_observation_huber_loss_param": 5.0000000000000000e-01, + "shape_dim_prior_factor_huber_loss_param": 10.0, + "invalid_ellipsoid_error_val": 1000.0 + }, + "visual_residual_params": { + "reprojection_error_huber_loss_param": 1.0 + }, + "long_term_map_params": { + "pair_huber_loss_param": 1.0 + }, + "relative_pose_factor_huber_loss": 1.0, + "relative_pose_cov_params": { + "transl_error_mult_for_transl_error": 2.5000000000000001e-02, + "transl_error_mult_for_rot_error": 2.5000000000000001e-02, + "rot_error_mult_for_transl_error": 2.5000000000000001e-02, + "rot_error_mult_for_rot_error": 2.5000000000000001e-02 + } + }, + "limit_traj_eval_params": { + "should_limit_trajectory_evaluation": 0, + "max_frame_id": 1 + }, + "sparsifier_params": { + "max_pose_inc_threshold_transl": 0.1, + "max_pose_inc_threshold_rot": 0.05 + } + } +} diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_gt_format.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_gt_format.sh index 6ae9f9cf..41698d20 100644 --- 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 \ No newline at end of file +# --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 \ No newline at end of file diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh index c44e2df4..d0493a41 100644 --- 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 index b71a9f66..86c05c40 100644 --- 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 index f6cb4c65..a000e61b 100644 --- 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 index 7dd301ff..73f6742c 100644 --- 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,7 +446,7 @@ 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 @@ -447,17 +454,17 @@ Visualization Manager: 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 index 92c3ecbf..2ba35d3b 100644 --- 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> - 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> + // 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 b/src/evaluation/tum/debug_transform.py similarity index 100% rename from src/evaluation/tum/debug.py rename to src/evaluation/tum/debug_transform.py diff --git a/src/evaluation/tum/plot_trajectory.py b/src/evaluation/tum/plot_trajectory.py new file mode 100644 index 00000000..48b10650 --- /dev/null +++ b/src/evaluation/tum/plot_trajectory.py @@ -0,0 +1,75 @@ +import matplotlib.pyplot as plt +import os +import json +import numpy as np +from scipy.spatial.transform import Rotation as R + +kDatadir = "/home/tiejean/Documents/mnt/oslam/TUM_results/fr2_desk" +kObViSLAMFilepath = os.path.join(kDatadir, "obvislam.json") +kLeGOLOAMFilepath = os.path.join(kDatadir, "legoloam.csv") +kORBSLAM3Filepath = os.path.join(kDatadir, "orbslam3.csv") + +def load_obvislam(filepath:str) -> np.array: + framed_poses = [] + + fp = open(filepath, "r") + if fp.closed: + raise FileNotFoundError("Failed to open file " + filepath) + data = json.load(fp) + for framed_pose in data["robot_poses"]["robot_pose_results_map"]: + fid = int(framed_pose["frame_id"]) + transl = np.array(framed_pose["pose"]["transl"]["Data"]) + rotvec = framed_pose["pose"]["rot"]["angle"] * np.array(framed_pose["pose"]["rot"]["axis"]["Data"]) + quat = R.from_rotvec(rotvec).as_quat() + framed_poses.append((fid, transl.tolist() + quat.tolist())) + fp.close() + + framed_poses = sorted(framed_poses, key=lambda x: x[0]) + poses = [framed_pose[1] for framed_pose in framed_poses] + poses = np.array(poses) + return poses + +def load_legoloam(filepath:str) -> np.array: + fp = open(filepath, "r") + if fp.closed: + raise FileNotFoundError("Failed to open file " + filepath) + poses = [] + for line in fp.readlines()[1:]: + tokens = [token.strip() for token in line.split(",")] + pose = [float(token) for token in tokens[2:]] + poses.append(pose) + fp.close() + poses = np.array(poses) + return poses + +def load_orbslam3(filepath:str) -> np.array: + fp = open(filepath, "r") + if fp.closed: + raise FileNotFoundError("Failed to open file " + filepath) + poses = [] + for line in fp.readlines()[1:]: + tokens = [token.strip() for token in line.split(",")] + pose = [float(token) for token in tokens[3:]] + poses.append(pose) + fp.close() + poses = np.array(poses) + fp.close() + +def plot(results:dict): + plt.figure() + for approach, poses in results.items(): + if approach == "ObVi-SLAM" or approach == "Groundtruth": + plt.plot(poses[:,0], poses[:,1], label=approach) + elif approach == "ORBSLAM3": + plt.plot(-poses[:,2], -poses[:,1], label=approach) + plt.legend() + plt.axis("equal") + plt.show() + +if __name__ == "__main__": + results = {} + results["ObVi-SLAM"] = load_obvislam(kObViSLAMFilepath) + results["Groundtruth"] = load_legoloam(kLeGOLOAMFilepath) + results["ORBSLAM3"] = load_legoloam(kORBSLAM3Filepath) + plot(results=results) + # import pdb; pdb.set_trace() \ No newline at end of file diff --git a/src/evaluation/videos/gt_transformer.cpp b/src/evaluation/videos/gt_transformer.cpp new file mode 100644 index 00000000..736ad598 --- /dev/null +++ b/src/evaluation/videos/gt_transformer.cpp @@ -0,0 +1,265 @@ +// +// Created by amanda on 2/19/23. +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +using namespace vslam_types_refactor; +using namespace pose; + +DEFINE_string(interpolated_traj, "", ""); +DEFINE_string( + lego_loam_frame_to_bl_extrinsics, + "", + "File containing the extrinsic calibration of the frame " + "that the (pseudo) ground truth outputs are with respect to " + "relative to the baselink. If the frame for ground truth is the " + "ouster's frame, for example, this provides the pose of the ouster" + " relative to the base_link"); +DEFINE_string(interp_out_file, "", ""); + +FullSequenceMetrics computeMetrics( + const std::vector< + std::vector>>>> + &comparison_trajectories_rel_baselink, + const std::vector>>> + &interp_gt_trajectories_rel_baselink, + const std::vector &ros_bag_names, + const std::string &odom_topic, + const std::vector> &waypoint_info_by_trajectory, + const std::vector &traj_with_waypoints_files, + const std::shared_ptr &vis_manager = + nullptr) { + FullSequenceMetrics full_metrics; + std::vector single_traj_ate_results; + + std::vector< + util::BoostHashMap>>> + poses_by_timestamp_by_trajectory; + for (size_t traj_num = 0; + traj_num < comparison_trajectories_rel_baselink.size(); + traj_num++) { + util::BoostHashMap>> + poses_by_timestamp; + for (const std::pair>> + ×tamp_and_pose : + comparison_trajectories_rel_baselink.at(traj_num)) { + poses_by_timestamp[timestamp_and_pose.first] = timestamp_and_pose.second; + } + poses_by_timestamp_by_trajectory.emplace_back(poses_by_timestamp); + } + + // Assumes odom is for base_link + std::vector>> + odom_poses_by_trajectory; + for (const std::string &rosbag_name : ros_bag_names) { + std::vector> odom_poses_for_bag; + getOdomPoseEsts(rosbag_name, odom_topic, odom_poses_for_bag); + odom_poses_by_trajectory.emplace_back(odom_poses_for_bag); + } + + RawWaypointConsistencyResults raw_consistency_results = + computeWaypointConsistencyResults(waypoint_info_by_trajectory, + comparison_trajectories_rel_baselink, + poses_by_timestamp_by_trajectory, + odom_poses_by_trajectory, + vis_manager); + + CHECK(raw_consistency_results.pose_and_waypoint_info_for_nodes_per_trajectory_ + .size() == traj_with_waypoints_files.size()) + << "Full trajectory with waypoint annotations structure must match " + "number of files to output such information to."; + for (size_t traj_num = 0; traj_num < traj_with_waypoints_files.size(); + traj_num++) { + std::string traj_with_wps_name = traj_with_waypoints_files.at(traj_num); + if (traj_with_wps_name.empty()) { + continue; + } + std::vector> + pose_and_waypoint_info_for_nodes_for_trajectory = + raw_consistency_results + .pose_and_waypoint_info_for_nodes_per_trajectory_.at(traj_num); + + file_io::writePose3dWsithWaypointInfoToFile( + traj_with_wps_name, pose_and_waypoint_info_for_nodes_for_trajectory); + } + + for (size_t traj_num = 0; + traj_num < comparison_trajectories_rel_baselink.size(); + traj_num++) { + std::vector>> gt_rel_bl_traj = + interp_gt_trajectories_rel_baselink[traj_num]; + std::vector> gt_pose_only; + for (const std::pair> >_entry : + gt_rel_bl_traj) { + gt_pose_only.emplace_back(gt_entry.second); + } + + std::vector>>> + comparison_traj_rel_bl = comparison_trajectories_rel_baselink[traj_num]; + std::vector>> comparison_pose_only; + for (const std::pair>> + &comparison_entry : comparison_traj_rel_bl) { + comparison_pose_only.emplace_back(comparison_entry.second); + } + + std::vector>> aligned_comparison_pose; + alignWithGroundTruth( + gt_pose_only, comparison_pose_only, aligned_comparison_pose); + + ATEResults traj_ate_results = + generateATEforRotAndTranslForSyncedAlignedTrajectories( + aligned_comparison_pose, gt_pose_only); + TrajectoryMetrics single_traj_metrics; + single_traj_metrics.ate_results_ = traj_ate_results; + single_traj_ate_results.emplace_back(traj_ate_results); + + for (const auto &waypoint_with_centroid_dev_by_trajectory : + raw_consistency_results + .centroid_deviations_by_waypoint_by_trajectory_) { + WaypointId waypoint = waypoint_with_centroid_dev_by_trajectory.first; + std::vector centroid_devs_for_waypoint_for_trajectory = + waypoint_with_centroid_dev_by_trajectory.second.at(traj_num); + std::vector orientation_devs_for_trajectory = + raw_consistency_results + .orientation_deviations_by_waypoint_by_trajectory_.at(waypoint) + .at(traj_num); + single_traj_metrics.waypoint_deviations_[waypoint] = + std::make_pair(centroid_devs_for_waypoint_for_trajectory, + orientation_devs_for_trajectory); + single_traj_metrics.all_rotation_deviations_.insert( + single_traj_metrics.all_rotation_deviations_.end(), + orientation_devs_for_trajectory.begin(), + orientation_devs_for_trajectory.end()); + single_traj_metrics.all_translation_deviations_.insert( + single_traj_metrics.all_translation_deviations_.end(), + centroid_devs_for_waypoint_for_trajectory.begin(), + centroid_devs_for_waypoint_for_trajectory.end()); + } + + full_metrics.indiv_trajectory_metrics_.emplace_back(single_traj_metrics); + } + + TrajectoryMetrics sequence_results; + sequence_results.ate_results_ = + combineSingleTrajectoryResults(single_traj_ate_results); + for (const TrajectoryMetrics &single_traj_metrics : + full_metrics.indiv_trajectory_metrics_) { + sequence_results.all_translation_deviations_.insert( + sequence_results.all_translation_deviations_.end(), + single_traj_metrics.all_translation_deviations_.begin(), + single_traj_metrics.all_translation_deviations_.end()); + sequence_results.all_rotation_deviations_.insert( + sequence_results.all_rotation_deviations_.end(), + single_traj_metrics.all_rotation_deviations_.begin(), + single_traj_metrics.all_rotation_deviations_.end()); + + for (const auto &waypoint_and_devs : + single_traj_metrics.waypoint_deviations_) { + std::pair, std::vector> all_waypoint_devs; + if (sequence_results.waypoint_deviations_.find(waypoint_and_devs.first) != + sequence_results.waypoint_deviations_.end()) { + all_waypoint_devs = + sequence_results.waypoint_deviations_.at(waypoint_and_devs.first); + } else { + all_waypoint_devs = + std::make_pair((std::vector){}, (std::vector){}); + } + std::vector centroid_devs_for_waypoint = all_waypoint_devs.first; + std::vector orientation_devs_for_waypoint = + all_waypoint_devs.second; + centroid_devs_for_waypoint.insert(centroid_devs_for_waypoint.end(), + waypoint_and_devs.second.first.begin(), + waypoint_and_devs.second.first.end()); + orientation_devs_for_waypoint.insert( + orientation_devs_for_waypoint.end(), + waypoint_and_devs.second.second.begin(), + waypoint_and_devs.second.second.end()); + sequence_results.waypoint_deviations_[waypoint_and_devs.first] = + std::make_pair(centroid_devs_for_waypoint, + orientation_devs_for_waypoint); + } + } + + full_metrics.sequence_metrics_ = sequence_results; + return full_metrics; +} + +int main(int argc, char **argv) { + google::ParseCommandLineFlags(&argc, &argv, false); + + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = true; + FLAGS_colorlogtostderr = true; + + + + ros::init(argc, argv, "gt_transformer"); + ros::NodeHandle node_handle; + + + std::vector>> + raw_pose_3ds_with_timestamp; + file_io::readPose3dsWithTimestampFromFile(FLAGS_interpolated_traj, + raw_pose_3ds_with_timestamp); + + Pose3D gt_traj_frame_rel_base_link; + std::vector> gt_traj_extrinsics_contents; + file_io::readPose3dsFromFile(FLAGS_lego_loam_frame_to_bl_extrinsics, + gt_traj_extrinsics_contents); + if (gt_traj_extrinsics_contents.empty()) { + LOG(ERROR) << "GT trajectory extrinsics missing"; + exit(1); + } + if (gt_traj_extrinsics_contents.size() > 1) { + LOG(WARNING) << "Coarse trajectory extrinsics file contained more than " + "one pose. Taking the first"; + } + + gt_traj_frame_rel_base_link = gt_traj_extrinsics_contents.front(); + + // std::unordered_map> gt_traj_map; + std::vector> gt_traj_calib; + + for (const std::pair> >_pose_entry : + raw_pose_3ds_with_timestamp) { + gt_traj_calib.emplace_back(combinePoses( + gt_pose_entry.second, poseInverse(gt_traj_frame_rel_base_link))); + } + std::vector> gt_trajectory; + LOG(INFO) << "Calibrated but not adjusted "; + LOG(INFO) << gt_traj_calib.front().transl_; + + gt_trajectory = adjustTrajectoryToStartAtOrigin(gt_traj_calib); + + + LOG(INFO) << "Calibrated but not adjusted wihtout time "; + LOG(INFO) << gt_trajectory.front().transl_; + + std::vector>> + adjusted_raw_pose_3ds_with_timestamp; + for (FrameId idx = 0; idx < gt_trajectory.size(); idx++) { + pose::Timestamp time = raw_pose_3ds_with_timestamp.at(idx).first; + Pose3D pose = gt_trajectory.at(idx); + + adjusted_raw_pose_3ds_with_timestamp.emplace_back(std::make_pair(time, pose)); + } + + + LOG(INFO) << "Calibrated and not adjusted "; + LOG(INFO) << adjusted_raw_pose_3ds_with_timestamp.front().second.transl_; + file_io::writePose3dsWithTimestampToFile(FLAGS_interp_out_file, adjusted_raw_pose_3ds_with_timestamp); +} \ No newline at end of file diff --git a/src/evaluation/videos/postprocess_markers.py b/src/evaluation/videos/postprocess_markers.py index 27ca7beb..2a0c9ef4 100644 --- a/src/evaluation/videos/postprocess_markers.py +++ b/src/evaluation/videos/postprocess_markers.py @@ -5,69 +5,63 @@ kBagname = "0__2023_05_11_18_35_54" kPrefix = "/evaluation_2023_07_v1_update_rev_lowerererish_conv_thresholds_manual_feat_adj_tighterer_vo_v4_" + kBagname + "/" - -kBagpathIn = "/home/tiejean/Documents/mnt/oslam/visualization/0__2023_05_11_18_35_54.bag" -kBagpathOut = "/home/tiejean/Documents/mnt/oslam/visualization/0__2023_05_11_18_35_54_postprocessed2.bag" - +kBagpathIn = "/home/tiejean/Documents/mnt/oslam/visualization/" + kBagname + ".bag" +kBagpathOut = "/home/tiejean/Documents/mnt/oslam/visualization/" + kBagname + "_postprocessed2.bag" kLeGOLOAMFilepath = "/home/tiejean/Documents/mnt/oslam/ut_vslam_results/postprocessing/interpolated_lego_loam_poses_bag_0_viz.cvs" def load_lego_loam(filepath: str): fp = open(filepath, "r") if fp.closed: raise FileNotFoundError("Failed to open file " + filepath) - # stamped_poses = {} poses = [] for line in fp.readlines()[1:]: tokens = [token.strip() for token in line.split(",")] sec, nsec = int(tokens[0]), int(tokens[1]) pose = [float(token) for token in tokens[2:]] poses.append(pose) - # stamped_poses[(sec, nsec)] = pose fp.close() - # return stamped_poses return poses if __name__ == "__main__": - interesting_topics = ["/est_pose", "/est_ellipsoids", "/est_/latest/cam_1_bb/image_raw", "/tf"] - interesting_topics = [kPrefix + topic for topic in interesting_topics] - - # stamped_ref_poses = load_lego_loam(kLeGOLOAMFilepath) ref_poses = load_lego_loam(kLeGOLOAMFilepath) bagIn = rosbag.Bag(kBagpathIn, 'r') bagOut = rosbag.Bag(kBagpathOut, 'w') - all_gt_dtimes = [] - all_gt_msgs = [] - for topic, msg, t in bagIn.read_messages(topics=(kPrefix + "/gt_pose")): - if msg.type != 4: + for topic, msg, t in bagIn.read_messages(): + if topic == (kPrefix + "/gt_pose"): continue - sec, nsec = msg.header.stamp.secs, msg.header.stamp.nsecs - all_gt_dtimes.append(sec + nsec * 1e-9) - all_gt_msgs.append(msg) - - for topic, msg, t in bagIn.read_messages(topics=[kPrefix + "/gt_pose", kPrefix + "/est_pose"]): - if topic == (kPrefix + "/est_pose"): + elif topic == (kPrefix + "/est_pose"): if msg.type != 4: continue sec, nsec = msg.header.stamp.secs, msg.header.stamp.nsecs dtime = sec + nsec * 1e-9 - msgEst = copy.deepcopy(msg) - tdiffs_gt = np.array(all_gt_dtimes)-dtime; idx_gt = np.argmin(np.abs(tdiffs_gt)) - msgGt = copy.deepcopy(all_gt_msgs[idx_gt]) + msgEst, msgGt = copy.deepcopy(msg), copy.deepcopy(msg) nEstPoses, nGtPoses = len(msgEst.points), len(msgGt.points) - if (nGtPoses >= nEstPoses): - # cap the number of poses to msgGt to be nEstPoses - msgGt.points = msgGt.points[:nEstPoses] - else: - for pose in ref_poses[nGtPoses:nEstPoses]: - # msgGt.points.append(Point(pose[0]+0.46618, pose[1], pose[2])) - msgGt.points.append(Point(pose[0], pose[1], pose[2])) + msgGt.points = [] + for pose in ref_poses[0:nEstPoses]: + msgGt.points.append(Point(pose[0], pose[1], pose[2])) + + msgEst.color.r = 0 + msgEst.color.g = 1 + msgEst.color.b = 1 + msgEst.scale.x *= 8 + msgEst.scale.y *= 1 + msgEst.scale.z *= 8 + + msgGt.color.r = 0 + msgGt.color.g = .5 + msgGt.color.b = 1 + msgGt.scale.x *= 8 + msgGt.scale.y *= 1 + msgGt.scale.z *= 8 bagOut.write(kPrefix + "/gt_pose", msgGt, t) bagOut.write(kPrefix + "/est_pose", msgEst, t) + else: + bagOut.write(topic, msg, t) # for topic, msg, t in bagIn.read_messages(): diff --git a/src/evaluation/videos/publish_map.py b/src/evaluation/videos/publish_map.py index ec399f50..4b230cce 100644 --- a/src/evaluation/videos/publish_map.py +++ b/src/evaluation/videos/publish_map.py @@ -2,6 +2,7 @@ import cv2 from nav_msgs.msg import OccupancyGrid import numpy as np +from scipy import ndimage kImage0 = "/home/tiejean/Desktop/EER0.png" @@ -12,15 +13,24 @@ msg = OccupancyGrid() img = cv2.imread(kImage0, cv2.IMREAD_GRAYSCALE) + img = ndimage.rotate(img, 2) + for i in range(img.shape[0]): + for j in range(img.shape[1]): + if img[i][j] == 0: + img[i][j] = 255 + # import pdb; pdb.set_trace() msg.header.stamp = rospy.get_rostime() msg.header.frame_id = "map" msg.info.map_load_time = rospy.get_rostime() - msg.info.resolution = .055 + msg.info.resolution = .054 + # msg.info.resolution = .055 msg.info.width = img.shape[1] msg.info.height = img.shape[0] - msg.info.origin.position.x = -21.5 + msg.info.origin.position.x = -23.5 msg.info.origin.position.y = -21 + # msg.info.origin.position.x = -21.5 + # msg.info.origin.position.y = -21 data = img data = cv2.rotate(data, cv2.ROTATE_180) data = cv2.flip(data, 1) diff --git a/src/refactoring/offline_object_visual_slam_main.cpp b/src/refactoring/offline_object_visual_slam_main.cpp index 870b9d32..e0ca90f4 100644 --- a/src/refactoring/offline_object_visual_slam_main.cpp +++ b/src/refactoring/offline_object_visual_slam_main.cpp @@ -168,9 +168,12 @@ readBoundingBoxesByTimestampFromFile( file_io::readBoundingBoxWithTimestampsFromFile(bounding_boxes_file_name, bounding_boxes_by_timestamp); // TODO (Taijing): Currently I hacked it to only take in some object classes + // const std::unordered_set classes = { + // "bottle", "cup", "teddy_bear", "tv", "mouse", + // "keyboard", "book", "potted_plant", "chair"}; const std::unordered_set classes = { "bottle", "cup", "teddy_bear", "tv", "mouse", - "keyboard", "book", "potted_plant", "chair"}; + "keyboard", "chair"}; std::vector bounding_boxes_by_timestamp_tmp; for (const auto &detection : bounding_boxes_by_timestamp) { if (classes.find(detection.semantic_class) != classes.end()) { diff --git a/src/shared b/src/shared index 9104776f..ef447c2e 160000 --- a/src/shared +++ b/src/shared @@ -1 +1 @@ -Subproject commit 9104776fa803014add8c5bc18d9b4ca1f2b308cb +Subproject commit ef447c2e99c97ed29e9b815af055246175b2a3b0