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

Got rid of template parameters POSE (which was a fiction) and Z (which is now derived as CAMERA::Measurement).

release/4.3a0
dellaert 2015-02-19 12:27:40 +01:00
parent 370b447e75
commit 8a64d5bffe
9 changed files with 64 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam.h
View File

@ -2301,23 +2301,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<POSE, CALIBRATION>
template<CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(double rankTol, double linThreshold,
bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor);
bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(double rankTol);
SmartProjectionPoseFactor();
void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i,
const CALIBRATION* K_i);
void add(const gtsam::Point2& measured_i, size_t poseKey_i,
const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i);
// enabling serialization functionality
//void serialize() const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2> SmartProjectionPose3Factor;
#include <gtsam/slam/StereoFactor.h>

15
gtsam/slam/SmartFactorBase.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
* The methods take a Cameras argument, which should behave like PinholeCamera, and
* the value of a point, which is kept in the base class.
*/
template<class POSE, class Z, class CAMERA, size_t D>
template<class CAMERA, size_t D>
class SmartFactorBase: public NonlinearFactor {
protected:
@ -53,10 +53,11 @@ protected:
* 2D measurement and noise model for each of the m views
* The order is kept the same as the keys that we use to create the factor.
*/
typedef typename CAMERA::Measurement Z;
std::vector<Z> measured_;
std::vector<SharedNoiseModel> noise_; ///< noise model used
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
@ -73,7 +74,7 @@ protected:
typedef NonlinearFactor Base;
/// shorthand for this class
typedef SmartFactorBase<POSE, Z, CAMERA, D> This;
typedef SmartFactorBase<CAMERA, D> This;
public:
@ -90,7 +91,7 @@ public:
* Constructor
* @param body_P_sensor is the transform from sensor to body frame (default identity)
*/
SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) :
SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor) {
}
@ -279,7 +280,7 @@ public:
* F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
* with respect to the point. The value of cameras/point are passed as parameters.
* Given a Point3, assumes dimensionality is 3.
* TODO We assume below the dimensionality of POSE is 6. Frank thinks the templating
* TODO We assume below the dimensionality of Pose3 is 6. Frank thinks the templating
* of this factor is only for show, and should just assume a PinholeCamera.
*/
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
@ -731,7 +732,7 @@ private:
}
};
template<class POSE, class Z, class CAMERA, size_t D>
const int SmartFactorBase<POSE, Z, CAMERA, D>::ZDim;
template<class CAMERA, size_t D>
const int SmartFactorBase<CAMERA, D>::ZDim;
} // \ namespace gtsam

30
gtsam/slam/SmartProjectionFactor.h
View File

@ -58,11 +58,10 @@ enum LinearizationMode {
};
/**
* SmartProjectionFactor: triangulates point
* TODO: why LANDMARK parameter?
* SmartProjectionFactor: triangulates point and keeps an estimate of it around.
*/
template<class POSE, class CALIBRATION, size_t D>
class SmartProjectionFactor: public SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> {
template<class CALIBRATION, size_t D>
class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>, D> {
protected:
// Some triangulation parameters
@ -92,7 +91,7 @@ protected:
typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> Base;
typedef SmartFactorBase<PinholeCamera<CALIBRATION>, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
@ -102,9 +101,7 @@ protected:
// and the factor is disregarded if the error is large
/// shorthand for this class
typedef SmartProjectionFactor<POSE, CALIBRATION, D> This;
static const int ZDim = 2; ///< Measurement dimension
typedef SmartProjectionFactor<CALIBRATION, D> This;
public:
@ -126,7 +123,7 @@ public:
*/
SmartProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
boost::optional<POSE> body_P_sensor = boost::none,
boost::optional<Pose3> body_P_sensor = boost::none,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
@ -338,7 +335,7 @@ public:
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
// std::cout << "In linearize: exception" << std::endl;
BOOST_FOREACH(gtsam::Matrix& m, Gs)
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(D, D);
BOOST_FOREACH(Vector& v, gs)
v = zero(D);
@ -420,16 +417,16 @@ public:
}
/// create factor
boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda);
else
return boost::make_shared< JacobianFactorQ<D, ZDim> >(this->keys_);
return boost::make_shared< JacobianFactorQ<D, 2> >(this->keys_);
}
/// Create a factor, takes values
boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
const Values& values, double lambda) const {
Cameras myCameras;
// TODO triangulate twice ??
@ -437,7 +434,7 @@ public:
if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda);
else
return boost::make_shared< JacobianFactorQ<D, ZDim> >(this->keys_);
return boost::make_shared< JacobianFactorQ<D, 2> >(this->keys_);
}
/// different (faster) way to compute Jacobian factor
@ -446,7 +443,7 @@ public:
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
else
return boost::make_shared< JacobianFactorSVD<D, ZDim> >(this->keys_);
return boost::make_shared< JacobianFactorSVD<D, 2> >(this->keys_);
}
/// Returns true if nonDegenerate
@ -709,7 +706,4 @@ private:
}
};
template<class POSE, class CALIBRATION, size_t D>
const int SmartProjectionFactor<POSE, CALIBRATION, D>::ZDim;
} // \ namespace gtsam

