7.2.2

什么是特征点、如何提取和匹配特征点、如何根据配对的特征点估计相机运动。

——————————

如何提取、匹配图像特征点然后估计两帧之间的相机运动和场景结构。

【两帧间视觉里程计】也称为

两视图几何

project(vo1)set(CMAKE_CXX_STANDARD

17)

${PROJECT_SOURCE_DIR}/cmake)find_package(OpenCV

4.2.0

REQUIRED)include_directories(${OpenCV_INCLUDE_DIRS}${G2O_INCLUDE_DIRS}${Sophus_INCLUDE_DIRS}/usr/include/eigen3/

orb_cv.cpp)

add_executable(pose_estimation_2d2d

target_link_libraries(pose_estimation_2d2d

${OpenCV_LIBS})#

target_link_libraries(triangulation

${OpenCV_LIBS})add_executable(pose_estimation_3d2d

target_link_libraries(pose_estimation_3d2dg2o_core

g2o_stuff${OpenCV_LIBS}${Sophus_LIBRARIES})add_executable(pose_estimation_3d3d

target_link_libraries(pose_estimation_3d3dg2o_core

g2o_stuff${OpenCV_LIBS}${Sophus_LIBRARIES})]]7.2.1

OpenCV

/home/xixi/Downloads/slambook2-master/ch7/orb_cv.cpp:16:31:

error:

opencv4/opencv2/features2d/features2d.hpp

#include

opencv4/opencv2/highgui/highgui.hpp

#include

cv::IMREAD_COLOR);assert(img_1.data

nullptr

descriptors_2;PtrFeatureDetector

detector

ORB::create();PtrDescriptorExtractor

descriptor

ORB::create();PtrDescriptorMatcher

matcher

DescriptorMatcher::create(BruteForce-Hamming);//--

第一步:检测

角点位置chrono::steady_clock::time_point

chrono::steady_clock::now();detector-detect(img_1,

keypoints_1);detector-detect(img_2,

keypoints_2);//--

descriptors_1);descriptor-compute(img_2,

keypoints_2,

descriptors_2);chrono::steady_clock::time_point

chrono::steady_clock::now();chrono::durationdouble

time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

DrawMatchesFlags::DEFAULT);imshow(ORB

features,

chrono::steady_clock::now();matcher-match(descriptors_1,

descriptors_2,

chrono::steady_clock::now();time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

minmax_element(matches.begin(),

DMatch

min_max.second-distance;printf(--

Max

min_dist);//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.std::vectorDMatch

good_matches;for

{good_matches.push_back(matches[i]);}}//--

第五步:绘制匹配结果Mat

img_goodmatch;drawMatches(img_1,

keypoints_1,

img_goodmatch);waitKey(0);return

去除误匹配

threshold40chrono::steady_clock::time_point

chrono::steady_clock::now();vectorcv::KeyPoint

keypoints1;cv::FAST(first_image,

keypoints1,

descriptor1;ComputeORB(first_image,

keypoints1,

descriptor2;cv::FAST(second_image,

keypoints2,

descriptor2);chrono::steady_clock::time_point

chrono::steady_clock::now();chrono::durationdouble

time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

chrono::steady_clock::now();BfMatch(descriptor1,

descriptor2,

chrono::steady_clock::now();time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

image_show;cv::drawMatches(first_image,

keypoints1,

image_show);cv::imshow(matches,

image_show);cv::imwrite(matches.png,

image_show);cv::waitKey(0);cout

done.

--------------------------------------------------------------------------------------------------

ORB

outsidebad_points;descriptors.push_back({});continue;}float

m01

d;}descriptors.push_back(desc);}cout

bad/total:

\bm{F}\bm{K}^{-T}\bm{E}\bm{K}^{-1}

FK−TEK−1

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_2d2d.cpp:36:31:

error:

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_2d2d.cpp:

function

pose_estimation_2d2d(std::vectorcv::KeyPoint,

cv::Mat,

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_2d2d.cpp:143:61:

error:

dumped)】卡了挺久。

重新复制原版CmakeLists.txt

build

../2.pngpose_estimation_2d2d.cpp

#include

opencv2/features2d/features2d.hpp

#include

cv;/*****************************************************

**************************************************/void

Mat

keypoints_1,std::vectorKeyPoint

matches);void

pose_estimation_2d2d(std::vectorKeyPoint

keypoints_1,std::vectorKeyPoint

matches,Mat

IMREAD_COLOR);assert(img_1.data

img_2.data

matches;find_feature_matches(img_1,

img_2,

t;pose_estimation_2d2d(keypoints_1,

keypoints_2,

pixel2cam(keypoints_1[m.queryIdx].pt,

K);Mat

pixel2cam(keypoints_2[m.trainIdx].pt,

K);Mat

keypoints_1,std::vectorKeyPoint

matches)

ORB::create();PtrDescriptorExtractor

descriptor

DescriptorMatcher::create(BruteForce-Hamming);//--

第一步:检测

keypoints_1);detector-detect(img_2,

keypoints_2);//--

descriptors_1);descriptor-compute(img_2,

keypoints_2,

min_dist);//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for

(int

{matches.push_back(match[i]);}}

}Point2d

