From 8688fc85d0a55f0f4c8221c05f9b6f61f6f2eb1f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 17 Dec 2013 16:18:31 +0000 Subject: [PATCH] MATLAB wrapping and Monocular VO example --- gtsam.h | 49 ++++++++++++++++ gtsam/geometry/EssentialMatrix.h | 42 ++++++++------ gtsam/geometry/Sphere2.cpp | 5 +- gtsam/geometry/Sphere2.h | 4 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 3 + gtsam/geometry/tests/testSphere2.cpp | 40 +++++++++++++ .../slam/tests/testEssentialMatrixFactor.cpp | 2 +- matlab/gtsam_examples/MonocularVOExample.m | 56 +++++++++++++++++++ matlab/gtsam_examples/StereoVOExample.m | 2 +- 9 files changed, 180 insertions(+), 23 deletions(-) create mode 100644 matlab/gtsam_examples/MonocularVOExample.m diff --git a/gtsam.h b/gtsam.h index 699e7e4b4..ced0b443e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -563,6 +563,49 @@ virtual class Pose3 : gtsam::Value { void serialize() const; }; +#include +virtual class Sphere2 : gtsam::Value { + // Standard Constructors + Sphere2(); + Sphere2(const gtsam::Point3& pose); + + // Testable + void print(string s) const; + bool equals(const gtsam::Sphere2& pose, double tol) const; + + // Other functionality + Matrix getBasis() const; + Matrix skew() const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Sphere2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Sphere2& s) const; +}; + +#include +virtual class EssentialMatrix : gtsam::Value { + // Standard Constructors + EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Sphere2& aTb); + + // Testable + void print(string s) const; + bool equals(const gtsam::EssentialMatrix& pose, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::EssentialMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::EssentialMatrix& s) const; + + // Other methods: + gtsam::Rot3 rotation() const; + gtsam::Sphere2 direction() const; + Matrix matrix() const; + double error(Vector vA, Vector vB); +}; + virtual class Cal3_S2 : gtsam::Value { // Standard Constructors Cal3_S2(); @@ -2180,6 +2223,12 @@ virtual class PoseRotationPrior : gtsam::NoiseModelFactor { typedef gtsam::PoseRotationPrior PoseRotationPrior2D; typedef gtsam::PoseRotationPrior PoseRotationPrior3D; +#include +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); +}; + #include pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index ed1f9b78a..90aa13e76 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -45,9 +45,31 @@ public: /// @} - /// @name Value + /// @name Testable /// @{ + /// print with optional string + void print(const std::string& s = "") const { + std::cout << s; + aRb_.print("R:\n"); + aTb_.print("d: "); + } + + /// assert equality up to a tolerance + bool equals(const EssentialMatrix& other, double tol=1e-8) const { + return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol); + } + + /// @} + + /// @name Manifold + /// @{ + + /// Dimensionality of tangent space = 5 DOF + inline static size_t Dim() { + return 5; + } + /// Return the dimensionality of the tangent space virtual size_t dim() const { return 5; @@ -70,23 +92,6 @@ public: /// @} - /// @name Testable - /// @{ - - /// print with optional string - void print(const std::string& s) const { - std::cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); - } - - /// assert equality up to a tolerance - bool equals(const EssentialMatrix& other, double tol) const { - return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol); - } - - /// @} - /// @name Essential matrix methods /// @{ @@ -110,6 +115,7 @@ public: boost::optional H = boost::none) const { if (H) { H->resize(1, 5); + // See math.lyx Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) * aTb_.getBasis(); diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index a8c98b205..506351bd0 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -13,12 +13,13 @@ * @file Sphere2.h * @date Feb 02, 2011 * @author Can Erdogan + * @author Frank Dellaert * @brief Develop a Sphere2 class - basically a point on a unit sphere */ #include #include -#include +#include using namespace std; @@ -54,7 +55,7 @@ Matrix Sphere2::getBasis() const { /* ************************************************************************* */ /// The print fuction void Sphere2::print(const std::string& s) const { - printf("(%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(), p_.z()); + cout << s << ":" << p_ << endl; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index d583853a6..83a9c09e3 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -13,17 +13,19 @@ * @file Sphere2.h * @date Feb 02, 2011 * @author Can Erdogan + * @author Frank Dellaert * @brief Develop a Sphere2 class - basically a point on a unit sphere */ #pragma once #include +#include namespace gtsam { /// Represents a 3D point on a unit sphere. -class Sphere2 { +class Sphere2 : public DerivedValue { private: diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 3f0cb07f7..83e41ce64 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -14,6 +14,9 @@ using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(EssentialMatrix) +GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) + //************************************************************************* // Create two cameras and corresponding essential matrix E Rot3 aRb = Rot3::yaw(M_PI_2); diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index ef28048ca..deb9ca99d 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -72,6 +72,46 @@ TEST(Sphere2, localCoordinates_retract) { } } +///* ************************************************************************* */ +//TEST( Pose2, between ) +//{ +// // < +// // +// // ^ +// // +// // *--0--*--* +// Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y +// Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x +// +// Matrix actualH1,actualH2; +// Pose2 expected(M_PI/2.0, Point2(2,2)); +// Pose2 actual1 = gT1.between(gT2); +// Pose2 actual2 = gT1.between(gT2,actualH1,actualH2); +// EXPECT(assert_equal(expected,actual1)); +// EXPECT(assert_equal(expected,actual2)); +// +// Matrix expectedH1 = Matrix_(3,3, +// 0.0,-1.0,-2.0, +// 1.0, 0.0,-2.0, +// 0.0, 0.0,-1.0 +// ); +// Matrix numericalH1 = numericalDerivative21(testing::between, gT1, gT2); +// EXPECT(assert_equal(expectedH1,actualH1)); +// EXPECT(assert_equal(numericalH1,actualH1)); +// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx +// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); +// +// Matrix expectedH2 = Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 1.0, 0.0, +// 0.0, 0.0, 1.0 +// ); +// Matrix numericalH2 = numericalDerivative22(testing::between, gT1, gT2); +// EXPECT(assert_equal(expectedH2,actualH2)); +// EXPECT(assert_equal(numericalH2,actualH2)); +// +//} +// /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 5a5753589..0c274cb0d 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -42,7 +42,7 @@ it1 = std::transform(P, P + 5, pA.begin(), it2 = std::transform(P, P + 5, pB.begin(), boost::bind(&Cam::project, &cameraB, _1, boost::none, boost::none)); -// Converto to homogenous coordinates +// Convert to homogeneous coordinates vector vA(5), vB(5); vector::iterator // it3 = std::transform(pA.begin(), pA.end(), vA.begin(), diff --git a/matlab/gtsam_examples/MonocularVOExample.m b/matlab/gtsam_examples/MonocularVOExample.m new file mode 100644 index 000000000..9fd2df463 --- /dev/null +++ b/matlab/gtsam_examples/MonocularVOExample.m @@ -0,0 +1,56 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010-2013, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Monocular VO Example with 5 landmarks and two cameras +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% import +import gtsam.* + +%% Create two cameras and corresponding essential matrix E +aRb = Rot3.Yaw(pi/2); +aTb = Point3 (0.1, 0, 0); +identity=Pose3; +aPb = Pose3(aRb, aTb); +cameraA = CalibratedCamera(identity); +cameraB = CalibratedCamera(aPb); + +%% Create 5 points +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) }; + +%% Project points in both cameras +for i=1:5 + pA{i}= cameraA.project(P{i}); + pB{i}= cameraB.project(P{i}); +end + +%% We start with a factor graph and add epipolar constraints to it +% Noise sigma is 1cm, assuming metric measurements +graph = NonlinearFactorGraph; +model = noiseModel.Isotropic.Sigma(1,0.01); +for i=1:5 + graph.add(EssentialMatrixFactor(1, pA{i}, pB{i}, model)); +end + +%% Create initial estimate +initial = Values; +trueE = EssentialMatrix(aRb,Sphere2(aTb)); +initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]'); +initial.insert(1, initialE); + +%% Optimize +parameters = LevenbergMarquardtParams; +% parameters.setVerbosity('ERROR'); +optimizer = LevenbergMarquardtOptimizer(graph, initial, parameters); +result = optimizer.optimize(); + +%% Print result (as essentialMatrix) and error +error = graph.error(result) +E = result.at(1) + diff --git a/matlab/gtsam_examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m index 8dbaa1a76..b437ca80c 100644 --- a/matlab/gtsam_examples/StereoVOExample.m +++ b/matlab/gtsam_examples/StereoVOExample.m @@ -33,7 +33,7 @@ graph.add(NonlinearEqualityPose3(x1, first_pose)); %% Create realistic calibration and measurement noise model % format: fx fy skew cx cy baseline K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); -stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); +stereo_model = noiseModel.Isotropic.Sigma(3,1); %% Add measurements % pose 1