Skip to content

Commit

Permalink
20190713
Browse files Browse the repository at this point in the history
  • Loading branch information
@ManiiXu
ManiiXu committed Jul 13, 2019
1 parent e174870 commit 00d7b44
Show file tree
Hide file tree
Showing 5 changed files with 41 additions and 40 deletions.
8 changes: 5 additions & 3 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
41 changes: 20 additions & 21 deletions feature_tracker/src/feature_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<pair<int, pair<cv::Point2f, int>>> 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<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
{
return a.first > b.first;
Expand Down Expand Up @@ -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)
Expand All @@ -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)
{
Expand Down Expand Up @@ -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
Expand Down
6 changes: 3 additions & 3 deletions feature_tracker/src/feature_tracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ void reduceVector(vector<int> &v, vector<uchar> status);

/**
* @class FeatureTracker
* @Description 对每个相机进行角点LK光流跟踪
* @Description 视觉前端预处理：对每个相机进行角点LK光流跟踪
*/
class FeatureTracker
{
Expand Down Expand Up @@ -73,10 +73,10 @@ class FeatureTracker
map<int, cv::Point2f> cur_un_pts_map;
map<int, cv::Point2f> 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
};
15 changes: 7 additions & 8 deletions feature_tracker/src/feature_tracker_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

/**
Expand All @@ -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!");
Expand Down Expand Up @@ -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//双目
{
Expand Down Expand Up @@ -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]);

Expand All @@ -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，跟踪的特征点，给后端优化用
Expand Down
11 changes: 6 additions & 5 deletions vins_estimator/src/factor/imu_factor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{

Expand Down Expand Up @@ -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<Eigen::Matrix<double, 15, 1>> 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<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
Expand Down Expand Up @@ -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<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
Expand Down Expand Up @@ -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<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
Expand Down

0 comments on commit 00d7b44

Please sign in to comment.