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

CALIBRATION template parameter

release/4.3a0
Frank Dellaert 2012-06-14 05:04:52 +00:00
parent 642e180ff7
commit c3901f6784
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/slam/ProjectionFactor.h
View File

@ -29,13 +29,13 @@ 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 POSE, class LANDMARK>
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
Point2 measured_; ///< 2D measurement
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
public:
@ -43,14 +43,13 @@ namespace gtsam {
typedef NoiseModelFactor2<POSE, LANDMARK> Base;
/// shorthand for this class
typedef GenericProjectionFactor<POSE, LANDMARK> This;
typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
GenericProjectionFactor() :
K_(new Cal3_S2(444, 555, 666, 777, 888)) {
GenericProjectionFactor() {
}
/**
@ -94,7 +93,7 @@ namespace gtsam {
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
try {
SimpleCamera camera(*K_, pose);
PinholeCamera<CALIBRATION> camera(*K_, pose);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
} catch( CheiralityException& e) {
@ -112,7 +111,7 @@ namespace gtsam {
}
/** return the calibration object */
inline const Cal3_S2::shared_ptr calibration() const {
inline const boost::shared_ptr<CALIBRATION> calibration() const {
return K_;
}

9
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -41,8 +42,14 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
TEST( ProjectionFactor, error )
TEST( ProjectionFactor, nonStandard )
{
GenericProjectionFactor<Pose3, Point3, Cal3DS2> f;
}
/* ************************************************************************* */
TEST( ProjectionFactor, error )
{
// Create the factor with a measurement that is 3 pixels off in x
Point2 z(323.,240.);
int i=1, j=1;