diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp index e0e7b4aef..f40687120 100644 --- a/cpp/VSLAMFactor.cpp +++ b/cpp/VSLAMFactor.cpp @@ -12,6 +12,15 @@ using namespace std; namespace gtsam{ +/* ************************************************************************* */ +VSLAMFactor::VSLAMFactor() { + /// Arbitrary values + cameraFrameNumber_ = 111; + landmarkNumber_ = 222; + cameraFrameName_ = symbol('x',cameraFrameNumber_); + landmarkName_ = symbol('l',landmarkNumber_); + K_ = Cal3_S2(444,555,666,777,888); +} /* ************************************************************************* */ VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const Cal3_S2 &K) : NonlinearFactor(z.vector(), sigma) diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h index 920187bc7..e7c72bde8 100644 --- a/cpp/VSLAMFactor.h +++ b/cpp/VSLAMFactor.h @@ -21,56 +21,73 @@ class VSLAMConfig; */ class VSLAMFactor : public NonlinearFactor, Testable { - private: +private: int cameraFrameNumber_, landmarkNumber_; - std::string cameraFrameName_, landmarkName_; - Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. - typedef NonlinearFactor ConvenientFactor; + std::string cameraFrameName_, landmarkName_; + Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. + typedef NonlinearFactor ConvenientFactor; - public: +public: - typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor - /** - * 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 - */ - VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const Cal3_S2& K); + /** + * Default constructor + */ + VSLAMFactor(); + + /** + * 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 + */ + VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const Cal3_S2& K); - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s="VSLAMFactor") const; + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s="VSLAMFactor") const; - /** - * equals - */ - bool equals(const VSLAMFactor&, double tol=1e-9) const; + /** + * equals + */ + bool equals(const VSLAMFactor&, double tol=1e-9) const; - /** - * predict the measurement - */ - Vector predict(const VSLAMConfig&) const; + /** + * predict the measurement + */ + Vector predict(const VSLAMConfig&) const; - /** - * calculate the error of the factor - */ - Vector error_vector(const VSLAMConfig&) const; + /** + * calculate the error of the factor + */ + Vector error_vector(const VSLAMConfig&) const; - /** - * linerarization - */ - GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const; + /** + * linerarization + */ + GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const; - int getCameraFrameNumber() const { return cameraFrameNumber_; } - int getLandmarkNumber() const { return landmarkNumber_; } + int getCameraFrameNumber() const { return cameraFrameNumber_; } + int getLandmarkNumber() const { return landmarkNumber_; } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(cameraFrameNumber_); + ar & BOOST_SERIALIZATION_NVP(landmarkNumber_); + ar & BOOST_SERIALIZATION_NVP(cameraFrameName_); + ar & BOOST_SERIALIZATION_NVP(landmarkName_); + ar & BOOST_SERIALIZATION_NVP(K_); + } }; }