Exemple #1
0
Matrix<double,7,1> Sim3
::log(const Sim3& sim3)
{
  Vector7d res;
  double scale = sim3.scale();

  Vector3d t = sim3.translation_;

  double theta;

  Vector4d omega_sigma = ScSO3::logAndTheta(sim3.scso3_, &theta);
  Vector3d omega = omega_sigma.head<3>();
  double sigma = omega_sigma[3];
  Matrix3d Omega = SO3::hat(omega);

  Matrix3d W = calcW(theta, sigma, scale, Omega);

  //Vector3d upsilon = W.jacobiSvd(ComputeFullU | ComputeFullV).solve(t);
  Vector3d upsilon = W.partialPivLu().solve(t);

  res.segment(0,3) = upsilon;
  res.segment(3,3) = omega;
  res[6] = sigma;
  return res;
}