以下均为简单笔记,如有错误,请多多指教。
- 使用Sophus的SE3类,自己设计g2o的节点与边,实现PnP和ICP的优化。
答:对于PnP问题,需要重新设计的节点包括相机节点、Landmark节点和一个相机-Landmark边;而对于ICP问题,由于边已经实现了,所以只需要实现节点即可。
PnP问题
PnP问题相对简单,其实可以参考g2o/types/sba/types_six_dof_expmap.h编写即可。
#include "sophus/se3.h"
// 相机顶点
class CameraVertex: public BaseVertex<6,Sophus::SE3>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CameraVertex(){}
// 使用sophus进行左乘更新
virtual void oplusImpl(const double *update)
{
Eigen::Matrix<double,6,1> seUpdate(update);
_estimate = Sophus::SE3::exp(seUpdate)*_estimate;
}
bool read(std::istream &is);
bool write(std::ostream &os) const;
}
//Landmark顶点
class LandmarkVertex: public BaseVertex<3,Eigen::Vector3d>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
LandmarkVertex(){}
virtual void oplusImpl(const double *update)
{
_estimate += Eigen::Vector3d(update);
}
bool read(std::istream &is);
bool write(std::ostream &os) const;
}
//边
//描述的就是Landmark投影到图像上
class CameraLandmarkEdge: public BaseBinaryEdge<2,Eigen::Vector2d,LandmarkVertex,CameraVertex>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CameraLandmarkEdge(){}
virtual void computeError()
{
const LandmarkVertex* point = dynamic_cast<const LandmarkVertex*> ( _vertices[0] );
const CameraVertex* pose = dynamic_cast<const CameraVertex*> ( _vertices[1] );
// Camera
const CameraParameters *cam = static_cast<const CameraParameters*>(parameer(0));
// measurement is p, point is p'
// 这里需要检查一下
Eigen::Matrix3d rotate = pose->estimate().rotationMatrix();
Eigen::Vector3d trans = pose->estimate().translation();
Eigen::Vector3d transp = rotate*point->estimate() + trans;
_error = _measurement - cam->cam_map( transp );
}
bool read ( istream& in ) {}
bool write ( ostream& out ) const {}
}
ICP问题
首先介绍一下在原文中ICP问题是怎么处理,代码如下所示。其中linearizeOplus是表示手动求解jac矩阵的函数;由于BaseUnaryEdge是单边,因此_jacobianOplusXi表示的就是表示该边针对李代数的导数。
根据SLAM十四讲P76中4.3.5的公式
∂
(
T
P
)
∂
δ
ξ
=
[
I
−
(
R
p
+
t
)
∧
0
T
0
T
]
\tfrac{\partial(TP)}{\partial\delta\xi}=\begin{bmatrix} I & -(Rp+t)^{\land} \\ 0^T & 0^T \end{bmatrix}
∂δξ∂(TP)=[I0T−(Rp+t)∧0T]
因此很明显可以发现在ICP中
f
=
X
−
T
P
f=X-TP
f=X−TP中对李代数的导数展开就可以得到_jacobianOplusXi中的内容,不过要注意的是顺序,公式中平移量在前,而代码中平移量在后。
// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}
virtual void computeError()
{
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
// measurement is p, point is p'
_error = _measurement - pose->estimate().map( _point );
}
virtual void linearizeOplus()
{
g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
g2o::SE3Quat T(pose->estimate());
Eigen::Vector3d xyz_trans = T.map(_point);
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
_jacobianOplusXi(0,0) = 0;
_jacobianOplusXi(0,1) = -z;
_jacobianOplusXi(0,2) = y;
_jacobianOplusXi(0,3) = -1;
_jacobianOplusXi(0,4) = 0;
_jacobianOplusXi(0,5) = 0;
_jacobianOplusXi(1,0) = z;
_jacobianOplusXi(1,1) = 0;
_jacobianOplusXi(1,2) = -x;
_jacobianOplusXi(1,3) = 0;
_jacobianOplusXi(1,4) = -1;
_jacobianOplusXi(1,5) = 0;
_jacobianOplusXi(2,0) = -y;
_jacobianOplusXi(2,1) = x;
_jacobianOplusXi(2,2) = 0;
_jacobianOplusXi(2,3) = 0;
_jacobianOplusXi(2,4) = 0;
_jacobianOplusXi(2,5) = -1;
}
bool read ( istream& in ) {}
bool write ( ostream& out ) const {}
protected:
Eigen::Vector3d _point;
};
本人设计的节点,感觉思路似乎很简答的~
#include "sophus/se3.h"
// 相机顶点
class CameraVertex: public BaseVertex<6,Sophus::SE3>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CameraVertex(){}
// 使用sophus进行更新
virtual void oplusImpl(const double *update)
{
Eigen::Matrix<double,6,1> seUpdate(update);
_estimate = Sophus::SE3::exp(seUpdate)*_estimate;
}
virtual bool read(std::istream &is);
virtual bool write(std::ostream &os) const;
}
// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, CameraVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}
virtual void computeError()
{
const CameraVertex* pose = static_cast<const CameraVertex*> ( _vertices[0] );
// measurement is p, point is p'
// 这里需要检查一下
Eigen::Matrix3d rotate = pose->estimate().rotationMatrix();
Eigen::Vector3d trans = pose->estimate().translation();
Eigen::Vector3d transp = rotate*_point->estimate() + trans;
_error = _measurement - transp;
}
bool read ( istream& in ) {}
bool write ( ostream& out ) const {}
protected:
Eigen::Vector3d _point;
};