From 35d5b56b65536b1038485f809594ae6f215242d7 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Tue, 2 Sep 2014 20:56:36 -0400
Subject: [PATCH] still debugging

---
 gtsam/slam/InitializePose3.cpp           |  8 +++--
 gtsam/slam/tests/testInitializePose3.cpp | 41 ++++++++++++++++++++++++
 2 files changed, 46 insertions(+), 3 deletions(-)

diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index afb9be15f..7bf29535b 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -183,13 +183,14 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   double rho = 2*a*b;
   double mu_max = maxNodeDeg * rho;
   double stepsize = 2/mu_max; // = 1/(a b dG)
-  size_t maxIter = 1000;
+  size_t maxIter = 1;
 
   // gradient iterations
-  vector<double> reshapedCost;
   for(size_t it=0; it < maxIter; it++){
     //////////////////////////////////////////////////////////////////////////
     // compute the gradient at each node
+    std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
+        <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
     double maxGrad = 0;
     BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
       Key key = key_value.key;
@@ -213,6 +214,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
       } // end of i-th gradient computation
 
       double normGradKey = (grad.at(key)).norm();
+      std::cout << "key  " << DefaultKeyFormatter(key) <<" grad " << grad.at(key) << std::endl;
       if(normGradKey>maxGrad)
         maxGrad = normGradKey;
     } // end of loop over nodes
@@ -274,7 +276,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
 Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
   Vector3 logRot = Rot3::Logmap(R1.between(R2));
   if(logRot.norm()<1e-4){ // some tolerance
-    Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.0, 0.0, 0.0)) ); // some perturbation
+    Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation
     logRot = Rot3::Logmap(R1pert.between(R2));
   }
   double th = logRot.norm();
diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp
index b13ab9c32..2eb0b3e67 100644
--- a/gtsam/slam/tests/testInitializePose3.cpp
+++ b/gtsam/slam/tests/testInitializePose3.cpp
@@ -123,6 +123,42 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
   EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9);
 }
 
+/* *************************************************************************** *
+TEST( InitializePose3, orientationsCheckGradient ) {
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+
+  // Wrong initial guess - initialization should fix the rotations
+  Values givenPoses;
+  givenPoses.insert(x0,simple::pose0);
+  givenPoses.insert(x1,simple::pose0);
+  givenPoses.insert(x2,simple::pose0);
+  givenPoses.insert(x3,simple::pose0);
+  Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses);
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
+  EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
+  EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
+  EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
+}
+
+/* *************************************************************************** */
+TEST( InitializePose3, singleGradient ) {
+  Rot3 R1 = Rot3();
+  Matrix M = Matrix3::Zero();
+  M(0,1) = -1; M(1,0) = 1; M(2,2) = 1;
+  Rot3 R2 = Rot3(M);
+  double a = 6.010534238540223;
+  double b = 1.0;
+
+  Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
+  Vector expected = Vector3::Zero();
+  expected(2) = 1.962658662803917;
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(expected, actual, 1e-6));
+}
+
 /* *************************************************************************** */
 TEST( InitializePose3, orientationsGradient ) {
   NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
@@ -135,6 +171,11 @@ TEST( InitializePose3, orientationsGradient ) {
   givenPoses.insert(x3,simple::pose0);
   Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses);
 
+  const Key keyAnchor = symbol('Z', 9999999);
+  givenPoses.insert(keyAnchor,simple::pose0);
+  string g2oFile = "/home/aspn/Desktop/toyExample.g2o";
+  writeG2o(pose3Graph, givenPoses, g2oFile);
+
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
   EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));