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

532
gtsam/slam/GeneralSFMFactor.h
View File

@ -51,295 +51,295 @@ 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;
/**
* Constructor
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation of the measurements
* @param cameraKey is the index of the camera
* @param landmarkKey is the index of the landmark
*/
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
virtual ~GeneralSFMFactor() {} ///< destructor
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter for printing out Symbols
*/
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/**
* equals
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
}
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
try {
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = JacobianC::Zero();
if (H2) *H2 = JacobianL::Zero();
// TODO warn if verbose output asked for
return zero(2);
}
}
class LinearizedFactor : public JacobianFactor {
// Fixed size matrices
// TODO: implement generic BinaryJacobianFactor<N,M1,M2> next to
// JacobianFactor
JacobianC AC_;
JacobianL AL_;
Vector2 b_;
public:
/// Constructor
LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2,
const Vector2& b,
const SharedDiagonal& model = SharedDiagonal())
: JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {}
// Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
gttic(updateHessian_LinearizedFactor);
// Whiten the factor if it has a noise model
const SharedDiagonal& model = get_model();
if (model && !model->isUnit()) {
if (model->isConstrained())
throw std::invalid_argument(
"JacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
JacobianFactor whitenedFactor = whiten();
whitenedFactor.updateHessian(infoKeys, info);
} else {
// First build an array of slots
DenseIndex slotC = Slot(infoKeys, keys_.front());
DenseIndex slotL = Slot(infoKeys, keys_.back());
DenseIndex slotB = info->nBlocks() - 1;
// We perform I += A'*A to the upper triangle
(*info)(slotC, slotC).selfadjointView().rankUpdate(AC_.transpose());
(*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_;
(*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_;
(*info)(slotL, slotL).selfadjointView().rankUpdate(AL_.transpose());
(*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_;
(*info)(slotB, slotB).selfadjointView().rankUpdate(b_.transpose());
}
}
};
/// Linearize using fixed-size matrices
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
// Only linearize if the factor is active
if (!this->active(values)) return boost::shared_ptr<LinearizedFactor>();
const Key key1 = this->key1(), key2 = this->key2();
JacobianC H1;
JacobianL H2;
Vector2 b;
try {
const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2);
Point2 reprojError(camera.project2(point, H1, H2) - measured());
b = -reprojError.vector();
} catch (CheiralityException& e) {
H1.setZero();
H2.setZero();
b.setZero();
// TODO warn if verbose output asked for
}
// Whiten the system if needed
const SharedNoiseModel& noiseModel = this->get_noiseModel();
if (noiseModel && !noiseModel->isUnit()) {
// TODO: implement WhitenSystem for fixed size matrices and include
// above
H1 = noiseModel->Whiten(H1);
H2 = noiseModel->Whiten(H2);
b = noiseModel->Whiten(b);
}
if (noiseModel && noiseModel->isConstrained()) {
using noiseModel::Constrained;
return boost::make_shared<LinearizedFactor>(
key1, H1, key2, H2, b,
boost::static_pointer_cast<Constrained>(noiseModel)->unit());
} else {
return boost::make_shared<LinearizedFactor>(key1, H1, key2, H2, b);
}
}
/** return the measured */
inline const Point2 measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
template<class CAMERA, class LANDMARK>
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
GeneralSFMFactor<CAMERA, LANDMARK> > {
};
/** /**
* Non-linear factor for a constraint derived from a 2D measurement. * Constructor
* Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. * @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation of the measurements
* @param cameraKey is the index of the camera
* @param landmarkKey is the index of the landmark
*/ */
template <class CALIBRATION> GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> { Base(model, cameraKey, landmarkKey), measured_(measured) {}
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
static const int DimK = FixedDimension<CALIBRATION>::value; GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
protected: virtual ~GeneralSFMFactor() {} ///< destructor
Point2 measured_; ///< the 2D measurement /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
public: /**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter for printing out Symbols
*/
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
typedef GeneralSFMFactor2<CALIBRATION> This; /**
typedef PinholeCamera<CALIBRATION> Camera; ///< typedef for camera type * equals
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base; ///< typedef for the base class */
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
}
// shorthand for a smart pointer to a factor /** h(x)-z */
typedef boost::shared_ptr<This> shared_ptr; Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
/** try {
* Constructor Point2 reprojError(camera.project2(point,H1,H2) - measured_);
* @param measured is the 2 dimensional location of point in image (the measurement) return reprojError.vector();
* @param model is the standard deviation of the measurements
* @param poseKey is the index of the camera
* @param landmarkKey is the index of the landmark
* @param calibKey is the index of the calibration
*/
GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor
virtual ~GeneralSFMFactor2() {} ///< destructor
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
} }
catch( CheiralityException& e) {
/** if (H1) *H1 = JacobianC::Zero();
* equals if (H2) *H2 = JacobianL::Zero();
*/ // TODO warn if verbose output asked for
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const
{
try {
Camera camera(pose3,calib);
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, 6);
if (H2) *H2 = zeros(2, 3);
if (H3) *H3 = zeros(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}
return zero(2); return zero(2);
} }
}
/** return the measured */ class LinearizedFactor : public JacobianFactor {
inline const Point2 measured() const { // Fixed size matrices
return measured_; // TODO: implement generic BinaryJacobianFactor<N,M1,M2> next to
} // JacobianFactor
JacobianC AC_;
JacobianL AL_;
Vector2 b_;
private: public:
/** Serialization function */ /// Constructor
friend class boost::serialization::access; LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2,
template<class Archive> const Vector2& b,
void serialize(Archive & ar, const unsigned int /*version*/) { const SharedDiagonal& model = SharedDiagonal())
ar & boost::serialization::make_nvp("NoiseModelFactor3", : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {}
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); // Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
gttic(updateHessian_LinearizedFactor);
// Whiten the factor if it has a noise model
const SharedDiagonal& model = get_model();
if (model && !model->isUnit()) {
if (model->isConstrained())
throw std::invalid_argument(
"JacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
JacobianFactor whitenedFactor = whiten();
whitenedFactor.updateHessian(infoKeys, info);
} else {
// First build an array of slots
DenseIndex slotC = Slot(infoKeys, keys_.front());
DenseIndex slotL = Slot(infoKeys, keys_.back());
DenseIndex slotB = info->nBlocks() - 1;
// We perform I += A'*A to the upper triangle
(*info)(slotC, slotC).selfadjointView().rankUpdate(AC_.transpose());
(*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_;
(*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_;
(*info)(slotL, slotL).selfadjointView().rankUpdate(AL_.transpose());
(*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_;
(*info)(slotB, slotB).selfadjointView().rankUpdate(b_.transpose());
}
} }
}; };
template<class CALIBRATION> /// Linearize using fixed-size matrices
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable< boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
GeneralSFMFactor2<CALIBRATION> > { // Only linearize if the factor is active
}; if (!this->active(values)) return boost::shared_ptr<LinearizedFactor>();
const Key key1 = this->key1(), key2 = this->key2();
JacobianC H1;
JacobianL H2;
Vector2 b;
try {
const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2);
Point2 reprojError(camera.project2(point, H1, H2) - measured());
b = -reprojError.vector();
} catch (CheiralityException& e) {
H1.setZero();
H2.setZero();
b.setZero();
// TODO warn if verbose output asked for
}
// Whiten the system if needed
const SharedNoiseModel& noiseModel = this->get_noiseModel();
if (noiseModel && !noiseModel->isUnit()) {
// TODO: implement WhitenSystem for fixed size matrices and include
// above
H1 = noiseModel->Whiten(H1);
H2 = noiseModel->Whiten(H2);
b = noiseModel->Whiten(b);
}
if (noiseModel && noiseModel->isConstrained()) {
using noiseModel::Constrained;
return boost::make_shared<LinearizedFactor>(
key1, H1, key2, H2, b,
boost::static_pointer_cast<Constrained>(noiseModel)->unit());
} else {
return boost::make_shared<LinearizedFactor>(key1, H1, key2, H2, b);
}
}
/** return the measured */
inline const Point2 measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
template<class CAMERA, class LANDMARK>
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
GeneralSFMFactor<CAMERA, LANDMARK> > {
};
/**
* 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..
*/
template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
static const int DimK = FixedDimension<CALIBRATION>::value;
protected:
Point2 measured_; ///< the 2D measurement
public:
typedef GeneralSFMFactor2<CALIBRATION> This;
typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Constructor
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation of the measurements
* @param poseKey is the index of the camera
* @param landmarkKey is the index of the landmark
* @param calibKey is the index of the calibration
*/
GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor
virtual ~GeneralSFMFactor2() {} ///< destructor
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/**
* equals
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const
{
try {
Camera camera(pose3,calib);
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, 6);
if (H2) *H2 = zeros(2, 3);
if (H3) *H3 = zeros(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}
return zero(2);
}
/** return the measured */
inline const Point2 measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
template<class CALIBRATION>
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
GeneralSFMFactor2<CALIBRATION> > {
};
} //namespace } //namespace

