From 902c48d4f19d623076af2cdfd3994c9e3de39bde Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jan 2014 09:46:52 -0500 Subject: [PATCH 01/19] Fixed small comment --- gtsam/slam/EssentialMatrixFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index c01a72a76..06f32890a 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -218,7 +218,7 @@ public: * Constructor * @param pA point in first camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates - * @param bRc extra rotation between "body" and "camera" frame + * @param cRb extra rotation from "body" to "camera" frame * @param model noise model should be in calibrated coordinates, as well */ EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, From aa6aee1157bbb897767259d3d2351eea291e46ab Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jan 2014 10:14:22 -0500 Subject: [PATCH 02/19] Fixed two "unused" warnings --- gtsam/nonlinear/ISAM2-impl.cpp | 5 ++++- gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp | 1 - 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 9abd62016..ffdbd6d10 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -116,7 +116,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli // Reorder and remove from ordering, solution, and fixed keys ordering.permuteInPlace(unusedToEnd); BOOST_REVERSE_FOREACH(Key key, unusedKeys) { - Ordering::value_type removed = ordering.pop_back(); +#ifndef NDEBUG + Ordering::value_type removed = +#endif + ordering.pop_back(); assert(removed.first == key); theta.erase(key); fixedVariables.erase(key); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index d9c59d0ea..bc3e591f9 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -266,7 +266,6 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { result.linearVariables = separatorValues_.size(); // Pull out parameters we'll use - const NonlinearOptimizerParams::Verbosity nloVerbosity = parameters_.verbosity; const LevenbergMarquardtParams::VerbosityLM lmVerbosity = parameters_.verbosityLM; double lambda = parameters_.lambdaInitial; From f8fbfaea50a6fb44101381862a2f9262740d8205 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jan 2014 10:14:59 -0500 Subject: [PATCH 03/19] Use precisions_ where possible --- gtsam/linear/HessianFactor.cpp | 4 ++-- gtsam/linear/NoiseModel.cpp | 4 +--- gtsam/linear/NoiseModel.h | 2 -- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 72503b470..5baee8712 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -254,10 +254,10 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) { if(jf.model_->isConstrained()) throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); else { - Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas()); + const Vector& precisions = jf.model_->precisions(); info_.copyStructureFrom(jf.Ab_); BlockInfo::constBlock A = jf.Ab_.full(); - matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A; + matrix_.noalias() = A.transpose() * precisions.asDiagonal() * A; } } else if(dynamic_cast(&gf)) { const HessianFactor& hf(static_cast(gf)); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 88b57046e..16b0a5bc1 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -308,8 +308,6 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { list Rd; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector invsigmas = reciprocal(sigmas_); - Vector weights = emul(invsigmas,invsigmas); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding @@ -321,7 +319,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { // Calculate weighted pseudo-inverse and corresponding precision gttic(constrained_QR_weightedPseudoinverse); - double precision = weightedPseudoinverse(a, weights, pseudo); + double precision = weightedPseudoinverse(a, precisions_, pseudo); gttoc(constrained_QR_weightedPseudoinverse); // If precision is zero, no information on this column diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 704106bc8..37e5f0090 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -342,11 +342,9 @@ namespace gtsam { Vector mu_; /** protected constructor takes sigmas */ - // Keeps only sigmas and calculates invsigmas when necessary Constrained(const Vector& sigmas = zero(1)) : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {} - // Keeps only sigmas and calculates invsigmas when necessary // allows for specifying mu Constrained(const Vector& mu, const Vector& sigmas) : Diagonal(sigmas), mu_(mu) {} From 651dd3e931d1e8c1359e9f86b5cd7a8fdd540024 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Jan 2014 11:59:57 -0500 Subject: [PATCH 04/19] Added a unit test for EssentialMatrixFactor3 with LevenbergMarquardt optimization --- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index a521e43fb..69e78c410 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) { truth.insert(i, LieScalar(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Optimize + LevenbergMarquardtParams parameters; + // parameters.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, truth, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actual = result.at(100); + EXPECT(assert_equal(bodyE, actual, 1e-1)); + for (size_t i = 0; i < 5; i++) + EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } } // namespace example1 From 2acffe885e3942a97ee2b85b6f529cdfd6a5387d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 Jan 2014 16:25:14 -0500 Subject: [PATCH 05/19] normalize --- gtsam/geometry/Point3.cpp | 54 ++++++++++++++-------- gtsam/geometry/Point3.h | 3 ++ gtsam/geometry/tests/testPoint3.cpp | 71 +++++++++++++++++------------ 3 files changed, 80 insertions(+), 48 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 4f5f43665..b6244630e 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3); /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol + && fabs(z_ - q.z()) < tol); } /* ************************************************************************* */ @@ -37,18 +38,18 @@ void Point3::print(const string& s) const { /* ************************************************************************* */ -bool Point3::operator== (const Point3& q) const { +bool Point3::operator==(const Point3& q) const { return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; } /* ************************************************************************* */ Point3 Point3::operator+(const Point3& q) const { - return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); + return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); } /* ************************************************************************* */ -Point3 Point3::operator- (const Point3& q) const { - return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); +Point3 Point3::operator-(const Point3& q) const { + return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); } /* ************************************************************************* */ @@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const { } /* ************************************************************************* */ -Point3 Point3::add(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = eye(3,3); +Point3 Point3::add(const Point3 &q, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = eye(3, 3); + if (H2) + *H2 = eye(3, 3); return *this + q; } /* ************************************************************************* */ -Point3 Point3::sub(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = -eye(3,3); +Point3 Point3::sub(const Point3 &q, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = eye(3, 3); + if (H2) + *H2 = -eye(3, 3); return *this - q; } /* ************************************************************************* */ Point3 Point3::cross(const Point3 &q) const { - return Point3( y_*q.z_ - z_*q.y_, - z_*q.x_ - x_*q.z_, - x_*q.y_ - y_*q.x_ ); + return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, + x_ * q.y_ - y_ * q.x_); } /* ************************************************************************* */ double Point3::dot(const Point3 &q) const { - return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); + return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); } /* ************************************************************************* */ double Point3::norm() const { - return sqrt( x_*x_ + y_*y_ + z_*z_ ); + return sqrt(x_ * x_ + y_ * y_ + z_ * z_); +} + +/* ************************************************************************* */ +Point3 Point3::normalize(boost::optional H) const { + Point3 normalized = *this / norm(); + if (H) { + // 3*3 Derivative + double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; + double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; + H->resize(3, 3); + *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + *H /= pow(x2 + y2 + z2, 1.5); + } + return normalized; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 5e2e0ff42..66924ec07 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -176,6 +176,9 @@ namespace gtsam { /** Distance of the point from the origin */ double norm() const; + /** normalize, with optional Jacobian */ + Point3 normalize(boost::optional H = boost::none) const; + /** cross product @return this x q */ Point3 cross(const Point3 &q) const; diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index ef4a3894c..58c59e83d 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -12,75 +12,86 @@ /** * @file testPoint3.cpp * @brief Unit tests for Point3 class - */ + */ -#include -#include #include +#include +#include +#include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) GTSAM_CONCEPT_LIE_INST(Point3) -static Point3 P(0.2,0.7,-2); +static Point3 P(0.2, 0.7, -2); /* ************************************************************************* */ TEST(Point3, Lie) { - Point3 p1(1,2,3); - Point3 p2(4,5,6); + Point3 p1(1, 2, 3); + Point3 p2(4, 5, 6); Matrix H1, H2; - EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2))); EXPECT(assert_equal(eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2))); EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.)))); - EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.)))); + EXPECT(assert_equal(Vector_(3, 3., 3., 3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ -TEST( Point3, arithmetic) -{ - CHECK(P*3==3*P); - CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) )); - CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1))); - CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1))); - CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2)); - CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3))); - CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2)); +TEST( Point3, arithmetic) { + CHECK(P * 3 == 3 * P); + CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); + CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); + CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); + CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); + CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); + CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); } /* ************************************************************************* */ -TEST( Point3, equals) -{ +TEST( Point3, equals) { CHECK(P.equals(P)); Point3 Q; CHECK(!P.equals(Q)); } /* ************************************************************************* */ -TEST( Point3, dot) -{ - Point3 origin, ones(1,1,1); - CHECK(origin.dot(Point3(1,1,0)) == 0); - CHECK(ones.dot(Point3(1,1,0)) == 2); +TEST( Point3, dot) { + Point3 origin, ones(1, 1, 1); + CHECK(origin.dot(Point3(1, 1, 0)) == 0); + CHECK(ones.dot(Point3(1, 1, 0)) == 2); } /* ************************************************************************* */ -TEST( Point3, stream) -{ - Point3 p(1,2, -3); +TEST( Point3, stream) { + Point3 p(1, 2, -3); std::ostringstream os; os << p; EXPECT(os.str() == "[1, 2, -3]';"); } +//************************************************************************* +TEST (Point3, normalize) { + Matrix actualH; + Point3 point(1, -2, 3); // arbitrary point + Point3 expected(point / sqrt(14)); + EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(&Point3::normalize, _1, boost::none), point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From fd9805c64f31063f13d18e88f80373fffc0c8fcd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 Jan 2014 16:25:47 -0500 Subject: [PATCH 06/19] Named constructor FromPoint3, with optional Jacobian --- gtsam/geometry/Sphere2.cpp | 15 +++++++++++++++ gtsam/geometry/Sphere2.h | 10 +++++++++- gtsam/geometry/tests/testSphere2.cpp | 18 +++++++++++++++--- 3 files changed, 39 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index b6cae287c..29a170c4d 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -27,6 +27,21 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +Sphere2 Sphere2::FromPoint3(const Point3& point, + boost::optional H) { + Sphere2 direction(point); + if (H) { + // 3*3 Derivative of representation with respect to point is 3*3: + Matrix D_p_point; + point.normalize(D_p_point); // TODO, this calculates norm a second time :-( + // Calculate the 2*3 Jacobian + H->resize(2, 3); + *H << direction.basis().transpose() * D_p_point; + } + return direction; +} + /* ************************************************************************* */ Sphere2 Sphere2::Random(boost::random::mt19937 & rng) { // TODO allow any engine without including all of boost :-( diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 0c549ed0b..8555b5d30 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -65,6 +65,10 @@ public: p_ = p_ / p_.norm(); } + /// Named constructor from Point3 with optional Jacobian + static Sphere2 FromPoint3(const Point3& point, + boost::optional H = boost::none); + /// Random direction, using boost::uniform_on_sphere static Sphere2 Random(boost::random::mt19937 & rng); @@ -85,7 +89,11 @@ public: /// @name Other functionality /// @{ - /// Returns the local coordinate frame to tangent plane + /** + * Returns the local coordinate frame to tangent plane + * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions + * tangent to the sphere at the current direction. + */ Matrix basis() const; /// Return skew-symmetric associated with 3D point on unit sphere diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index c6e519a44..b9aa84949 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -25,6 +25,7 @@ #include #include #include +#include using namespace boost::assign; using namespace gtsam; @@ -244,13 +245,24 @@ TEST(Sphere2, Random) { // Check that is deterministic given same random seed Point3 expected(-0.667578, 0.671447, 0.321713); Point3 actual = Sphere2::Random(rng).point3(); - EXPECT(assert_equal(expected,actual,1e-5)); + EXPECT(assert_equal(expected, actual, 1e-5)); // Check that means are all zero at least Point3 expectedMean, actualMean; for (size_t i = 0; i < 100; i++) actualMean = actualMean + Sphere2::Random(rng).point3(); - actualMean = actualMean/100; - EXPECT(assert_equal(expectedMean,actualMean,0.1)); + actualMean = actualMean / 100; + EXPECT(assert_equal(expectedMean, actualMean, 0.1)); +} + +//************************************************************************* +TEST (Sphere2, FromPoint3) { + Matrix actualH; + Point3 point(1, -2, 3); // arbitrary point + Sphere2 expected(point); + EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(Sphere2::FromPoint3, _1, boost::none), point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); } /* ************************************************************************* */ From 3cd45be423bbcb08b1005c28ba11ea53bace155a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 Jan 2014 16:26:19 -0500 Subject: [PATCH 07/19] Named constructor FromPose3, with Jacobians --- gtsam/geometry/EssentialMatrix.cpp | 127 +++++++++++++++++++ gtsam/geometry/EssentialMatrix.h | 91 ++----------- gtsam/geometry/tests/testEssentialMatrix.cpp | 28 +++- 3 files changed, 168 insertions(+), 78 deletions(-) create mode 100644 gtsam/geometry/EssentialMatrix.cpp diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp new file mode 100644 index 000000000..9e50ad92a --- /dev/null +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -0,0 +1,127 @@ +/* + * @file EssentialMatrix.cpp + * @brief EssentialMatrix class + * @author Frank Dellaert + * @date December 5, 2014 + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, + boost::optional H) { + const Rot3& _1R2_ = _1P2_.rotation(); + const Point3& _1T2_ = _1P2_.translation(); + if (!H) { + // just make a direction out of translation and create E + Sphere2 direction(_1T2_); + return EssentialMatrix(_1R2_, direction); + } else { + // Calculate the 5*6 Jacobian H = D_E_1P2 + // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation + // First get 2*3 derivative from Sphere2::FromPoint3 + Matrix D_direction_1T2; + Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2); + H->resize(5, 6); + H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left + H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left + H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right + H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right + return EssentialMatrix(_1R2_, direction); + } +} + +/* ************************************************************************* */ +void EssentialMatrix::print(const std::string& s) const { + std::cout << s; + aRb_.print("R:\n"); + aTb_.print("d: "); +} + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { + assert(xi.size() == 5); + Vector3 omega(sub(xi, 0, 3)); + Vector2 z(sub(xi, 3, 5)); + Rot3 R = aRb_.retract(omega); + Sphere2 t = aTb_.retract(z); + return EssentialMatrix(R, t); +} + +/* ************************************************************************* */ +Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { + return Vector(5) << // + aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); +} + +/* ************************************************************************* */ +Point3 EssentialMatrix::transform_to(const Point3& p, + boost::optional DE, boost::optional Dpoint) const { + Pose3 pose(aRb_, aTb_.point3()); + Point3 q = pose.transform_to(p, DE, Dpoint); + if (DE) { + // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 + // The last 3 columns are derivative with respect to change in translation + // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() + // Duy made an educated guess that this needs to be rotated to the local frame + Matrix H(3, 5); + H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + *DE = H; + } + return q; +} + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, + boost::optional HE, boost::optional HR) const { + + // The rotation must be conjugated to act in the camera frame + Rot3 c1Rc2 = aRb_.conjugate(cRb); + + if (!HE && !HR) { + // Rotate translation direction and return + Sphere2 c1Tc2 = cRb * aTb_; + return EssentialMatrix(c1Rc2, c1Tc2); + } else { + // Calculate derivatives + Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 + Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + if (HE) { + *HE = zeros(5, 5); + HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 + HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) + } + if (HR) { + throw std::runtime_error( + "EssentialMatrix::rotate: derivative HR not implemented yet"); + /* + HR->resize(5, 3); + HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? + HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? + */ + } + return EssentialMatrix(c1Rc2, c1Tc2); + } +} + +/* ************************************************************************* */ +double EssentialMatrix::error(const Vector& vA, const Vector& vB, // + boost::optional H) 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_.basis(); + *H << HR, HD; + } + return dot(vA, E_ * vB); +} + +/* ************************************************************************* */ + +} // gtsam + diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 54fd94bbc..3c5836b5e 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,7 +10,6 @@ #include #include #include -#include namespace gtsam { @@ -26,7 +25,7 @@ private: Rot3 aRb_; ///< Rotation between a and b Sphere2 aTb_; ///< translation direction from a to b - Matrix E_; ///< Essential matrix + Matrix3 E_; ///< Essential matrix public: @@ -48,6 +47,10 @@ public: aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { } + /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) + static EssentialMatrix FromPose3(const Pose3& _1P2_, + boost::optional H = boost::none); + /// Random, using Rot3::Random and Sphere2::Random template static EssentialMatrix Random(Engine & rng) { @@ -60,11 +63,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const { - std::cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); - } + void print(const std::string& s = "") const; /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { @@ -87,20 +86,10 @@ public: } /// Retract delta to manifold - virtual EssentialMatrix retract(const Vector& xi) const { - assert(xi.size() == 5); - Vector3 omega(sub(xi, 0, 3)); - Vector2 z(sub(xi, 3, 5)); - Rot3 R = aRb_.retract(omega); - Sphere2 t = aTb_.retract(z); - return EssentialMatrix(R, t); - } + virtual EssentialMatrix retract(const Vector& xi) const; /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& other) const { - return Vector(5) << // - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); - } + virtual Vector localCoordinates(const EssentialMatrix& other) const; /// @} @@ -108,17 +97,17 @@ public: /// @{ /// Rotation - const Rot3& rotation() const { + inline const Rot3& rotation() const { return aRb_; } /// Direction - const Sphere2& direction() const { + inline const Sphere2& direction() const { return aTb_; } /// Return 3*3 matrix representation - const Matrix& matrix() const { + inline const Matrix3& matrix() const { return E_; } @@ -131,20 +120,7 @@ public: */ Point3 transform_to(const Point3& p, boost::optional DE = boost::none, - boost::optional Dpoint = boost::none) const { - Pose3 pose(aRb_, aTb_.point3()); - Point3 q = pose.transform_to(p, DE, Dpoint); - if (DE) { - // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 - // The last 3 columns are derivative with respect to change in translation - // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() - // Duy made an educated guess that this needs to be rotated to the local frame - Matrix H(3, 5); - H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); - *DE = H; - } - return q; - } + boost::optional Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -152,36 +128,7 @@ public: * @param E essential matrix E in camera frame C */ EssentialMatrix rotate(const Rot3& cRb, boost::optional HE = - boost::none, boost::optional HR = boost::none) const { - - // The rotation must be conjugated to act in the camera frame - Rot3 c1Rc2 = aRb_.conjugate(cRb); - - if (!HE && !HR) { - // Rotate translation direction and return - Sphere2 c1Tc2 = cRb * aTb_; - return EssentialMatrix(c1Rc2, c1Tc2); - } else { - // Calculate derivatives - Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 - Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) { - *HE = zeros(5, 5); - HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 - HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) - } - if (HR) { - throw std::runtime_error( - "EssentialMatrix::rotate: derivative HR not implemented yet"); - /* - HR->resize(5, 3); - HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? - HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? - */ - } - return EssentialMatrix(c1Rc2, c1Tc2); - } - } + boost::none, boost::optional HR = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -194,17 +141,7 @@ public: /// epipolar error, algebraic double error(const Vector& vA, const Vector& vB, // - 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_.basis(); - *H << HR, HD; - } - return dot(vA, E_ * vB); - } + boost::optional H = boost::none) const; /// @} diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index da5240b15..56fc8d8ad 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -88,13 +88,39 @@ TEST (EssentialMatrix, rotate) { // Derivatives Matrix actH1, actH2; - try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception + try { + bodyE.rotate(cRb, actH1, actH2); + } catch (exception e) { + } // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); EXPECT(assert_equal(expH1, actH1, 1e-8)); // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); } +//************************************************************************* +TEST (EssentialMatrix, FromPose3_a) { + Matrix actualH; + Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//************************************************************************* +TEST (EssentialMatrix, FromPose3_b) { + Matrix actualH; + Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Point3 c1Tc2(0.4, 0.5, 0.6); + EssentialMatrix trueE(c1Rc2, c1Tc2); + Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 832a6fe5c732fb0589f850f0271d164bd270b8df Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 Jan 2014 16:46:22 -0500 Subject: [PATCH 08/19] New "between factor" that is agnostic about scale. --- gtsam/slam/EssentialMatrixConstraint.h | 145 ++++++++++++++++++ .../tests/testEssentialMatrixConstraint.cpp | 76 +++++++++ 2 files changed, 221 insertions(+) create mode 100644 gtsam/slam/EssentialMatrixConstraint.h create mode 100644 gtsam/slam/tests/testEssentialMatrixConstraint.cpp diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h new file mode 100644 index 000000000..013d78c1a --- /dev/null +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file EssentialMatrixConstraint.h + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + **/ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement + * @addtogroup SLAM + */ +class EssentialMatrixConstraint: public NoiseModelFactor2 { + +private: + + typedef EssentialMatrixConstraint This; + typedef NoiseModelFactor2 Base; + + EssentialMatrix measuredE_; /** The measurement is an essential matrix */ + +public: + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + EssentialMatrixConstraint() { + } + + /** + * Constructor + * @param key1 key for first pose + * @param key2 key for second pose + * @param measuredE measured EssentialMatrix + * @param model noise model, 5D ! + */ + EssentialMatrixConstraint(Key key1, Key key2, + const EssentialMatrix& measuredE, const SharedNoiseModel& model) : + Base(model, key1, key2), measuredE_(measuredE) { + } + + virtual ~EssentialMatrixConstraint() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) + << "," << keyFormatter(this->key2()) << ")\n"; + measuredE_.print(" measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && this->measuredE_.equals(e->measuredE_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& p1, const Pose3& p2, + boost::optional Hp1 = boost::none, // + boost::optional Hp2 = boost::none) const { + // compute relative Pose3 between p1 and p2 + Pose3 _1P2_ = p1.between(p2, Hp1, Hp2); + // convert to EssentialMatrix + Matrix D_hx_1P2; + EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_, + (Hp1 || Hp2) ? boost::optional(D_hx_1P2) : boost::none); + // Calculate derivatives if needed + if (Hp1) { + // Hp1 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p1 = *Hp1; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp1 = D_hx_1P2 * D_1P2_p1; + } + if (Hp2) { + // Hp2 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p2 = *Hp2; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp2 = D_hx_1P2 * D_1P2_p2; + } + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measuredE_.localCoordinates(hx); // 5D error + } + + /** return the measured */ + const EssentialMatrix& measured() const { + return measuredE_; + } + + /** number of variables attached to this factor */ + std::size_t size() const { + return 2; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measuredE_); + } +}; +// \class EssentialMatrixConstraint + +}/// namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp new file mode 100644 index 000000000..eb87100f6 --- /dev/null +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file testEssentialMatrixConstraint.cpp + * @brief Unit tests for EssentialMatrixConstraint Class + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( EssentialMatrixConstraint, test ) { + // Create a factor + Key poseKey1(1); + Key poseKey2(2); + Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20); + Point3 trueTranslation(+0.5, -1.0, +1.0); + Sphere2 trueDirection(trueTranslation); + EssentialMatrix measurement(trueRotation, trueDirection); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25); + EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model); + + // Create a linearization point at the zero-error point + Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)); + Pose3 pose2( + Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840), + Point3(-3.37493895, 6.14660244, -8.93650986)); + + Vector expected = zero(5); + Vector actual = factor.evaluateError(pose1,pose2); + CHECK(assert_equal(expected, actual, 1e-8)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, + boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, + boost::none, boost::none), pose2); + + // Use the factor to calculate the derivative + Matrix actualH1; + Matrix actualH2; + factor.evaluateError(pose1, pose2, actualH1, actualH2); + + // Verify we get the expected error + CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH2, actualH2, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 5f8bb217d7774cb1ee0784c04ca8203e25533dd4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 Jan 2014 16:54:35 -0500 Subject: [PATCH 09/19] Moved implementation to .cpp file --- gtsam/slam/EssentialMatrixConstraint.cpp | 75 ++++++++++++++++++++++++ gtsam/slam/EssentialMatrixConstraint.h | 46 ++------------- 2 files changed, 80 insertions(+), 41 deletions(-) create mode 100644 gtsam/slam/EssentialMatrixConstraint.cpp diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp new file mode 100644 index 000000000..e27bbc8c5 --- /dev/null +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file EssentialMatrixConstraint.cpp + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + **/ + +#include +//#include +//#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +void EssentialMatrixConstraint::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) + << "," << keyFormatter(this->key2()) << ")\n"; + measuredE_.print(" measured: "); + this->noiseModel_->print(" noise model: "); +} + +/* ************************************************************************* */ +bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, + double tol) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && this->measuredE_.equals(e->measuredE_, tol); +} + +/* ************************************************************************* */ +Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1, + const Pose3& p2, boost::optional Hp1, + boost::optional Hp2) const { + + // compute relative Pose3 between p1 and p2 + Pose3 _1P2_ = p1.between(p2, Hp1, Hp2); + + // convert to EssentialMatrix + Matrix D_hx_1P2; + EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_, + (Hp1 || Hp2) ? boost::optional(D_hx_1P2) : boost::none); + + // Calculate derivatives if needed + if (Hp1) { + // Hp1 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p1 = *Hp1; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp1 = D_hx_1P2 * D_1P2_p1; + } + if (Hp2) { + // Hp2 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p2 = *Hp2; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp2 = D_hx_1P2 * D_1P2_p2; + } + + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measuredE_.localCoordinates(hx); // 5D error +} + +} /// namespace gtsam diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 013d78c1a..f2eb76e2d 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -19,11 +19,7 @@ #pragma once #include -#include #include -#include - -#include namespace gtsam { @@ -73,50 +69,18 @@ public: /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) - << "," << keyFormatter(this->key2()) << ")\n"; - measuredE_.print(" measured: "); - this->noiseModel_->print(" noise model: "); - } + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** equals */ - virtual bool equals(const NonlinearFactor& expected, - double tol = 1e-9) const { - const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) - && this->measuredE_.equals(e->measuredE_, tol); - } + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const Pose3& p1, const Pose3& p2, + virtual Vector evaluateError(const Pose3& p1, const Pose3& p2, boost::optional Hp1 = boost::none, // - boost::optional Hp2 = boost::none) const { - // compute relative Pose3 between p1 and p2 - Pose3 _1P2_ = p1.between(p2, Hp1, Hp2); - // convert to EssentialMatrix - Matrix D_hx_1P2; - EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_, - (Hp1 || Hp2) ? boost::optional(D_hx_1P2) : boost::none); - // Calculate derivatives if needed - if (Hp1) { - // Hp1 will already contain the 6*6 derivative D_1P2_p1 - const Matrix& D_1P2_p1 = *Hp1; - // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: - *Hp1 = D_hx_1P2 * D_1P2_p1; - } - if (Hp2) { - // Hp2 will already contain the 6*6 derivative D_1P2_p1 - const Matrix& D_1P2_p2 = *Hp2; - // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: - *Hp2 = D_hx_1P2 * D_1P2_p2; - } - // manifold equivalent of h(x)-z -> log(z,h(x)) - return measuredE_.localCoordinates(hx); // 5D error - } + boost::optional Hp2 = boost::none) const; /** return the measured */ const EssentialMatrix& measured() const { From fd188a4978c8a9f41d047785a4167ae2743669b4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Jan 2014 00:37:21 -0500 Subject: [PATCH 10/19] Development of templated factors --- gtsam/slam/EssentialMatrixFactor.h | 110 ++++++++++++------ .../slam/tests/testEssentialMatrixFactor.cpp | 80 ++++++++++--- 2 files changed, 139 insertions(+), 51 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 142005d3c..0ab43aa4f 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -20,26 +20,46 @@ namespace gtsam { */ class EssentialMatrixFactor: public NoiseModelFactor1 { - Point2 pA_, pB_; ///< Measurements in image A and B - Vector vA_, vB_; ///< Homogeneous versions + Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; public: - /// Constructor + /** + * Constructor + * @param pA point in first camera, in calibrated coordinates + * @param pB point in second camera, in calibrated coordinates + * @param model noise model is about dot product in ideal, homogeneous coordinates + */ 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)) { + Base(model, key) { + vA_ = EssentialMatrix::Homogeneous(pA); + vB_ = EssentialMatrix::Homogeneous(pB); + } + + /** + * Constructor + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous coordinates + * @param K calibration object, will be used only in constructor + */ + template + EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model, boost::shared_ptr K) : + Base(model, key) { + assert(K); + vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); + vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print @@ -47,14 +67,16 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor with measurements\n (" - << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() - << ")'" << std::endl; + << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" + << std::endl; } /// vector of errors returns 1D vector Vector evaluateError(const EssentialMatrix& E, boost::optional H = boost::none) const { - return (Vector(1) << E.error(vA_, vB_, H)); + Vector error(1); + error << E.error(vA_, vB_, H); + return error; } }; @@ -67,26 +89,50 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 { Point3 dP1_; ///< 3D point corresponding to measurement in image 1 - Point2 p1_, p2_; ///< Measurements in image 1 and image 2 - Cal3_S2 K_; ///< Calibration + Point2 pn_; ///< Measurement in image 2, in ideal coordinates + double f_; ///< approximate conversion factor for error scaling typedef NoiseModelFactor2 Base; typedef EssentialMatrixFactor2 This; public: - /// Constructor + /** + * Constructor + * @param pA point in first camera, in calibrated coordinates + * @param pB point in second camera, in calibrated coordinates + * @param model noise model should be in pixels, as well + */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Cal3_S2& K, const SharedNoiseModel& model) : - Base(model, key1, key2), p1_(pA), p2_(pB), K_(K) { - Point2 xy = K_.calibrate(p1_); - dP1_ = Point3(xy.x(), xy.y(), 1); + const SharedNoiseModel& model) : + Base(model, key1, key2) { + dP1_ = Point3(pA.x(), pA.y(), 1); + pn_ = pB; + f_ = 1.0; + } + + /** + * Constructor + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param K calibration object, will be used only in constructor + * @param model noise model should be in pixels, as well + */ + template + EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model, boost::shared_ptr K) : + Base(model, key1, key2) { + assert(K); + Point2 p1 = K->calibrate(pA); + dP1_ = Point3(p1.x(), p1.y(), 1); + pn_ = K->calibrate(pB); + f_ = 0.5 * (K->fx() + K->fy()); } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print @@ -94,7 +140,7 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << p1_.vector().transpose() << ")' and (" << p2_.vector().transpose() + << dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose() << ")'" << std::endl; } @@ -112,40 +158,38 @@ public: // The point d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate - Point2 pi; + Point2 pn; if (!DE && !Dd) { Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); - Point2 pn = SimpleCamera::project_to_camera(dP2); - pi = K_.uncalibrate(pn); + pn = SimpleCamera::project_to_camera(dP2); } else { // Calculate derivatives. TODO if slow: optimize with Mathematica - // 3*2 3*3 3*3 2*3 2*2 - Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2, Dpi_pn; + // 3*2 3*3 3*3 2*3 + Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; Point3 _1T2 = E.direction().point3(D_1T2_dir); Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); - Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); - pi = K_.uncalibrate(pn, boost::none, Dpi_pn); + pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); if (DE) { Matrix DdP2_E(3, 5); DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) - *DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5) + *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) } if (Dd) // efficient backwards computation: - // (2*2) * (2*3) * (3*3) * (3*1) - *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector()))); + // (2*3) * (3*3) * (3*1) + *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector())); } - Point2 reprojectionError = pi - p2_; - return reprojectionError.vector(); + Point2 reprojectionError = pn - pn_; + return f_ * reprojectionError.vector(); } }; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 5f9233bb3..fabcd47cf 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -18,7 +18,11 @@ using namespace std; using namespace gtsam; -typedef noiseModel::Isotropic::shared_ptr Model; +// Noise model for first type of factor is evaluating algebraic error +noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, + 0.01); +// Noise model for second type of factor is evaluating pixel coordinates +noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); namespace example1 { @@ -42,8 +46,6 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } -Cal3_S2 K; - //************************************************************************* TEST (EssentialMatrixFactor, testData) { CHECK(readOK); @@ -76,10 +78,9 @@ TEST (EssentialMatrixFactor, testData) { //************************************************************************* 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); + EssentialMatrixFactor factor(1, pA(i), pB(i), model1); // Check evaluation Vector expected(1); @@ -100,7 +101,7 @@ TEST (EssentialMatrixFactor, factor) { } //************************************************************************* -TEST (EssentialMatrixFactor, fromConstraints) { +TEST (EssentialMatrixFactor, minimization) { // 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 @@ -109,9 +110,8 @@ TEST (EssentialMatrixFactor, fromConstraints) { // We start with a factor graph and add constraints to it // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; - Model model = noiseModel::Isotropic::Sigma(1, 0.01); for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model)); + graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1)); // Check error at ground truth Values truth; @@ -147,15 +147,13 @@ TEST (EssentialMatrixFactor, fromConstraints) { //************************************************************************* TEST (EssentialMatrixFactor2, factor) { EssentialMatrix E(aRb, aTb); - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1); for (size_t i = 0; i < 5; i++) { - EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), K, model); + EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); // Check evaluation Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); - const Point2 pn = SimpleCamera::project_to_camera(P2); - const Point2 pi = K.uncalibrate(pn); + const Point2 pi = SimpleCamera::project_to_camera(P2); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); @@ -185,9 +183,8 @@ TEST (EssentialMatrixFactor2, minimization) { // We start with a factor graph and add constraints to it // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; - Model model = noiseModel::Isotropic::Sigma(2, 0.01); for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model)); + graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2)); // Check error at ground truth Values truth; @@ -235,8 +232,56 @@ Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } -// Matches Cal3Bundler K(500, 0, 0); -Cal3_S2 K(500, 500, 0, 0, 0); +boost::shared_ptr // +K = boost::make_shared < Cal3Bundler > (500, 0, 0); + +Vector vA(size_t i) { + Point2 xy = K->calibrate(pA(i)); + return EssentialMatrix::Homogeneous(xy); +} +Vector vB(size_t i) { + Point2 xy = K->calibrate(pB(i)); + return EssentialMatrix::Homogeneous(xy); +} + +//************************************************************************* +TEST (EssentialMatrixFactor, extraTest) { + // Additional test with camera moving in positive X direction + + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K)); + + // 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(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); + +} //************************************************************************* TEST (EssentialMatrixFactor2, extraTest) { @@ -245,9 +290,8 @@ TEST (EssentialMatrixFactor2, extraTest) { // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; - Model model = noiseModel::Isotropic::Sigma(2, 1); for (size_t i = 0; i < data.number_tracks(); i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model)); + graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); // Check error at ground truth Values truth; From fafa3326f32073be6f453817c9c7bdfa9b09ebda Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Jan 2014 00:59:22 -0500 Subject: [PATCH 11/19] Streaming implemented with test --- gtsam/geometry/tests/testEssentialMatrix.cpp | 36 ++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 56fc8d8ad..1e94f12f3 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -10,6 +10,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -121,6 +122,41 @@ TEST (EssentialMatrix, FromPose3_b) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +/* ************************************************************************* */ +ostream& operator <<(ostream& os, const gtsam::EssentialMatrix& E) { + gtsam::Rot3 R = E.rotation(); + gtsam::Sphere2 d = E.direction(); + os.precision(10); + os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + return os; +} + +/* ************************************************************************* */ +istream& operator >>(istream& fs, gtsam::EssentialMatrix& E) { + double rx, ry, rz, dx, dy, dz; + + // Read the rotation rxyz + fs >> rx >> ry >> rz; + + // Read the translation dxyz + fs >> dx >> dy >> dz; + + // Create EssentialMatrix: Construct from rotation and translation + gtsam::Rot3 rot = gtsam::Rot3::RzRyRx(rx, ry, rz); + gtsam::Sphere2 dt = gtsam::Sphere2(dx, dy, dz); + E = gtsam::EssentialMatrix(rot, dt); + return fs; +} + +//************************************************************************* +TEST (EssentialMatrix, streaming) { + EssentialMatrix expected(c1Rc2, c1Tc2), actual; + stringstream ss; + ss << expected; + ss >> actual; + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 1eafec036b08821391693ade5b7f83d5e490971a Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Jan 2014 00:59:55 -0500 Subject: [PATCH 12/19] added testEssentialMatrix.run --- .cproject | 346 +++++++++++++++++++++++++++--------------------------- 1 file changed, 172 insertions(+), 174 deletions(-) diff --git a/.cproject b/.cproject index 7398f3d12..280ac750d 100644 --- a/.cproject +++ b/.cproject @@ -365,6 +365,38 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + make -j5 @@ -453,34 +485,10 @@ true true - + make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run + -j5 + testEssentialMatrix.run true true true @@ -559,7 +567,6 @@ make - tests/testBayesTree.run true false @@ -567,7 +574,6 @@ make - testBinaryBayesNet.run true false @@ -615,7 +621,6 @@ make - testSymbolicBayesNet.run true false @@ -623,7 +628,6 @@ make - tests/testSymbolicFactor.run true false @@ -631,7 +635,6 @@ make - testSymbolicFactorGraph.run true false @@ -647,12 +650,19 @@ make - tests/testBayesTree true false true + + make + -j2 + testVSLAMGraph + true + true + true + make -j2 @@ -735,6 +745,7 @@ make + testSimulated2DOriented.run true false @@ -774,6 +785,7 @@ make + testSimulated2D.run true false @@ -781,6 +793,7 @@ make + testSimulated3D.run true false @@ -794,14 +807,6 @@ true true - - make - -j2 - testVSLAMGraph - true - true - true - make -j2 @@ -841,21 +846,6 @@ false true - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j5 @@ -994,7 +984,6 @@ make - testGraph.run true false @@ -1002,7 +991,6 @@ make - testJunctionTree.run true false @@ -1010,7 +998,6 @@ make - testSymbolicBayesNetB.run true false @@ -1072,6 +1059,22 @@ true true + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -1440,6 +1443,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1520,10 +1531,66 @@ true true - + make -j2 - testGaussianFactor.run + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run true true true @@ -1624,86 +1691,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1768,6 +1755,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -2209,7 +2212,6 @@ cpack - -G DEB true false @@ -2217,7 +2219,6 @@ cpack - -G RPM true false @@ -2225,7 +2226,6 @@ cpack - -G TGZ true false @@ -2233,7 +2233,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2407,30 +2406,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testSPQRUtil.run - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -2471,18 +2446,26 @@ true true - + make -j2 - tests/testPose2.run + check true true true - + make -j2 - tests/testPose3.run + tests/testSPQRUtil.run + true + true + true + + + make + -j2 + clean true true true @@ -2577,12 +2560,27 @@ make - testErrors.run true false true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + From 6c9fd16ce81e92162c72e5032f39c16c9c56b857 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Jan 2014 01:03:40 -0500 Subject: [PATCH 13/19] Moved streaming to header --- gtsam/geometry/EssentialMatrix.h | 29 ++++++++++++++++++++ gtsam/geometry/tests/testEssentialMatrix.cpp | 26 ------------------ 2 files changed, 29 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 3c5836b5e..afacad508 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,6 +10,7 @@ #include #include #include +#include namespace gtsam { @@ -145,6 +146,34 @@ public: /// @} + /// @name Streaming operators + /// @{ + + // stream to stream + friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E) { + Rot3 R = E.rotation(); + Sphere2 d = E.direction(); + os.precision(10); + os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + return os; + } + + /// stream from stream + friend std::istream& operator >>(std::istream& is, EssentialMatrix& E) { + double rx, ry, rz, dx, dy, dz; + is >> rx >> ry >> rz; // Read the rotation rxyz + is >> dx >> dy >> dz; // Read the translation dxyz + + // Create EssentialMatrix from rotation and translation + Rot3 rot = Rot3::RzRyRx(rx, ry, rz); + Sphere2 dt = Sphere2(dx, dy, dz); + E = EssentialMatrix(rot, dt); + + return is; + } + + /// @} + }; } // gtsam diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 1e94f12f3..59cc15581 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -122,32 +122,6 @@ TEST (EssentialMatrix, FromPose3_b) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } -/* ************************************************************************* */ -ostream& operator <<(ostream& os, const gtsam::EssentialMatrix& E) { - gtsam::Rot3 R = E.rotation(); - gtsam::Sphere2 d = E.direction(); - os.precision(10); - os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; - return os; -} - -/* ************************************************************************* */ -istream& operator >>(istream& fs, gtsam::EssentialMatrix& E) { - double rx, ry, rz, dx, dy, dz; - - // Read the rotation rxyz - fs >> rx >> ry >> rz; - - // Read the translation dxyz - fs >> dx >> dy >> dz; - - // Create EssentialMatrix: Construct from rotation and translation - gtsam::Rot3 rot = gtsam::Rot3::RzRyRx(rx, ry, rz); - gtsam::Sphere2 dt = gtsam::Sphere2(dx, dy, dz); - E = gtsam::EssentialMatrix(rot, dt); - return fs; -} - //************************************************************************* TEST (EssentialMatrix, streaming) { EssentialMatrix expected(c1Rc2, c1Tc2), actual; From a3a37131a16cf022232bf04f3dd1af4852df38aa Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Jan 2014 01:05:32 -0500 Subject: [PATCH 14/19] Moved streaming to implementation file --- gtsam/geometry/EssentialMatrix.cpp | 31 +++++++++++++++++++++++++++--- gtsam/geometry/EssentialMatrix.h | 23 +++------------------- 2 files changed, 31 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 9e50ad92a..cccedc5bb 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -8,6 +8,8 @@ #include #include +using namespace std; + namespace gtsam { /* ************************************************************************* */ @@ -35,8 +37,8 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, } /* ************************************************************************* */ -void EssentialMatrix::print(const std::string& s) const { - std::cout << s; +void EssentialMatrix::print(const string& s) const { + cout << s; aRb_.print("R:\n"); aTb_.print("d: "); } @@ -95,7 +97,7 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) } if (HR) { - throw std::runtime_error( + throw runtime_error( "EssentialMatrix::rotate: derivative HR not implemented yet"); /* HR->resize(5, 3); @@ -121,6 +123,29 @@ double EssentialMatrix::error(const Vector& vA, const Vector& vB, // return dot(vA, E_ * vB); } +/* ************************************************************************* */ +ostream& operator <<(ostream& os, const EssentialMatrix& E) { + Rot3 R = E.rotation(); + Sphere2 d = E.direction(); + os.precision(10); + os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + return os; +} + +/* ************************************************************************* */ +istream& operator >>(istream& is, EssentialMatrix& E) { + double rx, ry, rz, dx, dy, dz; + is >> rx >> ry >> rz; // Read the rotation rxyz + is >> dx >> dy >> dz; // Read the translation dxyz + + // Create EssentialMatrix from rotation and translation + Rot3 rot = Rot3::RzRyRx(rx, ry, rz); + Sphere2 dt = Sphere2(dx, dy, dz); + E = EssentialMatrix(rot, dt); + + return is; +} + /* ************************************************************************* */ } // gtsam diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index afacad508..a7270fff0 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -149,28 +149,11 @@ public: /// @name Streaming operators /// @{ - // stream to stream - friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E) { - Rot3 R = E.rotation(); - Sphere2 d = E.direction(); - os.precision(10); - os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; - return os; - } + /// stream to stream + friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); /// stream from stream - friend std::istream& operator >>(std::istream& is, EssentialMatrix& E) { - double rx, ry, rz, dx, dy, dz; - is >> rx >> ry >> rz; // Read the rotation rxyz - is >> dx >> dy >> dz; // Read the translation dxyz - - // Create EssentialMatrix from rotation and translation - Rot3 rot = Rot3::RzRyRx(rx, ry, rz); - Sphere2 dt = Sphere2(dx, dy, dz); - E = EssentialMatrix(rot, dt); - - return is; - } + friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); /// @} From f675dc3bd4227fa888e9734aa637d745429734ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 24 Jan 2014 16:03:58 -0500 Subject: [PATCH 15/19] ignore .DS_Store --- .gitignore | 1 + doc/.gitignore | 1 + doc/images/.gitignore | 1 + examples/.gitignore | 1 + examples/Data/.gitignore | 1 + 5 files changed, 5 insertions(+) create mode 100644 doc/.gitignore create mode 100644 doc/images/.gitignore create mode 100644 examples/.gitignore create mode 100644 examples/Data/.gitignore diff --git a/.gitignore b/.gitignore index 84c048a73..6d6c4d9ad 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ /build/ +/*.DS_Store diff --git a/doc/.gitignore b/doc/.gitignore new file mode 100644 index 000000000..a7cbee1ba --- /dev/null +++ b/doc/.gitignore @@ -0,0 +1 @@ +/*.DS_Store diff --git a/doc/images/.gitignore b/doc/images/.gitignore new file mode 100644 index 000000000..a7cbee1ba --- /dev/null +++ b/doc/images/.gitignore @@ -0,0 +1 @@ +/*.DS_Store diff --git a/examples/.gitignore b/examples/.gitignore new file mode 100644 index 000000000..a7cbee1ba --- /dev/null +++ b/examples/.gitignore @@ -0,0 +1 @@ +/*.DS_Store diff --git a/examples/Data/.gitignore b/examples/Data/.gitignore new file mode 100644 index 000000000..a7cbee1ba --- /dev/null +++ b/examples/Data/.gitignore @@ -0,0 +1 @@ +/*.DS_Store From 0464b38ca0455ac12d8f11a4c0f56615d6c2f703 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 25 Jan 2014 10:07:34 -0500 Subject: [PATCH 16/19] Merged changes from develop --- gtsam/geometry/Rot3.cpp | 407 ++++++------ gtsam/geometry/Rot3.h | 4 + gtsam/geometry/Rot3M.cpp | 616 +++++++++---------- gtsam/geometry/Rot3Q.cpp | 5 - gtsam/geometry/Sphere2.cpp | 87 ++- gtsam/geometry/Sphere2.h | 21 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 5 +- gtsam/geometry/tests/testSphere2.cpp | 89 ++- gtsam/geometry/tests/timePose3.cpp | 22 +- 9 files changed, 696 insertions(+), 560 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 8dcf14d4b..ef82e9369 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -1,198 +1,209 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, 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 - - * -------------------------------------------------------------------------- */ - -/** - * @file Rot3.cpp - * @brief Rotation, common code between Rotation matrix and Quaternion - * @author Alireza Fathi - * @author Christian Potthast - * @author Frank Dellaert - * @author Richard Roberts - */ - -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { - -static const Matrix3 I3 = Matrix3::Identity(); - -/* ************************************************************************* */ -void Rot3::print(const std::string& s) const { - gtsam::print((Matrix)matrix(), s); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Point3& w, double theta) { - return rodriguez((Vector)w.vector(),theta); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { - return rodriguez(w.point3(),theta); -} - -/* ************************************************************************* */ -Rot3 Rot3::Random(boost::random::mt19937 & rng) { - // TODO allow any engine without including all of boost :-( - Sphere2 w = Sphere2::Random(rng); - boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); - double angle = randomAngle(rng); - return rodriguez(w,angle); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3(); - return rodriguez(w/t, t); -} - -/* ************************************************************************* */ -bool Rot3::equals(const Rot3 & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); -} - -/* ************************************************************************* */ -Point3 Rot3::operator*(const Point3& p) const { - return rotate(p); -} - -/* ************************************************************************* */ -Sphere2 Rot3::rotate(const Sphere2& p, - boost::optional HR, boost::optional Hp) const { - Sphere2 q = rotate(p.point3(Hp)); - if (Hp) - (*Hp) = q.basis().transpose() * matrix() * (*Hp); - if (HR) - (*HR) = -q.basis().transpose() * matrix() * p.skew(); - return q; -} - -/* ************************************************************************* */ -Sphere2 Rot3::operator*(const Sphere2& p) const { - return rotate(p); -} - -/* ************************************************************************* */ -// see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; - return q; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; - return res; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpInvL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix res = eye(3) + 0.5*x - s2*x2; - return res; -} - - -/* ************************************************************************* */ -Point3 Rot3::column(int index) const{ - if(index == 3) - return r3(); - else if(index == 2) - return r2(); - else if(index == 1) - return r1(); // default returns r1 - else - throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); -} - -/* ************************************************************************* */ -Vector3 Rot3::xyz() const { - Matrix I;Vector3 q; - boost::tie(I,q)=RQ(matrix()); - return q; -} - -/* ************************************************************************* */ -Vector3 Rot3::ypr() const { - Vector3 q = xyz(); - return Vector3(q(2),q(1),q(0)); -} - -/* ************************************************************************* */ -Vector3 Rot3::rpy() const { - return xyz(); -} - -/* ************************************************************************* */ -Vector Rot3::quaternion() const { - Quaternion q = toQuaternion(); - Vector v(4); - v(0) = q.w(); - v(1) = q.x(); - v(2) = q.y(); - v(3) = q.z(); - return v; -} - -/* ************************************************************************* */ -pair RQ(const Matrix3& A) { - - double x = -atan2(-A(2, 1), A(2, 2)); - Rot3 Qx = Rot3::Rx(-x); - Matrix3 B = A * Qx.matrix(); - - double y = -atan2(B(2, 0), B(2, 2)); - Rot3 Qy = Rot3::Ry(-y); - Matrix3 C = B * Qy.matrix(); - - double z = -atan2(-C(1, 0), C(1, 1)); - Rot3 Qz = Rot3::Rz(-z); - Matrix3 R = C * Qz.matrix(); - - Vector xyz = Vector3(x, y, z); - return make_pair(R, xyz); -} - -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Rot3& R) { - os << "\n"; - os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; - os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; - os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; - return os; -} - -/* ************************************************************************* */ - -} // namespace gtsam - +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file Rot3.cpp + * @brief Rotation, common code between Rotation matrix and Quaternion + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +static const Matrix3 I3 = Matrix3::Identity(); + +/* ************************************************************************* */ +void Rot3::print(const std::string& s) const { + gtsam::print((Matrix)matrix(), s); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Point3& w, double theta) { + return rodriguez((Vector)w.vector(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { + return rodriguez(w.point3(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::Random(boost::random::mt19937 & rng) { + // TODO allow any engine without including all of boost :-( + Sphere2 w = Sphere2::Random(rng); + boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); + double angle = randomAngle(rng); + return rodriguez(w,angle); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Vector& w) { + double t = w.norm(); + if (t < 1e-10) return Rot3(); + return rodriguez(w/t, t); +} + +/* ************************************************************************* */ +bool Rot3::equals(const Rot3 & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); +} + +/* ************************************************************************* */ +Point3 Rot3::operator*(const Point3& p) const { + return rotate(p); +} + +/* ************************************************************************* */ +Sphere2 Rot3::rotate(const Sphere2& p, + boost::optional HR, boost::optional Hp) const { + Sphere2 q = rotate(p.point3(Hp)); + if (Hp) + (*Hp) = q.basis().transpose() * matrix() * (*Hp); + if (HR) + (*HR) = -q.basis().transpose() * matrix() * p.skew(); + return q; +} + +/* ************************************************************************* */ +Sphere2 Rot3::unrotate(const Sphere2& p, + boost::optional HR, boost::optional Hp) const { + Sphere2 q = unrotate(p.point3(Hp)); + if (Hp) + (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); + if (HR) + (*HR) = q.basis().transpose() * q.skew(); + return q; +} + +/* ************************************************************************* */ +Sphere2 Rot3::operator*(const Sphere2& p) const { + return rotate(p); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3::unrotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + const Matrix Rt(transpose()); + Point3 q(Rt*p.vector()); // q = Rt*p + if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); + if (H2) *H2 = Rt; + return q; +} + +/* ************************************************************************* */ +/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s1 = sin(vi)/vi; + double s2 = (theta - sin(theta))/(theta*theta*theta); + Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; + return res; +} + +/* ************************************************************************* */ +/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpInvL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + Matrix res = eye(3) + 0.5*x - s2*x2; + return res; +} + + +/* ************************************************************************* */ +Point3 Rot3::column(int index) const{ + if(index == 3) + return r3(); + else if(index == 2) + return r2(); + else if(index == 1) + return r1(); // default returns r1 + else + throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); +} + +/* ************************************************************************* */ +Vector3 Rot3::xyz() const { + Matrix I;Vector3 q; + boost::tie(I,q)=RQ(matrix()); + return q; +} + +/* ************************************************************************* */ +Vector3 Rot3::ypr() const { + Vector3 q = xyz(); + return Vector3(q(2),q(1),q(0)); +} + +/* ************************************************************************* */ +Vector3 Rot3::rpy() const { + return xyz(); +} + +/* ************************************************************************* */ +Vector Rot3::quaternion() const { + Quaternion q = toQuaternion(); + Vector v(4); + v(0) = q.w(); + v(1) = q.x(); + v(2) = q.y(); + v(3) = q.z(); + return v; +} + +/* ************************************************************************* */ +pair RQ(const Matrix3& A) { + + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3 Qx = Rot3::Rx(-x); + Matrix3 B = A * Qx.matrix(); + + double y = -atan2(B(2, 0), B(2, 2)); + Rot3 Qy = Rot3::Ry(-y); + Matrix3 C = B * Qy.matrix(); + + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3 Qz = Rot3::Rz(-z); + Matrix3 R = C * Qz.matrix(); + + Vector xyz = Vector3(x, y, z); + return make_pair(R, xyz); +} + +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Rot3& R) { + os << "\n"; + os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; + os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; + os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; + return os; +} + +/* ************************************************************************* */ + +} // namespace gtsam + diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ea59fab17..d302a3502 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -331,6 +331,10 @@ namespace gtsam { Sphere2 rotate(const Sphere2& p, boost::optional HR = boost::none, boost::optional Hp = boost::none) const; + /// unrotate 3D direction from world frame to rotated coordinate frame + Sphere2 unrotate(const Sphere2& p, boost::optional HR = boost::none, + boost::optional Hp = boost::none) const; + /// rotate 3D direction from rotated coordinate frame to world frame Sphere2 operator*(const Sphere2& p) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 99c5c619e..6257d8400 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -1,308 +1,308 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, 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 - - * -------------------------------------------------------------------------- */ - -/** - * @file Rot3M.cpp - * @brief Rotation (internal: 3*3 matrix representation*) - * @author Alireza Fathi - * @author Christian Potthast - * @author Frank Dellaert - * @author Richard Roberts - */ - -#include // Get GTSAM_USE_QUATERNIONS macro - -#ifndef GTSAM_USE_QUATERNIONS - -#include -#include -#include - -using namespace std; - -namespace gtsam { - -static const Matrix3 I3 = Matrix3::Identity(); - -/* ************************************************************************* */ -Rot3::Rot3() : rot_(Matrix3::Identity()) {} - -/* ************************************************************************* */ -Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = col1.vector(); - rot_.col(1) = col2.vector(); - rot_.col(2) = col3.vector(); -} - -/* ************************************************************************* */ -Rot3::Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33) { - rot_ << R11, R12, R13, - R21, R22, R23, - R31, R32, R33; -} - -/* ************************************************************************* */ -Rot3::Rot3(const Matrix3& R) { - rot_ = R; -} - -/* ************************************************************************* */ -Rot3::Rot3(const Matrix& R) { - if (R.rows()!=3 || R.cols()!=3) - throw invalid_argument("Rot3 constructor expects 3*3 matrix"); - rot_ = R; -} - -///* ************************************************************************* */ -//Rot3::Rot3(const Matrix3& R) : rot_(R) {} - -/* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} - -/* ************************************************************************* */ -Rot3 Rot3::Rx(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - 1, 0, 0, - 0, ct,-st, - 0, st, ct); -} - -/* ************************************************************************* */ -Rot3 Rot3::Ry(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - ct, 0, st, - 0, 1, 0, - -st, 0, ct); -} - -/* ************************************************************************* */ -Rot3 Rot3::Rz(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - ct,-st, 0, - st, ct, 0, - 0, 0, 1); -} - -/* ************************************************************************* */ -// Considerably faster than composing matrices above ! -Rot3 Rot3::RzRyRx(double x, double y, double z) { - double cx=cos(x),sx=sin(x); - double cy=cos(y),sy=sin(y); - double cz=cos(z),sz=sin(z); - double ss_ = sx * sy; - double cs_ = cx * sy; - double sc_ = sx * cy; - double cc_ = cx * cy; - double c_s = cx * sz; - double s_s = sx * sz; - double _cs = cy * sz; - double _cc = cy * cz; - double s_c = sx * cz; - double c_c = cx * cz; - double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3( - _cc,- c_s + ssc, s_s + csc, - _cs, c_c + sss, -s_c + css, - -sy, sc_, cc_ - ); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; -#ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); -#endif - - double c = cos(theta), s = sin(theta), c_1 = 1 - c; - - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; - - return Rot3( - c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(Matrix3(rot_*R2.rot_)); -} - -/* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -rot_; - return Rot3(Matrix3(rot_.transpose())); -} - -/* ************************************************************************* */ -Rot3 Rot3::between (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I3; - return Rot3(Matrix3(rot_.transpose()*R2.rot_)); -} - -/* ************************************************************************* */ -Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1 || H2) { - if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = rot_; - } - return Point3(rot_ * p.vector()); -} - -/* ************************************************************************* */ -// Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R) { - - static const double PI = boost::math::constants::pi(); - - const Matrix3& rot = R.rot_; - // Get trace(R) - double tr = rot.trace(); - - // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. - // we do something special - if (std::abs(tr+1.0) < 1e-10) { - if(std::abs(rot(2,2)+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*rot(2,2) )) * - Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); - else if(std::abs(rot(1,1)+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*rot(1,1))) * - Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); - else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*rot(0,0))) * - Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); - } else { - double magnitude; - double tr_3 = tr-3.0; // always negative - if (tr_3<-1e-7) { - double theta = acos((tr-1.0)/2.0); - magnitude = theta/(2.0*sin(theta)); - } else { - // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) - // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) - magnitude = 0.5 - tr_3*tr_3/12.0; - } - return magnitude*Vector3( - rot(2,1)-rot(1,2), - rot(0,2)-rot(2,0), - rot(1,0)-rot(0,1)); - } -} - -/* ************************************************************************* */ -Rot3 Rot3::retractCayley(const Vector& omega) const { - const double x = omega(0), y = omega(1), z = omega(2); - const double x2 = x * x, y2 = y * y, z2 = z * z; - const double xy = x * y, xz = x * z, yz = y * z; - const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; - return (*this) - * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, - (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, - (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); -} - -/* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return (*this)*Expmap(omega); - } else if(mode == Rot3::CAYLEY) { - return retractCayley(omega); - } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix Omega = skewSymmetric(omega); - return (*this)*Cayley<3>(-Omega/2); - } else { - assert(false); - exit(1); - } -} - -/* ************************************************************************* */ -Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return Logmap(between(T)); - } else if(mode == Rot3::CAYLEY) { - // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a=A(0,0),b=A(0,1),c=A(0,2); - const double d=A(1,0),e=A(1,1),f=A(1,2); - const double g=A(2,0),h=A(2,1),i=A(2,2); - const double di = d*i, ce = c*e, cd = c*d, fg=f*g; - const double M = 1 + e - f*h + i + e*i; - const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = (a * f - cd + f) * K; - const double y = (b * f - ce - c) * K; - const double z = (fg - di - d) * K; - return -2 * Vector3(x, y, z); - } else if(mode == Rot3::SLOW_CAYLEY) { - // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); - // using templated version of Cayley - Eigen::Matrix3d Omega = Cayley<3>(A); - return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); - } else { - assert(false); - exit(1); - } -} - -/* ************************************************************************* */ -Matrix3 Rot3::matrix() const { - return rot_; -} - -/* ************************************************************************* */ -Matrix3 Rot3::transpose() const { - return rot_.transpose(); -} - -/* ************************************************************************* */ -Point3 Rot3::r1() const { return Point3(rot_.col(0)); } - -/* ************************************************************************* */ -Point3 Rot3::r2() const { return Point3(rot_.col(1)); } - -/* ************************************************************************* */ -Point3 Rot3::r3() const { return Point3(rot_.col(2)); } - -/* ************************************************************************* */ -Quaternion Rot3::toQuaternion() const { - return Quaternion(rot_); -} - -/* ************************************************************************* */ - -} // namespace gtsam - -#endif +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file Rot3M.cpp + * @brief Rotation (internal: 3*3 matrix representation*) + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include // Get GTSAM_USE_QUATERNIONS macro + +#ifndef GTSAM_USE_QUATERNIONS + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +static const Matrix3 I3 = Matrix3::Identity(); + +/* ************************************************************************* */ +Rot3::Rot3() : rot_(Matrix3::Identity()) {} + +/* ************************************************************************* */ +Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { + rot_.col(0) = col1.vector(); + rot_.col(1) = col2.vector(); + rot_.col(2) = col3.vector(); +} + +/* ************************************************************************* */ +Rot3::Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33) { + rot_ << R11, R12, R13, + R21, R22, R23, + R31, R32, R33; +} + +/* ************************************************************************* */ +Rot3::Rot3(const Matrix3& R) { + rot_ = R; +} + +/* ************************************************************************* */ +Rot3::Rot3(const Matrix& R) { + if (R.rows()!=3 || R.cols()!=3) + throw invalid_argument("Rot3 constructor expects 3*3 matrix"); + rot_ = R; +} + +///* ************************************************************************* */ +//Rot3::Rot3(const Matrix3& R) : rot_(R) {} + +/* ************************************************************************* */ +Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} + +/* ************************************************************************* */ +Rot3 Rot3::Rx(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); +} + +/* ************************************************************************* */ +Rot3 Rot3::Ry(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); +} + +/* ************************************************************************* */ +Rot3 Rot3::Rz(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + ct,-st, 0, + st, ct, 0, + 0, 0, 1); +} + +/* ************************************************************************* */ +// Considerably faster than composing matrices above ! +Rot3 Rot3::RzRyRx(double x, double y, double z) { + double cx=cos(x),sx=sin(x); + double cy=cos(y),sy=sin(y); + double cz=cos(z),sz=sin(z); + double ss_ = sx * sy; + double cs_ = cx * sy; + double sc_ = sx * cy; + double cc_ = cx * cy; + double c_s = cx * sz; + double s_s = sx * sz; + double _cs = cy * sz; + double _cc = cy * cz; + double s_c = sx * cz; + double c_c = cx * cz; + double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + return Rot3( + _cc,- c_s + ssc, s_s + csc, + _cs, c_c + sss, -s_c + css, + -sy, sc_, cc_ + ); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Vector& w, double theta) { + // get components of axis \omega + double wx = w(0), wy=w(1), wz=w(2); + double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; +#ifndef NDEBUG + double l_n = wwTxx + wwTyy + wwTzz; + if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); +#endif + + double c = cos(theta), s = sin(theta), c_1 = 1 - c; + + double swx = wx * s, swy = wy * s, swz = wz * s; + double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; + double C11 = c_1*wwTyy, C12 = c_1*wy*wz; + double C22 = c_1*wwTzz; + + return Rot3( + c + C00, -swz + C01, swy + C02, + swz + C01, c + C11, -swx + C12, + -swy + C02, swx + C12, c + C22); +} + +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + +/* ************************************************************************* */ +Rot3 Rot3::operator*(const Rot3& R2) const { + return Rot3(Matrix3(rot_*R2.rot_)); +} + +/* ************************************************************************* */ +Rot3 Rot3::inverse(boost::optional H1) const { + if (H1) *H1 = -rot_; + return Rot3(Matrix3(rot_.transpose())); +} + +/* ************************************************************************* */ +Rot3 Rot3::between (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*rot_); + if (H2) *H2 = I3; + return Rot3(Matrix3(rot_.transpose()*R2.rot_)); +} + +/* ************************************************************************* */ +Point3 Rot3::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + if (H1 || H2) { + if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_; + } + return Point3(rot_ * p.vector()); +} + +/* ************************************************************************* */ +// Log map at identity - return the canonical coordinates of this rotation +Vector3 Rot3::Logmap(const Rot3& R) { + + static const double PI = boost::math::constants::pi(); + + const Matrix3& rot = R.rot_; + // Get trace(R) + double tr = rot.trace(); + + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. + // we do something special + if (std::abs(tr+1.0) < 1e-10) { + if(std::abs(rot(2,2)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(2,2) )) * + Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); + else if(std::abs(rot(1,1)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(1,1))) * + Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); + else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + return (PI / sqrt(2.0+2.0*rot(0,0))) * + Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); + } else { + double magnitude; + double tr_3 = tr-3.0; // always negative + if (tr_3<-1e-7) { + double theta = acos((tr-1.0)/2.0); + magnitude = theta/(2.0*sin(theta)); + } else { + // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) + magnitude = 0.5 - tr_3*tr_3/12.0; + } + return magnitude*Vector3( + rot(2,1)-rot(1,2), + rot(0,2)-rot(2,0), + rot(1,0)-rot(0,1)); + } +} + +/* ************************************************************************* */ +Rot3 Rot3::retractCayley(const Vector& omega) const { + const double x = omega(0), y = omega(1), z = omega(2); + const double x2 = x * x, y2 = y * y, z2 = z * z; + const double xy = x * y, xz = x * z, yz = y * z; + const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; + return (*this) + * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, + (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, + (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); +} + +/* ************************************************************************* */ +Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { + if(mode == Rot3::EXPMAP) { + return (*this)*Expmap(omega); + } else if(mode == Rot3::CAYLEY) { + return retractCayley(omega); + } else if(mode == Rot3::SLOW_CAYLEY) { + Matrix Omega = skewSymmetric(omega); + return (*this)*Cayley<3>(-Omega/2); + } else { + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { + if(mode == Rot3::EXPMAP) { + return Logmap(between(T)); + } else if(mode == Rot3::CAYLEY) { + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + // Mathematica closed form optimization (procrastination?) gone wild: + const double a=A(0,0),b=A(0,1),c=A(0,2); + const double d=A(1,0),e=A(1,1),f=A(1,2); + const double g=A(2,0),h=A(2,1),i=A(2,2); + const double di = d*i, ce = c*e, cd = c*d, fg=f*g; + const double M = 1 + e - f*h + i + e*i; + const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); + const double x = (a * f - cd + f) * K; + const double y = (b * f - ce - c) * K; + const double z = (fg - di - d) * K; + return -2 * Vector3(x, y, z); + } else if(mode == Rot3::SLOW_CAYLEY) { + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + // using templated version of Cayley + Eigen::Matrix3d Omega = Cayley<3>(A); + return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); + } else { + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +Matrix3 Rot3::matrix() const { + return rot_; +} + +/* ************************************************************************* */ +Matrix3 Rot3::transpose() const { + return rot_.transpose(); +} + +/* ************************************************************************* */ +Point3 Rot3::r1() const { return Point3(rot_.col(0)); } + +/* ************************************************************************* */ +Point3 Rot3::r2() const { return Point3(rot_.col(1)); } + +/* ************************************************************************* */ +Point3 Rot3::r3() const { return Point3(rot_.col(2)); } + +/* ************************************************************************* */ +Quaternion Rot3::toQuaternion() const { + return Quaternion(rot_); +} + +/* ************************************************************************* */ + +} // namespace gtsam + +#endif diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 26890c370..c5990153a 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -62,11 +62,6 @@ namespace gtsam { /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : quaternion_(q) {} - /* ************************************************************************* */ - void Rot3::print(const std::string& s) const { - gtsam::print((Matrix)matrix(), s); - } - /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 29a170c4d..2069fe0ee 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -14,6 +14,7 @@ * @date Feb 02, 2011 * @author Can Erdogan * @author Frank Dellaert + * @author Alex Trevor * @brief The Sphere2 class - basically a point on a unit sphere */ @@ -113,7 +114,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional H) const { } /* ************************************************************************* */ -Sphere2 Sphere2::retract(const Vector& v) const { +Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { // Get the vector form of the point and the basis matrix Vector p = Point3::Logmap(p_); @@ -121,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const { // Compute the 3D xi_hat vector Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); - Vector newPoint = p + xi_hat; + + if (mode == Sphere2::EXPMAP) { + double xi_hat_norm = xi_hat.norm(); - // Project onto the manifold, i.e. the closest point on the circle to the new location; - // same as putting it onto the unit circle - Vector projected = newPoint / newPoint.norm(); + // Avoid nan + if (xi_hat_norm == 0.0) { + if (v.norm () == 0.0) + return Sphere2 (point3 ()); + else + return Sphere2 (-point3 ()); + } + + Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); + return Sphere2(exp_p_xi_hat); + } else if (mode == Sphere2::RENORM) { + // Project onto the manifold, i.e. the closest point on the circle to the new location; + // same as putting it onto the unit circle + Vector newPoint = p + xi_hat; + Vector projected = newPoint / newPoint.norm(); - return Sphere2(Point3::Expmap(projected)); + return Sphere2(Point3::Expmap(projected)); + } else { + assert (false); + exit (1); + } } /* ************************************************************************* */ -Vector Sphere2::localCoordinates(const Sphere2& y) const { + Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const { - // Make sure that the angle different between x and y is less than 90. Otherwise, - // we can project x + xi_hat from the tangent space at x to y. - assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y."); + if (mode == Sphere2::EXPMAP) { + Matrix B = basis(); + + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + double theta = acos(p.transpose() * q); - // Get the basis matrix - Matrix B = basis(); - - // Create the vector forms of p and q (the Point3 of y). - Vector p = Point3::Logmap(p_); - Vector q = Point3::Logmap(y.p_); - - // Compute the basis coefficients [v0,v1] = (B'q)/(p'q). - double alpha = p.transpose() * q; - assert(alpha != 0.0); - Matrix coeffs = (B.transpose() * q) / alpha; - Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); - return result; + // the below will be nan if theta == 0.0 + if (p == q) + return (Vector (2) << 0, 0); + else if (p == (Vector)-q) + return (Vector (2) << M_PI, 0); + + Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta)); + Vector result = B.transpose() * result_hat; + + return result; + } else if (mode == Sphere2::RENORM) { + // Make sure that the angle different between x and y is less than 90. Otherwise, + // we can project x + xi_hat from the tangent space at x to y. + assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y."); + + // Get the basis matrix + Matrix B = basis(); + + // Create the vector forms of p and q (the Point3 of y). + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + + // Compute the basis coefficients [v0,v1] = (B'q)/(p'q). + double alpha = p.transpose() * q; + assert(alpha != 0.0); + Matrix coeffs = (B.transpose() * q) / alpha; + Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); + return result; + } else { + assert (false); + exit (1); + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 8555b5d30..ac8124139 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -14,6 +14,7 @@ * @date Feb 02, 2011 * @author Can Erdogan * @author Frank Dellaert + * @author Alex Trevor * @brief Develop a Sphere2 class - basically a point on a unit sphere */ @@ -22,6 +23,10 @@ #include #include +#ifndef SPHERE2_DEFAULT_COORDINATES_MODE + #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM +#endif + // (Cumbersome) forward declaration for random generator namespace boost { namespace random { @@ -106,6 +111,13 @@ public: return p_; } + /// Return unit-norm Vector + Vector unitVector(boost::optional H = boost::none) const { + if (H) + *H = basis(); + return (p_.vector ()); + } + /// Signed, vector-valued error between two directions Vector error(const Sphere2& q, boost::optional H = boost::none) const; @@ -129,11 +141,16 @@ public: return 2; } + enum CoordinatesMode { + EXPMAP, ///< Use the exponential map to retract + RENORM ///< Retract with vector addtion and renormalize. + }; + /// The retract function - Sphere2 retract(const Vector& v) const; + Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; /// The local coordinates function - Vector localCoordinates(const Sphere2& s) const; + Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; /// @} }; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 59cc15581..865f27f81 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -89,10 +89,7 @@ TEST (EssentialMatrix, rotate) { // Derivatives Matrix actH1, actH2; - try { - bodyE.rotate(cRb, actH1, actH2); - } catch (exception e) { - } // avoid exception + try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); EXPECT(assert_equal(expH1, actH1, 1e-8)); diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index b9aa84949..706aa8fb4 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -13,6 +13,8 @@ * @file testSphere2.cpp * @date Feb 03, 2012 * @author Can Erdogan + * @author Frank Dellaert + * @author Alex Trevor * @brief Tests the Sphere2 class */ @@ -76,10 +78,35 @@ TEST(Sphere2, rotate) { } } +//******************************************************************************* +static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) { + return R.unrotate (p); +} + +TEST(Sphere2, unrotate) { + Rot3 R = Rot3::yaw(-M_PI/4.0); + Sphere2 p(1, 0, 0); + Sphere2 expected = Sphere2(1, 1, 0); + Sphere2 actual = R.unrotate (p); + EXPECT(assert_equal(expected, actual, 1e-8)); + Matrix actualH, expectedH; + // Use numerical derivatives to calculate the expected Jacobian + { + expectedH = numericalDerivative21(unrotate_, R, p); + R.unrotate(p, actualH, boost::none); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } + { + expectedH = numericalDerivative22(unrotate_, R, p); + R.unrotate(p, boost::none, actualH); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } +} + //******************************************************************************* TEST(Sphere2, error) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // + r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8)); EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5)); EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5)); @@ -102,8 +129,8 @@ TEST(Sphere2, error) { //******************************************************************************* TEST(Sphere2, distance) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // + r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8); @@ -147,9 +174,20 @@ TEST(Sphere2, retract) { Vector v(2); v << 0.5, 0; Sphere2 expected(Point3(1, 0, 0.5)); - Sphere2 actual = p.retract(v); + Sphere2 actual = p.retract(v, Sphere2::RENORM); EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8)); +} + +//******************************************************************************* +TEST(Sphere2, retract_expmap) { + Sphere2 p; + Vector v(2); + v << (M_PI/2.0), 0; + Sphere2 expected(Point3(0, 0, 1)); + Sphere2 actual = p.retract(v, Sphere2::EXPMAP); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8)); } //******************************************************************************* @@ -199,6 +237,39 @@ TEST(Sphere2, localCoordinates_retract) { } } +//******************************************************************************* +// Let x and y be two Sphere2's. +// The equality x.localCoordinates(x.retract(v)) == v should hold. +TEST(Sphere2, localCoordinates_retract_expmap) { + + size_t numIterations = 10000; + Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = + Vector_(3, 1.0, 1.0, 1.0); + Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI); + for (size_t i = 0; i < numIterations; i++) { + + // Sleep for the random number generator (TODO?: Better create all of them first). + sleep(0); + + // Create the two Sphere2s. + // Unlike the above case, we can use any two sphers. + Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + + // Magnitude of the rotation can be at most pi + if (v12.norm () > M_PI) + v12 = v12 / M_PI; + Sphere2 s2 = s1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = s1.localCoordinates(s2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + Sphere2 actual_s2 = s1.retract(actual_v12); + EXPECT(assert_equal(s2, actual_s2, 1e-3)); + } +} + //******************************************************************************* //TEST( Pose2, between ) //{ @@ -245,13 +316,13 @@ TEST(Sphere2, Random) { // Check that is deterministic given same random seed Point3 expected(-0.667578, 0.671447, 0.321713); Point3 actual = Sphere2::Random(rng).point3(); - EXPECT(assert_equal(expected, actual, 1e-5)); + EXPECT(assert_equal(expected,actual,1e-5)); // Check that means are all zero at least Point3 expectedMean, actualMean; for (size_t i = 0; i < 100; i++) actualMean = actualMean + Sphere2::Random(rng).point3(); - actualMean = actualMean / 100; - EXPECT(assert_equal(expectedMean, actualMean, 0.1)); + actualMean = actualMean/100; + EXPECT(assert_equal(expectedMean,actualMean,0.1)); } //************************************************************************* diff --git a/gtsam/geometry/tests/timePose3.cpp b/gtsam/geometry/tests/timePose3.cpp index 9538ad4b4..13e630706 100644 --- a/gtsam/geometry/tests/timePose3.cpp +++ b/gtsam/geometry/tests/timePose3.cpp @@ -17,23 +17,23 @@ #include -#include +#include #include using namespace std; using namespace gtsam; -/* ************************************************************************* */ +/* ************************************************************************* */ #define TEST(TITLE,STATEMENT) \ - gttic_(TITLE); \ - for(int i = 0; i < n; i++) \ - STATEMENT; \ - gttoc_(TITLE); + gttic_(TITLE); \ + for(int i = 0; i < n; i++) \ + STATEMENT; \ + gttoc_(TITLE); int main() { - int n = 5000000; - cout << "NOTE: Times are reported for " << n << " calls" << endl; + int n = 5000000; + cout << "NOTE: Times are reported for " << n << " calls" << endl; double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; @@ -47,9 +47,9 @@ int main() TEST(between, T.between(T2)) TEST(between_derivatives, T.between(T2,H1,H2)) TEST(Logmap, Pose3::Logmap(T.between(T2))) - - // Print timings - tictoc_print_(); + + // Print timings + tictoc_print_(); return 0; } From f01ee3b5d9d6b88361fc5e1c8bab5bee5c93499d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 25 Jan 2014 10:18:58 -0500 Subject: [PATCH 17/19] MATLAB changes from develop --- matlab/+gtsam/EXPECT.m | 10 ++++ matlab/+gtsam/covarianceEllipse.m | 7 ++- matlab/gtsam_examples/MonocularVOExample.m | 56 +++++++++++++++++++ .../gtsam_examples/Pose2SLAMExample_graph.m | 2 +- matlab/gtsam_examples/StereoVOExample.m | 2 +- 5 files changed, 72 insertions(+), 5 deletions(-) create mode 100644 matlab/+gtsam/EXPECT.m create mode 100644 matlab/gtsam_examples/MonocularVOExample.m diff --git a/matlab/+gtsam/EXPECT.m b/matlab/+gtsam/EXPECT.m new file mode 100644 index 000000000..382c200cc --- /dev/null +++ b/matlab/+gtsam/EXPECT.m @@ -0,0 +1,10 @@ +function EXPECT(name,assertion) +% EXPECT throw a warning if an assertion fails +% +% EXPECT(name,assertion) +% - name of test +% - assertion + +if (assertion~=1) + warning(['EXPECT ' name ' fails']); +end diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m index 7728fc866..387d7cd12 100644 --- a/matlab/+gtsam/covarianceEllipse.m +++ b/matlab/+gtsam/covarianceEllipse.m @@ -1,16 +1,17 @@ -function covarianceEllipse(x,P,color) +function covarianceEllipse(x,P,color, k) % covarianceEllipse plots a Gaussian as an uncertainty ellipse % Based on Maybeck Vol 1, page 366 % k=2.296 corresponds to 1 std, 68.26% of all probability % k=11.82 corresponds to 3 std, 99.74% of all probability % -% covarianceEllipse(x,P,color) +% covarianceEllipse(x,P,color,k) % it is assumed x and y are the first two components of state x +% k is scaling for std deviations, defaults to 1 std [e,s] = eig(P(1:2,1:2)); s1 = s(1,1); s2 = s(2,2); -k = 2.296; +if nargin<4, k = 2.296; end; [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); line(ex,ey,'color',color); 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/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index eb1b03950..b4957cce3 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -13,7 +13,7 @@ import gtsam.* %% Find data file -datafile = findExampleDataFile('w100-odom.graph'); +datafile = findExampleDataFile('w100.graph'); %% Initialize graph, initial estimate, and odometry noise model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); 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 From 05625ff25e7c38d163d1d50afdd165fc1169d79e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 25 Jan 2014 10:21:58 -0500 Subject: [PATCH 18/19] Sphere2 and EssentialMatrix --- gtsam.h | 43 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/gtsam.h b/gtsam.h index 19a662d35..5d582df11 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 basis() 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(); From a606d0eab9a8862c21d080ad1e4422d026e2c2f2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 25 Jan 2014 11:09:20 -0500 Subject: [PATCH 19/19] EssentialMatrixFactor --- gtsam.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam.h b/gtsam.h index 5d582df11..73a626a12 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2316,6 +2316,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);