/* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) * See LICENSE for the license information * -------------------------------------------------------------------------- */ /** * @file Similarity3.cpp * @brief Implementation of Similarity3 transform * @author Paul Drews */ #include #include #include namespace gtsam { Similarity3::Similarity3() : R_(), t_(), s_(1) { } Similarity3::Similarity3(double s) : s_(s) { } Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) : R_(R), t_(t), s_(s) { } Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : R_(R), t_(t), s_(s) { } bool Similarity3::equals(const Similarity3& sim, double tol) const { return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) && s_ < (sim.s_ + tol) && s_ > (sim.s_ - tol); } bool Similarity3::operator==(const Similarity3& other) const { return equals(other, 1e-9); } void Similarity3::print(const std::string& s) const { std::cout << std::endl; std::cout << s; rotation().print("R:\n"); translation().print("t: "); std::cout << "s: " << scale() << std::endl; } Similarity3 Similarity3::identity() { return Similarity3(); } Similarity3 Similarity3::operator*(const Similarity3& T) const { return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); } Similarity3 Similarity3::inverse() const { Rot3 Rt = R_.inverse(); Point3 sRt = R_.inverse() * (-s_ * t_); return Similarity3(Rt, sRt, 1.0 / s_); } Point3 Similarity3::transform_from(const Point3& p, // OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { if (H1) { const Matrix3 R = R_.matrix(); Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z()); *H1 << DR, R, R * p.vector(); print("From Derivative"); } if (H2) *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() return R_ * (s_ * p) + t_; // TODO: Effect of scale change is this, right? // No, this is incorrect. Zhaoyang Lv // sR t * (1+v)I 0 * p = s(1+v)R t * p = s(1+v)Rp + t = sRp + vRp + t // 0001 000 1 1 000 1 1 } Point3 Similarity3::operator*(const Point3& p) const { return transform_from(p); } Matrix7 Similarity3::AdjointMap() const { // ToDo: This adjoint might not be correct, it is based on delta = [u, w, lambda] // However, we use the convention delta = [w, u, lambda] const Matrix3 R = R_.matrix(); const Vector3 t = t_.vector(); Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; adj << s_ * R, A, -s_ * t, // 3*7 Z_3x3, R, Matrix31::Zero(), // 3*7 Matrix16::Zero(), 1; // 1*7 return adj; } Matrix33 Similarity3::GetV(Vector3 w, double lambda){ Matrix33 wx = skewSymmetric(w[0], w[1], w[2]); double lambdasquared = lambda * lambda; double thetasquared = w.transpose() * w; double theta = sqrt(thetasquared); double X, Y, Z, W, alpha, beta, gama, mu, upsilon, A, B, C; if (thetasquared > 1e-9 && lambdasquared > 1e-9) { X = sin(theta) / theta; Y = (1 - cos(theta)) / thetasquared; Z = (1 - X) / thetasquared; W = (0.5 - Y) / thetasquared; alpha = lambdasquared / (lambdasquared + thetasquared); beta = (exp(-lambda) - 1 + lambda) / lambdasquared; gama = Y - (lambda * Z); mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda)) / (lambdasquared * lambda); upsilon = Z - (lambda * W); A = (1 - exp(-lambda)) / lambda; B = alpha * (beta - gama) + gama; C = alpha * (mu - upsilon) + upsilon; } else if(thetasquared <= 1e-9 && lambdasquared > 1e-9) { //Taylor series expansions X = 1; Y = 0.5-thetasquared/24.0; Z = 1.0/6.0 - thetasquared/120.0; W = 1.0/24.0 - thetasquared/720.0; alpha = lambdasquared / (lambdasquared + thetasquared); beta = (exp(-lambda) - 1 + lambda) / lambdasquared; gama = Y - (lambda * Z); mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda)) / (lambdasquared * lambda); upsilon = Z - (lambda * W); A = (1 - exp(-lambda)) / lambda; B = alpha * (beta - gama) + gama; C = alpha * (mu - upsilon) + upsilon; } else if(thetasquared > 1e-9 && lambdasquared <= 1e-9) { X = sin(theta) / theta; Y = (1 - cos(theta)) / thetasquared; Z = (1 - X) / thetasquared; W = (0.5 - Y) / thetasquared; alpha = lambdasquared / (lambdasquared + thetasquared); beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0 - (lambda * lambdasquared) / 120; gama = Y - (lambda * Z); mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120 - (lambda * lambdasquared) / 720; upsilon = Z - (lambda * W); if (lambda < 1e-9) { A = 1 - lambda / 2.0 + lambdasquared / 6.0; } else { A = (1 - exp(-lambda)) / lambda; } B = alpha * (beta - gama) + gama; C = alpha * (mu - upsilon) + upsilon; } else { X = 1; Y = 0.5-thetasquared/24.0; Z = 1.0 / 6.0 - thetasquared / 120.0; W = 1.0 / 24.0 - thetasquared / 720.0; alpha = lambdasquared / (lambdasquared + thetasquared); beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0 - (lambda * lambdasquared) / 120; gama = Y - (lambda * Z); mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120 - (lambda * lambdasquared) / 720; upsilon = Z - (lambda * W); if (lambda < 1e-9) { A = 1 - lambda / 2.0 + lambdasquared / 6.0; } else { A = (1 - exp(-lambda)) / lambda; } B = gama; C = upsilon; } return A * Matrix33::Identity() + B * wx + C * wx * wx; } Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { // To get the logmap, calculate w and lambda, then solve for u as show at ethaneade.org // www.ethaneade.org/latex2html/lie/node29.html Vector3 w = Rot3::Logmap(s.R_); double lambda = log(s.s_); Vector7 result; result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda; if (Hm) { // incomplete } return result; } Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { Vector3 w(v.head<3>()); double lambda = v[6]; if (Hm) { Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>()); // incomplete } return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda)); } std::ostream &operator<<(std::ostream &os, const Similarity3& p) { os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " << p.scale() << "]\';"; return os; } const Matrix4 Similarity3::matrix() const { Matrix4 T; T.topRows<3>() << s_ * R_.matrix(), t_.vector(); T.bottomRows<1>() << 0, 0, 0, 1; return T; } Similarity3::operator Pose3() const { return Pose3(R_, s_ * t_); } }