completed lago example

examples/tests/testPlanarSLAMExample_lago.cpp

@@ -202,13 +202,13 @@ GaussianFactorGraph buildOrientationGraph(const vector<size_t>& spanningTree, co
 }
 
 /* ************************************************************************* */
-VectorValues initializeLago(const NonlinearFactorGraph& graph) {
+// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
+VectorValues initializeLago(const NonlinearFactorGraph& graph, vector<Key>& keysInBinary) {
   // Find a minimum spanning tree
   PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
       BetweenFactor<Pose2> >(graph);
 
   // Create a linear factor graph (LFG) of scalars
-  vector<Key> keysInBinary;
   map<Key, double> deltaThetaMap;
   vector<size_t> spanningTree; // ids of between factors forming the spanning tree T
   vector<size_t> chords; // ids of between factors corresponding to chords wrt T
@@ -226,6 +226,34 @@ VectorValues initializeLago(const NonlinearFactorGraph& graph) {
   return estimateLago;
 }
 
+/* ************************************************************************* */
+// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
+VectorValues initializeLago(const NonlinearFactorGraph& graph) {
+
+  vector<Key> keysInBinary;
+  return initializeLago(graph, keysInBinary);
+}
+
+/* ************************************************************************* */
+// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
+Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) {
+  Values initialGuessLago;
+
+  // get the orientation estimates from LAGO
+  vector<Key> keysInBinary;
+  VectorValues orientations = initializeLago(graph, keysInBinary);
+
+  // plug the orientations in the initialGuess
+  for(size_t i=0; i<keysInBinary.size(); i++){
+    Key key = keysInBinary[i];
+    Pose2 pose = initialGuess.at<Pose2>(key);
+    Vector orientation = orientations.at(key);
+    Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0));
+    initialGuessLago.insert(key, poseLago);
+  }
+  return initialGuessLago;
+}
+
 
 namespace simple {
 // We consider a small graph:
@@ -332,7 +360,7 @@ TEST( Lago, regularizedMeasurements ) {
 }
 
 /* *************************************************************************** */
-TEST( Lago, smallGraph_GTmeasurements ) {
+TEST( Lago, smallGraphVectorValues ) {
 
   VectorValues initialGuessLago = initializeLago(simple::graph());
 
@@ -343,6 +371,29 @@ TEST( Lago, smallGraph_GTmeasurements ) {
   EXPECT(assert_equal((Vector(1) << 1.5 * PI - 2*PI), initialGuessLago.at(x3), 1e-6));
 }
 
+/* *************************************************************************** */
+TEST( Lago, smallGraphValues ) {
+
+  // we set the orientations in the initial guess to zero
+  Values initialGuess;
+  initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0));
+  initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0));
+  initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0));
+  initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0));
+
+  // lago does not touch the Cartesian part and only fixed the orientations
+  Values actual = initializeLago(simple::graph(), initialGuess);
+
+  // we are in a noiseless case
+  Values expected;
+  expected.insert(x0,simple::pose0);
+  expected.insert(x1,simple::pose1);
+  expected.insert(x2,simple::pose2);
+  expected.insert(x3,simple::pose3);
+
+  EXPECT(assert_equal(expected, actual, 1e-6));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;