diff --git a/cpp/BetweenFactor.h b/cpp/BetweenFactor.h index 3b6d22109..8e25174c4 100644 --- a/cpp/BetweenFactor.h +++ b/cpp/BetweenFactor.h @@ -45,16 +45,17 @@ namespace gtsam { /** print */ void print(const std::string& s) const { Base::print(s); - measured_.print("measured "); - gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance"); + measured_.print("measured"); + gtsam::print(square_root_inverse_covariance_, + "Square Root Inverse Covariance"); } /** equals */ bool equals(const NonlinearFactor& expected, double tol) const { const BetweenFactor *e = dynamic_cast*> (&expected); - return e != NULL && Base::equals(expected) - && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(expected) && this->measured_.equals( + e->measured_, tol); } /** implement functions needed to derive from Factor */ @@ -62,24 +63,24 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - // h - z - T hx = between(p1, p2); - // TODO should be done by noise model + T hx = between(p1, p2, H1, H2); // h(x) if (H1 || H2) { - between(p1,p2,H1,H2); if (H1) *H1 = square_root_inverse_covariance_ * *H1; if (H2) *H2 = square_root_inverse_covariance_ * *H2; } // manifold equivalent of h(x)-z -> log(z,h(x)) - // TODO use noise model, error vector is not whitened yet return square_root_inverse_covariance_ * logmap(measured_, hx); } /** return the measured */ - inline const T measured() const {return measured_;} + inline const T measured() const { + return measured_; + } /** number of variables attached to this factor */ - inline std::size_t size() const { return 2;} + inline std::size_t size() const { + return 2; + } }; } /// namespace gtsam diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index 1cb4c144d..96a29c364 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -61,10 +61,8 @@ namespace gtsam { dT1(1,0), dT1(1,1), dR1(1,0), 0.0, 0.0, -1.0); } - if (H2) *H2 = Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); + static const Matrix I3 = eye(3); + if (H2) *H2 = I3; return dp; } diff --git a/cpp/Pose2Prior.h b/cpp/Pose2Prior.h index b777fcd83..293e4f6cf 100644 --- a/cpp/Pose2Prior.h +++ b/cpp/Pose2Prior.h @@ -15,55 +15,60 @@ namespace gtsam { -class Pose2Prior : public NonlinearFactor { -private: - typedef Pose2Config::Key Key; - Key key_; - Pose2 measured_; - Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ - //std::list keys_; + /** + * A class for a soft prior on any Lie type + * T is the Lie group type, Config where the T's are gotten from + */ + template + class PriorFactor: public NonlinearFactor1 { -public: + private: - typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor + typedef NonlinearFactor1 Base; - Pose2Prior(const Key& key, const Pose2& measured, const Matrix& measurement_covariance) : - key_(key),measured_(measured) { - square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); - keys_.push_back(key); - } + T prior_; /** The measurement */ + Matrix square_root_inverse_covariance_; /** sqrt(inv(covariance)) */ - /** implement functions needed for Testable */ - void print(const std::string& name) const { - std::cout << name << std::endl; - std::cout << "Factor "<< std::endl; - std::cout << "key "<< (std::string)key_<& expected, double tol) const {return false;} + public: - /** implement functions needed to derive from Factor */ - Vector error_vector(const Pose2Config& config) const { - Pose2 p = config[key_]; - return square_root_inverse_covariance_ * logmap(measured_,p); - } + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; - //std::list keys() const { return keys_; } - std::size_t size() const { return keys_.size(); } + /** Constructor */ + PriorFactor(const Key& key, const T& prior, const Matrix& covariance) : + Base(1.0, key), prior_(prior) { + square_root_inverse_covariance_ = inverse_square_root(covariance); + } + + /** implement functions needed for Testable */ + + /** print */ + void print(const std::string& s) const { + Base::print(s); + prior_.print("prior"); + gtsam::print(square_root_inverse_covariance_, + "Square Root Inverse Covariance"); + } + + /** equals */ + bool equals(const NonlinearFactor& expected, double tol) const { + const PriorFactor *e = dynamic_cast*> (&expected); + return e != NULL && Base::equals(expected) && this->prior_.equals( + e->prior_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const T& p, boost::optional H = boost::none) const { + if (H) (*H) = square_root_inverse_covariance_; + // manifold equivalent of h(x)-z -> log(z,h(x)) + return square_root_inverse_covariance_ * logmap(prior_, p); + } + }; + + /** This is just a typedef now */ + typedef PriorFactor Pose2Prior; - /** linearize */ - boost::shared_ptr linearize(const Pose2Config& config) const { - Pose2 p = config[key_]; - Vector b = - error_vector(config); - Matrix H(3,3); - H(0,0)=1; H(0,1)=0; H(0,2)=0; - H(1,0)=0; H(1,1)=1; H(1,2)=0; - H(2,0)=0; H(2,1)=0; H(2,2)=1; - boost::shared_ptr linearized(new GaussianFactor( - key_, square_root_inverse_covariance_ * H, - b, 1.0)); - return linearized; - } -}; } /// namespace gtsam diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 9e1377e74..69e0e9670 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -133,31 +133,11 @@ namespace gtsam { return pose.rotation().transpose(); } - /* ************************************************************************* */ - // direct measurement of the deviation of a pose from the origin - // used as soft prior - /* ************************************************************************* */ - Vector hPose (const Vector& x) { - Pose3 pose(x); // transform from vector to Pose3 - Vector w = pose.rotation().ypr(); // get angle differences - Vector d = pose.translation().vector(); // get translation differences - return concatVectors(2,&w,&d); - } - - /* ************************************************************************* */ - // derivative of direct measurement - // 6*6, entry i,j is how measurement error will change - /* ************************************************************************* */ - Matrix DhPose(const Vector& x) { - Matrix H = eye(6,6); - return H; - } - /* ************************************************************************* */ // compose = Pose3(compose(R1,R2),transform_from(p1,t2); Matrix Dcompose1(const Pose3& p1, const Pose3& p2) { - Matrix DR_R1 = eye(3); + static const Matrix DR_R1 = eye(3); Matrix DR_t1 = zeros(3, 3); Matrix DR = collect(2, &DR_R1, &DR_t1); Matrix Dt = Dtransform_from1(p1, p2.translation()); diff --git a/cpp/Pose3.h b/cpp/Pose3.h index 7b3a06c6e..ae5d31c4f 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -142,13 +142,4 @@ namespace gtsam { Pose3 between(const Pose3& p1, const Pose3& p2, boost::optional H1, boost::optional H2); - /** direct measurement of a pose */ - Vector hPose(const Vector& x); - - /** - * derivative of direct measurement - * 12*6, entry i,j is how measurement error will change - */ - Matrix DhPose(const Vector& x); - } // namespace gtsam diff --git a/cpp/testPose2Prior.cpp b/cpp/testPose2Prior.cpp index a4dffc5dc..c2c4c2eae 100644 --- a/cpp/testPose2Prior.cpp +++ b/cpp/testPose2Prior.cpp @@ -54,14 +54,14 @@ TEST( Pose2Prior, error ) static Pose2 prior(2,2,M_PI_2); static Pose2Prior factor(1,prior, covariance); -/* ************************************************************************* * +/* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // Hence i.e., b = approximates z-h(x0) = error_vector(x0) Vector h(const Pose2& p1) { return factor.evaluateError(p1); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( Pose2Prior, linearize ) { // Choose a linearization point at ground truth @@ -70,11 +70,10 @@ TEST( Pose2Prior, linearize ) // Actual linearization boost::shared_ptr actual = factor.linearize(x0); - CHECK(assert_equal(expected,*actual)); - // Numerical do not work out because BetweenFactor is approximate ? + // Test with numerical derivative Matrix numericalH = numericalDerivative11(h, prior, 1e-5); - CHECK(assert_equal(expectedH,numericalH)); + CHECK(assert_equal(numericalH,actual->get_A("x1"))); } /* ************************************************************************* */