370
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 U2 = rand() / (double) (RAND_MAX);
double U1 = rand() / (double)(RAND_MAX); V1 = 2 * U1 - 1; /* V1=[-1,1] */
double U2 = rand() / (double)(RAND_MAX); V2 = 2 * U2 - 1; /* V2=[-1,1] */
V1 = 2 * U1 - 1; /* V1=[-1,1] */ S = V1 * V1 + V2 * V2;
V2 = 2 * U2 - 1; /* V2=[-1,1] */ } while (S >= 1);
S = V1 * V1 + V2 * V2; return sqrt(-2.f * (double) log(S) / S) * V1;
} while(S>=1);
return sqrt(-2.0f * (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, skew_noise = 1e-5;
trans_noise = 1e-3, if (i == 0) {
focal_noise = 1, values.insert(X(i), cameras[i]);
skew_noise = 1e-5; } else {
if ( i == 0 ) {
values.insert(X(i), cameras[i]) ;
}
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(
GaussianFactor::shared_ptr actual = factor.linearize(values); values);
EXPECT(assert_equal(*expected,*actual,1e-9)); GaussianFactor::shared_ptr actual = factor.linearize(values);
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(
GaussianFactor::shared_ptr actual = factor.linearize(values); values);
EXPECT(assert_equal(*expected,*actual,1e-9)); GaussianFactor::shared_ptr actual = factor.linearize(values);
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(
GaussianFactor::shared_ptr actual = factor.linearize(values); values);
EXPECT(assert_equal(*expected,*actual,1e-9)); GaussianFactor::shared_ptr actual = factor.linearize(values);
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(
GaussianFactor::shared_ptr actual = factor.linearize(values); values);
EXPECT(assert_equal(*expected,*actual,1e-9)); GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected, *actual, 1e-9));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

288
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 U2 = rand() / (double) (RAND_MAX);
double U1 = rand() / (double)(RAND_MAX); V1 = 2 * U1 - 1; /* V1=[-1,1] */
double U2 = rand() / (double)(RAND_MAX); V2 = 2 * U2 - 1; /* V2=[-1,1] */
V1 = 2 * U1 - 1; /* V1=[-1,1] */ S = V1 * V1 + V2 * V2;
V2 = 2 * U2 - 1; /* V2=[-1,1] */ } while (S >= 1);
S = V1 * V1 + V2 * V2; return sqrt(-2.f * (double) log(S) / S) * V1;
} while(S>=1);
return sqrt(-2.0f * (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);
}
/* ************************************************************************* */ /* ************************************************************************* */