From af2ff05822fae1aa0e0a5b90a75d52e0d66d2591 Mon Sep 17 00:00:00 2001
From: Adrian Ratter <adrianr@cse.unsw.edu.au>
Date: Sun, 14 Sep 2014 14:36:22 +1000
Subject: [PATCH] Minor change to debugging output

---
 crosbot_graphslam/src/graphSlamCPU.cpp | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/crosbot_graphslam/src/graphSlamCPU.cpp b/crosbot_graphslam/src/graphSlamCPU.cpp
index 8e884a8..ad1f745 100644
--- a/crosbot_graphslam/src/graphSlamCPU.cpp
+++ b/crosbot_graphslam/src/graphSlamCPU.cpp
@@ -471,10 +471,11 @@ void GraphSlamCPU::updateTrack(Pose icpPose, PointCloudPtr cloud, ros::Time stam
    }*/
 
    //temp output for printing slam positions
-   //if (stampTime > 1364015711.983) {
-   /*if (numIterations == 100) {
-      cout << "OUTPUTTIND STATE NOW" << endl << endl;
-      FILE *f = fopen("/home/adrianrobolab/mapVerify/slamPositions.txt", "w");
+   /*if (stampTime > 1364015711.983 && !alreadyOutput) {
+      alreadyOutput = true;
+   //if (numIterations == 100) {
+      cout << "OUTPUTTING STATE NOW" << endl << endl;
+      FILE *f = fopen("/home/adrian/slamPositions.txt", "w");
 
       //int scanCount = 0;
       for(int mapI = 0; mapI <= currentLocalMap; mapI++) {
@@ -495,7 +496,7 @@ void GraphSlamCPU::updateTrack(Pose icpPose, PointCloudPtr cloud, ros::Time stam
                ostringstream ss;
                ss << localMaps[mapI].scans[scanI]->stamp;
 
-               fprintf(f, "FLASER 0 %lf %lf %lf 0 0 0 %s NAME %s\n", 
+               fprintf(f, "FLASER 0 0 0 0 %lf %lf %lf %s NAME %s\n", 
                      globPosX, globPosY, globPosTh, ss.str().c_str(), ss.str().c_str());
                //for (int c = 0; c < localMaps[mapI].scans[scanI]->points.size(); c++) {
                //   fprintf(f, " %lf %lf", localMaps[mapI].scans[scanI]->points[c].x,
@@ -680,16 +681,16 @@ void GraphSlamCPU::finishMap(double angleError, double icpTh, Pose icpPose) {
             evaluateTempConstraints();
             optimiseGraph(optType);
          }
-         /*for (int numIterations = 1; numIterations < NumOfOptimisationIts * 2; numIterations++) {
+         for (int numIterations = 1; numIterations < 10 * 2; numIterations++) {
             getGlobalHessianMatrix();
-            if (numIterations == NumOfOptimisationIts && loopClosed) {
+            if (numIterations == 10 && loopClosed) {
                optType = 0;
                evaluateTempConstraints();
             }
             calculateOptimisationChange(numIterations, optType);
             updateGlobalPositions();
 
-         }*/
+         }
          updateRobotMapCentres();
          ros::WallTime t2 = ros::WallTime::now();
          ros::WallDuration tote = t2 - t1;
@@ -716,7 +717,7 @@ void GraphSlamCPU::finishMap(double angleError, double icpTh, Pose icpPose) {
          if (foundMoreLoops) {
             cout << "****Optimising again" << endl;
             optimiseGraph(0);
-            /*for (int numIterations = 1; numIterations < NumOfOptimisationIts; numIterations++) {
+            /*for (int numIterations = 1; numIterations < 10; numIterations++) {
                getGlobalHessianMatrix();
                calculateOptimisationChange(numIterations, 0);
                updateGlobalPositions();
-- 
GitLab