参考资料
学习四元数的小demo:
Eigen_Quaternion_demo
主要的参考文档,Eigen官网中四元数部分:
QuaternionBase
勿道君行早 更有早行人
@输入:参考帧和当前帧的2D点坐标对(一般8对),内参
@输出:运动恢复R t
参考帧和当前帧的二维像素点通过特征点法拿到了匹配关系,那么从各自光心出发的两条射线交点即3D点,P和两个光心构成了极平面,$e1$ \ $e2$为极点,$l1$ \ $l2$是极线(Epipolar line)。
对极几何必须要有正确的匹配关系,否则当前帧无法找到参考帧$p1$对应的点在$p2$,那样的话就要用极线搜索了。
对极几何求R t的步骤是:
1.根据匹配的像素点对和内参求出基础矩阵F或者本质矩阵E。E=t^ R,只和运动有关,所以是我们求的本质,平移和旋转各3自由度,单目尺度歧义性自由度-1,所以E有5个自由度,一般用八点法,8对点求;F还包括内参$$F=K^{-T}EK^{-1}$$.
2.根据E或者R恢复R t。这里一般用SVD分解恢复出4个解,然后取点在相机前方的那个解。
主要代码:
g2o/types/sba/types_six_dof_expmap.h
g2o/types/sba/types_six_dof_expmap.cpp
SE3Quat
包括四元数和vector3
1 | VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() { |
1 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3>(){ |
1 | EdgeSE3Expmap::EdgeSE3Expmap() : |
用于Tracking中匀速运动模型跟踪等
1 | class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap> |
测量值是2维的Vector2d数据,即像素坐标,这条边连接着pose节点,是一元Unary边,只优化SE3的顶点变量,地图点XYZ固定,世界坐标系下的地图点是成员变量Xw,在Optimizer::PoseOptimization(Frame *pFrame)
中遍历当前帧可视的地图点调用,一个顶点N条边。
(一元边里面只有_jacobianOplusXi
,
1 | e = (u,v) - project(Tcw * Pw) |
二元边BaseBinaryEdge有_jacobianOplusXi
和_jacobianOplusXj
,这个比较好理解)
观测二维,所以残差也是二维,即当前帧的像素观测和地图点在当前帧的投影位置的残差,从而优化位姿。残差对位姿的求导所以雅克比维度是2X6。1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
Vector3d xyz_trans = vi->estimate().map(Xw);
double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;
_jacobianOplusXi(0,0) = x*y*invz_2 *fx;
_jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
_jacobianOplusXi(0,2) = y*invz *fx;
_jacobianOplusXi(0,3) = -invz *fx;
_jacobianOplusXi(0,4) = 0;
_jacobianOplusXi(0,5) = x*invz_2 *fx;
_jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
_jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
_jacobianOplusXi(1,2) = -x*invz *fy;
_jacobianOplusXi(1,3) = 0;
_jacobianOplusXi(1,4) = -invz *fy;
_jacobianOplusXi(1,5) = y*invz_2 *fy;
}
加完边后进行4次迭代,每次迭代优化10次,每次迭代完会有一次野值的筛选(根据卡方分布),TODO:野值边setLevel(1),内点setLevle(0),why,好像1代表不优化。
随机抽样一致算法(RANdom SAmple Consensus,RANSAC)
参考自wiki:
RANSAC WIKI
它采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。 RANSAC是一个非确定性算法,在某种意义上说,它会产生一个在一定概率下合理的结果,而更多次的迭代会使这一概率增加。
Welcome to Hexo! This is your very first post. Check documentation for more info. If you get any problems when using Hexo, you can find the answer in troubleshooting or you can ask me on GitHub.
1 | $ hexo new "My New Post" |
More info: Writing
1 | $ hexo server |
More info: Server
1 | $ hexo generate |
More info: Generating
1 | $ hexo deploy |
More info: Deployment