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