/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see .
*/
#include "Pinhole.h"
#include
// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole)
namespace ORB_SLAM3
{
// BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
long unsigned int GeometricCamera::nNextId = 0;
/**
* @brief 相机坐标系下的三维点投影到无畸变像素平面
* @param p3D 三维点
* @return 像素坐标
*/
cv::Point2f Pinhole::project(const cv::Point3f &p3D)
{
return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2],
mvParameters[1] * p3D.y / p3D.z + mvParameters[3]);
}
/**
* @brief 相机坐标系下的三维点投影到无畸变像素平面
* @param v3D 三维点
* @return 像素坐标
*/
Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D)
{
Eigen::Vector2d res;
res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2];
res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3];
return res;
}
/**
* @brief 相机坐标系下的三维点投影到无畸变像素平面
* @param v3D 三维点
* @return 像素坐标
*/
Eigen::Vector2f Pinhole::project(const Eigen::Vector3f &v3D)
{
Eigen::Vector2f res;
res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2];
res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3];
return res;
}
/**
* @brief 相机坐标系下的三维点投影到无畸变像素平面
* @param p3D 三维点
* @return 像素坐标
*/
Eigen::Vector2f Pinhole::projectMat(const cv::Point3f &p3D)
{
cv::Point2f point = this->project(p3D);
return Eigen::Vector2f(point.x, point.y);
}
/**
* @brief 貌似是调试遗留的产物
*/
float Pinhole::uncertainty2(const Eigen::Matrix &p2D)
{
return 1.0;
}
/**
* @brief 反投影
* @param p2D 特征点
* @return 归一化坐标
*/
Eigen::Vector3f Pinhole::unprojectEig(const cv::Point2f &p2D)
{
return Eigen::Vector3f(
(p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1],
1.f);
}
/**
* @brief 反投影
* @param p2D 特征点
* @return 归一化坐标
*/
cv::Point3f Pinhole::unproject(const cv::Point2f &p2D)
{
return cv::Point3f(
(p2D.x - mvParameters[2]) / mvParameters[0],
(p2D.y - mvParameters[3]) / mvParameters[1],
1.f);
}
/**
* @brief 求解二维像素坐标关于三维点坐标的雅克比矩阵
* @param v3D 三维点
* @return
*/
Eigen::Matrix Pinhole::projectJac(const Eigen::Vector3d &v3D)
{
Eigen::Matrix Jac;
Jac(0, 0) = mvParameters[0] / v3D[2];
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];
Jac(1, 2) = -mvParameters[1] * v3D[1] / (v3D[2] * v3D[2]);
return Jac;
}
/** 三角化恢复三维点 单目初始化时使用
* @param vKeys1 第一帧的关键点
* @param vKeys2 第二帧的关键点
* @param vMatches12 匹配关系，长度与vKeys1一样，对应位置存放vKeys2中关键点的下标
* @param R21 顾名思义
* @param t21 顾名思义
* @param vP3D 恢复出的三维点
* @param vbTriangulated 是否三角化成功，用于统计匹配点数量
*/
bool Pinhole::ReconstructWithTwoViews(const std::vector &vKeys1, const std::vector &vKeys2, const std::vector &vMatches12,
Sophus::SE3f &T21, std::vector &vP3D, std::vector &vbTriangulated)
{
if (!tvr)
{
Eigen::Matrix3f K = this->toK_();
tvr = new TwoViewReconstruction(K);
}
return tvr->Reconstruct(vKeys1, vKeys2, vMatches12, T21, vP3D, vbTriangulated);
}
/**
* @brief 返回内参矩阵
* @return K
*/
cv::Mat Pinhole::toK()
{
cv::Mat K = (cv::Mat_(3, 3) << mvParameters[0],
0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f);
return K;
}
/**
* @brief 返回内参矩阵
* @return K
*/
Eigen::Matrix3f Pinhole::toK_()
{
Eigen::Matrix3f K;
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的尺度的平方，1.2^2n
* @return 三维点恢复的成功与否
*/
bool Pinhole::epipolarConstrain(
GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc)
{
// Compute Fundamental Matrix
Eigen::Matrix3f t12x = Sophus::SO3f::hat(t12);
Eigen::Matrix3f K1 = this->toK_();
Eigen::Matrix3f K2 = pCamera2->toK_();
Eigen::Matrix3f F12 = K1.transpose().inverse() * t12x * R12 * K2.inverse();
// Epipolar line in second image l = x1'F12 = [a b c]
// u2,
// (u1, v1, 1) * F12 * (v2,) = 0 --> (a, b, c) * (u2, v2, 1)^t = 0 --> a*u2 + b*v2 + c = 0
// 1
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);
// 点到直线距离的公式
// d = |a*u2 + b*v2 + c| / sqrt(a^2 + b^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;
}
bool Pinhole::IsEqual(GeometricCamera *pCam)
{
if (pCam->GetType() != GeometricCamera::CAM_PINHOLE)
return false;
Pinhole *pPinholeCam = (Pinhole *)pCam;
if (size() != pPinholeCam->size())
return false;
bool is_same_camera = true;
for (size_t i = 0; i < size(); ++i)
{
if (abs(mvParameters[i] - pPinholeCam->getParameter(i)) > 1e-6)
{
is_same_camera = false;
break;
}
}
return is_same_camera;
}
}