16
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -37,8 +37,8 @@ namespace gtsam {
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
* @addtogroup SLAM
*/
template<class POSE, class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, CALIBRATION, 6> {
template<class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<CALIBRATION, 6> {
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
@ -48,10 +48,10 @@ protected:
public:
/// shorthand for base class type
typedef SmartProjectionFactor<POSE, CALIBRATION, 6> Base;
typedef SmartProjectionFactor<CALIBRATION, 6> Base;
/// shorthand for this class
typedef SmartProjectionPoseFactor<POSE, CALIBRATION> This;
typedef SmartProjectionPoseFactor<CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -67,7 +67,7 @@ public:
*/
SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
@ -211,7 +211,9 @@ private:
}; // end of class declaration
/// traits
template<class T1, class T2>
struct traits<SmartProjectionPoseFactor<T1, T2> > : public Testable<SmartProjectionPoseFactor<T1, T2> > {};
template<class CALIBRATION>
struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
SmartProjectionPoseFactor<CALIBRATION> > {
};
} // \ namespace gtsam

17
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -26,8 +26,7 @@ using namespace gtsam;
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
class PinholeFactor: public SmartFactorBase<Pose3, Point2,
PinholeCamera<Cal3Bundler>, 9> {
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler>, 9> {
virtual double error(const Values& values) const {
return 0.0;
}
@ -39,14 +38,14 @@ class PinholeFactor: public SmartFactorBase<Pose3, Point2,
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f;
f.add(Point2(),1,SharedNoiseModel());
f.add(Point2(),2,SharedNoiseModel());
EXPECT_LONGS_EQUAL(2*2,f.dim());
f.add(Point2(), 1, SharedNoiseModel());
f.add(Point2(), 2, SharedNoiseModel());
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
class StereoFactor: public SmartFactorBase<Pose3, StereoPoint2, StereoCamera, 9> {
class StereoFactor: public SmartFactorBase<StereoCamera, 9> {
virtual double error(const Values& values) const {
return 0.0;
}
@ -58,9 +57,9 @@ class StereoFactor: public SmartFactorBase<Pose3, StereoPoint2, StereoCamera, 9>
TEST(SmartFactorBase, Stereo) {
StereoFactor f;
f.add(StereoPoint2(),1,SharedNoiseModel());
f.add(StereoPoint2(),2,SharedNoiseModel());
EXPECT_LONGS_EQUAL(2*3,f.dim());
f.add(StereoPoint2(), 1, SharedNoiseModel());
f.add(StereoPoint2(), 2, SharedNoiseModel());
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
}
/* ************************************************************************* */

6
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -60,8 +60,8 @@ static Key poseKey1(x1);
static Point2 measurement1(323.0, 240.0);
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
typedef SmartProjectionPoseFactor<Pose3,Cal3_S2> SmartFactor;
typedef SmartProjectionPoseFactor<Pose3,Cal3Bundler> SmartFactorBundler;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
void projectToMultipleCameras(
SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark,
@ -1202,7 +1202,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
SmartProjectionPoseFactor<Pose3,Cal3Bundler> factor1(rankTol, linThreshold);
SmartFactorBundler factor1(rankTol, linThreshold);
boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
factor1.add(measurement1, poseKey1, model, Kbundler);
}

19
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -62,10 +62,9 @@ enum LinearizationMode {
/**
* SmartStereoProjectionFactor: triangulates point
* TODO: why LANDMARK parameter?
*/
template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> {
template<class CALIBRATION, size_t D>
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera, D> {
protected:
// Some triangulation parameters
@ -95,7 +94,7 @@ protected:
typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> Base;
typedef SmartFactorBase<StereoCamera, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
@ -105,7 +104,7 @@ protected:
// and the factor is disregarded if the error is large
/// shorthand for this class
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
typedef SmartStereoProjectionFactor<CALIBRATION, D> This;
enum {ZDim = 3}; ///< Dimension trait of measurement type
@ -131,7 +130,7 @@ public:
*/
SmartStereoProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
boost::optional<POSE> body_P_sensor = boost::none,
boost::optional<Pose3> body_P_sensor = boost::none,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
@ -370,7 +369,7 @@ public:
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) std::cout << "In linearize: exception" << std::endl;
BOOST_FOREACH(gtsam::Matrix& m, Gs)
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(D, D);
BOOST_FOREACH(Vector& v, gs)
v = zero(D);
@ -746,9 +745,9 @@ private:
};
/// traits
template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
struct traits<SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> > :
public Testable<SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> > {
template<class CALIBRATION, size_t D>
struct traits<SmartStereoProjectionFactor<CALIBRATION, D> > :
public Testable<SmartStereoProjectionFactor<CALIBRATION, D> > {
};
} // \ namespace gtsam

16
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -37,8 +37,8 @@ namespace gtsam {
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
template<class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION, 6> {
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
@ -50,10 +50,10 @@ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;
typedef SmartStereoProjectionFactor<CALIBRATION, 6> Base;
/// shorthand for this class
typedef SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> This;
typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -69,7 +69,7 @@ public:
*/
SmartStereoProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
@ -211,9 +211,9 @@ private:
}; // end of class declaration
/// traits
template<class POSE, class LANDMARK, class CALIBRATION>
struct traits<SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> > :
public Testable<SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> > {
template<class CALIBRATION>
struct traits<SmartStereoProjectionPoseFactor<CALIBRATION> > : public Testable<
SmartStereoProjectionPoseFactor<CALIBRATION> > {
};
} // \ namespace gtsam

4
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -62,8 +62,8 @@ static Key poseKey1(x1);
static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3_S2Stereo> SmartFactor;
typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
vector<StereoPoint2> stereo_projectToMultipleCameras(
const StereoCamera& cam1, const StereoCamera& cam2,