diff --git a/.cproject b/.cproject index 21ac9d913..2846f289c 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -584,6 +582,7 @@ make + tests/testBayesTree.run true false @@ -591,6 +590,7 @@ make + testBinaryBayesNet.run true false @@ -638,6 +638,7 @@ make + testSymbolicBayesNet.run true false @@ -645,6 +646,7 @@ make + tests/testSymbolicFactor.run true false @@ -652,6 +654,7 @@ make + testSymbolicFactorGraph.run true false @@ -667,6 +670,7 @@ make + tests/testBayesTree true false @@ -1002,6 +1006,7 @@ make + testErrors.run true false @@ -1231,6 +1236,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1313,7 +1358,6 @@ make - testSimulated2DOriented.run true false @@ -1353,7 +1397,6 @@ make - testSimulated2D.run true false @@ -1361,7 +1404,6 @@ make - testSimulated3D.run true false @@ -1375,46 +1417,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1672,6 +1674,7 @@ cpack + -G DEB true false @@ -1679,6 +1682,7 @@ cpack + -G RPM true false @@ -1686,6 +1690,7 @@ cpack + -G TGZ true false @@ -1693,6 +1698,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2419,6 +2425,7 @@ make + testGraph.run true false @@ -2426,6 +2433,7 @@ make + testJunctionTree.run true false @@ -2433,6 +2441,7 @@ make + testSymbolicBayesNetB.run true false @@ -2622,6 +2631,22 @@ true true + + make + -j5 + testInitializePose3.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + make -j2 @@ -2798,14 +2823,6 @@ true true - - make - -j5 - testLago.run - true - true - true - make -j5 @@ -2896,7 +2913,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index d4f93293c..232733273 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -44,24 +44,27 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); BOOST_FOREACH(const boost::shared_ptr& factor, g) { - Matrix3 Rijt; + Matrix3 Rij; boost::shared_ptr > pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) - Rijt = pose3Between->measured().rotation().matrix().transpose(); + Rij = pose3Between->measured().rotation().matrix(); else std::cout << "Error in buildLinearOrientationGraph" << std::endl; + // std::cout << "Rij \n" << Rij << std::endl; + const FastVector& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; - Matrix M9 = (Matrix(9,9) << Rijt, zero33, zero33, - zero33, Rijt, zero33, - zero33, zero33, Rijt); + Matrix M9 = Matrix::Zero(9,9); + M9.block(0,0,3,3) = Rij; + M9.block(3,3,3,3) = Rij; + M9.block(6,6,3,3) = Rij; linearGraph.add(key1, -I9, key2, M9, zero9, model); } // prior on the anchor orientation - linearGraph.add(keyAnchor, I9, (Vector(1) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model); + linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model); return linearGraph; } @@ -71,7 +74,18 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientations); Values validRot3; - + BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) { + Key key = it.first; + if (key != keyAnchor) { + const Vector& rotVector = it.second; + Matrix3 rotMat; + rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); + rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); + rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); + Rot3 initRot = Rot3(rotMat); + validRot3.insert(key, initRot); + } + } return validRot3; } @@ -112,9 +126,7 @@ Values computeOrientations(const NonlinearFactorGraph& pose3Graph) { VectorValues relaxedRot3 = relaxedGraph.optimize(); // normalize and compute Rot3 - Values initializedRot3 = normalizeRelaxedRotations(relaxedRot3); - - return initializedRot3; + return normalizeRelaxedRotations(relaxedRot3); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 4cd374b64..6800cc66c 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -46,19 +46,19 @@ namespace simple { // \ | / // x0 // -static Point3 p1 = Point3(0,0,0); -static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) ); -static Point3 p2 = Point3(1,2,0); -static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) ); -static Point3 p3 = Point3(0,2,0); -static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) ); -static Point3 p4 = Point3(-1,1,0); -static Rot3 R4 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) ); +static Point3 p0 = Point3(0,0,0); +static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) ); +static Point3 p1 = Point3(1,2,0); +static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) ); +static Point3 p2 = Point3(0,2,0); +static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) ); +static Point3 p3 = Point3(-1,1,0); +static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) ); -static Pose3 pose0 = Pose3(R1,p1); -static Pose3 pose1 = Pose3(R2,p2); -static Pose3 pose2 = Pose3(R3,p3); -static Pose3 pose3 = Pose3(R4,p4); +static Pose3 pose0 = Pose3(R0,p0); +static Pose3 pose1 = Pose3(R1,p1); +static Pose3 pose2 = Pose3(R2,p2); +static Pose3 pose3 = Pose3(R3,p3); NonlinearFactorGraph graph() { NonlinearFactorGraph g; @@ -83,7 +83,10 @@ TEST( InitializePose3, orientations ) { Values initial = InitializePose3::initializeOrientations(simple::graph()); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal(initial.at(0), simple::pose0, 1e-6)); + EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); + EXPECT(assert_equal(simple::R1, initial.at(x1), 1e-6)); + EXPECT(assert_equal(simple::R2, initial.at(x2), 1e-6)); + EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); }