diff --git a/.cproject b/.cproject index 9c03c5b7d..59ff12b72 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +600,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +648,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +656,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +664,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +680,7 @@ make + tests/testBayesTree true false @@ -848,6 +854,14 @@ true true + + make + -j4 + testSmartStereoProjectionPoseFactor.run + true + true + true + make -j5 @@ -1122,6 +1136,7 @@ make + testErrors.run true false @@ -1351,6 +1366,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1433,7 +1488,6 @@ make - testSimulated2DOriented.run true false @@ -1473,7 +1527,6 @@ make - testSimulated2D.run true false @@ -1481,7 +1534,6 @@ make - testSimulated3D.run true false @@ -1495,46 +1547,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1792,6 +1804,7 @@ cpack + -G DEB true false @@ -1799,6 +1812,7 @@ cpack + -G RPM true false @@ -1806,6 +1820,7 @@ cpack + -G TGZ true false @@ -1813,6 +1828,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2627,6 +2643,7 @@ make + testGraph.run true false @@ -2634,18 +2651,12 @@ make + testJunctionTree.run true false true - - make - testSymbolicBayesNetB.run - true - false - true - make -j5 @@ -2902,6 +2913,14 @@ true true + + make + -j4 + testRangeFactor.run + true + true + true + make -j2 @@ -3184,7 +3203,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 87bb77913..88c122f6e 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -26,12 +26,11 @@ namespace gtsam { * Binary factor for a range measurement * @addtogroup SLAM */ -template +template class RangeFactor: public NoiseModelFactor2 { private: double measured_; /** measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame typedef RangeFactor This; typedef NoiseModelFactor2 Base; @@ -45,10 +44,8 @@ public: } /* Default constructor */ RangeFactor(Key key1, Key key2, double measured, - const SharedNoiseModel& model, boost::optional body_P_sensor = - boost::none) : - Base(model, key1, key2), measured_(measured), body_P_sensor_( - body_P_sensor) { + const SharedNoiseModel& model) : + Base(model, key1, key2), measured_(measured) { } virtual ~RangeFactor() { @@ -64,16 +61,92 @@ public: Vector evaluateError(const T1& t1, const T2& t2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { double hx; - if (body_P_sensor_) { - 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); - } + 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 + +/** + * 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.range(t2, H1, H2); + hx = t1.compose(body_P_sensor_).range(t2, H1, H2); } return (Vector(1) << hx - measured_).finished(); } @@ -89,17 +162,14 @@ public: const This *e = dynamic_cast(&expected); return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) - || (body_P_sensor_ && e->body_P_sensor_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + && 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; - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); + this->body_P_sensor_.print(" sensor pose in body frame: "); Base::print("", keyFormatter); } diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 6bffa3ce9..a8455d685 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1)); typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; +typedef RangeFactorWithTransform RangeFactorWithTransform2D; +typedef RangeFactorWithTransform RangeFactorWithTransform3D; /* ************************************************************************* */ -Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { +Vector factorError2D(const Pose2& pose, const Point2& point, + const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } /* ************************************************************************* */ -Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { +Vector factorError3D(const Pose3& pose, const Point3& point, + const RangeFactor3D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, + const RangeFactorWithTransform2D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, + const RangeFactorWithTransform3D& factor) { return factor.evaluateError(pose, point); } @@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model, + body_P_sensor_3D); } /* ************************************************************************* */ @@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model, + body_P_sensor_2D); CHECK(assert_equal(factor2D_1, factor2D_2)); - RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D); - RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model, + body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model, + body_P_sensor_3D); CHECK(assert_equal(factor3D_1, factor3D_2)); } @@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -306,6 +344,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */