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

Merged changes from the trunk back into navigation and slam. Needed some data files for tests, as well. 

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/branches/2.4@20423 898a188c-9671-0410-8e00-e3fd810bbb7f
release/4.3a0
Frank Dellaert 2013-12-21 20:12:12 +00:00 committed by Richard Roberts
parent bd3126f7b4
commit 0dc1eac55c
24 changed files with 3072 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
examples/Data/5pointExample1.txt Normal file
View File

@ -0,0 +1,53 @@
2 5 10
0 0 0 -0
1 0 -6.123233995736766344e-18 -0.10000000000000000555
0 1 -0.10000000000000000555 -0
1 1 -1.2246467991473532688e-17 -0.2000000000000000111
0 2 0.10000000000000000555 -0
1 2 0 -0
0 3 0 -1
1 3 1 -0.20000000000000006661
0 4 0 1
1 4 -1 -0.19999999999999995559
3.141592653589793116
0
0
-0
0
0
1
0
0
2.2214414690791830509
2.2214414690791826068
0
-6.123233995736766344e-18
-0.10000000000000000555
0
1
0
0
0
0
1
-0.10000000000000000555
0
1
0.10000000000000000555
0
1
0
0.5
0.5
0
-0.5
0.5

1659
examples/Data/Balbianello.out Normal file
View File

File diff suppressed because it is too large Load Diff

80
examples/Data/dubrovnik-3-7-pre-rewritten.txt Normal file
View File

@ -0,0 +1,80 @@
3 7 19
0 0 -385.989990234375 387.1199951171875
1 0 -38.439998626708984375 492.1199951171875
2 0 -667.91998291015625 123.1100006103515625
0 1 383.8800048828125 -15.299989700317382812
1 1 559.75 -106.15000152587890625
0 2 591.54998779296875 136.44000244140625
1 2 863.8599853515625 -23.469970703125
2 2 494.720001220703125 112.51999664306640625
0 3 592.5 125.75
1 3 861.08001708984375 -35.219970703125
2 3 498.540008544921875 101.55999755859375
0 4 348.720001220703125 558.3800048828125
1 4 776.030029296875 483.529998779296875
2 4 7.7800288200378417969 326.350006103515625
0 5 14.010009765625 96.420013427734375
1 5 207.1300048828125 118.3600006103515625
0 6 202.7599945068359375 340.989990234375
1 6 543.18011474609375 294.80999755859375
2 6 -58.419979095458984375 110.8300018310546875
0.29656188120312942935
-0.035318354384285870207
0.31252101755032046793
0.47230274932665988752
-0.3572340863744113415
-2.0517704282499575896
1430.031982421875
-7.5572756941255647689e-08
3.2377570134516087119e-14
0.28532097381985194184
-0.27699838370789808817
0.048601169984112867206
-1.2598695987143850861
-0.049063798188844320869
-1.9586867140445654023
1432.137451171875
-7.3171918302250560373e-08
3.1759419042137054801e-14
0.057491325683772541433
0.34853090049579965592
0.47985129303736057116
8.1963904289063389541
6.5146840788718787252
-3.8392804395897406344
1572.047119140625
-1.5962623223231275915e-08
-1.6507904730136101212e-14
-11.317351620610928364
3.3594874875767186673
-42.755222607849105998
4.2648515634753199066
-8.4629358700849355301
-22.252086323427270997
10.996977688149536689
-9.2123370180278048025
-29.206739014051372294
10.935342607054865383
-9.4338917557810741954
-29.112263909175499776
15.714024935401759819
1.3745079651566265433
-59.286834979937104606
-1.3624227800805182031
-4.1979357415396094666
-21.034430148188398846
6.7690173115899296974
-4.7352452433700786827
-53.605307875695892506

80
examples/Data/dubrovnik-3-7-pre.txt Normal file
View File

@ -0,0 +1,80 @@
3 7 19
0 0 -3.859900e+02 3.871200e+02
1 0 -3.844000e+01 4.921200e+02
2 0 -6.679200e+02 1.231100e+02
0 1 3.838800e+02 -1.529999e+01
1 1 5.597500e+02 -1.061500e+02
0 2 5.915500e+02 1.364400e+02
1 2 8.638600e+02 -2.346997e+01
2 2 4.947200e+02 1.125200e+02
0 3 5.925000e+02 1.257500e+02
1 3 8.610800e+02 -3.521997e+01
2 3 4.985400e+02 1.015600e+02
0 4 3.487200e+02 5.583800e+02
1 4 7.760300e+02 4.835300e+02
2 4 7.780029e+00 3.263500e+02
0 5 1.401001e+01 9.642001e+01
1 5 2.071300e+02 1.183600e+02
0 6 2.027600e+02 3.409900e+02
1 6 5.431801e+02 2.948100e+02
2 6 -5.841998e+01 1.108300e+02
-1.6943983532198115e-02
1.1171804676513932e-02
2.4643508831711991e-03
7.3030995682610689e-01
-2.6490818471043420e-01
-1.7127892627337182e+00
1.4300319432711681e+03
-7.5572758535864072e-08
3.2377569465570913e-14
1.5049725341485708e-02
-1.8504564785154357e-01
-2.9278402790141456e-01
-1.0590476152349551e+00
-3.6017862414345798e-02
-1.5720340175803784e+00
1.4321374541298685e+03
-7.3171919892612292e-08
3.1759419019880947e-14
-3.0793597986873011e-01
3.2077907982952031e-01
2.2253985096991455e-01
8.5034483295909009e+00
6.7499603629668741e+00
-3.6383814384447088e+00
1.5720470590375264e+03
-1.5962623661947355e-08
-1.6507904848058800e-14
-1.2055995050700867e+01
1.2838775976205760e+01
-4.1099369264082803e+01
6.4168905904672933e+00
3.8897031177598462e-01
-2.3586282709150449e+01
1.3051100355717297e+01
3.8387587111611952e+00
-2.9777932175344951e+01
1.3060946673472820e+01
3.5910521225905803e+00
-2.9759080795372942e+01
1.4265764475421857e+01
2.4096216156436530e+01
-5.4823971067225500e+01
-2.5292283211391348e-01
2.2166082122808284e+00
-2.1712127480255084e+01
7.6465738085189585e+00
1.4185331909846619e+01
-5.2070299568846060e+01

