diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 04632e9e3..4d116c7ec 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -39,7 +39,7 @@ // have been provided with the library for solving robotics SLAM problems. #include #include -#include +#include #include // Standard headers, added last, so we know headers above work on their own diff --git a/gtsam.h b/gtsam.h index 2c3106d97..4bacd0273 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2262,7 +2262,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { }; -#include +#include template virtual class RangeFactor : gtsam::NoiseModelFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h new file mode 100644 index 000000000..19569e563 --- /dev/null +++ b/gtsam/sam/RangeFactor.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * 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 RangeFactor.h + * @brief Serializable factor induced by a range measurement between two points + *and/or poses + * @date July 2015 + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor for a range measurement + * @addtogroup SAM + */ +template +struct RangeFactor : public SerializableExpressionFactor2 { + private: + typedef RangeFactor This; + typedef SerializableExpressionFactor2 Base; + + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2) + + public: + /// default constructor + RangeFactor() {} + + RangeFactor(Key key1, Key key2, double measured, + const SharedNoiseModel& model) + : Base(key1, key2, model, measured) { + this->initialize(expression(key1, key2)); + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + // Return measurement expression + virtual Expression expression(Key key1, Key key2) const { + Expression t1_(key1); + Expression t2_(key2); + return Expression(t1_, &A1::range, t2_); + } + + /// print + void print(const std::string& s = "", + const KeyFormatter& kf = DefaultKeyFormatter) const { + std::cout << s << "RangeFactor" << std::endl; + Base::print(s, kf); + } +}; // \ RangeFactor + +/// traits +template +struct traits > : public Testable > {}; + +/** + * Binary factor for a range measurement, with a transform applied + * @addtogroup SAM + */ +template +class RangeFactorWithTransform + : public SerializableExpressionFactor2 { + private: + typedef RangeFactorWithTransform This; + typedef SerializableExpressionFactor2 Base; + + A1 body_T_sensor_; ///< The pose of the sensor in the body frame + + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2) + + public: + //// Default constructor + RangeFactorWithTransform() {} + + RangeFactorWithTransform(Key key1, Key key2, double measured, + const SharedNoiseModel& model, + const A1& body_T_sensor) + : Base(key1, key2, model, measured), body_T_sensor_(body_T_sensor) { + this->initialize(expression(key1, key2)); + } + + virtual ~RangeFactorWithTransform() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + // Return measurement expression + virtual Expression expression(Key key1, Key key2) const { + Expression body_T_sensor__(body_T_sensor_); + Expression nav_T_body_(key1); + Expression nav_T_sensor_(traits::Compose, nav_T_body_, + body_T_sensor__); + Expression t2_(key2); + return Expression(nav_T_sensor_, &A1::range, t2_); + } + + /** print contents */ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "RangeFactorWithTransform" << std::endl; + this->body_T_sensor_.print(" sensor pose in body frame: "); + Base::print(s, keyFormatter); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); + } +}; // \ RangeFactorWithTransform + +/// traits +template +struct traits > + : public Testable > {}; + +} // \ namespace gtsam diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp similarity index 99% rename from gtsam/slam/tests/testRangeFactor.cpp rename to gtsam/sam/tests/testRangeFactor.cpp index a8455d685..39cdaa2ab 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -16,14 +16,15 @@ * @date Oct 2012 */ -#include -#include +#include #include #include #include #include #include #include + +#include #include using namespace std; diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index d45dd5ce0..d50674d75 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -9,189 +9,14 @@ * -------------------------------------------------------------------------- */ -/** - * @file RangeFactor.H - * @author Frank Dellaert - **/ - #pragma once -#include -#include -#include +#ifdef _MSC_VER +#pragma message("RangeFactor is now an ExpressionFactor in SAM directory") +#else +#warning "RangeFactor is now an ExpressionFactor in SAM directory" +#endif -namespace gtsam { -/** - * Binary factor for a range measurement - * @addtogroup SLAM - */ -template -class RangeFactor: public NoiseModelFactor2 { -private: +#include - double measured_; /** measurement */ - - typedef RangeFactor This; - typedef NoiseModelFactor2 Base; - - // Concept requirements for this factor - GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2) - -public: - - RangeFactor() { - } /* Default constructor */ - - RangeFactor(Key key1, Key key2, double measured, - const SharedNoiseModel& model) : - Base(model, key1, key2), measured_(measured) { - } - - virtual ~RangeFactor() { - } - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - /** h(x)-z */ - Vector evaluateError(const T1& t1, const T2& t2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - double hx; - hx = t1.range(t2, H1, H2); - return (Vector(1) << hx - measured_).finished(); - } - - /** return the measured */ - double measured() const { - return measured_; - } - - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, - double tol = 1e-9) const { - const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) - && fabs(this->measured_ - e->measured_) < tol; - } - - /** print contents */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - std::cout << s << "RangeFactor, range = " << measured_ << std::endl; - Base::print("", keyFormatter); - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar - & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } -}; // \ RangeFactor - -/// traits -template -struct traits > : public Testable > {}; - -/** - * Binary factor for a range measurement, with a transform applied - * @addtogroup SLAM - */ -template -class RangeFactorWithTransform: public NoiseModelFactor2 { -private: - - double measured_; /** measurement */ - POSE body_P_sensor_; ///< The pose of the sensor in the body frame - - typedef RangeFactorWithTransform This; - typedef NoiseModelFactor2 Base; - - // Concept requirements for this factor - GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2) - -public: - - RangeFactorWithTransform() { - } /* Default constructor */ - - RangeFactorWithTransform(Key key1, Key key2, double measured, - const SharedNoiseModel& model, const POSE& body_P_sensor) : - Base(model, key1, key2), measured_(measured), body_P_sensor_( - body_P_sensor) { - } - - virtual ~RangeFactorWithTransform() { - } - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - /** h(x)-z */ - Vector evaluateError(const POSE& t1, const T2& t2, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - double hx; - if (H1) { - Matrix H0; - hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2); - *H1 = *H1 * H0; - } else { - hx = t1.compose(body_P_sensor_).range(t2, H1, H2); - } - return (Vector(1) << hx - measured_).finished(); - } - - /** return the measured */ - double measured() const { - return measured_; - } - - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, - double tol = 1e-9) const { - const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) - && fabs(this->measured_ - e->measured_) < tol - && body_P_sensor_.equals(e->body_P_sensor_); - } - - /** print contents */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - std::cout << s << "RangeFactor, range = " << measured_ << std::endl; - this->body_P_sensor_.print(" sensor pose in body frame: "); - Base::print("", keyFormatter); - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar - & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - } -}; // \ RangeFactorWithTransform - -/// traits -template -struct traits > : public Testable > {}; - -} // \ namespace gtsam diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index a8f85301e..a634928dc 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index f812cd308..a349a4992 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index d97f185e7..51d027b3c 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 6cc8a7b78..4062d0659 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -39,7 +39,7 @@ // have been provided with the library for solving robotics SLAM problems. #include #include -#include +#include #include // Standard headers, added last, so we know headers above work on their own diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index b689179a2..90d92897e 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -39,7 +39,7 @@ // have been provided with the library for solving robotics SLAM problems. #include #include -#include +#include // Standard headers, added last, so we know headers above work on their own #include diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 814f3c5a2..bbfcaa418 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -367,7 +367,7 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; -#include +#include template virtual class RangeFactor : gtsam::NonlinearFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 2e8bf8b4b..194fc6e87 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 3a0081bdb..88dc91ce7 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -532,7 +532,7 @@ TEST(GaussianFactorGraph, hasConstraints) #include #include #include -#include +#include /* ************************************************************************* */ TEST( GaussianFactorGraph, conditional_sigma_failure) { diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index d13b53dd5..08017100f 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -29,7 +29,7 @@ //#include #include #include -#include +#include #include #include #include