(转载请删除括号里的内容)
VINS-mono已经提出有几年了,但是作为经典的视觉惯导结合的SLAM,还是很重要的,即使网上已经出现了大量的代码分析,在阅读前辈的代码同时,总是会有自己的奇奇怪怪的问题,所以有必要在此记录下来。在此,我们只考虑代码中单目的部分,对双目的部分不加解释。
1.main函数
int main(int argc, char **argv)
{
//初始化节点
ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
readParameters(n);//读参数
//读取相机内参
for (int i = 0; i < NUM_OF_CAM; i++)
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
if(FISHEYE)
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
if(!trackerData[i].fisheye_mask.data)
{
ROS_INFO("load mask fail");
ROS_BREAK();
}
else
ROS_INFO("load mask success");
}
}
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
ros::spin();
return 0;
}
main函数中主要步骤有一下几点:
(1)初始化节点,设置消息级别。
(2)读取配置文件的参数,配置文件的位置和名称在相应的launch文件中已经写出来了。
(3)读取相机的内参。
(4)订阅和发布消息。订阅的消息在这里要调用回调函数,也就是img_callback().
image_callback()
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();
last_image_time = img_msg->header.stamp.toSec();
return;
}
// detect unstable camera stream
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!");
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();
// frequency control 如果发布图像的频率超过阈值,就不发送这一帧的图像
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
PUB_THIS_FRAME = true;
// reset the frequency control
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;
//将图像的编码格式从8UC1转换成mono8
cv_bridge::CvImageConstPtr ptr;
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);
cv::Mat show_img = ptr->image;
TicToc t_r;
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
//单目情况
if (i != 1 || !STEREO_TRACK)//ROW = image_height
//Mat::rowRange(int startRow, int endRow) const 按行读取数据到trackData
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));
}
#if SHOW_UNDISTORTION
trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
}
//更新全局id,将新提取的特征构点赋予全局id
for (unsigned int i = 0;; i++)
{
bool completed = false;
for (int j = 0; j < NUM_OF_CAM; j++)
if (j != 1 || !STEREO_TRACK)
completed |= trackerData[j].updateID(i);
if (!completed)
break;
}
if (PUB_THIS_FRAME)
{
//feature_points消息的封装
pub_count++;
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
sensor_msgs::ChannelFloat32 id_of_point;//特征点id
sensor_msgs::ChannelFloat32 u_of_point;//像素坐标(u,v)
sensor_msgs::ChannelFloat32 v_of_point;
sensor_msgs::ChannelFloat32 velocity_x_of_point;//像素的速度(vx, vy)
sensor_msgs::ChannelFloat32 velocity_y_of_point;
feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";
vector<set<int>> hash_ids(NUM_OF_CAM);
for (int i = 0; i < NUM_OF_CAM; i++)
{
auto &un_pts = trackerData[i].cur_un_pts;
auto &cur_pts = trackerData[i].cur_pts;
auto &ids = trackerData[i].ids;
auto &pts_velocity = trackerData[i].pts_velocity;
for (unsigned int j = 0; j < ids.size(); j++)
{
if (trackerData[i].track_cnt[j] > 1)
{
int p_id = ids[j];
hash_ids[i].insert(p_id);
geometry_msgs::Point32 p;//矫正后归一化平面的3d点(x,y,1)
p.x = un_pts[j].x;
p.y = un_pts[j].y;
p.z = 1;
feature_points->points.push_back(p);
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
u_of_point.values.。(cur_pts[j].x);
v_of_point.values.push_back(cur_pts[j].y);
velocity_x_of_point.values.push_back(pts_velocity[j].x);
velocity_y_of_point.values.push_back(pts_velocity[j].y);
}
}
}
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);
ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
// skip the first image; since no optical speed on frist image
if (!init_pub)
{
init_pub = 1;
}
else
pub_img.publish(feature_points);
if (SHOW_TRACK)
{
ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
//cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
cv::Mat stereo_img = ptr->image;
for (int i = 0; i < NUM_OF_CAM; i++)
{
cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);
for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
{
double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
}
}
pub_match.publish(ptr->toImageMsg());
}
}
ROS_INFO("whole feature tracker processing costs: %f", t_r.toc());
}
image_callback()中主要的步骤有以下几点:
(1)不稳定的图像帧检测和图像发布频率的控制。
(2)将图像的编码从8UC1->MONO8
(3)readImage()
(4)更新跟踪点的id
(5)对feature_points进行封装,在vins_estimator中订阅了该话题
(6)对feature_img进行封装,这个图像消息是在RVIZ的tracked_image中显示的
image_callback()中最为重要的函数就是readImage(),所以在这里重点看一下**readImage()**的代码
readImage()
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
cv::Mat img;
TicToc t_r;
cur_time = _cur_time;
//直方图均衡
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;
//如果是第一次读入图片
//cur_img和forw_img是光流追踪前后的两帧 forw_img是当前帧,cur_img是上一帧,prev_img是上一次发布的帧
if (forw_img.empty())
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
//forw_pts保存的是当前帧的特征点
forw_pts.clear();
//cur_pts存放的是上一帧的特征点
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
//status: 如果对用特征点在光流被发现,对应位置为1,否则为0
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++)
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);
reduceVector(track_cnt, status);
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
//track_cnt保存的是能够追踪到的角点被追踪的次数
for (auto &n : track_cnt)
n++;
if (PUB_THIS_FRAME)
{
rejectWithF();
ROS_DEBUG("set mask begins");
TicToc t_m;
setMask();
ROS_DEBUG("set mask costs %fms", t_m.toc());
ROS_DEBUG("detect feature begins");
TicToc t_t;
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
if (mask.size() != forw_img.size())
cout << "wrong size " << endl;
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
n_pts.clear();
ROS_DEBUG("detect feature costs: %fms", t_t.toc());
ROS_DEBUG("add feature begins");
TicToc t_a;
addPoints();
ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
}
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
cur_img = forw_img;
cur_pts = forw_pts;
//对角点坐标去畸变
undistortedPoints();
prev_time = cur_time;
}
readImage()中的主要步骤如下:
(1)判断是否需要直方图均衡化
(2)根据是否是第一次读入图像进行处理
(3)光流追踪
(4)对跟踪失败的点 和 跟踪成功但是在图像边缘的点进行剔除
(5)判断是否要发布该帧,不发布,当前帧的数据赋值给上一帧,结束
(5-1)发布,根据fundamental matrix对prev_pts和forw_pts剔除outlier
(5-2)调用setMask()函数
(5-3)调用goodFeaturesToTrack()提取新的角点,并调用addPoints()把角点push到forw_pts中
(6)undistortedPoints() 根据不同的相机模型进行去畸变矫正和深度归一化,计算速度
看一下根据跟踪结果对角点进行剔除的代码 reduceVector()
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status)
{
int j = 0;
for (int i = 0; i < int(v.size()); i++)
if (status[i])
v[j++] = v[i];
v.resize(j);
}
代码很简单,也不用解释了,代码很优美
接下来重点看一下
rejectWithF()
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计算基础矩阵
//F_THRESHOLD 点到极线(像素)的最大距离,超过此距离被认定是异常值
//0.99 期望置信度?
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());
}
}
这个函数里面主要是调用了cv::findFundamentalMat()函数,根据status对跟踪的点进行剔除。
2. 画个图吧
根据程序执行的流程,可以画出来一个思维导图,如下。
3. 代码中调用openCV的函数
3.1 cv::findFundamentalMat
由两幅图像中对应的点计算出Fundamental矩阵。
int cvFindFundamentalMat( const CvMat* points1,
const CvMat* points2,
CvMat* fundamental_matrix,
int method=CV_FM_RANSAC,
double param1=1.,
double param2=0.99,
CvMat* status=NULL);
参数说明:
points1:第一幅图像的点的数组
points2:第二幅图像的点的数组,大小和格式与第一幅图像相同
fundamental_matrix:输出的基础矩阵。大小是33 或者93(7点法可以返回3个矩阵)
method:
计算基础矩阵的方法
* CV_FM_7POINT 7点法,点数目 = 7
* CV_FM_8POINT 8点法,点数目 >= 8
* CV_FM_RANSAC RANSAC算法,点数目 >= 8
* CV_FM_LMEDS LMedS算法,点数目 >= 8
param1:这个参数只用于方法RANSAC 或 LMedS 。它是点到对极线的最大距离,超过这个值的点将被舍弃,不用于后面的计算。通常这个值的设定是0.5 or 1.0
param2:这个参数只用于方法RANSAC 或 LMedS 。 它表示矩阵正确的可信度。例如可以被设为0.99 。
status:具有N个元素的输出数组,在计算过程中没有被舍弃的点,元素被被置为1;否则置为0。这个数组只可以在方法RANSAC and LMedS 情况下使用;在其它方法的情况下,status一律被置为1。这个参数是可选参数。
3.2 cv::goodFeaturesToTrack
关于这个函数,网上写的也已经很多了,就复制过来大家看看吧。
void cv::goodFeaturesToTrack(
cv::InputArray image, // 输入图像(CV_8UC1 CV_32FC1)
cv::OutputArray corners, // 输出角点vector
int maxCorners, // 最大角点数目
double qualityLevel, // 质量水平系数(小于1.0的正数,一般在0.01-0.1之间)
double minDistance, // 最小距离,小于此距离的点忽略
cv::InputArray mask = noArray(), // mask=0的点忽略
int blockSize = 3, // 使用的邻域数
bool useHarrisDetector = false, // false ='Shi Tomasi metric'
double k = 0.04 // Harris角点检测时使用
);
第一个参数是输入图像(8位或32位单通道图)。
第二个参数是检测到的所有角点,类型为vector或数组,由实际给定的参数类型而定。如果是vector,那么它应该是一个包含cv::Point2f的vector对象;如果类型是cv::Mat,那么它的每一行对应一个角点,点的x、y位置分别是两列。
第三个参数用于限定检测到的点数的最大值。
第四个参数表示检测到的角点的质量水平(通常是0.10到0.01之间的数值,不能大于1.0)。
第五个参数用于区分相邻两个角点的最小距离(小于这个距离得点将进行合并)。
第六个参数是mask,如果指定,它的维度必须和输入图像一致,且在mask值为0处不进行角点检测。
第七个参数是blockSize,表示在计算角点时参与运算的区域大小,常用值为3,但是如果图像的分辨率较高则可以考虑使用较大一点的值。
第八个参数用于指定角点检测的方法,如果是true则使用Harris角点检测,false则使用Shi Tomasi算法。
第九个参数是在使用Harris算法时使用,最好使用默认值0.04。