26
gtsam/navigation/ImuBias.h
View File

@ -26,12 +26,12 @@
/*
* NOTES:
* - Earth-rate correction:
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user).
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
* + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant.
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user).
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
* + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant.
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
*
* - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
* - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
*/
namespace gtsam {
@ -40,11 +40,11 @@ namespace gtsam {
namespace imuBias {
class ConstantBias : public DerivedValue<ConstantBias> {
private:
private:
Vector3 biasAcc_;
Vector3 biasGyro_;
public:
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 6;
@ -144,17 +144,17 @@ namespace imuBias {
/// return dimensionality of tangent space
inline size_t dim() const { return dimension; }
/** Update the LieVector with a tangent space update */
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** Update the LieVector with a tangent space update */
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** @return the local coordinates of another object */
inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); }
/** @return the local coordinates of another object */
inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); }
/// @}
/// @name Group
/// @{
/** identity for group operation */
/** identity for group operation */
static ConstantBias identity() { return ConstantBias(); }
/** invert the object and yield a new one */
@ -213,7 +213,7 @@ namespace imuBias {
/// @}
}; // ConstantBias class
}; // ConstantBias class
} // namespace ImuBias

26
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -136,12 +136,12 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero());
Matrix3::Zero(), Matrix3::Zero());
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
@ -193,13 +193,13 @@ TEST( CombinedImuFactor, ErrorWithBiases )
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -224,14 +224,14 @@ TEST( CombinedImuFactor, ErrorWithBiases )
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
EXPECT(assert_equal(H1e, H1a.topRows(9)));
EXPECT(assert_equal(H2e, H2a.topRows(9)));
EXPECT(assert_equal(H3e, H3a.topRows(9)));
EXPECT(assert_equal(H4e, H4a.topRows(9)));
EXPECT(assert_equal(H5e, H5a.topRows(9)));
EXPECT(assert_equal(H1e, H1a.topRows(9)));
EXPECT(assert_equal(H2e, H2a.topRows(9)));
EXPECT(assert_equal(H3e, H3a.topRows(9)));
EXPECT(assert_equal(H4e, H4a.topRows(9)));
EXPECT(assert_equal(H5e, H5a.topRows(9)));
}
/* ************************************************************************* */

6
gtsam/navigation/tests/testImuBias.cpp
View File

@ -25,8 +25,8 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( ImuBias, Constructor)
{
Vector bias_acc(Vector_(3,0.1,0.2,0.4));
Vector bias_gyro(Vector_(3, -0.2, 0.5, 0.03));
Vector bias_acc((Vector(3) << 0.1,0.2,0.4));
Vector bias_gyro((Vector(3) << -0.2, 0.5, 0.03));
// Default Constructor
gtsam::imuBias::ConstantBias bias1;
@ -39,5 +39,5 @@ TEST( ImuBias, Constructor)
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

8
gtsam/slam/BoundingConstraint.h
View File

@ -74,9 +74,9 @@ struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
}
if (isGreaterThan_)
return Vector_(1, error);
return (Vector(1) << error);
else
return -1.0 * Vector_(1, error);
return -1.0 * (Vector(1) << error);
}
private:
@ -147,9 +147,9 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
}
if (isGreaterThan_)
return Vector_(1, error);
return (Vector(1) << error);
else
return -1.0 * Vector_(1, error);
return -1.0 * (Vector(1) << error);
}
private:

54
gtsam/slam/EssentialMatrixFactor.h Normal file
View File

@ -0,0 +1,54 @@
/*
* @file EssentialMatrixFactor.cpp
* @brief EssentialMatrixFactor class
* @author Frank Dellaert
* @date December 17, 2013
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <iostream>
namespace gtsam {
/**
* Factor that evaluates epipolar error p'Ep for given essential matrix
*/
class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
Point2 pA_, pB_; ///< Measurements in image A and B
Vector vA_, vB_; ///< Homogeneous versions
typedef NoiseModelFactor1<EssentialMatrix> Base;
public:
/// Constructor
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model) :
Base(model, key), pA_(pA), pB_(pB), //
vA_(EssentialMatrix::Homogeneous(pA)), //
vB_(EssentialMatrix::Homogeneous(pB)) {
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << " EssentialMatrixFactor with measurements\n ("
<< pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
<< ")'" << std::endl;
}
/// vector of errors returns 1D vector
Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
boost::none) const {
return (Vector(1) << E.error(vA_, vB_, H));
}
};
} // gtsam

99
gtsam/slam/RotateFactor.h Normal file
View File

