diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index f5c2600bd0917e63bb914707c5e182f5c6a15ce7..b6a6410f6d308f31feed72d0e7c97711456419a3 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -2,76 +2,39 @@
 ADD_EXECUTABLE(test_node_linked test_node_linked.cpp)
 TARGET_LINK_LIBRARIES(test_node_linked ${PROJECT_NAME})
 
-# numeric test
-ADD_EXECUTABLE(test_ceres_wrapper_numeric test_ceres_wrapper_numeric.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_numeric ${PROJECT_NAME})
-
-# jet test
-ADD_EXECUTABLE(test_ceres_wrapper_jet test_ceres_wrapper_jet.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet ${PROJECT_NAME})
-
-# jet individual blocks test
-ADD_EXECUTABLE(test_ceres_wrapper_jet_ind_block test_ceres_wrapper_jet_ind_block.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_ind_block ${PROJECT_NAME})
-
-# jet test with 2 sensors
-ADD_EXECUTABLE(test_ceres_wrapper_jet_2_sensors test_ceres_wrapper_jet_2_sensors.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_2_sensors ${PROJECT_NAME})
-
-# jet test manager
-#ADD_EXECUTABLE(test_ceres_manager test_ceres_manager.cpp)
-#TARGET_LINK_LIBRARIES(test_ceres_manager ${PROJECT_NAME})
-
-# Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-ADD_EXECUTABLE(test_ceres_odom_batch test_ceres_odom_batch.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_odom_batch ${PROJECT_NAME})
-
-# Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-ADD_EXECUTABLE(test_ceres_odom_iterative test_ceres_odom_iterative.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME})
-
 # Testing Eigen Permutations
-ADD_EXECUTABLE(test_permutations test_permutations.cpp)
+ADD_EXECUTABLE(test_permutations solver/test_permutations.cpp)
 TARGET_LINK_LIBRARIES(test_permutations ${PROJECT_NAME})
 
-
 IF(Suitesparse_FOUND)
     # Testing a ccolamd test
-    ADD_EXECUTABLE(test_ccolamd test_ccolamd.cpp)
+    ADD_EXECUTABLE(test_ccolamd solver/test_ccolamd.cpp)
     TARGET_LINK_LIBRARIES(test_ccolamd ${PROJECT_NAME})
     
     # Testing a blocks ccolamd test
-    ADD_EXECUTABLE(test_ccolamd_blocks test_ccolamd_blocks.cpp)
+    ADD_EXECUTABLE(test_ccolamd_blocks solver/test_ccolamd_blocks.cpp)
     TARGET_LINK_LIBRARIES(test_ccolamd_blocks ${PROJECT_NAME})
     
     # Testing an incremental blocks ccolamd test
-    ADD_EXECUTABLE(test_incremental_ccolamd_blocks test_incremental_ccolamd_blocks.cpp)
+    ADD_EXECUTABLE(test_incremental_ccolamd_blocks solver/test_incremental_ccolamd_blocks.cpp)
     TARGET_LINK_LIBRARIES(test_incremental_ccolamd_blocks ${PROJECT_NAME})
     
     # Testing an incremental QR with block ccolamd test
-    ADD_EXECUTABLE(test_iQR test_iQR.cpp)
+    ADD_EXECUTABLE(test_iQR solver/test_iQR.cpp)
     TARGET_LINK_LIBRARIES(test_iQR ${PROJECT_NAME})
     
     # Testing QR solver test tending to wolf
-    ADD_EXECUTABLE(test_iQR_wolf test_iQR_wolf.cpp)
+    ADD_EXECUTABLE(test_iQR_wolf solver/test_iQR_wolf.cpp)
     TARGET_LINK_LIBRARIES(test_iQR_wolf ${PROJECT_NAME})
     
     # Testing SPQR simple test
-    #ADD_EXECUTABLE(test_SPQR test_SPQR.cpp)
+    #ADD_EXECUTABLE(test_SPQR solver/test_SPQR.cpp)
     #TARGET_LINK_LIBRARIES(test_SPQR ${PROJECT_NAME})
 ENDIF(Suitesparse_FOUND)
 
 # Building and populating the wolf tree 
-# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
-
-# test ceres manager, wolf manager and wolf tree
-#ADD_EXECUTABLE(test_ceres_manager_tree test_ceres_manager_tree.cpp)
-#TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME})
-
-# test ceres covariance
-#ADD_EXECUTABLE(test_ceres_covariance test_ceres_covariance.cpp)
-#TARGET_LINK_LIBRARIES(test_ceres_covariance ${PROJECT_NAME})
+ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
+TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
 
 # IF (laser_scan_utils_FOUND)
 #     ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp)
@@ -87,7 +50,7 @@ IF(faramotics_FOUND)
                                 ${PROJECT_NAME})
                                 
         IF(Suitesparse_FOUND)
