From 0a44315a7f3b1b2f6e1953e5b73ece40c7097a01 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 20 Jun 2020 08:46:06 +1000 Subject: [PATCH 1/3] Add Pose3-Point3 factor --- gtsam_unstable/slam/PoseToPointFactor.h | 95 +++++++++++++++++++ .../slam/tests/testPoseToPointFactor.h | 84 ++++++++++++++++ 2 files changed, 179 insertions(+) create mode 100644 gtsam_unstable/slam/PoseToPointFactor.h create mode 100644 gtsam_unstable/slam/tests/testPoseToPointFactor.h diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h new file mode 100644 index 000000000..98efd6a0b --- /dev/null +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -0,0 +1,95 @@ +/** + * @file PoseToPointFactor.hpp + * @brief This factor can be used to track a 3D landmark over time by providing + * local measurements of its location. + * @author David Wisth + **/ +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * A class for a measurement between a pose and a point. + * @addtogroup SLAM + */ +class PoseToPointFactor: public NoiseModelFactor2 { + +private: + + typedef PoseToPointFactor This; + typedef NoiseModelFactor2 Base; + + Point3 measured_; /** the point measurement */ + +public: + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + PoseToPointFactor() {} + + /** Constructor */ + PoseToPointFactor(Key key1, Key key2, const Point3& measured, + const SharedNoiseModel& model) : + Base(model, key1, key2), measured_(measured) + { + } + + virtual ~PoseToPointFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "PoseToPointFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n" + << " measured: " << measured_.transpose() << std::endl; + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors + * Error = pose_est.inverse()*point_est - measured_ + * (The error is in the measurement coordinate system) + */ + Vector evaluateError(const Pose3& W_T_WI, + const Point3& W_r_WC, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + return W_T_WI.transformTo(W_r_WC, H1, H2) - measured_; + } + + /** return the measured */ + const Point3& measured() const { + return measured_; + } + +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_); + } + +}; // \class PoseToPointFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h new file mode 100644 index 000000000..0a13e78e2 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -0,0 +1,84 @@ +/** + * @file testPoseToPointFactor.cpp + * @brief + * @author David Wisth + * @date June 20, 2020 + */ + +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::noiseModel; + +/// Verify zero error when there is no noise +TEST(LandmarkFactor, errorNoiseless) { + Pose3 pose = Pose3::identity(); + Point3 point(1.0, 2.0, 3.0); + Point3 noise(0.0, 0.0, 0.0); + Point3 measured = t + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = Vector3(0.0, 0.0, 0.0); + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +/// Verify expected error in test scenario +TEST(LandmarkFactor, errorNoise) { + Pose3 pose = Pose3::identity(); + Point3 point(1.0 , 2.0, 3.0); + Point3 noise(-1.0, 0.5, 0.3); + Point3 measured = t + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = noise; + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +/// Check Jacobians are correct +TEST(LandmarkFactor, jacobian) { + // Measurement + gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); + + // Linearisation point + gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); + gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5*M_PI, -0.3*M_PI, 0.4*M_PI); + Pose3 p(p_R, p_t); + + gtsam::Point3 l = gtsam::Point3(3, 0, 5); + + // Factor + Key pose_key(1); + Key point_key(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + PoseToPointFactor factor(pose_key, point_key, l_meas, noise); + + // Calculate numerical derivatives + boost::function f = boost::bind( + &LandmarkFactor::evaluateError, factor, _1, _2, boost::none, boost::none); + Matrix numerical_H1 = numericalDerivative21(f, p, l); + Matrix numerical_H2 = numericalDerivative22(f, p, l); + + // Use the factor to calculate the derivative + Matrix actual_H1; + Matrix actual_H2; + factor.evaluateError(p, l, actual_H1, actual_H2); + + // Verify we get the expected error + EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8)); + EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From c422815b9402a0a0840c2902b8df647e6d527ca5 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 20 Jun 2020 09:03:17 +1000 Subject: [PATCH 2/3] Update incorrect test name --- gtsam_unstable/slam/tests/testPoseToPointFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h index 0a13e78e2..4b2793700 100644 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -13,7 +13,7 @@ using namespace gtsam; using namespace gtsam::noiseModel; /// Verify zero error when there is no noise -TEST(LandmarkFactor, errorNoiseless) { +TEST(PoseToPointFactor, errorNoiseless) { Pose3 pose = Pose3::identity(); Point3 point(1.0, 2.0, 3.0); Point3 noise(0.0, 0.0, 0.0); @@ -28,7 +28,7 @@ TEST(LandmarkFactor, errorNoiseless) { } /// Verify expected error in test scenario -TEST(LandmarkFactor, errorNoise) { +TEST(PoseToPointFactor, errorNoise) { Pose3 pose = Pose3::identity(); Point3 point(1.0 , 2.0, 3.0); Point3 noise(-1.0, 0.5, 0.3); @@ -43,7 +43,7 @@ TEST(LandmarkFactor, errorNoise) { } /// Check Jacobians are correct -TEST(LandmarkFactor, jacobian) { +TEST(PoseToPointFactor, jacobian) { // Measurement gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); From 0ee4e3b77eaaa879d5e995d92714698e94d11455 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 20 Jun 2020 09:45:24 +1000 Subject: [PATCH 3/3] Add more documentation and clang-format --- gtsam_unstable/slam/PoseToPointFactor.h | 81 +++++++++---------- .../slam/tests/testPoseToPointFactor.h | 22 ++--- 2 files changed, 50 insertions(+), 53 deletions(-) diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index 98efd6a0b..ec7da22ef 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -1,15 +1,15 @@ /** * @file PoseToPointFactor.hpp - * @brief This factor can be used to track a 3D landmark over time by providing - * local measurements of its location. + * @brief This factor can be used to track a 3D landmark over time by + *providing local measurements of its location. * @author David Wisth **/ #pragma once -#include -#include #include #include +#include +#include namespace gtsam { @@ -17,17 +17,14 @@ namespace gtsam { * A class for a measurement between a pose and a point. * @addtogroup SLAM */ -class PoseToPointFactor: public NoiseModelFactor2 { - -private: - +class PoseToPointFactor : public NoiseModelFactor2 { + private: typedef PoseToPointFactor This; typedef NoiseModelFactor2 Base; - Point3 measured_; /** the point measurement */ - -public: + Point3 measured_; /** the point measurement in local coordinates */ + public: // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -36,60 +33,58 @@ public: /** Constructor */ PoseToPointFactor(Key key1, Key key2, const Point3& measured, - const SharedNoiseModel& model) : - Base(model, key1, key2), measured_(measured) - { - } + const SharedNoiseModel& model) + : Base(model, key1, key2), measured_(measured) {} virtual ~PoseToPointFactor() {} /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "PoseToPointFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n" - << " measured: " << measured_.transpose() << std::endl; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n" + << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors - * Error = pose_est.inverse()*point_est - measured_ - * (The error is in the measurement coordinate system) - */ - Vector evaluateError(const Pose3& W_T_WI, - const Point3& W_r_WC, + * @brief Error = wTwi.inverse()*wPwp - measured_ + * @param wTwi The pose of the sensor in world coordinates + * @param wPwp The estimated point location in world coordinates + * + * Note: measured_ and the error are in local coordiantes. + */ + Vector evaluateError(const Pose3& wTwi, const Point3& wPwp, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const - { - return W_T_WI.transformTo(W_r_WC, H1, H2) - measured_; + boost::optional H2 = boost::none) const { + return wTwi.transformTo(wPwp, H1, H2) - measured_; } /** return the measured */ - const Point3& measured() const { - return measured_; - } - -private: + const Point3& measured() const { return measured_; } + 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_); + 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_); } -}; // \class PoseToPointFactor +}; // \class PoseToPointFactor -} /// namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h index 4b2793700..8f8563e9d 100644 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -5,9 +5,9 @@ * @date June 20, 2020 */ +#include #include #include -#include using namespace gtsam; using namespace gtsam::noiseModel; @@ -21,25 +21,27 @@ TEST(PoseToPointFactor, errorNoiseless) { Key pose_key(1); Key point_key(2); - PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(3, 0.05)); Vector expectedError = Vector3(0.0, 0.0, 0.0); Vector actualError = factor.evaluateError(pose, point); - EXPECT(assert_equal(expectedError,actualError, 1E-5)); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); } /// Verify expected error in test scenario TEST(PoseToPointFactor, errorNoise) { Pose3 pose = Pose3::identity(); - Point3 point(1.0 , 2.0, 3.0); + Point3 point(1.0, 2.0, 3.0); Point3 noise(-1.0, 0.5, 0.3); Point3 measured = t + noise; Key pose_key(1); Key point_key(2); - PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(3, 0.05)); Vector expectedError = noise; Vector actualError = factor.evaluateError(pose, point); - EXPECT(assert_equal(expectedError,actualError, 1E-5)); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); } /// Check Jacobians are correct @@ -48,8 +50,8 @@ TEST(PoseToPointFactor, jacobian) { gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); // Linearisation point - gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); - gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5*M_PI, -0.3*M_PI, 0.4*M_PI); + gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); + gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI); Pose3 p(p_R, p_t); gtsam::Point3 l = gtsam::Point3(3, 0, 5); @@ -61,8 +63,8 @@ TEST(PoseToPointFactor, jacobian) { PoseToPointFactor factor(pose_key, point_key, l_meas, noise); // Calculate numerical derivatives - boost::function f = boost::bind( - &LandmarkFactor::evaluateError, factor, _1, _2, boost::none, boost::none); + auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, + boost::none, boost::none); Matrix numerical_H1 = numericalDerivative21(f, p, l); Matrix numerical_H2 = numericalDerivative22(f, p, l);