diff --git a/cpp/BetweenFactor.h b/cpp/BetweenFactor.h new file mode 100644 index 000000000..54e2f3d21 --- /dev/null +++ b/cpp/BetweenFactor.h @@ -0,0 +1,91 @@ +/** + * @file BetweenFactor.h + * @authors Frank Dellaert, Viorela Ila + **/ +#pragma once + +#include + +#include "NonlinearFactor.h" +#include "GaussianFactor.h" +#include "Lie.h" +#include "Matrix.h" + +namespace gtsam { + + /** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * T is the Lie group type, Config where the T's are gotten from + */ + template + class BetweenFactor: public NonlinearFactor { + + private: + + T measured_; /** The measurement */ + std::string key1_, key2_; /** The keys of the two poses, order matters */ + std::list keys_; /** The keys as a list */ + Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** Constructor */ + BetweenFactor(const std::string& key1, const std::string& key2, + const T& measured, const Matrix& measurement_covariance) : + key1_(key1), key2_(key2), measured_(measured) { + square_root_inverse_covariance_ = inverse_square_root( + measurement_covariance); + keys_.push_back(key1); + keys_.push_back(key2); + } + + /** implement functions needed for Testable */ + + /** print */ + void print(const std::string& name) const { + std::cout << name << std::endl; + std::cout << "Factor " << std::endl; + std::cout << "key1 " << key1_ << std::endl; + std::cout << "key2 " << key2_ << std::endl; + measured_.print("measured "); + gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance"); + } + + /** equals */ + bool equals(const NonlinearFactor& expected, double tol) const { + return key1_ == expected.key1_ && key2_ == expected.key2_ + && measured_.equals(expected.measured_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector error_vector(const Config& config) const { + //z-h + T p1 = config.get(key1_), p2 = config.get(key2_); + return -logmap(between(p1, p2), measured_); + } + + /** keys as a list */ + inline std::list keys() const { return keys_;} + + /** number of variables attached to this factor */ + inline std::size_t size() const { return 2;} + + /** linearize */ + boost::shared_ptr linearize(const Config& config) const { + T p1 = config.get(key1_), p2 = config.get(key2_); + Vector b = -logmap(between(p1, p2), measured_); + Matrix H1 = Dbetween1(p1, p2); + Matrix H2 = Dbetween2(p1, p2); + boost::shared_ptr linearized(new GaussianFactor(key1_, + square_root_inverse_covariance_ * H1, key2_, + square_root_inverse_covariance_ * H2, b, 1.0)); + return linearized; + } + }; + +} /// namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index e203e80ac..0b6e14ad6 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -193,8 +193,8 @@ testSimulated3D_LDADD = libgtsam.la check_PROGRAMS += testSimulated3D # Pose constraints -headers += Pose2Factor.h Pose2Prior.h -sources += Pose2Config.cpp Pose2Graph.cpp Pose3Factor.cpp +headers += BetweenFactor.h Pose2Factor.h Pose2Prior.h Pose3Factor.h +sources += Pose2Config.cpp Pose2Graph.cpp check_PROGRAMS += testPose2Factor testPose2Graph testPose3Factor testPose2Factor_SOURCES = $(example) testPose2Factor.cpp testPose2Factor_LDADD = libgtsam.la diff --git a/cpp/Pose2Factor.h b/cpp/Pose2Factor.h index 7752c0527..dc5a77398 100644 --- a/cpp/Pose2Factor.h +++ b/cpp/Pose2Factor.h @@ -2,71 +2,16 @@ * @file Pose2Factor.H * @authors Frank Dellaert, Viorela Ila **/ + #pragma once -#include -#include "NonlinearFactor.h" -#include "GaussianFactor.h" -#include "VectorConfig.h" +#include "BetweenFactor.h" #include "Pose2.h" -#include "Matrix.h" #include "Pose2Config.h" -#include "ostream" namespace gtsam { -class Pose2Factor : public NonlinearFactor { -private: - std::string key1_, key2_; /** The keys of the two poses, order matters */ - Pose2 measured_; - Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ - std::list keys_; - -public: - - typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor - - Pose2Factor(const std::string& key1, const std::string& key2, - const Pose2& measured, const Matrix& measurement_covariance) : - key1_(key1),key2_(key2),measured_(measured) { - square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); - keys_.push_back(key1); - keys_.push_back(key2); - } - - /** implement functions needed for Testable */ - void print(const std::string& name) const { - std::cout << name << std::endl; - std::cout << "Factor "<< std::endl; - std::cout << "key1 "<< key1_<& expected, double tol) const {return false;} - - /** implement functions needed to derive from Factor */ - Vector error_vector(const Pose2Config& config) const { - //z-h - Pose2 p1 = config.get(key1_), p2 = config.get(key2_); - return -logmap(between(p1,p2), measured_); - } - - std::list keys() const { return keys_; } - std::size_t size() const { return keys_.size(); } - - /** linearize */ - boost::shared_ptr linearize(const Pose2Config& config) const { - Pose2 p1 = config.get(key1_), p2 = config.get(key2_); - Vector b = -logmap(between(p1,p2), measured_); - Matrix H1 = Dbetween1(p1,p2); - Matrix H2 = Dbetween2(p1,p2); - boost::shared_ptr linearized(new GaussianFactor( - key1_, square_root_inverse_covariance_ * H1, - key2_, square_root_inverse_covariance_ * H2, - b, 1.0)); - return linearized; - } -}; + /** This is just a typedef now */ + typedef BetweenFactor Pose2Factor; } /// namespace gtsam diff --git a/cpp/Pose3Factor.cpp b/cpp/Pose3Factor.cpp deleted file mode 100644 index d0db5436f..000000000 --- a/cpp/Pose3Factor.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/** - * @file UrbanOdometryFactor.cpp - * @brief A non-linear factor specialized to the Visual SLAM problem - * @author Frank Dellaert - * @author Viorela Ila - */ - -#include "Pose3Factor.h" - -using namespace std; -namespace gtsam { - -/* ************************************************************************* * -Pose3Factor::Pose3Factor() { - /// Arbitrary values - robotPoseNumber_ = 1; - robotPoseName_ = symbol('x',robotPoseNumber_); - keys_.push_back(robotPoseName_); -} -/* ************************************************************************* * -Pose3Factor::Pose3Factor(const Point2& z, double sigma, int cn, int ln) -: NonlinearFactor(z.vector(), sigma) - { - robotPoseNumber_ = cn; - landmarkNumber_ = ln; - robotPoseName_ = symbol('x',robotPoseNumber_); - landmarkName_ = symbol('l',landmarkNumber_); - keys_.push_back(robotPoseName_); - keys_.push_back(landmarkName_); - } - -/* ************************************************************************* * -void Pose3Factor::print(const std::string& s) const { - printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str()); - gtsam::print(this->z_, s+".z"); -} - -/* ************************************************************************* * -bool Pose3Factor::equals(const Pose3Factor& p, double tol) const { - if (&p == NULL) return false; - if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false; - if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false; - return true; -} - -/* ************************************************************************* * -// TODO Implement transformPoint2_from -// the difference here that we have a 2d point in a 3D coordinate -Vector Pose3Factor::predict(const Pose3Config& c) const { - Pose3 pose = c.cameraPose(robotPoseNumber_); - Point2 landmark = c.landmarkPoint(landmarkNumber_); - return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector(); -} - -/* ************************************************************************* * -Vector Pose3Factor::error_vector(const Pose3Config& c) const { - // Right-hand-side b = (z - h(x))/sigma - Point2 h = predict(c); - return (this->z_ - h); -} - -/* ************************************************************************* * -GaussianFactor::shared_ptr Pose3Factor::linearize(const Pose3Config& c) const -{ - // get arguments from config - Pose3 pose = c.cameraPose(robotPoseNumber_); - Point3 landmark = c.landmarkPoint(landmarkNumber_); - - SimpleCamera camera(*K_,pose); - - // Jacobians - Matrix Dh1 = Dproject_pose(camera, landmark); - Matrix Dh2 = Dproject_point(camera, landmark); - - // Right-hand-side b = (z - h(x)) - Vector h = project(camera, landmark).vector(); - Vector b = this->z_ - h; - - // Make new linearfactor, divide by sigma - GaussianFactor::shared_ptr - p(new GaussianFactor(robotPoseName_, Dh1, landmarkName_, Dh2, b, this->sigma_)); - return p; -} - -/* ************************************************************************* */ -} // namespace gtsam diff --git a/cpp/Pose3Factor.h b/cpp/Pose3Factor.h index 8e6c454d3..23cdcc6d2 100644 --- a/cpp/Pose3Factor.h +++ b/cpp/Pose3Factor.h @@ -1,88 +1,29 @@ /** - * @file Pose3Factor.h - * @brief A Nonlinear Factor, specialized for Urban application - * @author Frank Dellaert - * @author Viorela Ila - */ + * @file Pose3Factor.H + * @authors Frank Dellaert, Viorela Ila + **/ #pragma once #include -#include "NonlinearFactor.h" -#include "GaussianFactor.h" -#include "VectorConfig.h" +#include "BetweenFactor.h" #include "Pose3.h" -#include "Matrix.h" namespace gtsam { - typedef VectorConfig Config; - - class Pose3Factor: public NonlinearFactor { - private: - std::string key1_, key2_; /** The keys of the two poses, order matters */ - Pose3 measured_; - Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ + class Pose3Config: public std::map { public: - typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor - - Pose3Factor(const std::string& key1, const std::string& key2, - const Pose3& measured, const Matrix& measurement_covariance) : - key1_(key1), key2_(key2), measured_(measured) { - square_root_inverse_covariance_ = inverse_square_root( - measurement_covariance); + const Pose3& get(std::string key) const { + const_iterator it = find(key); + if (it == end()) throw std::invalid_argument("invalid key"); + return it->second; } - /** implement functions needed for Testable */ - void print(const std::string& name) const { - std::cout << name << std::endl; - std::cout << "Factor " << std::endl; - std::cout << "key1 " << key1_ << std::endl; - std::cout << "key2 " << key2_ << std::endl; - measured_.print("measured "); - gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance"); - } - bool equals(const NonlinearFactor& expected, double tol) const { - return false; - } - - /** implement functions needed to derive from Factor */ - Vector error_vector(const Config& config) const { - //z-h - Pose3 p1 = config.get(key1_), p2 = config.get(key2_); - // todo: removed vector() from Pose3, so emulating it here! This is incorrect! - Pose3 betw = between(p1,p2); - Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation()); - Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation()); - return concatVectors(2, &r_diff, &t_diff); - } - - std::list keys() const { - std::list l; - return l; - } - std::size_t size() const { - return 2; - } - - /** linearize */ - boost::shared_ptr linearize(const Config& config) const { - Pose3 p1 = config.get(key1_), p2 = config.get(key2_); - Pose3 betw = between(p1,p2); - // todo: removed vector() from Pose3, so emulating it here! This is incorrect! - Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation()); - Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation()); - Vector b = concatVectors(2, &r_diff, &t_diff); - Matrix H1 = Dbetween1(p1, p2); - Matrix H2 = Dbetween2(p1, p2); - boost::shared_ptr linearized(new GaussianFactor(key1_, - square_root_inverse_covariance_ * H1, key2_, - square_root_inverse_covariance_ * H2, b, 1.0)); - return linearized; - } }; -} /// namespace gtsam + /** This is just a typedef now */ + typedef BetweenFactor Pose3Factor; +} /// namespace gtsam diff --git a/cpp/testPose3Factor.cpp b/cpp/testPose3Factor.cpp index fbd56c060..1d2a3b643 100644 --- a/cpp/testPose3Factor.cpp +++ b/cpp/testPose3Factor.cpp @@ -11,8 +11,12 @@ using namespace std; using namespace gtsam; +/* ************************************************************************* */ TEST( Pose3Factor, constructor ) { + Pose3 measured; + Matrix measurement_covariance; + Pose3Factor("x1", "x2", measured, measurement_covariance); } /* ************************************************************************* */