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

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

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