void EdgeProjectPSI2UV::computeError() {
const VertexSBAPointXYZ * psi = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
const VertexSE3Expmap * T_p_from_world = static_cast<const VertexSE3Expmap*>(_vertices[1]);
const VertexSE3Expmap * T_anchor_from_world = static_cast<const VertexSE3Expmap*>(_vertices[2]);
const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0));
Vector2d obs(_measurement);
_error = obs - cam->cam_map(T_p_from_world->estimate()
*T_anchor_from_world->estimate().inverse()
*invert_depth(psi->estimate()));
}
inline Matrix3d d_Tinvpsi_d_psi(const SE3Quat & T, const Vector3d & psi) {
Matrix3d R = T.rotation().toRotationMatrix();
Vector3d x = invert_depth(psi);
Vector3d r1 = R.col(0);
Vector3d r2 = R.col(1);
Matrix3d J;
J.col(0) = r1;
J.col(1) = r2;
J.col(2) = -R*x;
J*=1./psi.z();
return J;
}
void G2oEdgeSim3ProjectUVQ::
computeError()
{
const G2oVertexPointXYZ* psi
= static_cast<const G2oVertexPointXYZ*>(_vertices[0]);
const G2oVertexSE3* T_p_from_world
= static_cast<const G2oVertexSE3*>(_vertices[1]);
const G2oVertexSE3* T_anchor_from_world
= static_cast<const G2oVertexSE3*>(_vertices[2]);
Vector2d obs(_measurement);
_error = obs-T_p_from_world->cam_map(
T_p_from_world->estimate()
*T_anchor_from_world->estimate().inverse()
*invert_depth(psi->estimate()));
}
void G2oEdgeProjectPSI2UVU
::computeError()
{
const G2oVertexPointXYZ * psi
= static_cast<const G2oVertexPointXYZ*>(_vertices[0]);
const G2oVertexSE3 * T_p_from_world
= static_cast<const G2oVertexSE3*>(_vertices[1]);
const G2oVertexSE3 * T_anchor_from_world
= static_cast<const G2oVertexSE3*>(_vertices[2]);
const G2oCameraParameters * cam
= static_cast<const G2oCameraParameters *>(parameter(0));
Vector3d obs(_measurement);
_error = obs - cam->stereocam_uvu_map(
T_p_from_world->estimate()
*T_anchor_from_world->estimate().inverse()
*invert_depth(psi->estimate()));
}
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);
}
