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

Some formatting/cleanup before fixing bug

release/4.3a0
dellaert 2015-06-14 10:56:22 -07:00
parent 4909fef21a
commit 2c99f68ed7
3 changed files with 607 additions and 583 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/slam/GeneralSFMFactor.h
View File

@ -51,30 +51,30 @@ class access;
namespace gtsam { namespace gtsam {
/** /**
* Non-linear factor for a constraint derived from a 2D measurement. * Non-linear factor for a constraint derived from a 2D measurement.
* The calibration is unknown here compared to GenericProjectionFactor * The calibration is unknown here compared to GenericProjectionFactor
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template <class CAMERA, class LANDMARK> template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> { class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA);
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK);
static const int DimC = FixedDimension<CAMERA>::value; static const int DimC = FixedDimension<CAMERA>::value;
static const int DimL = FixedDimension<LANDMARK>::value; static const int DimL = FixedDimension<LANDMARK>::value;
typedef Eigen::Matrix<double, 2, DimC> JacobianC; typedef Eigen::Matrix<double, 2, DimC> JacobianC;
typedef Eigen::Matrix<double, 2, DimL> JacobianL; typedef Eigen::Matrix<double, 2, DimL> JacobianL;
protected: protected:
Point2 measured_; ///< the 2D measurement Point2 measured_; ///< the 2D measurement
public: public:
typedef GeneralSFMFactor<CAMERA, LANDMARK> This; ///< typedef for this object typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
typedef NoiseModelFactor2<CAMERA, LANDMARK> Base; ///< typedef for the base class typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
@ -98,7 +98,7 @@ namespace gtsam {
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
/** /**
* print * print
@ -115,7 +115,7 @@ namespace gtsam {
*/ */
bool equals(const NonlinearFactor &p, double tol = 1e-9) const { bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p); const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
} }
/** h(x)-z */ /** h(x)-z */
@ -224,7 +224,7 @@ namespace gtsam {
return measured_; return measured_;
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
@ -233,32 +233,32 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; };
template<class CAMERA, class LANDMARK> template<class CAMERA, class LANDMARK>
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable< struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
GeneralSFMFactor<CAMERA, LANDMARK> > { GeneralSFMFactor<CAMERA, LANDMARK> > {
}; };
/** /**
* Non-linear factor for a constraint derived from a 2D measurement. * Non-linear factor for a constraint derived from a 2D measurement.
* Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
*/ */
template <class CALIBRATION> template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> { class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
static const int DimK = FixedDimension<CALIBRATION>::value; static const int DimK = FixedDimension<CALIBRATION>::value;
protected: protected:
Point2 measured_; ///< the 2D measurement Point2 measured_; ///< the 2D measurement
public: public:
typedef GeneralSFMFactor2<CALIBRATION> This; typedef GeneralSFMFactor2<CALIBRATION> This;
typedef PinholeCamera<CALIBRATION> Camera; ///< typedef for camera type typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base; ///< typedef for the base class typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
@ -280,7 +280,7 @@ namespace gtsam {
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
/** /**
* print * print
@ -297,7 +297,7 @@ namespace gtsam {
*/ */
bool equals(const NonlinearFactor &p, double tol = 1e-9) const { bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p); const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
} }
/** h(x)-z */ /** h(x)-z */
@ -326,7 +326,7 @@ namespace gtsam {
return measured_; return measured_;
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
@ -335,11 +335,11 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; };
template<class CALIBRATION> template<class CALIBRATION>
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable< struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
GeneralSFMFactor2<CALIBRATION> > { GeneralSFMFactor2<CALIBRATION> > {
}; };
} //namespace } //namespace

