diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h new file mode 100644 index 000000000..db6f92f8c --- /dev/null +++ b/gtsam/slam/GeneralSFMFactor.h @@ -0,0 +1,83 @@ +/* + * GeneralSFMFactor.h + * + * Created on: Dec 15, 2010 + * Author: nikai + * Description: a general SFM factor with an unknown calibration + */ + +#pragma once + + +namespace gtsam { + + /** + * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor + */ + template + class GeneralSFMFactor: + public NonlinearFactor2 , + Testable > { + protected: + Point2 z_; + + public: + + typedef typename CamK::Value Cam; + typedef GeneralSFMFactor Self ; + typedef NonlinearFactor2 Base; + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr > shared_ptr; + + /** + * Constructor + * @param z is the 2 dimensional location of point in image (the measurement) + * @param sigma is the standard deviation + * @param cameraFrameNumber is basically the frame number + * @param landmarkNumber is the index of the landmark + * @param K the constant calibration + */ + + GeneralSFMFactor():z_(0.0,0.0) {} + GeneralSFMFactor(const Point2 & p):z_(p) {} + GeneralSFMFactor(double x, double y):z_(x,y) {} + GeneralSFMFactor(const Point2& z, const SharedGaussian& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {} + + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s = "SFMFactor") const { + Base::print(s); + z_.print(s + ".z"); + } + + /** + * equals + */ + bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { + return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ; + } + + /** h(x)-z */ + Vector evaluateError( + const Cam& camera, + const Point3& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + + Point2 q(camera.project(point,H1,H2) - z_); + return q.vector() ; + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(z_); + } + }; + +} //namespace diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 25343a3b1..ad5ce4a9d 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -46,6 +46,7 @@ sources += pose3SLAM.cpp check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLAM # Visual SLAM +headers += GeneralSFMFactor.h sources += visualSLAM.cpp check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index aec8ca338..cdbe1b651 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -28,7 +28,86 @@ #include #include -namespace gtsam { namespace visualSLAM { +namespace gtsam { + + /** + * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. + * i.e. the main building block for visual SLAM. + */ + template + class GenericProjectionFactor : public NonlinearFactor2, Testable > { + protected: + + // Keep a copy of measurement and calibration for I/O + Point2 z_; + boost::shared_ptr K_; + + public: + + // shorthand for base class type + typedef NonlinearFactor2 Base; + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr > shared_ptr; + + /** + * Default constructor + */ + GenericProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + + /** + * Constructor + * @param z is the 2 dimensional location of point in image (the measurement) + * @param sigma is the standard deviation + * @param cameraFrameNumber is basically the frame number + * @param landmarkNumber is the index of the landmark + * @param K the constant calibration + */ + GenericProjectionFactor(const Point2& z, + const SharedGaussian& model, POSK j_pose, + LMK j_landmark, const shared_ptrK& K) : + Base(model, j_pose, j_landmark), z_(z), K_(K) { + } + + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s = "ProjectionFactor") const { + Base::print(s); + z_.print(s + ".z"); + } + + /** + * equals + */ + bool equals(const GenericProjectionFactor& p, double tol = 1e-9) const { + return Base::equals(p, tol) && this->z_.equals(p.z_, tol) + && this->K_->equals(*p.K_, tol); + } + + + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose, const Point3& point, + boost::optional H1, boost::optional H2) const { + SimpleCamera camera(*K_, pose); + Point2 reprojectionError(camera.project(point, H1, H2) - z_); + return reprojectionError.vector(); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(K_); + } + }; + + + namespace visualSLAM { /** * Typedefs that make up the visualSLAM namespace. @@ -44,86 +123,7 @@ namespace gtsam { namespace visualSLAM { typedef NonlinearEquality PointConstraint; typedef PriorFactor PosePrior; typedef PriorFactor PointPrior; - - - - - /** - * Non-linear factor for a constraint derived from a 2D measurement, - * i.e. the main building block for visual SLAM. - */ - template - class GenericProjectionFactor : public NonlinearFactor2, Testable > { - protected: - - // Keep a copy of measurement and calibration for I/O - Point2 z_; - boost::shared_ptr K_; - - public: - - // shorthand for base class type - typedef NonlinearFactor2 Base; - - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; - - /** - * Default constructor - */ - GenericProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} - - /** - * Constructor - * @param z is the 2 dimensional location of point in image (the measurement) - * @param sigma is the standard deviation - * @param cameraFrameNumber is basically the frame number - * @param landmarkNumber is the index of the landmark - * @param K the constant calibration - */ - GenericProjectionFactor(const Point2& z, - const SharedGaussian& model, POSK j_pose, - LMK j_landmark, const shared_ptrK& K) : - Base(model, j_pose, j_landmark), z_(z), K_(K) { - } - - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s = "ProjectionFactor") const { - Base::print(s); - z_.print(s + ".z"); - } - - /** - * equals - */ - bool equals(const GenericProjectionFactor& p, double tol = 1e-9) const { - return Base::equals(p, tol) && this->z_.equals(p.z_, tol) - && this->K_->equals(*p.K_, tol); - } - - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1, boost::optional H2) const { - SimpleCamera camera(*K_, pose); - Point2 reprojectionError(camera.project(point, H1, H2) - z_); - return reprojectionError.vector(); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(z_); - ar & BOOST_SERIALIZATION_NVP(K_); - } - }; - // Typedef for general use typedef GenericProjectionFactor ProjectionFactor;