挺好的讲解，首先看：
https://blog.csdn.net/weixin_38203573/article/details/79787499
https://blog.csdn.net/potxxx/article/details/86765115
内容：
二者关系
光流法
直接法
一、二者关系
引出原因：关键点和描述子计算非常耗时，可以保留特征点，使用光流法跟踪特征点运动。
关系：光流法描述像素在图像中运动，直接法利用相机运动模型计算特征点在下一时刻图像中位置。
使用条件：直接法利用图像的像素灰度信息计算相机运动，需要场景中存在明暗变化。
二、LK光流法
光流法常用来跟踪角点的运动。之后用跟踪的特征点，用ICP、PnP或对极几何估计相机运动。
适用场景：要求相机运动缓慢，采集频率高
调用函数calcOpticalFlowPyrLK
void cv::calcOpticalFlowPyrLK(InputArray prevImg,nextImg,prevPts,nextPts,
OutputArray status,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
)
1、对第一帧提取FAST特征点存到keypoints中（list类型，之后还会删除）
2、对其他帧用LK跟踪特征点
3. 更新keypoints列表，从prev_keypoints到next_keypoints
4. 画出 keypoints出为圆圈
int main(int argc, char** argv)
{
string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";
//读入txt文本到fin中
ifstream fin(associate_file);
string rgb_file, depth_file, time_rgb, time_depth;
// 因为要删除跟踪失败的点，keypoints使用list，元素类型是Point2f，坐标
// 逐帧操作的将color承接为last_color，然后color读取新的图
list< cv::Point2f > keypoints;
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);
// 1. 对第一帧提取FAST特征点存到keypoints中
if (index == 0)
{
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;
}
//实际只有10张图，后面10-99没有数据所以一直continue掉，index秒到99
if (color.data == nullptr || depth.data == nullptr)
continue;
// 2. 对其他帧用LK跟踪特征点
vector<cv::Point2f> next_keypoints;
vector<cv::Point2f> prev_keypoints;
for (auto kp : keypoints)
prev_keypoints.push_back(kp);
//调用calcOpticalFlowPyrLK函数
//status匹配状态，匹配上赋1，否则赋0；
vector<unsigned char> status;
vector<float> error;
cv::calcOpticalFlowPyrLK(last_color, color, prev_keypoints, next_keypoints, status, error);
// 3. 更新keypoints列表，从prev_keypoints到next_keypoints
int i = 0;
for (auto iter = keypoints.begin(); iter != keypoints.end(); i++)
{
//跟丢了删除，iter保持当前位置，不会++，但i会++；
//跟踪失败，next_keypoints中数据被跳过
if (status[i] == 0)
{
iter = keypoints.erase(iter);
continue;
}
//跟踪上的好点，才会让iter指向，
*iter = next_keypoints[i];
iter++;
}
cout << "tracked keypoints: " << keypoints.size() << endl;
if (keypoints.size() == 0)
{
cout << "all keypoints are lost." << endl;
break;
}
// 4. 画出 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;
}
从匹配信息associate.txt输入进去
fin >> time_rgb >> rgb_file >> time_depth >> depth_file;
13050353.359684 rgb/13050353.359684.png 1305031453.374112 depth/1305031453.374112.png
list类型的keypoints初始化第一帧检测角点后，将所有角点坐标存入。他的更新只有减少，没有增加(只有跟丢的点进行删除)
next_keypoints和prev_keypoints每帧跟踪循环内被定义的，也就是每次进循环被定义，出循环被释放.
list< Point2f > keypoints;
vector<cv::Point2f> next_keypoints;
vector<cv::Point2f> prev_keypoints;
重点注释：
int i = 0;
for (auto iter = keypoints.begin(); iter != keypoints.end(); i++)
{
if (status[i] == 0)
{
iter = keypoints.erase(iter);
continue;
}
*iter = next_keypoints[i];
iter++;
}
iter是keypoints的迭代器，正常情况下更新keypoints为下一帧特征点坐标，直接*iter=next_keypoints[i]即可。i++,iter++二者同步更新；
特殊情况下，出现跟踪失败的特征点，在keypoints先把这个位置删除，同时.erase()迭代器返回的是下一个的位置。与此同时这里的continue能结束本次循环，重新下一次循环i++，这样可以跳过这个数值，下一次*iter=next_keypoints[i]时，iter已经指向了下一个位置，同时i++了，完美赋值；
三、直接法
1、随着一批不需提取特征的方法，如SVO（选取关键点来采用直接法，这类方法称为稀疏方法（sparse））；LSD（选取整幅图像中有梯度的部分来采用直接法，这种方法称为半稠密方法（simi-dense）），直接法渐露其自身优势。DSO（Direct Sparse Odometry，稀疏直接运动估计算法），DSO的前端和LSD-SLAM相似，后端则抛弃了图优化的框架。
2、直接法将数据关联（data association）与位姿估计（pose estimation）放在了一个统一的非线性优化问题中，最小化光度误差。而特征点法则分步求解，即，先通过匹配特征点求出数据之间关联，再根据关联来估计位姿。这两步通常是独立的，在第二步中，可以通过重投影误差来判断数据关联中的外点，也可以用于修正匹配结果。
代码详解
调用函数：1、cvtColor 将图像从一个颜色空间转换为另一个颜色空间。
void cv :: cvtColor（src，dst，INT 代码）
//实例
Mat color, gray;
cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
1、直接法位姿估计的边，构造图优化 EdgeSE3ProjectDirect
g20中已有的节点和边
- 相机位姿顶点类VertexSE3Expmap（在这里直接使用）
- 3D路标点类VertexSBAPointXYZ
- 重投影误差边类EdgeProjectXYZ2UV
两个虚函数：computeError()计算误差，linearizeOplus()计算雅克比矩阵。
//自定义的光度误差 边EdgeSE3ProjectDirect，顶点为g2o中李代数位姿节点VertexSE3Expmap
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
//变量声明，3D点、内参、灰度图像指针image
public:
Eigen::Vector3d x_world_;
float cx_ = 0, cy_ = 0, fx_ = 0, fy_ = 0;
cv::Mat* image_ = nullptr;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//构造函数
EdgeSE3ProjectDirect() {}
EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image ){}
//1. 光度误差计算，3D点投影到图像平面
virtual void computeError()
{
//v是位姿pose,x_local是3D估计值
const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
//3D到2D像素点转换
float x = x_local[0]*fx_/x_local[2] + cx_;
float y = x_local[1]*fy_/x_local[2] + cy_;
//检查x y是否出界，距离图像四条边4个像素大小内为有效区域。
if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
{
_error ( 0,0 ) = 0.0;
this->setLevel ( 1 );
}
else
{
//经过在灰度图中插值获取得的灰度值getPixelValue（x,y）减去测量值灰度值
_error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
}
}
// 2. 计算线性增量，雅可比矩阵J
virtual void linearizeOplus( )
{
//先判断是否出界，重置
if ( level() == 1 )
{
_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
return;
}
//2.1 位姿估计，得到空间坐标系3D坐标
VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book
//2.2 3D转换为2D像素坐标
double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;
float u = x*fx_*invz + cx_;
float v = y*fy_*invz + cy_;
//2.3 像素对位姿偏导jacobian from se3 to u,v
Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
jacobian_uv_ksai ( 0,4 ) = 0;
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
jacobian_uv_ksai ( 1,3 ) = 0;
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
//2.4 像素梯度部分偏导，求得差分
Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
//2.5 总的雅克比是二者相乘
_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
}
virtual bool read ( std::istream& in ) {}
virtual bool write ( std::ostream& out ) const {}
protected:
//getPixelValue函数通过双线性差值获得浮点坐标对应插值后的像素值
inline float getPixelValue ( float x, float y )
{
uchar* data = & image_->data[ int ( y ) * image_->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[ image_->step ] +
xx*yy*data[image_->step+1]
);
}
};
2、直接法估计相机运动poseEstimationDirect（使用g2o）
bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
// 1.初始化g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
optimizer.setVerbose( true );
// 2.添加顶点相机位姿李代数pose
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
pose->setId ( 0 );
optimizer.addVertex ( pose );
// 3. 添加边，光度误差，一帧图上很多像素代表很多一元边
int id=1;
for ( Measurement m: measurements )
{
EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
m.pos_world,
K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
);
//这些边（误差），对应的顶点都是ID为0那一个pose顶点
edge->setVertex ( 0, pose );
//整个过程测量值只有第一帧的灰度值，后面的每一帧根据位姿找出像素点，再找到灰度值，都是估计值，
edge->setMeasurement ( m.grayscale );
//信息矩阵设置为单位阵，表征每个边的权重都一样
edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
//依次增加，给边设置一个ID
edge->setId ( id++ );
optimizer.addEdge ( edge );
}
cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
// 4. 开始优化
optimizer.initializeOptimization();
optimizer.optimize ( 30 );
Tcw = pose->estimate();
}
3、坐标转换project2Dto3D和project3Dto2D
// 1. 一次测量的值，包括一个世界坐标系下3D点与一个灰度值gray
struct Measurement
{
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}//初始化构造函数
Eigen::Vector3d pos_world;//声明成员函数
float grayscale;
};
//2. 像素坐标到3D坐标，RGBD照片d单位毫米，空间点zz单位米，scale单位换算
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
float zz = float ( d ) /scale;
float xx = zz* ( x-cx ) /fx;
float yy = zz* ( y-cy ) /fy;
return Eigen::Vector3d ( xx, yy, zz );
}
//3. 3D到像素坐标
inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{
float u = fx*x/z+cx;
float v = fy*y/z+cy;
return Eigen::Vector2d ( u,v );
}
4、主框架
int main ( int argc, char** argv )
{
//根据时间生成随机数
srand ( ( unsigned int ) time ( 0 ) );
string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";
ifstream fin ( associate_file );
string rgb_file, depth_file, time_rgb, time_depth;
cv::Mat color, depth, gray;
//Measurement类存储世界坐标点（以第一帧为参考的FAST关键点）和对应的灰度图像（由color->gray）的灰度值
vector<Measurement> measurements;
// 相机内参K
float cx = 325.5;
float cy = 253.5;
float fx = 518.0;
float fy = 519.0;
//RGBD相机单位毫米，3D点单位米
float depth_scale = 1000.0;
Eigen::Matrix3f K;
K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
// 位姿，变换矩阵T
Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();
cv::Mat prev_color;
// 我们以第一个图像为参考，对后续图像和参考图像做直接法
//每一副图像都会与第一帧图像做直接法计算第一帧到当前帧的R,t。
// 参考帧一直是第一帧，而不是循环流动当前帧的上一帧为参考帧
for ( int index=0; index<10; index++ )
{
cout<<"*********** loop "<<index<<" ************"<<endl;
//读入颜色和深度图像
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 ( color.data==nullptr || depth.data==nullptr )
continue;
//转换后的灰度图为g2o优化需要的边提供灰度值
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
// 1. 对第一帧提取FAST特征点
//遍历第一帧特征点数组，筛选，把3D坐标和灰度值放到measurement中
if ( index ==0 )
{
vector<cv::KeyPoint> keypoints;
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
detector->detect ( color, keypoints );
for ( auto kp:keypoints )
{
// 1.1去掉邻近边缘处的点
if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
continue;
// 1.2 深度图上关键点的深度值d ，cvRound()返回整数
ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
if ( d==0 )
continue;
// 找到深度值d后，2D点投影到3D世界坐标系
Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
//1.3 灰度图上关键点灰度值
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
//得到三维3D坐标和对应的灰度值，作为测量值
measurements.push_back ( Measurement ( p3d, grayscale ) );
}
//第一帧，赋值给pre_color，为什么用clone？？？
prev_color = color.clone();
continue;
}
// 2. 使用直接法计算相机运动T
//measurements是不变的，之后不断读入的fray灰度图变化
poseEstimationDirect ( measurements, &gray, K, Tcw );
cout<<"Tcw="<<Tcw.matrix() <<endl;
// 3.构建一张图画出后续的帧跟第一帧对比的效果
cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
//摆完两张图后画上直接法跟踪的关键点和连线
for ( Measurement m:measurements )
{
if ( rand() > RAND_MAX/5 )
continue;
//空间点3D坐标，及在第一帧的像素坐标
Eigen::Vector3d p = m.pos_world;
Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
//坐标变换T后，
Eigen::Vector3d p2 = Tcw*m.pos_world;
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
continue;
//随机色使用
float b = 255*float ( rand() ) /RAND_MAX;
float g = 255*float ( rand() ) /RAND_MAX;
float r = 255*float ( rand() ) /RAND_MAX;
//画跟踪的特征点圆和匹配直线
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
}
cv::imshow ( "result", img_show );
cv::waitKey ( 0 );
}
return 0;
}
2034
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
