三角化地图点

视觉slam中，地图点大部分都是通过三角化给出初值，再利用最小化重投影误差进行优化细化； 其三角化过程先打给粗糙记录下:

void triangulate ( const Eigen::Matrix3d& K, 
                   const Eigen::Matrix4d T1, const Eigen::Matrix4d& T2, 
                   const Eigen::Vector2d& uu1, const Eigen::Vector2d& uu2, 
                   Eigen::Vector4d& X )
{
    // construct P1 P2，，P1、P2 为世界坐标系下面的3d点坐标
    const Eigen::Matrix<double, 3, 4> P1 = K * T1.block(0,0, 3, 4);
    const Eigen::Matrix<double, 3, 4> P2 = K * T2.block(0, 0, 3, 4);
    
    // P = K* [R | t]
    const Eigen::Matrix<double, 1, 4>& P11 = P1.block(0, 0, 1, 4);
    const Eigen::Matrix<double, 1, 4>& P12 = P1.block(1, 0, 1, 4);
    const Eigen::Matrix<double, 1, 4>& P13 = P1.block(2, 0, 1, 4);
    
    const Eigen::Matrix<double, 1, 4>& P21 = P2.block(0, 0, 1, 4);
    const Eigen::Matrix<double, 1, 4>& P22 = P2.block(1, 0, 1, 4);
    const Eigen::Matrix<double, 1, 4>& P23 = P2.block(2, 0, 1, 4);
    
    const double& u1 = uu1[0];
    const double& v1 = uu1[1];
    const double& u2 = uu2[0];
    const double& v2 = uu2[1];
    
    // 构建W矩阵，用来存储线性方程组系数
    Eigen::Matrix4d W;
    W.block(0, 0, 1, 4) = v1 * P13 - P12;
    W.block(1, 0, 1, 4) = P11 - u1 * P13;
    W.block(2, 0, 1, 4) = v2 * P23 - P22;
    W.block(3, 0, 1, 4) = P21 - u2 * P23;
    
    // SVD  ..为什么使用svd分解，可百度下
    Eigen::JacobiSVD<Eigen::MatrixXd> svd ( W, Eigen::ComputeFullU | Eigen::ComputeFullV );
    Eigen::Matrix4d V = svd.matrixV();
    
    X = V.block(0, 3, 4, 1);
    X = X / X(3, 0);
} // triangulate