@ -0,0 +1,99 @@
/*
* @file RotateFactor.cpp
* @brief RotateFactor class
* @author Frank Dellaert
* @date December 17, 2013
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Rot3.h>
namespace gtsam {
/**
* Factor on unknown rotation iRC that relates two incremental rotations
* c1Rc2 = iRc' * i1Ri2 * iRc
* Which we can write (see doc/math.lyx)
* e^[z] = iRc' * e^[p] * iRc = e^([iRc'*p])
* with z and p measured and predicted angular velocities, and hence
* p = iRc * z
*/
class RotateFactor: public NoiseModelFactor1<Rot3> {
Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
typedef NoiseModelFactor1<Rot3> Base;
public:
/// Constructor
RotateFactor(Key key, const Rot3& P, const Rot3& Z,
const SharedNoiseModel& model) :
Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << "RotateFactor:" << std::endl;
p_.print("p");
z_.print("z");
}
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& R,
boost::optional<Matrix&> H = boost::none) const {
// predict p_ as q = R*z_, derivative H will be filled if not none
Point3 q = R.rotate(z_,H);
// error is just difference, and note derivative of that wrpt q is I3
return Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z();
}
};
/**
* Factor on unknown rotation R that relates two directions p_i = iRc * z_c
* Directions provide less constraints than a full rotation
*/
class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z
typedef NoiseModelFactor1<Rot3> Base;
public:
/// Constructor
RotateDirectionsFactor(Key key, const Sphere2& p, const Sphere2& z,
const SharedNoiseModel& model) :
Base(model, key), p_(p), z_(z) {
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << "RotateDirectionsFactor:" << std::endl;
p_.print("p");
z_.print("z");
}
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& R,
boost::optional<Matrix&> H = boost::none) const {
Sphere2 q = R * z_;
Vector e = p_.error(q, H);
if (H) {
Matrix DR;
R.rotate(z_, DR);
*H = (*H) * DR;
}
return e;
}
};
} // gtsam

334
gtsam/slam/dataset.cpp
View File

