12345qiupeng
/
orb_slam3_details
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

更新标注

master
liuyancheng 2021-09-22 19:42:25 +08:00
parent a55d24a467
commit 6117fcf244
4 changed files with 1003 additions and 703 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -47,3 +47,4 @@ cmake-build-debug/
*.pyc *.pyc
*.osa *.osa
.vscode/settings.json

1199
src/CameraModels/KannalaBrandt8.cpp
View File

File diff suppressed because it is too large Load Diff

501
src/CameraModels/Pinhole.cpp
View File

@ -20,195 +20,318 @@
#include <boost/serialization/export.hpp> #include <boost/serialization/export.hpp>
namespace ORB_SLAM3 { namespace ORB_SLAM3
{
long unsigned int GeometricCamera::nNextId=0; long unsigned int GeometricCamera::nNextId = 0;
cv::Point2f Pinhole::project(const cv::Point3f &p3D) { /**
return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2], * @brief 投影
mvParameters[1] * p3D.y / p3D.z + mvParameters[3]); * @param p3D 三维点
} * @return 像素坐标
*/
cv::Point2f Pinhole::project(const cv::Matx31f &m3D) { cv::Point2f Pinhole::project(const cv::Point3f &p3D)
return this->project(cv::Point3f(m3D(0),m3D(1),m3D(2))); {
} return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2],
mvParameters[1] * p3D.y / p3D.z + mvParameters[3]);
cv::Point2f Pinhole::project(const cv::Mat &m3D) { }
const float* p3D = m3D.ptr<float>();
/**
return this->project(cv::Point3f(p3D[0],p3D[1],p3D[2])); * @brief 投影
} * @param m3D 三维点
* @return 像素坐标
Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) { */
Eigen::Vector2d res; cv::Point2f Pinhole::project(const cv::Matx31f &m3D)
res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2]; {
res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3]; return this->project(cv::Point3f(m3D(0), m3D(1), m3D(2)));
}
return res;
} /**
* @brief 投影
cv::Mat Pinhole::projectMat(const cv::Point3f &p3D) { * @param v3D 三维点
cv::Point2f point = this->project(p3D); * @return 像素坐标
return (cv::Mat_<float>(2,1) << point.x, point.y); */
} cv::Point2f Pinhole::project(const cv::Mat &m3D)
{
float Pinhole::uncertainty2(const Eigen::Matrix<double,2,1> &p2D) const float *p3D = m3D.ptr<float>();
{
return 1.0; return this->project(cv::Point3f(p3D[0], p3D[1], p3D[2]));
} }
cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) { /**
return cv::Point3f((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1], * @brief 投影
1.f); * @param p3D 三维点
} * @return 像素坐标
*/
cv::Mat Pinhole::unprojectMat(const cv::Point2f &p2D){ Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D)
cv::Point3f ray = this->unproject(p2D); {
return (cv::Mat_<float>(3,1) << ray.x, ray.y, ray.z); Eigen::Vector2d res;
} res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2];
res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3];
cv::Matx31f Pinhole::unprojectMat_(const cv::Point2f &p2D) {
cv::Point3f ray = this->unproject(p2D); return res;
cv::Matx31f r{ray.x, ray.y, ray.z}; }
return r;
} /**
* @brief 投影
cv::Mat Pinhole::projectJac(const cv::Point3f &p3D) { * @param p3D 三维点
cv::Mat Jac(2, 3, CV_32F); * @return 像素坐标
Jac.at<float>(0, 0) = mvParameters[0] / p3D.z; */
Jac.at<float>(0, 1) = 0.f; cv::Mat Pinhole::projectMat(const cv::Point3f &p3D)
Jac.at<float>(0, 2) = -mvParameters[0] * p3D.x / (p3D.z * p3D.z); {
Jac.at<float>(1, 0) = 0.f; cv::Point2f point = this->project(p3D);
Jac.at<float>(1, 1) = mvParameters[1] / p3D.z; return (cv::Mat_<float>(2, 1) << point.x, point.y);
Jac.at<float>(1, 2) = -mvParameters[1] * p3D.y / (p3D.z * p3D.z); }
return Jac; /**
} * @brief 貌似是调试遗留的产物
*/
Eigen::Matrix<double, 2, 3> Pinhole::projectJac(const Eigen::Vector3d &v3D) { float Pinhole::uncertainty2(const Eigen::Matrix<double, 2, 1> &p2D)
Eigen::Matrix<double, 2, 3> Jac; {
Jac(0, 0) = mvParameters[0] / v3D[2]; return 1.0;
Jac(0, 1) = 0.f; }
Jac(0, 2) = -mvParameters[0] * v3D[0] / (v3D[2] * v3D[2]);
Jac(1, 0) = 0.f; /**
Jac(1, 1) = mvParameters[1] / v3D[2]; * @brief 反投影
Jac(1, 2) = -mvParameters[1] * v3D[1] / (v3D[2] * v3D[2]); * @param p2D 特征点
* @return
return Jac; */
} cv::Point3f Pinhole::unproject(const cv::Point2f &p2D)
{
cv::Mat Pinhole::unprojectJac(const cv::Point2f &p2D) { return cv::Point3f((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1],
cv::Mat Jac(3, 2, CV_32F); 1.f);
Jac.at<float>(0, 0) = 1 / mvParameters[0]; }
Jac.at<float>(0, 1) = 0.f;
Jac.at<float>(1, 0) = 0.f; /**
Jac.at<float>(1, 1) = 1 / mvParameters[1]; * @brief 反投影
Jac.at<float>(2, 0) = 0.f; * @param p2D 特征点
Jac.at<float>(2, 1) = 0.f; * @return
*/
return Jac; cv::Mat Pinhole::unprojectMat(const cv::Point2f &p2D)
} {
cv::Point3f ray = this->unproject(p2D);
bool Pinhole::ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12, return (cv::Mat_<float>(3, 1) << ray.x, ray.y, ray.z);
cv::Mat &R21, cv::Mat &t21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated){ }
if(!tvr){
cv::Mat K = this->toK(); /**
tvr = new TwoViewReconstruction(K); * @brief 反投影
} * @param p2D 特征点
* @return
return tvr->Reconstruct(vKeys1,vKeys2,vMatches12,R21,t21,vP3D,vbTriangulated); */
} cv::Matx31f Pinhole::unprojectMat_(const cv::Point2f &p2D)
{
cv::Point3f ray = this->unproject(p2D);
cv::Mat Pinhole::toK() { cv::Matx31f r{ray.x, ray.y, ray.z};
cv::Mat K = (cv::Mat_<float>(3, 3) return r;
<< mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f); }
return K;
} /**
* @brief 求解三维点关于二维像素坐标的雅克比矩阵
cv::Matx33f Pinhole::toK_() { * @param p3D 三维点
cv::Matx33f K{mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f}; * @return
*/
return K; cv::Mat Pinhole::projectJac(const cv::Point3f &p3D)
} {
cv::Mat Jac(2, 3, CV_32F);
bool Pinhole::epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) { Jac.at<float>(0, 0) = mvParameters[0] / p3D.z;
//Compute Fundamental Matrix Jac.at<float>(0, 1) = 0.f;
cv::Mat t12x = SkewSymmetricMatrix(t12); Jac.at<float>(0, 2) = -mvParameters[0] * p3D.x / (p3D.z * p3D.z);
cv::Mat K1 = this->toK(); Jac.at<float>(1, 0) = 0.f;
cv::Mat K2 = pCamera2->toK(); Jac.at<float>(1, 1) = mvParameters[1] / p3D.z;
cv::Mat F12 = K1.t().inv()*t12x*R12*K2.inv(); Jac.at<float>(1, 2) = -mvParameters[1] * p3D.y / (p3D.z * p3D.z);
// Epipolar line in second image l = x1'F12 = [a b c] return Jac;
const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0); }
const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2); /**
* @brief 求解三维点关于二维像素坐标的雅克比矩阵
const float num = a*kp2.pt.x+b*kp2.pt.y+c; * @param v3D 三维点
* @return
const float den = a*a+b*b; */
Eigen::Matrix<double, 2, 3> Pinhole::projectJac(const Eigen::Vector3d &v3D)
if(den==0) {
return false; Eigen::Matrix<double, 2, 3> Jac;
Jac(0, 0) = mvParameters[0] / v3D[2];
const float dsqr = num*num/den; Jac(0, 1) = 0.f;
Jac(0, 2) = -mvParameters[0] * v3D[0] / (v3D[2] * v3D[2]);
return dsqr<3.84*unc; Jac(1, 0) = 0.f;
} Jac(1, 1) = mvParameters[1] / v3D[2];
Jac(1, 2) = -mvParameters[1] * v3D[1] / (v3D[2] * v3D[2]);
bool Pinhole::epipolarConstrain_(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc) {
//Compute Fundamental Matrix return Jac;
auto t12x = SkewSymmetricMatrix_(t12); }
auto K1 = this->toK_();
auto K2 = pCamera2->toK_(); /**
cv::Matx33f F12 = K1.t().inv()*t12x*R12*K2.inv(); * @brief 求解二维像素坐标关于三维点的雅克比矩阵
* @param p2D 特征点
// Epipolar line in second image l = x1'F12 = [a b c] * @return
const float a = kp1.pt.x*F12(0,0)+kp1.pt.y*F12(1,0)+F12(2,0); */
const float b = kp1.pt.x*F12(0,1)+kp1.pt.y*F12(1,1)+F12(2,1); cv::Mat Pinhole::unprojectJac(const cv::Point2f &p2D)
const float c = kp1.pt.x*F12(0,2)+kp1.pt.y*F12(1,2)+F12(2,2); {
cv::Mat Jac(3, 2, CV_32F);
const float num = a*kp2.pt.x+b*kp2.pt.y+c; Jac.at<float>(0, 0) = 1 / mvParameters[0];
Jac.at<float>(0, 1) = 0.f;
const float den = a*a+b*b; Jac.at<float>(1, 0) = 0.f;
Jac.at<float>(1, 1) = 1 / mvParameters[1];
if(den==0) Jac.at<float>(2, 0) = 0.f;
return false; Jac.at<float>(2, 1) = 0.f;
const float dsqr = num*num/den; return Jac;
}
return dsqr<3.84*unc;
} /** 三角化恢复三维点
* @param vKeys1 第一帧的关键点
std::ostream & operator<<(std::ostream &os, const Pinhole &ph) { * @param vKeys2 第二帧的关键点
os << ph.mvParameters[0] << " " << ph.mvParameters[1] << " " << ph.mvParameters[2] << " " << ph.mvParameters[3]; * @param vMatches12 匹配关系，长度与vKeys1一样，对应位置存放vKeys2中关键点的下标
return os; * @param R21 顾名思义
} * @param t21 顾名思义
* @param vP3D 恢复出的三维点
std::istream & operator>>(std::istream &is, Pinhole &ph) { * @param vbTriangulated 是否三角化成功，用于统计匹配点数量
float nextParam; */
for(size_t i = 0; i < 4; i++){ bool Pinhole::ReconstructWithTwoViews(const std::vector<cv::KeyPoint> &vKeys1, const std::vector<cv::KeyPoint> &vKeys2, const std::vector<int> &vMatches12,
assert(is.good()); //Make sure the input stream is good cv::Mat &R21, cv::Mat &t21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated)
is >> nextParam; {
ph.mvParameters[i] = nextParam; if (!tvr)
{
} cv::Mat K = this->toK();
return is; tvr = new TwoViewReconstruction(K);
} }
cv::Mat Pinhole::SkewSymmetricMatrix(const cv::Mat &v) return tvr->Reconstruct(vKeys1, vKeys2, vMatches12, R21, t21, vP3D, vbTriangulated);
{ }
return (cv::Mat_<float>(3,3) << 0, -v.at<float>(2), v.at<float>(1),
v.at<float>(2), 0,-v.at<float>(0), /**
-v.at<float>(1), v.at<float>(0), 0); * @brief 返回内参矩阵
} * @return K
*/
cv::Matx33f Pinhole::SkewSymmetricMatrix_(const cv::Matx31f &v) cv::Mat Pinhole::toK()
{ {
cv::Matx33f skew{0.f, -v(2), v(1), cv::Mat K = (cv::Mat_<float>(3, 3)
v(2), 0.f, -v(0), << mvParameters[0],
-v(1), v(0), 0.f}; 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f);
return K;
return skew; }
}
/**
* @brief 返回内参矩阵
* @return K
*/
cv::Matx33f Pinhole::toK_()
{
cv::Matx33f K{mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f};
return K;
}
/**
* @brief 极线约束
* @param pCamera2 右相机
* @param kp1 左相机特征点
* @param kp2 右相机特征点
* @param R12 2->1的旋转
* @param t12 2->1的平移
* @param sigmaLevel 特征点1的尺度的平方
* @param unc 特征点2的尺度的平方
* @return 三维点恢复的成功与否
*/
bool Pinhole::epipolarConstrain(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc)
{
//Compute Fundamental Matrix
cv::Mat t12x = SkewSymmetricMatrix(t12);
cv::Mat K1 = this->toK();
cv::Mat K2 = pCamera2->toK();
cv::Mat F12 = K1.t().inv() * t12x * R12 * K2.inv();
// Epipolar line in second image l = x1'F12 = [a b c]
const float a = kp1.pt.x * F12.at<float>(0, 0) + kp1.pt.y * F12.at<float>(1, 0) + F12.at<float>(2, 0);
const float b = kp1.pt.x * F12.at<float>(0, 1) + kp1.pt.y * F12.at<float>(1, 1) + F12.at<float>(2, 1);
const float c = kp1.pt.x * F12.at<float>(0, 2) + kp1.pt.y * F12.at<float>(1, 2) + F12.at<float>(2, 2);
const float num = a * kp2.pt.x + b * kp2.pt.y + c;
const float den = a * a + b * b;
if (den == 0)
return false;
const float dsqr = num * num / den;
return dsqr < 3.84 * unc;
}
/**
* @brief 极线约束
* @param pCamera2 右相机
* @param kp1 左相机特征点
* @param kp2 右相机特征点
* @param R12 2->1的旋转
* @param t12 2->1的平移
* @param sigmaLevel 特征点1的尺度的平方
* @param unc 特征点2的尺度的平方
* @return 三维点恢复的成功与否
*/
bool Pinhole::epipolarConstrain_(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc)
{
//Compute Fundamental Matrix
auto t12x = SkewSymmetricMatrix_(t12);
auto K1 = this->toK_();
auto K2 = pCamera2->toK_();
cv::Matx33f F12 = K1.t().inv() * t12x * R12 * K2.inv();
// Epipolar line in second image l = x1'F12 = [a b c]
const float a = kp1.pt.x * F12(0, 0) + kp1.pt.y * F12(1, 0) + F12(2, 0);
const float b = kp1.pt.x * F12(0, 1) + kp1.pt.y * F12(1, 1) + F12(2, 1);
const float c = kp1.pt.x * F12(0, 2) + kp1.pt.y * F12(1, 2) + F12(2, 2);
const float num = a * kp2.pt.x + b * kp2.pt.y + c;
const float den = a * a + b * b;
if (den == 0)
return false;
const float dsqr = num * num / den;
return dsqr < 3.84 * unc;
}
std::ostream &operator<<(std::ostream &os, const Pinhole &ph)
{
os << ph.mvParameters[0] << " " << ph.mvParameters[1] << " " << ph.mvParameters[2] << " " << ph.mvParameters[3];
return os;
}
std::istream &operator>>(std::istream &is, Pinhole &ph)
{
float nextParam;
for (size_t i = 0; i < 4; i++)
{
assert(is.good()); //Make sure the input stream is good
is >> nextParam;
ph.mvParameters[i] = nextParam;
}
return is;
}
/**
* @brief 反对称矩阵
*/
cv::Mat Pinhole::SkewSymmetricMatrix(const cv::Mat &v)
{
return (cv::Mat_<float>(3, 3) << 0, -v.at<float>(2), v.at<float>(1),
v.at<float>(2), 0, -v.at<float>(0),
-v.at<float>(1), v.at<float>(0), 0);
}
/**
* @brief 反对称矩阵
*/
cv::Matx33f Pinhole::SkewSymmetricMatrix_(const cv::Matx31f &v)
{
cv::Matx33f skew{0.f, -v(2), v(1),
v(2), 0.f, -v(0),
-v(1), v(0), 0.f};
return skew;
}
} }

5
src/G2oTypes.cc
View File

@ -922,8 +922,13 @@ Eigen::Matrix3d Skew(const Eigen::Vector3d &w)
return W; return W;
} }
// BUG 应该改成svd.matrixV().transpose()
Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R) Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R)
{ {
// 这里关注一下
// 1. 对于行列数一样的矩阵，Eigen::ComputeThinU | Eigen::ComputeThinV 与 Eigen::ComputeFullU | Eigen::ComputeFullV 一样
// 2. 对于行列数不同的矩阵，例如3*4 或者 4*3 矩阵只有3个奇异向量，计算的时候如果是Thin 那么得出的UV矩阵列数只能是3，如果是full那么就是4
// 3. thin会损失一部分数据，但是会加快计算，对于大型矩阵解算方程时，可以用thin加速得到结果
Eigen::JacobiSVD<Eigen::Matrix3d> svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::JacobiSVD<Eigen::Matrix3d> svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV);
return svd.matrixU()*svd.matrixV(); return svd.matrixU()*svd.matrixV();
} }