diff --git a/config/tum_fr2_desk.json b/config/tum_fr2_desk.json new file mode 100644 index 00000000..e52b0d0f --- /dev/null +++ b/config/tum_fr2_desk.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": 2.0, + "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": 8.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": 1.0, + "consecutive_pose_orient_tol": 3.1415926535897931e+00, + "feature_outlier_percentage": 1.0000000000000001e-01, + "phase_one_opt_params": { + "max_num_iterations": 200, + "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": 200, + "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 + } + }, + "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": 1.0000000000000001e-01, + "transl_error_mult_for_rot_error": 1.0000000000000001e-01, + "rot_error_mult_for_transl_error": 1.0000000000000001e-01, + "rot_error_mult_for_rot_error": 1.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": 4.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": [ + 900.0, + 0.0, + 0.0, + 0.0, + 0.0, + 900.0, + 0.0, + 0.0, + 0.0, + 0.0, + 900.0, + 0.0, + 0.0, + 0.0, + 0.0, + 900.0 + ] + }, + "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": 2.0000000000000001e-01, + "max_pose_inc_threshold_rot": 1.0000000000000001e-01 + } + } +} diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh new file mode 100644 index 00000000..d2c49628 --- /dev/null +++ b/convenience_scripts/tum/taijing/tum_freiburg2_desk_obvislam.sh @@ -0,0 +1,61 @@ +# bagname=freiburg2_desk +# configname=base7a_2_fallback_a_2_tum + +# SLAMDIR=/home/tiejean/Documents/projects/ut_vslam +# DARADIR=/home/tiejean/Documents/mnt/oslam +# BBOX_DIR=$DARADIR/bounding_box_data +# CALIB_DIR=$DARADIR/calibration_tum + + +# vslam_in=$DARADIR/orb_post_process/sparsified_ut_vslam_in/$configname/$bagname/ + +# configfile=$SLAMDIR/config/${base7a_2_fallback_a_2}.json +# intrinsics_file=$CALIB_DIR/camera_matrix.txt +# extrinsics_file=$CALIB_DIR/extrinsics.txt +# bounding_boxes_by_timestamp_file=$BBOX_DIR/bounding_boxes_by_timestamp_${bagname}.csv +# poses_by_node_id_file=$ORB_POST_PROCESS +# sparsified_orb_out=$ORB_POST_PROCESS/sparsified_ut_vslam_in/$configname/$bagname/ +# poses_by_node_id_file=$vslam_in/poses/initial_robot_poses_by_node.txt +# nodes_by_timestamp_file=$vslam_in/timestamps/node_ids_and_timestamps.txt +# rosbag_file=$DARADIR/TUM/$bagname + +# ./bin/offline_object_visual_slam_main \ +# --param_prefix $bagname \ +# --intrinsics_file $intrinsics_file \ +# --extrinsics_file $extrinsics_file \ +# --bounding_boxes_by_timestamp_file $bounding_boxes_by_timestamp_file \ +# --poses_by_node_id_file $poses_by_node_id_file \ +# --nodes_by_timestamp_file $nodes_by_timestamp_file \ +# --rosbag_file $rosbag_file \ + +rosparam set /use_sim_time false +SLAM_DIR=/home/tiejean/Documents/projects/ut_vslam +config_file_directory=$SLAM_DIR/config/ +trajectory_sequence_file_directory=$SLAM_DIR/sequences/ + +root_data_dir=/home/tiejean/Documents/mnt/oslam/ +calibration_file_directory=${root_data_dir}calibration_tum/ +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/ +bounding_box_post_process_base_directory=$root_data_dir/bounding_box_data/ + +sequence_file_base_name="tum_fr2_desk" + +cd $SLAM_DIR + +config_file_base_name="tum_fr2_desk" +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} \ + --orb_slam_out_directory ${orb_slam_out_directory} \ + --rosbag_file_directory ${rosbag_file_directory} \ + --orb_post_process_base_directory ${orb_post_process_base_directory} \ + --calibration_file_directory ${calibration_file_directory} \ + --trajectory_sequence_file_directory ${trajectory_sequence_file_directory} \ + --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 + diff --git a/convenience_scripts/tum/taijing/tum_freiburg2_desk_preprocess.sh b/convenience_scripts/tum/taijing/tum_freiburg2_desk_preprocess.sh index 1a3211f1..b71a9f66 100644 --- a/convenience_scripts/tum/taijing/tum_freiburg2_desk_preprocess.sh +++ b/convenience_scripts/tum/taijing/tum_freiburg2_desk_preprocess.sh @@ -7,7 +7,7 @@ bagname=freiburg2_desk configname=base7a_2_fallback_a_2 configfile=$SLAMDIR/config/${base7a_2_fallback_a_2}.json -calibration_dir=$DARADIR/calibration/ +calibration_dir=$DARADIR/calibration_tum/ orb_out_dir=$ORB_OUT/$bagname/ unsparsified_orb_out=$ORB_POST_PROCESS/unsparsified_ut_vslam_in/$bagname/ sparsified_orb_out=$ORB_POST_PROCESS/sparsified_ut_vslam_in/$configname/$bagname/ diff --git a/sequences/tum_fr2_desk.json b/sequences/tum_fr2_desk.json new file mode 100644 index 00000000..26ab8207 --- /dev/null +++ b/sequences/tum_fr2_desk.json @@ -0,0 +1,14 @@ +{ + "sequence_info": { + "seq_id": "tum_fr2_desk", + "sequence": [ + { + "bag_base_name": "freiburg2_desk", + "waypoint_file_base_name": { + "has_v": 0 + } + } + ] + } + } + \ No newline at end of file diff --git a/src/refactoring/configuration/write_configuration.cpp b/src/refactoring/configuration/write_configuration.cpp index 6d354b71..d82b1c5a 100644 --- a/src/refactoring/configuration/write_configuration.cpp +++ b/src/refactoring/configuration/write_configuration.cpp @@ -25,8 +25,60 @@ ShapeDimensionPriors constructShapeDimPriorConfiguration() { std::unordered_map, ObjectDim>> shape_mean_and_std_devs_by_semantic_class; - Eigen::Vector3d chair_mean(0.62, 0.62, 0.975); - Eigen::Vector3d chair_std_dev(0.05, 0.05, 0.05); + + Eigen::Vector3d bottle_mean(0.05, 0.05, 0.12); + Eigen::Vector3d bottle_std_dev(0.05, 0.05, 0.05); + std::string bottle_class = "bottle"; + shape_mean_and_std_devs_by_semantic_class[bottle_class] = + std::make_pair(bottle_mean, bottle_std_dev); + + Eigen::Vector3d cup_mean(0.09, 0.09, 0.11); + Eigen::Vector3d cup_std_dev(0.1, 0.1, 0.1); + std::string cup_class = "cup"; + shape_mean_and_std_devs_by_semantic_class[cup_class] = + std::make_pair(cup_mean, cup_std_dev); + + Eigen::Vector3d teddy_bear_mean(0.5, 0.3, 1.0); + Eigen::Vector3d teddy_bear_std_dev(0.8, 0.8, 0.8); + std::string teddy_bear_class = "teddy_bear"; + shape_mean_and_std_devs_by_semantic_class[teddy_bear_class] = + std::make_pair(teddy_bear_mean, teddy_bear_std_dev); + + // This is actually for monitors + Eigen::Vector3d tv_mean(0.2, 0.15, 0.3); + Eigen::Vector3d tv_std_dev(0.1, 0.1, 0.1); + std::string tv_class = "tv"; + shape_mean_and_std_devs_by_semantic_class[tv_class] = + std::make_pair(tv_mean, tv_std_dev); + + // mouse for computer, not rat + Eigen::Vector3d mouse_mean(0.05, 0.1, 0.02); + Eigen::Vector3d mouse_std_dev(0.5, 0.5, 0.5); + std::string mouse_class = "mouse"; + shape_mean_and_std_devs_by_semantic_class[mouse_class] = + std::make_pair(mouse_mean, mouse_std_dev); + + Eigen::Vector3d keyboard_mean(.43, .13, 0.025); + Eigen::Vector3d keyboard_std_dev(0.05, 0.05, 0.05); + std::string keyboard_class = "keyboard"; + shape_mean_and_std_devs_by_semantic_class[keyboard_class] = + std::make_pair(keyboard_mean, keyboard_std_dev); + + // This may be a very bad estimate + Eigen::Vector3d book_mean(0.2, 0.2, 0.05); + Eigen::Vector3d book_std_dev(1, 1, 1); + std::string book_class = "book"; + shape_mean_and_std_devs_by_semantic_class[book_class] = + std::make_pair(book_mean, book_std_dev); + + Eigen::Vector3d potted_plant_mean(0.15, 0.15, 1); + Eigen::Vector3d potted_plant_std_dev(1, 1, 3); + std::string potted_plant_class = "potted_plant"; + shape_mean_and_std_devs_by_semantic_class[potted_plant_class] = + std::make_pair(potted_plant_mean, potted_plant_std_dev); + + Eigen::Vector3d chair_mean(0.47, 0.47, 0.8); + Eigen::Vector3d chair_std_dev(0.25, 0.25, 0.25); std::string chair_class = "chair"; shape_mean_and_std_devs_by_semantic_class[chair_class] = std::make_pair(chair_mean, chair_std_dev); diff --git a/src/refactoring/offline_object_visual_slam_main.cpp b/src/refactoring/offline_object_visual_slam_main.cpp index 1eecc2d9..870b9d32 100644 --- a/src/refactoring/offline_object_visual_slam_main.cpp +++ b/src/refactoring/offline_object_visual_slam_main.cpp @@ -120,6 +120,44 @@ DEFINE_bool(disable_log_to_stderr, false, "Set to true if the logging to standard error should be disabled"); +void loadImages(const std::string &imgdir, + const std::string &nodes_by_timestamp_file, + std::unordered_map> &images) { + std::vector nodes_by_timestamps_vec; + util::BoostHashMap + nodes_for_timestamps_map; + file_io::readNodeIdsAndTimestampsFromFile(nodes_by_timestamp_file, + nodes_by_timestamps_vec); + + for (const file_io::NodeIdAndTimestamp &raw_node_id_and_timestamp : + nodes_by_timestamps_vec) { + nodes_for_timestamps_map[std::make_pair( + raw_node_id_and_timestamp.seconds_, + raw_node_id_and_timestamp.nano_seconds_)] = + raw_node_id_and_timestamp.node_id_; + } + + for (const auto ×tamp_and_node : nodes_for_timestamps_map) { + auto sec = timestamp_and_node.first.first; + auto nsec = timestamp_and_node.first.second; + auto frame_id = timestamp_and_node.second; + double timestamp = sec + nsec/1e9; + const std::string imgpath = + imgdir + "/rgb/" + std::to_string(timestamp) + ".png"; + cv::Mat img = cv::imread(imgpath); + cv_bridge::CvImage cvImg; + cvImg.header.stamp.sec = sec; + cvImg.header.stamp.nsec = sec; + cvImg.header.frame_id = frame_id; + cvImg.encoding = sensor_msgs::image_encodings::BGR8; + cvImg.image = img; + images[frame_id][1] = cvImg.toImageMsg(); + images[frame_id][2] = cvImg.toImageMsg(); + } +} + std::unordered_map< vtr::FrameId, std::unordered_map>> @@ -129,6 +167,17 @@ readBoundingBoxesByTimestampFromFile( std::vector bounding_boxes_by_timestamp; 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"}; + std::vector bounding_boxes_by_timestamp_tmp; + for (const auto &detection : bounding_boxes_by_timestamp) { + if (classes.find(detection.semantic_class) != classes.end()) { + bounding_boxes_by_timestamp_tmp.emplace_back(detection); + } + } + bounding_boxes_by_timestamp = bounding_boxes_by_timestamp_tmp; LOG(INFO) << bounding_boxes_by_timestamp.size() << " bounding boxes read"; std::vector nodes_by_timestamps_vec; @@ -779,10 +828,10 @@ int main(int argc, char **argv) { std::unordered_map< vtr::FrameId, std::unordered_map> - images = image_utils::getImagesFromRosbag( - FLAGS_rosbag_file, - FLAGS_nodes_by_timestamp_file, - config.camera_info_.camera_topic_to_camera_id_); + images; + std::string rosbag_file = FLAGS_rosbag_file.substr(0, FLAGS_rosbag_file.size()-4); + loadImages(rosbag_file, FLAGS_nodes_by_timestamp_file, images); + LOG(INFO) << "Done reading images from rosbag"; MainLtmPtr long_term_map;