pose_estimation_2d2d(std::vectorKeyPoint

keypoints_1,std::vectorKeyPoint

matches,Mat

把匹配点转换为vectorPoint2f的形式vectorPoint2f

points1;

{points1.push_back(keypoints_1[matches[i].queryIdx].pt);points2.push_back(keypoints_2[matches[i].trainIdx].pt);}//--

计算基础矩阵Mat

fundamental_matrix;fundamental_matrix

points2,

essential_matrix;essential_matrix

points2,

homography_matrix;homography_matrix

points2,

此函数仅在Opencv3中提供recoverPose(essential_matrix,

points1,

无法获得像素的深度信息需要通过三角测量(Triangulation)或三角化

三角测量

通过不同位置对同一路标点进行观察从观察到的位置判断路标点的距离。

7.6

keypoints_1,std::vectorKeyPoint

matches);void

cv::IMREAD_COLOR);vectorKeyPoint

keypoints_1,

matches;find_feature_matches(img_1,

img_2,

t;pose_estimation_2d2d(keypoints_1,

keypoints_2,

points;triangulation(keypoints_1,

keypoints_2,

pixel2cam(keypoints_1[matches[i].queryIdx].pt,

keypoints_1[matches[i].queryIdx].pt,

get_color(depth1),

keypoints_2[matches[i].trainIdx].pt,

get_color(depth2),

img2_plot);cv::waitKey();return

}void

keypoints_1,std::vectorKeyPoint

matches)

ORB::create();PtrDescriptorExtractor

descriptor

DescriptorMatcher::create(BruteForce-Hamming);//--

第一步:检测

keypoints_1);detector-detect(img_2,

keypoints_2);//--

descriptors_1);descriptor-compute(img_2,

keypoints_2,

min_dist);//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for

(int

{matches.push_back(match[i]);}}

}void

把匹配点转换为vectorPoint2f的形式vectorPoint2f

points2;for

{points1.push_back(keypoints_1[matches[i].queryIdx].pt);points2.push_back(keypoints_2[matches[i].trainIdx].pt);}//--

计算本质矩阵Point2d

essential_matrix;essential_matrix

points2,

从本质矩阵中恢复旋转和平移信息.recoverPose(essential_matrix,

points1,

将像素坐标转换至相机坐标pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt,

K));pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt,

K));}Mat

