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

resolving merge conflict

release/4.3a0
Akshay Krishnan 2021-06-17 01:42:51 +00:00
parent 545dfd0be7 c4582941bf
commit d9a8111bbd
3 changed files with 110 additions and 254 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
examples/CreateSFMExampleData.cpp
View File

@ -15,8 +15,8 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/slam/dataset.h>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
@ -27,17 +27,15 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
void createExampleBALFile(const string& filename, const vector<Point3>& P, void createExampleBALFile(const string& filename, const vector<Point3>& P,
const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K = const Pose3& pose1, const Pose3& pose2,
Cal3Bundler()) { const Cal3Bundler& K = Cal3Bundler()) {
// Class that will gather all data // Class that will gather all data
SfmData data; SfmData data;
// Create two cameras and add them to data // Create two cameras and add them to data
data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose1, K));
data.cameras.push_back(SfmCamera(pose2, K)); data.cameras.push_back(SfmCamera(pose2, K));
for(const Point3& p: P) { for (const Point3& p : P) {
// Create the track // Create the track
SfmTrack track; SfmTrack track;
track.p = p; track.p = p;
@ -47,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
// Project points in both cameras // Project points in both cameras
for (size_t i = 0; i < 2; i++) for (size_t i = 0; i < 2; i++)
track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
// Add track to data // Add track to data
data.tracks.push_back(track); data.tracks.push_back(track);
@ -59,7 +57,6 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
/* ************************************************************************* */ /* ************************************************************************* */
void create5PointExample1() { void create5PointExample1() {
// Create two cameras poses // Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2); Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0); Point3 aTb(0.1, 0, 0);
@ -67,8 +64,8 @@ void create5PointExample1() {
// Create test data, we need at least 5 points // Create test data, we need at least 5 points
vector<Point3> P; vector<Point3> P;
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
// Assumes example is run in ${GTSAM_TOP}/build/examples // Assumes example is run in ${GTSAM_TOP}/build/examples
const string filename = "../../examples/Data/5pointExample1.txt"; const string filename = "../../examples/Data/5pointExample1.txt";
@ -78,7 +75,6 @@ void create5PointExample1() {
/* ************************************************************************* */ /* ************************************************************************* */
void create5PointExample2() { void create5PointExample2() {
// Create two cameras poses // Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2); Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(10, 0, 0); Point3 aTb(10, 0, 0);
@ -86,20 +82,19 @@ void create5PointExample2() {
// Create test data, we need at least 5 points // Create test data, we need at least 5 points
vector<Point3> P; vector<Point3> P;
P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80); Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80),
Point3(20, -50, 80);
// Assumes example is run in ${GTSAM_TOP}/build/examples // Assumes example is run in ${GTSAM_TOP}/build/examples
const string filename = "../../examples/Data/5pointExample2.txt"; const string filename = "../../examples/Data/5pointExample2.txt";
Cal3Bundler K(500, 0, 0); Cal3Bundler K(500, 0, 0);
createExampleBALFile(filename, P, pose1, pose2,K); createExampleBALFile(filename, P, pose1, pose2, K);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void create18PointExample1() { void create18PointExample1() {
// Create two cameras poses // Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2); Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0); Point3 aTb(0.1, 0, 0);
@ -107,11 +102,11 @@ void create18PointExample1() {
// Create test data, we need 15 points // Create test data, we need 15 points
vector<Point3> P; vector<Point3> P;
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5), Point3(-1, -0.5, 2), // Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5), Point3(-1, -0.5, 2), //
Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5),// Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5), //
Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), // Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), //
Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5), // Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5), //
Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5);
// Assumes example is run in ${GTSAM_TOP}/build/examples // Assumes example is run in ${GTSAM_TOP}/build/examples
@ -119,8 +114,6 @@ void create18PointExample1() {
createExampleBALFile(filename, P, pose1, pose2); createExampleBALFile(filename, P, pose1, pose2);
} }
/* ************************************************************************* */
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
create5PointExample1(); create5PointExample1();
create5PointExample2(); create5PointExample2();
@ -129,4 +122,3 @@ int main(int argc, char* argv[]) {
} }
/* ************************************************************************* */ /* ************************************************************************* */

131
examples/data/18pointExample1.txt
View File

@ -1,131 +0,0 @@
2 18 36
0 0 -0.10000000000000000555 0.5
1 0 -0.5 -0.19999999999999998335
0 1 -0.10000000000000000555 -0
1 1 -1.2246467991473532688e-17 -0.2000000000000000111
0 2 -0.10000000000000000555 -0.5
1 2 0.5 -0.20000000000000003886
0 3 0 0.5
1 3 -0.5 -0.099999999999999977796
0 4 0 -0
1 4 -6.123233995736766344e-18 -0.10000000000000000555
0 5 0 -0.5
1 5 0.5 -0.10000000000000003331
0 6 0.10000000000000000555 0.5
1 6 -0.5 3.0616169978683830179e-17
0 7 0.10000000000000000555 -0
1 7 0 -0
0 8 0.10000000000000000555 -0.5
1 8 0.5 -3.0616169978683830179e-17
0 9 -0.2000000000000000111 1
1 9 -1 -0.39999999999999996669
0 10 -0.2000000000000000111 -0
1 10 -2.4492935982947065376e-17 -0.4000000000000000222
0 11 -0.2000000000000000111 -1
1 11 1 -0.40000000000000007772
0 12 0 1
1 12 -1 -0.19999999999999995559
0 13 0 -0
1 13 -1.2246467991473532688e-17 -0.2000000000000000111
0 14 0 -1
1 14 1 -0.20000000000000006661
0 15 0.2000000000000000111 1
1 15 -1 6.1232339957367660359e-17
0 16 0.2000000000000000111 -0
1 16 0 -0
0 17 0.2000000000000000111 -1
1 17 1 -6.1232339957367660359e-17
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.10000000000000000555
-0.5
1
-0.10000000000000000555
0
1
-0.10000000000000000555
0.5
1
0
-0.5
1
0
0
1
0
0.5
1
0.10000000000000000555
-0.5
1
0.10000000000000000555
0
1
0.10000000000000000555
0.5
1
-0.10000000000000000555
-0.5
0.5
-0.10000000000000000555
0
0.5
-0.10000000000000000555
0.5
0.5
0
-0.5
0.5
0
0
0.5
0
0.5
0.5
0.10000000000000000555
-0.5
0.5
0.10000000000000000555
0
0.5
0.10000000000000000555
0.5
0.5

193
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -5,26 +5,24 @@
* @date December 17, 2013 * @date December 17, 2013
*/ */
#include <gtsam/slam/EssentialMatrixFactor.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <CppUnitLite/TestHarness.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Noise model for first type of factor is evaluating algebraic error // Noise model for first type of factor is evaluating algebraic error
noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, noiseModel::Isotropic::shared_ptr model1 =
0.01); noiseModel::Isotropic::Sigma(1, 0.01);
// Noise model for second type of factor is evaluating pixel coordinates // Noise model for second type of factor is evaluating pixel coordinates
noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2);
@ -45,30 +43,22 @@ PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), trueK);
Rot3 trueRotation(c1Rc2); Rot3 trueRotation(c1Rc2);
Unit3 trueDirection(c1Tc2); Unit3 trueDirection(c1Tc2);
EssentialMatrix trueE(trueRotation, trueDirection); EssentialMatrix trueE(trueRotation, trueDirection);
double baseline = 0.1; // actual baseline of the camera double baseline = 0.1; // actual baseline of the camera
Point2 pA(size_t i) { Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; }
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)); }
Point2 pB(size_t i) { Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(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) { TEST(EssentialMatrixFactor, testData) {
CHECK(readOK); CHECK(readOK);
// Check E matrix // Check E matrix
Matrix expected(3, 3); Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) Matrix aEb_matrix =
* c1Rc2.matrix(); skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix();
EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
// Check some projections // Check some projections
@ -90,7 +80,7 @@ TEST (EssentialMatrixFactor, testData) {
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor, factor) { TEST(EssentialMatrixFactor, factor) {
Key key(1); Key key(1);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor factor(key, pA(i), pB(i), model1); EssentialMatrixFactor factor(key, pA(i), pB(i), model1);
@ -104,10 +94,11 @@ TEST (EssentialMatrixFactor, factor) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected; Matrix Hexpected;
typedef Eigen::Matrix<double,1,1> Vector1; typedef Eigen::Matrix<double, 1, 1> Vector1;
Hexpected = numericalDerivative11<Vector1, EssentialMatrix>( Hexpected = numericalDerivative11<Vector1, EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE); boost::none),
trueE);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected, Hactual, 1e-8)); EXPECT(assert_equal(Hexpected, Hactual, 1e-8));
@ -118,10 +109,10 @@ TEST (EssentialMatrixFactor, factor) {
TEST(EssentialMatrixFactor, ExpressionFactor) { TEST(EssentialMatrixFactor, ExpressionFactor) {
Key key(1); Key key(1);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
boost::function<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f = boost::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
Expression<EssentialMatrix> E_(key); // leaf expression Expression<EssentialMatrix> E_(key); // leaf expression
Expression<double> expr(f, E_); // unary expression Expression<double> expr(f, E_); // unary expression
// Test the derivatives using Paul's magic // Test the derivatives using Paul's magic
Values values; Values values;
@ -144,13 +135,16 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
Key key(1); Key key(1);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
boost::function<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f = boost::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
boost::function<EssentialMatrix(const Rot3&, const Unit3&, OptionalJacobian<5, 3>, boost::function<EssentialMatrix(const Rot3 &, const Unit3 &,
OptionalJacobian<5, 2>)> g; OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)>
g;
Expression<Rot3> R_(key); Expression<Rot3> R_(key);
Expression<Unit3> d_(trueDirection); Expression<Unit3> d_(trueDirection);
Expression<EssentialMatrix> E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); Expression<EssentialMatrix> E_(&EssentialMatrix::FromRotationAndDirection,
R_, d_);
Expression<double> expr(f, E_); Expression<double> expr(f, E_);
// Test the derivatives using Paul's magic // Test the derivatives using Paul's magic
@ -171,7 +165,7 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor, minimization) { TEST(EssentialMatrixFactor, minimization) {
// Here we want to optimize directly on essential matrix constraints // Here we want to optimize directly on essential matrix constraints
// Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement, // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
// but GTSAM does the equivalent anyway, provided we give the right // but GTSAM does the equivalent anyway, provided we give the right
@ -190,8 +184,8 @@ TEST (EssentialMatrixFactor, minimization) {
// Check error at initial estimate // Check error at initial estimate
Values initial; Values initial;
EssentialMatrix initialE = trueE.retract( EssentialMatrix initialE =
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE); initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
@ -214,11 +208,10 @@ TEST (EssentialMatrixFactor, minimization) {
// Check errors individually // Check errors individually
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor2, factor) { TEST(EssentialMatrixFactor2, factor) {
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
@ -234,11 +227,13 @@ TEST (EssentialMatrixFactor2, factor) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind( boost::function<Vector(const EssentialMatrix &, double)> f =
&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none); boost::none, boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d); Hexpected1 =
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d); numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
Hexpected2 =
numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -247,7 +242,7 @@ TEST (EssentialMatrixFactor2, factor) {
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor2, minimization) { TEST(EssentialMatrixFactor2, minimization) {
// Here we want to optimize for E and inverse depths at the same time // Here we want to optimize for E and inverse depths at the same time
// We start with a factor graph and add constraints to it // We start with a factor graph and add constraints to it
@ -290,8 +285,7 @@ TEST (EssentialMatrixFactor2, minimization) {
EssentialMatrix bodyE = cRb.inverse() * trueE; EssentialMatrix bodyE = cRb.inverse() * trueE;
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor3, factor) { TEST(EssentialMatrixFactor3, factor) {
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2); EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2);
@ -307,11 +301,13 @@ TEST (EssentialMatrixFactor3, factor) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind( boost::function<Vector(const EssentialMatrix &, double)> f =
&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none); boost::none, boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d); Hexpected1 =
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d); numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 =
numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -320,13 +316,13 @@ TEST (EssentialMatrixFactor3, factor) {
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor3, minimization) { TEST(EssentialMatrixFactor3, minimization) {
// As before, we start with a factor graph and add constraints to it // As before, we start with a factor graph and add constraints to it
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
// but now we specify the rotation bRc // but now we specify the rotation bRc
graph.emplace_shared<EssentialMatrixFactor3>(100, i, pA(i), pB(i), cRb, model2); graph.emplace_shared<EssentialMatrixFactor3>(100, i, pA(i), pB(i), cRb,
model2);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -368,7 +364,6 @@ TEST(EssentialMatrixFactor4, factor) {
Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix HEexpected; Matrix HEexpected;
Matrix HKexpected; Matrix HKexpected;
@ -445,13 +440,14 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) {
EssentialMatrix initialE = EssentialMatrix initialE =
trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE); initial.insert(1, initialE);
initial.insert(2, trueK); initial.insert(2, trueK);
// add prior factor for calibration // add prior factor for calibration
Vector5 priorNoiseModelSigma; Vector5 priorNoiseModelSigma;
priorNoiseModelSigma << 10, 10, 10, 10, 10; priorNoiseModelSigma << 10, 10, 10, 10, 10;
graph.emplace_shared<PriorFactor<Cal3_S2>>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); graph.emplace_shared<PriorFactor<Cal3_S2>>(
2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
LevenbergMarquardtOptimizer optimizer(graph, initial); LevenbergMarquardtOptimizer optimizer(graph, initial);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
@ -476,8 +472,8 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) {
//************************************************************************* //*************************************************************************
TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) {
// We need 7 points here as the prior on the focal length parameters is weak // We need 7 points here as the prior on the focal length parameters is weak
// and the initialization is noisy. So in total we are trying to optimize 7 DOF, // and the initialization is noisy. So in total we are trying to optimize 7
// with a strong prior on the remaining 3 DOF. // DOF, with a strong prior on the remaining 3 DOF.
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < 7; i++) for (size_t i = 0; i < 7; i++)
graph.emplace_shared<EssentialMatrixFactor4<Cal3_S2>>(1, 2, pA(i), pB(i), graph.emplace_shared<EssentialMatrixFactor4<Cal3_S2>>(1, 2, pA(i), pB(i),
@ -501,8 +497,9 @@ TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) {
// add prior factor for calibration // add prior factor for calibration
Vector5 priorNoiseModelSigma; Vector5 priorNoiseModelSigma;
priorNoiseModelSigma << 20, 20, 1, 1, 1; priorNoiseModelSigma << 20, 20, 1, 1, 1;
graph.emplace_shared<PriorFactor<Cal3_S2>>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); graph.emplace_shared<PriorFactor<Cal3_S2>>(
2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
LevenbergMarquardtOptimizer optimizer(graph, initial); LevenbergMarquardtOptimizer optimizer(graph, initial);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
@ -528,8 +525,8 @@ TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) {
TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
graph.emplace_shared<EssentialMatrixFactor4<Cal3Bundler>>(1, 2, pA(i), pB(i), graph.emplace_shared<EssentialMatrixFactor4<Cal3Bundler>>(1, 2, pA(i),
model1); pB(i), model1);
Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3); Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -548,8 +545,9 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
// add prior factor for calibration // add prior factor for calibration
Vector3 priorNoiseModelSigma; Vector3 priorNoiseModelSigma;
priorNoiseModelSigma << 0.1, 0.1, 0.1; priorNoiseModelSigma << 0.1, 0.1, 0.1;
graph.emplace_shared<PriorFactor<Cal3Bundler>>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); graph.emplace_shared<PriorFactor<Cal3Bundler>>(
2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
LevenbergMarquardtOptimizer optimizer(graph, initial); LevenbergMarquardtOptimizer optimizer(graph, initial);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
@ -571,7 +569,6 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
1e-6); 1e-6);
} }
} // namespace example1 } // namespace example1
//************************************************************************* //*************************************************************************
@ -585,14 +582,10 @@ Rot3 aRb = data.cameras[1].pose().rotation();
Point3 aTb = data.cameras[1].pose().translation(); Point3 aTb = data.cameras[1].pose().translation();
EssentialMatrix trueE(aRb, Unit3(aTb)); EssentialMatrix trueE(aRb, Unit3(aTb));
double baseline = 10; // actual baseline of the camera double baseline = 10; // actual baseline of the camera
Point2 pA(size_t i) { Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; }
return data.tracks[i].measurements[0].second; Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; }
}
Point2 pB(size_t i) {
return data.tracks[i].measurements[1].second;
}
Cal3Bundler trueK = Cal3Bundler(500, 0, 0); Cal3Bundler trueK = Cal3Bundler(500, 0, 0);
boost::shared_ptr<Cal3Bundler> K = boost::make_shared<Cal3Bundler>(trueK); boost::shared_ptr<Cal3Bundler> K = boost::make_shared<Cal3Bundler>(trueK);
@ -622,8 +615,8 @@ TEST(EssentialMatrixFactor, extraMinimization) {
// Check error at initial estimate // Check error at initial estimate
Values initial; Values initial;
EssentialMatrix initialE = trueE.retract( EssentialMatrix initialE =
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE); initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
@ -647,11 +640,10 @@ TEST(EssentialMatrixFactor, extraMinimization) {
// Check errors individually // Check errors individually
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor2, extraTest) { TEST(EssentialMatrixFactor2, extraTest) {
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K);
@ -667,11 +659,13 @@ TEST (EssentialMatrixFactor2, extraTest) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind( boost::function<Vector(const EssentialMatrix &, double)> f =
&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none); boost::none, boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d); Hexpected1 =
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d); numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
Hexpected2 =
numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -680,14 +674,15 @@ TEST (EssentialMatrixFactor2, extraTest) {
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor2, extraMinimization) { TEST(EssentialMatrixFactor2, extraMinimization) {
// Additional test with camera moving in positive X direction // Additional test with camera moving in positive X direction
// We start with a factor graph and add constraints to it // We start with a factor graph and add constraints to it
// Noise sigma is 1, assuming pixel measurements // Noise sigma is 1, assuming pixel measurements
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < data.number_tracks(); i++) for (size_t i = 0; i < data.number_tracks(); i++)
graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2, K); graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2,
K);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -715,8 +710,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrixFactor3, extraTest) { TEST(EssentialMatrixFactor3, extraTest) {
// The "true E" in the body frame is // The "true E" in the body frame is
EssentialMatrix bodyE = cRb.inverse() * trueE; EssentialMatrix bodyE = cRb.inverse() * trueE;
@ -735,11 +729,13 @@ TEST (EssentialMatrixFactor3, extraTest) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind( boost::function<Vector(const EssentialMatrix &, double)> f =
&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none); boost::none, boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d); Hexpected1 =
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d); numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 =
numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -755,4 +751,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */