From 23d4c0fd9f9b9453c929beee94c559668822c6c1 Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 13:27:38 -0800 Subject: [PATCH 1/7] Deprecated several Point3 methods --- examples/SFMExample.cpp | 2 +- examples/SFMExampleExpressions.cpp | 2 +- examples/SelfCalibrationExample.cpp | 2 +- examples/VisualISAM2Example.cpp | 2 +- examples/VisualISAMExample.cpp | 2 +- gtsam/geometry/Point3.h | 4 +++- gtsam/geometry/tests/testPoint3.cpp | 2 +- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam_unstable/dynamics/PoseRTV.cpp | 2 +- gtsam_unstable/dynamics/VelocityConstraint.h | 6 +++--- gtsam_unstable/geometry/Event.h | 4 ++-- 11 files changed, 16 insertions(+), 14 deletions(-) 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 From 4319bece1e7fdcb09d0814d5e83452fcbfd8d37b Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 15:16:11 -0800 Subject: [PATCH 2/7] Deprecated some more methods --- gtsam.h | 39 +++++++++----------------- gtsam/geometry/Point3.cpp | 5 ++-- gtsam/geometry/Point3.h | 20 ++++--------- gtsampy.h | 39 +++++++++----------------- python/handwritten/geometry/Point3.cpp | 7 ++--- 5 files changed, 39 insertions(+), 71 deletions(-) diff --git a/gtsam.h b/gtsam.h index 16f247a34..c42cf7031 100644 --- a/gtsam.h +++ b/gtsam.h @@ -127,13 +127,13 @@ namespace std { void pop_back();*/ }; //typedef std::vector - + #include template class list { - - + + }; } @@ -360,17 +360,6 @@ class Point3 { // Group static gtsam::Point3 identity(); - gtsam::Point3 inverse() const; - gtsam::Point3 compose(const gtsam::Point3& p2) const; - gtsam::Point3 between(const gtsam::Point3& p2) const; - - // Manifold - gtsam::Point3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point3& p) const; - - // Lie Group - static gtsam::Point3 Expmap(Vector v); - static Vector Logmap(const gtsam::Point3& p); // Standard Interface Vector vector() const; @@ -1069,7 +1058,7 @@ class SymbolicBayesTree { void clear(); void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - + gtsam::SymbolicConditional* marginalFactor(size_t key) const; gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; @@ -1079,19 +1068,19 @@ class SymbolicBayesTree { // BayesTreeClique(); // BayesTreeClique(CONDITIONAL* conditional); // // BayesTreeClique(const std::pair& result) : Base(result) {} -// +// // bool equals(const This& other, double tol) const; // void print(string s) const; // void printTree() const; // Default indent of "" // void printTree(string indent) const; // size_t numCachedSeparatorMarginals() const; -// +// // CONDITIONAL* conditional() const; // bool isRoot() const; // size_t treeSize() const; // // const std::list& children() const { return children_; } // // derived_ptr parent() const { return parent_.lock(); } -// +// // // FIXME: need wrapped versions graphs, BayesNet // // BayesNet shortcut(derived_ptr root, Eliminate function) const; // // FactorGraph marginal(derived_ptr root, Eliminate function) const; @@ -1345,7 +1334,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; gtsam::JacobianFactor whiten() const; - + pair eliminate(const gtsam::Ordering& keys) const; void setModel(bool anyConstrained, const Vector& sigmas); @@ -1422,7 +1411,7 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph clone() const; gtsam::GaussianFactorGraph negate() const; - // Optimizing and linear algebra + // Optimizing and linear algebra gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; gtsam::VectorValues optimizeGradientSearch() const; @@ -1511,7 +1500,7 @@ virtual class GaussianBayesNet { //Constructors GaussianBayesNet(); GaussianBayesNet(const gtsam::GaussianConditional* conditional); - + // Testable void print(string s) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const; @@ -1527,7 +1516,7 @@ virtual class GaussianBayesNet { gtsam::GaussianConditional* back() const; void push_back(gtsam::GaussianConditional* conditional); void push_back(const gtsam::GaussianBayesNet& bayesNet); - + gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; gtsam::VectorValues optimizeGradientSearch() const; @@ -1551,7 +1540,7 @@ virtual class GaussianBayesTree { bool empty() const; size_t numCachedSeparatorMarginals() const; void saveGraph(string s) const; - + gtsam::VectorValues optimize() const; gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; @@ -1833,7 +1822,7 @@ class Values { /// Fixed size versions, for n in 1..9 Vector atFixed(size_t j, size_t n); - + /// version for double void insertDouble(size_t j, double c); double atDouble(size_t j) const; @@ -2004,7 +1993,7 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - + void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); void setIterativeParams(gtsam::IterativeOptimizationParameters* params); diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index bffda9fef..29d59d9e3 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.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) @@ -75,6 +75,7 @@ double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = I_3x3; @@ -82,13 +83,13 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, return *this + q; } -/* ************************************************************************* */ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = I_3x3; if (H2) *H2 = I_3x3; return *this - q; } +#endif /* ************************************************************************* */ Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const { diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 20d316915..978d7b963 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -107,11 +107,6 @@ namespace gtsam { double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) const; - /** @deprecated The following function has been deprecated, use distance above */ - inline double dist(const Point3& p2) const { - return (p2 - *this).norm(); - } - /** Distance of the point from the origin, with Jacobian */ double norm(OptionalJacobian<1,3> H = boost::none) const; @@ -145,14 +140,6 @@ namespace gtsam { /// get z inline double z() const {return z_;} - /** add two points, add(this,q) is same as this + q */ - Point3 add (const Point3 &q, - OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const; - - /** subtract two points, sub(this,q) is same as this - q */ - Point3 sub (const Point3 &q, - OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const; - /// @} /// Output stream operator @@ -168,7 +155,12 @@ namespace gtsam { Point3 retract(const Vector3& v) const { return compose(Point3(v));} static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} - /// @} + inline double dist(const Point3& p2) const { return (p2 - *this).norm(); } + Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + /// @} #endif private: diff --git a/gtsampy.h b/gtsampy.h index ca014ad5d..9cadf6be3 100644 --- a/gtsampy.h +++ b/gtsampy.h @@ -127,13 +127,13 @@ namespace std { void pop_back();*/ }; //typedef std::vector - + #include template class list { - - + + }; } @@ -360,17 +360,6 @@ class Point3 { // Group static gtsam::Point3 identity(); - gtsam::Point3 inverse() const; - gtsam::Point3 compose(const gtsam::Point3& p2) const; - gtsam::Point3 between(const gtsam::Point3& p2) const; - - // Manifold - gtsam::Point3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point3& p) const; - - // Lie Group - static gtsam::Point3 Expmap(Vector v); - static Vector Logmap(const gtsam::Point3& p); // Standard Interface Vector vector() const; @@ -1069,7 +1058,7 @@ class SymbolicBayesTree { void clear(); void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - + gtsam::SymbolicConditional* marginalFactor(size_t key) const; gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; @@ -1079,19 +1068,19 @@ class SymbolicBayesTree { // BayesTreeClique(); // BayesTreeClique(CONDITIONAL* conditional); // // BayesTreeClique(const std::pair& result) : Base(result) {} -// +// // bool equals(const This& other, double tol) const; // void print(string s) const; // void printTree() const; // Default indent of "" // void printTree(string indent) const; // size_t numCachedSeparatorMarginals() const; -// +// // CONDITIONAL* conditional() const; // bool isRoot() const; // size_t treeSize() const; // // const std::list& children() const { return children_; } // // derived_ptr parent() const { return parent_.lock(); } -// +// // // FIXME: need wrapped versions graphs, BayesNet // // BayesNet shortcut(derived_ptr root, Eliminate function) const; // // FactorGraph marginal(derived_ptr root, Eliminate function) const; @@ -1345,7 +1334,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; gtsam::JacobianFactor whiten() const; - + pair eliminate(const gtsam::Ordering& keys) const; void setModel(bool anyConstrained, const Vector& sigmas); @@ -1422,7 +1411,7 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph clone() const; gtsam::GaussianFactorGraph negate() const; - // Optimizing and linear algebra + // Optimizing and linear algebra gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; gtsam::VectorValues optimizeGradientSearch() const; @@ -1511,7 +1500,7 @@ virtual class GaussianBayesNet { //Constructors GaussianBayesNet(); GaussianBayesNet(const gtsam::GaussianConditional* conditional); - + // Testable void print(string s) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const; @@ -1527,7 +1516,7 @@ virtual class GaussianBayesNet { gtsam::GaussianConditional* back() const; void push_back(gtsam::GaussianConditional* conditional); void push_back(const gtsam::GaussianBayesNet& bayesNet); - + gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; gtsam::VectorValues optimizeGradientSearch() const; @@ -1551,7 +1540,7 @@ virtual class GaussianBayesTree { bool empty() const; size_t numCachedSeparatorMarginals() const; void saveGraph(string s) const; - + gtsam::VectorValues optimize() const; gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; @@ -1833,7 +1822,7 @@ class Values { /// Fixed size versions, for n in 1..9 Vector atFixed(size_t j, size_t n); - + /// version for double void insertDouble(size_t j, double c); double atDouble(size_t j) const; @@ -2004,7 +1993,7 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - + void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); void setIterativeParams(gtsam::IterativeOptimizationParameters* params); diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 664b4ffda..5a3ba8fb5 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.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,16 +36,13 @@ class_("Point3") .def(init()) .def("identity", &Point3::identity) .staticmethod("identity") - .def("add", &Point3::add) .def("cross", &Point3::cross) - .def("dist", &Point3::dist) .def("distance", &Point3::distance) .def("dot", &Point3::dot) .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) .def("norm", &Point3::norm) .def("normalize", &Point3::normalize) .def("print", &Point3::print, print_overloads(args("s"))) - .def("sub", &Point3::sub) .def("vector", &Point3::vector) .def("x", &Point3::x) .def("y", &Point3::y) @@ -61,4 +58,4 @@ class_("Point3") .def(self == self) ; -} \ No newline at end of file +} From a19aa793d7b8f9a80c853da668c5cca5e0dcf92a Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:30:44 -0800 Subject: [PATCH 3/7] Cleaned up a number of Point3/Rot3 related uses --- gtsam/geometry/tests/testCalibratedCamera.cpp | 12 +++----- gtsam/geometry/tests/testPinholeCamera.cpp | 4 +-- gtsam/geometry/tests/testPinholePose.cpp | 4 +-- gtsam/geometry/tests/testPoint3.cpp | 12 ++++---- gtsam/geometry/tests/testRot3.cpp | 29 +++++++++---------- gtsam/geometry/tests/testSimpleCamera.cpp | 12 +++----- gtsam/geometry/tests/testStereoCamera.cpp | 6 ++-- gtsam/navigation/GPSFactor.cpp | 4 +-- gtsam/navigation/ScenarioRunner.h | 2 +- gtsam/navigation/tests/testNavState.cpp | 2 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 8 ++--- gtsam/slam/tests/testStereoFactor.cpp | 6 +--- gtsam_unstable/dynamics/VelocityConstraint.h | 2 +- tests/simulated3D.h | 6 ++-- 14 files changed, 48 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5bc645a58..cc7a0c0f8 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.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) @@ -30,13 +30,9 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) // Camera situated at 0.5 meters high, looking down -static const Pose3 pose1((Matrix3() << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); - +static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), + Point3(0, 0, 0.5)); + static const CalibratedCamera camera(pose1); static const Point3 point1(-0.08,-0.08, 0.0); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index b2579c0d9..ce9932879 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.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) @@ -34,7 +34,7 @@ typedef PinholeCamera Camera; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static const Camera camera(pose, K); static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 8f3eadc51..3c79a45e3 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.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) @@ -35,7 +35,7 @@ typedef PinholePose Camera; static const Cal3_S2::shared_ptr K = boost::make_shared(625, 625, 0, 0, 0); -static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static const Camera camera(pose, K); static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a695b95dc..d0123af83 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -61,12 +61,12 @@ TEST(Point3, Lie) { /* ************************************************************************* */ TEST( Point3, arithmetic) { CHECK(P * 3 == 3 * P); - CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); - CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); - CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); - CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); - CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); - CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); + CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); + CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); + CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); + CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); + CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); + CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 4b3c84e01..ce5d3268c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.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) @@ -97,10 +97,10 @@ TEST( Rot3, equals) // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... Rot3 slow_but_correct_Rodrigues(const Vector& w) { double t = norm_2(w); - Matrix J = skewSymmetric(w / t); + Matrix3 J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); - Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); - return R; + Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); + return Rot3(R); } /* ************************************************************************* */ @@ -201,7 +201,7 @@ TEST(Rot3, log) // Windows and Linux have flipped sign in quaternion mode #if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) w = (Vector(3) << x*PI, y*PI, z*PI).finished(); - R = Rot3::Rodrigues(w); + R = Rot3::Rodrigues(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); #else CHECK_OMEGA(x*PI,y*PI,z*PI) @@ -274,14 +274,13 @@ TEST(Rot3, manifold_expmap) } /* ************************************************************************* */ -class AngularVelocity: public Point3 { -public: - AngularVelocity(const Point3& p) : - Point3(p) { - } - AngularVelocity(double wx, double wy, double wz) : - Point3(wx, wy, wz) { - } +class AngularVelocity : public Vector3 { + public: + template + inline AngularVelocity(const Eigen::MatrixBase& v) + : Vector3(v) {} + + AngularVelocity(double wx, double wy, double wz) : Vector3(wx, wy, wz) {} }; AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { @@ -294,10 +293,10 @@ TEST(Rot3, BCH) // Approximate exmap by BCH formula AngularVelocity w1(0.2, -0.1, 0.1); AngularVelocity w2(0.01, 0.02, -0.03); - Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); + Rot3 R1 = Rot3::Expmap (w1), R2 = Rot3::Expmap (w2); Rot3 R3 = R1 * R2; Vector expected = Rot3::Logmap(R3); - Vector actual = BCH(w1, w2).vector(); + Vector actual = BCH(w1, w2); CHECK(assert_equal(expected, actual,1e-5)); } diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 515542298..8db8113a1 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.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) @@ -28,13 +28,9 @@ using namespace gtsam; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix3() << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); - +static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), + Point3(0, 0, 0.5)); + static const SimpleCamera camera(pose1, K); static const Point3 point1(-0.08,-0.08, 0.0); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 3adf2257d..6508965ec 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.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) @@ -74,11 +74,11 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -static Pose3 camPose((Matrix)(Matrix(3,3) << +static Pose3 camPose(Rot3((Matrix3() << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ).finished(), + ).finished()), Point3(0,0,6.25)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 87913cda6..2f4cb9bc3 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.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) @@ -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() -nT_).vector(); + return p.translation().vector() -nT_.vector(); } //*************************************************************************** diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index b038a831b..537785e06 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -73,7 +73,7 @@ class ScenarioRunner { // An accelerometer measures acceleration in body, but not gravity Vector3 actualSpecificForce(double t) const { - Rot3 bRn = scenario_->rotation(t).transpose(); + Rot3 bRn(scenario_->rotation(t).transpose()); return scenario_->acceleration_b(t) - bRn * gravity_n(); } diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index a62ca06a8..2fbb9fee3 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -81,7 +81,7 @@ TEST( NavState, Velocity) { TEST( NavState, BodyVelocity) { Matrix39 aH, eH; Velocity3 actual = kState1.bodyVelocity(aH); - EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); + EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); eH = numericalDerivative11( boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index a634928dc..07abb557f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -105,16 +105,16 @@ static vector genPoint3() { static vector genCameraDefaultCalibration() { vector X; - X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)))); - X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)))); return X; } static vector genCameraVariableCalibration() { const Cal3_S2 K(640, 480, 0.1, 320, 240); vector X; - X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K)); - X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K)); return X; } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index b6d2e2ef8..45b978c27 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -31,11 +31,7 @@ using namespace std; using namespace gtsam; -static Pose3 camera1((Matrix) (Matrix(3, 3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), +static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0,0,6.25)); static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 4d1518f4b..4ce4d5758 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -115,7 +115,7 @@ private: case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; default: assert(false); break; } - return (p2 - hx).vector(); + return p2.vector() - hx.vector(); } }; diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 46945558a..cf69a8fa3 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.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) @@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1 { */ Vector evaluateError(const Point3& x, boost::optional H = boost::none) const { - return (prior(x, H) - measured_).vector(); + return prior(x, H).vector() - measured_.vector(); } }; @@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { */ Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - measured_).vector(); + return mea(x1, x2, H1, H2).vector() - measured_.vector(); } }; From a86a3eee3e7b427910f6ca7b24a3a7206701358d Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:31:57 -0800 Subject: [PATCH 4/7] Fixed BCH compile issue --- gtsam/base/Lie.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 1576aca1d..23ac0f4df 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.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) @@ -295,19 +295,18 @@ private: }; /** - * Three term approximation of the Baker�Campbell�Hausdorff formula + * Three term approximation of the Baker-Campbell-Hausdorff formula * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 - * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula + * http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula */ /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? template T BCH(const T& X, const T& Y) { static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; T X_Y = bracket(X, Y); - return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, - bracket(X, X_Y)); + return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y))); } /** From 3052afe42b9f916916beda3826942852d647f48b Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:32:25 -0800 Subject: [PATCH 5/7] Explicit template of insert --- examples/SFMExample.cpp | 2 +- examples/SFMExampleExpressions.cpp | 2 +- examples/SelfCalibrationExample.cpp | 2 +- examples/VisualISAM2Example.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index eaabb3ea9..893454936 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] + 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 8a59100da..30db9144d 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] + 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 066b16e8a..1c5d155dc 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] + 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 c0b4b5b0a..78bc463ec 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] +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 From 6eeeb3fef1bd389af8c8913ae4bd6e7e9a005c03 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 8 Feb 2016 18:58:43 -0800 Subject: [PATCH 6/7] Fixed forgotten compose --- gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index 39481f929..001d75eb2 100644 --- a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -75,7 +75,7 @@ int main() { Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + Point3 t2 = t1 + Point3(Vel1*measurement_dt); Pose3 Pose2(R2, t2); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); Vector3 Vel2 = Vel1 + dv; From 50e8fa9b5468ba1cf792b0b5a1063fa1be73314f Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 8 Feb 2016 18:58:57 -0800 Subject: [PATCH 7/7] Fix timing script --- timing/timeSFMBAL.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index e9449af7b..cd859326c 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -49,8 +49,12 @@ SfM_data preamble(int argc, char* argv[]) { // Load BAL file SfM_data db; - string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre"); - bool success = readBAL(argc > 1 ? argv[argc - 1] : defaultFilename, db); + string filename; + if (argc > 1) + filename = argv[argc - 1]; + else + filename = findExampleDataFile("dubrovnik-16-22106-pre"); + bool success = readBAL(argv[argc - 1], db); if (!success) throw runtime_error("Could not access file!"); return db; }