From ab1f6562c802bf815d42dc327e7d704b1b475d1e Mon Sep 17 00:00:00 2001 From: = Date: Sat, 6 Aug 2016 00:40:04 -0400 Subject: [PATCH 1/3] Fixes compile errors when using BOOST version 1.61.0 --- examples/TimeTBB.cpp | 1 + gtsam/base/GenericValue.h | 1 + gtsam/base/Matrix.cpp | 1 + gtsam/base/Vector.cpp | 1 + gtsam/base/deprecated/LieScalar.h | 1 + gtsam/geometry/BearingRange.h | 1 + gtsam/geometry/Cal3_S2Stereo.h | 1 + gtsam/geometry/Point2.cpp | 1 + gtsam/geometry/Point3.cpp | 1 + gtsam/geometry/Quaternion.h | 1 + gtsam/geometry/Rot2.cpp | 1 + gtsam/geometry/SO3.h | 1 + gtsam/linear/JacobianFactor.cpp | 20 +++++++++---------- gtsam/linear/JacobianFactor.h | 2 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 1 + gtsam/nonlinear/internal/ExecutionTrace.h | 1 + gtsam/nonlinear/tests/testCallRecord.cpp | 1 + gtsam_unstable/geometry/Event.h | 1 + 18 files changed, 26 insertions(+), 12 deletions(-) diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index aa2195078..178fa513f 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -20,6 +20,7 @@ #include #include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 17783c5b9..7c4d0398d 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -28,6 +28,7 @@ #include #include #include // operator typeid +#include namespace gtsam { diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d74e1c122..0c37b405e 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -32,6 +32,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 8f17e4c6e..6dc9800ca 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h index f9c424e95..68632addc 100644 --- a/gtsam/base/deprecated/LieScalar.h +++ b/gtsam/base/deprecated/LieScalar.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 27fe2f197..b1e003864 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 365a6c40e..29ccd194d 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -18,6 +18,7 @@ #pragma once #include +#include namespace gtsam { diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 2152a7c39..74b9a2bec 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -17,6 +17,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 091906d5f..8df5f5607 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -16,6 +16,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 0ab4a5ee6..af9481181 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,6 +21,7 @@ #include #include // Logmap/Expmap derivatives #include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 9780cb49a..4cabe7140 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -17,6 +17,7 @@ */ #include +#include using namespace std; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 3152fa2fb..0396fbce0 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 06c8f3f64..4597759e3 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -219,15 +219,13 @@ FastVector _convertOrCastToJacobians( /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, boost::optional ordering, - boost::optional variableSlots) { + boost::optional p_variableSlots) { gttic(JacobianFactor_combine_constructor); // Compute VariableSlots if one was not provided - boost::optional computedVariableSlots; - if (!variableSlots) { - computedVariableSlots = VariableSlots(graph); - variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots - } + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = + p_variableSlots ? p_variableSlots.get() : VariableSlots(graph); // Cast or convert to Jacobians FastVector jacobians = _convertOrCastToJacobians( @@ -238,15 +236,15 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then // be added after all of the ordered variables. FastVector orderedSlots; - orderedSlots.reserve(variableSlots->size()); + orderedSlots.reserve(variableSlots.size()); if (ordering) { // If an ordering is provided, arrange the slots first that ordering FastList unorderedSlots; size_t nOrderingSlotsUsed = 0; orderedSlots.resize(ordering->size()); FastMap inverseOrdering = ordering->invert(); - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) { + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) { FastMap::const_iterator orderingPosition = inverseOrdering.find(item->first); if (orderingPosition == inverseOrdering.end()) { @@ -267,8 +265,8 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, } else { // If no ordering is provided, arrange the slots as they were, which will be sorted // numerically since VariableSlots uses a map sorting on Key. - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) orderedSlots.push_back(item); } gttoc(Order_slots); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 14ff46241..d7814f541 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -154,7 +154,7 @@ namespace gtsam { explicit JacobianFactor( const GaussianFactorGraph& graph, boost::optional ordering = boost::none, - boost::optional variableSlots = boost::none); + boost::optional p_variableSlots = boost::none); /** Virtual destructor */ virtual ~JacobianFactor() {} diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 904e08770..f1135d2f0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -23,6 +23,7 @@ #include #include #include +#include class NonlinearOptimizerMoreOptimizationTest; diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ed811764a..a147f505e 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -25,6 +25,7 @@ #include #include +#include namespace gtsam { namespace internal { diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 208f0b284..5fc4e208d 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -23,6 +23,7 @@ #include #include +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 40c70696b..5bd37d2d2 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -21,6 +21,7 @@ #include #include +#include namespace gtsam { From 799c5d8bb90d929da369d9b1caaef0081458d8bd Mon Sep 17 00:00:00 2001 From: = Date: Tue, 18 Oct 2016 14:00:12 -0400 Subject: [PATCH 2/3] Replace iostream with iosfwd. --- gtsam/geometry/Quaternion.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index af9481181..4f5495263 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,7 +21,7 @@ #include #include // Logmap/Expmap derivatives #include -#include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> From efd966b45adc9bd2c9a49b436889cb0010b440ef Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Tue, 1 Nov 2016 15:11:57 -0400 Subject: [PATCH 3/3] Move print methods to cpp files wherever possible --- gtsam/base/GenericValue.h | 3 +-- gtsam/geometry/Cal3_S2Stereo.cpp | 39 +++++++++++++++++++++++++++++++ gtsam/geometry/Cal3_S2Stereo.h | 12 +++------- gtsam/geometry/Quaternion.h | 2 +- gtsam/geometry/SO3.cpp | 7 ++++++ gtsam/geometry/SO3.h | 6 ++--- gtsam_unstable/geometry/Event.cpp | 14 +++++++++++ gtsam_unstable/geometry/Event.h | 11 +++------ 8 files changed, 70 insertions(+), 24 deletions(-) create mode 100644 gtsam/geometry/Cal3_S2Stereo.cpp diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 7c4d0398d..5b59f4872 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,9 +26,8 @@ #include #include -#include -#include // operator typeid #include +#include // operator typeid namespace gtsam { diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp new file mode 100644 index 000000000..22966ee37 --- /dev/null +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3_S2Stereo.cpp + * @brief The most common 5DOF 3D->2D calibration + Stereo baseline + * @author Chris Beall + */ + +#include + +#include + +namespace gtsam { +using namespace std; + +/* ************************************************************************* */ +void Cal3_S2Stereo::print(const std::string& s) const { + K_.print(s+"K: "); + std::cout << s << "Baseline: " << b_ << std::endl; + } + +/* ************************************************************************* */ +bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { + if (fabs(b_ - other.b_) > tol) return false; + return K_.equals(other.K_,tol); +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 29ccd194d..db49448ec 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -63,16 +63,10 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s = "") const { - K_.print(s+"K: "); - std::cout << s << "Baseline: " << b_ << std::endl; - } + void print(const std::string& s = "") const; /// Check if equal up to specified tolerance - bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const { - if (fabs(b_ - other.b_) > tol) return false; - return K_.equals(other.K_,tol); - } + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 4f5495263..af9481181 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,7 +21,7 @@ #include #include // Logmap/Expmap derivatives #include -#include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index bd111d9b1..07c03ab49 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -116,6 +117,12 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { return so3::ExpmapFunctor(axis, theta).expmap(); } +/* ************************************************************************* */ +void SO3::print(const std::string& s) const { + std::cout << s << *this << std::endl; + } + +/* ************************************************************************* */ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { so3::DexpFunctor impl(omega); diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 0396fbce0..53f2c2130 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -24,7 +24,7 @@ #include #include -#include +#include namespace gtsam { @@ -68,9 +68,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s) const { - std::cout << s << *this << std::endl; - } + void print(const std::string& s) const; bool equals(const SO3 & R, double tol) const { return equal_with_abs_tol(*this, R, tol); diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index dda2c32e6..c503514a6 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -18,7 +18,21 @@ */ #include +#include namespace gtsam { +/* ************************************************************************* */ +void Event::print(const std::string& s) const { + std::cout << s << "time = " << time_ << "location = " << location_.transpose(); +} + +/* ************************************************************************* */ +bool Event::equals(const Event& other, double tol) const { + return std::abs(time_ - other.time_) < tol + && traits::Equals(location_, other.location_, tol); +} + +/* ************************************************************************* */ + } //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 5bd37d2d2..d9bacd106 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -21,7 +21,7 @@ #include #include -#include +#include namespace gtsam { @@ -60,15 +60,10 @@ public: } /** print with optional string */ - void print(const std::string& s = "") const { - std::cout << s << "time = " << time_ << "location = " << location_.transpose(); - } + void print(const std::string& s = "") const; /** equals with an tolerance */ - bool equals(const Event& other, double tol = 1e-9) const { - return std::abs(time_ - other.time_) < tol - && traits::Equals(location_, other.location_, tol); - } + bool equals(const Event& other, double tol = 1e-9) const; /// Updates a with tangent space delta inline Event retract(const Vector4& v) const {