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

move GeneralSFMFactor from vSLAM to library. Move GenericProjectionFactor to namespace gtsam so that it can be used in other slam namespaces.

release/4.3a0
Kai Ni 2010-12-15 23:53:10 +00:00
parent 9af0a20439
commit d3315b2886
3 changed files with 164 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam/slam/GeneralSFMFactor.h Normal file
View File

@ -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 Cfg, class CamK, class LmK>
class GeneralSFMFactor:
public NonlinearFactor2<Cfg, CamK, LmK> ,
Testable<GeneralSFMFactor<Cfg, CamK, LmK> > {
protected:
Point2 z_;
public:
typedef typename CamK::Value Cam;
typedef GeneralSFMFactor<Cfg, CamK, LmK> Self ;
typedef NonlinearFactor2<Cfg, CamK, LmK> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GeneralSFMFactor<Cfg, LmK, CamK> > 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<Cfg, CamK, LmK> &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<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
Point2 q(camera.project(point,H1,H2) - z_);
return q.vector() ;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(z_);
}
};
} //namespace

1
gtsam/slam/Makefile.am
View File

@ -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

160
gtsam/slam/visualSLAM.h
View File

@ -28,7 +28,86 @@
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
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 CFG, class LMK, class POSK>
class GenericProjectionFactor : public NonlinearFactor2<CFG, POSK, LMK>, Testable<GenericProjectionFactor<CFG, LMK, POSK> > {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 z_;
boost::shared_ptr<Cal3_S2> K_;
public:
// shorthand for base class type
typedef NonlinearFactor2<CFG, POSK, LMK> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor<CFG, LMK, POSK> > 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<CFG, LMK, POSK>& 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<Matrix&> H1, boost::optional<Matrix&> 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<class ARCHIVE>
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<Values, PointKey> PointConstraint;
typedef PriorFactor<Values, PoseKey> PosePrior;
typedef PriorFactor<Values, PointKey> PointPrior;
/**
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
*/
template <class CFG=Values, class LMK=PointKey, class POSK=PoseKey>
class GenericProjectionFactor : public NonlinearFactor2<CFG, POSK, LMK>, Testable<GenericProjectionFactor<CFG, LMK, POSK> > {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 z_;
boost::shared_ptr<Cal3_S2> K_;
public:
// shorthand for base class type
typedef NonlinearFactor2<CFG, POSK, LMK> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor<CFG, LMK, POSK> > 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<CFG, LMK, POSK>& 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<Matrix&> H1, boost::optional<Matrix&> 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
// Typedef for general use
typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;