diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 99551db7a..b7eef0a2f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -109,7 +109,7 @@ namespace gtsam { /* ************************************************************************* */ Pose3 retract(const Vector& d) { - return expmap(d); + return retract(d); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ba2e9a23e..422cf4357 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -23,142 +23,142 @@ namespace gtsam { - /** - * A 3D pose (R,t) : (Rot3,Point3) - * @ingroup geometry - */ - class Pose3 { - public: - static const size_t dimension = 6; +/** + * A 3D pose (R,t) : (Rot3,Point3) + * @ingroup geometry + */ +class Pose3 { +public: + static const size_t dimension = 6; - /** Pose Concept requirements */ - typedef Rot3 Rotation; - typedef Point3 Translation; + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; - private: - Rot3 R_; - Point3 t_; +private: + Rot3 R_; + Point3 t_; - public: +public: - /** Default constructor is origin */ - Pose3() {} + /** Default constructor is origin */ + Pose3() {} - /** Copy constructor */ - Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} + /** Copy constructor */ + Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} - /** Construct from R,t */ - Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} + /** Construct from R,t */ + Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} - /** Constructor from 4*4 matrix */ - Pose3(const Matrix &T) : - R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), - T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} + /** Constructor from 4*4 matrix */ + Pose3(const Matrix &T) : + R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), + T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} - /** Constructor from 12D vector */ - Pose3(const Vector &V) : - R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), - t_(V(9), V(10),V(11)) {} + /** Constructor from 12D vector */ + Pose3(const Vector &V) : + R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), + t_(V(9), V(10),V(11)) {} - inline const Rot3& rotation() const { return R_; } - inline const Point3& translation() const { return t_; } + inline const Rot3& rotation() const { return R_; } + inline const Point3& translation() const { return t_; } - inline double x() const { return t_.x(); } - inline double y() const { return t_.y(); } - inline double z() const { return t_.z(); } + inline double x() const { return t_.x(); } + inline double y() const { return t_.y(); } + inline double z() const { return t_.z(); } - /** convert to 4*4 matrix */ - Matrix matrix() const; + /** convert to 4*4 matrix */ + Matrix matrix() const; - /** print with optional string */ - void print(const std::string& s = "") const; + /** print with optional string */ + void print(const std::string& s = "") const; - /** assert equality up to a tolerance */ - bool equals(const Pose3& pose, double tol = 1e-9) const; + /** assert equality up to a tolerance */ + bool equals(const Pose3& pose, double tol = 1e-9) const; - /** Compose two poses */ - inline Pose3 operator*(const Pose3& T) const { - return Pose3(R_*T.R_, t_ + R_*T.t_); - } + /** Compose two poses */ + inline Pose3 operator*(const Pose3& T) const { + return Pose3(R_*T.R_, t_ + R_*T.t_); + } - Pose3 transform_to(const Pose3& pose) const; + Pose3 transform_to(const Pose3& pose) const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } - /** Lie requirements */ + /** Lie requirements */ - /** Dimensionality of the tangent space */ - inline size_t dim() const { return dimension; } + /** Dimensionality of the tangent space */ + inline size_t dim() const { return dimension; } - /** identity */ - inline static Pose3 identity() { - return Pose3(); - } + /** identity */ + inline static Pose3 identity() { + return Pose3(); + } - /** - * Derivative of inverse - */ - Pose3 inverse(boost::optional H1=boost::none) const; + /** + * Derivative of inverse + */ + Pose3 inverse(boost::optional H1=boost::none) const; - /** - * composes two poses (first (*this) then p2) - * with optional derivatives - */ - Pose3 compose(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /** + * composes two poses (first (*this) then p2) + * with optional derivatives + */ + Pose3 compose(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** receives the point in Pose coordinates and transforms it to world coordinates */ - Point3 transform_from(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** receives the point in Pose coordinates and transforms it to world coordinates */ + Point3 transform_from(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** syntactic sugar for transform */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } + /** syntactic sugar for transform */ + inline Point3 operator*(const Point3& p) const { return transform_from(p); } - /** receives the point in world coordinates and transforms it to Pose coordinates */ - Point3 transform_to(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** receives the point in world coordinates and transforms it to Pose coordinates */ + Point3 transform_to(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** Exponential map around another pose */ - Pose3 retract(const Vector& d) const; + /** Exponential map around another pose */ + Pose3 retract(const Vector& d) const; - /** Logarithm map around another pose T1 */ - Vector localCoordinates(const Pose3& T2) const; + /** Logarithm map around another pose T1 */ + Vector localCoordinates(const Pose3& T2) const; - /** non-approximated versions of Expmap/Logmap */ - static Pose3 Expmap(const Vector& xi); - static Vector Logmap(const Pose3& p); + /** non-approximated versions of Expmap/Logmap */ + static Pose3 Expmap(const Vector& xi); + static Vector Logmap(const Pose3& p); - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives - */ - Pose3 between(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + * as well as optionally the derivatives + */ + Pose3 between(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** - * Calculate Adjoint map - * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) - */ - Matrix AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } + /** + * Calculate Adjoint map + * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) + */ + Matrix AdjointMap() const; + inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } - /** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = (wx,wy,wz) 3D angular velocity - * v (vx,vy,vz) = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { - return Matrix_(4,4, - 0.,-wz, wy, vx, - wz, 0.,-wx, vy, - -wy, wx, 0., vz, - 0., 0., 0., 0.); - } + /** + * wedge for Pose3: + * @param xi 6-dim twist (omega,v) where + * omega = (wx,wy,wz) 3D angular velocity + * v (vx,vy,vz) = 3D velocity + * @return xihat, 4*4 element of Lie algebra that can be exponentiated + */ + static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { + return Matrix_(4,4, + 0.,-wz, wy, vx, + wz, 0.,-wx, vy, + -wy, wx, 0., vz, + 0., 0., 0., 0.); + } /** * Calculate range to a landmark @@ -178,26 +178,26 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - } - }; // Pose3 class +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + } +}; // Pose3 class - /** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = 3D angular velocity - * v = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - template <> - inline Matrix wedge(const Vector& xi) { - return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); - } +/** + * wedge for Pose3: + * @param xi 6-dim twist (omega,v) where + * omega = 3D angular velocity + * v = 3D velocity + * @return xihat, 4*4 element of Lie algebra that can be exponentiated + */ +template <> +inline Matrix wedge(const Vector& xi) { + return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); +} } // namespace gtsam diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 7b9e2deca..1f23352b3 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -159,7 +159,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl; // Compute expmapped solution - const VALUES x_d(x0.expmap(result.dx_d, ordering)); + const VALUES x_d(x0.retract(result.dx_d, ordering)); // Compute decrease in f result.f_error = f.error(x_d); diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index 52437aa61..5a7d929f6 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -306,7 +306,7 @@ namespace gtsam { DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), *graph_, *values_, *ordering_, error_); - shared_values newValues(new T(values_->expmap(result.dx_d, *ordering_))); + shared_values newValues(new T(values_->retract(result.dx_d, *ordering_))); cout << "newValues: " << newValues.get() << endl; return newValuesErrorLambda_(newValues, result.f_error, result.Delta); } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index a7f2ab001..85044009f 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -405,7 +405,7 @@ TEST(DoglegOptimizer, Iterate) { DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config)); Delta = result.Delta; EXPECT(result.f_error < fg->error(*config)); // Check that error decreases - simulated2D::Values newConfig(config->expmap(result.dx_d, *ord)); + simulated2D::Values newConfig(config->retract(result.dx_d, *ord)); (*config) = newConfig; DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in }