@ -12,7 +12,7 @@
/**
* @file dataset.cpp
* @date Jan 22, 2010
* @author nikai
* @author nikai, Luca Carlone
* @brief utility functions for loading datasets
*/
@ -50,6 +50,7 @@ string findExampleDataFile(const string& name) {
namesToSearch.push_back(name);
namesToSearch.push_back(name + ".graph");
namesToSearch.push_back(name + ".txt");
namesToSearch.push_back(name + ".out");
// Find first name that exists
BOOST_FOREACH(const fs::path& root, rootsToSearch) {
@ -61,10 +62,10 @@ string findExampleDataFile(const string& name) {
// If we did not return already, then we did not find the file
throw std::invalid_argument(
"gtsam::findExampleDataFile could not find a matching file in\n"
SOURCE_TREE_DATASET_DIR " or\n"
INSTALLED_DATASET_DIR " named\n" +
name + ", " + name + ".graph, or " + name + ".txt");
"gtsam::findExampleDataFile could not find a matching file in\n"
SOURCE_TREE_DATASET_DIR " or\n"
INSTALLED_DATASET_DIR " named\n" +
name + ", " + name + ".graph, or " + name + ".txt");
}
#endif
@ -150,7 +151,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
if (!model) {
Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2));
Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2));
model = noiseModel::Diagonal::Variances(variances, smart);
}
@ -178,7 +179,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
continue;
noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
graph->add(BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise));
// Insert poses or points if they do not exist yet
@ -210,13 +211,13 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
{
double rangeVar = v1;
double bearingVar = v1 / 10.0;
measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, bearingVar, rangeVar));
measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar));
}
else
{
if(!haveLandmark) {
cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
"non-uniform covariance on LANDMARK measurements in this file." << endl;
"non-uniform covariance on LANDMARK measurements in this file." << endl;
haveLandmark = true;
}
}
@ -228,7 +229,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
}
cout << "load2D read a graph file with " << initial->size()
<< " vertices and " << graph->nrFactors() << " factors" << endl;
<< " vertices and " << graph->nrFactors() << " factors" << endl;
return make_pair(graph, initial);
}
@ -385,7 +386,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
continue;
noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
graph->add(BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise));
// Insert poses or points if they do not exist yet
@ -402,9 +403,318 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
}
cout << "load2D read a graph file with " << initial->size()
<< " vertices and " << graph->nrFactors() << " factors" << endl;
<< " vertices and " << graph->nrFactors() << " factors" << endl;
return make_pair(graph, initial);
}
/* ************************************************************************* */
Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL
/* R = [ 1 0 0
* 0 -1 0
* 0 0 -1]
*/
Matrix3 R_mat = Matrix3::Zero(3,3);
R_mat(0,0) = 1.0; R_mat(1,1) = -1.0; R_mat(2,2) = -1.0;
return Rot3(R_mat);
}
/* ************************************************************************* */
Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz)
{
Rot3 R90 = openGLFixedRotation();
Rot3 wRc = ( R.inverse() ).compose(R90);
// Our camera-to-world translation wTc = -R'*t
return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz)));
}
/* ************************************************************************* */
Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz)
{
Rot3 R90 = openGLFixedRotation();
Rot3 cRw_openGL = R90.compose( R.inverse() );
Point3 t_openGL = cRw_openGL.rotate(Point3(-tx,-ty,-tz));
return Pose3(cRw_openGL, t_openGL);
}
/* ************************************************************************* */
Pose3 gtsam2openGL(const Pose3& PoseGTSAM)
{
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z());
}
/* ************************************************************************* */
bool readBundler(const string& filename, SfM_data &data)
{
// Load the data file
ifstream is(filename.c_str(),ifstream::in);
if(!is)
{
cout << "Error in readBundler: can not find the file!!" << endl;
return false;
}
// Ignore the first line
char aux[500];
is.getline(aux,500);
// Get the number of camera poses and 3D points
size_t nrPoses, nrPoints;
is >> nrPoses >> nrPoints;
// Get the information for the camera poses
for( size_t i = 0; i < nrPoses; i++ )
{
// Get the focal length and the radial distortion parameters
float f, k1, k2;
is >> f >> k1 >> k2;
Cal3Bundler K(f, k1, k2);
// Get the rotation matrix
float r11, r12, r13;
float r21, r22, r23;
float r31, r32, r33;
is >> r11 >> r12 >> r13
>> r21 >> r22 >> r23
>> r31 >> r32 >> r33;
// Bundler-OpenGL rotation matrix
Rot3 R(
r11, r12, r13,
r21, r22, r23,
r31, r32, r33);
// Check for all-zero R, in which case quit
if(r11==0 && r12==0 && r13==0)
{
cout << "Error in readBundler: zero rotation matrix for pose " << i << endl;
return false;
}
// Get the translation vector
float tx, ty, tz;
is >> tx >> ty >> tz;
Pose3 pose = openGL2gtsam(R,tx,ty,tz);
data.cameras.push_back(SfM_Camera(pose,K));
}
// Get the information for the 3D points
for( size_t j = 0; j < nrPoints; j++ )
{
SfM_Track track;
// Get the 3D position
float x, y, z;
is >> x >> y >> z;
track.p = Point3(x,y,z);
// Get the color information
float r, g, b;
is >> r >> g >> b;
track.r = r/255.f;
track.g = g/255.f;
track.b = b/255.f;
// Now get the visibility information
size_t nvisible = 0;
is >> nvisible;
for( size_t k = 0; k < nvisible; k++ )
{
size_t cam_idx = 0, point_idx = 0;
float u, v;
is >> cam_idx >> point_idx >> u >> v;
track.measurements.push_back(make_pair(cam_idx,Point2(u,-v)));
}
data.tracks.push_back(track);
}
is.close();
return true;
}
/* ************************************************************************* */
bool readBAL(const string& filename, SfM_data &data)
{
// Load the data file
ifstream is(filename.c_str(),ifstream::in);
if(!is)
{
cout << "Error in readBAL: can not find the file!!" << endl;
return false;
}
// Get the number of camera poses and 3D points
size_t nrPoses, nrPoints, nrObservations;
is >> nrPoses >> nrPoints >> nrObservations;
data.tracks.resize(nrPoints);
// Get the information for the observations
for( size_t k = 0; k < nrObservations; k++ )
{
size_t i = 0, j = 0;
float u, v;
is >> i >> j >> u >> v;
data.tracks[j].measurements.push_back(make_pair(i,Point2(u,-v)));
}
// Get the information for the camera poses
for( size_t i = 0; i < nrPoses; i++ )
{
// Get the rodriguez vector
float wx, wy, wz;
is >> wx >> wy >> wz;
Rot3 R = Rot3::rodriguez(wx, wy, wz);// BAL-OpenGL rotation matrix
// Get the translation vector
float tx, ty, tz;
is >> tx >> ty >> tz;
Pose3 pose = openGL2gtsam(R,tx,ty,tz);
// Get the focal length and the radial distortion parameters
float f, k1, k2;
is >> f >> k1 >> k2;
Cal3Bundler K(f, k1, k2);
data.cameras.push_back(SfM_Camera(pose,K));
}
// Get the information for the 3D points
for( size_t j = 0; j < nrPoints; j++ )
{
// Get the 3D position
float x, y, z;
is >> x >> y >> z;
SfM_Track& track = data.tracks[j];
track.p = Point3(x,y,z);
track.r = 0.4f;
track.g = 0.4f;
track.b = 0.4f;
}
is.close();
return true;
}
/* ************************************************************************* */
bool writeBAL(const string& filename, SfM_data &data)
{
// Open the output file
ofstream os;
os.open(filename.c_str());
os.precision(20);
if (!os.is_open()) {
cout << "Error in writeBAL: can not open the file!!" << endl;
return false;
}
// Write the number of camera poses and 3D points
size_t nrObservations=0;
for (size_t j = 0; j < data.number_tracks(); j++){
nrObservations += data.tracks[j].number_measurements();
}
// Write observations
os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl;
os << endl;
for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j
SfM_Track track = data.tracks[j];
for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j
size_t i = track.measurements[k].first; // camera id
double u0 = data.cameras[i].calibration().u0();
double v0 = data.cameras[i].calibration().v0();
if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;}
double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin
double pixelBALy = - (track.measurements[k].second.y() - v0); // center of image is the origin
Point2 pixelMeasurement(pixelBALx, pixelBALy);
os << i /*camera id*/ << " " << j /*point id*/ << " "
<< pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl;
}
}
os << endl;
// Write cameras
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
Pose3 poseGTSAM = data.cameras[i].pose();
Cal3Bundler cameraCalibration = data.cameras[i].calibration();
Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
os << poseOpenGL.translation().vector() << endl;
os << cameraCalibration.fx() << endl;
os << cameraCalibration.k1() << endl;
os << cameraCalibration.k2() << endl;
os << endl;
}
// Write the points
for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j
Point3 point = data.tracks[j].p;
os << point.x() << endl;
os << point.y() << endl;
os << point.z() << endl;
os << endl;
}
os.close();
return true;
}
bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
// CHECKS
Values valuesPoses = values.filter<Pose3>();
if( valuesPoses.size() != data.number_cameras()){
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
<<") and values (#cameras " << valuesPoses.size() << ")!!" << endl;
return false;
}
Values valuesPoints = values.filter<Point3>();
if( valuesPoints.size() != data.number_tracks()){
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
}
if(valuesPoints.size() + valuesPoses.size() != values.size()){
cout << "writeBALfromValues write only poses and points values!!" << endl;
return false;
}
if(valuesPoints.size()==0 || valuesPoses.size()==0){
cout << "writeBALfromValues: No point or pose in values!!" << endl;
return false;
}
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
Key poseKey = symbol('x',i);
Pose3 pose = values.at<Pose3>(poseKey);
Cal3Bundler K = data.cameras[i].calibration();
PinholeCamera<Cal3Bundler> camera(pose, K);
data.cameras[i] = camera;
}
for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
Key pointKey = symbol('l',j);
if(values.exists(pointKey)){
Point3 point = values.at<Point3>(pointKey);
data.tracks[j].p = point;
}else{
data.tracks[j].r = 1.0;
data.tracks[j].g = 0.0;
data.tracks[j].b = 0.0;
data.tracks[j].p = Point3();
}
}
return writeBAL(filename, data);
}
} // \namespace gtsam

