From 23b5254c76d2aa2096f14c21d410320aae9616fb Mon Sep 17 00:00:00 2001
From: acoromin <acoromin@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Mon, 23 Mar 2015 14:47:10 +0000
Subject: [PATCH] Buf fixed that allocated too much memory for state vector

---
 src/examples/test_ceres_2_lasers.cpp | 6 +++---
 src/examples/test_ceres_laser.cpp    | 4 ++--
 src/landmark_base.h                  | 6 ++++++
 src/map_base.cpp                     | 2 +-
 src/node_base.cpp                    | 9 +++++----
 src/node_base.h                      | 3 ++-
 src/wolf_problem.cpp                 | 3 +++
 7 files changed, 22 insertions(+), 11 deletions(-)

diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 6ca7d8436..12ad327fe 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -135,9 +135,9 @@ int main(int argc, char** argv)
 	string modelFileName;
 
 	//model and initial view point
-	modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
+	//modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
     //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
-    //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
+    modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
 	devicePose.setPose(2,8,0.2,0,0,0);
 	viewPoint.setPose(devicePose);
 	viewPoint.moveForward(10);
@@ -176,7 +176,7 @@ int main(int argc, char** argv)
 	ground_truth.head(3) = pose_odom;
 	odom_trajectory.head(3) = pose_odom;
 
-	WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, pose_odom, 0.3, window_size);
+	WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e6, pose_odom, 0.3, window_size);
 
 	//std::cout << "START TRAJECTORY..." << std::endl;
 	// START TRAJECTORY ============================================================================================
diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp
index 424b5c535..cf8375e6d 100644
--- a/src/examples/test_ceres_laser.cpp
+++ b/src/examples/test_ceres_laser.cpp
@@ -318,9 +318,9 @@ int main(int argc, char** argv)
 	string modelFileName;
 
 	//model and initial view point
-	modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
+	//modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
     //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
-    //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
+    modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
 	devicePose.setPose(2,8,0.2,0,0,0);
 	//glut initialization
 	//glutInit(&argc, argv);
diff --git a/src/landmark_base.h b/src/landmark_base.h
index c23d4fe98..33d678bb8 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -18,6 +18,12 @@ class NodeTerminus;
 #include "node_linked.h"
 #include "map_base.h"
 
+// why v, w and a ?
+// add descriptor as a StateBase -> Could be estimated or not. Aperture could be one case of "descriptor"that can be estimated or not
+// orientation ? -> Is it also a descriptor ?
+// init and end Time stamps 
+//
+
 //class LandmarkBase
 class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
 {
diff --git a/src/map_base.cpp b/src/map_base.cpp
index f892f9da2..02d809215 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -3,7 +3,7 @@
 MapBase::MapBase() :
     NodeLinked(MID, "MAP")
 {
-    //
+    std::cout << "MapBase::MapBase(): " << __LINE__ << std::endl;
 }
 
 MapBase::~MapBase()
diff --git a/src/node_base.cpp b/src/node_base.cpp
index 8423ac3c7..57bcc414c 100644
--- a/src/node_base.cpp
+++ b/src/node_base.cpp
@@ -3,12 +3,13 @@
 //init static node counter
 unsigned int NodeBase::node_id_count_ = 0;
 
-NodeBase::NodeBase(std::string _label) :
-        label_(_label), //
+NodeBase::NodeBase(std::string _label, bool _verbose) :
+        label_(_label),
         node_id_(++node_id_count_),
-		node_pending_(ADD_PENDING)
+		node_pending_(ADD_PENDING), 
+		verbose_(_verbose)
 {
-    //    std::cout << "NodeID::constructor. Id: " << node_id_ << std::endl;
+        if (verbose_) std::cout << "NodeBase::NodeBase(). Id: " << node_id_ << " Label: " << label_ << std::endl;
 }
 
 NodeBase::~NodeBase()
diff --git a/src/node_base.h b/src/node_base.h
index afe5b7621..0716fff51 100644
--- a/src/node_base.h
+++ b/src/node_base.h
@@ -19,6 +19,7 @@ class NodeBase
         unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
         static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
         PendingStatus node_pending_; ///< Pending to be added/updated in the filter or optimizer
+        bool verbose_; 
 
     public: 
 
@@ -27,7 +28,7 @@ class NodeBase
          * Constructor from label
 		 * 
          */
-        NodeBase(std::string _label);
+        NodeBase(std::string _label, bool _verbose = false);
 
         /** \brief Default destructor
          *
diff --git a/src/wolf_problem.cpp b/src/wolf_problem.cpp
index d60d725e7..73e45e1f4 100644
--- a/src/wolf_problem.cpp
+++ b/src/wolf_problem.cpp
@@ -11,8 +11,11 @@ WolfProblem::WolfProblem(unsigned int _size) :
 {
 //	map_ptr_ = new MapBase;
 //	trajectory_ptr_ = new TrajectoryBase;
+    std::cout << "WolfProblem::WolfProblem(): " << __LINE__ << std::endl;
 	map_ptr_->linkToUpperNode( this );
+    std::cout << "WolfProblem::WolfProblem(): " << __LINE__ << std::endl;
 	trajectory_ptr_->linkToUpperNode( this );
+    std::cout << "WolfProblem::WolfProblem(): " << __LINE__ << std::endl;
 }
 
 WolfProblem::WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr, unsigned int _size) :
-- 
GitLab