pts_4d;cv::triangulatePoints(T1,

T2,

将明显变小。

说明平移较大时在同样的相机分辨率下三角化测量将会更精确。

种方法

ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.error-icon{fill:#552222;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.error-text{fill:#552222;stroke:#552222;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.edge-thickness-normal{stroke-width:2px;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.marker{fill:#333333;stroke:#333333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.marker.cross{stroke:#333333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-LppxVVDY4FQ1fZ1n

ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.cluster-label

text{fill:#333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.cluster-label

span{color:#333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.label

text,#mermaid-svg-LppxVVDY4FQ1fZ1n

span{fill:#333;color:#333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.node

rect,#mermaid-svg-LppxVVDY4FQ1fZ1n

.node

circle,#mermaid-svg-LppxVVDY4FQ1fZ1n

.node

ellipse,#mermaid-svg-LppxVVDY4FQ1fZ1n

.node

polygon,#mermaid-svg-LppxVVDY4FQ1fZ1n

.node

path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.node

.label{text-align:center;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.node.clickable{cursor:pointer;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.arrowheadPath{fill:#333333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.edgePath

.path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.edgeLabel{background-color:#e8e8e8;text-align:center;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.edgeLabel

rect{opacity:0.5;background-color:#e8e8e8;fill:#e8e8e8;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.cluster

rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.cluster

text{fill:#333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

.cluster

span{color:#333;}#mermaid-svg-LppxVVDY4FQ1fZ1n

div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet

ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80,

100%,

#aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-LppxVVDY4FQ1fZ1n

:root{--mermaid-font-family:"trebuchet

双目/RGB-D

1、已知一组3D点的位置以及它们在某个相机中的投影位置求该相机的位姿。

3、把

利用更多的信息用迭代的方式对相机位姿进行优化尽可能消除噪声的影响。

先使用

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:37:11:

error:

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:45:11:

error:

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:54:31:

error:

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:64:28:

error:

/usr/local/include/g2o/stuff/tuple_tools.h:41:46:

error:

std::make_index_sequencestd::tuple_size_vstd::decay_tT());

解决办法

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:318:10:

error:

g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));

类似第6讲直接替换

g2o::BlockSolverg2o::BlockSolverTraits3,

BlockSolverType;

g2o::LinearSolverDenseBlockSolverType::PoseMatrixType

LinearSolverType;

线性求解器类型*/std::unique_ptrg2o::BlockSolverX::LinearSolverType

linearSolver

g2o::LinearSolverDenseg2o::BlockSolverX::PoseMatrixType());//

LM,

g2o::OptimizationAlgorithmGaussNewton(g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));*/std::unique_ptrg2o::BlockSolverX

solver_ptr

g2o::BlockSolverX(std::move(linearSolver)));g2o::OptimizationAlgorithmGaussNewton*

solver

g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));g2o::SparseOptimizer

optimizer;

图模型optimizer.setAlgorithm(solver);

设置求解器optimizer.setVerbose(true);

打开调试输出报错5

CMakeFiles/pose_estimation_3d2d.dir/pose_estimation_3d2d.cpp.o:

function

bundleAdjustmentGaussNewton(std::vectorEigen::Matrixdouble,

Eigen::aligned_allocatorEigen::Matrixdouble,

const,

std::vectorEigen::Matrixdouble,

Eigen::aligned_allocatorEigen::Matrixdouble,

const,

pose_estimation_3d2d.cpp:(.text0x2a4f):

undefined

Sophus::SE3::operator*(Eigen::Matrixdouble,

const)

pose_estimation_3d2d.cpp:(.text0x3254):

undefined

Sophus::SE3::exp(Eigen::Matrixdouble,

const)

add_executable(pose_estimation_3d2d

target_link_libraries(pose_estimation_3d2dg2o_core

g2o_stuff${OpenCV_LIBS}${Sophus_LIBRARIES})cd

build

../2_depth.pngpose_estimation_3d2d.cpp

#include

opencv4/opencv2/features2d/features2d.hpp

#include

opencv4/opencv2/highgui/highgui.hpp

#include

opencv4/opencv2/calib3d/calib3d.hpp

#include

g2o/core/optimization_algorithm_gauss_newton.h

#include

g2o/solvers/dense/linear_solver_dense.h

#include

keypoints_1,std::vectorKeyPoint

matches);//

Eigen::aligned_allocatorEigen::Vector2d

VecVector2d;

Eigen::aligned_allocatorEigen::Vector3d

VecVector3d;void

bundleAdjustmentGaussNewton(const

VecVector3d

IMREAD_COLOR);assert(img_1.data

img_2.data

matches;find_feature_matches(img_1,

img_2,

short(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];if

bad

pixel2cam(keypoints_1[m.queryIdx].pt,

K);pts_3d.push_back(Point3f(p1.x

dd,

dd));pts_2d.push_back(keypoints_2[m.trainIdx].pt);}cout

3d-2d

endl;chrono::steady_clock::time_point

chrono::steady_clock::now();Mat

t;solvePnP(pts_3d,

r为旋转向量形式用Rodrigues公式转换为矩阵chrono::steady_clock::time_point

chrono::steady_clock::now();chrono::durationdouble

time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

{pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x,

pts_3d[i].y,

pts_3d[i].z));pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x,

pts_2d[i].y));}cout

chrono::steady_clock::now();bundleAdjustmentGaussNewton(pts_3d_eigen,

pts_2d_eigen,

chrono::steady_clock::now();time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

chrono::steady_clock::now();bundleAdjustmentG2O(pts_3d_eigen,

pts_2d_eigen,

chrono::steady_clock::now();time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

keypoints_1,std::vectorKeyPoint

matches)

ORB::create();PtrDescriptorExtractor

descriptor

DescriptorMatcher::create(BruteForce-Hamming);//--

第一步:检测

keypoints_1);detector-detect(img_2,

keypoints_2);//--

descriptors_1);descriptor-compute(img_2,

keypoints_2,

min_dist);//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for

(int

{matches.push_back(match[i]);}}

}Point2d

bundleAdjustmentGaussNewton(const

VecVector3d

e.squaredNorm();Eigen::Matrixdouble,

J;J

public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;virtual

void

public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeProjection(const

Eigen::Vector3d

g2o::BlockSolverg2o::BlockSolverTraits3,

BlockSolverType;

g2o::LinearSolverDenseBlockSolverType::PoseMatrixType

LinearSolverType;

线性求解器类型*/std::unique_ptrg2o::BlockSolverX::LinearSolverType

linearSolver

g2o::LinearSolverDenseg2o::BlockSolverX::PoseMatrixType());//

LM,

g2o::OptimizationAlgorithmGaussNewton(g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));*/std::unique_ptrg2o::BlockSolverX

solver_ptr

g2o::BlockSolverX(std::move(linearSolver)));g2o::OptimizationAlgorithmGaussNewton*

solver

g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));g2o::SparseOptimizer

