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

Added key template parameters to visualSLAM::projectionFactor without changing the interface. Added dimension checking function for Point2 type.

release/4.3a0
Alex Cunningham 2010-02-15 16:37:37 +00:00
parent 1323386f12
commit 17706d4ebd
2 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/Point2.h
View File

@ -20,6 +20,10 @@ namespace gtsam {
* Functional, so no set functions: once created, a point is constant. * Functional, so no set functions: once created, a point is constant.
*/ */
class Point2: Testable<Point2>, public Lie<Point2> { class Point2: Testable<Point2>, public Lie<Point2> {
public:
/// dimension of the variable - used to autodetect sizes
static inline size_t dim() {return 2;}
private: private:
double x_, y_; double x_, y_;

13
cpp/visualSLAM.h
View File

@ -34,8 +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.
*/ */
template <class Cfg=Config> template <class Cfg=Config, class LmK=PointKey, class PosK=PoseKey>
class GenericProjectionFactor : public NonlinearFactor2<Cfg, PoseKey, Pose3, PointKey, Point3>, Testable<GenericProjectionFactor<Cfg> > { class GenericProjectionFactor : public NonlinearFactor2<Cfg, PosK, Pose3, LmK, 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
@ -45,7 +45,7 @@ namespace gtsam { namespace visualSLAM {
public: public:
// shorthand for base class type // shorthand for base class type
typedef NonlinearFactor2<Cfg, PoseKey, Pose3, PointKey, Point3> Base; typedef NonlinearFactor2<Cfg, PosK, Pose3, LmK, Point3> Base;
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor> shared_ptr; typedef boost::shared_ptr<GenericProjectionFactor> shared_ptr;
@ -64,8 +64,8 @@ namespace gtsam { namespace visualSLAM {
* @param K the constant calibration * @param K the constant calibration
*/ */
GenericProjectionFactor(const Point2& z, GenericProjectionFactor(const Point2& z,
const SharedGaussian& model, PoseKey j_pose, const SharedGaussian& model, PosK j_pose,
PointKey j_landmark, const shared_ptrK& K) : LmK 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) {
} }
@ -113,7 +113,8 @@ namespace gtsam { namespace visualSLAM {
} }
}; };
typedef GenericProjectionFactor<Config> ProjectionFactor; // Typedef for general use
typedef GenericProjectionFactor<Config, PointKey, PoseKey> ProjectionFactor;