330
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -49,7 +49,8 @@ typedef NonlinearEquality<Point3> Point3Constraint;
class Graph: public NonlinearFactorGraph { class Graph: public NonlinearFactorGraph {
public: public:
void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { void addMeasurement(int i, int j, const Point2& z,
const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, X(i), L(j))); push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
} }
@ -65,98 +66,100 @@ public:
}; };
static double getGaussian() static double getGaussian() {
{ double S, V1, V2;
double S,V1,V2;
// Use Box-Muller method to create gauss noise from uniform noise // Use Box-Muller method to create gauss noise from uniform noise
do do {
{ double U1 = rand() / (double) (RAND_MAX);
double U1 = rand() / (double)(RAND_MAX); double U2 = rand() / (double) (RAND_MAX);
double U2 = rand() / (double)(RAND_MAX);
V1 = 2 * U1 - 1; /* V1=[-1,1] */ V1 = 2 * U1 - 1; /* V1=[-1,1] */
V2 = 2 * U2 - 1; /* V2=[-1,1] */ V2 = 2 * U2 - 1; /* V2=[-1,1] */
S = V1 * V1 + V2 * V2; S = V1 * V1 + V2 * V2;
} while(S>=1); } while (S >= 1);
return sqrt(-2.0f * (double)log(S) / S) * V1; return sqrt(-2.f * (double) log(S) / S) * V1;
} }
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, equals ) TEST( GeneralSFMFactor, equals ) {
{
// Create two identical factors and make sure they're equal // Create two identical factors and make sure they're equal
Point2 z(323.,240.); Point2 z(323., 240.);
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection> factor1(
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> boost::shared_ptr<Projection> factor2(
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
EXPECT(assert_equal(*factor1, *factor2)); EXPECT(assert_equal(*factor1, *factor2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, error ) { TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.); Point2 z(3., 0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); const SharedNoiseModel sigma(noiseModel::Unit::Create(2));
Projection factor(z, sigma, X(1), L(1)); Projection factor(z, sigma, X(1), L(1));
// For the following configuration, the factor predicts 320,240 // For the following configuration, the factor predicts 320,240
Values values; Values values;
Rot3 R; Rot3 R;
Point3 t1(0,0,-6); Point3 t1(0, 0, -6);
Pose3 x1(R,t1); Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1); Point3 l1;
EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor.unwhitenedError(values))); values.insert(L(1), l1);
EXPECT(
assert_equal(((Vector ) Vector2(-3., 0.)),
factor.unwhitenedError(values)));
} }
static const double baseline = 5.0 ; static const double baseline = 5.;
/* ************************************************************************* */ /* ************************************************************************* */
static vector<Point3> genPoint3() { static vector<Point3> genPoint3() {
const double z = 5; const double z = 5;
vector<Point3> landmarks ; vector<Point3> landmarks;
landmarks.push_back(Point3 (-1.0,-1.0, z)); landmarks.push_back(Point3(-1., -1., z));
landmarks.push_back(Point3 (-1.0, 1.0, z)); landmarks.push_back(Point3(-1., 1., z));
landmarks.push_back(Point3 ( 1.0, 1.0, z)); landmarks.push_back(Point3(1., 1., z));
landmarks.push_back(Point3 ( 1.0,-1.0, z)); landmarks.push_back(Point3(1., -1., z));
landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); landmarks.push_back(Point3(-2., -2., 2 * z));
landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); landmarks.push_back(Point3(-2., 2., 2 * z));
landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); landmarks.push_back(Point3(2., 2., 2 * z));
landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); landmarks.push_back(Point3(2., -2., 2 * z));
return landmarks ; return landmarks;
} }
static vector<GeneralCamera> genCameraDefaultCalibration() { static vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> X ; vector<GeneralCamera> X;
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.))));
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.))));
return X ; return X;
} }
static vector<GeneralCamera> genCameraVariableCalibration() { static vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3_S2 K(640,480,0.01,320,240); const Cal3_S2 K(640, 480, 0.1, 320, 240);
vector<GeneralCamera> X ; vector<GeneralCamera> X;
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K));
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K));
return X ; return X;
} }
static boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) { static boost::shared_ptr<Ordering> getOrdering(
const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering); boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; for (size_t i = 0; i < landmarks.size(); ++i)
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; ordering->push_back(L(i));
return ordering ; for (size_t i = 0; i < cameras.size(); ++i)
ordering->push_back(X(i));
return ordering;
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_defaultK ) { TEST( GeneralSFMFactor, optimize_defaultK ) {
@ -165,32 +168,32 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = cameras.size()*landmarks.size() ; const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline * 0.1;
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y()+noise*getGaussian(), landmarks[i].y() + noise * getGaussian(),
landmarks[i].z()+noise*getGaussian()); landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt) ; values.insert(L(i), pt);
} }
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
// Create an ordering of the variables // Create an ordering of the variables
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
@ -202,38 +205,37 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<GeneralCamera> cameras = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = cameras.size()*landmarks.size() ; const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline * 0.1;
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
if ( i == 0 ) { if (i == 0) {
Point3 pt(landmarks[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y()+noise*getGaussian(), landmarks[i].y() + noise * getGaussian(),
landmarks[i].z()+noise*getGaussian()); landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt) ; values.insert(L(i), pt);
} } else {
else { values.insert(L(i), landmarks[i]);
values.insert(L(i), landmarks[i]) ;
} }
} }
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
const double reproj_error = 1e-5; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -246,35 +248,35 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
vector<GeneralCamera> cameras = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
const double noise = baseline*0.1; const double noise = baseline * 0.1;
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = landmarks.size()*cameras.size(); const size_t nMeasurements = landmarks.size() * cameras.size();
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y()+noise*getGaussian(), landmarks[i].y() + noise * getGaussian(),
landmarks[i].z()+noise*getGaussian()); landmarks[i].z() + noise * getGaussian());
//Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
values.insert(L(i), pt) ; values.insert(L(i), pt);
} }
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
graph.addCameraConstraint(i, cameras[i]); graph.addCameraConstraint(i, cameras[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -288,50 +290,45 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = landmarks.size()*cameras.size(); const size_t nMeasurements = landmarks.size() * cameras.size();
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) { for (size_t i = 0; i < cameras.size(); ++i) {
const double const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
rot_noise = 1e-5,
trans_noise = 1e-3,
focal_noise = 1,
skew_noise = 1e-5; skew_noise = 1e-5;
if ( i == 0 ) { if (i == 0) {
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
} } else {
else {
Vector delta = (Vector(11) << Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation
rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation trans_noise, trans_noise, trans_noise, // translation
focal_noise, focal_noise, // f_x, f_y focal_noise, focal_noise, // f_x, f_y
skew_noise, // s skew_noise, // s
trans_noise, trans_noise // ux, uy trans_noise, trans_noise // ux, uy
).finished(); ).finished();
values.insert(X(i), cameras[i].retract(delta)) ; values.insert(X(i), cameras[i].retract(delta));
} }
} }
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
values.insert(L(i), landmarks[i]) ; values.insert(L(i), landmarks[i]);
} }
// fix X0 and all landmarks, allow only the cameras[1] to move // fix X0 and all landmarks, allow only the cameras[1] to move
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) for (size_t i = 0; i < landmarks.size(); ++i)
graph.addPoint3Constraint(i, landmarks[i]); graph.addPoint3Constraint(i, landmarks[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -344,38 +341,40 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = cameras.size()*landmarks.size() ; const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline * 0.1;
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y()+noise*getGaussian(), landmarks[i].y() + noise * getGaussian(),
landmarks[i].z()+noise*getGaussian()); landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt) ; values.insert(L(i), pt);
} }
// Constrain position of system with the first camera constrained to the origin // Constrain position of system with the first camera constrained to the origin
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras // Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -386,17 +385,21 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3 // Tests range factor between a GeneralCamera and a Pose3
Graph graph; Graph graph;
graph.addCameraConstraint(0, GeneralCamera()); graph.addCameraConstraint(0, GeneralCamera());
graph.push_back(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); graph.push_back(
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
Values init; Values init;
init.insert(X(0), GeneralCamera()); init.insert(X(0), GeneralCamera());
init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
// The optimal value between the 2m range factor and 1m prior is 1.5m // The optimal value between the 2m range factor and 1m prior is 1.5m
Values expected; Values expected;
expected.insert(X(0), GeneralCamera()); expected.insert(X(0), GeneralCamera());
expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0))); expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.)));
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9; params.absoluteErrorTol = 1e-9;
@ -410,16 +413,23 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3 // Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0))); graph.push_back(
graph.push_back(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(),
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); noiseModel::Isotropic::Sigma(6, 1.)));
graph.push_back(
RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
Values init; Values init;
init.insert(X(0), CalibratedCamera()); init.insert(X(0), CalibratedCamera());
init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
Values expected; Values expected;
expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); expected.insert(X(0),
CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0))); expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0)));
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
@ -432,50 +442,58 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GeneralSFMFactor, Linearize) { TEST(GeneralSFMFactor, Linearize) {
Point2 z(3.,0.); Point2 z(3., 0.);
// Create Values // Create Values
Values values; Values values;
Rot3 R; Rot3 R;
Point3 t1(0,0,-6); Point3 t1(0, 0, -6);
Pose3 x1(R,t1); Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1); Point3 l1;
values.insert(L(1), l1);
// Test with Empty Model // Test with Empty Model
{ {
const SharedNoiseModel model; const SharedNoiseModel model;
Projection factor(z, model, X(1), L(1)); Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values); GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected,*actual,1e-9)); EXPECT(assert_equal(*expected, *actual, 1e-9));
} }
// Test with Unit Model // Test with Unit Model
{ {
const SharedNoiseModel model(noiseModel::Unit::Create(2)); const SharedNoiseModel model(noiseModel::Unit::Create(2));
Projection factor(z, model, X(1), L(1)); Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values); GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected,*actual,1e-9)); EXPECT(assert_equal(*expected, *actual, 1e-9));
} }
// Test with Isotropic noise // Test with Isotropic noise
{ {
const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2,0.5)); const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.5));
Projection factor(z, model, X(1), L(1)); Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values); GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected,*actual,1e-9)); EXPECT(assert_equal(*expected, *actual, 1e-9));
} }
// Test with Constrained Model // Test with Constrained Model
{ {
const SharedNoiseModel model(noiseModel::Constrained::All(2)); const SharedNoiseModel model(noiseModel::Constrained::All(2));
Projection factor(z, model, X(1), L(1)); Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values); GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected,*actual,1e-9)); EXPECT(assert_equal(*expected, *actual, 1e-9));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

