VINS(feature_traker模块)细节笔记
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图像消息的话题，然后转换成cv::Mat格式，在上面画一个愿并且使用OpenCV显示图像，最后将其转换成ROS消息发布。
在你的package.xml和CMakeLists.xml（或者在你使用catkin_create_pkg时）添加一下依赖：
sensor_msgs
cv_bridge
roscpp
std_msgs
image_transport
具体节点代码：
#include 接下来我们对代码进行一个分解：
#include 使用在ROS中发布和订阅图像image_transport允许您订阅压缩图像流。 请记住在package.xml中添加image_transport的依赖配置。
#include 包含CvBridge的头文件以及image encodings(包含了很多有用的常量和函数)，记得在package.xml中添加cv_bridge的依赖配置。
#include 包含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 - 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
#include 具体算法推导参书本。
计算基础矩阵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
) ：具有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值