diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 8da9847b8..eaabb3ea9 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -99,7 +99,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); initialEstimate.print("Initial Estimates:\n"); /* Optimize the graph and print results */ diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index df5488ec3..8a59100da 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -88,7 +88,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) - initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); cout << "initial error = " << graph.error(initial) << endl; /* Optimize the graph and print results */ diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index f5702e7fb..066b16e8a 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -84,7 +84,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); /* Optimize the graph and print results */ Values result = DoglegOptimizer(graph, initialEstimate).optimize(); diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 75c0a2fa5..c0b4b5b0a 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -113,7 +113,7 @@ int main(int argc, char* argv[]) { // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.insert(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15)); } else { // Update iSAM with the new factors diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 9380410ea..a839289c2 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -120,7 +120,7 @@ int main(int argc, char* argv[]) { Point3 noise(-0.25, 0.20, 0.15); for (size_t j = 0; j < points.size(); ++j) { // Intentionally initialize the variables off from the ground truth - Point3 initial_lj = points[j].compose(noise); + Point3 initial_lj = points[j] + noise; initialEstimate.insert(Symbol('l', j), initial_lj); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index e19b74d1c..20d316915 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.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) @@ -158,6 +158,7 @@ namespace gtsam { /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ Point3 inverse() const { return -(*this);} @@ -168,6 +169,7 @@ namespace gtsam { static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} /// @} +#endif private: diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 246d23c9a..a695b95dc 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.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) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 16117845e..d210be789 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Camera().localCoordinates(camera); -Vector3 X = Point3::Logmap(point); +Vector3 X = point.vector(); Vector2 expectedMeasurement(1.2431567, 1.2525694); Matrix E1 = numericalDerivative21(adapted, P, X); Matrix E2 = numericalDerivative22(adapted, P, X); diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 1a7fdf3de..b3953dd23 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -161,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const { // predict point for constraint // NOTE: uses simple Euler approach for prediction - Point3 pred_t2 = t().retract(v2 * dt); + Point3 pred_t2 = t() + Point3(v2 * dt); return pred_t2; } diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index c41ea220c..4d1518f4b 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -110,9 +110,9 @@ private: const Point3& p1 = x1.t(), p2 = x2.t(); Point3 hx; switch(mode) { - case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break; - case dynamics::EULER_START: hx = p1.retract(v1 * dt); break; - case dynamics::EULER_END : hx = p1.retract(v2 * dt); break; + case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break; + case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break; + case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; default: assert(false); break; } return (p2 - hx).vector(); diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index dd362c7f4..5bd453424 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.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) @@ -72,7 +72,7 @@ public: /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { - return Event(time_ + v[0], location_.retract(v.tail(3))); + return Event(time_ + v[0], location_ + Point3(v.tail<3>())); } /// Returns inverse retraction