OKVIS 计算 liftJacobian

// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres. bool PoseLocalParameterization3d::liftJacobian(const double* x, double* jacobian) { Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor> > J_lift(jacobian); J_lift.setZero(); const Eigen::Quaterniond q_inv(x[6], -x[3], -x[4], -x[5]); Eigen::Matrix4d Qplus = okvis::kinematics::oplus(q_inv); Eigen::Matrix<double, 3, 4> Jq_pinv = Eigen::Matrix<double, 3, 4>::Zero(); Jq_pinv(0, 0) = 2.0; Jq_pinv(1, 1) = 2.0; Jq_pinv(2, 2) = 2.0; J_lift.bottomRightCorner<3, 4>() = Jq_pinv * Qplus; return true;l }

Jlift(3,7)=0 q−1=q(x[6],−x[3],−x[4],−x[5])

Jq−1p=⎧⎩⎨⎪⎪200020002000⎫⎭⎬⎪⎪

Jlift(3:5,3:6)=Jq−1p∗qR{q−1}

版权声明：本文为博主原创文章，未经博主允许不得转载。