diff --git a/.cproject b/.cproject index 0a809847f..07c34bf38 100644 --- a/.cproject +++ b/.cproject @@ -307,14 +307,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -341,6 +333,7 @@ make + tests/testBayesTree.run true false @@ -348,6 +341,7 @@ make + testBinaryBayesNet.run true false @@ -395,6 +389,7 @@ make + testSymbolicBayesNet.run true false @@ -402,6 +397,7 @@ make + tests/testSymbolicFactor.run true false @@ -409,6 +405,7 @@ make + testSymbolicFactorGraph.run true false @@ -424,11 +421,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -517,22 +523,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -549,6 +539,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -573,34 +579,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -685,26 +683,34 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run true true true @@ -1055,6 +1061,7 @@ make + testGraph.run true false @@ -1062,6 +1069,7 @@ make + testJunctionTree.run true false @@ -1069,6 +1077,7 @@ make + testSymbolicBayesNetB.run true false @@ -1228,6 +1237,7 @@ make + testErrors.run true false @@ -1273,14 +1283,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1361,6 +1363,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1513,98 +1523,74 @@ true true - + make - -j2 - tests/testStereoCamera.run + -j5 + testStereoCamera.run true true true - + make - -j2 - tests/testRot3M.run + -j5 + testRot3M.run true true true - + make - -j2 - tests/testPoint3.run + -j5 + testPoint3.run true true true - + make - -j2 - tests/testCalibratedCamera.run + -j5 + testCalibratedCamera.run true true true - + make - -j2 - tests/timeRot3.run + -j5 + timeStereoCamera.run true true true - - make - -j2 - tests/timePose3.run - true - true - true - - - make - -j2 - tests/timeStereoCamera.run - true - true - true - - + make -j1 VERBOSE=1 - geometry.testHomography2.run + testHomography2.run true false true - + make -j5 - geometry.testPoint2.run + testPoint2.run true true true - + make -j5 - geometry.testPose2.run + testPose2.run true true true - + make -j5 - geometry.testPose3.run - true - true - true - - - make - -j5 - testSimpleCamera.run + testPose3.run true true true @@ -1691,7 +1677,6 @@ make - testSimulated2DOriented.run true false @@ -1731,7 +1716,6 @@ make - testSimulated2D.run true false @@ -1739,7 +1723,6 @@ make - testSimulated3D.run true false @@ -1939,7 +1922,6 @@ make - tests/testGaussianISAM2 true false @@ -1961,102 +1943,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2258,6 +2144,7 @@ cpack + -G DEB true false @@ -2265,6 +2152,7 @@ cpack + -G RPM true false @@ -2272,6 +2160,7 @@ cpack + -G TGZ true false @@ -2279,6 +2168,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2420,34 +2310,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2491,6 +2445,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 799668dd4..a630d8be7 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -283,4 +284,40 @@ namespace gtsam { } return r; } + + /* ************************************************************************* */ + boost::optional align(const vector& pairs) { + const size_t n = pairs.size(); + if (n<3) return boost::none; // we need at least three pairs + + // calculate centroids + Vector cp = zero(3),cq = zero(3); + BOOST_FOREACH(const Point3Pair& pair, pairs) { + cp += pair.first.vector(); + cq += pair.second.vector(); + } + double f = 1.0/n; + cp *= f; cq *= f; + + // Add to form H matrix + Matrix H = zeros(3,3); + BOOST_FOREACH(const Point3Pair& pair, pairs) { + Vector dp = pair.first.vector() - cp; + Vector dq = pair.second.vector() - cq; + H += dp * dq.transpose(); + } + + // Compute SVD + Matrix U,V; + Vector S; + svd(H,U,S,V); + + // Recover transform with correction from Eggert97machinevisionandapplications + Matrix UVtranspose = U * V.transpose(); + Matrix detWeighting = eye(3,3); + detWeighting(2,2) = UVtranspose.determinant(); + Rot3 R(Matrix(V * detWeighting * U.transpose())); + Point3 t = Point3(cq) - R * Point3(cp); + return Pose3(R, t); + } } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4a64f48b5..923f7b9bb 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -179,7 +179,7 @@ namespace gtsam { * as detailed in [Kobilarov09siggraph] eq. (15) and C_TLN. * The full formula is documented in [Celledoni99cmame] * Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and - * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421Р438, 2003. + * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421� 438, 2003. */ static Matrix6 dExpInv_TLN(const Vector& xi); @@ -300,4 +300,11 @@ namespace gtsam { return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); } + /** + * Calculate pose between a vector of 3D point correspondences (p,q) + * where q = Pose3::transform_from(p) = t + R*p + */ + typedef std::pair Point3Pair; + GTSAM_EXPORT boost::optional align(const std::vector& pairs); + } // namespace gtsam diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 1d78d3c14..94c0ecac6 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -19,6 +19,10 @@ #include #include #include + +#include // for operator += +using namespace boost::assign; + #include #include @@ -633,6 +637,40 @@ TEST( Pose3, adjoint) { EXPECT(assert_equal(expected,res,1e-5)); } +/* ************************************************************************* */ +TEST(Pose3, align_1) { + Pose3 expected(Rot3(), Point3(10,10,0)); + + vector correspondences; + Point3Pair pq1(make_pair(Point3(0,0,0), Point3(10,10,0))); + Point3Pair pq2(make_pair(Point3(20,10,0), Point3(30,20,0))); + Point3Pair pq3(make_pair(Point3(10,20,0), Point3(20,30,0))); + correspondences += pq1, pq2, pq3; + + boost::optional actual = align(correspondences); + EXPECT(assert_equal(expected, *actual)); +} + +/* ************************************************************************* */ +TEST(Pose3, align_2) { + Point3 t(20,10,5); + Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1); + Pose3 expected(R, t); + + vector correspondences; + Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30); + Point3 q1 = expected.transform_from(p1), + q2 = expected.transform_from(p2), + q3 = expected.transform_from(p3); + Point3Pair pq1(make_pair(p1, q1)); + Point3Pair pq2(make_pair(p2, q2)); + Point3Pair pq3(make_pair(p3, q3)); + correspondences += pq1, pq2, pq3; + + boost::optional actual = align(correspondences); + EXPECT(assert_equal(expected, *actual, 1e-5)); +} + /* ************************************************************************* */ /// exp(xi) exp(y) = exp(xi + x) /// Hence, y = log (exp(-xi)*exp(xi+x))