187
gtsam/slam/dataset.h
View File

@ -12,7 +12,7 @@
/**
* @file dataset.h
* @date Jan 22, 2010
* @author nikai
* @author nikai, Luca Carlone
* @brief utility functions for loading datasets
*/
@ -20,62 +20,157 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <vector>
#include <utility> // for pair
#include <string>
namespace gtsam {
#ifndef MATLAB_MEX_FILE
/**
* Find the full path to an example dataset distributed with gtsam. The name
* may be specified with or without a file extension - if no extension is
* give, this function first looks for the .graph extension, then .txt. We
* first check the gtsam source tree for the file, followed by the installed
* example dataset location. Both the source tree and installed locations
* are obtained from CMake during compilation.
* @return The full path and filename to the requested dataset.
* @throw std::invalid_argument if no matching file could be found using the
* search process described above.
*/
GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
/**
* Find the full path to an example dataset distributed with gtsam. The name
* may be specified with or without a file extension - if no extension is
* give, this function first looks for the .graph extension, then .txt. We
* first check the gtsam source tree for the file, followed by the installed
* example dataset location. Both the source tree and installed locations
* are obtained from CMake during compilation.
* @return The full path and filename to the requested dataset.
* @throw std::invalid_argument if no matching file could be found using the
* search process described above.
*/
GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
#endif
/**
* Load TORO 2D Graph
* @param dataset/model pair as constructed by [dataset]
* @param maxID if non-zero cut out vertices >= maxID
* @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID = 0, bool addNoise = false, bool smart = true);
/**
* Load TORO 2D Graph
* @param dataset/model pair as constructed by [dataset]
* @param maxID if non-zero cut out vertices >= maxID
* @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID = 0, bool addNoise = false, bool smart = true);
/**
* Load TORO 2D Graph
* @param filename
* @param model optional noise model to use instead of one specified by file
* @param maxID if non-zero cut out vertices >= maxID
* @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<
noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
bool smart = true);
/**
* Load TORO 2D Graph
* @param filename
* @param model optional noise model to use instead of one specified by file
* @param maxID if non-zero cut out vertices >= maxID
* @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<
noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
bool smart = true);
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
const std::string& filename,
gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
const std::string& filename,
gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
const noiseModel::Diagonal::shared_ptr model, const std::string& filename);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
const noiseModel::Diagonal::shared_ptr model, const std::string& filename);
/**
* Load TORO 3D Graph
*/
GTSAM_EXPORT bool load3D(const std::string& filename);
/**
* Load TORO 3D Graph
*/
GTSAM_EXPORT bool load3D(const std::string& filename);
/// A measurement with its camera index
typedef std::pair<size_t,gtsam::Point2> SfM_Measurement;
/// Define the structure for the 3D points
struct SfM_Track
{
gtsam::Point3 p; ///< 3D position of the point
float r,g,b; ///< RGB color of the 3D point
std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
size_t number_measurements() const { return measurements.size();}
};
/// Define the structure for the camera poses
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera;
/// Define the structure for SfM data
struct SfM_data
{
std::vector<SfM_Camera> cameras; ///< Set of cameras
std::vector<SfM_Track> tracks; ///< Sparse set of points
size_t number_cameras() const { return cameras.size();} ///< The number of camera poses
size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points
};
/**
* @brief This function parses a bundler output file and stores the data into a
* SfM_data structure
* @param filename The name of the bundler file
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
* SfM_data structure
* @param filename The name of the BAL file
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool readBAL(const std::string& filename, SfM_data &data);
/**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
* SfM_data structure
* @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
/**
* @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
* SfM_data structure and a value structure (measurements are the same as the SfM input data,
* while camera poses and values are read from Values)
* @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored
* @param values structure where the graph values are stored
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values);
/**
* @brief This function converts an openGL camera pose to an GTSAM camera pose
* @param R rotation in openGL
* @param tx x component of the translation in openGL
* @param ty y component of the translation in openGL
* @param tz z component of the translation in openGL
* @return Pose3 in GTSAM format
*/
GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
/**
* @brief This function converts a GTSAM camera pose to an openGL camera pose
* @param R rotation in GTSAM
* @param tx x component of the translation in GTSAM
* @param ty y component of the translation in GTSAM
* @param tz z component of the translation in GTSAM
* @return Pose3 in openGL format
*/
GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
/**
* @brief This function converts a GTSAM camera pose to an openGL camera pose
* @param PoseGTSAM pose in GTSAM format
* @return Pose3 in openGL format
*/
GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
} // namespace gtsam

