From 00d7b44f51467a5349143017d5e090fc7f0c57c4 Mon Sep 17 00:00:00 2001 From: ManiiXu <323218777@qq.com> Date: Sat, 13 Jul 2019 18:49:01 +0800 Subject: [PATCH] 20190713 --- README.md | 8 ++-- feature_tracker/src/feature_tracker.cpp | 41 ++++++++++---------- feature_tracker/src/feature_tracker.h | 6 +-- feature_tracker/src/feature_tracker_node.cpp | 15 ++++--- vins_estimator/src/factor/imu_factor.h | 11 +++--- 5 files changed, 41 insertions(+), 40 deletions(-) diff --git a/README.md b/README.md index 477cc42..e89b0cf 100644 --- a/README.md +++ b/README.md @@ -27,11 +27,13 @@ Forked from VINS-Mono: https://github.com/HKUST-Aerial-Robotics/VINS-Mono [VINS-Mono代码解读——视觉惯性联合初始化流程](https://blog.csdn.net/qq_41839222/article/details/88942414) -[VINS-Mono理论学习——视觉惯性对齐与外参标定](https://blog.csdn.net/qq_41839222/article/details/89106128) + [VINS-Mono理论学习——视觉惯性对齐与外参标定](https://blog.csdn.net/qq_41839222/article/details/89106128) - VINS-Mono代码解读——滑动窗口的边缘化与非线性优化 + [VINS-Mono理论学习——后端非线性优化](https://blog.csdn.net/qq_41839222/article/details/93593844) - VINS-Mono理论学习——非线性优化 + VINS-Mono理论学习——边缘化 + + VINS-Mono代码解读——滑动窗口的非线性优化流程 [VINS-Mono代码解读——回环检测与重定位](https://blog.csdn.net/qq_41839222/article/details/87878550) diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 7b190e2..a55ed82 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -52,13 +52,13 @@ void FeatureTracker::setMask() mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255)); // prefer to keep features that are tracked for long time - //构造(cnt，pts，id)序列 + // 构造(cnt，pts，id)序列 vector>> cnt_pts_id; for (unsigned int i = 0; i < forw_pts.size(); i++) cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i]))); - //对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序 + //对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序（lambda表达式） sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair> &a, const pair> &b) { return a.first > b.first; @@ -114,8 +114,8 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) TicToc t_r; cur_time = _cur_time; - //如果EQUALIZE=1，表示太亮或太暗 - if (EQUALIZE)//判断是否进行直方图均衡化处理 + //如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理 + if (EQUALIZE) { //自适应直方图均衡 //createCLAHE(double clipLimit, Size tileGridSize) @@ -130,18 +130,17 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) if (forw_img.empty()) { //如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据 - //将读入的图像赋给当前帧forw_img - //同时，还将读入的图像赋给prev_img、cur_img，这是为了避免后面使用到这些数据时，它们是空的 + //将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_img prev_img = cur_img = forw_img = img; } else { - //否则，说明之前就已经有图像读入 - //所以只需要更新当前帧forw_img的数据 + //否则，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据 forw_img = img; } - forw_pts.clear();//此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除 + //此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除 + forw_pts.clear(); if (cur_pts.size() > 0) { @@ -202,18 +201,18 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) if (mask.size() != forw_img.size()) cout << "wrong size " << endl; /** - *void cv::goodFeaturesToTrack( 在mask中不为0的区域检测新的特征点 - * InputArray image, 输入图像 - * OutputArray corners, 存放检测到的角点的vector - * int maxCorners, 返回的角点的数量的最大值 - * double qualityLevel, 角点质量水平的最低阈值（范围为0到1，质量最高角点的水平为1），小于该阈值的角点被拒绝 - * double minDistance, 返回角点之间欧式距离的最小值 - * InputArray mask = noArray(), 和输入图像具有相同大小，类型必须为CV_8UC1,用来描述图像中感兴趣的区域，只在感兴趣区域中检测角点 - * int blockSize = 3, 计算协方差矩阵时的窗口大小 - * bool useHarrisDetector = false, 指示是否使用Harris角点检测，如不指定则使用shi-tomasi算法 - * double k = 0.04 Harris角点检测需要的k值 - *) - */ + *void cv::goodFeaturesToTrack( 在mask中不为0的区域检测新的特征点 + * InputArray image, 输入图像 + * OutputArray corners, 存放检测到的角点的vector + * int maxCorners, 返回的角点的数量的最大值 + * double qualityLevel, 角点质量水平的最低阈值（范围为0到1，质量最高角点的水平为1），小于该阈值的角点被拒绝 + * double minDistance, 返回角点之间欧式距离的最小值 + * InputArray mask = noArray(), 和输入图像具有相同大小，类型必须为CV_8UC1,用来描述图像中感兴趣的区域，只在感兴趣区域中检测角点 + * int blockSize = 3, 计算协方差矩阵时的窗口大小 + * bool useHarrisDetector = false, 指示是否使用Harris角点检测，如不指定则使用shi-tomasi算法 + * double k = 0.04 Harris角点检测需要的k值 + *) + */ cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask); } else diff --git a/feature_tracker/src/feature_tracker.h b/feature_tracker/src/feature_tracker.h index 7aa891a..dadf9ca 100644 --- a/feature_tracker/src/feature_tracker.h +++ b/feature_tracker/src/feature_tracker.h @@ -27,7 +27,7 @@ void reduceVector(vector &v, vector status); /** * @class FeatureTracker -* @Description 对每个相机进行角点LK光流跟踪 +* @Description 视觉前端预处理：对每个相机进行角点LK光流跟踪 */ class FeatureTracker { @@ -73,10 +73,10 @@ class FeatureTracker map cur_un_pts_map; map prev_un_pts_map; - camodocal::CameraPtr m_camera; + camodocal::CameraPtr m_camera;//相机模型 double cur_time; double prev_time; - static int n_id;//用来作为特征点id，每检测到一个新的特征点，就将n_id作为该特征点的id，然后n_id加1 + static int n_id;//特征点id，每检测到一个新的特征点，就将n_id作为该特征点的id，然后n_id加1 }; diff --git a/feature_tracker/src/feature_tracker_node.cpp b/feature_tracker/src/feature_tracker_node.cpp index 176ba12..603239e 100644 --- a/feature_tracker/src/feature_tracker_node.cpp +++ b/feature_tracker/src/feature_tracker_node.cpp @@ -24,7 +24,7 @@ FeatureTracker trackerData[NUM_OF_CAM]; double first_image_time; int pub_count = 1; bool first_image_flag = true; -double last_image_time = 0; +double last_image_time = 0;//上一帧相机的时间戳 bool init_pub = 0; /** @@ -41,13 +41,12 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) if(first_image_flag) { first_image_flag = false; - first_image_time = img_msg->header.stamp.toSec();//记录图像帧的时间 + first_image_time = img_msg->header.stamp.toSec();//记录第一个图像帧的时间 last_image_time = img_msg->header.stamp.toSec(); return; } - // detect unstable camera stream - // 通过判断时间间隔，有问题则restart + // 通过时间间隔判断相机数据流是否稳定，有问题则restart if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time) { ROS_WARN("image discontinue! reset the feature tracker!"); @@ -103,7 +102,7 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { ROS_DEBUG("processing camera %d", i); if (i != 1 || !STEREO_TRACK)//单目 - //FeatureTracker::readImage()函数读取图像数据进行处理 + //readImage()函数读取图像数据进行处理 trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec()); else//双目 { @@ -239,10 +238,10 @@ int main(int argc, char **argv) //设置logger的级别。 只有级别大于或等于level的日志记录消息才会得到处理。 ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); - //读取config->euroc->euroc_config.yaml中的一些配置参数 + //读取yaml中的一些配置参数 readParameters(n); - //读取每个相机实例读取对应的相机内参 + //读取每个相机实例对应的相机内参 for (int i = 0; i < NUM_OF_CAM; i++) trackerData[i].readIntrinsicParameter(CAM_NAMES[i]); @@ -262,7 +261,7 @@ int main(int argc, char **argv) } } - //订阅话题IMAGE_TOPIC(/cam0/image_raw),执行回调函数 + //订阅话题IMAGE_TOPIC(/cam0/image_raw),执行回调函数img_callback ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback); //发布feature，实例feature_points，跟踪的特征点，给后端优化用 diff --git a/vins_estimator/src/factor/imu_factor.h b/vins_estimator/src/factor/imu_factor.h index 2c57c91..8c6aa37 100644 --- a/vins_estimator/src/factor/imu_factor.h +++ b/vins_estimator/src/factor/imu_factor.h @@ -16,7 +16,8 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> IMUFactor(IntegrationBase* _pre_integration):pre_integration(_pre_integration) { } - //IMU对应的残差，需要自己计算jacobian + // IMU对应的残差，需要自己计算jacobian + // parameters[0~3]分别对应了4组优化变量的参数块 virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { @@ -57,12 +58,12 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> pre_integration->repropagate(Bai, Bgi); } #endif - // IMU残差 + // 构建IMU残差residual Eigen::Map> residual(residuals); residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj); - // LLT分解，residual还需乘以信息矩阵的sqrt_info + // LLT分解，residual 还需乘以信息矩阵的sqrt_info // 因为优化函数其实是d=r^T P^-1 r ，P表示协方差，而ceres只接受最小二乘优化 // 因此需要把P^-1做LLT分解，使d=(L^T r)^T (L^T r) = r'^T r Eigen::Matrix sqrt_info = Eigen::LLT>(pre_integration->covariance.inverse()).matrixL().transpose(); @@ -114,7 +115,7 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> //ROS_BREAK(); } } - // 第i帧的imu速度、ba、bg + // 第i帧的imu速度vbi、bai、bgi if (jacobians[1]) { Eigen::Map> jacobian_speedbias_i(jacobians[1]); @@ -164,7 +165,7 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> //ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8); //ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8); } - // 第j帧的IMU速度、ba、bg + // 第j帧的IMU速度vbj、baj、bgj if (jacobians[3]) { Eigen::Map> jacobian_speedbias_j(jacobians[3]);