OpenGV

OpenGV: The OpenGV library

对于3d-2d的场景，求解

Vocabulary

Bearing vector: 3d 向量，2个自由度

cental 和 non-central的：这里就引出了non-central 的概念，多相机中，在central case， 这个是最简单的，每个相机都单独存在， 在non-central case， 多个相机公用了一个view point

opengv 在debug模式下编译，会导致卡死

---

bearing vector的计算方式

gt这个是三维点， position是body坐标，    camOffsets是0， camRotations是单位阵

//project the point into the viewpoint frame
point_t bodyPoint = rotation.transpose()*(gt.col(i) - position);
 
//project the point into the camera frame
bearingVectors.push_back(camRotation.transpose()*(bodyPoint - camOffset));
 
//normalize the bearing-vector to 1
bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();

实际上bearing vector就是局部坐标的单位方向向量, 

目标函数

Eigen::Matrix<double,4,1> p_hom;
p_hom[3] = 1.0;
//get point in homogeneous form
p_hom.block<3,1>(0,0) = _adapter.getPoint(_indices[i]);
point_t bodyReprojection = inverseSolution * p_hom;
reprojection = reprojection / reprojection.norm();
 
//compute the score
double factor = 1.0;
fvec[i] = factor *
          (1.0 -
          (reprojection.transpose() * _adapter.getBearingVector(_indices[i])));

目标函数的计算流程： 三维点投到body系下，生成一个单位向量。与观测的向量做点积。

二维的观测如何转换成bearing vector呢？

uv to xyz

如果观测到uv, 那么也能很轻松的算出一个带scale的相机坐标系的坐标，然后再归一化，就得到了向量。

状态量：

 使用的是cayley这个状态量，然后转换为rotation matrix，输出如下：

 

同样在sophus中：

 输出：

可以看出这个lie algebra与cayley不太一样

Cayley-Rodrigues Parameters to Express Orientations in Robotics 

 这个cayley，也是一个定义的与rotation相关的东西。