2
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -72,7 +72,7 @@ TEST( AntiFactor, NegativeHessian)
size_t variable_count = originalFactor->size();
for(size_t i = 0; i < variable_count; ++i){
for(size_t j = i; j < variable_count; ++j){
Matrix expected_G = -originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j);
Matrix expected_G = -Matrix(originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j));
Matrix actual_G = antiHessian->info(antiHessian->begin()+i, antiHessian->begin()+j);
CHECK(assert_equal(expected_G, actual_G, 1e-5));
}

4
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -1,8 +1,8 @@
/**
* @file testBetweenFactor.cpp
* @file testBetweenFactor.cpp
* @brief
* @author Duy-Nguyen Ta
* @date Aug 2, 2013
* @date Aug 2, 2013
*/
#include <gtsam/base/numericalDerivative.h>

183
gtsam/slam/tests/testDataset.cpp
View File

@ -12,7 +12,7 @@
/**
* @file testDataset.cpp
* @brief Unit test for dataset.cpp
* @author Richard Roberts
* @author Richard Roberts, Luca Carlone
*/
#include <CppUnitLite/TestHarness.h>
@ -20,11 +20,13 @@
#include <boost/algorithm/string.hpp>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/slam/dataset.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(dataSet, findExampleDataFile) {
const string expected_end = "examples/Data/example.graph";
const string actual = findExampleDataFile("example");
@ -33,6 +35,185 @@ TEST(dataSet, findExampleDataFile) {
EXPECT(assert_equal(expected_end, actual_end));
}
/* ************************************************************************* */
TEST( dataSet, Balbianello)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("Balbianello");
SfM_data mydata;
CHECK(readBundler(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
const SfM_Track& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfM_Camera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,1));
}
/* ************************************************************************* */
TEST( dataSet, readBAL_Dubrovnik)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data mydata;
CHECK(readBAL(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
EXPECT_LONGS_EQUAL(7,mydata.number_tracks());
const SfM_Track& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfM_Camera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,12));
}
/* ************************************************************************* */
TEST( dataSet, openGL2gtsam)
{
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(0.0,0.0,0.0);
Pose3 poseGTSAM = Pose3(R,t);
Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns!
Rot3 cRw(
r1.x(), r2.x(), r3.x(),
-r1.y(), -r2.y(), -r3.y(),
-r1.z(), -r2.z(), -r3.z());
Rot3 wRc = cRw.inverse();
Pose3 actual = Pose3(wRc,t);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( dataSet, gtsam2openGL)
{
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(1.0,20.0,10.0);
Pose3 actual = Pose3(R,t);
Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
Pose3 expected = gtsam2openGL(poseGTSAM);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( dataSet, writeBAL_Dubrovnik)
{
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data readData;
readBAL(filenameToRead, readData);
// Write readData to file filenameToWrite
const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten");
CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote
SfM_data writtenData;
CHECK(readBAL(filenameToWrite, writtenData));
// Check that what we read is the same as what we wrote
EXPECT(assert_equal(readData.number_cameras(),writtenData.number_cameras()));
EXPECT(assert_equal(readData.number_tracks(),writtenData.number_tracks()));
for (size_t i = 0; i < readData.number_cameras(); i++){
PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
EXPECT(assert_equal(expectedCamera,actualCamera));
}
for (size_t j = 0; j < readData.number_tracks(); j++){
// check point
SfM_Track expectedTrack = writtenData.tracks[j];
SfM_Track actualTrack = readData.tracks[j];
Point3 expectedPoint = expectedTrack.p;
Point3 actualPoint = actualTrack.p;
EXPECT(assert_equal(expectedPoint,actualPoint));
// check rgb
Point3 expectedRGB = Point3( expectedTrack.r, expectedTrack.g, expectedTrack.b );
Point3 actualRGB = Point3( actualTrack.r, actualTrack.g, actualTrack.b);
EXPECT(assert_equal(expectedRGB,actualRGB));
// check measurements
for (size_t k = 0; k < actualTrack.number_measurements(); k++){
EXPECT(assert_equal(expectedTrack.measurements[k].first,actualTrack.measurements[k].first));
EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second));
}
}
}
/* ************************************************************************* */
TEST( dataSet, writeBALfromValues_Dubrovnik){
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data readData;
readBAL(filenameToRead, readData);
Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
Values value;
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
Key poseKey = symbol('x',i);
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(poseKey, pose);
}
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Key pointKey = symbol('l',j);
Point3 point = poseChange.transform_from( readData.tracks[j].p );
value.insert(pointKey, point);
}
// Write values and readData to a file
const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten");
writeBALfromValues(filenameToWrite, readData, value);
// Read the file we wrote
SfM_data writtenData;
readBAL(filenameToWrite, writtenData);
// Check that the reprojection errors are the same and the poses are correct
// Check number of things
EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
const SfM_Track& track0 = writtenData.tracks[0];
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
const SfM_Camera& camera0 = writtenData.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,12));
Pose3 expectedPose = camera0.pose();
Key poseKey = symbol('x',0);
Pose3 actualPose = value.at<Pose3>(poseKey);
EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Key pointKey = symbol('l',0);
Point3 actualPoint = value.at<Point3>(pointKey);
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

147
gtsam/slam/tests/testEssentialMatrixFactor.cpp Normal file
View File

