This section learns the VINS-Mono front-end processing of image data and interprets the operations in the callback function img_callback().
VINS-Mono/Fusion code learning series:
Learning VINS-Mono/Fusion source code from scratch (1): Main function
learning VINS-Mono/Fusion source code from scratch (2): Front-end image tracking
learning VINS-Mono/Fusion from scratch Source code (3): IMU pre-integration formula derivation and
learning VINS-Mono/Fusion from scratch source code (4): Error Kalman filter
learning VINS-Mono/Fusion from scratch source code (5): VIO initialization
learning VINS- from scratch Mono/Fusion source code (6): back-end optimization
Learning VINS-Mono/Fusion source code from scratch (2): main function
1 Image callback function img_callback()
In the main function of the feature_tracker node, the image topic is read and the img_callback() callback function is called for processing.
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
Several tasks completed in img_callback():
- Check image data stream continuity
- Control how often image data is sent to the backend
- Conversion from ros format to opencv format
- Read image information (readImage function)
- Image frame publishing/visualization (determine whether required based on preset)
- Check image data stream continuity
if(first_image_flag)//判断是不是第一帧图像
{
//是第一帧，修改flag，记录时间戳
first_image_flag = false;
first_image_time = img_msg->header.stamp.toSec();
last_image_time = img_msg->header.stamp.toSec();
return;
}
//检查数据流连续性
//当前帧和上一帧时间差是不是超过1s?间隔时间过长，连续性差
//当前帧时间戳是不是小于上一帧时间戳？发生数据错乱？
if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
{
//对系统重置了，restart
ROS_WARN("image discontinue! reset the feature tracker!");
first_image_flag = true;
last_image_time = 0;
pub_count = 1;
std_msgs::Bool restart_flag;
restart_flag.data = true;
pub_restart.publish(restart_flag);
return;
}
last_image_time = img_msg->header.stamp.toSec();
- Control how often image data is sent to the backend
//pub_count记录向后端发送了多少次
//这里就是计算发送的频率（次数/时间）是否小于预设频率FREQ（一般是10Hz）
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
PUB_THIS_FRAME = true;
//检查发送频率和预设是否过分接近
//如果过分接近就要重启一次，避免短时间内发送大量图像数据
if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
{
first_image_time = img_msg->header.stamp.toSec();
pub_count = 0;
}
}
else
PUB_THIS_FRAME = false;
- Conversion from ros format to opencv format
cv_bridge::CvImageConstPtr ptr;//使用opencv的图像指针
if (img_msg->encoding == "8UC1")//灰度图
{
sensor_msgs::Image img;
img.header = img_msg->header;
img.height = img_msg->height;
img.width = img_msg->width;
img.is_bigendian = img_msg->is_bigendian;
img.step = img_msg->step;
img.data = img_msg->data;
img.encoding = "mono8";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
2 Image reading readImage()
for (int i = 0; i < NUM_OF_CAM; i++)//NUM_OF_CAM=1
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK)
//对每个相机读取图像信息，这里就一个相机
//注意它针对多个相机的存储方式rowRange(ROW * i, ROW * (i + 1))
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
else
{
if (EQUALIZE)//是否要做图像均衡化
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
}
else
//不做均衡化，直接赋值
trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
}
}
Tasks implemented internally by readImage():
- Image equalization (EQUALIZE)
- Optical flow tracking (cv::calcOpticalFlowPyrLK)
- Extract new feature points (cv::goodFeaturesToTrack)
- Remove distortion and calculate feature point velocity (undistortedPoints)
2.1 Image equalization
if (EQUALIZE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else
img = _img;
2.2 Optical flow tracking
Call the opencv function cv::calcOpticalFlowPyrLK to implement optical flow tracking.
forw_img - the current newly obtained image
cur_img - the previous frame
forw_pts - container, which stores the feature point information of the current frame image
cur_pts - the feature points of the previous frame image
The tracking part uses three methods to remove failure points and outliers points:
(1) calcOpticalFlowPyrLK status bit status judgment
(2) inBorder checks whether the tracked points exceed the picture boundary
(3) rejectWithF to eliminate outliers through epipolar constraints
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
//对当前帧forw_img和上一帧cur_img进行LK光流追踪，状态位status记录各个特征点追踪情况
//status为1就表明该特征点被成功跟踪
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
for (int i = 0; i < int(forw_pts.size()); i++)
//inBorder判断当前帧跟踪到的特征点是否超出了图像范围
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
//reduceVector根据status，将跟失败的点去除
//采用双指针思路，第一个索引遍历整个Vector，第二个索引记录成功点
//保留下来的就是跟踪成功的点
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);//特征点id
reduceVector(cur_un_pts, status);//去畸变的点坐标
reduceVector(track_cnt, status);//跟踪到的次数
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
In rejectWithF(), the feature points of two adjacent frames are projected into the virtual pixel coordinate system, and then the essential matrix is calculated, and the status is also obtained to determine the tracking situation.
FOCAL_LENGTH=460 is the preset virtual focal length. The advantage of this is that for different camera parameters, there is no need to adjust the ransac threshold F_THRESHOLD.
void FeatureTracker::rejectWithF()
{
if (forw_pts.size() >= 8)
{
ROS_DEBUG("FM ransac begins");
TicToc t_f;
vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector3d tmp_p;
m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[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_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[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_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
}
vector<uchar> status;
cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
int size_a = cur_pts.size();
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(cur_un_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
}
}
The setMask() feature point is uniformized. For the pinhole model, all image grayscale values are set to 255. Then the image feature points are sorted according to the number of tracking times. The number of tracking times is ranked first, indicating that the feature point is stable.
After sorting, starting from the first feature point, draw a circle with a radius of MIN_DIST, and set the gray value to 0, so that subsequent points can no longer fall within this circle, so that the feature points are evenly distributed.
void FeatureTracker::setMask()
{
if(FISHEYE)
mask = fisheye_mask.clone();
else
mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
// prefer to keep features that are tracked for long time
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])));
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;
});
forw_pts.clear();
ids.clear();
track_cnt.clear();
for (auto &it : cnt_pts_id)
{
if (mask.at<uchar>(it.second.first) == 255)
{
forw_pts.push_back(it.second.first);
ids.push_back(it.second.second);
track_cnt.push_back(it.first);
cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
}
}
}
2.3 Extraction of new feature points
Use the opencv function goodFeaturesToTrack to further extract more feature points.
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
In the next step of addPoints(), the newly extracted feature points are added to the container, their ids are uniformly set to -1, and the number of tracking times is 1.
void FeatureTracker::addPoints()
{
for (auto &p : n_pts)
{
forw_pts.push_back(p);
ids.push_back(-1);
track_cnt.push_back(1);
}
}
2.4 De-distortion
Distortion itself is a highly nonlinear relationship. The farther away from the center of the image, the more serious the distortion.
vins-mono defines the undistortedPoints() function to remove distortion and calculate the velocity of feature points. The dedistortion method here is rather magical and is an iterative approximation idea.
Assume a distortion point a', the actual pixel coordinate is a, and let the position a' correspond to the de-distortion point b, so that bb'<aa', because b is closer to the center of the image. Starting from a' and going forward bb', we get point c, which is in the middle of aa'. Follow this idea several times and eventually approach point a.
The above process is implemented in liftProjective().
//PinholeCamera::liftProjective()
int n = 8;
Eigen::Vector2d d_u;
distortion(Eigen::Vector2d(mx_d, my_d), d_u);
mx_u = mx_d - d_u(0);
my_u = my_d - d_u(1);
for (int i = 1; i < n; ++i)//默认迭代8次
{
distortion(Eigen::Vector2d(mx_u, my_u), d_u);
mx_u = mx_d - d_u(0);//注意减号，代表方向
my_u = my_d - d_u(1);//这个d_u本质上就是(x_d-x,y_d-y)
}
void
PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const
{
double k1 = mParameters.k1();
double k2 = mParameters.k2();
double p1 = mParameters.p1();
double p2 = mParameters.p2();
double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
mx2_u = p_u(0) * p_u(0);//x^2
my2_u = p_u(1) * p_u(1);//y^2
mxy_u = p_u(0) * p_u(1);//x*y
rho2_u = mx2_u + my2_u;//r^2
rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
}