int main(int argc, char **argv) { ros::init(argc, argv, "vins_estimator"); ros::NodeHandle n("~"); ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); readParameters(n); estimator.setParameter(); #ifdef EIGEN_DONT_PARALLELIZE ROS_DEBUG("EIGEN_DONT_PARALLELIZE"); #endif ROS_WARN("waiting for image and imu..."); registerPub(n); //NOTE tcpNoDelay: a TCP transport is used, specifies whether or not to use TCP_NODELAY to provide a potentially lower-latency connection. ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback); ros::Subscriber sub_raw_image = n.subscribe(IMAGE_TOPIC, 2000, raw_image_callback); std::thread measurement_process{process}; std::thread loop_detection; if (LOOP_CLOSURE) { ROS_WARN("LOOP_CLOSURE true"); loop_detection = std::thread(process_loop_detection); m_camera = CameraFactory::instance()->generateCameraFromYamlFile(CAM_NAMES); } ros::spin(); return 0; }
作为estimator的主函数,首先进行了ros初始化,之后定义了三个subscriber分别为:
1. sub imu
2. sub image
3. sub raw image
其中sub imu订阅了/imu0 topic然后将imu数据push back到imu buffer,
sub image 订阅了feature tracker的publisher ,并将feature push back 到feature buffer
sub raw image 订阅了/cam0/raw_image topic并将图像通过cv::bridge传入
之后创建了两个线程分别为:
std::thread measurement_process{process};
以及
std::thread loop_detection;
我们首先来看getMeasurements()函数:
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
代码定义了一个vector,里面的element 是std::pair,这个pair由一个imu vector以及一个 特征点指针构成。
这个函数的作用是将imu buffer的数据以及feature buffer 的数据返回。
另外一个比较重要的是void send_imu(const sensor_msgs::ImuConstPtr &imu_msg)函数,
在process函数内,将measurement的返回值的first部分,也就是IMU vector内的每一个元素,调用send_imu();
而send imu() 的目的就是将测量值的imu部分input到estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz))部分。
processIMU这个estimator的成员函数将measurement 的IMU部分进行了预积分处理。
在process()内还有一个estimator.processImage()函数,我们来看一下这个函数
void Estimator::processImage(const map<int, vector<pair<int, Vector3d>>> &image, const std_msgs::Header &header) { ROS_DEBUG("new image coming ------------------------------------------"); ROS_DEBUG("Adding feature points %lu", image.size()); if (f_manager.addFeatureCheckParallax(frame_count, image)) marginalization_flag = MARGIN_OLD; else marginalization_flag = MARGIN_SECOND_NEW; ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept"); ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe"); ROS_DEBUG("Solving %d", frame_count); ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount()); Headers[frame_count] = header; ImageFrame imageframe(image, header.stamp.toSec()); imageframe.pre_integration = tmp_pre_integration; all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe)); tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]}; if(ESTIMATE_EXTRINSIC == 2) { ROS_INFO("calibrating extrinsic param, rotation movement is needed"); if (frame_count != 0) { vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count); Matrix3d calib_ric; if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric)) { ROS_WARN("initial extrinsic rotation calib success"); ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric); ric[0] = calib_ric; RIC[0] = calib_ric; ESTIMATE_EXTRINSIC = 1; } } } if (solver_flag == INITIAL) { if (frame_count == WINDOW_SIZE) { bool result = false; if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1) { result = initialStructure(); initial_timestamp = header.stamp.toSec(); } if(result) { solver_flag = NON_LINEAR; solveOdometry(); slideWindow(); f_manager.removeFailures(); ROS_INFO("Initialization finish!"); last_R = Rs[WINDOW_SIZE]; last_P = Ps[WINDOW_SIZE]; last_R0 = Rs[0]; last_P0 = Ps[0]; } else slideWindow(); } else frame_count++; } else { TicToc t_solve; solveOdometry(); ROS_DEBUG("solver costs: %fms", t_solve.toc()); if (failureDetection()) { ROS_WARN("failure detection!"); failure_occur = 1; clearState(); setParameter(); ROS_WARN("system reboot!"); return; } TicToc t_margin; slideWindow(); f_manager.removeFailures(); ROS_DEBUG("marginalization costs: %fms", t_margin.toc()); // prepare output of VINS key_poses.clear(); for (int i = 0; i <= WINDOW_SIZE; i++) key_poses.push_back(Ps[i]); last_R = Rs[WINDOW_SIZE]; last_P = Ps[WINDOW_SIZE]; last_R0 = Rs[0]; last_P0 = Ps[0]; } }
process()这个函数主要完成了以下几件事:
1. 将测量值的IMU部分进行预积分处理;
2. processImage,将测量值的特征点部分进行处理并进行imu融合,在processImage函数中主要完成以下两项工作:
- 滑窗 slideWindow();
- 滑窗内位姿优化:位于solveOdometry 中的 optimization()函数中,为VINS中IMU与视觉融合的重点部分。;