对于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, 那么也能很轻松的算出一个带scale的相机坐标系的坐标,然后再归一化,就得到了向量。
状态量:
使用的是cayley这个状态量,然后转换为rotation matrix,输出如下:
同样在sophus中:
输出:
可以看出这个lie algebra与cayley不太一样
Cayley-Rodrigues Parameters to Express Orientations in Robotics
这个cayley,也是一个定义的与rotation相关的东西。