VINS代码详解(feature_traker模块中的函数详解)
说明:VINS的学习过程主要参考VINS-Mono论文学习与代码解读——目录与参考。由于本人基础较差,有很多细节内容不理解,因此针对本人需求编写如下笔记。
模块的接口
订阅的Topic:
- 订阅话题IMAGE_TOPIC(/cam0/image_raw),执行回调函数img_callback
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
发布的Topic:
- 发布feature,实例feature_points,跟踪的特征点,给后端优化用
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
- 发布feature_img,实例ptr,跟踪的特征点图,给RVIZ用和调试用
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
- 发布restart
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
模块工作流程
推荐阅读在代码中分析VINS—图解feature_tracker.cpp。
一些需要了解的函数
cv_bridge::toCvCopy()
参考:
Converting between ROS images and OpenCV images (C++)
ROS学习笔记–cv_bridge
cv_bridge
是在ROS图像消息和OpenCV图像之间进行转换的一个功能包。ROS以自己的sensor_msgs / Image
消息格式传递图像,但许多用户希望将图像与OpenCV结合使用。 CvBridge是一个ROS库,提供ROS和OpenCV之间的接口。 可以在vision_opencv
stack的cv_bridge
包中找到 cv_bridge。
把ROS图像消息转成OpenCV图像数据
cv_bridge定义了一个包含OpenCV图像、编码类型以及ROS header的cvImage类型。CvImage尤其包含了sensor_msgs/Image
的信息。如下:
namespace cv_bridge {
class CvImage
{
public:
std_msgs::Header header;
std::string encoding;
cv::Mat image;
};
typedef boost::shared_ptr<CvImage> CvImagePtr;
typedef boost::shared_ptr<CvImage const> CvImageConstPtr;
}
当进行类型转换时,cvbridge
可以处理两种使用需求:
- 我们希望直接修改数据,那么可以拷贝一份图像数据
- 我们不希望修改数据,我们可以在不拷贝的情况下安全地读取图像数据
cvbridge
提供了如下函数进行图像格式转换:
// Case 1: Always copy, returning a mutable CvImage
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
const std::string& encoding = std::string());
CvImagePtr toCvCopy(const sensor_msgs::Image& source,
const std::string& encoding = std::string());
// Case 2: Share if possible, returning a const CvImage
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
const std::string& encoding = std::string());
CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
const boost::shared_ptr<void const>& tracked_object,
const std::string& encoding = std::string());
函数的输入是图像消息的指针,以及一个可选的编码参数用于规定目标CvImage的编码。
如果用户没有指定图像编码信息(或者编码参数为空字符串),目标图像的编码将与源图像一样,在这种情况下toCvShare
可以保证不会拷贝图像数据。图像编码可以是一下任意一种OpenCV支持的图像编码:
- 8UC[1-4]
- 8SC[1-4]
- 16UC[1-4]
- 16SC[1-4]
- 32SC[1-4]
- 32FC[1-4]
- 64FC[1-4]
注:[1-4]指色彩通道数
对于常用的图像编码,CvBridge
提供了可选的色彩或者像素深度的转换。为了使用该特性,需要将编码指定为以下格式之一:
- mono8: CV_8UC1, grayscale image
- mono16: CV_16UC1, 16-bit grayscale image
- bgr8: CV_8UC3, color image with blue-green-red color order
- rgb8: CV_8UC3, color image with red-green-blue color order
- bgra8: CV_8UC4, BGR color image with an alpha channel
- rgba8: CV_8UC4, RGB color image with an alpha channel
其中mono8和bgr8是大多数OpenCV函数所期望的图像编码格式。
最后,CvBridge也可以识别OpenCV中8UC1类型的Bayer pattern编码,CvBridge将不会对Bayer pattern进行转换,一般是由image_proc进行转换的。CvBridge可以识别一下以下集中Bayer编码:
- bayer_rggb8
- bayer_bggr8
- bayer_gbrg8
- bayer_grbg8
把OpenCV图像转成ROS图像消息
class CvImage
{
sensor_msgs::ImagePtr toImageMsg() const;
// Overload mainly intended for aggregate messages that contain
// a sensor_msgs::Image as a member.
void toImageMsg(sensor_msgs::Image& ros_image) const;
};
如果CvImage是用户自己创建的,不要忘了填充header和编码字段。使用实例:
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Publisher pub = it.advertise("camera/image", 1);
cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
cv::waitKey(30);
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
ros::Rate loop_rate(5);
while (nh.ok()) {
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
}
一个ROS节点范例
节点工作:节点侦听一个ROS图像消息的话题,然后转换成cv::Mat
格式,在上面画一个愿并且使用OpenCV显示图像,最后将其转换成ROS消息发布。
在你的package.xml和CMakeLists.xml(或者在你使用catkin_create_pkg时)添加一下依赖:
sensor_msgs
cv_bridge
roscpp
std_msgs
image_transport
具体节点代码:
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
static const std::string OPENCV_WINDOW = "Image window";
class ImageConverter
{
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
public:
ImageConverter()
: it_(nh_)
{
// Subscrive to input video feed and publish output video feed
image_sub_ = it_.subscribe("/camera/image_raw", 1,
&ImageConverter::imageCb, this);
image_pub_ = it_.advertise("/image_converter/output_video", 1);
cv::namedWindow(OPENCV_WINDOW);
}
~ImageConverter()
{
cv::destroyWindow(OPENCV_WINDOW);
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Draw an example circle on the video stream
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
// Update GUI Window
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_converter");
ImageConverter ic;
ros::spin();
return 0;
}
接下来我们对代码进行一个分解:
#include <image_transport/image_transport.h>
使用在ROS中发布和订阅图像image_transport允许您订阅压缩图像流。 请记住在package.xml中添加image_transport的依赖配置。
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
包含CvBridge的头文件以及image encodings(包含了很多有用的常量和函数),记得在package.xml中添加cv_bridge的依赖配置。
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
包含OpenCV的图像处理和GUI模块头文件,记得在package.xml中include opencv2的依赖。
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
public:
ImageConverter()
: it_(nh_)
{
// Subscrive to input video feed and publish output video feed
image_sub_ = it_.subscribe("/camera/image_raw", 1,
&ImageConverter::imageCb, this);
image_pub_ = it_.advertise("/image_converter/output_video", 1);
用image_transport订阅一个图像主题“in”和发布一个图像主题“out”。
cv::namedWindow(OPENCV_WINDOW);
}
~ImageConverter()
{
cv::destroyWindow(OPENCV_WINDOW);
}
在初始化和析构时调用OpenCV HighGUI来创建及销毁窗口。
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
在我们的回调函数中,首先将ROS图像消息转换为了CvImage以在OpenCV中使用。因为我们需要在图像中画圆,所以需要一个图像的拷贝,应使用toCvCopy()。sensor_msgs::image_encodings::BGR8是”bgr8”字符串常量。
注意:penCV期望彩色图像使用BGR通道顺序。
我们应该调用toCvCopy()/toCvShared()时来捕获异常错误,因为这些函数不会校验数据的有效性。
// Draw an example circle on the video stream
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
// Update GUI Window
在图像中画一个红色的圆圈并进行显示。
cv::waitKey(3);
将CvImage转换为ROS图像消息并将其发布到“out”话题上。
要运行节点,您需要一个图像流。运行一个摄像头或播放bag文件以生成图像流。 现在,您可以运行此节点,将“in”重新映射(remapping)到实际的图像流主题。
如果你成功地转换为OpenCV图像,你将在创建的窗口中看到添加圆圈之后的图像。
你可以通过rostopic或image_view查看图像来确认节点是否正确地发布了图像。
共享图像数据的例子
在上节中我们创建了图像的拷贝,但共享图像也很容易:
namespace enc = sensor_msgs::image_encodings;
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Process cv_ptr->image using OpenCV
}
如果输入图像的编码是”bgr8”,cv_ptr将会是图像数据的一个别名而非拷贝。如果输入图像不是”bgr8”编码但是可转换为”bgr8”编码(如”mono8”),CvBridge将会为cv_ptr分配一个新的buffer并执行转换。如果没有异常捕获语句的话一行代码就能共享图像了,但可能输入图像的编码无法转换为目标编码从而导致节点崩溃。例如,当输入图像是从一个Bayer pattern摄像机的image_raw话题接收的,CvBridge将会抛出异常,因为不支持Bayer到color的自动转换。
一个稍微更复杂的例子:
namespace enc = sensor_msgs::image_encodings;
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImageConstPtr cv_ptr;
try
{
if (enc::isColor(msg->encoding))
cv_ptr = cv_bridge::toCvShare(msg, enc::BGR8);
else
cv_ptr = cv_bridge::toCvShare(msg, enc::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Process cv_ptr->image using OpenCV
}
在这个例子中,如果可以的话我们将使用color的编码,不行的话就是用monochrome类型的编码,如果输入图像是”bgr8”或”mono8”编码,将不会进行拷贝。
对比度有限的自适应直方图均衡化 cv::createCLAHE()
参考:
Histograms - 2: Histogram Equalization
CLAHE (Contrast Limited Adaptive Histogram Equalization)被广泛应用于图像去雾、低照度图像增强、水下图像效果调节、以及数码照片改善等方面。
#include <opencv2/imgproc.hpp>
//自适应直方图均衡
//createCLAHE(double clipLimit, Size tileGridSize)
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());
- C++
Ptr<CLAHE> cv::createCLAHE ( double clipLimit = 40.0,
Size tileGridSize = Size(8, 8)
)
- Python:
cv.createCLAHE( [, clipLimit[, tileGridSize]] ) -> retval
Parameters
- clipLimit: Threshold for contrast limiting.
- tileGridSize: Size of grid for histogram equalization. Input image will be divided into equally sized rectangular tiles. tileGridSize defines the number of tiles in row and column.
具有金字塔的迭代Lucas-Kanade方法计算稀疏特征集的光流cv::calcOpticalFlowPyrLK()
参考:
Opencv学习(4)——CalcOpticalFlowPyrLK()函数解析
《视觉SLAM十四讲》
函数的声明:
void cv::calcOpticalFlowPyrLK (
InputArray prevImg,
InputArray nextImg,
InputArray prevPts,
InputOutputArray nextPts,
OutputArray status,
OutputArray err,
Size winSize = Size(21, 21),
int maxLevel = 3,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),
int flags = 0,
double minEigThreshold = 1e-4
)
- prevImg :buildOpticalFlowPyramid构造的第一个8位输入图像或金字塔。
- nextImg :与prevImg相同大小和相同类型的第二个输入图像或金字塔
- prevPts :需要找到流的2D点的矢量(vector of 2D points for which the flow needs to be found;);点坐标必须是单精度浮点数。
- nextPts :输出二维点的矢量(具有单精度浮点坐标),包含第二图像中输入特征的计算新位置;当传递OPTFLOW_USE_INITIAL_FLOW标志时,向量必须与输入中的大小相同。
- status :输出状态向量(无符号字符);如果找到相应特征的流,则向量的每个元素设置为1,否则设置为0。
- err :输出错误的矢量; 向量的每个元素都设置为相应特征的错误,错误度量的类型可以在flags参数中设置; 如果未找到流,则未定义错误(使用status参数查找此类情况)。
- winSize :每个金字塔等级的搜索窗口的winSize大小。
- maxLevel :基于0的最大金字塔等级数;如果设置为0,则不使用金字塔(单级),如果设置为1,则使用两个级别,依此类推;如果将金字塔传递给输入,那么算法将使用与金字塔一样多的级别,但不超过maxLevel。
- criteria :参数,指定迭代搜索算法的终止条件(在指定的最大迭代次数criteria.maxCount之后或当搜索窗口移动小于criteria.epsilon时)。
- flags :操作标志:
- OPTFLOW_USE_INITIAL_FLOW:使用初始估计,存储在nextPts中;如果未设置标志,则将prevPts复制到nextPts并将其视为初始估计。
- OPTFLOW_LK_GET_MIN_EIGENVALS:使用最小特征值作为误差测量(参见minEigThreshold描述);如果没有设置标志,则将原稿周围的色块和移动点之间的L1距离除以窗口中的像素数,用作误差测量。
- minEigThreshold :算法计算光流方程的2x2正常矩阵的最小特征值,除以窗口中的像素数;如果此值小于minEigThreshold,则过滤掉相应的功能并且不处理其流程,因此它允许删除坏点并获得性能提升。
使用实例(来自《视觉SLAM十四讲》):
#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
int main( int argc, char** argv )
{
if ( argc != 2 )
{
cout<<"usage: useLK path_to_dataset"<<endl;
return 1;
}
string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";
ifstream fin( associate_file );
if ( !fin )
{
cerr<<"I cann't find associate.txt!"<<endl;
return 1;
}
string rgb_file, depth_file, time_rgb, time_depth;
list< cv::Point2f > keypoints; // 因为要删除跟踪失败的点,使用list
cv::Mat color, depth, last_color;
for ( int index=0; index<100; index++ )
{
fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
color = cv::imread( path_to_dataset+"/"+rgb_file );
depth = cv::imread( path_to_dataset+"/"+depth_file, -1 );
if (index ==0 )
{
// 对第一帧提取FAST特征点
vector<cv::KeyPoint> kps;
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
detector->detect( color, kps );
for ( auto kp:kps )
keypoints.push_back( kp.pt );
last_color = color;
continue;
}
if ( color.data==nullptr || depth.data==nullptr )
continue;
// 对其他帧用LK跟踪特征点
vector<cv::Point2f> next_keypoints;
vector<cv::Point2f> prev_keypoints;
for ( auto kp:keypoints )
prev_keypoints.push_back(kp);
vector<unsigned char> status;
vector<float> error;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"LK Flow use time:"<<time_used.count()<<" seconds."<<endl;
// 把跟丢的点删掉
int i=0;
for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++)
{
// 如果是0则跟踪失败
if ( status[i] == 0 )
{
iter = keypoints.erase(iter);
continue;
}
*iter = next_keypoints[i];
iter++;
}
cout<<"tracked keypoints: "<<keypoints.size()<<endl;
if (keypoints.size() == 0)
{
cout<<"all keypoints are lost."<<endl;
break;
}
// 画出 keypoints
cv::Mat img_show = color.clone();
for ( auto kp:keypoints )
cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);
cv::imshow("corners", img_show);
cv::waitKey(0);
last_color = color;
}
return 0;
}
具体算法推导参书本。
计算基础矩阵cv::findFundamentalMat
具体理论推导参考《SLAM十四讲》2D-2D:对极几何
基本问题模型是:如何基于两帧图像还原出相机的运动
核心公式:
对极约束:
p
2
T
K
−
T
t
∧
R
K
−
1
p
1
=
0
\mathbf{p}^T_2\mathbf{K}^{-T}\mathbf{t}^\wedge \mathbf{R}\mathbf{K}^{-1}\mathbf{p}_1=0
p2TK−Tt∧RK−1p1=0
其中
K
\mathbf{K}
K是相机内参矩阵,
R
,
t
\mathbf{R}, \mathbf{t}
R,t为两个坐标系的相机运动。
本质矩阵:
E
=
t
∧
R
\mathbf{E}=\mathbf{t}^\wedge \mathbf{R}
E=t∧R
基础矩阵:
F
=
K
−
T
E
K
−
1
\mathbf{F}=\mathbf{K}^{-T}\mathbf{E}\mathbf{K}^{-1}
F=K−TEK−1
重要性质:
x
2
T
E
x
1
=
p
2
T
F
p
1
=
0
\mathbf{x}^T_2 \mathbf{E} \mathbf{x}_1= \mathbf{p}^T_2 F \mathbf{p}_1=0
x2TEx1=p2TFp1=0
其中
x
1
,
x
2
\mathbf{x}_1, \mathbf{x}_2
x1,x2是两个像素点的归一化平面上的坐标。
估计本质矩阵的方法通常采用八点法(《视觉SLAM十四讲》),简单来说就是利用多组匹配点构建一个线性方程组,系数矩阵由特征点位置构成,然后利用SVD分解或者优化的方法计算。
使用opencv函数计算基础矩阵
CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2,
int method = FM_RANSAC,
double param1 = 3., double param2 = 0.99,
OutputArray mask = noArray() );
/** @overload */
CV_EXPORTS Mat findFundamentalMat( InputArray points1, InputArray points2,
OutputArray mask, int method = FM_RANSAC,
double param1 = 3., double param2 = 0.99 );
输入参数:
- points1 :第一幅图像点的数组,大小为2xN/Nx2 或 3xN/Nx3 (N 点的个数),多通道的1xN或Nx1也可以。点坐标应该是浮点数(双精度或单精度)。:
- points2:第二副图像的点的数组,格式、大小与第一幅图像相同。
- method :计算基本矩阵的方法
- CV_FM_7POINT – 7-点算法,点数目= 7
- CV_FM_8POINT – 8-点算法,点数目 >= 8
- CV_FM_RANSAC – RANSAC 算法,点数目 >= 8
- CV_FM_LMEDS - LMedS 算法,点数目 >= 8
- param1:这个参数只用于方法RANSAC。它是点到对极线的最大距离,超过这个值的点将被舍弃,不用于后面的计算。
- param2:这个参数只用于方法RANSAC 或 LMedS 。 它表示矩阵正确的可信度。例如可以被设为0.99 。
- status (vector<uchar>):具有N个元素的输出数组,在计算过程中没有被舍弃的点,元素被被置为1;否则置为0。这个数组只可以在方法RANSAC and LMedS 情况下使用;在其它方法的情况下,status一律被置为1。这个参数是可选参数。
函数返回:
- fundamental_matrix:
输出的基本矩阵。大小是 3x3 或者 9x3 ,(7-点法最多可返回三个矩阵)。
@code
// Example. Estimation of fundamental matrix using the RANSAC algorithm
int point_count = 100;
vector<Point2f> points1(point_count);
vector<Point2f> points2(point_count);
// initialize the points here ...
for( int i = 0; i < point_count; i++ )
{
points1[i] = ...;
points2[i] = ...;
}
Mat fundamental_matrix =
findFundamentalMat(points1, points2, FM_RANSAC, 3, 0.99);
@endcode
cv::goodFeaturesToTrack()
角点检测
CV_EXPORTS_W void goodFeaturesToTrack( InputArray image, OutputArray corners,
int maxCorners, double qualityLevel, double minDistance,
InputArray mask = noArray(), int blockSize = 3,
bool useHarrisDetector = false, double k = 0.04 );
- InputArray image:输入图像(8位或32位单通道图)
- OutputArray corners: 检测到的所有角点,类型为vector或数组,由实际给定的参数类型而定。如果是vector,那么它应该是一个包含cv::Point2f的vector对象;如果类型是cv::Mat,那么它的每一行对应一个角点,点的x、y位置分别是两列。
- int maxCorners:返回的角点的数量的最大值
- double qualityLevel:角点质量水平的最低阈值(通常是0.10到0.01之间的数值,不能大于1.0,质量最高角点的水平为1),小于该阈值的角点被拒绝
- double minDistance:返回角点之间欧式距离的最小值,用于区分相邻两个角点的最小距离(小于这个距离得点将进行合并)
- InputArray mask = noArray(): 和输入图像具有相同大小,类型必须为CV_8UC1,用来描述图像中感兴趣的区域,只在感兴趣区域中检测角点
- int blockSize = 3:表示在计算角点时参与运算的区域大小,常用值为3,但是如果图像的分辨率较高则可以考虑使用较大一点的值。
- bool useHarrisDetector = false:指示是否使用Harris角点检测,如不指定则使用shi-tomasi算法
- double k = 0.04:Harris角点检测需要的k值