From 0a7fd27f28bc155c198618ea3ebd00067e451b11 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 10 Feb 2016 17:48:52 -0800 Subject: [PATCH] Working on getting a simple typedef to compile - as well as dealing with Point3() now creating uninitialized memory. --- gtsam/base/serializationTestHelpers.h | 27 +-- gtsam/geometry/EssentialMatrix.cpp | 2 +- gtsam/geometry/OrientedPlane3.cpp | 4 +- gtsam/geometry/Point3.cpp | 27 +-- gtsam/geometry/Point3.h | 173 +++--------------- gtsam/geometry/Pose3.cpp | 52 +++--- gtsam/geometry/Pose3.h | 10 +- gtsam/geometry/Rot3.cpp | 4 +- gtsam/geometry/Rot3.h | 36 +--- gtsam/geometry/Rot3M.cpp | 10 +- gtsam/geometry/Unit3.cpp | 21 +-- gtsam/geometry/tests/testPinholeCamera.cpp | 12 +- gtsam/geometry/tests/testPinholePose.cpp | 13 +- gtsam/geometry/tests/testPoint3.cpp | 18 +- gtsam/geometry/tests/testPose3.cpp | 14 +- gtsam/geometry/tests/testRot3.cpp | 4 +- gtsam/geometry/tests/testSimpleCamera.cpp | 10 +- gtsam/geometry/tests/testStereoCamera.cpp | 2 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/geometry/triangulation.h | 2 +- gtsam/navigation/GPSFactor.cpp | 8 +- gtsam/navigation/GPSFactor.h | 5 +- gtsam/navigation/NavState.cpp | 13 +- gtsam/navigation/NavState.h | 7 +- gtsam/navigation/PreintegrationBase.cpp | 4 +- gtsam/navigation/Scenario.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 6 +- gtsam/slam/InitializePose3.cpp | 2 +- gtsam/slam/dataset.cpp | 6 +- gtsam/slam/dataset.h | 3 +- gtsam_unstable/geometry/Event.h | 2 +- gtsam_unstable/geometry/Similarity3.cpp | 4 +- .../geometry/tests/testSimilarity3.cpp | 6 +- 33 files changed, 155 insertions(+), 356 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index f408427d8..c55e5067b 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -11,7 +11,7 @@ /** * @file serializationTestHelpers.h - * @brief + * @brief * @author Alex Cunningham * @author Richard Roberts * @date Feb 7, 2012 @@ -30,6 +30,11 @@ const bool verbose = false; namespace gtsam { namespace serializationTestHelpers { +// templated default object creation so we only need to declare one friend (if applicable) +template +T create() { + return T(); +} // Templated round-trip serialization template @@ -44,7 +49,7 @@ void roundtrip(const T& input, T& output) { // This version requires equality operator template bool equality(const T& input = T()) { - T output; + T output = create(); roundtrip(input,output); return input==output; } @@ -52,7 +57,7 @@ bool equality(const T& input = T()) { // This version requires Testable template bool equalsObj(const T& input = T()) { - T output; + T output = create(); roundtrip(input,output); return assert_equal(input, output); } @@ -60,7 +65,7 @@ bool equalsObj(const T& input = T()) { // De-referenced version for pointers, requires equals method template bool equalsDereferenced(const T& input) { - T output; + T output = create(); roundtrip(input,output); return input->equals(*output); } @@ -79,7 +84,7 @@ void roundtripXML(const T& input, T& output) { // This version requires equality operator template bool equalityXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return input==output; } @@ -87,7 +92,7 @@ bool equalityXML(const T& input = T()) { // This version requires Testable template bool equalsXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return assert_equal(input, output); } @@ -95,7 +100,7 @@ bool equalsXML(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return input->equals(*output); } @@ -114,7 +119,7 @@ void roundtripBinary(const T& input, T& output) { // This version requires equality operator template bool equalityBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return input==output; } @@ -122,7 +127,7 @@ bool equalityBinary(const T& input = T()) { // This version requires Testable template bool equalsBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return assert_equal(input, output); } @@ -130,7 +135,7 @@ bool equalsBinary(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return input->equals(*output); } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 7b90edc39..699705fa5 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -107,7 +107,7 @@ ostream& operator <<(ostream& os, const EssentialMatrix& E) { Rot3 R = E.rotation(); Unit3 d = E.direction(); os.precision(10); - os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + os << R.xyz().transpose() << " " << d.point3().transpose() << " "; return os; } diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 95290497d..bb9925e23 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -39,7 +39,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); Vector3 unit_vec = n_rotated.unitVector(); - double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_; + double pred_d = n_.unitVector().dot(xr.translation()) + d_; if (Hr) { *Hr = zeros(3, 6); @@ -47,7 +47,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hr->block<1, 3>(2, 3) = unit_vec; } if (Hp) { - Vector2 hpp = n_.basis().transpose() * xr.translation().vector(); + Vector2 hpp = n_.basis().transpose() * xr.translation(); *Hp = Z_3x3; Hp->block<2, 2>(0, 0) = D_rotated_pose; Hp->block<1, 2>(2, 0) = hpp; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 181ff3e97..01b5ed132 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -21,40 +21,16 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ +#ifndef GTSAM_USE_VECTOR3_POINTS bool Point3::equals(const Point3 &q, double tol) const { return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); } -/* ************************************************************************* */ void Point3::print(const string& s) const { cout << s << *this << endl; } -#ifndef GTSAM_USE_VECTOR3_POINTS -/* ************************************************************************* */ -bool Point3::operator==(const Point3& q) const { - return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; -} - -Point3 Point3::operator+(const Point3& q) const { - return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); -} - -Point3 Point3::operator-(const Point3& q) const { - return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); -} - -Point3 Point3::operator*(double s) const { - return Point3(x_ * s, y_ * s, z_ * s); -} - -Point3 Point3::operator/(double s) const { - return Point3(x_ / s, y_ / s, z_ / s); -} -#endif - /* ************************************************************************* */ double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { @@ -102,6 +78,7 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, } #endif +#endif /* ************************************************************************* */ double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 75b8a7829..ba06e1d85 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -30,6 +30,12 @@ namespace gtsam { //#define GTSAM_USE_VECTOR3_POINTS #ifdef GTSAM_USE_VECTOR3_POINTS + + // As of GTSAM4, we just typedef Point3 to Vector3 + typedef Vector3 Point3; + +#else + /** * A 3D point is just a Vector3 with some additional methods * @addtogroup geometry @@ -38,13 +44,17 @@ namespace gtsam { class GTSAM_EXPORT Point3 : public Vector3 { public: + enum { dimension = 3 }; /// @name Standard Constructors /// @{ - /// Default constructor creates a zero-Point3 - Point3() { setZero(); } +#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// Default constructor now creates *uninitialized* point !!!! + Point3() {} +#endif + /// Construct from x, y, and z coordinates Point3(double x, double y, double z): Vector3(x,y, z) {} @@ -68,7 +78,7 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @{ /// identity for group operation - inline static Point3 identity() { return Point3();} + inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); } /// @} /// @name Vector Space @@ -82,7 +92,7 @@ class GTSAM_EXPORT Point3 : public Vector3 { double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; + Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // @@ -104,136 +114,10 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3); - } - }; -#else -/** - * A 3D point - * @addtogroup geometry - * \nosubgrouping - */ -class GTSAM_EXPORT Point3 { - - private: - - double x_, y_, z_; - - public: - enum { dimension = 3 }; - - /// @name Standard Constructors - /// @{ - - /// Default constructor creates a zero-Point3 - Point3(): x_(0), y_(0), z_(0) {} - - /// Construct from x, y, and z coordinates - Point3(double x, double y, double z): x_(x), y_(y), z_(z) {} - - /// @} - /// @name Advanced Constructors - /// @{ - - /// Construct from 3-element vector - explicit Point3(const Vector3& v) { - x_ = v(0); - y_ = v(1); - z_ = v(2); - } - - /// @} - /// @name Testable - /// @{ - - /** print with optional string */ - void print(const std::string& s = "") const; - - /** equals with an tolerance */ - bool equals(const Point3& p, double tol = 1e-9) const; - - /// @} - /// @name Group - /// @{ - - /// identity for group operation - inline static Point3 identity() { return Point3();} - - /// inverse - Point3 operator - () const { return Point3(-x_,-y_,-z_);} - - /// add vector on right - inline Point3 operator +(const Vector3& v) const { - return Point3(x_ + v[0], y_ + v[1], z_ + v[2]); - } - - /// add - Point3 operator + (const Point3& q) const; - - /// subtract - Point3 operator - (const Point3& q) const; - - /// @} - /// @name Vector Space - /// @{ - - ///multiply with a scalar - Point3 operator * (double s) const; - - ///divide by a scalar - Point3 operator / (double s) const; - - /** distance between two points */ - double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) const; - - /** Distance of the point from the origin, with Jacobian */ - double norm(OptionalJacobian<1,3> H = boost::none) const; - - /** normalize, with optional Jacobian */ - Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; - - /** cross product @return this x q */ - Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // - OptionalJacobian<3, 3> H_q = boost::none) const; - - /** dot product @return this * q*/ - double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // - OptionalJacobian<1, 3> H_q = boost::none) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// equality - bool operator ==(const Point3& q) const; - - /** return vectorized form (column-wise)*/ - Vector3 vector() const { return Vector3(x_,y_,z_); } - - /// get x - inline double x() const {return x_;} - - /// get y - inline double y() const {return y_;} - - /// get z - inline double z() const {return z_;} - - /// @} - - /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ + Point3() { setZero(); } Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} @@ -252,37 +136,26 @@ class GTSAM_EXPORT Point3 { private: - /// @name Advanced Interface - /// @{ - /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) - { - ar & BOOST_SERIALIZATION_NVP(x_); - ar & BOOST_SERIALIZATION_NVP(y_); - ar & BOOST_SERIALIZATION_NVP(z_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3); } - - /// @} }; -/// Syntactic sugar for multiplying coordinates by a scalar s*p -inline Point3 operator*(double s, const Point3& p) { return p*s;} - -#endif - -// Convenience typedef -typedef std::pair Point3Pair; -std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); - template<> struct traits : public internal::VectorSpace {}; template<> struct traits : public internal::VectorSpace {}; +#endif + +// Convenience typedef +typedef std::pair Point3Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); + /// distance between two points double distance(const Point3& p1, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 44eb5c226..ccf39f153 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -48,8 +48,7 @@ Pose3 Pose3::inverse() const { // Experimental - unit tests of derivatives based on it do not check out yet Matrix6 Pose3::AdjointMap() const { const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); - Matrix3 A = skewSymmetric(t) * R; + Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; Matrix6 adj; adj << R, Z_3x3, A, R; return adj; @@ -101,12 +100,12 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, void Pose3::print(const string& s) const { cout << s; R_.print("R:\n"); - t_.print("t: "); + traits::Print(t_, "t: "); } /* ************************************************************************* */ bool Pose3::equals(const Pose3& pose, double tol) const { - return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol); + return R_.equals(pose.R_, tol) && traits::Equals(t_, pose.t_, tol); } /* ************************************************************************* */ @@ -115,14 +114,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { if (H) *H = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi - Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); + Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); - Rot3 R = Rot3::Expmap(omega.vector()); + Rot3 R = Rot3::Expmap(omega); double theta2 = omega.dot(omega); if (theta2 > std::numeric_limits::epsilon()) { - Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Point3 omega_cross_v = omega.cross(v); // points towards axis - Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; + Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis + Vector3 omega_cross_v = omega.cross(v); // points towards axis + Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; return Pose3(R, t); } else { return Pose3(R, v); @@ -132,19 +131,20 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { /* ************************************************************************* */ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { if (H) *H = LogmapDerivative(p); - Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); - double t = w.norm(); + const Vector3 w = Rot3::Logmap(p.rotation()); + const Vector3 T = p.translation(); + const double t = w.norm(); if (t < 1e-10) { Vector6 log; log << w, T; return log; } else { - Matrix3 W = skewSymmetric(w / t); + const Matrix3 W = skewSymmetric(w / t); // Formula from Agrawal06iros, equation (14) // simplified with Mathematica, and multiplying in T to avoid matrix math - double Tan = tan(0.5 * t); - Vector3 WT = W * T; - Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); + const double Tan = tan(0.5 * t); + const Vector3 WT = W * T; + const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); Vector6 log; log << w, u; return log; @@ -178,7 +178,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { H->topLeftCorner<3,3>() = DR; } Vector6 xi; - xi << omega, T.translation().vector(); + xi << omega, T.translation(); return xi; #endif } @@ -264,7 +264,7 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { Matrix4 Pose3::matrix() const { static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished(); Matrix4 mat; - mat << R_.matrix(), t_.vector(), A14; + mat << R_.matrix(), t_, A14; return mat; } @@ -288,7 +288,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, if (Dpoint) { *Dpoint = R; } - return Point3(R * p.vector()) + t_; + return R_ * p + t_; } /* ************************************************************************* */ @@ -297,7 +297,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p.vector() - t_.vector())); + const Point3 q(Rt*(p - t_)); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Dpose) << @@ -321,7 +321,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, return local.norm(); } else { Matrix13 D_r_local; - const double r = local.norm(D_r_local); + const double r = norm(local, D_r_local); if (H1) *H1 = D_r_local * D_local_pose; if (H2) *H2 = D_r_local * D_local_point; return r; @@ -361,10 +361,10 @@ boost::optional align(const vector& pairs) { return boost::none; // we need at least three pairs // calculate centroids - Vector3 cp = Vector3::Zero(), cq = Vector3::Zero(); + Point3 cp(0,0,0), cq(0,0,0); BOOST_FOREACH(const Point3Pair& pair, pairs) { - cp += pair.first.vector(); - cq += pair.second.vector(); + cp += pair.first; + cq += pair.second; } double f = 1.0 / n; cp *= f; @@ -373,10 +373,10 @@ boost::optional align(const vector& pairs) { // Add to form H matrix Matrix3 H = Z_3x3; BOOST_FOREACH(const Point3Pair& pair, pairs) { - Vector3 dp = pair.first.vector() - cp; - Vector3 dq = pair.second.vector() - cq; + Point3 dp = pair.first - cp; + Point3 dq = pair.second - cq; H += dp * dq.transpose(); - } + } // Compute SVD Matrix U, V; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index fa4b2e51a..ba4b9737b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -52,8 +52,7 @@ public: /// @{ /** Default constructor is origin */ - Pose3() { - } + Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} /** Copy constructor */ Pose3(const Pose3& pose) : @@ -65,13 +64,6 @@ public: R_(R), t_(t) { } -#ifndef GTSAM_USE_VECTOR3_POINTS - /** Construct from R,t */ - Pose3(const Rot3& R, const Vector3& t) : - R_(R), t_(t) { - } -#endif - /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index ef384c3ef..696c9df68 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -82,7 +82,7 @@ Unit3 Rot3::operator*(const Unit3& p) const { Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { const Matrix3& Rt = transpose(); - Point3 q(Rt * p.vector()); // q = Rt*p + Point3 q(Rt * p); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); if (H1) *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b26475578..30385bd8c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -171,22 +171,6 @@ namespace gtsam { return Rot3(q); } -#ifndef GTSAM_USE_VECTOR3_POINTS - /** - * Convert from axis/angle representation - * @param axis is the rotation axis, unit length - * @param angle rotation angle - * @return incremental rotation - */ - static Rot3 AxisAngle(const Vector3& axis, double angle) { -#ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); -#else - return Rot3(SO3::AxisAngle(axis,angle)); -#endif - } -#endif - /** * Convert from axis/angle representation * @param axisw is the rotation axis, unit length @@ -197,7 +181,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS return gtsam::Quaternion(Eigen::AngleAxis(angle, axis.vector())); #else - return Rot3(SO3::AxisAngle(axis.vector(),angle)); + return Rot3(SO3::AxisAngle(axis,angle)); #endif } @@ -365,24 +349,6 @@ namespace gtsam { Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2=boost::none) const; -#ifndef GTSAM_USE_VECTOR3_POINTS - /// operator* for Vector3 - inline Vector3 operator*(const Vector3& v) const { - return rotate(Point3(v)).vector(); - } - - /// rotate for Vector3 - Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { - return rotate(Point3(v), H1, H2).vector(); - } - /// unrotate for Vector3 - Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { - return unrotate(Point3(v), H1, H2).vector(); - } -#endif - /// @} /// @name Group Action on Unit3 /// @{ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index c3636000b..03cde1a01 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = col1.vector(); - rot_.col(1) = col2.vector(); - rot_.col(2) = col3.vector(); + rot_.col(0) = (Vector3)col1; + rot_.col(1) = (Vector3)col2; + rot_.col(2) = (Vector3)col3; } /* ************************************************************************* */ @@ -121,7 +121,7 @@ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = rot_; - return Point3(rot_ * p.vector()); + return rot_ * p; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 69d4276dd..a64f99eb1 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -45,9 +45,8 @@ namespace gtsam { Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; - Point3 normalized = normalize(point, H ? &D_p_point : 0); Unit3 direction; - direction.p_ = normalized.vector(); + direction.p_ = normalize(point, H ? &D_p_point : 0); if (H) *H << direction.basis().transpose() * D_p_point; return direction; @@ -90,22 +89,18 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point - Point3 axis; + Point3 axis(0, 0, 1); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); - } else if ((mz <= mx) && (mz <= my)) { - axis = Point3(0.0, 0.0, 1.0); - } else { - assert(false); } // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // the chosen axis. Matrix33 H_B1_n; - Point3 B1 = n.cross(axis, H ? &H_B1_n : 0); + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0); // Normalize result to get a unit vector: b1 = B1 / |B1|. Matrix33 H_b1_B1; @@ -114,7 +109,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // No need to normalize this, p and b1 are orthogonal unit vectors. Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = n.cross(b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); + Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); // Create the basis by stacking b1 and b2. B_.reset(Matrix32()); @@ -176,7 +171,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, // Compute the dot product of the Point3s. Matrix13 H_dot_pn, H_dot_qn; - double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); + double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); if (H_p) { (*H_p) << H_dot_pn * H_pn_p; @@ -209,7 +204,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ // 2D error here is projecting q into the tangent plane of this (p). Matrix62 H_B_p; Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); - Vector2 xi = Bt * qn.vector(); + Vector2 xi = Bt * qn; if (H_p) { // Derivatives of each basis vector. @@ -217,8 +212,8 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); // Derivatives of the two entries of xi wrt the basis vectors. - Matrix13 H_xi1_b1 = qn.vector().transpose(); - Matrix13 H_xi2_b2 = qn.vector().transpose(); + Matrix13 H_xi1_b1 = qn.transpose(); + Matrix13 H_xi2_b2 = qn.transpose(); // Assemble dxi/dp = dxi/dB * dB/dp. Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ce9932879..4682a6507 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -103,16 +103,16 @@ TEST( PinholeCamera, level2) TEST( PinholeCamera, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); - EXPECT(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal(camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) { double result = camera.range(point1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(Hexpected1, D1, 1e-7)); EXPECT(assert_equal(Hexpected2, D2, 1e-7)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 3c79a45e3..c5137e3b3 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -74,16 +74,16 @@ TEST(PinholeCamera, Pose) { TEST( PinholePose, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); EXPECT(assert_equal( camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -120,7 +120,7 @@ TEST( PinholePose, backprojectInfinity) /* ************************************************************************* */ TEST( PinholePose, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -212,8 +212,7 @@ TEST( PinholePose, range0) { double result = camera.range(point1, D1, D2); Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, - 1e-9); + EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 37ad105c7..c7fb44716 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -71,14 +71,14 @@ TEST( Point3, arithmetic) { /* ************************************************************************* */ TEST( Point3, equals) { - CHECK(P.equals(P)); - Point3 Q; - CHECK(!P.equals(Q)); + CHECK(traits::Equals(P,P)); + Point3 Q(0,0,0); + CHECK(!traits::Equals(P,Q)); } /* ************************************************************************* */ TEST( Point3, dot) { - Point3 origin, ones(1, 1, 1); + Point3 origin(0,0,0), ones(1, 1, 1); CHECK(origin.dot(Point3(1, 1, 0)) == 0); CHECK(ones.dot(Point3(1, 1, 0)) == 2); } @@ -111,20 +111,20 @@ TEST (Point3, norm) { Matrix actualH; Point3 point(3,4,5); // arbitrary point double expected = sqrt(50); - EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8); + EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8); Matrix expectedH = numericalDerivative11(norm_proxy, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } /* ************************************************************************* */ double testFunc(const Point3& P, const Point3& Q) { - return P.distance(Q); + return distance(P,Q); } TEST (Point3, distance) { Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3); Matrix H1, H2; - double d = P.distance(Q, H1, H2); + double d = distance(P, Q, H1, H2); double expectedDistance = 55.542686; Matrix numH1 = numericalDerivative21(testFunc, P, Q); Matrix numH2 = numericalDerivative22(testFunc, P, Q); @@ -137,9 +137,9 @@ TEST (Point3, distance) { TEST(Point3, cross) { Matrix aH1, aH2; boost::function f = - boost::bind(&Point3::cross, _1, _2, boost::none, boost::none); + boost::bind(>sam::cross, _1, _2, boost::none, boost::none); const Point3 omega(0, 1, 0), theta(4, 6, 8); - omega.cross(theta, aH1, aH2); + cross(omega, theta, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 56e1e022c..61550d540 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -62,7 +62,7 @@ TEST( Pose3, retract_first_order) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2)); + EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2)); } @@ -72,7 +72,7 @@ TEST( Pose3, retract_expmap) { Vector v = zero(6); v(0) = 0.3; Pose3 pose = Pose3::Expmap(v); - EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2)); + EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2)); EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); } @@ -82,7 +82,7 @@ TEST( Pose3, expmap_a_full) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } @@ -93,7 +93,7 @@ TEST( Pose3, expmap_a_full2) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } @@ -388,7 +388,7 @@ TEST( Pose3, transform_to_translate) /* ************************************************************************* */ TEST( Pose3, transform_to_rotate) { - Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3()); + Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0)); Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 expected(-1,2,10); EXPECT(assert_equal(expected, actual, 0.001)); @@ -406,7 +406,7 @@ TEST( Pose3, transform_to) /* ************************************************************************* */ TEST( Pose3, transform_from) { - Point3 actual = T3.transform_from(Point3()); + Point3 actual = T3.transform_from(Point3(0,0,0)); Point3 expected = Point3(1.,2.,3.); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index ce5d3268c..f96198b94 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -542,8 +542,8 @@ TEST(Rot3, quaternion) { Point3 expected1 = R1*p1; Point3 expected2 = R2*p2; - Point3 actual1 = Point3(q1*p1.vector()); - Point3 actual2 = Point3(q2*p2.vector()); + Point3 actual1 = Point3(q1*p1); + Point3 actual2 = Point3(q2*p2); EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(expected2, actual2)); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 8db8113a1..71dcabd4e 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -63,16 +63,16 @@ TEST( SimpleCamera, level2) TEST( SimpleCamera, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - SimpleCamera camera = SimpleCamera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + SimpleCamera camera = SimpleCamera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); CHECK(assert_equal( camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -100,7 +100,7 @@ TEST( SimpleCamera, backproject) /* ************************************************************************* */ TEST( SimpleCamera, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down SimpleCamera camera(Pose3(rot, origin), K); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 6508965ec..497599a6a 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -46,7 +46,7 @@ TEST( StereoCamera, project) // point X Y Z in meters Point3 p(0, 0, 5); StereoPoint2 result = stereoCam.project(p); - CHECK(assert_equal(StereoPoint2(320,320-150,240),result)); + CHECK(assert_equal(StereoPoint2(320, 320 - 150, 240), result)); // point X Y Z in meters Point3 p2(1, 1, 5); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index dbe315807..07789acaa 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -377,7 +377,7 @@ TEST(Unit3, retract_expmap) { TEST(Unit3, Random) { boost::mt19937 rng(42); // Check that means are all zero at least - Point3 expectedMean, actualMean; + Point3 expectedMean(0,0,0), actualMean(0,0,0); for (size_t i = 0; i < 100; i++) actualMean = actualMean + Unit3::Random(rng).point3(); actualMean = actualMean / 100; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index c6e613b14..15029b8ad 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -436,7 +436,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, BOOST_FOREACH(const CAMERA& camera, cameras) { const Pose3& pose = camera.pose(); if (params.landmarkDistanceThreshold > 0 - && pose.translation().distance(point) + && distance(pose.translation(), point) > params.landmarkDistanceThreshold) return TriangulationResult::Degenerate(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 2f4cb9bc3..b2cb5ed82 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -25,14 +25,14 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; - nT_.print(" prior mean: "); + cout << " prior mean: " << nT_ << "\n"; noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** @@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p, H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 3) << p.rotation().matrix(); } - return p.translation().vector() -nT_.vector(); + return p.translation() -nT_; } //*************************************************************************** @@ -66,7 +66,7 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, // Construct initial pose Pose3 nTb(nRb, nT); // nTb - return make_pair(nTb, nV.vector()); + return make_pair(nTb, nV); } //*************************************************************************** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index cd5fa71d2..46e194460 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -48,8 +48,7 @@ public: typedef GPSFactor This; /** default constructor - only use for serialization */ - GPSFactor() { - } + GPSFactor(): nT_(0, 0, 0) {} virtual ~GPSFactor() { } diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 860eaa85c..7e5d85cde 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -89,7 +89,7 @@ void NavState::print(const string& s) const { //------------------------------------------------------------------------------ bool NavState::equals(const NavState& other, double tol) const { - return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) && equal_with_abs_tol(v_, other.v_, tol); } @@ -121,11 +121,6 @@ Point3 NavState::operator*(const Point3& b_t) const { return Point3(R_ * b_t + t_); } -//------------------------------------------------------------------------------ -Velocity3 NavState::operator*(const Velocity3& b_v) const { - return Velocity3(R_ * b_v + v_); -} - //------------------------------------------------------------------------------ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, OptionalJacobian<9, 9> H) { @@ -189,7 +184,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { throw runtime_error("NavState::Logmap derivative not implemented yet"); TIE(nRb, n_p, n_v, nTb); - Vector3 n_t = n_p.vector(); + Vector3 n_t = n_p; // NOTE(frank): See Pose3::Logmap Vector9 xi; @@ -300,7 +295,7 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper dV(xi) << ((-2.0 * dt) * omega_cross_vel); if (secondOrder) { - const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector())); + const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t)); dP(xi) -= (0.5 * dt2) * omega_cross2_t; dV(xi) -= dt * omega_cross2_t; } diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 4c8b50776..02da46317 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -49,7 +49,7 @@ public: /// Default constructor NavState() : - v_(Vector3::Zero()) { + t_(0,0,0), v_(Vector3::Zero()) { } /// Construct from attitude, position, velocity NavState(const Rot3& R, const Point3& t, const Velocity3& v) : @@ -97,7 +97,7 @@ public: } /// Return position as Vector3 Vector3 t() const { - return t_.vector(); + return t_; } /// Return velocity as Vector3. Computation-free. const Vector3& v() const { @@ -147,9 +147,6 @@ public: /// Act on position alone, n_t = nRb * b_t + n_t Point3 operator*(const Point3& b_t) const; - /// Act on velocity alone, n_v = nRb * b_v + n_v - Velocity3 operator*(const Velocity3& b_v) const; - /// @} /// @name Manifold /// @{ diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c3f203849..7209391f1 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -94,7 +94,7 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs; // Centrifugal acceleration - const Vector3 b_arm = p().body_P_sensor->translation().vector(); + const Vector3 b_arm = p().body_P_sensor->translation(); if (!b_arm.isZero()) { // Subtract out the the centripetal acceleration from the unbiased one // to get linear acceleration vector in the body frame: @@ -188,7 +188,7 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, if (p().body_P_sensor) { // More complicated derivatives in case of non-trivial sensor pose *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().vector().isZero()) + if (!p().body_P_sensor->translation().isZero()) *C += *B* D_correctedAcc_omega; *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index ad684f5f8..e4b380ff9 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -80,7 +80,7 @@ class AcceleratingScenario : public Scenario { AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, const Vector3& a_n, const Vector3& omega_b = Vector3::Zero()) - : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} + : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} Pose3 pose(double t) const override { return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index b81332f5e..f05504d6b 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -84,9 +84,9 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } else if (const GenericValue* p = dynamic_cast*>(&value)) { t << p->value().x(), p->value().y(), 0; } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t = p->value().translation().vector(); + t = p->value().translation(); } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t = p->value().vector(); + t = p->value(); } else { return boost::none; } diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 26580f41e..ddc7b18d6 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -327,7 +327,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ Key key = key_value.key; const Rot3& rot = initialRot.at(key); - Pose3 initializedPose = Pose3(rot, Point3()); + Pose3 initializedPose = Pose3(rot, Point3(0,0,0)); initialPose.insert(key, initializedPose); } // add prior diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 50516afe4..c79fb5823 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1,5 +1,5 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -808,7 +808,7 @@ bool writeBAL(const string& filename, SfM_data &data) { Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().vector() << endl; + os << poseOpenGL.translation() << endl; os << cameraCalibration.fx() << endl; os << cameraCalibration.k1() << endl; os << cameraCalibration.k2() << endl; @@ -880,7 +880,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, dataValues.tracks[j].r = 1.0; dataValues.tracks[j].g = 0.0; dataValues.tracks[j].b = 0.0; - dataValues.tracks[j].p = Point3(); + dataValues.tracks[j].p = Point3(0,0,0); } } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 54e27229c..dc7804c2d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -147,6 +147,7 @@ typedef std::pair SIFT_Index; /// Define the structure for the 3D points struct SfM_Track { + SfM_Track():p(0,0,0) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 5bd453424..77b02d9fb 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -35,7 +35,7 @@ public: /// Default Constructor Event() : - time_(0) { + time_(0), location_(0,0,0) { } /// Constructor from time and location diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 17d54b04d..516e1fb5b 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -23,11 +23,11 @@ namespace gtsam { Similarity3::Similarity3() : - R_(), t_(), s_(1) { + t_(0,0,0), s_(1) { } Similarity3::Similarity3(double s) : - s_(s) { + t_(0,0,0), s_(s) { } Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) : diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 9c7620d70..713ea3054 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -71,7 +71,7 @@ TEST(Similarity3, Constructors) { TEST(Similarity3, Getters) { Similarity3 sim3_default; EXPECT(assert_equal(Rot3(), sim3_default.rotation())); - EXPECT(assert_equal(Point3(), sim3_default.translation())); + EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation())); EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); @@ -158,7 +158,7 @@ TEST( Similarity3, retract_first_order) { Similarity3 id; Vector v = zero(7); v(0) = 0.3; - EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v), 1e-2)); + EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); // v(3) = 0.2; // v(4) = 0.7; // v(5) = -2;