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 jac_quat3_euler3(Eigen::Matrix<double, 6, 6>& J, const SE3Quat& t)
{
const Vector3d& tr0 = t.translation();
const Quaterniond& q0 = t.rotation();
double delta=1e-6;
double idelta= 1. / (2. * delta);
for (int i=0; i<6; i++){
SE3Quat ta, tb;
if (i<3){
Vector3d tra=tr0;
Vector3d trb=tr0;
tra[i] -= delta;
trb[i] += delta;
ta = SE3Quat(q0, tra);
tb = SE3Quat(q0, trb);
} else {
Quaterniond qa=q0;
Quaterniond qb=q0;
if (i == 3) {
qa.x() -= delta;
qb.x() += delta;
}
else if (i == 4) {
qa.y() -= delta;
qb.y() += delta;
}
else if (i == 5) {
qa.z() -= delta;
qb.z() += delta;
}
qa.normalize();
qb.normalize();
ta = SE3Quat(qa, tr0);
tb = SE3Quat(qb, tr0);
}
Vector3d dtr = (tb.translation() - ta.translation())*idelta;
Vector3d taAngles, tbAngles;
quat_to_euler(ta.rotation(), taAngles(2), taAngles(1), taAngles(0));
quat_to_euler(tb.rotation(), tbAngles(2), tbAngles(1), tbAngles(0));
Vector3d da = (tbAngles - taAngles) * idelta; //TODO wraparounds not handled
for (int j=0; j<6; j++){
if (j<3){
J(j, i) = dtr(j);
} else {
J(j, i) = da(j-3);
}
}
}
}
Isometry3d fromSE3Quat(const SE3Quat& t)
{
Isometry3d result = (Eigen::Isometry3d) t.rotation();
result.translation() = t.translation();
return result;
}