ORB-SLAM3源码的学习：GeometricTools文件
前言
GeometricTools提供了两种几何计算功能：1.计算两个关键帧之间的基础矩阵、2.通过三角化算法从两个视角恢复三维点。这部分功能在ORB-SLAM2中就已经介绍过了，这里不过多赘述。
1.头文件GeometricTools.h
除了计算基础矩阵和三角化恢复三维点外，头文件中还提供了两种用于比较矩阵的模板函数。第一个函数用于比较一个OpenCV矩阵和一个Eigen矩阵，第二个函数用于比较两个Eigen矩阵。
#ifndef GEOMETRIC_TOOLS_H
#define GEOMETRIC_TOOLS_H
#include
#include
#include
namespace ORB_SLAM3
{
class KeyFrame;
class GeometricTools
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Compute the Fundamental matrix between KF1 and KF2
static Eigen::Matrix3f ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2);
//Triangulate point with KF1 and KF2
static bool Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2,Eigen::Matrix &Tc1w ,Eigen::Matrix &Tc2w , Eigen::Vector3f &x3D);
template
static bool CheckMatrices(const cv::Mat &cvMat, const Eigen::Matrix &eigMat) {
const float epsilon = 1e-3;
// std::cout << cvMat.cols - cols << cvMat.rows - rows << std::endl;
if(rows != cvMat.rows || cols != cvMat.cols) {
std::cout << "wrong cvmat size\n";
return false;
}
for(int i = 0; i < rows; i++)
for(int j = 0; j < cols; j++)
if ((cvMat.at(i,j) > (eigMat(i,j) + epsilon)) ||
(cvMat.at(i,j) < (eigMat(i,j) - epsilon))){
std::cout << "cv mat:\n" << cvMat << std::endl;
std::cout << "eig mat:\n" << eigMat << std::endl;
return false;
}
return true;
}
template
static bool CheckMatrices( const Eigen::Matrix &eigMat1, const Eigen::Matrix &eigMat2) {
const float epsilon = 1e-3;
for(int i = 0; i < rows; i++)
for(int j = 0; j < cols; j++)
if ((eigMat1(i,j) > (eigMat2(i,j) + epsilon)) ||
(eigMat1(i,j) < (eigMat2(i,j) - epsilon))){
std::cout << "eig mat 1:\n" << eigMat1 << std::endl;
std::cout << "eig mat 2:\n" << eigMat2 << std::endl;
return false;
}
return true;
}
};
}// namespace ORB_SLAM
#endif // GEOMETRIC_TOOLS_H 2.源文件GeometricTools.cc
#include "GeometricTools.h"
#include "KeyFrame.h"
namespace ORB_SLAM3
{
/**
* @brief 计算两个关键帧间的基础矩阵
* @param pKF1 关键帧1
* @param pKF2 关键帧2
*/
Eigen::Matrix3f GeometricTools::ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2)
{
// 获取关键帧1的旋转平移
Sophus::SE3 Tc1w = pKF1->GetPose();
Sophus::Matrix3 Rc1w = Tc1w.rotationMatrix();
Sophus::SE3::TranslationMember tc1w = Tc1w.translation();
// 获取关键帧2的旋转平移
Sophus::SE3 Tc2w = pKF2->GetPose();
Sophus::Matrix3 Rc2w = Tc2w.rotationMatrix();
Sophus::SE3::TranslationMember tc2w = Tc2w.translation();
// 计算2->1的旋转平移
Sophus::Matrix3 Rc1c2 = Rc1w * Rc2w.transpose();
Eigen::Vector3f tc1c2 = -Rc1c2 * tc2w + tc1w;
Eigen::Matrix3f tc1c2x = Sophus::SO3f::hat(tc1c2);
const Eigen::Matrix3f K1 = pKF1->mpCamera->toK_();
const Eigen::Matrix3f K2 = pKF2->mpCamera->toK_();
return K1.transpose().inverse() * tc1c2x * Rc1c2 * K2.inverse();
}
/**
* @brief 三角化获得三维点
* @param x_c1 点在关键帧1下的归一化坐标
* @param x_c2 点在关键帧2下的归一化坐标
* @param Tc1w 关键帧1投影矩阵 [K*R | K*t]
* @param Tc2w 关键帧2投影矩阵 [K*R | K*t]
* @param x3D 三维点坐标，作为结果输出
*/
bool GeometricTools::Triangulate(
Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2, Eigen::Matrix &Tc1w, Eigen::Matrix &Tc2w,
Eigen::Vector3f &x3D)
{
Eigen::Matrix4f A;
// x = a*P*X， 左右两面乘Pc的反对称矩阵 a*[x]^ * P *X = 0 构成了A矩阵，中间涉及一个尺度a，因为都是归一化平面，但右面是0所以直接可以约掉不影响最后的尺度
// 0 -1 v P(0) -P.row(1) + v*P.row(2)
// 1 0 -u * P(1) = P.row(0) - u*P.row(2)
// -v u 0 P(2) u*P.row(1) - v*P.row(0)
// 发现上述矩阵线性相关，所以取前两维，两个点构成了4行的矩阵，就是如下的操作，求出的是4维的结果[X,Y,Z,A]，所以需要除以最后一维使之为1，就成了[X,Y,Z,1]这种齐次形式
A.block<1,4>(0,0) = x_c1(0) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(0,0);
A.block<1,4>(1,0) = x_c1(1) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(1,0);
A.block<1,4>(2,0) = x_c2(0) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(0,0);
A.block<1,4>(3,0) = x_c2(1) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(1,0);
// 解方程 AX=0
Eigen::JacobiSVD svd(A, Eigen::ComputeFullV);
Eigen::Vector4f x3Dh = svd.matrixV().col(3);
if(x3Dh(3)==0)
return false;
// Euclidean coordinates
x3D = x3Dh.head(3)/x3Dh(3);
return true;
}
} //namespace ORB_SLAM
结束语
以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。