void EdgeProjectPSI2UV::linearizeOplus() { VertexSBAPointXYZ* vpoint = static_cast<VertexSBAPointXYZ*>(_vertices[0]); Vector3d psi_a = vpoint->estimate(); VertexSE3Expmap * vpose = static_cast<VertexSE3Expmap *>(_vertices[1]); SE3Quat T_cw = vpose->estimate(); VertexSE3Expmap * vanchor = static_cast<VertexSE3Expmap *>(_vertices[2]); const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0)); SE3Quat A_aw = vanchor->estimate(); SE3Quat T_ca = T_cw*A_aw.inverse(); Vector3d x_a = invert_depth(psi_a); Vector3d y = T_ca*x_a; Matrix<double,2,3> Jcam = d_proj_d_y(cam->focal_length, y); _jacobianOplus[0] = -Jcam*d_Tinvpsi_d_psi(T_ca, psi_a); _jacobianOplus[1] = -Jcam*d_expy_d_y(y); _jacobianOplus[2] = Jcam*T_ca.rotation().toRotationMatrix()*d_expy_d_y(x_a); }
void G2oEdgeProjectPSI2UVU::linearizeOplus() { G2oVertexPointXYZ* vpoint = static_cast<G2oVertexPointXYZ*>(_vertices[0]); Vector3d psi_a = vpoint->estimate(); G2oVertexSE3 * vpose = static_cast<G2oVertexSE3 *>(_vertices[1]); SE3 T_cw = vpose->estimate(); G2oVertexSE3 * vanchor = static_cast<G2oVertexSE3 *>(_vertices[2]); const G2oCameraParameters * cam = static_cast<const G2oCameraParameters *>(parameter(0)); SE3 A_aw = vanchor->estimate(); SE3 T_ca = T_cw*A_aw.inverse(); Vector3d x_a = invert_depth(psi_a); Vector3d y = T_ca*x_a; Matrix3d Jcam = d_stereoproj_d_y(cam->focal_length_, cam->baseline_, y); _jacobianOplus[0] = -Jcam*d_Tinvpsi_d_psi(T_ca, psi_a); _jacobianOplus[1] = -Jcam*d_expy_d_y(y); _jacobianOplus[2] = Jcam*T_ca.rotation_matrix()*d_expy_d_y(x_a); }