一、利用cv的光流追踪函数进行光流跟踪
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
)
//使用具有金字塔的迭代Lucas-Kanade方法计算稀疏特征集的光流。
//参数：
//
//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，则过滤掉相应的功能并且不处理其流程，因此它允许删除坏点并获得性能提升。
//该函数实现了金字塔中Lucas-Kanade光流的稀疏迭代版本
源码如下：
//
// Created by nnz on 2020/11/6.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace cv;
using namespace Eigen;
string file_1 = "../LK1.png"; // first image
string file_2 = "../LK2.png"; // second image
int main(int argc,char ** argv)
{
cout<<"*******************************using opencv*************************"<<endl;
//读入图片
Mat img1=imread(file_1,0);
Mat img2=imread(file_2,0);
//获取keypoints
//定义容器存放keypoints
vector<KeyPoint> kp1;
//cv::FAST(img1,keypoints1,40);//这是直接提取Fast角点
// 1）参数maxCorners：检测到的最大角点数量；
//（2）参数qualityLevel：输出角点的质量等级，取值范围是 [ 0 , 1 ]；如果某个候选点的角点响应值小于（qualityLeve * 最大角点响应值），则该点会被抛弃，相当于判定某候选点为角点的阈值；
//（3）参数minDistance：两个角点间的最小距离，如果某两个角点间的距离小于minDistance，则会被认为是同一个角点；
//（4）参数mask：如果有该掩膜，则只计算掩膜内的角点；
//（5）参数blockSize：计算角点响应值的邻域大小，默认值为3；如果输入图像的分辨率比较大，可以选择比较大的blockSize；
//（6）参数useHarrisDector：布尔类型，如果为true则使用Harris角点检测；默认为false，使用shi-tomas角点检测算法；
//（7）参数k：只在使用Harris角点检测时才生效，也就是计算角点响应值时的系数k。
//建立GFTT角点探测器
Ptr<GFTTDetector> detector=GFTTDetector::create(500,0.01,20);
//算出图1的keypoints-kp1
detector->detect(img1,kp1);
//在图2中追踪图1的keypoints
vector<Point2f> pt1, pt2;//要把Keypoint类型转为Point2f
for (auto &kp: kp1) pt1.push_back(kp.pt);
vector<uchar> status;
vector<float> error;
cv::calcOpticalFlowPyrLK(img1,img2,pt1,pt2,status,error);
Mat img2_CV;
cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);
for (int i = 0; i < pt2.size(); i++)
{
if (status[i])//status[i]表示在图2中是否跟踪到图1中对应的点
{
cv::circle(img2_CV, pt2[i], 2, cv::Scalar(100, 250, 50), 2);//在图2中用圆标出跟踪到的点
cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(100, 250, 50));//
}
}
cv::imshow("tracked by opencv", img2_CV);
cv::waitKey(0);
cout<<"*******************************using opencv*************************"<<endl;
return 0;
}
/*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
) */
//使用具有金字塔的迭代Lucas-Kanade方法计算稀疏特征集的光流。
//参数：
//
//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，则过滤掉相应的功能并且不处理其流程，因此它允许删除坏点并获得性能提升。
//该函数实现了金字塔中Lucas-Kanade光流的稀疏迭代版本
/*
功能：绘制连接两个点的线段
函数原型：
void cvLine( CvArr img, CvPoint pt1, CvPoint pt2, CvScalar color, int thickness=1, int line_type=8, int shift=0 );*
参数说明：
img 图像。
pt1 线段的第一个端点。
pt2 线段的第二个端点。
color 线段的颜色。
thickness 线段的粗细程度。
line_type 线段的类型。
8 (or 0) - 8-connected line（8邻接)连接 线。
4 - 4-connected line(4邻接)连接线。
CV_AA - antialiased 线条。shift 坐标点的小数点位数。*/
/*
cvCircle(CvArr* img, CvPoint center, int radius, CvScalar color, int thickness=1, int lineType=8, int shift=0)
img为图像指针，单通道多通道都行，不须要特殊要求
center为画圆的圆心坐标
radius为圆的半径
color为设定圆的颜色，比方用CV_RGB(255, 0,0)设置为红色
thickness为设置圆线条的粗细，值越大则线条越粗，为负数则是填充效果*/
运行结果如下：
二、手写单层光流追踪-高斯牛顿-非线性优化
源码如下：
//
// Created by nnz on 2020/11/6.
//
/*理一下计算光流的的步骤
以关键点为中心 从上下左右扩展一个4*4的小块 假设小块的移动是一样的，所以把这个块里的关键点一起高斯牛顿
利用非线性 高斯牛顿 来优化光流
自变量是 dx dy 也就是在图2中把图1关键点的位置在水平上加dx 在纵向加dy
e=I(x,y)(图1)-I(x+dx,y+dy)(图2)
cost=e*e
雅克比 J矩阵则是 I在水平方向上的梯度 和 I在纵向上的梯度 构成（图2）
H=J*J^T
b=-e*J
Hdx=b
*/
//单层光流的原理
//把原始图片的点都算一遍（用块的形式）
/*static Ptr<GFTTDetector> create( int maxCorners=1000, double qualityLevel=0.01, double minDistance=1,
int blockSize=3, bool useHarrisDetector=false, double k=0.04 );*/
// 1）参数maxCorners：检测到的最大角点数量；
//（2）参数qualityLevel：输出角点的质量等级，取值范围是 [ 0 , 1 ]；如果某个候选点的角点响应值小于（qualityLeve * 最大角点响应值），则该点会被抛弃，相当于判定某候选点为角点的阈值；
//（3）参数minDistance：两个角点间的最小距离，如果某两个角点间的距离小于minDistance，则会被认为是同一个角点；
//（4）参数mask：如果有该掩膜，则只计算掩膜内的角点；
//（5）参数blockSize：计算角点响应值的邻域大小，默认值为3；如果输入图像的分辨率比较大，可以选择比较大的blockSize；
//（6）参数useHarrisDector：布尔类型，如果为true则使用Harris角点检测；默认为false，使用shi-tomas角点检测算法；
//（7）参数k：只在使用Harris角点检测时才生效，也就是计算角点响应值时的系数k。
#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace cv;
using namespace Eigen;
string file_1 = "../LK1.png"; // first image
string file_2 = "../LK2.png"; // second image
//定义光流追踪的类
class OpticalFlowTracker {
public:
OpticalFlowTracker(
const Mat &img1_,
const Mat &img2_,
const vector<KeyPoint> &kp1_,
vector<KeyPoint> &kp2_,
vector<bool> &success_,
bool inverse_ = true, bool has_initial_ = false) :
img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
has_initial(has_initial_) {} //初始化
void calculateOpticalFlow(const Range &range);//这是计算光流的函数
private://属性
const Mat &img1;
const Mat &img2;
const vector<KeyPoint> &kp1;
vector<KeyPoint> &kp2;
vector<bool> &success;
bool inverse = true;
bool has_initial = false;
};
//单层光流
void OpticalFlowSingleLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse = false,
bool has_initial_guess = false
);
//计算灰度
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
// boundary check
if (x < 0) x = 0;
if (y < 0) y = 0;
if (x >= img.cols) x = img.cols - 1;
if (y >= img.rows) y = img.rows - 1;
uchar *data = &img.data[int(y) * img.step + int(x)];
float xx = x - floor(x);
float yy = y - floor(y);
return float(
(1 - xx) * (1 - yy) * data[0] +
xx * (1 - yy) * data[1] +
(1 - xx) * yy * data[img.step] +
xx * yy * data[img.step + 1]
);
}
int main(int argc,char ** argv)
{
cout<<"*******************************using singl-LK_by_gn*************************"<<endl;
//读入图片
Mat img1=imread(file_1,0);
Mat img2=imread(file_2,0);
//获取keypoints
//定义容器存放keypoints
vector<KeyPoint> kp1;
//cv::FAST(img1,keypoints1,40);//这是直接提取Fast角点
//建立GFTT角点探测器
Ptr<GFTTDetector> detector=GFTTDetector::create(500,0.01,20);
//算出图1的keypoints-kp1
detector->detect(img1,kp1);
//在图2中追踪图1的keypoints
vector<KeyPoint> kp2_single;
vector<bool> success_single;
OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);
Mat img2_single;
cv::cvtColor(img2, img2_single, CV_GRAY2BGR);
for (int i = 0; i < kp2_single.size(); i++) {
if (success_single[i]) {
cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));
}
}
cv::imshow("tracked by single-gn ", img2_single);
cv::waitKey(0);
cout<<"*******************************using singl-LK_by_gn*************************"<<endl;
return 0;
}
void OpticalFlowSingleLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse ,
bool has_initial
)
{
kp2.resize(kp1.size());//让kp2的大小和 kp1一样
success.resize(kp1.size());//每一个追踪成功与否的标志
OpticalFlowTracker tracker(img1,img2,kp1,kp2,success,inverse,has_initial);//初始化 构建光流追踪类
//并行循环 std还没有学到这个，看不太懂，暂且当做计算光流函数调用？
parallel_for_(Range(0,kp1.size()),
std::bind(&OpticalFlowTracker::calculateOpticalFlow,&tracker,placeholders::_1));
}
//类外实现成员函数
void OpticalFlowTracker::calculateOpticalFlow(const Range &range)
{
//
int half_patch_size=4;//4*4的块的边
int iterations=10;//迭代次数
for(size_t i=range.start;i<range.end;i++)
{
auto kp=kp1[i];//用kp存图1中的关键点
double dx=0,dy=0;//待优化系数初始值
if(has_initial)//待优化的系数如果需要初始化 在刚开始的时候不可能进行下面的操作，因为kp2这时候没有点
{
dx=kp2[i].pt.x-kp.pt.x;
dy=kp2[i].pt.y-kp.pt.y;
}
double cost = 0, lastCost = 0;
bool succ = true; // indicate if this point succeeded
//进行高斯牛顿的迭代
Eigen::Matrix2d H = Eigen::Matrix2d::Zero(); // hessian
Eigen::Vector2d b = Eigen::Vector2d::Zero(); // bias
Eigen::Vector2d J; // jacobian
for(int it=0;it<iterations;it++)
{
if(inverse==false)//默认 inverse=false
{
//对 H ,b 进行初始化
H = Eigen::Matrix2d::Zero();
b = Eigen::Vector2d::Zero();
}
else
{
//只对b进行初始化
b = Eigen::Vector2d::Zero();
}
cost=0;//每次迭代cost都要先置为0
//计算 cost 和雅克比 J
//4*4的块，假设在这个块里的关键点的移动都是一样的，所以直接对这个4*4的块进行高斯牛顿
for (int x = -half_patch_size; x < half_patch_size; x++)
{
for (int y = -half_patch_size; y < half_patch_size; y++)
{ //计算e=I(x,y)(图1)-I(x+dx,y+dy)(图2)
double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;
if(inverse==false)
{ //这里乘了一个-1 ，表示梯度是减小的
J = -1.0 * Eigen::Vector2d(
0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
);
}
else if(i==0)//如果是第一次迭代，雅克比矩阵用图1的点来算
{
// in inverse mode, J keeps same for all iterations
// NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
J = -1.0 * Eigen::Vector2d(
0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
);
}
// compute H, b and set cost; 这里应该就很熟悉了 见 SLAM十四讲p129页
b += -error * J;
cost += error * error;
if (inverse == false || it == 0)
{
// also update H
H += J * J.transpose();
}
}//一次迭代的高斯牛顿相关矩阵计算结束
}
//计算增量 update
Eigen::Vector2d update = H.ldlt().solve(b);
if (std::isnan(update[0]))
{//判断增量有没有用
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
succ = false;//表示追踪失败
break;
}
if (it > 0 && cost > lastCost)
{//满足条件就停止迭代
break;
}
//更新优化系数
dx+=update[0];
dy+=update[1];
lastCost=cost;
succ=true;//这个快跟踪成功
if (update.norm() < 1e-2)
{// 如果增量比较小了，就停止迭代
// converge
break;
}
}
//结束迭代
success[i]=succ;
kp2[i].pt= kp.pt + Point2f(dx, dy);
}//所有的点迭代完
}
运行结果如下：
三、手写多层光流-高斯牛顿-非线性优化
源码如下：
//
// Created by nnz on 2020/11/7.
//
//
// Created by nnz on 2020/11/6.
//
/*理一下计算光流的的步骤
以关键点为中心 从上下左右扩展一个4*4的小块 假设小块的移动是一样的，所以把这个块里的关键点一起高斯牛顿
利用非线性 高斯牛顿 来优化光流
自变量是 dx dy 也就是在图2中把图1关键点的位置在水平上加dx 在纵向加dy
e=I(x,y)(图1)-I(x+dx,y+dy)(图2)
cost=e*e
雅克比 J矩阵则是 I在水平方向上的梯度 和 I在纵向上的梯度 构成（图2）
H=J*J^T
b=-e*J
Hdx=b
*/
//单层光流的原理
//把原始图片的点都算一遍（用块的形式）
/*static Ptr<GFTTDetector> create( int maxCorners=1000, double qualityLevel=0.01, double minDistance=1,
int blockSize=3, bool useHarrisDetector=false, double k=0.04 );*/
// 1）参数maxCorners：检测到的最大角点数量；
//（2）参数qualityLevel：输出角点的质量等级，取值范围是 [ 0 , 1 ]；如果某个候选点的角点响应值小于（qualityLeve * 最大角点响应值），则该点会被抛弃，相当于判定某候选点为角点的阈值；
//（3）参数minDistance：两个角点间的最小距离，如果某两个角点间的距离小于minDistance，则会被认为是同一个角点；
//（4）参数mask：如果有该掩膜，则只计算掩膜内的角点；
//（5）参数blockSize：计算角点响应值的邻域大小，默认值为3；如果输入图像的分辨率比较大，可以选择比较大的blockSize；
//（6）参数useHarrisDector：布尔类型，如果为true则使用Harris角点检测；默认为false，使用shi-tomas角点检测算法；
//（7）参数k：只在使用Harris角点检测时才生效，也就是计算角点响应值时的系数k。
#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace cv;
using namespace Eigen;
string file_1 = "../LK1.png"; // first image
string file_2 = "../LK2.png"; // second image
//定义光流追踪的类
class OpticalFlowTracker {
public:
OpticalFlowTracker(
const Mat &img1_,
const Mat &img2_,
const vector<KeyPoint> &kp1_,
vector<KeyPoint> &kp2_,
vector<bool> &success_,
bool inverse_ = true, bool has_initial_ = false) :
img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
has_initial(has_initial_) {} //初始化
void calculateOpticalFlow(const Range &range);//这是计算光流的函数
private://属性
const Mat &img1;
const Mat &img2;
const vector<KeyPoint> &kp1;
vector<KeyPoint> &kp2;
vector<bool> &success;
bool inverse = true;
bool has_initial = false;
};
//单层光流
void OpticalFlowSingleLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,//追踪成功与否的标志
bool inverse = false,
bool has_initial_guess = false
);
//多层光流
void OpticalFlowMultiLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse=false);
//计算灰度
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
// boundary check
if (x < 0) x = 0;
if (y < 0) y = 0;
if (x >= img.cols) x = img.cols - 1;
if (y >= img.rows) y = img.rows - 1;
uchar *data = &img.data[int(y) * img.step + int(x)];//step[0]是矩阵中一行元素的字节数。 step[1]是矩阵中一个元素的字节数
float xx = x - floor(x);//floor是向下 取整// 取小数部分
// 如： floor(3.14) = 3.0
//floor(9.999999) = 9.0
//floor(-3.14) = -4.0
//floor(-9.999999) = -10
float yy = y - floor(y);//取小数部分
return float(
(1 - xx) * (1 - yy) * data[0] + //data： uchar类型的指针，指向Mat数据矩阵的首地址。可以理解为标示一个房屋的门牌号
xx * (1 - yy) * data[1] +
(1 - xx) * yy * data[img.step] +
xx * yy * data[img.step + 1]
);
}
int main(int argc,char ** argv)
{
cout<<"*******************************using mult-LK_by_gn*************************"<<endl;
//读入图片
Mat img1=imread(file_1,0);
Mat img2=imread(file_2,0);
//获取keypoints
//定义容器存放keypoints
vector<KeyPoint> kp1;
//cv::FAST(img1,keypoints1,40);//这是直接提取Fast角点
//建立GFTT角点探测器
Ptr<GFTTDetector> detector=GFTTDetector::create(500,0.01,20);
//算出图1的keypoints-kp1
detector->detect(img1,kp1);
//在图2中追踪图1的keypoints
vector<KeyPoint> kp2_mult;
vector<bool> success_mult;
OpticalFlowMultiLevel(img1,img2,kp1,kp2_mult,success_mult,true);
Mat img2_single;
cv::cvtColor(img2, img2_single, CV_GRAY2BGR);
for (int i = 0; i < kp2_mult.size(); i++) {
if (success_mult[i]) {
cv::circle(img2_single, kp2_mult[i].pt, 2, cv::Scalar(0, 250, 0), 2);
cv::line(img2_single, kp1[i].pt, kp2_mult[i].pt, cv::Scalar(0, 250, 0));
}
}
cv::imshow("tracked by mult-gn ", img2_single);
cv::waitKey(0);
cout<<"*******************************using mult-LK_by_gn*************************"<<endl;
return 0;
}
//类外实现成员函数
void OpticalFlowTracker::calculateOpticalFlow(const Range &range)
{
//
int half_patch_size=4;//4*4的块的边
int iterations=10;//迭代次数
for(size_t i=range.start;i<range.end;i++)
{
auto kp=kp1[i];//用kp存图1中的关键点
double dx=0,dy=0;//待优化系数初始值
if(has_initial)//待优化的系数如果需要初始化 在刚开始的时候不可能进行下面的操作，因为kp2这时候没有点
{
dx=kp2[i].pt.x-kp.pt.x;
dy=kp2[i].pt.y-kp.pt.y;
}
double cost = 0, lastCost = 0;
bool succ = true; // indicate if this point succeeded
//进行高斯牛顿的迭代
Eigen::Matrix2d H = Eigen::Matrix2d::Zero(); // hessian
Eigen::Vector2d b = Eigen::Vector2d::Zero(); // bias
Eigen::Vector2d J; // jacobian
for(int it=0;it<iterations;it++)
{
if(inverse==false)//默认 inverse=false
{
//对 H ,b 进行初始化
H = Eigen::Matrix2d::Zero();
b = Eigen::Vector2d::Zero();
}
else
{
//只对b进行初始化
b = Eigen::Vector2d::Zero();
}
cost=0;//每次迭代cost都要先置为0
//计算 cost 和雅克比 J
//4*4的块，假设在这个块里的关键点的移动都是一样的，所以直接对这个4*4的块进行高斯牛顿
for (int x = -half_patch_size; x < half_patch_size; x++)
{
for (int y = -half_patch_size; y < half_patch_size; y++)
{ //计算e=I(x,y)(图1)-I(x+dx,y+dy)(图2)
double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;
if(inverse==false)
{ //这里乘了一个-1 ，表示梯度是减小的
J = -1.0 * Eigen::Vector2d(
0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
);
}
else if(it==0)//如果是第一次迭代，雅克比矩阵用图1的点来算
{
// in inverse mode, J keeps same for all iterations
// NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
J = -1.0 * Eigen::Vector2d(
0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
);
}
// compute H, b and set cost; 这里应该就很熟悉了 见 SLAM十四讲p129页
b += -error * J;
cost += error * error;
if (inverse == false || it == 0)
{
// also update H
H += J * J.transpose();
}
}//一次迭代的高斯牛顿相关矩阵计算结束
}
//计算增量 update
Eigen::Vector2d update = H.ldlt().solve(b);
if (std::isnan(update[0]))
{//判断增量有没有用
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
succ = false;//表示追踪失败
break;
}
if (it > 0 && cost > lastCost)
{//满足条件就停止迭代
break;
}
//更新优化系数
dx+=update[0];
dy+=update[1];
lastCost=cost;
succ=true;//这个快跟踪成功
if (update.norm() < 1e-2)
{// 如果增量比较小了，就停止迭代
// converge
break;
}
}
//结束迭代
success[i]=succ;
kp2[i].pt= kp.pt + Point2f(dx, dy);
}//所有的点迭代完
}
//单层光流
void OpticalFlowSingleLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse ,
bool has_initial
)
{
kp2.resize(kp1.size());//让kp2的大小和 kp1一样 因为多级光流有让kp2初始化 因此得让kp2有和 kp1一样的大小
success.resize(kp1.size());//每一个追踪成功与否的标志
OpticalFlowTracker tracker(img1,img2,kp1,kp2,success,inverse,has_initial);//初始化 构建光流追踪类
//并行循环 std还没有学到这个，看不太懂，暂且当做计算光流函数调用？
parallel_for_(Range(0,kp1.size()),
std::bind(&OpticalFlowTracker::calculateOpticalFlow,&tracker,placeholders::_1));
}
//多层光流
void OpticalFlowMultiLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse){
// parameters
int pyramids = 4;//金字塔的层数 也就是要降采样四次
double pyramid_scale = 0.5;//缩放比例
double scales[] = {1.0, 0.5, 0.25, 0.125};//分别是 从原图缩放到1/8
// create pyramids
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
vector<Mat> pyr1, pyr2; // image pyramids 容器，分别存放 不同缩放比例的图片
//利用循环得到四层不同比例的图片
for (int i = 0; i < pyramids; i++) {
if (i == 0) {
pyr1.push_back(img1);//pyr1[0] 和 pyr2[0] 放的分别是图1和图2的原图
pyr2.push_back(img2);
} else {
Mat img1_pyr, img2_pyr;//降采样后临时存放在 img1_pyr, img2_pyr
//利用resize函数，调整 重置img1_pyr, img2_pyr的尺寸，
cv::resize(pyr1[i - 1], img1_pyr,
cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
cv::resize(pyr2[i - 1], img2_pyr,
cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
pyr1.push_back(img1_pyr);
pyr2.push_back(img2_pyr);
}
}
//完成图像的缩放
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "build pyramid time: " << time_used.count() << endl;
//接下来缩放关键点咯
// coarse-to-fine LK tracking in pyramids
vector<KeyPoint> kp1_pyr, kp2_pyr;
//这个循环得到的是最小的那一层缩放后的点
for (auto &kp:kp1) {
auto kp_top = kp;
kp_top.pt *= scales[pyramids - 1];
kp1_pyr.push_back(kp_top);
kp2_pyr.push_back(kp_top);
}
//循环从最小的那一层开始，然后慢慢恢复到原始图片
for (int level = pyramids - 1; level >= 0; level--)
{
// from coarse to fine
success.clear();
t1 = chrono::steady_clock::now();
//这里应该就可以知道 has_initial是什么了，就是看图2中的点有没有初始化呗
OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse, true);
t2 = chrono::steady_clock::now();
auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "track pyr " << level << " cost time: " << time_used.count() << endl;
if (level > 0) {
for (auto &kp: kp1_pyr)
kp.pt /= pyramid_scale;//这里是 点*2 相当于把点放大为上一层的点（图1中的点）
for (auto &kp: kp2_pyr)
kp.pt /= pyramid_scale;//这里是 点*2 相当于把点放大为上一层的点（图2中预测的点）
}
}
//把四层都走完了
// 最后再把走完四次降采样的点返回到kp2
for (auto &kp: kp2_pyr)
kp2.push_back(kp);
}
运行结果如下：
四、利用直接法进行位姿估计-高斯牛顿-非线性优化
源码如下：
//图片是从left 到1 left到2 、、、、、、left 到5 其中通过视差图得到深度信息
#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>
using namespace std;
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;//相机内参
// baseline
double baseline = 0.573;
// paths
string left_file = "./left.png";
string disparity_file = "./disparity.png";
boost::format fmt_others("./%06d.png"); // other files
// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
/// class for accumulator jacobians in parallel
class JacobianAccumulator {
public:
JacobianAccumulator(
const cv::Mat &img1_,
const cv::Mat &img2_,
const VecVector2d &px_ref_,
const vector<double> depth_ref_,
Sophus::SE3d &T21_) :
img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
}
/// accumulate jacobians in a range
void accumulate_jacobian(const cv::Range &range);
/// get hessian matrix
Matrix6d hessian() const { return H; }
/// get bias
Vector6d bias() const { return b; }
/// get total cost
double cost_func() const { return cost; }
/// get projected points
VecVector2d projected_points() const { return projection; }
/// reset h, b, cost to zero
void reset() {
H = Matrix6d::Zero();
b = Vector6d::Zero();
cost = 0;
}
private:
const cv::Mat &img1;
const cv::Mat &img2;
const VecVector2d &px_ref;//左图的像素点
const vector<double> depth_ref;//左图的像素点对应的深度
Sophus::SE3d &T21;//从左图变换到友图的变换矩阵
VecVector2d projection; // projected points
std::mutex hessian_mutex;//上锁
// 初始化 H b
Matrix6d H = Matrix6d::Zero();
Vector6d b = Vector6d::Zero();
double cost = 0;
};
/**
* pose estimation using direct method
* @param img1
* @param img2
* @param px_ref
* @param depth_ref
* @param T21
*/
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3d &T21
);
/**
* pose estimation using direct method
* @param img1
* @param img2
* @param px_ref
* @param depth_ref
* @param T21
*/
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3d &T21
);
// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
// boundary check
if (x < 0) x = 0;
if (y < 0) y = 0;
if (x >= img.cols) x = img.cols - 1;
if (y >= img.rows) y = img.rows - 1;
uchar *data = &img.data[int(y) * img.step + int(x)];
float xx = x - floor(x);
float yy = y - floor(y);
return float(
(1 - xx) * (1 - yy) * data[0] +
xx * (1 - yy) * data[1] +
(1 - xx) * yy * data[img.step] +
xx * yy * data[img.step + 1]
);
}
int main(int argc, char **argv) {
cv::Mat left_img = cv::imread(left_file, 0);
cv::Mat disparity_img = cv::imread(disparity_file, 0);
// let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
cv::RNG rng;
int nPoints = 2000;
int boarder = 20;
VecVector2d pixels_ref;
vector<double> depth_ref;
// generate pixels in ref and load depth data
//利用opencv的随机产生器，我们在边界以内随机产生2000个点
for (int i = 0; i < nPoints; i++) {
int x = rng.uniform(boarder, left_img.cols - boarder); // don't pick pixels close to boarder
int y = rng.uniform(boarder, left_img.rows - boarder); // don't pick pixels close to boarder
int disparity = disparity_img.at<uchar>(y, x);
double depth = fx * baseline / disparity; // you know this is disparity to depth 这是利用视差求深度
//公式就是fx * baseline / disparity baseline是基线，disparity是视差
depth_ref.push_back(depth); //得到左图像素点的深度信息
pixels_ref.push_back(Eigen::Vector2d(x, y));//得到左图中随机产生的像素点 放在pixels_ref中
}
// estimates 01~05.png's pose using this information
Sophus::SE3d T_cur_ref;
//利用直接法求出左图与5个图的位姿变幻
for (int i = 1; i < 6; i++) { // 1~10
cv::Mat img = cv::imread((fmt_others % i).str(), 0);
// try single layer by uncomment this line
// DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
}
return 0;
}
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3d &T21) {
const int iterations = 10;
double cost = 0, lastCost = 0;
auto t1 = chrono::steady_clock::now();
JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);//定义雅克比类，调用有参构造初始化jaco_accu
//迭代
for (int iter = 0; iter < iterations; iter++)
{
// H = Matrix6d::Zero();b = Vector6d::Zero();cost = 0;
jaco_accu.reset();//功能就是上面的注释，也就是初始化
//和手写光流一样，利用并行循环，调用雅克比类里面的计算雅克比的函数
cv::parallel_for_(cv::Range(0, px_ref.size()),
std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
Matrix6d H = jaco_accu.hessian();//得到H矩阵
Vector6d b = jaco_accu.bias();//得到b矩阵
// solve update and put it into estimation
Vector6d update = H.ldlt().solve(b);//求增量
T21 = Sophus::SE3d::exp(update) * T21;//更新待优化系数，也就是位姿
cost = jaco_accu.cost_func();//得到cost
if (std::isnan(update[0])) //判断更新量是否有效
{
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
break;
}
if (iter > 0 && cost > lastCost)//满足这个条件停止迭代
{
cout << "cost increased: " << cost << ", " << lastCost << endl;
break;
}
if (update.norm() < 1e-3)//如果增量足够小，也停止迭代
{
// converge
break;
}
lastCost = cost;
cout << "iteration: " << iter << ", cost: " << cost << endl;
}
cout << "T21 = \n" << T21.matrix() << endl;
auto t2 = chrono::steady_clock::now();
auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "direct method for single layer: " << time_used.count() << endl;
// plot the projected pixels here
cv::Mat img2_show;
cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
VecVector2d projection = jaco_accu.projected_points();
for (size_t i = 0; i < px_ref.size(); ++i) {
auto p_ref = px_ref[i];
auto p_cur = projection[i];
if (p_cur[0] > 0 && p_cur[1] > 0) {
cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
cv::Scalar(0, 250, 0));
}
}
cv::imshow("current", img2_show);
cv::waitKey();
}
void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {
// parameters
const int half_patch_size = 1;//这里又是一个边界
int cnt_good = 0;
Matrix6d hessian = Matrix6d::Zero();
Vector6d bias = Vector6d::Zero();
double cost_tmp = 0;
//遍历所有的点
for (size_t i = range.start; i < range.end; i++) {
// compute the projection in the second image
Eigen::Vector3d point_ref =
depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);//左图相机下的坐标
Eigen::Vector3d point_cur = T21 * point_ref;//得到右图下相机的坐标 里面的额T21是待优化的位姿
if (point_cur[2] < 0) // depth invalid
continue;
float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;//得到右图下对应的像素点
//判断这个点是不是在边界里面
if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
v > img2.rows - half_patch_size)
continue;
projection[i] = Eigen::Vector2d(u, v);//把右图的像素点放在 projection[i]里面进行保存
double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
Z2 = Z * Z, Z_inv = 1.0 / Z , Z2_inv = Z_inv * Z_inv;
cnt_good++;
// and compute error and jacobian
//以像素点为中心 上下左右扩展 1 的小块里，这里的原因应该和光流一样，假设这个小块里面的点的移动是一样的
for (int x = -half_patch_size; x <= half_patch_size; x++)
for (int y = -half_patch_size; y <= half_patch_size; y++) {
double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
GetPixelValue(img2, u + x, v + y);
Matrix26d J_pixel_xi;
Eigen::Vector2d J_img_pixel;
//下面的雅克比矩阵是书上的公式 雅克比矩阵分成了两个部分，一个是I对水平方向的梯度 一个是水平方向对位姿（李代数）的梯度
J_pixel_xi(0, 0) = fx * Z_inv;
J_pixel_xi(0, 1) = 0;
J_pixel_xi(0, 2) = -fx * X * Z2_inv;
J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
J_pixel_xi(0, 5) = -fx * Y * Z_inv;
J_pixel_xi(1, 0) = 0;
J_pixel_xi(1, 1) = fy * Z_inv;
J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
J_pixel_xi(1, 5) = fy * X * Z_inv;
J_img_pixel = Eigen::Vector2d(
0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
);
// total jacobian
Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();//得到雅克比矩阵
hessian += J * J.transpose();//得到H矩阵
bias += -error * J;//b矩阵
cost_tmp += error * error;
}
}
if (cnt_good) {
// set hessian, bias and cost
unique_lock<mutex> lck(hessian_mutex);
H += hessian;
b += bias;
cost += cost_tmp / cnt_good;
}
}
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3d &T21) {
// parameters
int pyramids = 4;
double pyramid_scale = 0.5;
double scales[] = {1.0, 0.5, 0.25, 0.125};
// create pyramids //构建金字塔
vector<cv::Mat> pyr1, pyr2; // image pyramids
for (int i = 0; i < pyramids; i++) {
if (i == 0) {
pyr1.push_back(img1);
pyr2.push_back(img2);
} else {
cv::Mat img1_pyr, img2_pyr;
cv::resize(pyr1[i - 1], img1_pyr,
cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
cv::resize(pyr2[i - 1], img2_pyr,
cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
pyr1.push_back(img1_pyr);//左图
pyr2.push_back(img2_pyr);//右图
}
}
double fxG = fx, fyG = fy, cxG = cx, cyG = cy; // backup the old values
//利用循环，我们可以从最小层到最大层（原始层）
for (int level = pyramids - 1; level >= 0; level--) {
VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
for (auto &px: px_ref) {
px_ref_pyr.push_back(scales[level] * px);
}
// scale fx, fy, cx, cy in different pyramid levels 这里面的内参也要缩放 但深度不用，因为深度不变
fx = fxG * scales[level];
fy = fyG * scales[level];
cx = cxG * scales[level];
cy = cyG * scales[level];
//对不同不理的图片进行单层的位姿估计，
DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
}
}
运行结果：
slam十四讲-直接法实验
832
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
