diff --git a/camera_model/CMakeLists.txt b/camera_model/CMakeLists.txt index 20fd41f14..e4e2bffe5 100644 --- a/camera_model/CMakeLists.txt +++ b/camera_model/CMakeLists.txt @@ -24,7 +24,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES camera_model CATKIN_DEPENDS roscpp std_msgs - DEPENDS system_lib +# DEPENDS system_lib ) include_directories( diff --git a/config/A3/A3_config.yaml b/config/A3/A3_config.yaml index 4bf527222..94e4d1d41 100644 --- a/config/A3/A3_config.yaml +++ b/config/A3/A3_config.yaml @@ -6,58 +6,45 @@ image_topic: "/djiros/image" output_path: "/config/A3/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path #camera calibration -# model_type: MEI -# camera_name: camera -# image_width: 752 -# image_height: 480 -# mirror_parameters: -# xi: 2.5826025650890592e+00 -# distortion_parameters: -# k1: 3.9389417936108251e-01 -# k2: 2.2073276594959599e+00 -# p1: -1.9253757033376501e-03 -# p2: -6.1503758445210599e-04 -# projection_parameters: -# gamma1: 1.3068921176079280e+03 -# gamma2: 1.3065892394640514e+03 -# u0: 3.6959543908262725e+02 -# v0: 2.1891731132428231e+02 -model_type: PINHOLE +model_type: MEI camera_name: camera image_width: 752 image_height: 480 +mirror_parameters: + xi: 2.5826025650890592e+00 distortion_parameters: - k1: -2.727e-01 - k2: 6.375e-02 - p1: -5.850e-05 - p2: -9.779e-05 + k1: 3.9389417936108251e-01 + k2: 2.2073276594959599e+00 + p1: -1.9253757033376501e-03 + p2: -6.1503758445210599e-04 projection_parameters: - fx: 3.651e+02 - fy: 3.652e+02 - cx: 3.695e+02 - cy: 2.182e+02 + gamma1: 1.3068921176079280e+03 + gamma2: 1.3065892394640514e+03 + u0: 3.6959543908262725e+02 + v0: 2.1891731132428231e+02 + # Extrinsic parameter between IMU and Camera. -estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. +estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. ex_calib_result_path: "/config/A3/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path. #If you choose 0 or 1, you should write down the following matrix. #Rotation from camera frame to imu frame, imu^R_cam -# extrinsicRotation: !!opencv-matrix -# rows: 3 -# cols: 3 -# dt: d -# data: [ 0, -1, 0, -# 1, 0, 0, -# 0, 0, 1] +extrinsicRotation: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 0, -1, 0, + 1, 0, 0, + 0, 0, 1] -# #Translation from camera frame to imu frame, imu^T_cam -# extrinsicTranslation: !!opencv-matrix -# rows: 3 -# cols: 1 -# dt: d -# data: [ 0, 0.05, 0] +#Translation from camera frame to imu frame, imu^T_cam +extrinsicTranslation: !!opencv-matrix + rows: 3 + cols: 1 + dt: d + data: [ 0, 0.05, 0] #feature traker paprameters @@ -88,4 +75,4 @@ loop_closure: 1 #if you want to use loop closure to minimize the drift, set lo #also give the camera calibration file same as feature_tracker node pattern_file: "/support_files/brief_pattern.yml" voc_file: "/support_files/brief_k10L6.bin" -min_loop_num: 25 +min_loop_num: 30 diff --git a/vins_estimator/src/estimator.cpp b/vins_estimator/src/estimator.cpp index 594c91d7d..15521900b 100644 --- a/vins_estimator/src/estimator.cpp +++ b/vins_estimator/src/estimator.cpp @@ -52,6 +52,8 @@ void Estimator::clearState() solver_flag = INITIAL; initial_timestamp = 0; all_image_frame.clear(); + relocalize = false; + retrive_data_vector.clear(); if (tmp_pre_integration != nullptr) delete tmp_pre_integration; @@ -566,6 +568,21 @@ void Estimator::double2vector() dep(i) = para_Feature[i][0]; f_manager.setDepth(dep); + if (LOOP_CLOSURE && relocalize && retrive_data_vector[0].relative_pose) + { + + Matrix3d vio_loop_r; + Vector3d vio_loop_t; + vio_loop_r = rot_diff * Quaterniond(retrive_data_vector[0].loop_pose[6], retrive_data_vector[0].loop_pose[3], retrive_data_vector[0].loop_pose[4], retrive_data_vector[0].loop_pose[5]).normalized().toRotationMatrix(); + vio_loop_t = rot_diff * Vector3d(retrive_data_vector[0].loop_pose[0] - para_Pose[0][0], + retrive_data_vector[0].loop_pose[1] - para_Pose[0][1], + retrive_data_vector[0].loop_pose[2] - para_Pose[0][2]) + origin_P0; + double relocalize_yaw; + relocalize_yaw = Utility::R2ypr(retrive_data_vector[0].R_old).x() - Utility::R2ypr(vio_loop_r).x(); + relocalize_r = Utility::ypr2R(Vector3d(relocalize_yaw, 0, 0)); + relocalize_t = retrive_data_vector[0].P_old- relocalize_r * vio_loop_t; + } + } bool Estimator::failureDetection() @@ -688,71 +705,57 @@ void Estimator::optimization() f_m_cnt++; } } + relocalize = false; + //loop close factor if(LOOP_CLOSURE) { - //loop close factor - //front_pose.measurements.clear(); - if(front_pose.header != retrive_pose_data.header) - { - front_pose = retrive_pose_data; - } - if(!front_pose.measurements.empty()) - { - //the retrive pose is in the current window - if(front_pose.header >= Headers[0].stamp.toSec()) + int loop_constraint_num = 0; + for (int k = 0; k < (int)retrive_data_vector.size(); k++) + { + for(int i = 0; i < WINDOW_SIZE; i++) { - //tmp_retrive_pose_buf.push(front_pose); - for(int i = 0; i < WINDOW_SIZE; i++) + if(retrive_data_vector[k].header == Headers[i].stamp.toSec()) { - if(front_pose.header == Headers[i].stamp.toSec()) + relocalize = true; + ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); + problem.AddParameterBlock(retrive_data_vector[k].loop_pose, SIZE_POSE, local_parameterization); + loop_window_index = i; + loop_constraint_num++; + int retrive_feature_index = 0; + int feature_index = -1; + for (auto &it_per_id : f_manager.feature) { - for (int k = 0; k < 7; k++) - front_pose.loop_pose[k] = para_Pose[i][k]; - ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); - problem.AddParameterBlock(front_pose.loop_pose, SIZE_POSE, local_parameterization); - loop_window_index = i; - - int retrive_feature_index = 0; - int feature_index = -1; - int loop_factor_cnt = 0; - for (auto &it_per_id : f_manager.feature) - { - it_per_id.used_num = it_per_id.feature_per_frame.size(); - if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) - continue; - - ++feature_index; - int start = it_per_id.start_frame; - //feature has been obeserved in ith frame - int end = (start + it_per_id.feature_per_frame.size() - i - 1); - if(start <= i && end >=0) - { - while(front_pose.features_ids[retrive_feature_index] < it_per_id.feature_id) - { - retrive_feature_index++; - } - - if(front_pose.features_ids[retrive_feature_index] == it_per_id.feature_id) - { - Vector3d pts_j = Vector3d(front_pose.measurements[retrive_feature_index].x, front_pose.measurements[retrive_feature_index].y, 1.0); - Vector3d pts_i = it_per_id.feature_per_frame[0].point; - - ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j); - problem.AddResidualBlock(f, loss_function, para_Pose[start], front_pose.loop_pose, para_Ex_Pose[0], para_Feature[feature_index]); - - retrive_feature_index++; - loop_factor_cnt++; - } - + it_per_id.used_num = it_per_id.feature_per_frame.size(); + if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) + continue; + + ++feature_index; + int start = it_per_id.start_frame; + if(start <= i) + { + while(retrive_data_vector[k].features_ids[retrive_feature_index] < it_per_id.feature_id) + { + retrive_feature_index++; } - } + + if(retrive_data_vector[k].features_ids[retrive_feature_index] == it_per_id.feature_id) + { + Vector3d pts_j = Vector3d(retrive_data_vector[k].measurements[retrive_feature_index].x, retrive_data_vector[k].measurements[retrive_feature_index].y, 1.0); + Vector3d pts_i = it_per_id.feature_per_frame[0].point; + ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j); + problem.AddResidualBlock(f, loss_function, para_Pose[start], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]); + + retrive_feature_index++; + } + } } + } } } + ROS_DEBUG("loop constraint num: %d", loop_constraint_num); } - ROS_DEBUG("visual measurement count: %d", f_m_cnt); ROS_DEBUG("prepare for ceres: %f", t_prepare.toc()); @@ -776,22 +779,31 @@ void Estimator::optimization() ROS_DEBUG("Iterations : %d", static_cast(summary.iterations.size())); ROS_DEBUG("solver costs: %f", t_solver.toc()); - if(LOOP_CLOSURE) + // relative info between two loop frame + if(LOOP_CLOSURE && relocalize) { - for(int i = 0; i< WINDOW_SIZE; i++) + for (int k = 0; k < (int)retrive_data_vector.size(); k++) { - if(front_pose.header == Headers[i].stamp.toSec()) + for(int i = 0; i< WINDOW_SIZE; i++) { - Matrix3d Rs_i = Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix(); - Vector3d Ps_i = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]); - Matrix3d Rs_loop = Quaterniond(front_pose.loop_pose[6], front_pose.loop_pose[3], front_pose.loop_pose[4], front_pose.loop_pose[5]).normalized().toRotationMatrix(); - Vector3d Ps_loop = Vector3d( front_pose.loop_pose[0], front_pose.loop_pose[1], front_pose.loop_pose[2]); - - front_pose.relative_t = Rs_loop.transpose() * (Ps_i - Ps_loop); - front_pose.relative_q = Rs_loop.transpose() * Rs_i; - front_pose.relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs_i).x() - Utility::R2ypr(Rs_loop).x()); - } - } + if(retrive_data_vector[k].header == Headers[i].stamp.toSec()) + { + retrive_data_vector[k].relative_pose = true; + Matrix3d Rs_i = Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix(); + Vector3d Ps_i = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]); + Matrix3d Rs_loop = Quaterniond(retrive_data_vector[k].loop_pose[6], retrive_data_vector[k].loop_pose[3], retrive_data_vector[k].loop_pose[4], retrive_data_vector[k].loop_pose[5]).normalized().toRotationMatrix(); + Vector3d Ps_loop = Vector3d( retrive_data_vector[k].loop_pose[0], retrive_data_vector[k].loop_pose[1], retrive_data_vector[k].loop_pose[2]); + + retrive_data_vector[k].relative_t = Rs_loop.transpose() * (Ps_i - Ps_loop); + retrive_data_vector[k].relative_q = Rs_loop.transpose() * Rs_i; + retrive_data_vector[k].relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs_i).x() - Utility::R2ypr(Rs_loop).x()); + cout << "loop after cere " << Ps_loop.transpose() << endl; + if (abs(retrive_data_vector[k].relative_yaw) > 30.0 || retrive_data_vector[k].relative_t.norm() > 20.0) + retrive_data_vector[k].relative_pose = false; + + } + } + } } double2vector(); diff --git a/vins_estimator/src/estimator.h b/vins_estimator/src/estimator.h index 408440a90..0f7bc8389 100644 --- a/vins_estimator/src/estimator.h +++ b/vins_estimator/src/estimator.h @@ -28,12 +28,10 @@ struct RetriveData int cur_index; double header; Vector3d P_old; - Quaterniond Q_old; - Vector3d P_cur; - Quaterniond Q_cur; + Matrix3d R_old; vector measurements; vector features_ids; - bool use; + bool relative_pose; Vector3d relative_t; Quaterniond relative_q; double relative_yaw; @@ -128,7 +126,11 @@ class Estimator double para_Retrive_Pose[SIZE_POSE]; RetriveData retrive_pose_data, front_pose; + vector retrive_data_vector; int loop_window_index; + bool relocalize; + Vector3d relocalize_t; + Matrix3d relocalize_r; MarginalizationInfo *last_marginalization_info; vector last_marginalization_parameter_blocks; diff --git a/vins_estimator/src/estimator_node.cpp b/vins_estimator/src/estimator_node.cpp index 777ac13d5..2f3207cdf 100644 --- a/vins_estimator/src/estimator_node.cpp +++ b/vins_estimator/src/estimator_node.cpp @@ -26,6 +26,8 @@ queue imu_buf; queue feature_buf; std::mutex m_posegraph_buf; queue optimize_posegraph_buf; +queue keyframe_buf; +queue retrive_data_buf; int sum_of_wait = 0; @@ -35,6 +37,8 @@ std::mutex i_buf; std::mutex m_loop_drift; std::mutex m_keyframedatabase_resample; std::mutex m_update_visualization; +std::mutex m_keyframe_buf; +std::mutex m_retrive_data_buf; double latest_time; Eigen::Vector3d tmp_P; @@ -49,16 +53,14 @@ queue> image_buf; LoopClosure *loop_closure; KeyFrameDatabase keyframe_database; -int process_keyframe_cnt = 0; -int miss_keyframe_num = 0; -int keyframe_freq = 0; int global_frame_cnt = 0; -int loop_check_cnt = 0; //camera param camodocal::CameraPtr m_camera; vector erase_index; -Eigen::Vector3d loop_correct_t = Eigen::Vector3d(0, 0, 0); -Eigen::Matrix3d loop_correct_r = Eigen::Matrix3d::Identity(); +std_msgs::Header cur_header; +Eigen::Vector3d relocalize_t{Eigen::Vector3d(0, 0, 0)}; +Eigen::Matrix3d relocalize_r{Eigen::Matrix3d::Identity()}; + void predict(const sensor_msgs::ImuConstPtr &imu_msg) { @@ -96,8 +98,8 @@ void update() { TicToc t_predict; latest_time = current_time; - tmp_P = loop_correct_r * estimator.Ps[WINDOW_SIZE] + loop_correct_t; - tmp_Q = loop_correct_r * estimator.Rs[WINDOW_SIZE]; + tmp_P = relocalize_r * estimator.Ps[WINDOW_SIZE] + relocalize_t; + tmp_Q = relocalize_r * estimator.Rs[WINDOW_SIZE]; tmp_V = estimator.Vs[WINDOW_SIZE]; tmp_Ba = estimator.Bas[WINDOW_SIZE]; tmp_Bg = estimator.Bgs[WINDOW_SIZE]; @@ -164,6 +166,50 @@ void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) } } +void raw_image_callback(const sensor_msgs::ImageConstPtr &img_msg) +{ + cv_bridge::CvImagePtr img_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); + //image_pool[img_msg->header.stamp.toNSec()] = img_ptr->image; + if(LOOP_CLOSURE) + { + i_buf.lock(); + image_buf.push(make_pair(img_ptr->image, img_msg->header.stamp.toSec())); + i_buf.unlock(); + } +} + +void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg) +{ + m_buf.lock(); + feature_buf.push(feature_msg); + m_buf.unlock(); + con.notify_one(); +} + +void send_imu(const sensor_msgs::ImuConstPtr &imu_msg) +{ + double t = imu_msg->header.stamp.toSec(); + if (current_time < 0) + current_time = t; + double dt = t - current_time; + current_time = t; + + double ba[]{0.0, 0.0, 0.0}; + double bg[]{0.0, 0.0, 0.0}; + + double dx = imu_msg->linear_acceleration.x - ba[0]; + double dy = imu_msg->linear_acceleration.y - ba[1]; + double dz = imu_msg->linear_acceleration.z - ba[2]; + + double rx = imu_msg->angular_velocity.x - bg[0]; + double ry = imu_msg->angular_velocity.y - bg[1]; + double rz = imu_msg->angular_velocity.z - bg[2]; + //ROS_DEBUG("IMU %f, dt: %f, acc: %f %f %f, gyr: %f %f %f", t, dt, dx, dy, dz, rx, ry, rz); + + estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); +} + +//thread:loop detection void process_loop_detection() { if(loop_closure == NULL) @@ -179,65 +225,29 @@ void process_loop_detection() while(LOOP_CLOSURE) { - m_posegraph_buf.lock(); - int index = -1; - if (!optimize_posegraph_buf.empty()) + KeyFrame* cur_kf = NULL; + m_keyframe_buf.lock(); + while(!keyframe_buf.empty()) { - index = optimize_posegraph_buf.front(); - optimize_posegraph_buf.pop(); + if(cur_kf!=NULL) + delete cur_kf; + cur_kf = keyframe_buf.front(); + keyframe_buf.pop(); } - m_posegraph_buf.unlock(); - if(index != -1) + m_keyframe_buf.unlock(); + if (cur_kf != NULL) { - Vector3d correct_t = Vector3d::Zero(); - Matrix3d correct_r = Matrix3d::Identity(); - TicToc t_posegraph; - keyframe_database.optimize4DoFLoopPoseGraph(index, - correct_t, - correct_r); - ROS_DEBUG("t_posegraph %f ms", t_posegraph.toc()); - m_loop_drift.lock(); - loop_correct_r = correct_r; - loop_correct_t = correct_t; - m_loop_drift.unlock(); - m_update_visualization.lock(); - keyframe_database.updateVisualization(); - m_update_visualization.unlock(); - nav_msgs::Path refine_path = keyframe_database.getPath(); - updateLoopPath(refine_path); - } - - if (loop_check_cnt < global_frame_cnt) - { - erase_index.clear(); + cur_kf->global_index = global_frame_cnt; + cur_kf->buildKeyFrameFeatures(estimator, m_camera); m_keyframedatabase_resample.lock(); - keyframe_database.resample(erase_index); + keyframe_database.add(cur_kf); m_keyframedatabase_resample.unlock(); - - m_update_visualization.lock(); - keyframe_database.updateVisualization(); - m_update_visualization.unlock(); - - if(!erase_index.empty()) - loop_closure->eraseIndex(erase_index); - - KeyFrame* cur_kf = keyframe_database.getLastUncheckKeyframe(); - assert(loop_check_cnt == cur_kf->global_index); - loop_check_cnt++; - cur_kf->check_loop = 1; cv::Mat current_image; - current_image = cur_kf->image; - - std::vector measurements_old; - std::vector measurements_old_norm; - std::vector measurements_cur; - std::vector features_id; - std::vector measurements_cur_origin = cur_kf->measurements; + current_image = cur_kf->image; bool loop_succ = false; int old_index = -1; - TicToc t_loop; vector cur_pts; vector old_pts; TicToc t_brief; @@ -245,7 +255,8 @@ void process_loop_detection() //printf("loop extract %d feature using %lf\n", cur_kf->keypoints.size(), t_brief.toc()); TicToc t_loopdetect; loop_succ = loop_closure->startLoopClosure(cur_kf->keypoints, cur_kf->descriptors, cur_pts, old_pts, old_index); - ROS_DEBUG("t_loopdetect %f ms", t_loopdetect.toc()); + double t_loop = t_loopdetect.toc(); + ROS_DEBUG("t_loopdetect %f ms", t_loop); if(loop_succ) { KeyFrame* old_kf = keyframe_database.getKeyframe(old_index); @@ -254,48 +265,59 @@ void process_loop_detection() ROS_WARN("NO such frame in keyframe_database"); ROS_BREAK(); } - ROS_DEBUG("loop succ %d with %drd image", loop_check_cnt, old_index); + ROS_DEBUG("loop succ %d with %drd image", global_frame_cnt, old_index); assert(old_index!=-1); - Vector3d T_w_i_old, T_w_i_refine; - Matrix3d R_w_i_old, R_w_i_refine; - - old_kf->getOriginPose(T_w_i_old, R_w_i_old); - cur_kf->findConnectionWithOldFrame(old_kf, cur_pts, old_pts, - measurements_old, measurements_old_norm, m_camera); - measurements_cur = cur_kf->measurements; - features_id = cur_kf->features_id; - - /** - *** send features and ids to VINS - **/ - //if(measurements_old_norm.size()>MIN_LOOP_NUM && process_keyframe_cnt - old_index > 50 && old_index > 50) + Vector3d T_w_i_old, T_w_i_cur; + Matrix3d R_w_i_old, R_w_i_cur; + + old_kf->getPose(T_w_i_old, R_w_i_old); + cur_kf->getOriginPose(T_w_i_cur, R_w_i_cur); + std::vector measurements_old; + std::vector measurements_old_norm; + std::vector measurements_cur; + std::vector features_id_matched; + cur_kf->findConnectionWithOldFrame(old_kf, measurements_old, measurements_old_norm, features_id_matched, m_camera); + measurements_cur = cur_kf->measurements_matched; + + // send loop info to VINS relocalization int loop_fusion = 0; - if( (int)measurements_old_norm.size() > MIN_LOOP_NUM && loop_check_cnt - old_index > 35 && old_index > 30) + if( (int)measurements_old_norm.size() > MIN_LOOP_NUM && global_frame_cnt - old_index > 35 && old_index > 30) { Quaterniond Q_loop_old(R_w_i_old); - Quaterniond Q_loop_cur(R_w_i_refine); + Quaterniond Q_cur(R_w_i_old); RetriveData retrive_data; retrive_data.cur_index = cur_kf->global_index; retrive_data.header = cur_kf->header; retrive_data.P_old = T_w_i_old; - retrive_data.Q_old = Q_loop_old; - retrive_data.P_cur = T_w_i_refine; - retrive_data.Q_cur = Q_loop_cur; - retrive_data.use = true; + retrive_data.R_old = R_w_i_old; + retrive_data.relative_pose = false; retrive_data.measurements = measurements_old_norm; - retrive_data.features_ids = features_id; - estimator.retrive_pose_data = (retrive_data); - - //cout << "old pose " << T_w_i_old.transpose() << endl; - //cout << "refinded pose " << T_w_i_refine.transpose() << endl; - // add loop edge in current frame + retrive_data.features_ids = features_id_matched; + retrive_data.loop_pose[0] = T_w_i_cur.x(); + retrive_data.loop_pose[1] = T_w_i_cur.y(); + retrive_data.loop_pose[2] = T_w_i_cur.z(); + retrive_data.loop_pose[3] = Q_cur.x(); + retrive_data.loop_pose[4] = Q_cur.y(); + retrive_data.loop_pose[5] = Q_cur.z(); + retrive_data.loop_pose[6] = Q_cur.w(); + m_retrive_data_buf.lock(); + retrive_data_buf.push(retrive_data); + m_retrive_data_buf.unlock(); cur_kf->detectLoop(old_index); - //keyframe_database.addLoop(old_index); old_kf->is_looped = 1; loop_fusion = 1; + + m_update_visualization.lock(); + keyframe_database.addLoop(old_index); + CameraPoseVisualization* posegraph_visualization = keyframe_database.getPosegraphVisualization(); + pubPoseGraph(posegraph_visualization, cur_header); + m_update_visualization.unlock(); } + + + // visualization loop info if(0) { int COL = current_image.cols; @@ -306,7 +328,7 @@ void process_loop_detection() cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); cv::Mat loop_match_img2; loop_match_img2 = loop_match_img.clone(); - + for(int i = 0; i< (int)cur_pts.size(); i++) { cv::Point2f cur_pt = cur_pts[i]; @@ -328,8 +350,6 @@ void process_loop_detection() << cur_kf->global_index << "-" << old_index << "-" << loop_fusion <<".jpg"; cv::imwrite( convert.str().c_str(), loop_match_img); - - for(int i = 0; i< (int)measurements_cur.size(); i++) { @@ -357,56 +377,65 @@ void process_loop_detection() } //release memory cur_kf->image.release(); - } + global_frame_cnt++; + if (t_loop > 300 || keyframe_database.size() > MAX_KEYFRAME_NUM) + { + m_keyframedatabase_resample.lock(); + erase_index.clear(); + keyframe_database.downsample(erase_index); + m_keyframedatabase_resample.unlock(); + if(!erase_index.empty()) + loop_closure->eraseIndex(erase_index); + } + } std::chrono::milliseconds dura(10); std::this_thread::sleep_for(dura); } } -void raw_image_callback(const sensor_msgs::ImageConstPtr &img_msg) +//thread: pose_graph optimization +void process_pose_graph() { - cv_bridge::CvImagePtr img_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); - //image_pool[img_msg->header.stamp.toNSec()] = img_ptr->image; - if(LOOP_CLOSURE) + while(true) { - i_buf.lock(); - image_buf.push(make_pair(img_ptr->image, img_msg->header.stamp.toSec())); - i_buf.unlock(); - } -} - -void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg) -{ - m_buf.lock(); - feature_buf.push(feature_msg); - m_buf.unlock(); - con.notify_one(); -} - -void send_imu(const sensor_msgs::ImuConstPtr &imu_msg) -{ - double t = imu_msg->header.stamp.toSec(); - if (current_time < 0) - current_time = t; - double dt = t - current_time; - current_time = t; - - double ba[]{0.0, 0.0, 0.0}; - double bg[]{0.0, 0.0, 0.0}; - - double dx = imu_msg->linear_acceleration.x - ba[0]; - double dy = imu_msg->linear_acceleration.y - ba[1]; - double dz = imu_msg->linear_acceleration.z - ba[2]; - - double rx = imu_msg->angular_velocity.x - bg[0]; - double ry = imu_msg->angular_velocity.y - bg[1]; - double rz = imu_msg->angular_velocity.z - bg[2]; - //ROS_DEBUG("IMU %f, dt: %f, acc: %f %f %f, gyr: %f %f %f", t, dt, dx, dy, dz, rx, ry, rz); + m_posegraph_buf.lock(); + int index = -1; + while (!optimize_posegraph_buf.empty()) + { + index = optimize_posegraph_buf.front(); + optimize_posegraph_buf.pop(); + } + m_posegraph_buf.unlock(); + if(index != -1) + { + Vector3d correct_t = Vector3d::Zero(); + Matrix3d correct_r = Matrix3d::Identity(); + TicToc t_posegraph; + keyframe_database.optimize4DoFLoopPoseGraph(index, + correct_t, + correct_r); + ROS_DEBUG("t_posegraph %f ms", t_posegraph.toc()); + m_loop_drift.lock(); + relocalize_r = correct_r; + relocalize_t = correct_t; + m_loop_drift.unlock(); + m_update_visualization.lock(); + keyframe_database.updateVisualization(); + CameraPoseVisualization* posegraph_visualization = keyframe_database.getPosegraphVisualization(); + m_update_visualization.unlock(); + pubOdometry(estimator, cur_header, relocalize_t, relocalize_r); + pubPoseGraph(posegraph_visualization, cur_header); + nav_msgs::Path refine_path = keyframe_database.getPath(); + updateLoopPath(refine_path); + } - estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); + std::chrono::milliseconds dura(3000); + std::this_thread::sleep_for(dura); + } } +// thread: visual-inertial odometry void process() { while (true) @@ -440,119 +469,98 @@ void process() ROS_ASSERT(z == 1); image[feature_id].emplace_back(camera_id, Vector3d(x, y, z)); } - estimator.processImage(image, img_msg->header); /** *** start build keyframe database for loop closure **/ if(LOOP_CLOSURE) { + // remove previous loop + vector::iterator it = estimator.retrive_data_vector.begin(); + for(; it != estimator.retrive_data_vector.end(); ) + { + if ((*it).header < estimator.Headers[0].stamp.toSec()) + { + it = estimator.retrive_data_vector.erase(it); + } + else + it++; + } + m_retrive_data_buf.lock(); + while(!retrive_data_buf.empty()) + { + RetriveData tmp_retrive_data = retrive_data_buf.front(); + retrive_data_buf.pop(); + estimator.retrive_data_vector.push_back(tmp_retrive_data); + } + m_retrive_data_buf.unlock(); + //WINDOW_SIZE - 2 is key frame if(estimator.marginalization_flag == 0 && estimator.solver_flag == estimator.NON_LINEAR) { - if (keyframe_freq % 3 == 0) + Vector3d T_w_i = estimator.Ps[WINDOW_SIZE - 2]; + Matrix3d R_w_i = estimator.Rs[WINDOW_SIZE - 2]; + i_buf.lock(); + while(!image_buf.empty() && image_buf.front().second < estimator.Headers[WINDOW_SIZE - 2].stamp.toSec()) { - keyframe_freq = 0; - /** - ** save the newest keyframe to the keyframe database - ** only need to save the pose to the keyframe database - **/ - Vector3d T_w_i = estimator.Ps[WINDOW_SIZE - 2]; - Matrix3d R_w_i = estimator.Rs[WINDOW_SIZE - 2]; - i_buf.lock(); - while(!image_buf.empty() && image_buf.front().second < estimator.Headers[WINDOW_SIZE - 2].stamp.toSec()) - { - image_buf.pop(); - } - i_buf.unlock(); - //assert(estimator.Headers[WINDOW_SIZE - 1].stamp.toSec() == image_buf.front().second); - // relative_T i-1_T_i relative_R i-1_R_i - cv::Mat KeyFrame_image; - KeyFrame_image = image_buf.front().first; - - const char *pattern_file = PATTERN_FILE.c_str(); - KeyFrame* keyframe = new KeyFrame(estimator.Headers[WINDOW_SIZE - 2].stamp.toSec(), global_frame_cnt, T_w_i, R_w_i, image_buf.front().first, pattern_file); - keyframe->setExtrinsic(TIC[0], RIC[0]); - /* - ** we still need save the measurement to the keyframe(not database) for add connection with looped old pose - ** and save the pointcloud to the keyframe for reprojection search correspondance - */ - keyframe->buildKeyFrameFeatures(estimator, m_camera); - m_keyframedatabase_resample.lock(); - keyframe_database.add(keyframe); - m_keyframedatabase_resample.unlock(); - global_frame_cnt++; + image_buf.pop(); } - + i_buf.unlock(); + //assert(estimator.Headers[WINDOW_SIZE - 1].stamp.toSec() == image_buf.front().second); + // relative_T i-1_T_i relative_R i-1_R_i + cv::Mat KeyFrame_image; + KeyFrame_image = image_buf.front().first; + + const char *pattern_file = PATTERN_FILE.c_str(); + Vector3d cur_T; + Matrix3d cur_R; + cur_T = relocalize_r * T_w_i + relocalize_t; + cur_R = relocalize_r * R_w_i; + KeyFrame* keyframe = new KeyFrame(estimator.Headers[WINDOW_SIZE - 2].stamp.toSec(), T_w_i, R_w_i, cur_T, cur_R, image_buf.front().first, pattern_file); + keyframe->setExtrinsic(TIC[0], RIC[0]); + m_keyframe_buf.lock(); + keyframe_buf.push(keyframe); + m_keyframe_buf.unlock(); // update loop info - m_keyframedatabase_resample.lock(); - for (int i = 0; i < WINDOW_SIZE; i++) + if (!estimator.retrive_data_vector.empty() && estimator.retrive_data_vector[0].relative_pose) { - if(estimator.Headers[i].stamp.toSec() == estimator.front_pose.header) + if(estimator.Headers[0].stamp.toSec() == estimator.retrive_data_vector[0].header) { - KeyFrame* cur_kf = keyframe_database.getKeyframe(estimator.front_pose.cur_index); - if (abs(estimator.front_pose.relative_yaw) > 30.0 || estimator.front_pose.relative_t.norm() > 20.0) + KeyFrame* cur_kf = keyframe_database.getKeyframe(estimator.retrive_data_vector[0].cur_index); + if (abs(estimator.retrive_data_vector[0].relative_yaw) > 30.0 || estimator.retrive_data_vector[0].relative_t.norm() > 20.0) { ROS_DEBUG("Wrong loop"); cur_kf->removeLoop(); - break; } - cur_kf->updateLoopConnection( estimator.front_pose.relative_t, - estimator.front_pose.relative_q, - estimator.front_pose.relative_yaw); - break; - } - } - /* - ** update the keyframe pose when this frame slides out the window and optimize loop graph - */ - int search_cnt = 0; - - for(int i = 0; i < keyframe_database.size(); i++) - { - search_cnt++; - KeyFrame* kf = keyframe_database.getLastKeyframe(i); - if(kf->header == estimator.Headers[0].stamp.toSec()) - { - kf->updateOriginPose(estimator.Ps[0], estimator.Rs[0]); - //update edge - // if loop happens in this frame, update pose graph; - if (kf->update_loop_info) + else { + cur_kf->updateLoopConnection( estimator.retrive_data_vector[0].relative_t, + estimator.retrive_data_vector[0].relative_q, + estimator.retrive_data_vector[0].relative_yaw); m_posegraph_buf.lock(); - optimize_posegraph_buf.push(kf->global_index); + optimize_posegraph_buf.push(estimator.retrive_data_vector[0].cur_index); m_posegraph_buf.unlock(); } - break; - } - else - { - if(search_cnt > 2 * WINDOW_SIZE) - break; } } - m_keyframedatabase_resample.unlock(); - keyframe_freq++; } - std_msgs::Header header = img_msg->header; - m_update_visualization.lock(); - CameraPoseVisualization* posegraph_visualization = keyframe_database.getPosegraphVisualization(); - //ROS_WARN("pub visualization begin!"); - pubPoseGraph(posegraph_visualization, header); - //ROS_WARN("pub visualization end!"); - m_update_visualization.unlock(); } double whole_t = t_s.toc(); printStatistics(estimator, whole_t); std_msgs::Header header = img_msg->header; header.frame_id = "world"; - pubOdometry(estimator, header, loop_correct_t, loop_correct_r); + cur_header = header; m_loop_drift.lock(); - pubKeyPoses(estimator, header, loop_correct_t, loop_correct_r); - pubCameraPose(estimator, header, loop_correct_t, loop_correct_r); - pubPointCloud(estimator, header, loop_correct_t, loop_correct_r); - pubTF(estimator, header, loop_correct_t, loop_correct_r); + if (estimator.relocalize) + { + relocalize_t = estimator.relocalize_t; + relocalize_r = estimator.relocalize_r; + } + pubOdometry(estimator, header, relocalize_t, relocalize_r); + pubKeyPoses(estimator, header, relocalize_t, relocalize_r); + pubCameraPose(estimator, header, relocalize_t, relocalize_r); + pubPointCloud(estimator, header, relocalize_t, relocalize_r); + pubTF(estimator, header, relocalize_t, relocalize_r); m_loop_drift.unlock(); - //ROS_ERROR("end: %f, at %f", img_msg->header.stamp.toSec(), ros::Time::now().toSec()); } m_buf.lock(); @@ -583,11 +591,12 @@ int main(int argc, char **argv) ros::Subscriber sub_raw_image = n.subscribe(IMAGE_TOPIC, 2000, raw_image_callback); std::thread measurement_process{process}; - std::thread loop_detection; + std::thread loop_detection, pose_graph; if (LOOP_CLOSURE) { ROS_WARN("LOOP_CLOSURE true"); loop_detection = std::thread(process_loop_detection); + pose_graph = std::thread(process_pose_graph); m_camera = CameraFactory::instance()->generateCameraFromYamlFile(CAM_NAMES); } ros::spin(); diff --git a/vins_estimator/src/loop-closure/keyframe.cpp b/vins_estimator/src/loop-closure/keyframe.cpp index 44e67879e..acec84d68 100644 --- a/vins_estimator/src/loop-closure/keyframe.cpp +++ b/vins_estimator/src/loop-closure/keyframe.cpp @@ -1,20 +1,20 @@ #include "keyframe.h" -KeyFrame::KeyFrame(double _header, int _global_index, Eigen::Vector3d _T_w_i, Eigen::Matrix3d _R_w_i, +KeyFrame::KeyFrame(double _header, Eigen::Vector3d _vio_T_w_i, Eigen::Matrix3d _vio_R_w_i, + Eigen::Vector3d _cur_T_w_i, Eigen::Matrix3d _cur_R_w_i, cv::Mat &_image, const char *_brief_pattern_file) -:header{_header}, global_index{_global_index}, image{_image}, BRIEF_PATTERN_FILE(_brief_pattern_file) +:header{_header}, image{_image}, BRIEF_PATTERN_FILE(_brief_pattern_file) { - T_w_i = _T_w_i; - R_w_i = _R_w_i; + T_w_i = _cur_T_w_i; + R_w_i = _cur_R_w_i; COL = image.cols; ROW = image.rows; use_retrive = false; is_looped = 0; has_loop = 0; update_loop_info = 0; - origin_T_w_i = _T_w_i; - origin_R_w_i = _R_w_i; - check_loop = 0; + origin_T_w_i = _vio_T_w_i; + origin_R_w_i = _vio_R_w_i; } /*****************************************utility function************************************************/ @@ -36,20 +36,20 @@ static void reduceVector(vector &v, vector status) v.resize(j); } -void KeyFrame::rejectWithF(vector &measurements_old, - vector &measurements_old_norm, +void KeyFrame::rejectWithF(vector &measurements_cur, vector &measurements_old, + vector &measurements_old_norm, vector &features_id_matched, const camodocal::CameraPtr &m_camera) { if (measurements_old.size() >= 8) { measurements_old_norm.clear(); - vector un_measurements(measurements.size()), un_measurements_old(measurements_old.size()); - for (int i = 0; i < (int)measurements.size(); i++) + vector un_measurements(measurements_cur.size()), un_measurements_old(measurements_old.size()); + for (int i = 0; i < (int)measurements_cur.size(); i++) { double FOCAL_LENGTH = 460.0; Eigen::Vector3d tmp_p; - m_camera->liftProjective(Eigen::Vector2d(measurements[i].x, measurements[i].y), tmp_p); + m_camera->liftProjective(Eigen::Vector2d(measurements_cur[i].x, measurements_cur[i].y), tmp_p); tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0; tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0; un_measurements[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); @@ -63,11 +63,10 @@ void KeyFrame::rejectWithF(vector &measurements_old, vector status; cv::findFundamentalMat(un_measurements, un_measurements_old, cv::FM_RANSAC, 1.0, 0.99, status); - reduceVector(point_clouds, status); - reduceVector(measurements, status); reduceVector(measurements_old, status); reduceVector(measurements_old_norm, status); - reduceVector(features_id, status); + reduceVector(measurements_matched, status); + reduceVector(features_id_matched, status); } } /*****************************************utility function************************************************/ @@ -106,16 +105,9 @@ void KeyFrame::buildKeyFrameFeatures(Estimator &estimator, const camodocal::Came m_camera->spaceToPlane(point, point_uv); measurements.push_back(cv::Point2f(point_uv.x(), point_uv.y())); pts_normalize.push_back(cv::Point2f(point.x()/point.z(), point.y()/point.z())); - features_id.push_back(it_per_id.feature_id); - //features 3D pos from first measurement and inverse depth - Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth; - point_clouds.push_back(estimator.Rs[it_per_id.start_frame] * (qic * pts_i + tic) + estimator.Ps[it_per_id.start_frame]); } } - measurements_origin = measurements; - point_clouds_origin = point_clouds; - features_id_origin = features_id; } /** @@ -163,11 +155,11 @@ bool KeyFrame::searchInAera(cv::Point2f center_cur, float area_size, return false; } -void KeyFrame::searchByDes(const Eigen::Vector3d T_w_i_old, const Eigen::Matrix3d R_w_i_old, - std::vector &measurements_old, +void KeyFrame::searchByDes(std::vector &measurements_old, std::vector &measurements_old_norm, const std::vector &descriptors_old, const std::vector &keypoints_old, + std::vector &features_id_matched, const camodocal::CameraPtr &m_camera) { //ROS_INFO("loop_match before cur %d %d, old %d", (int)window_descriptors.size(), (int)measurements.size(), (int)descriptors_old.size()); @@ -181,29 +173,21 @@ void KeyFrame::searchByDes(const Eigen::Vector3d T_w_i_old, const Eigen::Matrix3 status.push_back(0); measurements_old.push_back(pt); } - reduceVector(measurements, status); + measurements_matched = measurements; + features_id_matched = features_id; reduceVector(measurements_old, status); - reduceVector(features_id, status); - reduceVector(point_clouds, status); - reduceVector(measurements_old_norm, status); - - rejectWithF(measurements_old, measurements_old_norm, m_camera); - //rejectWithF(measurements_old, measurements_old_norm, m_camera); + reduceVector(measurements_matched, status); + reduceVector(features_id_matched, status); + rejectWithF(measurements_matched, measurements_old, measurements_old_norm, features_id_matched, m_camera); //ROS_INFO("loop_match after cur %d %d, old %d\n", (int)window_descriptors.size(), (int)measurements.size(), (int)descriptors_old.size()); } -/** -*** interface to VINS -*** input: looped old keyframe which include image and pose, and feature correnspondance given by BoW -*** output: ordered old feature correspondance with current KeyFrame and the translation drift -**/ bool KeyFrame::findConnectionWithOldFrame(const KeyFrame* old_kf, - const std::vector &cur_pts, const std::vector &old_pts, std::vector &measurements_old, std::vector &measurements_old_norm, - const camodocal::CameraPtr &m_camera) + std::vector &features_id_matched, const camodocal::CameraPtr &m_camera) { TicToc t_match; - searchByDes(old_kf->T_w_i, old_kf->R_w_i, measurements_old, measurements_old_norm, old_kf->descriptors, old_kf->keypoints, m_camera); + searchByDes(measurements_old, measurements_old_norm, old_kf->descriptors, old_kf->keypoints, features_id_matched, m_camera); ROS_DEBUG("loop final use num %d %lf---------------", (int)measurements_old.size(), t_match.toc()); return true; diff --git a/vins_estimator/src/loop-closure/keyframe.h b/vins_estimator/src/loop-closure/keyframe.h index 7e8473158..6330d687d 100644 --- a/vins_estimator/src/loop-closure/keyframe.h +++ b/vins_estimator/src/loop-closure/keyframe.h @@ -33,10 +33,11 @@ class BriefExtractor: public FeatureExtractor class KeyFrame { public: - KeyFrame(double _header, int _global_index, Eigen::Vector3d _T_w_c, Eigen::Matrix3d _R_w_c, cv::Mat &_image, const char *_brief_pattern_file); + KeyFrame(double _header, Eigen::Vector3d _vio_T_w_c, Eigen::Matrix3d _vio_R_w_c, + Eigen::Vector3d _cur_T_w_c, Eigen::Matrix3d _cur_R_w_c,cv::Mat &_image, const char *_brief_pattern_file); void setExtrinsic(Eigen::Vector3d T, Eigen::Matrix3d R); - void rejectWithF(vector &measurements_old, - vector &measurements_old_norm, + void rejectWithF(vector &measurements_cur, vector &measurements_old, + vector &measurements_old_norm, vector &features_id_matched, const camodocal::CameraPtr &m_camera); void extractBrief(cv::Mat &image); @@ -51,17 +52,16 @@ class KeyFrame const std::vector &keypoints_old, cv::Point2f &best_match); - void searchByDes(const Eigen::Vector3d T_w_i_old, const Eigen::Matrix3d R_w_i_old, - std::vector &measurements_old, + void searchByDes( std::vector &measurements_old, std::vector &measurements_old_norm, const std::vector &descriptors_old, const std::vector &keypoints_old, + std::vector &features_id_matched, const camodocal::CameraPtr &m_camera); bool findConnectionWithOldFrame(const KeyFrame* old_kf, - const std::vector &cur_pts, const std::vector &old_pts, std::vector &measurements_old, std::vector &measurements_old_norm, - const camodocal::CameraPtr &m_camera); + std::vector &features_id_matched, const camodocal::CameraPtr &m_camera); void updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i); @@ -89,13 +89,12 @@ class KeyFrame // data double header; - std::vector point_clouds, point_clouds_origin; //feature in origin image plane - std::vector measurements, measurements_origin; + std::vector measurements, measurements_matched; //feature in normalize image plane std::vector pts_normalize; //feature ID - std::vector features_id, features_id_origin; + std::vector features_id; //feature descriptor std::vector descriptors; //keypoints @@ -114,8 +113,6 @@ class KeyFrame // index t_x t_y t_z q_w q_x q_y q_z yaw // old_R_cur old_T_cur - - bool check_loop; // looped by other frame bool is_looped; int resample_index; diff --git a/vins_estimator/src/loop-closure/keyframe_database.cpp b/vins_estimator/src/loop-closure/keyframe_database.cpp index bb850dfe6..197d21d7e 100644 --- a/vins_estimator/src/loop-closure/keyframe_database.cpp +++ b/vins_estimator/src/loop-closure/keyframe_database.cpp @@ -8,7 +8,6 @@ KeyFrameDatabase::KeyFrameDatabase() t_drift = Eigen::Vector3d(0, 0, 0); yaw_drift = 0; r_drift = Eigen::Matrix3d::Identity(); - max_frame_num = 400; total_length = 0; last_P = Eigen::Vector3d(0, 0, 0); } @@ -21,9 +20,6 @@ void KeyFrameDatabase::add(KeyFrame* pKF) Vector3d P; Matrix3d R; pKF->getPose(P, R); - P = r_drift * P + t_drift; - R = r_drift * R; - pKF->updatePose(P, R); Quaterniond Q; Q = R; @@ -79,16 +75,15 @@ void KeyFrameDatabase::add(KeyFrame* pKF) } -void KeyFrameDatabase::resample(vector &erase_index) +void KeyFrameDatabase::downsample(vector &erase_index) { ROS_DEBUG("resample keyframe begin!"); unique_lock lock(mMutexkeyFrameList); - if ((int)keyFrameList.size() < max_frame_num) - return; + int frame_num = (int)keyFrameList.size(); if (mOptimiazationPosegraph.try_lock()) { erase_index.clear(); - double min_dis = total_length / (max_frame_num * 0.7); + double min_dis = total_length / (frame_num * 0.7); list::iterator it = keyFrameList.begin(); Vector3d last_P = Vector3d(0, 0, 0); @@ -98,7 +93,7 @@ void KeyFrameDatabase::resample(vector &erase_index) Matrix3d tmp_r; (*it)->getPose(tmp_t, tmp_r); double dis = (tmp_t - last_P).norm(); - if(it == keyFrameList.begin() || dis > min_dis || (*it)->update_loop_info || (*it)->is_looped || !(*it)->check_loop) + if(it == keyFrameList.begin() || dis > min_dis || (*it)->has_loop || (*it)->is_looped) { last_P = tmp_t; it++; @@ -179,18 +174,6 @@ KeyFrame* KeyFrameDatabase::getLastKeyframe(int last_index) return *rit; } -KeyFrame* KeyFrameDatabase::getLastUncheckKeyframe() -{ - unique_lock lock(mMutexkeyFrameList); - list::reverse_iterator rit = keyFrameList.rbegin(); - for (; rit != keyFrameList.rend(); rit++) - { - if ((*rit)->check_loop == 1) - break; - } - assert(rit != keyFrameList.rbegin()); - return *(--rit); -} void KeyFrameDatabase::optimize4DoFLoopPoseGraph(int cur_index, Eigen::Vector3d &loop_correct_t, Eigen::Matrix3d &loop_correct_r) { ROS_DEBUG("optimizae pose graph begin!"); @@ -273,7 +256,6 @@ void KeyFrameDatabase::optimize4DoFLoopPoseGraph(int cur_index, Eigen::Vector3d //add loop edge if((*it)->update_loop_info) { - int connected_index = getKeyframe((*it)->loop_index)->resample_index; assert((*it)->loop_index >= earliest_loop_index); Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix()); @@ -294,7 +276,7 @@ void KeyFrameDatabase::optimize4DoFLoopPoseGraph(int cur_index, Eigen::Vector3d } ceres::Solve(options, &problem, &summary); - std::cout << summary.BriefReport() << "\n"; + //std::cout << summary.BriefReport() << "\n"; i = 0; for (it = keyFrameList.begin(); it != keyFrameList.end(); it++) @@ -337,6 +319,7 @@ void KeyFrameDatabase::optimize4DoFLoopPoseGraph(int cur_index, Eigen::Vector3d void KeyFrameDatabase::updateVisualization() { + ROS_DEBUG("updateVisualization begin"); unique_lock mlockPath(mPath); unique_lock mlockPosegraph(mPosegraphVisualization); total_length = 0; @@ -399,9 +382,6 @@ void KeyFrameDatabase::updateVisualization() Matrix3d R_previous; (*lit)->getPose(P_previous, R_previous); posegraph_visualization->add_loopedge(P, P_previous); - - - } // add key frame to path for visualization @@ -423,13 +403,16 @@ void KeyFrameDatabase::updateVisualization() refine_path.poses.push_back(pose_stamped); } + ROS_DEBUG("updateVisualization end"); } void KeyFrameDatabase::addLoop(int loop_index) { unique_lock lock(mPosegraphVisualization); - KeyFrame* cur_KF = getLastKeyframe(); + if (earliest_loop_index > loop_index || earliest_loop_index == -1) + earliest_loop_index = loop_index; + KeyFrame* cur_KF = getLastKeyframe(); KeyFrame* connected_KF = getKeyframe(loop_index); Vector3d conncected_P, P; Matrix3d connected_R, R; diff --git a/vins_estimator/src/loop-closure/keyframe_database.h b/vins_estimator/src/loop-closure/keyframe_database.h index c6f334638..8b2351328 100644 --- a/vins_estimator/src/loop-closure/keyframe_database.h +++ b/vins_estimator/src/loop-closure/keyframe_database.h @@ -17,14 +17,13 @@ class KeyFrameDatabase public: KeyFrameDatabase(); void add(KeyFrame* pKF); - void resample(vector &erase_index); + void downsample(vector &erase_index); void erase(KeyFrame* pKF); int size(); void optimize4DoFLoopPoseGraph(int cur_index, Eigen::Vector3d &loop_correct_t, Eigen::Matrix3d &loop_correct_r); KeyFrame* getKeyframe(int index); KeyFrame* getLastKeyframe(); KeyFrame* getLastKeyframe(int last_index); - KeyFrame* getLastUncheckKeyframe(); void getKeyframeIndexList(vector &keyframe_index_list); void updateVisualization(); void addLoop(int loop_index); @@ -41,7 +40,6 @@ class KeyFrameDatabase Vector3d t_drift; double yaw_drift; Matrix3d r_drift; - int max_frame_num; double total_length; Vector3d last_P; nav_msgs::Path refine_path; diff --git a/vins_estimator/src/parameters.cpp b/vins_estimator/src/parameters.cpp index 60aa3d198..0a74af7b3 100644 --- a/vins_estimator/src/parameters.cpp +++ b/vins_estimator/src/parameters.cpp @@ -26,6 +26,7 @@ std::string IMAGE_TOPIC; std::string IMU_TOPIC; int IMAGE_ROW, IMAGE_COL; std::string VINS_FOLDER_PATH; +int MAX_KEYFRAME_NUM; template T readParam(ros::NodeHandle &n, std::string name) @@ -130,6 +131,7 @@ void readParameters(ros::NodeHandle &n) INIT_DEPTH = 5.0; BIAS_ACC_THRESHOLD = 0.1; BIAS_GYR_THRESHOLD = 0.1; - + MAX_KEYFRAME_NUM = 1000; + fsSettings.release(); } diff --git a/vins_estimator/src/parameters.h b/vins_estimator/src/parameters.h index e5737ae93..29e73e9e0 100644 --- a/vins_estimator/src/parameters.h +++ b/vins_estimator/src/parameters.h @@ -38,6 +38,7 @@ extern std::string VINS_FOLDER_PATH; extern int LOOP_CLOSURE; extern int MIN_LOOP_NUM; +extern int MAX_KEYFRAME_NUM; extern std::string PATTERN_FILE; extern std::string VOC_FILE; extern std::string CAM_NAMES; diff --git a/vins_estimator/src/utility/visualization.cpp b/vins_estimator/src/utility/visualization.cpp index 69a074300..c8cc9e64c 100644 --- a/vins_estimator/src/utility/visualization.cpp +++ b/vins_estimator/src/utility/visualization.cpp @@ -17,7 +17,7 @@ static Vector3d last_path(0.0, 0.0, 0.0); void registerPub(ros::NodeHandle &n) { - pub_latest_odometry = n.advertise("imu_propogate", 1000); + pub_latest_odometry = n.advertise("imu_propagate", 1000); pub_path = n.advertise("path_no_loop", 1000); pub_loop_path = n.advertise("path", 1000); pub_odometry = n.advertise("odometry", 1000);