12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added default constructor and Serialization

release/4.3a0
Manohar Paluri 2009-11-16 23:49:04 +00:00
parent 20830a1534
commit 256c094039
2 changed files with 64 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/VSLAMFactor.cpp
View File

@ -12,6 +12,15 @@
using namespace std; using namespace std;
namespace gtsam{ 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) VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const Cal3_S2 &K)
: NonlinearFactor<VSLAMConfig>(z.vector(), sigma) : NonlinearFactor<VSLAMConfig>(z.vector(), sigma)

93
cpp/VSLAMFactor.h
View File

@ -21,56 +21,73 @@ class VSLAMConfig;
*/ */
class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor> class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
{ {
private: private:
int cameraFrameNumber_, landmarkNumber_; int cameraFrameNumber_, landmarkNumber_;
std::string cameraFrameName_, landmarkName_; std::string cameraFrameName_, landmarkName_;
Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this.
typedef NonlinearFactor<VSLAMConfig> ConvenientFactor; typedef NonlinearFactor<VSLAMConfig> ConvenientFactor;
public: public:
typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor
/** /**
* Constructor * Default constructor
* @param z is the 2 dimensional location of point in image (the measurement) */
* @param sigma is the standard deviation VSLAMFactor();
* @param cameraFrameNumber is basically the frame number
* @param landmarkNumber is the index of the landmark /**
* @param K the constant calibration * Constructor
*/ * @param z is the 2 dimensional location of point in image (the measurement)
VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const Cal3_S2& K); * @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 * print
* @param s optional string naming the factor * @param s optional string naming the factor
*/ */
void print(const std::string& s="VSLAMFactor") const; void print(const std::string& s="VSLAMFactor") const;
/** /**
* equals * equals
*/ */
bool equals(const VSLAMFactor&, double tol=1e-9) const; bool equals(const VSLAMFactor&, double tol=1e-9) const;
/** /**
* predict the measurement * predict the measurement
*/ */
Vector predict(const VSLAMConfig&) const; Vector predict(const VSLAMConfig&) const;
/** /**
* calculate the error of the factor * calculate the error of the factor
*/ */
Vector error_vector(const VSLAMConfig&) const; Vector error_vector(const VSLAMConfig&) const;
/** /**
* linerarization * linerarization
*/ */
GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const; GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const;
int getCameraFrameNumber() const { return cameraFrameNumber_; } int getCameraFrameNumber() const { return cameraFrameNumber_; }
int getLandmarkNumber() const { return landmarkNumber_; } int getLandmarkNumber() const { return landmarkNumber_; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
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_);
}
}; };
} }