@ -0,0 +1,147 @@
/*
* @file testEssentialMatrixFactor.cpp
* @brief Test EssentialMatrixFactor class
* @author Frank Dellaert
* @date December 17, 2013
*/
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
const string filename = findExampleDataFile("5pointExample1.txt");
SfM_data data;
bool readOK = readBAL(filename, data);
Rot3 aRb = data.cameras[1].pose().rotation();
Point3 aTb = data.cameras[1].pose().translation();
Point2 pA(size_t i) {
return data.tracks[i].measurements[0].second;
}
Point2 pB(size_t i) {
return data.tracks[i].measurements[1].second;
}
Vector vA(size_t i) {
return EssentialMatrix::Homogeneous(pA(i));
}
Vector vB(size_t i) {
return EssentialMatrix::Homogeneous(pB(i));
}
//*************************************************************************
TEST (EssentialMatrixFactor, testData) {
CHECK(readOK);
// Check E matrix
Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
Matrix aEb_matrix = skewSymmetric(aTb.x(), aTb.y(), aTb.z()) * aRb.matrix();
EXPECT(assert_equal(expected, aEb_matrix,1e-8));
// Check some projections
EXPECT(assert_equal(Point2(0,0),pA(0),1e-8));
EXPECT(assert_equal(Point2(0,0.1),pB(0),1e-8));
EXPECT(assert_equal(Point2(0,-1),pA(4),1e-8));
EXPECT(assert_equal(Point2(-1,0.2),pB(4),1e-8));
// Check homogeneous version
EXPECT(assert_equal((Vector(3) << -1,0.2,1),vB(4),1e-8));
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);
// Check epipolar constraint
EssentialMatrix trueE(aRb, aTb);
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i),vB(i)), 1e-7);
}
//*************************************************************************
TEST (EssentialMatrixFactor, factor) {
EssentialMatrix trueE(aRb, aTb);
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor factor(1, pA(i), pB(i), model);
// Check evaluation
Vector expected(1);
expected << 0;
Matrix HActual;
Vector actual = factor.evaluateError(trueE, HActual);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix HExpected;
HExpected = numericalDerivative11<EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE);
// Verify the Jacobian is correct
EXPECT(assert_equal(HExpected, HActual, 1e-8));
}
}
//*************************************************************************
TEST (EssentialMatrixFactor, fromConstraints) {
// Here we want to optimize directly on essential matrix constraints
// Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
// but GTSAM does the equivalent anyway, provided we give the right
// factors. In this case, the factors are the constraints.
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1,
0.01);
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model));
// Check error at ground truth
Values truth;
EssentialMatrix trueE(aRb, aTb);
truth.insert(1, trueE);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1));
initial.insert(1, initialE);
EXPECT_DOUBLES_EQUAL(640, graph.error(initial), 1e-2);
// Optimize
LevenbergMarquardtParams parameters;
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(1);
EXPECT(assert_equal(trueE, actual,1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
// Check errors individually
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i),vB(i)), 1e-6);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
TEST( GeneralSFMFactor, equals )
{
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
Vector z = (Vector(2) << 323.,240.);
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) {
Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values)));
}
static const double baseline = 5.0 ;
@ -309,7 +309,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
}
else {
Vector delta = Vector_(11,
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

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

@ -87,7 +87,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
TEST( GeneralSFMFactor_Cal3Bundler, equals )
{
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
Vector z = (Vector(2) << 323.,240.);
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
@ -112,7 +112,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) {
Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values)));
}
@ -309,7 +309,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
}
else {
Vector delta = Vector_(9,
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

10
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -18,8 +18,8 @@
using namespace gtsam;
const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas(Vector_(1, 0.1));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
typedef PoseRotationPrior<Pose2> Pose2RotationPrior;
typedef PoseRotationPrior<Pose3> Pose3RotationPrior;
@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1;
// Pose3 examples
const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector_(3, 0.1, 0.2, 0.3));
const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3) << 0.1, 0.2, 0.3));
// Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
@ -62,7 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A);
Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1;
EXPECT(assert_equal(Vector_(3,-0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
@ -84,7 +84,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A);
Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1;
EXPECT(assert_equal(Vector_(1,-M_PI_2), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));

12
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -15,8 +15,8 @@
using namespace gtsam;
const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3));
typedef PoseTranslationPrior<Pose2> Pose2TranslationPrior;
typedef PoseTranslationPrior<Pose3> Pose3TranslationPrior;
@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
@ -81,7 +81,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3 pose1(rot3B, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
@ -103,7 +103,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3 pose1(rot3C, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
@ -125,7 +125,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A);
Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1;
EXPECT(assert_equal(Vector_(2,-3.0,-4.0), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));

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

@ -107,7 +107,7 @@ TEST( ProjectionFactor, Error ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = Vector_(2, -3.0, 0.0);
Vector expectedError = (Vector(2) << -3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -130,7 +130,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = Vector_(2, -3.0, 0.0);
Vector expectedError = (Vector(2) << -3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -153,8 +153,8 @@ TEST( ProjectionFactor, Jacobian ) {
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
Matrix H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.);
Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
@ -179,8 +179,8 @@ TEST( ProjectionFactor, JacobianWithTransform ) {
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = Matrix_(2, 6, -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
Matrix H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376);
Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));

8
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -117,7 +117,7 @@ TEST( RangeFactor, Error2D ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = Vector_(1, 0.295630141);
Vector expectedError = (Vector(1) << 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -142,7 +142,7 @@ TEST( RangeFactor, Error2DWithTransform ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = Vector_(1, 0.295630141);
Vector expectedError = (Vector(1) << 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -164,7 +164,7 @@ TEST( RangeFactor, Error3D ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = Vector_(1, 0.295630141);
Vector expectedError = (Vector(1) << 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -189,7 +189,7 @@ TEST( RangeFactor, Error3DWithTransform ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = Vector_(1, 0.295630141);
Vector expectedError = (Vector(1) << 0.295630141);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));

185
gtsam/slam/tests/testRotateFactor.cpp Normal file
View File

@ -0,0 +1,185 @@
/*
* @file testRotateFactor.cpp
* @brief Test RotateFactor class
* @author Frank Dellaert
* @date December 17, 2013
*/
#include <gtsam/slam/RotateFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <vector>
using namespace std;
using namespace boost::assign;
using namespace gtsam;
//*************************************************************************
// Create some test data
// Let's assume IMU is aligned with aero (X-forward,Z down)
// And camera is looking forward.
Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
Rot3 iRc(cameraX, cameraY, cameraZ);
// Now, let's create some rotations around IMU frame
Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), //
i2Ri3 = Rot3::rodriguez(p2, 1), //
i3Ri4 = Rot3::rodriguez(p3, 1);
// The corresponding rotations in the camera frame
Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
c2Zc3 = iRc.inverse() * i2Ri3 * iRc, //
c3Zc4 = iRc.inverse() * i3Ri4 * iRc;
// The corresponding rotated directions in the camera frame
Sphere2 z1 = iRc.inverse() * p1, //
z2 = iRc.inverse() * p2, //
z3 = iRc.inverse() * p3;
typedef noiseModel::Isotropic::shared_ptr Model;
//*************************************************************************
TEST (RotateFactor, checkMath) {
EXPECT(assert_equal(c1Zc2, Rot3::rodriguez(z1, 1)));
EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1)));
EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1)));
}
//*************************************************************************
TEST (RotateFactor, test) {
Model model = noiseModel::Isotropic::Sigma(3, 0.01);
RotateFactor f(1, i1Ri2, c1Zc2, model);
EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8));
Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867);
EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
}
//*************************************************************************
TEST (RotateFactor, minimization) {
// Let's try to recover the correct iRc by minimizing
NonlinearFactorGraph graph;
Model model = noiseModel::Isotropic::Sigma(3, 0.01);
graph.add(RotateFactor(1, i1Ri2, c1Zc2, model));
graph.add(RotateFactor(1, i2Ri3, c2Zc3, model));
graph.add(RotateFactor(1, i3Ri4, c3Zc4, model));
// Check error at ground truth
Values truth;
truth.insert(1, iRc);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
double degree = M_PI / 180;
Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
initial.insert(1, initialE);
EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1);
// Optimize
LevenbergMarquardtParams parameters;
//parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
Rot3 actual = result.at<Rot3>(1);
EXPECT(assert_equal(iRc, actual,1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
//*************************************************************************
TEST (RotateDirectionsFactor, test) {
Model model = noiseModel::Isotropic::Sigma(2, 0.01);
RotateDirectionsFactor f(1, p1, z1, model);
EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8));
Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
Vector expectedE = (Vector(2) << -0.08867, -0.20197);
EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
}
//*************************************************************************
TEST (RotateDirectionsFactor, minimization) {
// Let's try to recover the correct iRc by minimizing
NonlinearFactorGraph graph;
Model model = noiseModel::Isotropic::Sigma(2, 0.01);
graph.add(RotateDirectionsFactor(1, p1, z1, model));
graph.add(RotateDirectionsFactor(1, p2, z2, model));
graph.add(RotateDirectionsFactor(1, p3, z3, model));
// Check error at ground truth
Values truth;
truth.insert(1, iRc);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
double degree = M_PI / 180;
Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
initial.insert(1, initialE);
EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1);
// Optimize
LevenbergMarquardtParams parameters;
//parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
Rot3 actual = result.at<Rot3>(1);
EXPECT(assert_equal(iRc, actual,1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

14
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -31,7 +31,7 @@
using namespace std;
using namespace gtsam;
static Pose3 camera1(Matrix_(3,3,
static Pose3 camera1((Matrix) (Matrix(3, 3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
@ -102,7 +102,7 @@ TEST( StereoFactor, Error ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
Vector expectedError = Vector_(3, -3.0, +2.0, -1.0);
Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -123,7 +123,7 @@ TEST( StereoFactor, ErrorWithTransform ) {
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
Vector expectedError = Vector_(3, -3.0, +2.0, -1.0);
Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -144,10 +144,10 @@ TEST( StereoFactor, Jacobian ) {
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = Matrix_(3, 6, 0.0, -625.0, 0.0, -100.0, 0.0, 0.0,
Matrix H1Expected = (Matrix(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0,
0.0, -625.0, 0.0, -100.0, 0.0, -8.0,
625.0, 0.0, 0.0, 0.0, -100.0, 0.0);
Matrix H2Expected = Matrix_(3, 3, 100.0, 0.0, 0.0,
Matrix H2Expected = (Matrix(3, 3) << 100.0, 0.0, 0.0,
100.0, 0.0, 8.0,
0.0, 100.0, 0.0);
@ -172,10 +172,10 @@ TEST( StereoFactor, JacobianWithTransform ) {
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = Matrix_(3, 6, -100.0, 0.0, 650.0, 0.0, 100.0, 0.0,
Matrix H1Expected = (Matrix(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0,
-100.0, -8.0, 649.2, -8.0, 100.0, 0.0,
-10.0, -650.0, 0.0, 0.0, 0.0, 100.0);
Matrix H2Expected = Matrix_(3, 3, 0.0, -100.0, 0.0,
Matrix H2Expected = (Matrix(3, 3) << 0.0, -100.0, 0.0,
8.0, -100.0, 0.0,
0.0, 0.0, -100.0);