C++ (Cpp) d_expy_d_y примеры использования

Пример #1

Файл: types_six_dof_expmap.cpp Проект: Roboterbastler/ORB_SLAM

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);
}

Пример #2

Файл: anchored_points.cpp Проект: asimay/ScaViSLAM_ros

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);
}