-            ADD_EXECUTABLE(test_iQR_wolf2 test_iQR_wolf2.cpp)
+            ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
             TARGET_LINK_LIBRARIES(test_iQR_wolf2 
                                 ${pose_state_time_LIBRARIES} 
                                 ${faramotics_LIBRARIES}
diff --git a/src/examples/test_SPQR.cpp b/src/examples/solver/test_SPQR.cpp
similarity index 100%
rename from src/examples/test_SPQR.cpp
rename to src/examples/solver/test_SPQR.cpp
diff --git a/src/examples/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp
similarity index 100%
rename from src/examples/test_ccolamd.cpp
rename to src/examples/solver/test_ccolamd.cpp
diff --git a/src/examples/test_ccolamd_blocks.cpp b/src/examples/solver/test_ccolamd_blocks.cpp
similarity index 100%
rename from src/examples/test_ccolamd_blocks.cpp
rename to src/examples/solver/test_ccolamd_blocks.cpp
diff --git a/src/examples/test_iQR.cpp b/src/examples/solver/test_iQR.cpp
similarity index 100%
rename from src/examples/test_iQR.cpp
rename to src/examples/solver/test_iQR.cpp
diff --git a/src/examples/test_iQR_wolf.cpp b/src/examples/solver/test_iQR_wolf.cpp
similarity index 100%
rename from src/examples/test_iQR_wolf.cpp
rename to src/examples/solver/test_iQR_wolf.cpp
diff --git a/src/examples/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
similarity index 98%
rename from src/examples/test_iQR_wolf2.cpp
rename to src/examples/solver/test_iQR_wolf2.cpp
index 17db520b4767a2239a9d8a58508d85e716b06949..a75890a5a1079db6836b04e711a28f78372f96b2 100644
--- a/src/examples/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -171,8 +171,8 @@ int main(int argc, char *argv[])
     ground_truth.head(3) = pose_odom;
     odom_trajectory.head(3) = pose_odom;
 
-    WolfManager* wolf_manager_QR = new WolfManager(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
-    WolfManager* wolf_manager_ceres = new WolfManager(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+    WolfManager* wolf_manager_QR = new WolfManager(PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+    WolfManager* wolf_manager_ceres = new WolfManager(PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
 
     // Ceres initialization
     ceres::Solver::Options ceres_options;
diff --git a/src/examples/test_incremental_ccolamd_blocks.cpp b/src/examples/solver/test_incremental_ccolamd_blocks.cpp
similarity index 100%
rename from src/examples/test_incremental_ccolamd_blocks.cpp
rename to src/examples/solver/test_incremental_ccolamd_blocks.cpp
diff --git a/src/examples/test_permutations.cpp b/src/examples/solver/test_permutations.cpp
similarity index 100%
rename from src/examples/test_permutations.cpp
rename to src/examples/solver/test_permutations.cpp
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 482dddd4cb62b4dbc82335621b56edf5fddbfbe3..b691262ccbf8a51e91e9dbdae1f85ed926983774 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -166,7 +166,7 @@ int main(int argc, char** argv)
     ground_truth.head(3) = pose_odom;
     odom_trajectory.head(3) = pose_odom;
 
-    WolfManager* wolf_manager = new WolfManager(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
+    WolfManager* wolf_manager = new WolfManager(PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
     
     // Ceres wrapper
     ceres::Solver::Options ceres_options;
diff --git a/src/examples/test_ceres_covariance.cpp b/src/examples/test_ceres_covariance.cpp
deleted file mode 100644
index 0c4e6d281d517da093737a73fd328c013dcabeb8..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_covariance.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-class rangeBetweenPoints
-{
-	private:
-		double d_;
-
-	public:
-		rangeBetweenPoints(double d):
-			d_(d)
-		{
-			//
-		}
-
-		template <typename T>
-		bool operator()(const T* const x , const T* const y, T* e) const
-		{
-			e[0] = T(d_) - ((x[0] - y[0])*(x[0] - y[0]) + (x[1] * y[1])*(x[1] * y[1]));
-			return true;
-		}
-};
-
-//This main is a single iteration of the WOLF. 
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv) 
-{
-    std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-    
-    //dimension 
-    const unsigned int DIM = 19; //just to test, all will be DIM-dimensional
-    
-    // init
-    google::InitGoogleLogging(argv[0]);
-    
-    // build problem
-    double x[2];
-    double y[2];
-    x[0] = 0;
-    x[1] = 0;
-    y[0] = 1;
-    y[1] = 1;
-    ceres::Problem problem;
-	problem.AddParameterBlock(x, 2);
-	problem.AddParameterBlock(y, 2);
-	std::vector<double*> state_blocks({&x[0], &y[0]});
-
-    // cost function
-	ceres::CostFunction* range_cost_function1 = new ceres::AutoDiffCostFunction<rangeBetweenPoints, 1, 2, 2>(new rangeBetweenPoints(1.0));
-	ceres::CostFunction* range_cost_function2 = new ceres::AutoDiffCostFunction<rangeBetweenPoints, 1, 2, 2>(new rangeBetweenPoints(1.1));
-
-    // Add residual block
-    problem.AddResidualBlock(range_cost_function1, nullptr, state_blocks);
-    problem.AddResidualBlock(range_cost_function2, nullptr, state_blocks);
-
-    // Solve
-    ceres::Solver::Options solver_options;
-	ceres::Solver::Summary summary;
-	ceres::Solve(solver_options, &problem, &summary);
-
-	//display results
-	std::cout << "Ceres Report:" << std::endl;
-	std::cout << summary.FullReport() << "\n";
-
-    // Compute covariance
-	ceres::Covariance::Options covariance_options;
-	ceres::Covariance covariance(covariance_options);
-
-    std::vector<std::pair<const double*, const double*> > covariance_blocks;
-    covariance_blocks.push_back(std::make_pair(x, x));
-    covariance_blocks.push_back(std::make_pair(y, y));
-    covariance_blocks.push_back(std::make_pair(x, y));
-
-    CHECK(covariance.Compute(covariance_blocks, &problem));
-
-    double covariance_xx[3 * 3];
-    double covariance_yy[2 * 2];
-    double covariance_xy[3 * 2];
-    covariance.GetCovarianceBlock(x, x, covariance_xx);
-    covariance.GetCovarianceBlock(y, y, covariance_yy);
-    covariance.GetCovarianceBlock(x, y, covariance_xy);
-
-    std::cout << "Covariances:" << std::endl;
-    std::cout << "Marginal x:" << covariance_xx << std::endl;
-    std::cout << "Marginal y:" << covariance_yy << std::endl;
-    std::cout << "Cross xy:" << covariance_xy << std::endl;
-       
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
-
diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp
deleted file mode 100644
index a63dd899579855f3fb4c699051ef87aad7f3981b..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_laser.cpp
+++ /dev/null
@@ -1,584 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "sensor_base.h"
-#include "sensor_odom_2D.h"
-#include "sensor_gps_fix.h"
-#include "feature_base.h"
-#include "frame_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
-#include "capture_base.h"
-#include "capture_relative.h"
-#include "capture_odom_2D.h"
-#include "capture_gps_fix.h"
-#include "capture_laser_2D.h"
-#include "state_base.h"
-#include "constraint_sparse.h"
-#include "constraint_gps_2D.h"
-#include "constraint_odom_2D_theta.h"
-#include "constraint_odom_2D_complex_angle.h"
-#include "trajectory_base.h"
-#include "map_base.h"
-#include "wolf_problem.h"
-
-// ceres wrapper include
-#include "ceres_wrapper/ceres_manager.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-//laser_scan_utils
-#include "iri-algorithms/laser_scan_utils/corner_detector.h"
-#include "iri-algorithms/laser_scan_utils/entities.h"
-
-//function travel around
-void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
-{
-    if (ii<=120)
-    {
-    	displacement_ = 0.1;
-    	rotation_ = 0;
-    }
-    else if ( (ii>120) && (ii<=170) )
-    {
-    	displacement_ = 0.2;
-    	rotation_ = 1.8*M_PI/180;
-    }
-    else if ( (ii>170) && (ii<=220) )
-    {
-    	displacement_ = 0;
-    	rotation_ = -1.8*M_PI/180;
-    }
-    else if ( (ii>220) && (ii<=310) )
-    {
-    	displacement_ = 0.1;
-    	rotation_ = 0;
-    }
-    else if ( (ii>310) && (ii<=487) )
-    {
-    	displacement_ = 0.1;
-    	rotation_ = -1.*M_PI/180;
-    }
-    else if ( (ii>487) && (ii<=600) )
-    {
-    	displacement_ = 0.2;
-    	rotation_ = 0;
-    }
-    else if ( (ii>600) && (ii<=700) )
-    {
-    	displacement_ = 0.1;
-    	rotation_ = -1.*M_PI/180;
-    }
-    else  if ( (ii>700) && (ii<=780) )
-    {
-    	displacement_ = 0;
-    	rotation_ = -1.*M_PI/180;
-    }
-    else
-    {
-    	displacement_ = 0.3;
-    	rotation_ = 0.0*M_PI/180;
-    }
-
-    pose.moveForward(displacement_);
-    pose.rt.setEuler( pose.rt.head()+rotation_, pose.rt.pitch(), pose.rt.roll() );
-}
-
-class WolfManager
-{
-    protected:
-		bool use_complex_angles_;
-		RadarOdom+* problem_;
-        std::queue<CaptureBase*> new_captures_;
-        SensorBase* sensor_prior_;
-        unsigned int window_size_;
-        FrameBaseIter first_window_frame_;
-
-    public:
-        WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const unsigned int& _w_size=10) :
-        	use_complex_angles_(_complex_angle),
-			problem_(new RadarOdom+(_state_length)),
-			sensor_prior_(_sensor_prior),
-			window_size_(_w_size)
-		{
-        	Eigen::VectorXs init_frame(use_complex_angles_ ? 4 : 3);
-        	if (use_complex_angles_)
-        		init_frame << 2, 8, 1, 0;
-        	else
-        		init_frame << 2, 8, 0;
-        	createFrame(init_frame, 0);
-		}
-
-        virtual ~WolfManager()
-        {
-        	delete problem_;
-        }
-
-        void createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
-        {
-        	// Create frame and add it to the trajectory
-        	StateBlock* new_position = new StateBlock(problem_->getNewStatePtr());
-			problem_->addState(new_position, _frame_state.head(2));
-
-			StateBlock* new_orientation;
-        	if (use_complex_angles_)
-        		new_orientation = new StateComplexAngle(problem_->getNewStatePtr());
-        	else
-        		new_orientation = new StateBlock(problem_->getNewStatePtr());
-
-			problem_->addState(new_orientation, _frame_state.tail(new_orientation->getSize()));
-
-			problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation));
-
-			// add a zero odometry capture (in order to integrate)
-			problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new CaptureOdom2D(_time_stamp,
-																								  sensor_prior_,
-																								  Eigen::Vector2s::Zero(),
-																								  Eigen::Matrix2s::Zero()));
-        }
-
-        void addCapture(CaptureBase* _capture)
-        {
-        	new_captures_.push(_capture);
-        	std::cout << "added new capture: " << _capture->nodeId() << std::endl;
-        }
-
-        void update()
-        {
-        	while (!new_captures_.empty())
-        	{
-        		// EXTRACT NEW CAPTURE
-        		CaptureBase* new_capture = new_captures_.front();
-        		new_captures_.pop();
-
-        		// ODOMETRY SENSOR
-        		if (new_capture->getSensorPtr() == sensor_prior_)
-        		{
-        			// FIND PREVIOUS RELATIVE CAPTURE
-					CaptureMotion* previous_relative_capture = nullptr;
-					for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++)
-					{
-						//std::cout << "capture: " << (*capture_it)->nodeId() << std::endl;
-						if ((*capture_it)->getSensorPtr() == sensor_prior_)
-						{
-							previous_relative_capture = (CaptureMotion*)(*capture_it);
-							break;
-						}
-					}
-
-					// INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR
-					std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl;
-					previous_relative_capture->integrateCapture((CaptureMotion*)(new_capture));
-					Eigen::VectorXs prior = previous_relative_capture->computePrior();
-
-        			// NEW KEY FRAME (if enough time from last frame)
-					std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl;
-					if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > 0.1)
-        			{
-        				auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
-
-        				// NEW FRAME
-						createFrame(prior, new_capture->getTimeStamp());
-
-        				// COMPUTE PREVIOUS FRAME CAPTURES
-						for (auto capture_it = previous_frame_ptr->getCaptureListPtr()->begin(); capture_it != previous_frame_ptr->getCaptureListPtr()->end(); capture_it++)
-							(*capture_it)->processCapture();
-
-						// WINDOW of FRAMES (remove or fix old frames)
-						if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_)
-						{
-							//problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin());
-							(*first_window_frame_)->fix();
-							first_window_frame_++;
-						}
-						else
-							first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin();
-        			}
-        		}
-        		else
-        		{
-        			// ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture)
-					std::cout << "adding not odometry capture..." << std::endl;
-        			for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++)
-        			{
-        				if ((*capture_it)->getSensorPtr() == new_capture->getSensorPtr())
-        				{
-        					//std::cout << "removing previous capture..." << std::endl;
-							problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it);
-							//std::cout << "removed!" << std::endl;
-							break;
-        				}
-        			}
-        			problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
-        		}
-        	}
-        }
-
-        const Eigen::VectorXs getState() const
-        {
-        	return problem_->getState();
-        }
-
-        StateBlockList* getStateListPtr()
-		{
-        	return problem_->getStateListPtr();
-		}
-
-        void getConstraintsList(ConstraintBaseList& corr_list)
-        {
-        	problem_->getTrajectoryPtr()->getConstraintList(corr_list);
-        }
-
-        RadarOdom+* getProblemPtr()
-        {
-        	return problem_;
-        }
-
-        void printTree()
-        {
-        	problem_->print();
-        }
-};
-
-int main(int argc, char** argv)
-{
-	std::cout << "\n ========= 2D Robot with odometry and GPS ===========\n";
-
-	// USER INPUT ============================================================================================
-	if (argc!=4 || atoi(argv[1])<1 || atoi(argv[1])>1100 || atoi(argv[2]) < 0 || atoi(argv[3]) < 0 || atoi(argv[3]) > 1)
-	{
-		std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl;
-		std::cout << "     - NI is the number of iterations (0 < NI < 1100)" << std::endl;
-		std::cout << "     - WS is the window size (0 < WS)" << std::endl;
-		std::cout << "     - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl;
-		std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-		return -1;
-	}
-
-	clock_t t1, t2;
-	t1=clock();
-
-	unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-	unsigned int window_size = (unsigned int) atoi(argv[2]);
-	bool complex_angle = (bool) atoi(argv[3]);
-
-	// INITIALIZATION ============================================================================================
-	//init random generators
-	WolfScalar odom_std= 0.01;
-	WolfScalar gps_std= 1;
-	std::default_random_engine generator(1);
-	std::normal_distribution<WolfScalar> distribution_odom(0.001, odom_std); //odometry noise
-	std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
-
-	//init google log
-	google::InitGoogleLogging(argv[0]);
-
-	// Ceres initialization
-	ceres::Solver::Options ceres_options;
-	ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
-	ceres_options.max_line_search_step_contraction = 1e-3;
-	//    ceres_options.minimizer_progress_to_stdout = false;
-	//    ceres_options.line_search_direction_type = ceres::LBFGS;
-	//    ceres_options.max_num_iterations = 100;
-	ceres::Problem::Options problem_options;
-	problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-	problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-	problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-	CeresManager* ceres_manager = new CeresManager(problem_options);
-	std::ofstream log_file, landmark_file;  //output file
-
-	// Faramotics stuff
-	Cpose3d viewPoint;
-	Cpose3d devicePose;
-	vector<Cpose3d> devicePoses;
-	vector<float> myScan;
-	string modelFileName;
-
-	//model and initial view point
-	//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";
-	devicePose.setPose(2,8,0.2,0,0,0);
-	//glut initialization
-	//glutInit(&argc, argv);
-    faramotics::initGLUT(argc, argv);
-    
-	//create a viewer for the 3D model and scan points
-    CdynamicSceneRender* myRender = new CdynamicSceneRender(1200,700,90*M_PI/180,90*700.0*M_PI/(1200.0*180.0),0.2,100);
-	myRender->loadAssimpModel(modelFileName,true); //with wireframe
-	//create scanner and load 3D model
-	CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);//HOKUYO_UTM30LX_180DEG or LEUZE_RS4
-	myScanner->loadAssimpModel(modelFileName);
-    
-	//variables
-	Eigen::Vector2s odom_reading, gps_fix_reading;
-	Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion
-	Eigen::VectorXs pose_odom(3); //current odometry integred pose
-	Eigen::VectorXs ground_truth(n_execution*3); //all true poses
-	Eigen::VectorXs odom_trajectory(n_execution*3); //open loop trajectory
-	Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
-	Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
-	TimeStamp time_stamp;
-	Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
-
-	// Wolf manager initialization
-	SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(6,1), odom_std, odom_std);
-	//SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(6,1), gps_std);
-	Eigen::VectorXs laser_pose(6);
-	laser_pose << 0,0,0,0,0,0; //origin, no rotation
-	//SensorLaser2D laser_sensor(Eigen::MatrixXs::Zero(6,1),-M_PI/2, M_PI/2, M_PI/720, 0.2, 100.0, 0.01);//OLD FASHION
-	SensorLaser2D laser_sensor(Eigen::MatrixXs::Zero(6,1));
-	WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, window_size);
-
-	// Initial pose
-	pose_odom << 2,8,0;
-	ground_truth.head(3) = pose_odom;
-	odom_trajectory.head(3) = pose_odom;
-
-	std::cout << "START TRAJECTORY..." << std::endl;
-	// START TRAJECTORY ============================================================================================
-	for (unsigned int step=1; step < n_execution; step++)
-	{
-		//get init time
-		t2=clock();
-
-		// ROBOT MOVEMENT ---------------------------
-		//std::cout << "ROBOT MOVEMENT..." << std::endl;
-		// moves the device position
-		t1=clock();
-		motionCampus(step, devicePose, odom_reading(0), odom_reading(1));
-		devicePoses.push_back(devicePose);
-
-
-		// SENSOR DATA ---------------------------
-		//std::cout << "SENSOR DATA..." << std::endl;
-		// store groundtruth
-		ground_truth.segment(step*3,3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
-
-		// compute odometry
-		odom_reading(0) += distribution_odom(generator);
-		odom_reading(1) += distribution_odom(generator);
-
-		// odometry integration
-		pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2));
-		pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2));
-		pose_odom(2) = pose_odom(2) + odom_reading(1);
-		odom_trajectory.segment(step*3,3) = pose_odom;
-
-		// compute GPS
-		//gps_fix_reading << devicePose.pt(0), devicePose.pt(1), 0;
-		//gps_fix_reading(0) += distribution_gps(generator);
-		//gps_fix_reading(1) += distribution_gps(generator);
-
-		//compute scan
-		myScan.clear();
-		myScanner->computeScan(devicePose,myScan);
-		mean_times(0) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-
-		// ADD CAPTURES ---------------------------
-		//std::cout << "ADD CAPTURES..." << std::endl;
-		t1=clock();
-		// adding new sensor captures
-	    time_stamp.setToNow();
-		wolf_manager->addCapture(new CaptureOdom2D(time_stamp, &odom_sensor, odom_reading, odom_std * Eigen::MatrixXs::Identity(2,2)));
-		//wolf_manager->addCapture(new CaptureGPSFix(time_stamp, &gps_sensor, gps_fix_reading, gps_std * MatrixXs::Identity(3,3)));
-		wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_sensor, myScan));
-
-        // updating problem
-		wolf_manager->update();
-		mean_times(1) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-
-		// UPDATING CERES ---------------------------
-		//std::cout << "UPDATING CERES..." << std::endl;
-		t1=clock();
-		// update state units and constraints in ceres
-		ceres_manager->update(wolf_manager->getProblemPtr());
-		mean_times(2) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-
-		// SOLVE OPTIMIZATION ---------------------------
-		//std::cout << "SOLVING..." << std::endl;
-		t1=clock();
-		ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
-		//std::cout << summary.FullReport() << std::endl;
-		mean_times(3) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-		// COMPUTE COVARIANCES ---------------------------
-		//std::cout << "COMPUTING COVARIANCES..." << std::endl;
-		t1=clock();
-//		if (step > 2)
-//			ceres_manager->computeCovariances(wolf_manager->getProblemPtr()->getStateListPtr(), wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr());
-		mean_times(4) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-		// DRAWING STUFF ---------------------------
-		t1=clock();
-		// draw detected corners
-		std::list<laserscanutils::Corner> corner_list;
-		std::vector<double> corner_vector;
-		CaptureLaser2D last_scan(time_stamp, &laser_sensor, myScan);
-		last_scan.extractCorners(corner_list);
-		for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
-		{
-			corner_vector.push_back(corner_it->pt_(0));
-			corner_vector.push_back(corner_it->pt_(1));
-		}
-		myRender->drawCorners(devicePose,corner_vector);
-
-		// draw landmarks
-		std::vector<double> landmark_vector;
-		for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ )
-		{
-			WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
-			landmark_vector.push_back(*position_ptr); //x
-			landmark_vector.push_back(*(position_ptr+1)); //y
-			landmark_vector.push_back(0.2); //z
-		}
-		myRender->drawLandmarks(landmark_vector);
-
-		//Set view point and render the scene
-		//locate visualization view point, somewhere behind the device
-		viewPoint.setPose(devicePose);
-		viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-		viewPoint.moveForward(-5);
-		myRender->setViewPoint(viewPoint);
-		myRender->drawPoseAxis(devicePose);
-		myRender->drawScan(devicePose,myScan,180.*M_PI/180.,90.*M_PI/180.); //draw scan
-		myRender->render();
-		mean_times(5) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-		// TIME MANAGEMENT ---------------------------
-		mean_times(6) += ((double)clock()-t2)/CLOCKS_PER_SEC;
-
-		if (mean_times(5) < 0.1)
-			usleep(100000-1e-6*mean_times(5));
-
-//		std::cout << "\nTree after step..." << std::endl;
-//		wolf_manager->printTree();
-	}
-
-	// DISPLAY RESULTS ============================================================================================
-	mean_times /= n_execution;
-	std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
-	std::cout << "  data generation:    " << mean_times(0) << std::endl;
-	std::cout << "  wolf managing:      " << mean_times(1) << std::endl;
-	std::cout << "  ceres managing:     " << mean_times(2) << std::endl;
-	std::cout << "  ceres optimization: " << mean_times(3) << std::endl;
-	std::cout << "  ceres covariance:   " << mean_times(4) << std::endl;
-	std::cout << "  results drawing:    " << mean_times(5) << std::endl;
-	std::cout << "  loop time:          " << mean_times(6) << std::endl;
-
-//	std::cout << "\nTree before deleting..." << std::endl;
-//	wolf_manager->printTree();
-
-	// Draw Final result -------------------------
-	std::vector<double> landmark_vector;
-	for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ )
-	{
-		WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
-		landmark_vector.push_back(*position_ptr); //x
-		landmark_vector.push_back(*(position_ptr+1)); //y
-		landmark_vector.push_back(0.2); //z
-	}
-	myRender->drawLandmarks(landmark_vector);
-	viewPoint.setPose(devicePoses.front());
-	viewPoint.moveForward(10);
-	viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-	viewPoint.moveForward(-10);
-	myRender->setViewPoint(viewPoint);
-	myRender->render();
-
-	// Print Final result in a file -------------------------
-	// Vehicle poses
-	int i = 0;
-	Eigen::VectorXs state_poses(n_execution * 3);
-	for (auto frame_it = wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->begin(); frame_it != wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->end(); frame_it++ )
-	{
-		if (complex_angle)
-			state_poses.segment(i,3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr()+1), atan2(*(*frame_it)->getOPtr()->getPtr(), *((*frame_it)->getOPtr()->getPtr()+1));
-		else
-			state_poses.segment(i,3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr()+1), *(*frame_it)->getOPtr()->getPtr();
-		i+=3;
-	}
-
-	// Landmarks
-	i = 0;
-	Eigen::VectorXs landmarks(wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->size()*2);
-	for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ )
-	{
-		Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr());
-		landmarks.segment(i,2) = landmark;
-		i+=2;
-	}
-
-	// Print log files
-	std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
-	log_file.open(filepath, std::ofstream::out); //open log file
-
-	if (log_file.is_open())
-	{
-		log_file << 0 << std::endl;
-		for (unsigned int ii = 0; ii<n_execution; ii++)
-			log_file << state_poses.segment(ii*3,3).transpose()
-					 << "\t" << ground_truth.segment(ii*3,3).transpose()
-					 << "\t" << (state_poses.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose()
-					 << "\t" << odom_trajectory.segment(ii*3,3).transpose()
-					 << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl;
-		log_file.close(); //close log file
-		std::cout << std::endl << "Result file " << filepath << std::endl;
-	}
-	else
-		std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-
-	std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt"));
-	landmark_file.open(filepath2, std::ofstream::out); //open log file
-
-	if (landmark_file.is_open())
-	{
-		for (unsigned int ii = 0; ii<landmarks.size(); ii+=2)
-			landmark_file << landmarks.segment(ii,2).transpose() << std::endl;
-		landmark_file.close(); //close log file
-		std::cout << std::endl << "Landmark file " << filepath << std::endl;
-	}
-	else
-		std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-
-	std::cout << "Press any key for ending... " << std::endl << std::endl;
-	std::getchar();
-
-    delete myRender;
-    delete myScanner;
-	delete wolf_manager;
-	std::cout << "wolf deleted" << std::endl;
-	delete ceres_manager;
-	std::cout << "ceres_manager deleted" << std::endl;
-
-	std::cout << " ========= END ===========" << std::endl << std::endl;
-
-	//exit
-	return 0;
-}
diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp
deleted file mode 100644
index 65221225f234122e5d6f2f2c3e56fce9610fc7b4..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_manager.cpp
+++ /dev/null
@@ -1,844 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "sensor_base.h"
-#include "frame_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
-#include "state_theta.h"
-#include "capture_base.h"
-#include "state_base.h"
-#include "wolf.h"
-
-// ceres wrapper includes
-#include "ceres_wrapper/complex_angle_parametrization.h"
-
-/**
- * This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data.
- *
- **/
-
-using namespace Eigen;
-
-class ConstraintXBase
-{
-	protected:
-		WolfScalar *measurement_ptr_;
-
-    public:
-
-		ConstraintXBase(WolfScalar * _measurement_ptr) :
-        	measurement_ptr_(_measurement_ptr)
-        {
-        }
-
-        virtual ~ConstraintXBase()
-        {
-        }
-
-        virtual ConstraintType getType() const = 0;
-        virtual const std::vector<WolfScalar*> getBlockPtrVector() = 0;
-};
-
-template <const unsigned int MEASUREMENT_SIZE,
-				unsigned int BLOCK_0_SIZE,
-				unsigned int BLOCK_1_SIZE = 0,
-				unsigned int BLOCK_2_SIZE = 0,
-				unsigned int BLOCK_3_SIZE = 0,
-				unsigned int BLOCK_4_SIZE = 0,
-				unsigned int BLOCK_5_SIZE = 0,
-				unsigned int BLOCK_6_SIZE = 0,
-				unsigned int BLOCK_7_SIZE = 0,
-				unsigned int BLOCK_8_SIZE = 0,
-				unsigned int BLOCK_9_SIZE = 0>
-class ConstraintSparse: public ConstraintXBase
-{
-    protected:
-		std::vector<WolfScalar*> state_block_ptr_vector_;
-		std::vector<unsigned int> block_sizes_vector_;
-
-    public:
-		static const unsigned int measurementSize = MEASUREMENT_SIZE;
-		static const unsigned int block0Size = BLOCK_0_SIZE;
-		static const unsigned int block1Size = BLOCK_1_SIZE;
-		static const unsigned int block2Size = BLOCK_2_SIZE;
-		static const unsigned int block3Size = BLOCK_3_SIZE;
-		static const unsigned int block4Size = BLOCK_4_SIZE;
-		static const unsigned int block5Size = BLOCK_5_SIZE;
-		static const unsigned int block6Size = BLOCK_6_SIZE;
-		static const unsigned int block7Size = BLOCK_7_SIZE;
-		static const unsigned int block8Size = BLOCK_8_SIZE;
-		static const unsigned int block9Size = BLOCK_9_SIZE;
-
-		ConstraintSparse(WolfScalar* _measurementPtr, WolfScalar** _blockPtrArray) :
-        	ConstraintXBase(_measurementPtr),
-			block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
-        {
-			for (unsigned int i = 0; i<block_sizes_vector_.size(); i++)
-			{
-				if (block_sizes_vector_.at(i) == 0)
-				{
-					block_sizes_vector_.resize(i);
-					break;
-				}
-				else
-					state_block_ptr_vector_.push_back(_blockPtrArray[i]);
-			}
-        }
-
-		ConstraintSparse(WolfScalar* _measurementPtr,
-							 WolfScalar* _state0Ptr,
-							 WolfScalar* _state1Ptr = nullptr,
-							 WolfScalar* _state2Ptr = nullptr,
-							 WolfScalar* _state3Ptr = nullptr,
-							 WolfScalar* _state4Ptr = nullptr,
-							 WolfScalar* _state5Ptr = nullptr,
-							 WolfScalar* _state6Ptr = nullptr,
-							 WolfScalar* _state7Ptr = nullptr,
-							 WolfScalar* _state8Ptr = nullptr,
-							 WolfScalar* _state9Ptr = nullptr ) :
-			ConstraintXBase(_measurementPtr),
-			state_block_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
-			block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
-		{
-			for (unsigned int i = 0; i<block_sizes_vector_.size(); i++)
-			{
-				if (block_sizes_vector_.at(i) == 0)
-				{
-					block_sizes_vector_.resize(i);
-					state_block_ptr_vector_.resize(i);
-					break;
-				}
-			}
-
-			//TODO: Check if while size OK, pointers NULL
-		}
-
-        virtual ~ConstraintSparse()
-        {
-        }
-
-        virtual ConstraintType getType() const = 0;
-
-		virtual const std::vector<WolfScalar *> getBlockPtrVector()
-		{
-			return state_block_ptr_vector_;
-		}
-};
-
-class ConstraintGPS2D : public ConstraintSparse<2,2>
-{
-	public:
-		static const unsigned int N_BLOCKS = 1;
-		const double stdev_ = 1;
-
-		ConstraintGPS2D(WolfScalar* _measurementPtr, WolfScalar* _statePtr) :
-			ConstraintSparse<2,2>(_measurementPtr, _statePtr)
-		{
-		}
-
-		ConstraintGPS2D(WolfScalar* _measurementPtr, StateBlock* _statePtr) :
-			ConstraintSparse<2,2>(_measurementPtr, _statePtr->getPtr())
-		{
-		}
-
-		virtual ~ConstraintGPS2D()
-		{
-		}
-
-		template <typename T>
-		bool operator()(const T* const _x, T* _residuals) const
-		{
-			_residuals[0] = (T(*this->measurement_ptr_) - _x[0]) / T(stdev_);
-			_residuals[1] = (T(*(this->measurement_ptr_+1)) - _x[1]) / T(stdev_);
-			return true;
-		}
-
-		virtual ConstraintType getType() const
-		{
-			return CTR_GPS_FIX_2D;
-		}
-};
-
-class Constraint2DOdometry : public ConstraintSparse<2,2,2,2,2>
-{
-	public:
-		static const unsigned int N_BLOCKS = 4;
-		const double stdev_ = 0.01; //model parameters
-
-		Constraint2DOdometry(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) :
-			ConstraintSparse<2,2,2,2,2>(_measurementPtr, _blockPtrs)
-		{
-		}
-
-		Constraint2DOdometry(WolfScalar* _measurementPtr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
-			ConstraintSparse<2,2,2,2,2>(_measurementPtr, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
-		{
-		}
-
-		Constraint2DOdometry(WolfScalar* _measurementPtr, StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr) :
-			ConstraintSparse<2,2,2,2,2>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
-		{
-		}
-
-		virtual ~Constraint2DOdometry()
-		{
-		}
-
-        template <typename T>
-        bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
-        {
-			// Expected measurement
-			T expected_range = (_p1[0]-_p2[0])*(_p1[0]-_p2[0]) + (_p1[1]-_p2[1])*(_p1[1]-_p2[1]); //square of the range
-			T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
-			//T expected_rotation = atan2(_o2[1], _o2[0]) -atan2(_o1[1],_o1[0]);
-
-			// Residuals
-			_residuals[0] = (expected_range - T((*this->measurement_ptr_)*(*this->measurement_ptr_))) / T(stdev_);
-			_residuals[1] = (expected_rotation - T(*(this->measurement_ptr_+1))) / T(stdev_);
-
-			return true;
-        }
-
-        virtual ConstraintType getType() const
-        {
-        	return CTR_ODOM_2D_COMPLEX_ANGLE;
-        }
-};
-
-class Constraint2DOdometryTheta : public ConstraintSparse<2,2,1,2,1>
-{
-	public:
-		static const unsigned int N_BLOCKS = 4;
-		const double stdev_ = 0.01; //model parameters
-
-		Constraint2DOdometryTheta(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) :
-			ConstraintSparse<2,2,1,2,1>(_measurementPtr, _blockPtrs)
-		{
-		}
-
-		Constraint2DOdometryTheta(WolfScalar* _measurementPtr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
-			ConstraintSparse<2,2,1,2,1>(_measurementPtr, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
-		{
-		}
-
-		Constraint2DOdometryTheta(WolfScalar* _measurementPtr, StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr) :
-			ConstraintSparse<2,2,1,2,1>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
-		{
-		}
-
-		virtual ~Constraint2DOdometryTheta()
-		{
-		}
-
-        template <typename T>
-        bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
-        {
-			// Expected measurement
-			T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
-			T expected_rotation = _o2[0]-_o1[0];
-
-			// Residuals
-			_residuals[0] = (expected_range - T((*this->measurement_ptr_)*(*this->measurement_ptr_))) / T(stdev_);
-			_residuals[1] = (expected_rotation - T(*(this->measurement_ptr_+1))) / T(stdev_);
-
-			return true;
-        }
-
-        virtual ConstraintType getType() const
-        {
-        	return CTR_ODOM_2D;
-        }
-};
-
-class CaptureXBase
-{
-	public:
-		VectorXs capture;
-		TimeStamp time_stamp;
-		SensorBase* sensor_ptr_; ///< Pointer to sensor
-
-		CaptureXBase(const VectorXs& _capture, const WolfScalar& _time_stamp, SensorBase* _sensor_ptr) :
-			capture(_capture),
-			time_stamp(_time_stamp),
-			sensor_ptr_(_sensor_ptr)
-		{
-		}
-
-		virtual ~CaptureXBase()
-		{
-		}
-
-		const SensorType getSensorType() const
-		{
-			return sensor_ptr_->getSensorType();
-		}
-
-		WolfScalar* getPtr()
-		{
-			return capture.data();
-		}
-};
-
-class WolfManager
-{
-    protected:
-		VectorXs state_;
-		unsigned int first_empty_state_;
-		bool use_complex_angles_;
-		std::vector<FrameBase*> frames_;
-        std::vector<ConstraintXBase*> constraints_;
-        std::vector<VectorXs> odom_captures_;
-        std::vector<VectorXs> gps_captures_;
-        std::queue<CaptureXBase*> new_captures_;
-        std::vector<CaptureXBase*> captures_;
-
-    public: 
-        WolfManager(const unsigned int& _state_length=1000, const bool _complex_angle=false) :
-        	state_(_state_length),
-			first_empty_state_(0),
-        	use_complex_angles_(_complex_angle),
-        	frames_(0),
-			constraints_(0)
-		{
-        	VectorXs init_frame(use_complex_angles_ ? 4 : 3);
-        	if (use_complex_angles_)
-        		init_frame << 0, 0, 1, 0;
-        	else
-        		init_frame << 0, 0, 0;
-        	createFrame(init_frame, 0);
-		}
-
-        virtual ~WolfManager()
-        {
-//        	std::cout << "Destroying WolfManager...\n";
-//        	std::cout << "Clearing constraints_...\n";
-//        	constraints_.clear();
-//        	std::cout << "Clearing frames...\n";
-//        	frames_.clear();
-//        	std::cout << "Clearing odom_captures_...\n";
-//        	odom_captures_.clear();
-//        	std::cout << "Clearing gps_captures_...\n";
-//        	gps_captures_.clear();
-//        	captures_.clear();
-//
-//        	std::cout << "all cleared...\n";
-        }
-
-        unsigned int getConstraintsSize()
-        {
-        	return constraints_.size();
-        }
-
-        void createFrame(const VectorXs& _frame_state, const TimeStamp& _time_stamp)
-        {
-        	// Store in state_
-        	state_.segment(first_empty_state_, use_complex_angles_ ? 4 : 3) << _frame_state;
-
-        	// Create frame
-        	if (use_complex_angles_)
-// 				frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp,
-// 															   StateBaseShPtr(new StateBase(state_.data()+first_empty_state_)),
-// 															   StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2)))));
-                frames_.push_back(new FrameBase(_time_stamp,
-                                                new StateBlock(state_.data()+first_empty_state_),
-                                                new StateComplexAngle(state_.data()+first_empty_state_+2)));
-
-        	else
-// 				frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp,
-// 						   	   	   	   	   	   	   	   	   	   StateBaseShPtr(new StateBase(state_.data()+first_empty_state_)),
-// 															   StateBaseShPtr(new StateBase(state_.data()+first_empty_state_+2)))));
-                frames_.push_back(new FrameBase(_time_stamp,
-                                                new StateBlock(state_.data()+first_empty_state_),
-                                                new StateBlock(state_.data()+first_empty_state_+2)));
-        	// Update first free state location index
-        	first_empty_state_ += use_complex_angles_ ? 4 : 3;
-        }
-
-        void addCapture(const VectorXs& _odom_capture, const WolfScalar& _time_stamp, SensorBase* _sensor_ptr)
-        {
-        	new_captures_.push(new CaptureXBase(_odom_capture, _time_stamp, _sensor_ptr));
-        }
-
-        void computeOdomCapture(CaptureXBase* _odom_capture)
-		{
-        	FrameBase* prev_frame_ptr = frames_.back();
-
-        	// STORE CAPTURE
-        	captures_.push_back(_odom_capture);
-        	VectorXs capture_data = _odom_capture->capture;
-
-        	// PRIOR
-        	VectorXs pose_predicted(use_complex_angles_ ? 4 : 3);
-        	Map<VectorXs> previous_pose(prev_frame_ptr->getPPtr()->getPtr(), use_complex_angles_ ? 4 : 3);
-        	if (use_complex_angles_)
-			{
-				double new_pose_predicted_2 = previous_pose(2) * cos(capture_data(1)) - previous_pose(3) * sin(capture_data(1));
-				double new_pose_predicted_3 = previous_pose(2) * sin(capture_data(1)) + previous_pose(3) * cos(capture_data(1));
-				pose_predicted(0) = previous_pose(0) + capture_data(0) * new_pose_predicted_2;
-				pose_predicted(1) = previous_pose(1) + capture_data(0) * new_pose_predicted_3;
-				pose_predicted(2) = new_pose_predicted_2;
-				pose_predicted(3) = new_pose_predicted_3;
-			}
-			else
-			{
-				pose_predicted(0) = previous_pose(0) + capture_data(0) * cos(previous_pose(2) + (capture_data(1)));
-				pose_predicted(1) = previous_pose(1) + capture_data(0) * sin(previous_pose(2) + (capture_data(1)));
-				pose_predicted(2) = previous_pose(2) + (capture_data(1));
-			}
-
-        	// NEW FRAME
-        	createFrame(pose_predicted, _odom_capture->time_stamp);
-
-			// CORRESPONDENCE ODOMETRY
-			if (use_complex_angles_)
-				constraints_.push_back(new Constraint2DOdometry(_odom_capture->getPtr(),
-															   prev_frame_ptr->getPPtr()->getPtr(),
-															   prev_frame_ptr->getOPtr()->getPtr(),
-															   frames_.back()->getPPtr()->getPtr(),
-															   frames_.back()->getOPtr()->getPtr()));
-
-			else
-				constraints_.push_back(new Constraint2DOdometryTheta(_odom_capture->getPtr(),
-																	prev_frame_ptr->getPPtr()->getPtr(),
-																	prev_frame_ptr->getOPtr()->getPtr(),
-																	frames_.back()->getPPtr()->getPtr(),
-																	frames_.back()->getOPtr()->getPtr()));
-		}
-
-        void computeGPSCapture(CaptureXBase* _gps_capture)
-		{
-			// STORE CAPTURE
-        	captures_.push_back(_gps_capture);
-
-			// CORRESPONDENCE GPS
-			constraints_.push_back(new ConstraintGPS2D(_gps_capture->getPtr(), frames_.back()->getPPtr()->getPtr()));
-		}
-
-        void update(std::queue<StateBlock*>& new_state_units, std::queue<ConstraintXBase*>& new_constraints)
-        {
-        	while (!new_captures_.empty())
-        	{
-        		switch (new_captures_.front()->getSensorType())
-        		{
-        			case GPS_FIX:
-        				computeGPSCapture(new_captures_.front());
-        				new_constraints.push(constraints_.back());
-        				break;
-        			case ODOM_2D:
-        				computeOdomCapture(new_captures_.front());
-        				new_constraints.push(constraints_.back());
-        				new_state_units.push(frames_.back()->getPPtr());
-        				new_state_units.push(frames_.back()->getOPtr());
-        				break;
-        			default:
-        				std::cout << "unknown capture...\n";
-        		}
-        		new_captures_.pop();
-        	}
-        }
-
-        VectorXs getState()
-        {
-        	return state_;
-        }
-
-        ConstraintXBase* getConstraintPrt(unsigned int i)
-        {
-        	return constraints_.at(i);
-        }
-
-        std::queue<StateBlock*> getStateUnitsPtrs(unsigned int i)
-		{
-			return std::queue<StateBlock*>({frames_.at(i)->getPPtr(),frames_.at(i)->getOPtr()});
-		}
-};
-
-class CeresManager
-{
-	protected:
-
-		std::vector<std::pair<ceres::ResidualBlockId, ConstraintXBase*>> constraint_list_;
-		ceres::Problem* ceres_problem_;
-
-	public:
-		CeresManager(ceres::Problem* _ceres_problem) :
-			ceres_problem_(_ceres_problem)
-		{
-		}
-
-		~CeresManager()
-		{
-//			std::vector<double*> state_units;
-//			ceres_problem_->GetParameterBlocks(&state_units);
-//
-//			for (unsigned int i = 0; i< state_units.size(); i++)
-//				removeStateUnit(state_units.at(i));
-//
-//			std::cout << "all state units removed! \n";
-			std::cout << "residuals: " << ceres_problem_->NumResiduals() << "\n";
-			std::cout << "parameters: " << ceres_problem_->NumParameters() << "\n";
-		}
-
-		ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options)
-		{
-			// create summary
-			ceres::Solver::Summary ceres_summary_;
-
-			// run Ceres Solver
-			ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
-
-			// compute covariances
-			ceres::Covariance::Options covariance_options;
-			covariance_options.algorithm_type = ceres::DENSE_SVD;
-			covariance_options.null_space_rank = -1;
-			ceres::Covariance covariance_(covariance_options);
-
-			std::vector<double*> parameter_blocks;
-			ceres_problem_->GetParameterBlocks(&parameter_blocks);
-
-			std::vector<std::pair<const double*, const double*>> covariance_blocks;
-			covariance_blocks.push_back(std::make_pair(parameter_blocks[0],parameter_blocks[1]));
-			covariance_.Compute(covariance_blocks, ceres_problem_);
-
-			double covariance_12[3 * 3];
-			covariance_.GetCovarianceBlock(parameter_blocks[0], parameter_blocks[1], covariance_12);
-
-			std::cout << "12 cov: " << covariance_12[0] << " " <<
-										covariance_12[1] << " " <<
-										covariance_12[2] << " " <<
-										covariance_12[3] << " " <<
-										covariance_12[4] << " " <<
-										covariance_12[5] << " " <<
-										covariance_12[6] << " " <<
-										covariance_12[7] << " " <<
-										covariance_12[8] << " " <<std::endl;
-
-			//display results
-			return ceres_summary_;
-		}
-
-		void addConstraints(std::queue<ConstraintXBase*>& _new_constraints)
-		{
-			//std::cout << _new_constraints.size() << " new constraints\n";
-			while (!_new_constraints.empty())
-			{
-				addConstraint(_new_constraints.front());
-				_new_constraints.pop();
-			}
-		}
-
-		void removeConstraints()
-		{
-			for (unsigned int i = 0; i<constraint_list_.size(); i++)
-			{
-				ceres_problem_->RemoveResidualBlock(constraint_list_.at(i).first);
-			}
-			constraint_list_.clear();
-			std::cout << ceres_problem_->NumResidualBlocks() << " residual blocks \n";
-		}
-
-		void addConstraint(ConstraintXBase* _corr_ptr)
-		{
-			ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getBlockPtrVector());
-			constraint_list_.push_back(std::pair<ceres::ResidualBlockId, ConstraintXBase*>(blockIdx,_corr_ptr));
-		}
-
-		void addStateUnits(std::queue<StateBlock*>& _new_state_units)
-		{
-			while (!_new_state_units.empty())
-			{
-				addStateBlock(_new_state_units.front());
-				_new_state_units.pop();
-			}
-		}
-
-		void removeStateUnit(WolfScalar* _st_ptr)
-		{
-			ceres_problem_->RemoveParameterBlock(_st_ptr);
-		}
-
-		void addStateBlock(StateBlock* _st_ptr)
-		{
-			//std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
-			//_st_ptr->print();
-
-			switch (_st_ptr->getType())
-			{
-				case ST_COMPLEX_ANGLE:
-				{
-					//std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
-					//ceres_problem_->SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization);
-					ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
-					break;
-				}
-//				case PARAM_QUATERNION:
-//				{
-//					std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl;
-//					ceres_problem_->SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization);
-//					ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization);
-//					break;
-//				}
-				case ST_POINT_1D: //equivalent case ST_THETA:
-				{
-					//std::cout << "No Local Parametrization to be added" << std::endl;
-					ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
-					break;
-				}
-				case ST_POINT_2D:
-				{
-					//std::cout << "No Local Parametrization to be added" << std::endl;
-					ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlock*)_st_ptr)->BLOCK_SIZE, nullptr);
-					break;
-				}
-				case ST_POINT_3D:
-				{
-					//std::cout << "No Local Parametrization to be added" << std::endl;
-					ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlock*)_st_ptr)->BLOCK_SIZE, nullptr);
-					break;
-				}
-				default:
-					std::cout << "Unknown  Local Parametrization type!" << std::endl;
-			}
-		}
-
-		ceres::CostFunction* createCostFunction(ConstraintXBase* _corrPtr)
-		{
-			switch (_corrPtr->getType())
-			{
-				case CTR_GPS_FIX_2D:
-				{
-					ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
-					return new ceres::AutoDiffCostFunction<ConstraintGPS2D,
-															specific_ptr->measurementSize,
-															specific_ptr->block0Size,
-															specific_ptr->block1Size,
-															specific_ptr->block2Size,
-															specific_ptr->block3Size,
-															specific_ptr->block4Size,
-															specific_ptr->block5Size,
-															specific_ptr->block6Size,
-															specific_ptr->block7Size,
-															specific_ptr->block8Size,
-															specific_ptr->block9Size>(specific_ptr);
-					break;
-				}
-				case CTR_ODOM_2D_COMPLEX_ANGLE:
-				{
-					Constraint2DOdometry* specific_ptr = (Constraint2DOdometry*)(_corrPtr);
-					return new ceres::AutoDiffCostFunction<Constraint2DOdometry,
-															specific_ptr->measurementSize,
-															specific_ptr->block0Size,
-															specific_ptr->block1Size,
-															specific_ptr->block2Size,
-															specific_ptr->block3Size,
-															specific_ptr->block4Size,
-															specific_ptr->block5Size,
-															specific_ptr->block6Size,
-															specific_ptr->block7Size,
-															specific_ptr->block8Size,
-															specific_ptr->block9Size>(specific_ptr);
-					break;
-				}
-				case CTR_ODOM_2D:
-				{
-					Constraint2DOdometryTheta* specific_ptr = (Constraint2DOdometryTheta*)(_corrPtr);
-					return new ceres::AutoDiffCostFunction<Constraint2DOdometryTheta,
-															specific_ptr->measurementSize,
-															specific_ptr->block0Size,
-															specific_ptr->block1Size,
-															specific_ptr->block2Size,
-															specific_ptr->block3Size,
-															specific_ptr->block4Size,
-															specific_ptr->block5Size,
-															specific_ptr->block6Size,
-															specific_ptr->block7Size,
-															specific_ptr->block8Size,
-															specific_ptr->block9Size>(specific_ptr);
-					break;
-				}
-				default:
-					std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
-					return nullptr;
-			}
-		}
-};
-
-int main(int argc, char** argv) 
-{
-	std::cout << " ========= 2D Robot with odometry and GPS ===========\n\n";
-
-    // USER INPUT ============================================================================================
-	if (argc!=3 || atoi(argv[1])<1 || atoi(argv[2]) < 0 || atoi(argv[2]) > 1)
-	{
-		std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl;
-		std::cout << "     - NI is the number of iterations (NI > 0)" << std::endl;
-		std::cout << "     - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl;
-		std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-		return -1;
-	}
-
-	clock_t t1, t2;
-	t1=clock();
-
-	NodeLinked<NodeTerminus,NodeTerminus> node(TOP,"TRAJECTORY");
-
-	unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-	bool complex_angle = (bool) atoi(argv[2]);
-
-	// INITIALIZATION ============================================================================================
-	//init random generators
-	std::default_random_engine generator(1);
-	std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise
-	std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise
-
-	//init google log
-	google::InitGoogleLogging(argv[0]);
-
-	// Ceres initialization
-	ceres::Solver::Options ceres_options;
-	ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
-	ceres_options.max_line_search_step_contraction = 1e-3;
-	//    ceres_options.minimizer_progress_to_stdout = false;
-	//    ceres_options.line_search_direction_type = ceres::LBFGS;
-	//    ceres_options.max_num_iterations = 100;
-	ceres::Problem* ceres_problem = new ceres::Problem();
-	CeresManager* ceres_manager = new CeresManager(ceres_problem);
-	std::ofstream log_file;  //output file
-	// Wolf manager initialization
-	WolfManager* wolf_manager = new WolfManager(n_execution * (complex_angle ? 4 : 3), complex_angle);
-
-	//variables
-	Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion
-	Eigen::VectorXs pose_true(3); //current true pose
-	Eigen::VectorXs pose_odom(3); //current true pose
-	Eigen::VectorXs ground_truth(n_execution*3); //all true poses
-	Eigen::VectorXs odom_trajectory(n_execution*3); //all true poses
-	Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
-	Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
-	std::queue<StateBlock*> new_state_units; // new state units in wolf that must be added to ceres
-	std::queue<ConstraintXBase*> new_constraints; // new constraints in wolf that must be added to ceres
-	SensorBase* odom_sensor = new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(6,1),0);
-	SensorBase* gps_sensor = new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(6,1),0);
-
-	// Initial pose
-	pose_true << 0,0,0;
-	pose_odom << 0,0,0;
-	ground_truth.head(3) = pose_true;
-	odom_trajectory.head(3) = pose_true;
-
-	// SENSOR DATA ============================================================================================
-	for (unsigned int ii = 1; ii<n_execution; ii++)
-	{
-		// inventing odometry ground truth
-		if ( ii < (unsigned int)floor(n_execution/2) )
-			odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments.
-		else
-			odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments.
-
-		// Computing ground truth trajectory
-		pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2) + odom_inc_true(ii*2+1));
-		pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2) + odom_inc_true(ii*2+1));
-		pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1);
-		ground_truth.segment(ii*3,3) << pose_true;
-
-		// corrupting sensor readings (odometry and GPS)
-		odom_readings.segment(ii*2,2) << odom_inc_true(ii*2) + distribution_odom(generator),
-										 odom_inc_true(ii*2+1) + distribution_odom(generator); //true range and theta with noise
-		gps_fix_readings.segment(ii*3,3) << pose_true(0) + distribution_gps(generator),
-											pose_true(1) + distribution_gps(generator),
-											0. + distribution_gps(generator);
-
-		// Computing ground truth trajectory
-		pose_odom(0) = pose_odom(0) + odom_readings(ii*2) * cos(pose_odom(2) + odom_readings(ii*2+1));
-		pose_odom(1) = pose_odom(1) + odom_readings(ii*2) * sin(pose_odom(2) + odom_readings(ii*2+1));
-		pose_odom(2) = pose_odom(2) + odom_readings(ii*2+1);
-		odom_trajectory.segment(ii*3,3) << pose_odom;
-	}
-	std::cout << "sensor data created!\n";
-
-	// START TRAJECTORY ============================================================================================
-    new_state_units = wolf_manager->getStateUnitsPtrs(0); // First pose to be added in ceres
-    for (unsigned int step=1; step < n_execution; step++)
-	{
-    	// adding sensor captures
-		wolf_manager->addCapture(odom_readings.segment(step*2,2),step,odom_sensor);
-		wolf_manager->addCapture(gps_fix_readings.segment(step*3,3),step,gps_sensor);
-
-		// updating problem
-		wolf_manager->update(new_state_units, new_constraints);
-
-		// adding new state units and constraints to ceres
-		ceres_manager->addStateUnits(new_state_units);
-		ceres_manager->addConstraints(new_constraints);
-	}
-
-    // SOLVE OPTIMIZATION ============================================================================================
-	ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
-	t2=clock();
-	double seconds = ((double)t2-t1)/CLOCKS_PER_SEC;
-
-	// DISPLAY RESULTS ============================================================================================
-	std::cout << summary.FullReport() << std::endl;
-	std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl;
-	std::cout << "total seconds: " << seconds << std::endl;
-
-	// change from complex angle to theta
-	VectorXs state = wolf_manager->getState();
-	VectorXs state_theta(n_execution * 3);
-	if (complex_angle)
-		for (unsigned int ii = 0; ii<n_execution; ii++)
-			state_theta.segment(ii*3,3) << state(ii*4), state(ii*4+1), atan2(state(ii*4+2), state(ii*4+3));
-	else
-		state_theta = state;
-
-	// Print log file
-	std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
-	log_file.open(filepath, std::ofstream::out); //open log file
-
-	if (log_file.is_open())
-	{
-		log_file << seconds << std::endl;
-		for (unsigned int ii = 0; ii<n_execution; ii++)
-			log_file << state_theta.segment(ii*3,3).transpose()
-					 << "\t" << ground_truth.segment(ii*3,3).transpose()
-					 << "\t" << (state_theta.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose()
-					 << "\t" << odom_trajectory.segment(ii*3,3).transpose()
-					 << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl;
-		log_file.close(); //close log file
-		std::cout << std::endl << "Result file " << filepath << std::endl;
-	}
-	else
-		std::cout << std::endl << "Failed to write the file " << filepath << std::endl;
-
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-    //ceres_manager->removeConstraints();
-    delete wolf_manager;
-    std::cout << "everything deleted!\n";
-    delete ceres_manager;
-    std::cout << "...deleted!\n";
-    delete ceres_problem;
-    std::cout << "amost... deleted!\n";
-
-    //exit
-    return 0;
-}
-
diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp
deleted file mode 100644
index 9773203764a742873075e2c515b0566915595647..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_manager_tree.cpp
+++ /dev/null
@@ -1,360 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "sensor_base.h"
-#include "sensor_odom_2D.h"
-#include "sensor_gps_fix.h"
-#include "frame_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
-#include "capture_base.h"
-#include "capture_relative.h"
-#include "capture_odom_2D.h"
-#include "capture_gps_fix.h"
-#include "state_base.h"
-#include "constraint_sparse.h"
-#include "constraint_gps_2D.h"
-#include "constraint_odom_2D_theta.h"
-#include "constraint_odom_2D_complex_angle.h"
-
-// ceres wrapper include
-#include "ceres_wrapper/ceres_manager.h"
-
-/**
- * This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data.
- *
- **/
-
-using namespace Eigen;
-
-class WolfManager
-{
-    protected:
-		VectorXs state_;
-		unsigned int first_empty_state_;
-		bool use_complex_angles_;
-		TrajectoryBase* trajectory_;
-        std::vector<VectorXs> odom_captures_;
-        std::vector<VectorXs> gps_captures_;
-        std::queue<CaptureBase*> new_captures_;
-        SensorBase* sensor_prior_;
-
-    public: 
-        WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length=1000) :
-        	state_(_state_length),
-			first_empty_state_(0),
-        	use_complex_angles_(_complex_angle),
-			trajectory_(new TrajectoryBase()),
-			sensor_prior_(_sensor_prior)
-		{
-        	VectorXs init_frame(use_complex_angles_ ? 4 : 3);
-        	if (use_complex_angles_)
-        		init_frame << 0, 0, 1, 0;
-        	else
-        		init_frame << 0, 0, 0;
-        	createFrame(init_frame, 0);
-		}
-
-        virtual ~WolfManager()
-        {
-        	delete trajectory_;
-        }
-
-        void createFrame(const VectorXs& _frame_state, const TimeStamp& _time_stamp)
-        {
-        	// Store in state_
-        	state_.segment(first_empty_state_, use_complex_angles_ ? 4 : 3) << _frame_state;
-
-        	// Create frame and add it to the trajectory
-        	if (use_complex_angles_)
-        	{
-                FrameBase* new_frame = new FrameBase(_time_stamp,
-                                                     new StateBlock(state_.data()+first_empty_state_),
-                                                     new StateComplexAngle(state_.data()+first_empty_state_+2));
-        		trajectory_->addFrame(new_frame);
-        	}
-        	else
-        	{
-                FrameBase* new_frame = new FrameBase(_time_stamp,
-                                                     new StateBlock(state_.data()+first_empty_state_),
-                                                     new StateBlock(state_.data()+first_empty_state_+2));
-        		trajectory_->addFrame(new_frame);
-        	}
-
-        	// Update first free state location index
-        	first_empty_state_ += use_complex_angles_ ? 4 : 3;
-        }
-
-        void addCapture(CaptureBase* _capture)
-        {
-        	new_captures_.push(_capture);
-        }
-
-        void update(std::list<StateBlock*>& new_state_units, std::list<ConstraintBase*>& new_constraints)
-        {
-        	// TODO: management due to time stamps
-        	while (!new_captures_.empty())
-        	{
-        		// EXTRACT NEW CAPTURE
-        		CaptureBase* new_capture = new_captures_.front();
-        		new_captures_.pop();
-
-        		// NEW FRAME (if the specific sensor)
-        		// TODO: accumulate odometries
-        		if (new_capture->getSensorPtr() == sensor_prior_)
-        		{
-        			// ADD CAPTURE TO THE PREVIOUS FRAME
-					trajectory_->getFrameListPtr()->back()->addCapture(new_capture);
-
-					// COMPUTE PRIOR
-					trajectory_->getFrameListPtr()->back()->setState(new_capture->computePrior());
-
-					// CREATE NEW FRAME
-					createFrame(VectorXs::Zero(use_complex_angles_ ? 4 : 3), new_capture->getTimeStamp());
-
-					// NEW STATE UNITS TO BE ADDED BY CERES
-					//new_state_units.insert(new_state_units.end(), trajectory_.getFrameList.back()->getStateList().begin(), trajectory_.getFrameList.back()->getStateList().end());
-					new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getPPtr());
-					new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getOPtr());
-        		}
-        		else
-        		{
-        			// ADD CAPTURE TO THE LAST FRAME
-					trajectory_->getFrameListPtr()->back()->addCapture(new_capture);
-        		}
-
-        		// COMPUTE CAPTURE (features, constraints)
-        		new_capture->processCapture();
-
-        		// ADD CORRESPONDENCES TO THE new_constraints OUTPUT PARAM
-        		for (FeatureBaseIter feature_list_iter=new_capture->getFeatureListPtr()->begin(); feature_list_iter!=new_capture->getFeatureListPtr()->end(); feature_list_iter++)
-				{
-					for (ConstraintBaseIter constraint_list_iter=(*feature_list_iter)->getConstraintListPtr()->begin(); constraint_list_iter!=(*feature_list_iter)->getConstraintListPtr()->end(); constraint_list_iter++)
-					{
-						new_constraints.push_back(*constraint_list_iter);
-					}
-				}
-        	}
-        }
-
-        VectorXs getState()
-        {
-        	return state_;
-        }
-
-        std::list<StateBlock*> getStateList()
-		{
-        	std::list<StateBlock*> st_list;
-
-        	for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
-			{
-        		//st_list.insert(st_list.end(), (*frame_list_iter)->getStateList().begin(), (*frame_list_iter)->getStateList().end());
-        		st_list.push_back((*frame_list_iter)->getPPtr());
-        		st_list.push_back((*frame_list_iter)->getOPtr());
-			}
-
-			return st_list;
-		}
-
-        std::list<ConstraintBase*> getConstraintsList()
-        {
-        	std::list<ConstraintBase*> corr_list;
-
-        	for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
-			{
-				for (CaptureBaseIter capture_list_iter=(*frame_list_iter)->getCaptureListPtr()->begin(); capture_list_iter!=(*frame_list_iter)->getCaptureListPtr()->end(); capture_list_iter++)
-				{
-					for (FeatureBaseIter feature_list_iter=(*capture_list_iter)->getFeatureListPtr()->begin(); feature_list_iter!=(*capture_list_iter)->getFeatureListPtr()->end(); feature_list_iter++)
-					{
-						corr_list.insert(corr_list.end(),(*feature_list_iter)->getConstraintListPtr()->begin(), (*feature_list_iter)->getConstraintListPtr()->end());
-					}
-				}
-			}
-        	return corr_list;
-        }
-
-        void printTree()
-        {
-        	trajectory_->print();
-        }
-};
-
-int main(int argc, char** argv) 
-{
-	std::cout << "\n ========= 2D Robot with odometry and GPS ===========\n";
-
-    // USER INPUT ============================================================================================
-	if (argc!=3 || atoi(argv[1])<1 || atoi(argv[2]) < 0 || atoi(argv[2]) > 1)
-	{
-		std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl;
-		std::cout << "     - NI is the number of iterations (NI > 0)" << std::endl;
-		std::cout << "     - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl;
-		std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-		return -1;
-	}
-
-	clock_t t1, t2;
-	t1=clock();
-
-	unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-	bool complex_angle = (bool) atoi(argv[2]);
-
-	// INITIALIZATION ============================================================================================
-	//init random generators
-	WolfScalar odom_std= 0.01;
-	WolfScalar gps_std= 1;
-	std::default_random_engine generator(1);
-	std::normal_distribution<WolfScalar> distribution_odom(0.001, odom_std); //odometry noise
-	std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
-
-	//init google log
-	google::InitGoogleLogging(argv[0]);
-
-	// Ceres initialization
-	ceres::Solver::Options ceres_options;
-	ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
-	ceres_options.max_line_search_step_contraction = 1e-3;
-	//    ceres_options.minimizer_progress_to_stdout = false;
-	//    ceres_options.line_search_direction_type = ceres::LBFGS;
-	//    ceres_options.max_num_iterations = 100;
-	ceres::Problem::Options problem_options;
-	problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-	problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-	problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-	CeresManager* ceres_manager = new CeresManager(problem_options);
-	std::ofstream log_file;  //output file
-
-
-	//variables
-	Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion
-	Eigen::VectorXs pose_true(3); //current true pose
-	Eigen::VectorXs pose_odom(3); //current true pose
-	Eigen::VectorXs ground_truth(n_execution*3); //all true poses
-	Eigen::VectorXs odom_trajectory(n_execution*3); //all true poses
-	Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
-	Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
-	std::list<StateBlock*> new_state_units; // new state units in wolf that must be added to ceres
-	std::list<ConstraintBase*> new_constraints; // new constraints in wolf that must be added to ceres
-
-	// Wolf manager initialization
-	SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(6,1), odom_std, odom_std);
-	SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(6,1), gps_std);
-	WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, n_execution * (complex_angle ? 4 : 3));
-
-	// Initial pose
-	pose_true << 0,0,0;
-	pose_odom << 0,0,0;
-	ground_truth.head(3) = pose_true;
-	odom_trajectory.head(3) = pose_true;
-
-	// SENSOR DATA ============================================================================================
-	for (unsigned int ii = 1; ii<n_execution; ii++)
-	{
-		// inventing odometry ground truth
-		if ( ii < (unsigned int)floor(n_execution/2) )
-			odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments.
-		else
-			odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments.
-
-		// Computing ground truth trajectory
-		pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2) + odom_inc_true(ii*2+1));
-		pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2) + odom_inc_true(ii*2+1));
-		pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1);
-		ground_truth.segment(ii*3,3) << pose_true;
-
-		// corrupting sensor readings (odometry and GPS)
-		odom_readings.segment(ii*2,2) << odom_inc_true(ii*2) + distribution_odom(generator),
-										 odom_inc_true(ii*2+1) + distribution_odom(generator); //true range and theta with noise
-		gps_fix_readings.segment(ii*3,3) << pose_true(0) + distribution_gps(generator),
-											pose_true(1) + distribution_gps(generator),
-											0. + distribution_gps(generator);
-
-		// Computing ground truth trajectory
-		pose_odom(0) = pose_odom(0) + odom_readings(ii*2) * cos(pose_odom(2) + odom_readings(ii*2+1));
-		pose_odom(1) = pose_odom(1) + odom_readings(ii*2) * sin(pose_odom(2) + odom_readings(ii*2+1));
-		pose_odom(2) = pose_odom(2) + odom_readings(ii*2+1);
-		odom_trajectory.segment(ii*3,3) << pose_odom;
-	}
-
-	// START TRAJECTORY ============================================================================================
-    new_state_units = wolf_manager->getStateList(); // First pose to be added in ceres
-    for (unsigned int step=1; step < n_execution; step++)
-	{
-    	// adding new sensor captures
-		wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(step*0.01), &odom_sensor, odom_readings.segment(step*2,2), odom_std * MatrixXs::Identity(2,2)));
-		wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(step*0.01), &gps_sensor, gps_fix_readings.segment(step*3,3), gps_std * MatrixXs::Identity(3,3)));
-
-		// updating problem
-		wolf_manager->update(new_state_units, new_constraints);
-
-		// adding new state units and constraints to ceres
-		ceres_manager->addStateUnits(&new_state_units);
-		ceres_manager->addConstraints(&new_constraints);
-	}
-
-	//std::cout << "Resulting tree:\n";
-	//wolf_manager->printTree();
-
-    // SOLVE OPTIMIZATION ============================================================================================
-	ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
-	t2=clock();
-	double seconds = ((double)t2-t1)/CLOCKS_PER_SEC;
-
-	// DISPLAY RESULTS ============================================================================================
-	std::cout << summary.FullReport() << std::endl;
-	std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl;
-	std::cout << "total seconds: " << seconds << std::endl;
-
-	// change from complex angle to theta
-	VectorXs state = wolf_manager->getState();
-	VectorXs state_theta(n_execution * 3);
-	if (complex_angle)
-		for (unsigned int ii = 0; ii<n_execution; ii++)
-			state_theta.segment(ii*3,3) << state(ii*4), state(ii*4+1), atan2(state(ii*4+2), state(ii*4+3));
-	else
-		state_theta = state;
-
-	// Print log file
-	std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
-	log_file.open(filepath, std::ofstream::out); //open log file
-
-	if (log_file.is_open())
-	{
-		log_file << seconds << std::endl;
-		for (unsigned int ii = 0; ii<n_execution; ii++)
-			log_file << state_theta.segment(ii*3,3).transpose()
-					 << "\t" << ground_truth.segment(ii*3,3).transpose()
-					 << "\t" << (state_theta.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose()
-					 << "\t" << odom_trajectory.segment(ii*3,3).transpose()
-					 << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl;
-		log_file.close(); //close log file
-		std::cout << std::endl << "Result file " << filepath << std::endl;
-	}
-	else
-		std::cout << std::endl << "Failed to write the file " << filepath << std::endl;
-
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-
-    delete ceres_manager;
-    delete wolf_manager;
-
-    //exit
-    return 0;
-}
diff --git a/src/examples/test_ceres_odom_batch.cpp b/src/examples/test_ceres_odom_batch.cpp
deleted file mode 100644
index 0178bd5c10f9097eca906d52bffdcce773897c13..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_odom_batch.cpp
+++ /dev/null
@@ -1,450 +0,0 @@
-// Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-
-//std includes
-#include <ctime>
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <vector>
-#include <list>
-#include <random>
-#include <cmath>
-// #include <memory>
-// #include <typeinfo>
-
-//ceres
-#include "ceres/ceres.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "node_terminus.h"
-#include "node_linked.h"
-
-//forward declarations
-class TrajectoryBaseX;
-class FrameBaseX;
-class CaptureBaseX;
-class FeatureBaseX;
-class CorrespondenceBaseX;
-
-//class TrajectoryBaseX
-class TrajectoryBaseX : public NodeLinked<NodeTerminus,FrameBaseX>
-{
-    protected:
-        unsigned int length_; //just something to play
-        
-    public:
-        TrajectoryBaseX(const unsigned int _len) :
-            NodeLinked(TOP, "TRAJECTORY"),
-            length_(_len)
-        {
-            //
-        };
-        
-        virtual ~TrajectoryBaseX()
-        {
-            
-        };
-};
-
-//class FrameBaseX
-class FrameBaseX : public NodeLinked<TrajectoryBaseX,CaptureBaseX>
-{
-    protected:
-        double time_stamp_; //frame ts
-        
-    public:
-        FrameBaseX(double _ts) :
-            NodeLinked(MID, "FRAME"),
-            time_stamp_(_ts)
-        {
-            //
-        };
-        
-        virtual ~FrameBaseX()
-        {
-            
-        };
-};
-
-//class CaptureBaseX
-class CaptureBaseX : public NodeLinked<FrameBaseX,FeatureBaseX>
-{
-    protected:
-        double time_stamp_; //capture ts
-        
-    public:
-        CaptureBaseX(double _ts) :
-            NodeLinked(MID, "CAPTURE"),
-            time_stamp_(_ts)
-        {
-            //
-        };
-        
-        virtual ~CaptureBaseX()
-        {
-            
-        };
-};
-
-//class FeatureBaseX
-class FeatureBaseX : public NodeLinked<CaptureBaseX,CorrespondenceBaseX>
-{
-    protected:
-        
-    public:
-        FeatureBaseX() :
-            NodeLinked(MID, "FEATURE")
-        {
-            //
-        };
-        
-        virtual ~FeatureBaseX()
-        {
-            //
-        };
-};
-
-//class CorrespondenceBaseX
-class CorrespondenceBaseX : public NodeLinked<FeatureBaseX,NodeTerminus>
-{
-    protected:
-        unsigned int nblocks_; //number of state blocks in which the correspondence depends on.
-        std::vector<unsigned int> block_indexes_; //state vector indexes indicating start of each state block. This vector has nblocks_ size. 
-        std::vector<unsigned int> block_sizes_; //sizes of each state block. This vector has nblocks_ size. 
-        ceres::CostFunction* cost_function_ptr_;
-        
-    public:
-        CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bindexes, const std::vector<unsigned int> & _bsizes) :
-            NodeLinked(BOTTOM, "CORRESPONDENCE"),
-            nblocks_(_nb),
-            block_indexes_(_bindexes),
-            block_sizes_(_bsizes)
-        {
-            assert(block_sizes_.size() == nblocks_);
-        };
-        
-        virtual ~CorrespondenceBaseX()
-        {
-            //
-        };
-        
-        ceres::CostFunction * getCostFunctionPtr()
-        {
-            return cost_function_ptr_;
-        };
-        	
-        virtual void display() const
-        {
-                unsigned int ii; 
-                std::cout << "number of blocks: " << nblocks_ << std::endl;
-                std::cout << "block indexes: ";
-                for (ii=0; ii<block_indexes_.size(); ii++) std::cout << block_indexes_.at(ii) << " ";
-                std::cout << std::endl;
-                std::cout << "block sizes: ";
-                for (ii=0; ii<block_sizes_.size(); ii++) std::cout << block_sizes_.at(ii) << " ";
-                std::cout << std::endl;
-        };
-};
-
-class Odom2DFunctor
-{
-    protected:
-        Eigen::Vector2s odom_inc_; //incremental odometry measurements (range, theta). Could be a map to data hold by capture or feature
-        const double odom_stdev_ = 0.01; //model parameters
-    
-    public:
-        //constructor
-        Odom2DFunctor(const Eigen::Vector2s & _odom):
-            odom_inc_(_odom) 
-        {
-            //
-        };
-        
-        //destructor
-        virtual ~Odom2DFunctor()
-        {
-            //
-        };
-        
-        //cost function
-        template <typename T>
-        bool operator()(const T* const _x0, const T* const _x1, T* _residual) const
-        {
-            T dr2, dth;
-            
-            //expected range and theta increments, given the state points
-            dr2 = (_x0[0]-_x1[0])*(_x0[0]-_x1[0]) + (_x0[1]-_x1[1])*(_x0[1]-_x1[1]); //square of the range
-            dth = _x1[2] - _x0[2]; //
-            
-            //residuals in range and theta components 
-            _residual[0] = (dr2 - T(odom_inc_(0)*odom_inc_(0))) / T(odom_stdev_);
-            _residual[1] = (dth - T(odom_inc_(1))) / T(odom_stdev_);
-            
-            //return 
-            return true;
-        };
-};
-
-//Specialized correspondence class for 2D odometry
-class CorrespondenceOdom2D : public CorrespondenceBaseX
-{
-    protected:
-        Eigen::Map<Eigen::Vector3s> pose_previous_;
-        Eigen::Map<Eigen::Vector3s> pose_current_; 
-        Eigen::Map<const Eigen::Vector2s> odom_inc_; 
-        
-    public:
-        CorrespondenceOdom2D(WolfScalar * _st, const Eigen::Vector2s & _odom) :
-            CorrespondenceBaseX(2,{0,3},{3,3}),
-            pose_previous_(_st + block_indexes_.at(0), block_sizes_.at(0)),
-            pose_current_(_st + block_indexes_.at(1), block_sizes_.at(1)),
-            odom_inc_(_odom.data())
-        {
-            cost_function_ptr_ = new ceres::AutoDiffCostFunction<Odom2DFunctor,2,3,3>(new Odom2DFunctor(_odom));
-        };
-        
-        virtual ~CorrespondenceOdom2D()
-        {
-            //delete cost_function_ptr_;
-        };
-                
-        double * getPosePreviousPtr()
-        {
-            return pose_previous_.data();
-        };
-        
-        double * getPoseCurrentPtr()
-        {
-            return pose_current_.data();
-        };        
-        
-        virtual void display() const
-        {
-            std::cout << "---- Odom Correspondence ----" << std::endl;
-            CorrespondenceBaseX::display();
-            std::cout << "pose_previous_: " << pose_previous_.transpose() << std::endl;
-            std::cout << "pose_current_: " << pose_current_.transpose() << std::endl;
-            std::cout << "odom_inc_: " << odom_inc_.transpose() << std::endl;
-        };      
-};
-
-class GPSFixFunctor
-{
-    protected:
-        Eigen::Vector3s gps_fix_; //GPS fix XYZ. Could be a map to data hold by capture or feature
-        const double gps_stdev_ = 1; //model parameters
-    
-    public:
-        //constructor
-        GPSFixFunctor(const Eigen::Vector3s & _gps_fix):
-            gps_fix_(_gps_fix)
-        {
-            //
-        };
-        
-        //destructor
-        virtual ~GPSFixFunctor()
-        {
-            //
-        };
-        
-        //cost function
-        template <typename T>
-        bool operator()(const T* const _x0, T* _residual) const
-        {
-            T dist_x = T( gps_fix_(0) ) - _x0[0];
-            T dist_y = T( gps_fix_(1) ) - _x0[1];
-            _residual[0] = dist_x / T(gps_stdev_);
-            _residual[1] = dist_y / T(gps_stdev_);
-            _residual[2] = T(0.); //T( gps_fix_(2) ) - _x0[2]; //in this case, third component of the state is heading, not Z, so it is ignored
-            return true;
-        };
-};
-
-//Specialized correspondence class for GPS Fix data
-class CorrespondenceGPSFix : public CorrespondenceBaseX
-{
-    protected:
-        Eigen::Map<Eigen::Vector3s> location_;
-        Eigen::Map<const Eigen::Vector3s> gps_fix_;
-        //TODO: add ceres_residual_block_id_ to manage add/remove block
-
-    public:
-        CorrespondenceGPSFix(WolfScalar * _st, const Eigen::Vector3s & _gps_fix) :
-            CorrespondenceBaseX(1,{0},{3}), 
-            location_(_st + block_indexes_.at(0) , block_sizes_.at(0)),
-            gps_fix_(_gps_fix.data())
-        {
-            cost_function_ptr_ = new ceres::AutoDiffCostFunction<GPSFixFunctor,3,3>(new GPSFixFunctor(_gps_fix));
-        };
-        
-        virtual ~CorrespondenceGPSFix()
-        {
-            //delete cost_function_ptr_;
-        };
-        
-        double * getLocation()
-        {
-            return location_.data();
-        }
-                
-        virtual void display() const
-        {
-            std::cout << "---- GPS Fix Correspondence ----" << std::endl;
-            CorrespondenceBaseX::display();
-            std::cout << "location_: " << location_.transpose() << std::endl;
-            std::cout << "gps_fix_: " << gps_fix_.transpose() << std::endl;            
-        };        
-};
-
-
-int main(int argc, char** argv) 
-{    
-	clock_t t1, t2;
-	t1 = clock();
-	
-    //Welcome message
-    std::cout << std::endl << " ========= WOLF-CERES test. Simple Odometry + GPS fix problem (with non-template classes) ===========" << std::endl << std::endl;
-
-    //user input
-    if (argc!=2)
-    {
-        std::cout << "Please call me with: [./test_ceres_wrapper_non_template NI NW], where:" << std::endl;
-        std::cout << "       - NI is the number of iterations" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-    
-    //init google log
-    google::InitGoogleLogging(argv[0]);
-
-    //variables    
-    std::list<unsigned int> block_ids; //id of each added block to ceres problem 
-    Eigen::VectorXs odom_inc_true;//invented motion
-    Eigen::Vector3s pose_true; //current true pose
-    Eigen::VectorXs ground_truth; //accumulated true poses
-    Eigen::Vector3s pose_predicted; // current predicted pose
-    Eigen::VectorXs predicted_trajectory; // current predicted pose
-    Eigen::VectorXs state; //running window winth solver result
-    Eigen::Vector2s odom_reading; //current odometry reading
-    Eigen::Vector3s gps_fix_reading; //current GPS fix reading
-    Eigen::VectorXs gps_log; //log of all gps readings
-    CorrespondenceOdom2D *odom_corresp; //pointer to odometry correspondence
-    CorrespondenceGPSFix *gps_fix_corresp; //pointer to GPS fix correspondence
-    ceres::Problem problem; //ceres problem 
-    ceres::Solver::Options options; //ceres solver options
-    ceres::Solver::Summary summary; //ceres solver summary
-    std::ofstream log_file;  //output file
-
-    //resize vectors according user input number of iterations
-    odom_inc_true.resize(n_execution*2); //2 odometry components per iteration
-    ground_truth.resize(n_execution*3);// 3 components per iteration
-    gps_log.resize(n_execution*3); //3 components per iteration
-    state.resize(n_execution*3); //3 components per window element
-    predicted_trajectory.resize(n_execution*3); //3 components per window element
-    
-    //init true odom and true pose
-    for (unsigned int ii = 0; ii<n_execution; ii++)
-    {
-        if ( ii < (unsigned int)floor(n_execution/2) )
-            odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments. 
-        else
-            odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments. 
-    }
-    pose_true << 0,0,0;
-    pose_predicted << 0,0,0;
-    ground_truth.middleRows(0,3) << pose_true; //init point pushed to ground truth
-    state.middleRows(0,3) << 0,0,0; //init state at origin
-    predicted_trajectory.middleRows(0,3) << pose_predicted;
-    
-    //init random generators
-    std::default_random_engine generator(1);
-    std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise
-    std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise
-    
-    //test loop
-    for (unsigned int ii = 1; ii<n_execution; ii++) //ii over iterations
-    {
-        //inventing a simple motion
-        pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2)+odom_inc_true(ii*2+1)); 
-        pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2)+odom_inc_true(ii*2+1)); 
-        pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1);
-        ground_truth.middleRows(ii*3,3) << pose_true;
-        //std::cout << "pose_true(" << ii << ") = " << pose_true.transpose() << std::endl;
-        
-        //inventing sensor readings for odometry and GPS
-        odom_reading << odom_inc_true(ii*2)+distribution_odom(generator), odom_inc_true(ii*2+1)+distribution_odom(generator); //true range and theta with noise
-        gps_fix_reading << pose_true(0) + distribution_gps(generator), pose_true(1) + distribution_gps(generator), 0. + distribution_gps(generator);
-        gps_log.middleRows(ii*3,3) << gps_fix_reading;//log the reading
-        
-        //setting initial guess as an odometry prediction, using noisy odometry
-        pose_predicted(0) = pose_predicted(0) + odom_reading(0) * cos(pose_predicted(2)+odom_reading(1)); 
-        pose_predicted(1) = pose_predicted(1) + odom_reading(0) * sin(pose_predicted(2)+odom_reading(1)); 
-        pose_predicted(2) = pose_predicted(2) + odom_reading(1);
-        state.middleRows(ii*3,3) << pose_predicted;
-        predicted_trajectory.middleRows(ii*3,3) << pose_predicted;
-        //std::cout << "pose_predicted(" << ii << ") = " << pose_predicted.transpose() << std::endl;
-        
-        //creating odom correspondence, exceptuating first iteration. Adding it to the problem 
-        odom_corresp = new CorrespondenceOdom2D(state.data()+(ii-1)*3, odom_reading);
-        //odom_corresp->display();
-        problem.AddResidualBlock(odom_corresp->getCostFunctionPtr(),nullptr, odom_corresp->getPosePreviousPtr(), odom_corresp->getPoseCurrentPtr());
-        delete odom_corresp;
-        
-        //creating gps correspondence and adding it to the problem 
-        gps_fix_corresp = new CorrespondenceGPSFix(state.data()+ii*3, gps_fix_reading);
-        //gps_fix_corresp->display();
-        problem.AddResidualBlock(gps_fix_corresp->getCostFunctionPtr(),nullptr, gps_fix_corresp->getLocation());
-        delete gps_fix_corresp;
-        
-        //set options and solve (sliding window)
-//         options.minimizer_progress_to_stdout = true;
-//         ceres::Solve(options, &problem, &summary);
-    }
-    
-    //display initial guess
-    //std::cout << "INITIAL GUESS IS: " << state.transpose() << std::endl;
-    
-    //set options and solve (batch mode)
-    //options.minimizer_progress_to_stdout = true;
-    options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;//
-    options.max_line_search_step_contraction = 1e-3;
-    ceres::Solve(options, &problem, &summary);
-    
-    t2=clock();
-	double seconds = ((double)t2-t1)/CLOCKS_PER_SEC;
-	std::cout << summary.FullReport() << std::endl;
-	std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl;
-	std::cout << "total seconds: " << seconds << std::endl;
-    
-    //display/log results, by setting cout flags properly
-    std::string homepath = getenv("HOME");
-	log_file.open(homepath + "/Desktop/log_file.txt", std::ofstream::out); //open log file
-	if (log_file.is_open())
-	{
-		log_file << seconds << std::endl;
-		for (unsigned int ii = 0; ii<n_execution; ii++) 
-			log_file << state.middleRows(ii*3,3).transpose() 
-					 << " " << ground_truth.middleRows(ii*3,3).transpose() 
-					 << " " << (state.middleRows(ii*3,3)-ground_truth.middleRows(ii*3,3)).transpose()  
-					 << " " << gps_log.middleRows(ii*3,3).transpose()
-					 << " " << predicted_trajectory.middleRows(ii*3,3).transpose() << std::endl;
-		log_file.close(); //close log file
-		std::cout << std::endl << " Result to file ~/Desktop/log_data.txt" << std::endl;
-	}
-	else
-		std::cout << std::endl << " Failed to write the file ~/Desktop/log_data.txt" << std::endl;
-    
-    //free memory (not necessary since ceres::problem holds their ownership)
-//     delete odom_corresp;
-//     delete gps_fix_corresp;
-    
-    //End message
-    std::cout << " =========================== END ===============================" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
diff --git a/src/examples/test_ceres_odom_iterative.cpp b/src/examples/test_ceres_odom_iterative.cpp
deleted file mode 100644
index 91e7550827db7079781f8a6a586aea0bfc11e6f3..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_odom_iterative.cpp
+++ /dev/null
@@ -1,459 +0,0 @@
-// Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-
-//std includes
-#include <cstdlib>
-#include <stdlib.h> //getenv
-#include <iostream>
-#include <fstream>
-#include <string>
-#include <vector>
-#include <list>
-#include <random>
-#include <cmath>
-// #include <memory>
-// #include <typeinfo>
-
-//ceres
-#include "ceres/ceres.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "node_terminus.h"
-#include "node_linked.h"
-
-//forward declarations
-class TrajectoryBaseX;
-class FrameBaseX;
-class CaptureBaseX;
-class FeatureBaseX;
-class CorrespondenceBaseX;
-
-//class TrajectoryBaseX
-class TrajectoryBaseX : public NodeLinked<NodeTerminus,FrameBaseX>
-{
-    protected:
-        unsigned int length_; //just something to play
-        
-    public:
-        TrajectoryBaseX(const unsigned int _len) :
-            NodeLinked(TOP, "TRAJECTORY"),
-            length_(_len)
-        {
-            //
-        };
-        
-        virtual ~TrajectoryBaseX()
-        {
-            
-        };
-};
-
-//class FrameBaseX
-class FrameBaseX : public NodeLinked<TrajectoryBaseX,CaptureBaseX>
-{
-    protected:
-        double time_stamp_; //frame ts
-        
-    public:
-        FrameBaseX(double _ts) :
-            NodeLinked(MID, "FRAME"),
-            time_stamp_(_ts)
-        {
-            //
-        };
-        
-        virtual ~FrameBaseX()
-        {
-            
-        };
-};
-
-//class CaptureBaseX
-class CaptureBaseX : public NodeLinked<FrameBaseX,FeatureBaseX>
-{
-    protected:
-        double time_stamp_; //capture ts
-        
-    public:
-        CaptureBaseX(double _ts) :
-            NodeLinked(MID, "CAPTURE"),
-            time_stamp_(_ts)
-        {
-            //
-        };
-        
-        virtual ~CaptureBaseX()
-        {
-            
-        };
-};
-
-//class FeatureBaseX
-class FeatureBaseX : public NodeLinked<CaptureBaseX,CorrespondenceBaseX>
-{
-    protected:
-        
-    public:
-        FeatureBaseX() :
-            NodeLinked(MID, "FEATURE")
-        {
-            //
-        };
-        
-        virtual ~FeatureBaseX()
-        {
-            //
-        };
-};
-
-//class CorrespondenceBaseX
-class CorrespondenceBaseX : public NodeLinked<FeatureBaseX,NodeTerminus>
-{
-    protected:
-        unsigned int nblocks_; //number of state blocks in which the correspondence depends on.
-        //std::vector<unsigned int> block_indexes_; //state vector indexes indicating start of each state block. This vector has nblocks_ size. 
-        std::vector<unsigned int> block_sizes_; //sizes of each state block. This vector has nblocks_ size. 
-        ceres::CostFunction* cost_function_ptr_;
-        ceres::ResidualBlockId ceres_residual_block_id_;
-        
-    public:
-        //CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bindexes, const std::vector<unsigned int> & _bsizes) :
-        CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bsizes) :
-            NodeLinked(BOTTOM, "CORRESPONDENCE"),
-            nblocks_(_nb),
-            //block_indexes_(_bindexes),
-            block_sizes_(_bsizes)
-        {
-            assert(block_sizes_.size() == nblocks_);
-        };
-        
-        virtual ~CorrespondenceBaseX()
-        {
-            //
-        };
-        
-//         ceres::CostFunction * getCostFunctionPtr()
-//         {
-//             return cost_function_ptr_;
-//         };
-        
-        virtual void addToProblem(ceres::Problem & _problem) = 0;
-        
-        virtual void removeFromProblem(ceres::Problem & _problem)
-        {
-            _problem.RemoveResidualBlock(ceres_residual_block_id_);
-        }
-        	
-        virtual void display() const
-        {
-                unsigned int ii; 
-                std::cout << "number of state blocks: " << nblocks_ << std::endl;
-                //std::cout << "state block indexes: ";
-                //for (ii=0; ii<block_indexes_.size(); ii++) std::cout << block_indexes_.at(ii) << " ";
-                std::cout << std::endl;
-                std::cout << "state block sizes: ";
-                for (ii=0; ii<block_sizes_.size(); ii++) std::cout << block_sizes_.at(ii) << " ";
-                std::cout << std::endl;
-                std::cout << "ceres residual block id: " << ceres_residual_block_id_ << std::endl;
-        };
-};
-
-class Odom2DFunctor
-{
-    protected:
-        Eigen::Vector2s odom_inc_; //incremental odometry measurements (range, theta). Could be a map to data hold by capture or feature
-        const double odom_stdev_ = 0.01; //model parameters
-    
-    public:
-        //constructor
-        Odom2DFunctor(const Eigen::Vector2s & _odom):
-            odom_inc_(_odom) 
-        {
-            //
-        };
-        
-        //destructor
-        virtual ~Odom2DFunctor()
-        {
-            //
-        };
-        
-        //cost function
-        template <typename T>
-        bool operator()(const T* const _x0, const T* const _x1, T* _residual) const
-        {
-            T dr2, dth;
-            
-            //expected range and theta increments, given the state points
-            dr2 = (_x0[0]-_x1[0])*(_x0[0]-_x1[0]) + (_x0[1]-_x1[1])*(_x0[1]-_x1[1]); //square of the range
-            dth = _x1[2] - _x0[2]; //
-            
-            //residuals in range and theta components 
-            _residual[0] = (T(dr2) - T(odom_inc_(0)*odom_inc_(0))) / T(odom_stdev_);
-            _residual[1] = (T(dth) - T(odom_inc_(1))) / T(odom_stdev_);
-            
-            //return 
-            return true;
-        };
-};
-
-//Specialized correspondence class for 2D odometry
-class CorrespondenceOdom2D : public CorrespondenceBaseX
-{
-    protected:
-        Eigen::Map<Eigen::Vector3s> pose_previous_;
-        Eigen::Map<Eigen::Vector3s> pose_current_; 
-        Eigen::Map<const Eigen::Vector2s> odom_inc_; 
-        
-    public:
-        CorrespondenceOdom2D(WolfScalar * _st_prev, WolfScalar * _st_curr, const Eigen::Vector2s & _odom) :
-            CorrespondenceBaseX(2,{3,3}),
-            pose_previous_(_st_prev),//, block_sizes_.at(0)), //size 3 is already defined at declaration
-            pose_current_(_st_curr),//, block_sizes_.at(1)), //size 3 is already defined at declaration
-            odom_inc_(_odom.data())
-        {
-            cost_function_ptr_ = new ceres::AutoDiffCostFunction<Odom2DFunctor,2,3,3>(new Odom2DFunctor(_odom));
-        };
-        
-        virtual ~CorrespondenceOdom2D()
-        {
-            //delete cost_function_ptr_;
-        };
-                
-//         double * getPosePreviousPtr()
-//         {
-//             return pose_previous_.data();
-//         };
-//         
-//         double * getPoseCurrentPtr()
-//         {
-//             return pose_current_.data();
-//         };        
-        
-        virtual void addToProblem(ceres::Problem & _problem)
-        {
-            ceres_residual_block_id_ = _problem.AddResidualBlock(cost_function_ptr_,nullptr, pose_previous_.data(), pose_current_.data());
-        }
-
-        virtual void display() const
-        {
-            std::cout << "---- Odom Correspondence ----" << std::endl;
-            CorrespondenceBaseX::display();
-            std::cout << "pose_previous_: " << pose_previous_.transpose() << std::endl;
-            std::cout << "pose_current_: " << pose_current_.transpose() << std::endl;
-            std::cout << "odom_inc_: " << odom_inc_.transpose() << std::endl;
-        };      
-};
-
-class GPSFixFunctor
-{
-    protected:
-        Eigen::Vector3s gps_fix_; //GPS fix XYZ. Could be a map to data hold by capture or feature
-        const double gps_stdev_ = 1; //model parameters
-    
-    public:
-        //constructor
-        GPSFixFunctor(const Eigen::Vector3s & _gps_fix):
-            gps_fix_(_gps_fix)
-        {
-            //
-        };
-        
-        //destructor
-        virtual ~GPSFixFunctor()
-        {
-            //
-        };
-        
-        //cost function
-        template <typename T>
-        bool operator()(const T* const _x0, T* _residual) const
-        {
-            T dist_x = T( gps_fix_(0) ) - _x0[0];
-            T dist_y = T( gps_fix_(1) ) - _x0[1];
-            _residual[0] = dist_x / T(gps_stdev_);
-            _residual[1] = dist_y / T(gps_stdev_);
-            _residual[2] = T(0.); //T( gps_fix_(2) ) - _x0[2]; //in this case, third component of the state is heading, not Z, so it is ignored
-            return true;
-        };
-};
-
-//Specialized correspondence class for GPS Fix data
-class CorrespondenceGPSFix : public CorrespondenceBaseX
-{
-    protected:
-        Eigen::Map<Eigen::Vector3s> state_position_;
-        Eigen::Map<const Eigen::Vector3s> gps_fix_;
-
-    public:
-        CorrespondenceGPSFix(WolfScalar * _st, const Eigen::Vector3s & _gps_fix) :
-            CorrespondenceBaseX(1,{3}), 
-            state_position_(_st),// block_sizes_.at(0)), //size 3 is already defined at declaration
-            gps_fix_(_gps_fix.data())
-        {
-            cost_function_ptr_ = new ceres::AutoDiffCostFunction<GPSFixFunctor,3,3>(new GPSFixFunctor(_gps_fix));
-        };
-        
-        virtual ~CorrespondenceGPSFix()
-        {
-            //delete cost_function_ptr_;
-        };
-        
-//         double * getLocation()
-//         {
-//             return location_.data();
-//         }
-        
-        virtual void addToProblem(ceres::Problem & _problem)
-        {
-            ceres_residual_block_id_ = _problem.AddResidualBlock(cost_function_ptr_,nullptr,state_position_.data()); 
-        }
-                
-        virtual void display() const
-        {
-            std::cout << "---- GPS Fix Correspondence ----" << std::endl;
-            CorrespondenceBaseX::display();
-            std::cout << "state_position_: " << state_position_.transpose() << std::endl;
-            std::cout << "gps_fix_: " << gps_fix_.transpose() << std::endl;            
-        };        
-};
-
-
-int main(int argc, char** argv) 
-{    
-    //Welcome message
-    std::cout << std::endl << " ========= WOLF-CERES test. Simple Odometry + GPS fix problem (with non-template classes) ===========" << std::endl << std::endl;
-
-    //user input
-    if (argc!=3)
-    {
-        std::cout << "Please call me with: [./test_ceres_odom_iterative NI NW], where:" << std::endl;
-        std::cout << "       - NI is the number of iterations" << std::endl;
-        std::cout << "       - NW is the size of the window" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-    unsigned int n_window = (unsigned int) atoi(argv[2]); //size of the window.
-    
-    //init google log
-    google::InitGoogleLogging(argv[0]);
-
-    //variables    
-    unsigned int ii, jj, jj_previous; //iterators
-    Eigen::VectorXs odom_inc_true;//invented motion
-    Eigen::Vector3s pose_true; //current true pose
-    Eigen::VectorXs ground_truth; //accumulated true poses
-    Eigen::Vector3s pose_predicted; // current predicted pose
-    Eigen::VectorXs state; //running window winth solver result
-    Eigen::VectorXs state_prior; //state prior, just before solving the problem
-    Eigen::Vector2s odom_reading; //current odometry reading
-    Eigen::Vector3s gps_fix_reading; //current GPS fix reading
-    Eigen::VectorXs gps_log; //log of all gps readings
-    Eigen::VectorXs results_log; //log of optimized poses
-    CorrespondenceOdom2D *odom_corresp; //pointer to odometry correspondence
-    CorrespondenceGPSFix *gps_fix_corresp; //pointer to GPS fix correspondence
-    ceres::Problem problem; //ceres problem 
-    ceres::Solver::Options options; //ceres solver options
-    ceres::Solver::Summary summary; //ceres solver summary
-    std::ofstream log_file;  //output file
-
-    //resize vectors according user input number of iterations
-    odom_inc_true.resize(n_execution*2); //2 odometry components per iteration
-    ground_truth.resize(n_execution*3);// 3 components per iteration
-    gps_log.resize(n_execution*3); //3 components per iteration
-    results_log.resize(n_execution*3); //3 components per iteration
-    state.resize(n_window*3); //3 components per window element
-    state_prior.resize(n_execution*3); //3 components per window element
-    
-    //init true odom and true pose
-    for (ii = 0; ii<n_execution; ii++)
-    {
-        if ( ii < (unsigned int)floor(n_execution/2) )
-            odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments. 
-        else
-            odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments. 
-    }
-    pose_true << 0,0,0;
-    pose_predicted << 0,0,0;
-    ground_truth.middleRows(0,3) << pose_true; //init point pushed to ground truth
-    state.middleRows(0,3) << 0,0,0; //init state at origin
-    
-    //init random generators
-    std::default_random_engine generator;
-    std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise
-    std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise
-    
-    //test loop
-    for (ii = 1; ii<n_execution; ii++) //ii over iterations, jj over the window
-    {
-        //set jj index (over the window)
-        jj = ii%n_window;
-        jj_previous = (ii-1)%n_window;
-        
-        //inventing a simple motion
-        pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2)+odom_inc_true(ii*2+1)); 
-        pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2)+odom_inc_true(ii*2+1)); 
-        pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1);
-        ground_truth.middleRows(ii*3,3) << pose_true;
-        //std::cout << "pose_true(" << ii << ") = " << pose_true.transpose() << std::endl;
-        
-        //inventing sensor readings for odometry and GPS
-        odom_reading << odom_inc_true(ii*2)+distribution_odom(generator), odom_inc_true(ii*2+1)+distribution_odom(generator); //true range and theta with noise
-        gps_fix_reading << pose_true(0) + distribution_gps(generator), pose_true(1) + distribution_gps(generator), 0. + distribution_gps(generator);
-        gps_log.middleRows(ii*3,3) << gps_fix_reading;//log the reading
-        
-        //setting initial guess from the last optimized pose, using noisy odometry
-        pose_predicted(0) = state(jj_previous*3) + odom_reading(0) * cos(state(jj_previous*3+2)+odom_reading(1)); 
-        pose_predicted(1) = state(jj_previous*3+1) + odom_reading(0) * sin(state(jj_previous*3+2)+odom_reading(1)); 
-        pose_predicted(2) = state(jj_previous*3+2) + odom_reading(1);
-        
-        //window management
-        state.middleRows(jj*3,3) << pose_predicted;
-        
-        //creating odom correspondence. Adding it to the problem 
-        odom_corresp = new CorrespondenceOdom2D(state.data()+jj_previous*3, state.data()+jj*3, odom_reading);
-        odom_corresp->addToProblem(problem);
-        //odom_corresp->display();
-        delete odom_corresp;
-        
-        //creating gps correspondence and adding it to the problem 
-        gps_fix_corresp = new CorrespondenceGPSFix(state.data()+ii*3, gps_fix_reading);
-        gps_fix_corresp->addToProblem(problem);
-        //gps_fix_corresp->display();
-        delete gps_fix_corresp;
-        
-        //set options and solve (sliding window)
-//         options.minimizer_progress_to_stdout = true;
-//         ceres::Solve(options, &problem, &summary);
-    }
-    
-    //display initial guess
-    //std::cout << "INITIAL GUESS IS: " << state.transpose() << std::endl;
-    
-    //set options and solve (batch mode)
-    //options.minimizer_progress_to_stdout = true;
-    state_prior = state;
-    options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
-    options.max_line_search_step_contraction = 1e-3;
-    ceres::Solve(options, &problem, &summary);
-    
-    //display/log results, by setting cout flags properly
-    std::string filename( getenv("HOME") );
-    filename += "/Desktop/log_data.txt";
-    std::cout << std::endl << " Result to file " << filename << std::endl;
-    log_file.open(filename, std::ofstream::out); //open log file
-    for (unsigned int ii = 0; ii<n_execution; ii++) 
-        log_file << state.middleRows(ii*3,3).transpose() << " " << ground_truth.middleRows(ii*3,3).transpose() << " " << (state.middleRows(ii*3,3)-ground_truth.middleRows(ii*3,3)).transpose() << " " << gps_log.middleRows(ii*3,3).transpose() << " " <<  state_prior.middleRows(ii*3,3).transpose() <<std::endl;        
-    log_file.close(); //close log file
-  
-    //free memory (not necessary since ceres::problem holds their ownership)
-//     delete odom_corresp;
-//     delete gps_fix_corresp;
-    
-    //End message
-    std::cout << " =========================== END ===============================" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
diff --git a/src/examples/test_ceres_wrapper_jet.cpp b/src/examples/test_ceres_wrapper_jet.cpp
deleted file mode 100644
index e2dbd1e85efe6f44040c19b1cffd6074b0966dcd..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_wrapper_jet.cpp
+++ /dev/null
@@ -1,216 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "sensor_base.h"
-#include "frame_base.h"
-#include "capture_base.h"
-#include "state_block.h"
-#include "wolf.h"
-
-// ceres wrapper includes
-#include "ceres_wrapper/complex_angle_parametrization.h"
-
-//Wolf includes
-#include "wolf.h"
-#include "node_terminus.h"
-#include "node_linked.h"
-
-//namespaces
-using namespace std;
-using namespace Eigen;
-
-/**
- * This class emulates a Wolf top tree node class, such as vehicle. 
- * This class will be removed when effective linking to Wolf, and using actual Vehicle class. 
- * It holds:
- *      - a map to a state vector
- *      - a map to an error vector
- *      - a method to compute the error from the state
- * 
- **/
-
-class WolfVehicle
-{
-    protected:
-        VectorXs state_prior_; //state storage where to compute prior
-        Map<VectorXs> state_map_; //state point to be evaluated by Wolf tree constraints
-
-        //Just to generate fake measurements
-        std::default_random_engine generator_; //just to generate measurements
-        std::normal_distribution<double> distribution_; //just to generate measurements
-        MatrixXs measurements_; //just a set of invented measurements
-
-        unsigned int measurements_size_, state_size_;
-
-    public: 
-        WolfVehicle() :
-            state_prior_(),
-            state_map_(nullptr,0),
-            distribution_(0.0,0.01),
-			measurements_size_(0),
-			state_size_(0)
-        {
-            //
-        }
-        
-        virtual ~WolfVehicle()
-        {
-            //
-        }
-        
-        WolfScalar *getPrior()
-        {
-            return state_prior_.data();
-        }
-        
-        void resizeState(unsigned int _state_size)
-        {
-        	state_size_ = _state_size;
-            state_prior_.resize(_state_size);
-        }
-
-        void inventMeasurements(unsigned int _sz)
-        {
-        	// for testing
-        	measurements_size_ = _sz;
-        	measurements_.resize(state_size_,measurements_size_);
-
-            for(unsigned int ii=0; ii<measurements_size_*state_size_; ii++)
-                measurements_(ii) = 1 + distribution_(generator_); //just inventing a sort of noise measurements
-
-            //std::cout << "invented measurements_ = " << std::endl << measurements_.transpose() << std::endl;
-        }
-        
-        void computePrior()
-        {
-            state_prior_.setZero();
-        }
-        
-        template <typename T>
-        bool operator()(const T* const _x, T* _residuals) const
-        {
-        	// Remap the vehicle state to the const evaluation point
-			Map<const Matrix<T,Dynamic,1>> state_map_const_(_x, state_size_);
-
-			// Map residuals vector to matrix (with sizes of the measurements matrix)
-			Map<Matrix<T,Dynamic,Dynamic>> mapped_residuals(_residuals, state_size_, measurements_size_);
-
-			// Compute error or residuals
-			mapped_residuals = measurements_.cast<T>() - state_map_const_.replicate(1,measurements_size_);
-
-			// std::cout << typeid(T).name() << std::endl;
-			// for (unsigned int j = 0; j<state_size_; j++)
-			// {
-			// 	std::cout << "_x(" << j <<") = " << _x[j] << std::endl;
-			// 	for (unsigned int i = 0; i<measurements_size_; i++)
-			// 		std::cout << "mapped_residuals(" << j << "," << i <<") = " << mapped_residuals(j,i) << std::endl;
-			// }
-
-        	return true;
-        }
-
-//        bool operator()(const WolfScalar* const _x, WolfScalar* _residuals) const
-//		{
-//        	// Remap the vehicle state to the const evaluation point
-//			Map<const VectorXs> state_map_const_(_x, state_size_);
-//
-//			// Map residuals vector to matrix (with sizes of the measurements matrix)
-//			Map<MatrixXs> mapped_residuals(_residuals, state_size_, measurements_size_);
-//
-//			// Compute error or residuals
-//			mapped_residuals = measurements_ - state_map_const_.replicate(1,measurements_size_);
-//
-//			return true;
-//		}
-
-        void print()
-        {
-            //std::cout << "measurements_: " << std::endl << measurements_.transpose() << std::endl << std::endl;
-            std::cout << "state_prior_ : " << std::endl << state_prior_.transpose() << std::endl << std::endl;
-            //std::cout << "state_const_ : " << std::endl << state_map_const_.transpose() << std::endl << std::endl;
-        }
-};
-
-//This main is a single iteration of the WOLF. 
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv) 
-{
-    std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-
-//    NodeLinked<NodeTerminus,NodeTerminus> node(TOP,"TRAJECTORY");
-
-    //dimension 
-    const unsigned int STATE_DIM = 5; //just to test, all will be DIM-dimensional
-    const unsigned int MEASUREMENT_DIM = 1; //just to test, all will be DIM-dimensional
-
-    // init
-    google::InitGoogleLogging(argv[0]);
-    
-    using ceres::AutoDiffCostFunction;
-    using ceres::CostFunction;
-
-    //wolf vehicle & ceres functor
-    WolfVehicle *functorPtr = new WolfVehicle();
-
-    // Ceres problem initialization
-    ceres::Solver::Options options;
-    options.minimizer_progress_to_stdout = false;
-    options.minimizer_type = ceres::TRUST_REGION;
-    options.line_search_direction_type = ceres::LBFGS;
-    options.max_num_iterations = 10;
-    ceres::Solver::Summary summary;
-    ceres::Problem problem;
-
-    // fixed dim problem
-    functorPtr->resizeState(STATE_DIM);
-    functorPtr->computePrior();
-
-	// set measures. This will be replaced by the WOLF-ROS front-end, getting sensor readings from sensors and performing measurements to build the whole wolf tree
-	functorPtr->inventMeasurements(MEASUREMENT_DIM);
-
-	// cost function
-	CostFunction* cost_function = new AutoDiffCostFunction<WolfVehicle,1,1>(functorPtr);
-	for (unsigned int st=0; st < STATE_DIM; st++)
-	{
-		problem.AddResidualBlock(cost_function, NULL, functorPtr->getPrior()+st);
-	}
-	//CostFunction* cost_function = new AutoDiffCostFunction<WolfVehicle,STATE_DIM*MEASUREMENT_DIM,STATE_DIM>(functorPtr);
-	//problem.AddResidualBlock(cost_function, NULL, functorPtr->getPrior());
-
-	// run Ceres Solver
-	ceres::Solve(options, &problem, &summary);
-
-	//display results
-	std::cout << summary.BriefReport() << "\n";
-    functorPtr->print();
-
-    //clean
-    std::cout << "Cleaning ... " << std::endl << std::endl;
-    //problem.RemoveResidualBlock(rbId);
-    delete cost_function;
-    delete functorPtr;
-    
-    //end Wolf iteration
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
-
diff --git a/src/examples/test_ceres_wrapper_jet_2_sensors.cpp b/src/examples/test_ceres_wrapper_jet_2_sensors.cpp
deleted file mode 100644
index 5fcfc457b6fc7aa9e70934dff25fe5f04fad20da..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_wrapper_jet_2_sensors.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "ceres/loss_function.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-
-/**
- * This class emulates a Wolf top tree node class, such as vehicle. 
- * This class will be removed when effective linking to Wolf, and using actual Vehicle class. 
- * It holds:
- *      - a map to a state vector
- *      - a map to an error vector
- *      - a method to compute the error from the state
- * 
- **/
-using namespace Eigen;
-
-class CorrespondenceBase
-{
-	protected:
-    	VectorXs measurement_;
-
-    public:
-
-        CorrespondenceBase(const unsigned int & _measurement_size) :
-			measurement_(_measurement_size)
-        {
-        }
-
-        virtual ~CorrespondenceBase()
-        {
-        }
-
-        virtual void inventMeasurement(const VectorXs& _measurement, std::default_random_engine& _generator, std::normal_distribution<WolfScalar>& _distribution)
-		{
-			measurement_ = _measurement;
-			for(unsigned int ii=0; ii<measurement_.size(); ii++)
-				measurement_(ii) += _distribution(_generator); //just inventing a sort of noise measurements
-			//std::cout << "measurement_" << measurement_ << std::endl;
-		}
-};
-
-class CorrespondenceCeresBase
-{
-	protected:
-        ceres::CostFunction* cost_function_;
-
-    public:
-
-        CorrespondenceCeresBase()
-        {
-        }
-
-        virtual ~CorrespondenceCeresBase()
-        {
-        }
-
-        virtual void addBlock(ceres::Problem & ceres_problem) = 0;
-};
-
-template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_SIZE = 1>
-class Correspondence1Sparse: public CorrespondenceBase
-{
-    protected:
-    	Map<Matrix<WolfScalar, BLOCK_SIZE, 1>> state_block_map_;
-
-    public:
-
-        Correspondence1Sparse(WolfScalar* _statePtr) :
-        	CorrespondenceBase(MEASUREMENT_SIZE),
-			state_block_map_(_statePtr,BLOCK_SIZE)
-        {
-        }
-
-        virtual ~Correspondence1Sparse()
-        {
-        }
-
-        virtual WolfScalar* getBlockPtr()
-        {
-            return state_block_map_.data();
-        }
-
-        template <typename T>
-        //void compute_residuals(const Matrix<T,Dynamic,1>& _x, Matrix<T,Dynamic,1> residuals)
-        void compute_residuals(Map<const Matrix<T,Dynamic,1>>& _st, Map<Matrix<T,Dynamic,1>> residuals) const
-        {
-        	residuals = measurement_.cast<T>() - _st;
-        	// std::cout << typeid(T).name() << std::endl;
-			// for (unsigned int j = 0; j<BLOCK_SIZE; j++)
-			// {
-			// 	std::cout << "_x(" << j <<") = " << _x[j] << std::endl;
-			// 	std::cout << "mapped_residuals(" << j <<") = " << mapped_residuals(j) << std::endl;
-			// }
-        }
-};
-
-template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_SIZE = 1>
-class Correspondence1SparseCeres: public CorrespondenceCeresBase, public Correspondence1Sparse<MEASUREMENT_SIZE, BLOCK_SIZE>
-{
-    public:
-
-        Correspondence1SparseCeres(WolfScalar* _statePtr) :
-			CorrespondenceCeresBase(),
-        	Correspondence1Sparse<MEASUREMENT_SIZE, BLOCK_SIZE>(_statePtr)
-        {
-			cost_function_  = new ceres::AutoDiffCostFunction<Correspondence1SparseCeres,MEASUREMENT_SIZE,BLOCK_SIZE>(this);
-        }
-
-        virtual ~Correspondence1SparseCeres()
-        {
-        }
-
-        template <typename T>
-        bool operator()(const T* const _x, T* _residuals) const
-        {
-        	//std::cout << "adress of x: " << _x << std::endl;
-
-        	// Remap the vehicle state to the const evaluation point
-			Map<const Matrix<T,Dynamic,1>> state_map_const(_x, BLOCK_SIZE);
-
-			// Map residuals vector to matrix (with sizes of the measurements matrix)
-			Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, MEASUREMENT_SIZE);
-
-			// Compute error or residuals
-			this->template compute_residuals<T>(state_map_const, mapped_residuals);
-
-        	return true;
-        }
-
-        virtual void addBlock(ceres::Problem & ceres_problem)
-        {
-        	//std::cout << " adding correspondence_1_sparse_ceres...";
-        	ceres_problem.AddResidualBlock(cost_function_, NULL, this->state_block_map_.data());
-        	//std::cout << " added!" << std::endl;
-        }
-};
-
-
-
-template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_1_SIZE = 1, unsigned int BLOCK_2_SIZE = 1>
-class Correspondence2Sparse: public CorrespondenceBase
-{
-    protected:
-		Map<Matrix<WolfScalar, BLOCK_1_SIZE, 1>> state_block_1_map_;
-		Map<Matrix<WolfScalar, BLOCK_2_SIZE, 1>> state_block_2_map_;
-
-    public:
-
-        Correspondence2Sparse(WolfScalar* _block1Ptr, WolfScalar* _block2Ptr) :
-        	CorrespondenceBase(MEASUREMENT_SIZE),
-			state_block_1_map_(_block1Ptr,BLOCK_1_SIZE),
-			state_block_2_map_(_block2Ptr,BLOCK_2_SIZE)
-        {
-        }
-
-        virtual ~Correspondence2Sparse()
-        {
-        }
-
-        template <typename T>
-        void compute_residuals(Map<const Matrix<T,Dynamic,1>>& _st1, Map<const Matrix<T,Dynamic,1>>& _st2, Map<Matrix<T,Dynamic,1>> residuals) const
-        {
-        	Matrix<T,Dynamic,1> expected_measurement = ((_st1 - _st2).transpose() * (_st1 - _st2)).cwiseSqrt();
-			VectorXd meas = this->measurement_;
-			residuals = (meas).cast<T>() - expected_measurement;
-        }
-
-        WolfScalar *getBlock1Ptr()
-        {
-            return state_block_1_map_.data();
-        }
-
-        WolfScalar *getBlock2Ptr()
-        {
-            return state_block_2_map_.data();
-        }
-};
-
-template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_1_SIZE = 1, unsigned int BLOCK_2_SIZE = 1>
-class Correspondence2SparseCeres: public CorrespondenceCeresBase, public Correspondence2Sparse<MEASUREMENT_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE>
-{
-    public:
-
-        Correspondence2SparseCeres(WolfScalar* _block1Ptr, WolfScalar* _block2Ptr) :
-			CorrespondenceCeresBase(),
-			Correspondence2Sparse<MEASUREMENT_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE>(_block1Ptr, _block2Ptr)
-        {
-			cost_function_  = new ceres::AutoDiffCostFunction<Correspondence2SparseCeres,MEASUREMENT_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE>(this);
-        }
-
-        virtual ~Correspondence2SparseCeres()
-        {
-        }
-
-        template <typename T>
-        bool operator()(const T* const _x1, const T* const _x2, T* _residuals) const
-        {
-        	// print inputs
-        	// std::cout << "_x1 = ";
-        	// for (int i=0; i < BLOCK_1_SIZE; i++)
-        	// 	std::cout << _x1[i] << " ";
-        	// std::cout << std::endl;
-        	// std::cout << "_x2 = ";
-        	// for (int i=0; i < BLOCK_2_SIZE; i++)
-        	// 	std::cout << _x2[i] << " ";
-        	// std::cout << std::endl;
-        	// std::cout << "measurement = ";
-        	// for (int i=0; i < MEASUREMENT_SIZE; i++)
-        	// 	std::cout << this->measurement_(i) << " ";
-        	// std::cout << std::endl;
-
-        	// Remap the vehicle state to the const evaluation point
-			Map<const Matrix<T,Dynamic,1>> x1_map_const(_x1, BLOCK_1_SIZE);
-			Map<const Matrix<T,Dynamic,1>> x2_map_const(_x2, BLOCK_2_SIZE);
-
-			// Map residuals vector to matrix (with sizes of the measurements matrix)
-			Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, MEASUREMENT_SIZE);
-
-			// Compute error or residuals
-			this->template compute_residuals(x1_map_const, x2_map_const, mapped_residuals);
-
-			// print outputs
-			// std::cout << "expected    = ";
-			// for (int i=0; i < MEASUREMENT_SIZE; i++)
-			// 	std::cout << expected_measurement(i) << " ";
-			// std::cout << std::endl;
-			// std::cout << "_residuals  = ";
-			// for (int i=0; i < MEASUREMENT_SIZE; i++)
-			// 	std::cout << _residuals[i] << " ";
-			// std::cout << std::endl << std::endl;
-
-			return true;
-        }
-
-        virtual void addBlock(ceres::Problem & ceres_problem)
-        {
-        	//std::cout << " adding correspondence_2_sparse_ceres...";
-        	ceres_problem.AddResidualBlock(cost_function_, NULL, this->state_block_1_map_.data(), this->state_block_2_map_.data());
-        	//std::cout << " added!" << std::endl;
-        }
-};
-
-class WolfProblem
-{
-    protected:
-        VectorXs state_; //state storage
-        std::vector<CorrespondenceCeresBase*> correspondences_;
-
-        unsigned int state_size_;
-
-    public: 
-        WolfProblem() :
-            state_(),
-			correspondences_(0),
-			state_size_(0)
-        {
-        }
-        
-        virtual ~WolfProblem()
-        {
-        }
-        
-        WolfScalar *getPrior()
-		{
-			return state_.data();
-		}
-
-        CorrespondenceCeresBase* getCorrespondence(const unsigned int _idx)
-        {
-        	//std::cout << correspondences_.size() << " correspondences" << std::endl;
-        	return correspondences_[_idx];
-        }
-
-        unsigned int getCorrespondencesSize()
-        {
-        	return correspondences_.size();
-        }
-        
-        void resizeState(unsigned int _state_size)
-        {
-        	state_size_ = _state_size;
-            state_.resize(_state_size);
-        }
-
-        void addCorrespondence(CorrespondenceCeresBase* _absCorrPtr)
-        {
-        	correspondences_.push_back(_absCorrPtr);
-        	//std::cout << correspondences_.size() << " correspondence added!" << std::endl;
-        }
-
-        void computePrior()
-        {
-            state_.setRandom();
-        }
-
-        void print()
-        {
-            std::cout << "state_ : " << std::endl << state_.transpose() << std::endl << std::endl;
-        }
-};
-
-//This main is a single iteration of the WOLF. 
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv) 
-{
-    std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-    
-    //dimension 
-    const unsigned int DIM = 3;
-    const unsigned int N_STATES = 10;
-    const unsigned int STATE_DIM = DIM * N_STATES;
-    const unsigned int MEAS_A_DIM = 3;
-    const unsigned int MEAS_B_DIM = 1;
-    const unsigned int N_MEAS_A = 100;
-    const unsigned int N_MEAS_B = 50;
-    //const double w_A = 1;
-    //const double w_B = 10;
-
-    // init
-    google::InitGoogleLogging(argv[0]);
-    std::default_random_engine generator;
-    std::normal_distribution<WolfScalar> distribution_A(0.0,0.01);
-    std::normal_distribution<WolfScalar> distribution_B(0.0,0.1);
-    VectorXs actualState(STATE_DIM);
-    for (unsigned int i=0;i<STATE_DIM; i++)
-    	actualState(i) = i;
-
-    using ceres::AutoDiffCostFunction;
-    using ceres::CostFunction;
-
-    // Ceres problem initialization
-    ceres::Solver::Options options;
-    options.minimizer_progress_to_stdout = false;
-    options.minimizer_type = ceres::TRUST_REGION;
-    options.line_search_direction_type = ceres::LBFGS;
-    options.max_num_iterations = 100;
-    ceres::Solver::Summary summary;
-    ceres::Problem ceres_problem;
-
-    //wolf problem
-	WolfProblem *wolf_problem = new WolfProblem();
-    wolf_problem->resizeState(STATE_DIM);
-    wolf_problem->computePrior();
-    std::cout << "Real state: " << actualState.transpose() << std::endl;
-    wolf_problem->print();
-
-    // Correspondences
-    // SENSOR A: Absolute measurements of the whole state
-    for(unsigned int mA=0; mA < N_MEAS_A; mA++)
-    {
-    	for (unsigned int st=0; st < N_STATES; st++)
-		{
-			Correspondence1SparseCeres<MEAS_A_DIM, DIM>* corrAPtr = new Correspondence1SparseCeres<MEAS_A_DIM, DIM>(wolf_problem->getPrior()+st*DIM);
-			VectorXs actualMeasurement = actualState.segment(st*DIM,DIM);
-			corrAPtr->inventMeasurement(actualMeasurement,generator,distribution_A);
-			wolf_problem->addCorrespondence(corrAPtr);
-		}
-    }
-	// SENSOR B: Relative distances between points
-    for(unsigned int mB=0; mB < N_MEAS_B; mB++)
-	{
-    	for (unsigned int st_from=0; st_from < N_STATES-1; st_from++)
-    	{
-    		for (unsigned int st_to=st_from+1; st_to < N_STATES; st_to++)
-			{
-    			Correspondence2SparseCeres<MEAS_B_DIM, DIM, DIM>* corrBPtr = new Correspondence2SparseCeres<MEAS_B_DIM, DIM, DIM>(wolf_problem->getPrior()+st_from*DIM,wolf_problem->getPrior()+st_to*DIM);
-				VectorXs actualMeasurement = ((actualState.segment(st_from*DIM,DIM) - actualState.segment(st_to*DIM,DIM)).transpose() * (actualState.segment(st_from*DIM,DIM) - actualState.segment(st_to*DIM,DIM))).cwiseSqrt();
-				corrBPtr->inventMeasurement(actualMeasurement,generator,distribution_B);
-				wolf_problem->addCorrespondence(corrBPtr);
-			}
-    	}
-//    	correspondence_2_sparse_ceres<MEAS_B_DIM, DIM, DIM>* corrBPtr = new correspondence_2_sparse_ceres<MEAS_B_DIM, DIM, DIM>(wolf_problem->getPrior(),wolf_problem->getPrior()+DIM);
-//		VectorXs actualMeasurement = ((actualState.head(DIM) - actualState.tail(DIM)).transpose() * (actualState.head(DIM) - actualState.tail(DIM))).cwiseSqrt();
-//		corrBPtr->inventMeasurement(actualMeasurement,generator,distribution_B);
-//		wolf_problem->addCorrespondence(corrBPtr);
-	}
-
-	// cost function
-    std::cout << "Number of blocks: " << std::endl << wolf_problem->getCorrespondencesSize() << std::endl;
-	for (unsigned int block=0; block < wolf_problem->getCorrespondencesSize(); block++)
-	{
-		//std::cout << "block " << block << "...";
-		wolf_problem->getCorrespondence(block)->addBlock(ceres_problem);
-		//std::cout << " added!" << std::endl;
-	}
-
-	// run Ceres Solver
-	ceres::Solve(options, &ceres_problem, &summary);
-
-	//display results
-	std::cout << summary.FullReport() << "\n";
-    wolf_problem->print();
-
-    //clean
-    std::cout << "Cleaning ... " << std::endl << std::endl;
-    //ceres_problem.RemoveResidualBlock(rbId);
-    delete wolf_problem;
-    
-    //end Wolf iteration
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
-
diff --git a/src/examples/test_ceres_wrapper_jet_ind_block.cpp b/src/examples/test_ceres_wrapper_jet_ind_block.cpp
deleted file mode 100644
index ce98b4ed5748fde1294b694ce1a0dda25516fa9e..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_wrapper_jet_ind_block.cpp
+++ /dev/null
@@ -1,214 +0,0 @@
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-
-/**
- * This class emulates a Wolf top tree node class, such as vehicle. 
- * This class will be removed when effective linking to Wolf, and using actual Vehicle class. 
- * It holds:
- *      - a map to a state vector
- *      - a map to an error vector
- *      - a method to compute the error from the state
- * 
- **/
-using namespace Eigen;
-
-class AbsoluteCorrespondence
-{
-    protected:
-        Map<VectorXs> state_block_mapped_;
-
-        //Just to generate fake measurements
-        std::default_random_engine generator_; //just to generate measurements
-        std::normal_distribution<WolfScalar> distribution_; //just to generate measurements
-        VectorXs measurement_; // invented measurement
-        ceres::CostFunction* cost_function;
-
-    public:
-
-        AbsoluteCorrespondence(WolfScalar* _statePtr, const unsigned int & _block_size) :
-        	state_block_mapped_(_statePtr,_block_size),
-            distribution_(0.0,0.01),
-			measurement_(_block_size),
-			cost_function(new ceres::AutoDiffCostFunction<AbsoluteCorrespondence,1,1>(this))
-        {
-        }
-
-        virtual ~AbsoluteCorrespondence()
-        {
-        }
-
-        WolfScalar *getPrior()
-        {
-            return state_block_mapped_.data();
-        }
-
-        void inventMeasurement(std::default_random_engine& _generator)
-        {
-            for(unsigned int ii=0; ii<state_block_mapped_.size(); ii++)
-                measurement_(ii) = 1 + distribution_(_generator); //just inventing a sort of noise measurements
-        }
-
-        template <typename T>
-        bool operator()(const T* const _x, T* _residuals) const
-        {
-        	// Remap the vehicle state to the const evaluation point
-			Map<const Matrix<T,Dynamic,1>> state_map_const_(_x, state_block_mapped_.size());
-
-			// Map residuals vector to matrix (with sizes of the measurements matrix)
-			Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, state_block_mapped_.size());
-
-			// Compute error or residuals
-			mapped_residuals = measurement_.cast<T>() - state_map_const_;
-
-			// std::cout << typeid(T).name() << std::endl;
-			// for (unsigned int j = 0; j<block_size_; j++)
-			// {
-			// 	std::cout << "_x(" << j <<") = " << _x[j] << std::endl;
-			// 	std::cout << "mapped_residuals(" << j <<") = " << mapped_residuals(j) << std::endl;
-			// }
-
-        	return true;
-        }
-
-        void addBlock(ceres::Problem & ceres_problem)
-        {
-        	ceres_problem.AddResidualBlock(cost_function, NULL, getPrior());
-        }
-};
-
-class WolfProblem
-{
-    protected:
-        VectorXs state_; //state storage
-        std::vector<AbsoluteCorrespondence*> correspondences_;
-
-        unsigned int state_size_;
-
-    public: 
-        WolfProblem() :
-            state_(),
-			correspondences_(0),
-			state_size_(0)
-        {
-        }
-        
-        virtual ~WolfProblem()
-        {
-        }
-        
-        WolfScalar *getPrior()
-		{
-			return state_.data();
-		}
-
-        AbsoluteCorrespondence* getCorrespondence(const unsigned int _idx)
-        {
-            return correspondences_[_idx];
-        }
-
-        unsigned int getCorrespondencesSize()
-        {
-        	return correspondences_.size();
-        }
-        
-        void resizeState(unsigned int _state_size)
-        {
-        	state_size_ = _state_size;
-            state_.resize(_state_size);
-        }
-
-        void addCorrespondence(AbsoluteCorrespondence* _absCorrPtr)
-        {
-        	correspondences_.push_back(_absCorrPtr);
-        }
-
-        void inventMeasurements(std::default_random_engine& _generator)
-        {
-        	for (std::vector<AbsoluteCorrespondence*>::iterator it = correspondences_.begin(); it != correspondences_.end(); it++)
-        		(*it)->inventMeasurement(_generator);
-        }
-        
-        void computePrior()
-        {
-            state_.setZero();
-        }
-
-        void print()
-        {
-            std::cout << "state_ : " << std::endl << state_.transpose() << std::endl << std::endl;
-        }
-};
-
-//This main is a single iteration of the WOLF. 
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv) 
-{
-    std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-    
-    //dimension 
-    const unsigned int STATE_DIM = 50; //just to test, all will be DIM-dimensional
-    const unsigned int N_MEASUREMENTS = 1000;
-    // init
-    google::InitGoogleLogging(argv[0]);
-    std::default_random_engine generator;
-
-    using ceres::AutoDiffCostFunction;
-    using ceres::CostFunction;
-
-    // Ceres problem initialization
-    ceres::Solver::Options options;
-    options.minimizer_progress_to_stdout = false;
-    options.minimizer_type = ceres::TRUST_REGION;
-    options.line_search_direction_type = ceres::LBFGS;
-    options.max_num_iterations = 10;
-    ceres::Solver::Summary summary;
-    ceres::Problem ceres_problem;
-
-    //wolf problem
-	WolfProblem *wolf_problem = new WolfProblem();
-    wolf_problem->resizeState(STATE_DIM);
-    wolf_problem->computePrior();
-    for(unsigned int st=0; st < N_MEASUREMENTS; st++)
-		for (unsigned int st=0; st < STATE_DIM; st++)
-			wolf_problem->addCorrespondence(new AbsoluteCorrespondence(wolf_problem->getPrior()+st,1));
-
-	// set measures. This will be replaced by the WOLF-ROS front-end, getting sensor readings from sensors and performing measurements to build the whole wolf tree
-    wolf_problem->inventMeasurements(generator);
-
-	// cost function
-    std::cout << "Number of blocks: " << std::endl << wolf_problem->getCorrespondencesSize() << std::endl;
-	for (unsigned int block=0; block < wolf_problem->getCorrespondencesSize(); block++)
-		wolf_problem->getCorrespondence(block)->addBlock(ceres_problem);
-
-	// run Ceres Solver
-	ceres::Solve(options, &ceres_problem, &summary);
-
-	//display results
-	std::cout << summary.FullReport() << "\n";
-    wolf_problem->print();
-
-    //clean
-    std::cout << "Cleaning ... " << std::endl << std::endl;
-    //ceres_problem.RemoveResidualBlock(rbId);
-    delete wolf_problem;
-    
-    //end Wolf iteration
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
-
diff --git a/src/examples/test_ceres_wrapper_numeric.cpp b/src/examples/test_ceres_wrapper_numeric.cpp
deleted file mode 100644
index 60528eafb5938967a443454f29132c5658f46a94..0000000000000000000000000000000000000000
--- a/src/examples/test_ceres_wrapper_numeric.cpp
+++ /dev/null
@@ -1,220 +0,0 @@
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf.h"
-
-/**
- * This class emulates a Wolf top tree node class, such as vehicle. 
- * This class will be removed when effective linking to Wolf, and using actual Vehicle class. 
- * It holds:
- *      - a map to a state vector
- *      - a map to an error vector
- *      - a method to compute the error from the state
- * 
- **/
-class WolfVehicle
-{
-    protected:
-        Eigen::VectorXs state_; //state storage where to compute prior, and where result is placed. Is the allocation for the state vector
-        Eigen::Map<const Eigen::VectorXs> state_map_; //state point to be evaluated by Wolf tree constraints 
-        Eigen::Map<Eigen::VectorXs> error_map_; //error computed by the wolf tree
-        unsigned int state_size_;
-        unsigned int error_size_;        
-        
-        //Just to generate fake measurements
-        std::default_random_engine generator_; //just to generate measurements
-        std::normal_distribution<double> distribution_; //just to generate measurements
-        Eigen::VectorXs measurements_; //just a set of invented measurements
-
-    public: 
-        WolfVehicle() :
-            state_(),
-            state_map_(nullptr,0),
-            error_map_(nullptr,0),
-            state_size_(0),
-            error_size_(0),
-            distribution_(0.0,0.1)
-        {
-            //
-        }
-        
-        virtual ~WolfVehicle()
-        {
-            //
-        }
-        
-        void setSizes(unsigned int _state_size, unsigned int _error_size)
-        {
-            state_size_ = _state_size;
-            error_size_ = _error_size;
-            state_.resize(_state_size);
-        }
-        
-        void remapState(const WolfScalar *_ptr)
-        {
-            new (&state_map_) Eigen::Map<const Eigen::VectorXs>(_ptr, state_size_);            
-        }        
-
-        void remapError(WolfScalar *_ptr)
-        {
-            new (&error_map_) Eigen::Map<Eigen::VectorXs>(_ptr, error_size_);            
-        }                
-        
-        void inventMeasurements(unsigned int _sz)
-        {
-            measurements_.resize(_sz);
-            for(unsigned int ii=0; ii<_sz; ii++)
-            {
-                measurements_(ii) = 1 + distribution_(generator_); //just inventing a sort of noise measurements
-            }
-        }
-        
-        WolfScalar *getState()
-        {
-            return state_.data();
-        }        
-                
-        void getState(Eigen::VectorXs & _v)
-        {  
-            _v.resize(state_.size());
-            _v = state_;
-        }
-        
-        void computePrior()
-        {
-            state_.setOnes();//just a fake prior
-        }                
-        
-        void computeError()
-        {
-            for(unsigned int ii=0; ii<error_map_.size(); ii++)
-            {
-                error_map_(ii) = measurements_(ii) - state_map_(ii); //just a trivial error function
-            }
-        }
-
-        void print()
-        {
-            std::cout << "measurements_: " << measurements_.transpose() << std::endl;
-            std::cout << "state_: " << state_.transpose() << std::endl;
-            //std::cout << "state_map_: " << state_map_.transpose() << std::endl;
-        }                
-};
-
-/**
- * This class wrapps the Wolf Vehicle and implements the operator(), to be used by CERES optimizer
- *      - the operator() overloaded
-**/
-class CeresWolfFunctor
-{
-    protected: 
-        std::shared_ptr<WolfVehicle> vehicle_ptr_; //pointer to Wolf Vehicle object
-        
-    public:
-        CeresWolfFunctor(std::shared_ptr<WolfVehicle> & _wv) :
-            vehicle_ptr_(_wv)
-        {
-            //std::cout << "CeresWolfFunctor(): " << vehicle_ptr_.use_count() << " " << state_ptr_.use_count() << " " << std::endl;
-        }
-        
-        virtual ~CeresWolfFunctor()
-        {
-            //
-        }
-        bool operator()(const WolfScalar * const _x, double* _residuals) const
-        {
-            // 1. Remap the vehicle state to the provided x
-            vehicle_ptr_->remapState(_x);
-
-            // 2. Remap the error to the provided address
-            vehicle_ptr_->remapError(_residuals);
-
-            // 3. Compute error
-            vehicle_ptr_->computeError();
-
-            // 4. return 
-            return true;
-        }
-        
-};
-
-//This main is a single iteration of the WOLF. 
-//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper
-int main(int argc, char** argv) 
-{
-    std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl;
-    
-    //dimension 
-    const unsigned int DIM = 19; //just to test, all will be DIM-dimensional
-    
-    // init
-    google::InitGoogleLogging(argv[0]);
-    
-    //wolf vehicle 
-    std::shared_ptr<WolfVehicle> vehiclePtr(std::make_shared<WolfVehicle>());
-    
-    //ceres functor
-    CeresWolfFunctor *functorPtr = new CeresWolfFunctor(vehiclePtr);
-  
-    // Allocate the cost function !!!! Difference is to create in the same line both objects -> in the last (): (new ... ) SEE test_ceres_basic.cpp
-    ceres::NumericDiffCostFunction<CeresWolfFunctor,ceres::CENTRAL,DIM,DIM>* 
-           cost_function_static = new ceres::NumericDiffCostFunction<CeresWolfFunctor,ceres::CENTRAL,DIM,DIM>(functorPtr);  
-           
-    //********************** start Wolf iteration *************************
-           
-        // set measures. This will be replaced by the WOLF-ROS front-end, getting sensor reading from sensors (callbacks) and performing measurements to build the whole wolf tree
-        vehiclePtr->inventMeasurements(DIM);
-        
-        // Resizing & remapping. Dimensions may come from a call to WolfVehicle::getSizes()
-        vehiclePtr->setSizes(DIM,DIM);
-
-        // Compute and gets the Prior (initial guess). This would be the prior given by Wolf
-        vehiclePtr->computePrior();
-               
-        // build Ceres problem 
-        ceres::Problem *problem = new ceres::Problem();
-        problem->AddResidualBlock(cost_function_static, nullptr, vehiclePtr->getState());
-
-        // run Ceres Solver
-        ceres::Solver::Options options;
-        options.minimizer_progress_to_stdout = true;
-        options.minimizer_type = ceres::TRUST_REGION;
-        options.line_search_direction_type = ceres::LBFGS;
-        options.max_num_iterations = 10;
-        ceres::Solver::Summary summary;
-        ceres::Solve(options, problem, &summary);
-
-        //display results
-        std::cout << std::endl<< "Ceres Report:" << std::endl;
-        std::cout << summary.BriefReport() << "\n";
-        std::cout << std::endl << "Wolf vectors:" << std::endl;        
-        vehiclePtr->print();
-        
-    //********************** end Wolf iteration *************************        
-        
-    //clean
-    //std::cout << "Cleaning ... " << std::endl << std::endl;
-    //something to do ??
-    delete problem;
-    //delete vehiclePtr;
-    //delete cost_function_static;
-    //delete cost_function_static;
-       
-    //end Wolf iteration
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
-
diff --git a/src/examples/test_wolf_tree.cpp b/src/examples/test_wolf_tree.cpp
index 7dfb31390a3e8edccdb7f0d78bf6f2fcd6840a50..3ff333680dde3574c55b9d9dd7c6c5a3b4ea1ecf 100644
--- a/src/examples/test_wolf_tree.cpp
+++ b/src/examples/test_wolf_tree.cpp
@@ -1,4 +1,4 @@
-// Testing a full wolf tree avoiding template classes for NodeLinked derived classes
+// Testing create and delete full wolf tree with odometry captures
 
 //std includes
 #include <iostream>
@@ -7,59 +7,40 @@
 #include <cmath>
 #include <queue>
 
-//ceres
-#include "ceres/ceres.h"
-
 //Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "capture_base.h"
-#include "sensor_base.h"
+#include "wolf_manager.h"
 
 
 int main(int argc, char** argv) 
-{    
-    //wolf manager variables
-    std::queue<FrameBase> trajectory_; //this will be owned by the wolf manager
-    std::list<CaptureBaseShPtr> pending_captures_; //this will be owned by the wolf manager 
-    std::list<CaptureBaseShPtr>::iterator pending_it_; //this will be owned by the wolf manager 
-    Eigen::VectorXs sp(6);
-    sp << 0.1,0.1,0.1,0,0,0;
-    SensorBase sensor1(ABSOLUTE_POSE,sp); //just one sensor. This will be owned by the wolf manager
-    sp << 0.2,0.2,0.2,0,0,0;
-    SensorBase sensor2(ABSOLUTE_POSE,sp); //just another sensor. This will be owned by the wolf manager
-
-    //ROS callbacks varaibles (will be get from message)
-    TimeStamp ros_ts; //this plays the role of ros::Time, or msg->header.stamp
-    Eigen::VectorXs sensor_reading(4); //this plays the role of the ROS message content (sensor reading). Reading of dim=4 (example)
-    
+{
     //Welcome message
     std::cout << std::endl << " ========= WOLF TREE test ===========" << std::endl << std::endl;
 
+    SensorOdom2D* odom_sensor_ptr_ = new SensorOdom2D(new StateBlock(Eigen::Vector3s::Zero()),
+                                                      new StateBlock(Eigen::Vector1s::Zero()), 0.1, 0.1);
+
+    WolfManager* wolf_manager_ = new WolfManager(PO_2D,                             //frame structure
+                                                 odom_sensor_ptr_,                  //odom sensor
+                                                 Eigen::Vector3s::Zero(),           //prior
+                                                 Eigen::Matrix3s::Identity()*0.01,  //prior cov
+                                                 5,                                 //window size
+                                                 1);                                //time for new keyframe
+    wolf_manager_->addSensor(odom_sensor_ptr_);
+
     //main loop
-    for (unsigned int ii = 0; ii<10; ii++)
+    for (unsigned int ii = 0; ii<1000; ii++)
     {
-        //1. a new sensor data arrives (this part will be placed on ROS callbacks)
-        ros_ts.setToNow();
-        sensor_reading << 1,2,3,4;
-        std::shared_ptr<CaptureBase> capture( new CaptureBase(ros_ts.get(), &sensor1, sensor_reading) );
-        pending_captures_.push_back(capture);
-        
-        //2. Process pending_captures_, deciding for each new capture wheter a Frame has to be created or they have to be linked to the last one
-        for (pending_it_ = pending_captures_.begin(); pending_it_ != pending_captures_.end(); ++pending_it_)
-        {
-            capture->processCapture(); //This should create features    
-            
-        }
-        
-        //3. Stablish correspondences
-        
-        //4. Call ceres solver
-        
-        //5. publish results
-        
+        // Add sintetic odometry
+        wolf_manager_->addCapture(new CaptureOdom2D(TimeStamp(ii*0.1),
+                                                    TimeStamp(ii*0.1+0.01),
+                                                    odom_sensor_ptr_,
+                                                    Eigen::Vector3s(0.1, 0. ,0.05)));
+        // update wolf tree
+        wolf_manager_->update();
     }
-    
+
+    delete wolf_manager_; //not necessary to delete anything more, wolf will do it!
+
     //End message
     std::cout << " =========================== END ===============================" << std::endl << std::endl;