optimizer;

图模型optimizer.setAlgorithm(solver);

设置求解器optimizer.setVerbose(true);

打开调试输出//

vertex_posevertex_pose-setId(0);vertex_pose-setEstimate(Sophus::SE3());optimizer.addVertex(vertex_pose);//

KEigen::Matrix3d

K_eigen);edge-setId(index);edge-setVertex(0,

vertex_pose);edge-setMeasurement(p2d);edge-setInformation(Eigen::Matrix2d::Identity());optimizer.addEdge(edge);index;}chrono::steady_clock::time_point

chrono::steady_clock::now();optimizer.setVerbose(true);optimizer.initializeOptimization();optimizer.optimize(10);chrono::steady_clock::time_point

chrono::steady_clock::now();chrono::durationdouble

time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

vertex_pose-estimate().matrix()

endl;pose

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d3d.cpp:81:50:

error:

Sophus::SO3d::hat(xyz_trans);去掉d改成

SO3

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d3d.cpp:

function

/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d3d.cpp:298:41:

error:

pose-estimate().rotationMatrix();|

^~~~~~~~~~~~~~|

pose-estimate().rotation_matrix();cd

build

../2_depth.pngpose_estimation_3d3d.cpp

#include

opencv4/opencv2/features2d/features2d.hpp

#include

opencv4/opencv2/highgui/highgui.hpp

#include

opencv4/opencv2/calib3d/calib3d.hpp

#include

g2o/core/optimization_algorithm_gauss_newton.h

#include

g2o/core/optimization_algorithm_levenberg.h

#include

g2o/solvers/dense/linear_solver_dense.h

#include

keypoints_1,std::vectorKeyPoint

matches);//

public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;virtual

void

public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeProjectXYZRGBDPoseOnly(const

Eigen::Vector3d

pose-estimate();Eigen::Vector3d

xyz_trans

_point;_jacobianOplusXi.block3,

3(0,

-Eigen::Matrix3d::Identity();_jacobianOplusXi.block3,

3(0,

Sophus::SO3::hat(xyz_trans);}bool

read(istream

matches;find_feature_matches(img_1,

img_2,

short(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];ushort

depth2.ptrunsigned

short(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];if

(d1

pixel2cam(keypoints_1[m.queryIdx].pt,

K);Point2d

pixel2cam(keypoints_2[m.trainIdx].pt,

K);float

5000.0;pts1.push_back(Point3f(p1.x

dd1,

dd1));pts2.push_back(Point3f(p2.x

dd2,

keypoints_1,std::vectorKeyPoint

matches)

ORB::create();PtrDescriptorExtractor

descriptor

DescriptorMatcher::create(BruteForce-Hamming);//--

第一步:检测

keypoints_1);detector-detect(img_2,

keypoints_2);//--

descriptors_1);descriptor-compute(img_2,

keypoints_2,

min_dist);//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for

(int

{matches.push_back(match[i]);}}

}Point2d

WEigen::JacobiSVDEigen::Matrix3d

svd(W,

Eigen::ComputeFullV);Eigen::Matrix3d

svd.matrixV();cout

g2o::BlockSolverg2o::BlockSolverTraits3,

BlockSolverType;

g2o::LinearSolverDenseBlockSolverType::PoseMatrixType

LinearSolverType;

线性求解器类型*/std::unique_ptrg2o::BlockSolverX::LinearSolverType

linearSolver

g2o::LinearSolverDenseg2o::BlockSolverX::PoseMatrixType());//

LM,

g2o::OptimizationAlgorithmGaussNewton(g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));*/std::unique_ptrg2o::BlockSolverX

solver_ptr

g2o::BlockSolverX(std::move(linearSolver)));g2o::OptimizationAlgorithmGaussNewton*

solver

g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));g2o::SparseOptimizer

optimizer;

图模型optimizer.setAlgorithm(solver);

设置求解器optimizer.setVerbose(true);

打开调试输出//

posepose-setId(0);pose-setEstimate(Sophus::SE3());optimizer.addVertex(pose);//

edgesfor

EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x,

pts2[i].y,

pose);edge-setMeasurement(Eigen::Vector3d(pts1[i].x,

pts1[i].y,

pts1[i].z));edge-setInformation(Eigen::Matrix3d::Identity());optimizer.addEdge(edge);}chrono::steady_clock::time_point

chrono::steady_clock::now();optimizer.initializeOptimization();optimizer.optimize(10);chrono::steady_clock::time_point

chrono::steady_clock::now();chrono::durationdouble

time_used

chrono::duration_castchrono::durationdouble(t2

t1);cout

pose-estimate().rotation_matrix();Eigen::Vector3d

pose-estimate().translation();R

(Mat_double(3,