发表于

2D-2D：对极几何

A.对极约束

@输入：参考帧和当前帧的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
2
VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() {
}

路标3D点顶点

1
2
VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3>(){
}

位姿–位姿 边

1
2
3
EdgeSE3Expmap::EdgeSE3Expmap() :
  BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>() {
}
阅读全文 »

发表于

Optimizer::PoseOptimization

用于Tracking中匀速运动模型跟踪等

EdgeSE3ProjectXYZOnlyPose

1
class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

测量值是2维的Vector2d数据，即像素坐标，这条边连接着pose节点，是一元Unary边，只优化SE3的顶点变量，地图点XYZ固定，世界坐标系下的地图点是成员变量Xw，在Optimizer::PoseOptimization(Frame *pFrame)中遍历当前帧可视的地图点调用，一个顶点N条边。
（一元边里面只有_jacobianOplusXi ， 

1
2
3
e = (u,v) - project(Tcw * Pw)  

Tcw = CameraPose

二元边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
23
void 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代表不优化。

阅读全文 »

发表于

Environment: in Ubuntu16.04, with ROS(kinetic). GCC>=5(for Optris) and cmake >=3(for tiscamera).
You can use this commands to check it:

1
2
cmake --version
gcc --version

阅读全文 »

发表于

RANSAC

随机抽样一致算法（RANdom SAmple Consensus,RANSAC）
参考自wiki：
RANSAC WIKI
它采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。 RANSAC是一个非确定性算法，在某种意义上说，它会产生一个在一定概率下合理的结果，而更多次的迭代会使这一概率增加。

阅读全文 »

发表于

其他配套配置：

抓包pcap–用于将pcap的原始文件转化成ROS结构，要在velodyne驱动前安装。

1
sudo apt-get install libpcap-dev

velodyne驱动配置：

阅读全文 »

发表于

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.

Quick Start

Create a new post

1
$ hexo new "My New Post"

More info: Writing

Run server

1
$ hexo server

More info: Server

Generate static files

1
$ hexo generate

More info: Generating

Deploy to remote sites

1
$ hexo deploy

More info: Deployment