| 52 | } |
| 53 | |
| 54 | void read_parameters(ros::NodeHandle &nh, ConfigSetting &config_setting) { |
| 55 | // pre-preocess |
| 56 | nh.param<double>("ds_size", config_setting.ds_size_, 0.5); |
| 57 | nh.param<int>("maximum_corner_num", config_setting.maximum_corner_num_, 100); |
| 58 | |
| 59 | // key points |
| 60 | nh.param<double>("plane_merge_normal_thre", |
| 61 | config_setting.plane_merge_normal_thre_, 0.1); |
| 62 | nh.param<double>("plane_detection_thre", config_setting.plane_detection_thre_, |
| 63 | 0.01); |
| 64 | nh.param<double>("voxel_size", config_setting.voxel_size_, 2.0); |
| 65 | nh.param<int>("voxel_init_num", config_setting.voxel_init_num_, 10); |
| 66 | nh.param<double>("proj_image_resolution", |
| 67 | config_setting.proj_image_resolution_, 0.5); |
| 68 | nh.param<double>("proj_dis_min", config_setting.proj_dis_min_, 0); |
| 69 | nh.param<double>("proj_dis_max", config_setting.proj_dis_max_, 2); |
| 70 | nh.param<double>("corner_thre", config_setting.corner_thre_, 10); |
| 71 | |
| 72 | // std descriptor |
| 73 | nh.param<int>("descriptor_near_num", config_setting.descriptor_near_num_, 10); |
| 74 | nh.param<double>("descriptor_min_len", config_setting.descriptor_min_len_, 2); |
| 75 | nh.param<double>("descriptor_max_len", config_setting.descriptor_max_len_, |
| 76 | 50); |
| 77 | nh.param<double>("non_max_suppression_radius", |
| 78 | config_setting.non_max_suppression_radius_, 2.0); |
| 79 | nh.param<double>("std_side_resolution", config_setting.std_side_resolution_, |
| 80 | 0.2); |
| 81 | |
| 82 | // candidate search |
| 83 | nh.param<int>("skip_near_num", config_setting.skip_near_num_, 50); |
| 84 | nh.param<int>("candidate_num", config_setting.candidate_num_, 50); |
| 85 | nh.param<int>("sub_frame_num", config_setting.sub_frame_num_, 10); |
| 86 | nh.param<double>("rough_dis_threshold", config_setting.rough_dis_threshold_, |
| 87 | 0.01); |
| 88 | nh.param<double>("vertex_diff_threshold", |
| 89 | config_setting.vertex_diff_threshold_, 0.5); |
| 90 | nh.param<double>("icp_threshold", config_setting.icp_threshold_, 0.5); |
| 91 | nh.param<double>("normal_threshold", config_setting.normal_threshold_, 0.2); |
| 92 | nh.param<double>("dis_threshold", config_setting.dis_threshold_, 0.5); |
| 93 | |
| 94 | // std::cout << "Sucessfully load parameters:" << std::endl; |
| 95 | // std::cout << "----------------Main Parameters-------------------" |
| 96 | // << std::endl; |
| 97 | // std::cout << "voxel size:" << config_setting.voxel_size_ << std::endl; |
| 98 | // std::cout << "loop detection threshold: " << config_setting.icp_threshold_ |
| 99 | // << std::endl; |
| 100 | // std::cout << "sub-frame number: " << config_setting.sub_frame_num_ |
| 101 | // << std::endl; |
| 102 | // std::cout << "candidate number: " << config_setting.candidate_num_ |
| 103 | // << std::endl; |
| 104 | // std::cout << "maximum corners size: " << config_setting.maximum_corner_num_ |
| 105 | // << std::endl; |
| 106 | } |
| 107 | |
| 108 | void load_pose_with_time( |
| 109 | const std::string &pose_file, |