diff --git a/.cproject b/.cproject index 1b4d0b1ec..90e48745b 100644 --- a/.cproject +++ b/.cproject @@ -788,18 +788,18 @@ true true - + make -j5 - testGaussianFactorGraph.run + testGaussianFactorGraphUnordered.run true true true - + make -j5 - testGaussianBayesNet.run + testGaussianBayesNetUnordered.run true true true @@ -2631,6 +2631,22 @@ true true + + make + -j5 + testGPSFactor.run + true + true + true + + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + make -j5 diff --git a/examples/Data/simpleGraph10gradIter.txt b/examples/Data/simpleGraph10gradIter.txt new file mode 100644 index 000000000..c50171468 --- /dev/null +++ b/examples/Data/simpleGraph10gradIter.txt @@ -0,0 +1,11 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816 +VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026 +VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610 +VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180 +VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502 +EDGE_SE3:QUAT 1 2 1.000000 2.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 2 3 -0.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 3 4 1.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 3 1 0.000001 2.000000 0.000000 0.0000000 0.0000000 1.0000000 0.0000002 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 1 4 -1.000000 1.000000 0.000000 0.0000000 0.0000000 -0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.0000000 0.0000000 0.0000000 1.0000000 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index c571cd8e7..97c8a541e 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -147,7 +147,7 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { /* ************************************************************************* */ // Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter) { +Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 @@ -219,7 +219,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - // std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; + //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -243,7 +243,10 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Key key = key_value.key; if (key != keyAnchor) { const Rot3& R = inverseRot.at(key); - estimateRot.insert(key, Rref.compose(R.inverse())); + if(setRefFrame) + estimateRot.insert(key, Rref.compose(R.inverse())); + else + estimateRot.insert(key, R.inverse()); } } return estimateRot; diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index a19933bb5..dba5ceac3 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -39,7 +39,8 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 10000); +GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, + const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph); diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 76e278f28..0cea2cead 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -153,18 +153,12 @@ TEST( InitializePose3, iterationGradient ) { givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); size_t maxIter = 1; // test gradient at the first iteration - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter); + bool setRefFrame = false; + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); - // const Key keyAnchor = symbol('Z', 9999999); - // Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984, - // 0.002460082752984, 0.999993962808392, -0.002454045561375, - // -0.002454045561375, 0.002460082752984, 0.999993962808392); - // Rot3 RzExpected = Rot3(Mz); - // EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-6)); - - Matrix M0 = (Matrix(3,3) << 0.999344848920642, -0.036021919324717, 0.003506317718352, - 0.036032601656108, 0.999346013522419, -0.003032634950127, - -0.003394783302464, 0.003156989865691, 0.999989254372924); + Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, + 0.033572116359134, 0.999436104312325, -0.000621610948719, + -0.000983333645009, 0.000654992453817, 0.999999302019670); Rot3 R0Expected = Rot3(M0); EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-5)); @@ -199,42 +193,30 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); // do 10 gradient iterations - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10); + bool setRefFrame = false; + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); // const Key keyAnchor = symbol('Z', 9999999); // givenPoses.insert(keyAnchor,simple::pose0); // string g2oFile = "/home/aspn/Desktop/toyExample.g2o"; // writeG2o(pose3Graph, givenPoses, g2oFile); - // const Key keyAnchor = symbol('Z', 9999999); - // Matrix Mz = (Matrix(3,3) << 0.983348036379704, -0.181672808000167, 0.004650825895948, - // 0.181688570817424, 0.983350839452522, -0.003223318529546, - // -0.003987804220587, 0.004014645856811, 0.999983989889910); - // Rot3 RzExpected = Rot3(Mz); - // EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-4)); + const string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter"); + NonlinearFactorGraph::shared_ptr matlabGraph; + Values::shared_ptr matlabValues; + bool is3D = true; + boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D); - Matrix M0 = (Matrix(3,3) << 0.946965375724015, -0.321288672646614, 0.005492359133630, - 0.321308000189570, 0.946969747977338, -0.003076593882320, - -0.004212623179851, 0.004678166811270, 0.999980184084280); - Rot3 R0Expected = Rot3(M0); + Rot3 R0Expected = matlabValues->at(1).rotation(); EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-4)); - Matrix M1 = (Matrix(3,3) << 0.998716651908197, -0.050557818750367, 0.002992685163299, - 0.050577444118653, 0.998696360370342, -0.006892164352146, - -0.002640330984207, 0.007034681672788, 0.999971770554577); - Rot3 R1Expected = Rot3(M1); + Rot3 R1Expected = matlabValues->at(2).rotation(); EXPECT(assert_equal(R1Expected, orientations.at(x1), 1e-4)); - Matrix M2 = (Matrix(3,3) << 0.837033946147607, 0.547150466557869, 0.000734807505930, - -0.547150098128722, 0.837029489230720, 0.002899012916604, - 0.000971140718506, -0.002828622220494, 0.999995527881019); - Rot3 R2Expected = Rot3(M2); + Rot3 R2Expected = matlabValues->at(3).rotation(); EXPECT(assert_equal(R2Expected, orientations.at(x2), 1e-3)); - Matrix M3 = (Matrix(3,3) << 0.999422151199602, -0.033471815469964, 0.005916186331164, - 0.033474965560969, 0.999439461971174, -0.000434206705656, - -0.005898336397012, 0.000631999933520, 0.999982404947123); - Rot3 R3Expected = Rot3(M3); + Rot3 R3Expected = matlabValues->at(4).rotation(); EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-4)); }