276
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -49,7 +49,8 @@ typedef NonlinearEquality<Point3> Point3Constraint;
/* ************************************************************************* */ /* ************************************************************************* */
class Graph: public NonlinearFactorGraph { class Graph: public NonlinearFactorGraph {
public: public:
void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { void addMeasurement(const int& i, const int& j, const Point2& z,
const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, X(i), L(j))); push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
} }
@ -65,97 +66,101 @@ public:
}; };
static double getGaussian() static double getGaussian() {
{ double S, V1, V2;
double S,V1,V2;
// Use Box-Muller method to create gauss noise from uniform noise // Use Box-Muller method to create gauss noise from uniform noise
do do {
{ double U1 = rand() / (double) (RAND_MAX);
double U1 = rand() / (double)(RAND_MAX); double U2 = rand() / (double) (RAND_MAX);
double U2 = rand() / (double)(RAND_MAX);
V1 = 2 * U1 - 1; /* V1=[-1,1] */ V1 = 2 * U1 - 1; /* V1=[-1,1] */
V2 = 2 * U2 - 1; /* V2=[-1,1] */ V2 = 2 * U2 - 1; /* V2=[-1,1] */
S = V1 * V1 + V2 * V2; S = V1 * V1 + V2 * V2;
} while(S>=1); } while (S >= 1);
return sqrt(-2.0f * (double)log(S) / S) * V1; return sqrt(-2.f * (double) log(S) / S) * V1;
} }
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, equals ) TEST( GeneralSFMFactor_Cal3Bundler, equals ) {
{
// Create two identical factors and make sure they're equal // Create two identical factors and make sure they're equal
Point2 z(323.,240.); Point2 z(323., 240.);
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection> factor1(
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> boost::shared_ptr<Projection> factor2(
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
EXPECT(assert_equal(*factor1, *factor2)); EXPECT(assert_equal(*factor1, *factor2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, error ) { TEST( GeneralSFMFactor_Cal3Bundler, error ) {
Point2 z(3.,0.); Point2 z(3., 0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection> factor(new Projection(z, sigma, X(1), L(1)));
factor(new Projection(z, sigma, X(1), L(1)));
// For the following configuration, the factor predicts 320,240 // For the following configuration, the factor predicts 320,240
Values values; Values values;
Rot3 R; Rot3 R;
Point3 t1(0,0,-6); Point3 t1(0, 0, -6);
Pose3 x1(R,t1); Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1); Point3 l1;
EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values))); values.insert(L(1), l1);
EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values)));
} }
static const double baseline = 5.;
static const double baseline = 5.0 ;
/* ************************************************************************* */ /* ************************************************************************* */
static vector<Point3> genPoint3() { static vector<Point3> genPoint3() {
const double z = 5; const double z = 5;
vector<Point3> landmarks ; vector<Point3> landmarks;
landmarks.push_back(Point3 (-1.0,-1.0, z)); landmarks.push_back(Point3(-1., -1., z));
landmarks.push_back(Point3 (-1.0, 1.0, z)); landmarks.push_back(Point3(-1., 1., z));
landmarks.push_back(Point3 ( 1.0, 1.0, z)); landmarks.push_back(Point3(1., 1., z));
landmarks.push_back(Point3 ( 1.0,-1.0, z)); landmarks.push_back(Point3(1., -1., z));
landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); landmarks.push_back(Point3(-2., -2., 2 * z));
landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); landmarks.push_back(Point3(-2., 2., 2 * z));
landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); landmarks.push_back(Point3(2., 2., 2 * z));
landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); landmarks.push_back(Point3(2., -2., 2 * z));
return landmarks ; return landmarks;
} }
static vector<GeneralCamera> genCameraDefaultCalibration() { static vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> cameras ; vector<GeneralCamera> cameras;
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); cameras.push_back(
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.))));
return cameras ; cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.))));
return cameras;
} }
static vector<GeneralCamera> genCameraVariableCalibration() { static vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3Bundler K(500,1e-3,1e-3); const Cal3Bundler K(500, 1e-3, 1e-3);
vector<GeneralCamera> cameras ; vector<GeneralCamera> cameras;
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); cameras.push_back(
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
return cameras ; cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
return cameras;
} }
static boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) { static boost::shared_ptr<Ordering> getOrdering(
const std::vector<GeneralCamera>& cameras,
const std::vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering); boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; for (size_t i = 0; i < landmarks.size(); ++i)
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; ordering->push_back(L(i));
return ordering ; for (size_t i = 0; i < cameras.size(); ++i)
ordering->push_back(X(i));
return ordering;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -166,32 +171,32 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) {
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = cameras.size()*landmarks.size() ; const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline * 0.1;
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y()+noise*getGaussian(), landmarks[i].y() + noise * getGaussian(),
landmarks[i].z()+noise*getGaussian()); landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt) ; values.insert(L(i), pt);
} }
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
// Create an ordering of the variables // Create an ordering of the variables
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
@ -203,38 +208,37 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) {
vector<GeneralCamera> cameras = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = cameras.size()*landmarks.size() ; const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline * 0.1;
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
if ( i == 0 ) { if (i == 0) {
Point3 pt(landmarks[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y()+noise*getGaussian(), landmarks[i].y() + noise * getGaussian(),
landmarks[i].z()+noise*getGaussian()); landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt) ; values.insert(L(i), pt);
} } else {
else { values.insert(L(i), landmarks[i]);
values.insert(L(i), landmarks[i]) ;
} }
} }
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
const double reproj_error = 1e-5; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -247,35 +251,35 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) {
vector<GeneralCamera> cameras = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
const double noise = baseline*0.1; const double noise = baseline * 0.1;
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = landmarks.size()*cameras.size(); const size_t nMeasurements = landmarks.size() * cameras.size();
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y()+noise*getGaussian(), landmarks[i].y() + noise * getGaussian(),
landmarks[i].z()+noise*getGaussian()); landmarks[i].z() + noise * getGaussian());
//Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
values.insert(L(i), pt) ; values.insert(L(i), pt);
} }
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
graph.addCameraConstraint(i, cameras[i]); graph.addCameraConstraint(i, cameras[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -289,46 +293,43 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = landmarks.size()*cameras.size(); const size_t nMeasurements = landmarks.size() * cameras.size();
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) { for (size_t i = 0; i < cameras.size(); ++i) {
const double const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
rot_noise = 1e-5, trans_noise = 1e-3, distort_noise = 1e-3;
focal_noise = 1, distort_noise = 1e-3; if (i == 0) {
if ( i == 0 ) { values.insert(X(i), cameras[i]);
values.insert(X(i), cameras[i]) ; } else {
}
else {
Vector delta = (Vector(9) << Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation
rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2 focal_noise, distort_noise, distort_noise // f, k1, k2
).finished(); ).finished();
values.insert(X(i), cameras[i].retract(delta)) ; values.insert(X(i), cameras[i].retract(delta));
} }
} }
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
values.insert(L(i), landmarks[i]) ; values.insert(L(i), landmarks[i]);
} }
// fix X0 and all landmarks, allow only the cameras[1] to move // fix X0 and all landmarks, allow only the cameras[1] to move
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) for (size_t i = 0; i < landmarks.size(); ++i)
graph.addPoint3Constraint(i, landmarks[i]); graph.addPoint3Constraint(i, landmarks[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -341,43 +342,48 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < cameras.size() ; ++j) { for (size_t j = 0; j < cameras.size(); ++j) {
for ( size_t i = 0 ; i < landmarks.size() ; ++i) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point2 pt = cameras[j].project(landmarks[i]) ; Point2 pt = cameras[j].project(landmarks[i]);
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = cameras.size()*landmarks.size() ; const size_t nMeasurements = cameras.size() * landmarks.size();
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline * 0.1;
Values values; Values values;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) for (size_t i = 0; i < cameras.size(); ++i)
values.insert(X(i), cameras[i]) ; values.insert(X(i), cameras[i]);
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { for (size_t i = 0; i < landmarks.size(); ++i) {
Point3 pt(landmarks[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x() + noise * getGaussian(),
landmarks[i].y()+noise*getGaussian(), landmarks[i].y() + noise * getGaussian(),
landmarks[i].z()+noise*getGaussian()); landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt) ; values.insert(L(i), pt);
} }
// Constrain position of system with the first camera constrained to the origin // Constrain position of system with the first camera constrained to the origin
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras // Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(cameras,landmarks); Ordering ordering = *getOrdering(cameras, landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */