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

Added a config template parameter for ProjectionFactors

release/4.3a0
Alex Cunningham 2010-02-06 05:08:52 +00:00
parent af9f45ff24
commit bb74b5c882
2 changed files with 29 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/visualSLAM.cpp
View File

@ -18,17 +18,17 @@ namespace gtsam {
namespace visualSLAM { namespace visualSLAM {
/* ************************************************************************* */ // /* ************************************************************************* */
void ProjectionFactor::print(const std::string& s) const { // void ProjectionFactor::print(const std::string& s) const {
Base::print(s); // Base::print(s);
z_.print(s + ".z"); // z_.print(s + ".z");
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
bool ProjectionFactor::equals(const ProjectionFactor& p, double tol) const { // bool ProjectionFactor::equals(const ProjectionFactor& p, double tol) const {
return Base::equals(p, tol) && z_.equals(p.z_, tol) // return Base::equals(p, tol) && z_.equals(p.z_, tol)
&& K_->equals(*p.K_, tol); // && K_->equals(*p.K_, tol);
} // }
// /* ************************************************************************* */ // /* ************************************************************************* */
// bool compareLandmark(const std::string& key, // bool compareLandmark(const std::string& key,

25
cpp/visualSLAM.h
View File

@ -34,7 +34,8 @@ namespace gtsam { namespace visualSLAM {
* Non-linear factor for a constraint derived from a 2D measurement, * Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM. * i.e. the main building block for visual SLAM.
*/ */
class ProjectionFactor: public NonlinearFactor2<Config, PoseKey, Pose3, PointKey, Point3>, Testable<ProjectionFactor> { template <class Cfg=Config>
class GenericProjectionFactor : public NonlinearFactor2<Cfg, PoseKey, Pose3, PointKey, Point3>, Testable<GenericProjectionFactor<Cfg> > {
private: private:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -44,15 +45,15 @@ namespace gtsam { namespace visualSLAM {
public: public:
// shorthand for base class type // shorthand for base class type
typedef NonlinearFactor2<Config, PoseKey, Pose3, PointKey, Point3> Base; typedef NonlinearFactor2<Cfg, PoseKey, Pose3, PointKey, Point3> Base;
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<ProjectionFactor> shared_ptr; typedef boost::shared_ptr<GenericProjectionFactor> shared_ptr;
/** /**
* Default constructor * Default constructor
*/ */
ProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} GenericProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
/** /**
* Constructor * Constructor
@ -62,7 +63,7 @@ namespace gtsam { namespace visualSLAM {
* @param landmarkNumber is the index of the landmark * @param landmarkNumber is the index of the landmark
* @param K the constant calibration * @param K the constant calibration
*/ */
ProjectionFactor(const Point2& z, GenericProjectionFactor(const Point2& z,
const SharedGaussian& model, PoseKey j_pose, const SharedGaussian& model, PoseKey j_pose,
PointKey j_landmark, const shared_ptrK& K) : PointKey j_landmark, const shared_ptrK& K) :
z_(z), K_(K), Base(model, j_pose, j_landmark) { z_(z), K_(K), Base(model, j_pose, j_landmark) {
@ -72,12 +73,18 @@ namespace gtsam { namespace visualSLAM {
* print * print
* @param s optional string naming the factor * @param s optional string naming the factor
*/ */
void print(const std::string& s = "ProjectionFactor") const; void print(const std::string& s = "ProjectionFactor") const {
Base::print(s);
z_.print(s + ".z");
}
/** /**
* equals * equals
*/ */
bool equals(const ProjectionFactor&, double tol = 1e-9) const; bool equals(const GenericProjectionFactor<Cfg>& p, double tol = 1e-9) const {
return Base::equals(p, tol) && z_.equals(p.z_, tol)
&& K_->equals(*p.K_, tol);
}
// /** h(x) */ // /** h(x) */
// Point2 predict(const Pose3& pose, const Point3& point) const { // Point2 predict(const Pose3& pose, const Point3& point) const {
@ -106,6 +113,10 @@ namespace gtsam { namespace visualSLAM {
} }
}; };
typedef GenericProjectionFactor<Config> ProjectionFactor;
// /** // /**
// * Non-linear factor graph for visual SLAM // * Non-linear factor graph for visual SLAM