diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 04dc8922322eae1890cbbcb1769a99c5a4e2d6eb..88ff12b8d47ee72be9b164b23912dec3d94696ec 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -64,6 +64,8 @@ SET(HDRS
     sensor_gps_fix.h
     state_base.h
     state_point.h
+    state_orientation.h
+    state_theta.h
     state_complex_angle.h
     time_stamp.h
     trajectory_base.h
@@ -91,6 +93,8 @@ SET(SRCS
     sensor_odom_2D.cpp
     sensor_gps_fix.cpp
     state_base.cpp
+    state_orientation.cpp
+    state_theta.cpp
     state_complex_angle.cpp
     time_stamp.cpp
     trajectory_base.cpp
diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp
index 2217d43280abcfa479e481e77a4f8caecdbcb842..bd366c0332436c7c7ec2c40391e029fd9314abe7 100644
--- a/src/capture_gps_fix.cpp
+++ b/src/capture_gps_fix.cpp
@@ -26,11 +26,9 @@ CaptureGPSFix::~CaptureGPSFix()
 void CaptureGPSFix::processCapture()
 {
 	// EXTRACT AND ADD FEATURES
-//    FeatureBase* new_feature = new FeatureGPSFix(data_,data_covariance_);
     addFeature(new FeatureGPSFix(data_,data_covariance_));
 
     // ADD CONSTRAINT
-//    ConstraintBase* gps_constraint = new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()->getPPtr());
 	getFeatureListPtr()->front()->addConstraint(new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()->getPPtr()));
 }
 
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index fd09248ade5284fc0ee24977c323ecb1e30fcd02..5a8e564b5be2f787ffdc4cd67e3185b291a32c13 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -32,29 +32,32 @@ CaptureLaser2D::~CaptureLaser2D()
 void CaptureLaser2D::processCapture()
 {
     //variables
-    std::list<Eigen::Vector4s> corners;
+    //std::list<Eigen::Vector4s> corners;
+    std::list<laserscanutils::Corner> corners;
     
     //extract corners from range data
     extractCorners(corners);
     //std::cout << corners.size() << " corners extracted" << std::endl;
     
     //generate a feature for each corner
-    //std::cout << "CaptureLaser2D::createFeatures..." << std::endl;
     createFeatures(corners);
+    //std::cout << getFeatureListPtr()->size() << " Features created" << std::endl;
     
     //Establish constraints for each feature
-    //std::cout << "CaptureLaser2D::establishConstraints..." << std::endl;
     establishConstraints();
+    //std::cout << "Constraints created" << std::endl;
 }
 
-unsigned int CaptureLaser2D::extractCorners(std::list<Eigen::Vector4s> & _corner_list) const
+unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const
 {
     return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _corner_list);
 }
 
-void CaptureLaser2D::createFeatures(std::list<Eigen::Vector4s> & _corner_list)
+void CaptureLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corner_list)
 {
+	// TODO: Sensor model
     Eigen::Matrix4s cov_mat;
+    Eigen::Vector4s meas;
     
     //init constant cov
     cov_mat << 0.01, 0,    0,    0,
@@ -65,8 +68,11 @@ void CaptureLaser2D::createFeatures(std::list<Eigen::Vector4s> & _corner_list)
     //for each corner in the list create a feature
     for (auto corner_it = _corner_list.begin(); corner_it != _corner_list.end(); corner_it ++)
     {
-    	(*corner_it)(2) = 0;
-        this->addFeature( (FeatureBase*)(new FeatureCorner2D( (*corner_it), cov_mat ) ) );
+    	meas.head(2) = (*corner_it).pt_.head(2);
+    	meas(2) = (*corner_it).orientation_;
+    	meas(3) = (*corner_it).aperture_;
+    	//TODO: add the rest of descriptors and struct atributes
+        this->addFeature( (FeatureBase*)(new FeatureCorner2D( meas, cov_mat ) ) );
     }
 }
 
@@ -74,21 +80,14 @@ void CaptureLaser2D::establishConstraints()
 {
 	// Global transformation TODO: Change by a function
 	Eigen::Vector2s t_robot(*getFramePtr()->getPPtr()->getPtr(),*(getFramePtr()->getPPtr()->getPtr()+1));
-	WolfScalar o = *(getFramePtr()->getOPtr()->getPtr());
-	Eigen::Matrix2s R_robot;
-	if (getFramePtr()->getOPtr()->getStateType() == ST_THETA)
-		R_robot << cos(o),-sin(o),
-			 sin(o), cos(o);
-    else
-    {
-    	//TODO
-    }
+	Eigen::Matrix2s R_robot = ((StateOrientation*)(getFramePtr()->getOPtr()))->getRotationMatrix().topLeftCorner<2,2>();
+	WolfScalar& robot_orientation = *(getFramePtr()->getOPtr()->getPtr());
 
 	// Sensor transformation
 	Eigen::Vector2s t_sensor = getSensorPtr()->getSensorPosition()->head(2);
-	Eigen::Matrix2s R_sensor = getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>().transpose();
+	Eigen::Matrix2s R_sensor = getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>();
 
-    //Brute force closest (xy and theta) landmark search
+    //Brute force closest (xy and theta) landmark search //TODO: B&B
 //    std::cout << "Brute force closest (xy and theta) landmark search: N features:" << getFeatureListPtr()->size() << std::endl;
 //    std::cout << "N landmark:" << getTop()->getMapPtr()->getLandmarkListPtr()->size() << std::endl;
 //    std::cout << "Vehicle transformation: " << std::endl;
@@ -96,30 +95,37 @@ void CaptureLaser2D::establishConstraints()
 //	std::cout << "rot:" << R << std::endl;
     for (auto feature_it = getFeatureListPtr()->begin(); feature_it != getFeatureListPtr()->end(); feature_it++ )
 	{
-		double max_distance_matching2 = 0.5; //TODO: max_distance_matching depending on localization and landmarks uncertainty
-		double max_theta_matching = M_PI / 8; //TODO: max_theta_matching depending on localization and landmarks uncertainty
+    	WolfScalar max_distance_matching2 = 0.5;
+    	WolfScalar max_theta_matching = M_PI / 8;
 
 		//Find the closest landmark to the feature
 		LandmarkCorner2D* correspondent_landmark = nullptr;
-    	Eigen::Map<Eigen::Vector2s> feature_position((*feature_it)->getMeasurementPtr()->data());
-    	Eigen::Map<Eigen::Vector1s> feature_orientation = ((*feature_it)->getMeasurementPtr()->data()+2);
+		const Eigen::Vector2s& feature_position = (*feature_it)->getMeasurement().head(2);
+    	const WolfScalar& feature_orientation = (*feature_it)->getMeasurement()(2);
+    	const WolfScalar& feature_aperture = (*feature_it)->getMeasurement()(3);
 
     	Eigen::Vector2s feature_global_position = R_robot * (R_sensor * feature_position + t_sensor) + t_robot;
-    	Eigen::Vector1s feature_global_orientation;
-    	feature_global_orientation(0) = feature_orientation(0) + o + atan2(R_sensor(1,0),R_sensor(0,0));
+    	WolfScalar feature_global_orientation = feature_orientation + robot_orientation + atan2(R_sensor(1,0),R_sensor(0,0));
+    	// fit in (-pi, pi]
+    	feature_global_orientation = (feature_global_orientation > 0 ? fmod(feature_global_orientation+M_PI, 2 * M_PI)-M_PI : fmod(feature_global_orientation-M_PI, 2 * M_PI)+M_PI);
     	//feature_global_orientation(0) = 0;
     	double min_distance2 = max_distance_matching2;
 
-//    	std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl;
+    	std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl;
 //    	std::cout << "local position: " << feature_position.transpose() << " orientation:" << feature_orientation << std::endl;
-//    	std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl;
+    	std::cout << "global position: " << feature_global_position.transpose() <<
+    				 " orientation: " << feature_global_orientation <<
+    				 " aperture:" << feature_aperture << std::endl;
     	for (auto landmark_it = getTop()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != getTop()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ )
 		{
     		Eigen::Map<Eigen::Vector2s> landmark_position((*landmark_it)->getPPtr()->getPtr());
-    		WolfScalar landmark_orientation = *((*landmark_it)->getOPtr()->getPtr());
+    		std::cout << "landmark: " << (*landmark_it)->nodeId() << " " << landmark_position.transpose();
+    		WolfScalar& landmark_orientation = *((*landmark_it)->getOPtr()->getPtr());
+    		std::cout << " orientation: " << landmark_orientation << std::endl;
+    		const WolfScalar& landmark_aperture = (*landmark_it)->getDescriptor()(0);
 
     		WolfScalar distance2 = (landmark_position-feature_global_position).transpose() * (landmark_position-feature_global_position);
-    		WolfScalar theta_distance = fabs(landmark_orientation-feature_global_orientation(0));
+    		WolfScalar theta_distance = fabs(landmark_orientation-feature_global_orientation);
 
     		if (theta_distance > M_PI)
     			theta_distance -= 2 * M_PI;
@@ -127,40 +133,46 @@ void CaptureLaser2D::establishConstraints()
 			{
 				if (theta_distance < max_theta_matching)
 				{
-//					std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl;
-//					std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl;
+					std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl;
+					std::cout << "global position: " << landmark_position.transpose() << " orientation: " << landmark_orientation << std::endl;
 
 					correspondent_landmark = (LandmarkCorner2D*)(*landmark_it);
 					min_distance2 = distance2;
 				}
 				else
 				{
-					std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl;
-					std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl;
-					std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl;
-					std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl;
+//					std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl;
+//					std::cout << "global position:" << feature_global_position.transpose() <<
+//								 " orientation:" << feature_global_orientation <<
+//								 " aperture:" << feature_aperture << std::endl;
+//					std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl;
+//					std::cout << "global position:" << landmark_position.transpose() <<
+//								 " orientation:" << landmark_orientation <<
+//								 " aperture:" << landmark_aperture << std::endl;
 				}
 			}
 		}
     	if (correspondent_landmark == nullptr)
     	{
-    		//std::cout << "No landmark found. Creating a new one..." << std::endl;
+    		std::cout << "No landmark found. Creating a new one..." << std::endl;
     		StateBase* new_landmark_state_position = new StatePoint2D(getTop()->getNewStatePtr());
     		getTop()->addState(new_landmark_state_position, feature_global_position);
     		StateBase* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr());
-    		getTop()->addState(new_landmark_state_orientation, feature_global_orientation);
+    		getTop()->addState(new_landmark_state_orientation, Eigen::Map<Eigen::Vector1s>(&feature_global_orientation,1));
 
-    		correspondent_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation);
+    		correspondent_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, feature_aperture);
     		LandmarkBase* corr_landmark(correspondent_landmark);
     		getTop()->getMapPtr()->addLandmark(corr_landmark);
 
-    		//std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl;
-			//std::cout << "global position: " << *corr_landmark->getPPtr()->getPtr() << " " << *(corr_landmark->getPPtr()->getPtr()+1) << " orientation:" << *corr_landmark->getOPtr()->getPtr() << std::endl;
+//    		std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl;
+//			std::cout << "global position: " << *corr_landmark->getPPtr()->getPtr() << " " << *(corr_landmark->getPPtr()->getPtr()+1) <<
+//						 " orientation: " << *corr_landmark->getOPtr()->getPtr() <<
+//						 " aperture:" << corr_landmark->getDescriptor()(0) <<  std::endl;
     	}
     	else
     		correspondent_landmark->hit();
 
-    	//std::cout << "Creating new constraint: Landmark " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << " & feature " << (*feature_it)->nodeId() << std::endl;
+//    	std::cout << "Creating new constraint: Landmark " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << " & feature " << (*feature_it)->nodeId() << std::endl;
 
     	// Add constraint to the correspondent landmark
     	(*feature_it)->addConstraint(new ConstraintCorner2DTheta(*feature_it,
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 648c0bd404b63bdfd29c616b4824022f64c66af5..8f4c537af9b9f54dd98f60a46a723086a8d5fdde 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -10,8 +10,9 @@
 #include <eigen3/Eigen/Geometry>
 
 //laser_scan_utils
-#include "iri-algorithms/laser_scan_utils/scan_params.h"
-#include "iri-algorithms/laser_scan_utils/corners.h"
+#include "iri-algorithms/laser_scan_utils/scan_basics.h"
+#include "iri-algorithms/laser_scan_utils/corner_detector.h"
+#include "iri-algorithms/laser_scan_utils/entities.h"
 
 //wolf includes
 #include "constraint_corner_2D_theta.h"
@@ -20,6 +21,8 @@
 #include "feature_corner_2D.h"
 #include "landmark_corner_2D.h"
 #include "state_point.h"
+#include "state_orientation.h"
+#include "state_theta.h"
 
 //wolf forward declarations
 //#include "feature_corner_2D.h"
@@ -73,7 +76,7 @@ class CaptureLaser2D : public CaptureBase
          * Extract corners and push-back to Feature down list . 
          * 
          **/
-        virtual unsigned int extractCorners(std::list<Eigen::Vector4s> & _corner_list) const;
+        virtual unsigned int extractCorners(std::list<laserscanutils::Corner> & _corner_list) const;
 //         virtual unsigned int extractCorners_old(std::list<Eigen::Vector4s> & _corner_list) const;
 //         void fitLine(unsigned int _idx_from, unsigned int _idx_to, const Eigen::MatrixXs& _points, Line& line_) const;
 
@@ -86,7 +89,7 @@ class CaptureLaser2D : public CaptureBase
         
 //         void fitLine(unsigned int _idx_from, unsigned int _idx_to, const Eigen::MatrixXs& _points, Line& line_) const;
 
-        virtual void createFeatures(std::list<Eigen::Vector4s> & _corner_list); //TODO: should be const .... JVN: No, because new feature is added to the list
+        virtual void createFeatures(std::list<laserscanutils::Corner> & _corner_list); //TODO: should be const .... JVN: No, because new feature is added to the list
         
         void establishConstraints();
 
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 5948c5e8a8f6160814ea0049352c80175cae34ba..4d34b7c5fbe159d2d5019e128f30e9bb1040d060 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -8,17 +8,10 @@ CeresManager::CeresManager(ceres::Problem::Options _options) :
 	covariance_options.null_space_rank = -1;
 	covariance_ = new ceres::Covariance(covariance_options);
 }
-//num_threads(1), use_dense_linear_algebra(true), min_singular_value_threshold(1e-12), null_space_rank(-1), apply_loss_function(false)
-
 
 CeresManager::~CeresManager()
 {
-	std::vector<double*> state_units;
-
-	ceres_problem_->GetParameterBlocks(&state_units);
-
-	for (uint i = 0; i< state_units.size(); i++)
-		removeStateUnit(state_units.at(i));
+	removeAllStateUnits();
 
 	std::cout << "all state units removed! \n";
 	std::cout << "residual blocks: " << ceres_problem_->NumResidualBlocks() << "\n";
@@ -42,24 +35,8 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
 	return ceres_summary_;
 }
 
-void CeresManager::computeCovariances(WolfProblem* _problem_ptr)//StateBaseList* _state_list_ptr, StateBase* _current_state_unit)
+void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
 {
-//	std::cout << "_state_list_ptr.size() " << _state_list_ptr->size() << std::endl;
-//
-//	// create vector of pointer pairs
-//	std::vector<std::pair<const double*, const double*>> covariance_blocks;
-////	for (auto st_it = _state_list_ptr->begin(); st_it != _state_list_ptr->end(); st_it++)
-////		if ((*st_it)->getPtr() != _current_state_unit->getPtr())
-////			covariance_blocks.push_back(std::pair<const double*, const double*>((*st_it)->getPtr(),_current_state_unit->getPtr()));
-//
-//	WolfScalar* block_1_ptr = _current_state_unit->getPtr();
-//	WolfScalar* block_2_ptr = _current_state_unit->getPtr();
-//	std::cout << "are blocks? " << ceres_problem_->HasParameterBlock(block_1_ptr) << ceres_problem_->HasParameterBlock(block_2_ptr) << std::endl;
-//	covariance_blocks.push_back(std::make_pair(block_1_ptr,block_2_ptr));
-//	std::cout << "covariance_blocks.size() " << covariance_blocks.size() << std::endl;
-//	// Compute covariances
-//	covariance_.Compute(covariance_blocks, ceres_problem_);
-
 	std::vector<std::pair<const double*, const double*>> covariance_blocks;
 
 	// Last frame state units
@@ -99,63 +76,69 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)//StateBaseList*
 
 void CeresManager::update(WolfProblem* _problem_ptr)
 {
-	// ADD/UPDATE STATE UNITS
-	for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
+	// IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
+	if (_problem_ptr->isReallocated())
 	{
-		if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
+		// Remove all parameter blocks (residual blocks will be also removed)
+		removeAllStateUnits();
+
+		// Add all parameter blocks
+		for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
 			addStateUnit(*state_unit_it);
 
-		else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
-			updateStateUnitStatus(*state_unit_it);
+		// Add all residual blocks
+		ConstraintBaseList ctr_list;
+		_problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
+		for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
+			addConstraint(*ctr_it);
 	}
-	//std::cout << "state units updated!" << std::endl;
-
-	// REMOVE STATE UNITS
-	while (!_problem_ptr->getRemovedStateListPtr()->empty())
+	else
 	{
-		removeStateUnit(_problem_ptr->getRemovedStateListPtr()->front());
-		_problem_ptr->getRemovedStateListPtr()->pop_front();
-	}
-	//std::cout << "state units removed!" << std::endl;
+		// ADD/UPDATE STATE UNITS
+		for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
+		{
+			if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
+				addStateUnit(*state_unit_it);
 
-	// ADD CONSTRAINTS
-	ConstraintBaseList ctr_list;
-	_problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
-	//std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
-	for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
-		if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
-			addConstraint((*ctr_it));
+			else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
+				updateStateUnitStatus(*state_unit_it);
+		}
+		//std::cout << "state units updated!" << std::endl;
+
+		// REMOVE STATE UNITS
+		while (!_problem_ptr->getRemovedStateListPtr()->empty())
+		{
+			ceres_problem_->RemoveParameterBlock(_problem_ptr->getRemovedStateListPtr()->front());
+			_problem_ptr->getRemovedStateListPtr()->pop_front();
+		}
+		//std::cout << "state units removed!" << std::endl;
 
-	//std::cout << "constraints updated!" << std::endl;
+		// ADD CONSTRAINTS
+		ConstraintBaseList ctr_list;
+		_problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
+		//std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
+		for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
+			if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
+				addConstraint(*ctr_it);
+
+		//std::cout << "constraints updated!" << std::endl;
+	}
 }
 
 void CeresManager::addConstraint(ConstraintBase* _corr_ptr)
 {
 	ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector());
 //	constraint_map_[_corr_ptr->nodeId()] = blockIdx;
-
 	_corr_ptr->setPendingStatus(NOT_PENDING);
 }
 
-void CeresManager::addConstraints(ConstraintBaseList* _new_constraints_list_ptr)
-{
-	//std::cout << _new_constraints.size() << " new constraints\n";
-	for(auto constraint_it = _new_constraints_list_ptr->begin(); constraint_it!=_new_constraints_list_ptr->end(); constraint_it++)
-		addConstraint(*constraint_it);
-}
-
 void CeresManager::removeConstraint(const unsigned int& _corr_idx)
 {
+	// TODO: necessari? outliers?
 //	ceres_problem_->RemoveResidualBlock(constraint_map_[_corr_idx]);
 //	constraint_map_.erase(_corr_idx);
 }
 
-void CeresManager::removeConstraints(const std::list<unsigned int>& _corr_idx_list)
-{
-	for (auto idx_it=_corr_idx_list.begin(); idx_it!=_corr_idx_list.end(); idx_it++)
-		removeConstraint(*idx_it);
-}
-
 void CeresManager::addStateUnit(StateBase* _st_ptr)
 {
 	//std::cout << "Adding State Unit " << _st_ptr->nodeId() << std::endl;
@@ -176,7 +159,13 @@ void CeresManager::addStateUnit(StateBase* _st_ptr)
 //					ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization);
 //					break;
 //				}
-		case ST_POINT_1D: // equivalent ST_THETA:
+		case ST_THETA:
+		{
+			//std::cout << "No Local Parametrization to be added" << std::endl;
+			ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateTheta*)_st_ptr)->BLOCK_SIZE, nullptr);
+			break;
+		}
+		case ST_POINT_1D:
 		{
 			//std::cout << "No Local Parametrization to be added" << std::endl;
 			ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
@@ -203,21 +192,14 @@ void CeresManager::addStateUnit(StateBase* _st_ptr)
 	_st_ptr->setPendingStatus(NOT_PENDING);
 }
 
-void CeresManager::addStateUnits(StateBaseList* _new_state_units)
+void CeresManager::removeAllStateUnits()
 {
-	for(auto state_unit_it = _new_state_units->begin(); state_unit_it!=_new_state_units->end(); state_unit_it++)
-		addStateUnit(*state_unit_it);
-}
+	std::vector<double*> parameter_blocks;
 
-void CeresManager::removeStateUnit(WolfScalar* _st_ptr)
-{
-	ceres_problem_->RemoveParameterBlock(_st_ptr);
-}
+	ceres_problem_->GetParameterBlocks(&parameter_blocks);
 
-void CeresManager::removeStateUnits(std::list<WolfScalar*> _st_ptr_list)
-{
-	for(auto state_unit_it = _st_ptr_list.begin(); state_unit_it!=_st_ptr_list.end(); state_unit_it++)
-		removeStateUnit(*state_unit_it);
+	for (unsigned int i = 0; i< parameter_blocks.size(); i++)
+		ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
 }
 
 void CeresManager::updateStateUnitStatus(StateBase* _st_ptr)
@@ -232,12 +214,6 @@ void CeresManager::updateStateUnitStatus(StateBase* _st_ptr)
 	_st_ptr->setPendingStatus(NOT_PENDING);
 }
 
-void CeresManager::updateStateUnitStatus(StateBaseList* _st_ptr_list)
-{
-	for(auto state_unit_it = _st_ptr_list->begin(); state_unit_it!=_st_ptr_list->end(); state_unit_it++)
-		updateStateUnitStatus(*state_unit_it);
-}
-
 ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr)
 {
 	//std::cout << "adding ctr " << _corrPtr->nodeId() << std::endl;
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index 5b19540f531f5748c58406a22dc034fd892c3d98..74b81d5eac3ac20f2fd7191c8e6ddc6d09584386 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -11,6 +11,7 @@
 #include "state_base.h"
 #include "state_point.h"
 #include "state_complex_angle.h"
+#include "state_theta.h"
 #include "constraint_sparse.h"
 #include "constraint_gps_2D.h"
 #include "constraint_odom_2D_theta.h"
@@ -38,35 +39,21 @@ class CeresManager
 
 		ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
 
-		void computeCovariances(WolfProblem* _problem_ptr);//StateBaseList* _state_units_list, StateBase* _current_state_unit);
+		void computeCovariances(WolfProblem* _problem_ptr);
 
 		void update(const WolfProblemPtr _problem_ptr);
 
 		void addConstraint(ConstraintBase* _corr_ptr);
 
-		// TODO: not necessary
-		void addConstraints(ConstraintBaseList* _new_constraints_list_ptr);
-
-		// TODO: not necessary
+		// TODO: not necessary?
 		void removeConstraint(const unsigned int& _corr_idx);
 
-		// TODO: not necessary
-		void removeConstraints(const std::list<unsigned int>& _corr_idx_list);
-
 		void addStateUnit(StateBase* _st_ptr);
 
-		// TODO: not necessary
-		void addStateUnits(StateBaseList* _st_ptr_list);
-
-		void removeStateUnit(WolfScalar* _st_ptr);
-
-		void removeStateUnits(std::list<WolfScalar*> _st_ptr_list);
+		void removeAllStateUnits();
 
 		void updateStateUnitStatus(StateBase* _st_ptr);
 
-		// TODO: not necessary
-		void updateStateUnitStatus(StateBaseList* _st_ptr_list);
-
 		ceres::CostFunction* createCostFunction(ConstraintBase* _corrPtr);
 };
 
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index d493e76e96cb62f2ef1efda87bbffebb67db036e..6d1f1c614e2557f0567cd9d4c4938faabbd87c64 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -3,8 +3,8 @@
 ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp) :
     NodeLinked(BOTTOM, "CONSTRAINT"),
     type_(_tp),
-	measurement_ptr_(_ftr_ptr->getMeasurementPtr()),
-	measurement_covariance_ptr_(_ftr_ptr->getMeasurementCovariancePtr())
+	measurement_(_ftr_ptr->getMeasurement()),
+	measurement_covariance_(_ftr_ptr->getMeasurementCovariance())
 {
 	//
 }
@@ -19,9 +19,9 @@ ConstraintType ConstraintBase::getConstraintType() const
     return type_;
 }
 
-const Eigen::VectorXs * ConstraintBase::getMeasurementPtr()
+const Eigen::VectorXs& ConstraintBase::getMeasurement()
 {
-	return upperNodePtr()->getMeasurementPtr();
+	return measurement_;
 }
 
 FeatureBase* ConstraintBase::getFeaturePtr() const
diff --git a/src/constraint_base.h b/src/constraint_base.h
index c167cbaa64c651653631ad53e8cdf758731e576f..4824c899658284f3c3bd1698cc4b217cd164eafb 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -21,8 +21,8 @@ class ConstraintBase : public NodeLinked<FeatureBase,NodeTerminus>
 {
     protected:
         ConstraintType type_; //type of constraint (types defined at wolf.h)
-        Eigen::VectorXs * measurement_ptr_; // TODO:TBD: pointer, map or copy of the feature measurement?
-        Eigen::MatrixXs * measurement_covariance_ptr_; // TODO:TBD: pointer, map or copy of the feature measurement covariance?
+        const Eigen::VectorXs& measurement_; // TODO:TBD: pointer, map or copy of the feature measurement?
+        const Eigen::MatrixXs& measurement_covariance_; // TODO:TBD: pointer, map or copy of the feature measurement covariance?
         
     public:
         /** \brief Constructor
@@ -58,7 +58,7 @@ class ConstraintBase : public NodeLinked<FeatureBase,NodeTerminus>
 		 * Returns a pointer to the feature measurement
 		 *
 		 **/
-        const Eigen::VectorXs * getMeasurementPtr();
+        const Eigen::VectorXs& getMeasurement();
 
         /** \brief Returns a pointer to its capture
 		 *
diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h
index 60d4a5fbe9f3fb0a16f174884e126464a135628c..06a3a370c907a8db9759d88f3c9841f7a1279ba3 100644
--- a/src/constraint_corner_2D_theta.h
+++ b/src/constraint_corner_2D_theta.h
@@ -56,7 +56,6 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
 //			std::cout << getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
 //			std::cout << "atan2: " << atan2(getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,1),getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,0)) << std::endl;
 
-
 			// sensor transformation
 			Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).cast<T>();
 			Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>().transpose()).cast<T>();
@@ -75,9 +74,9 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
 				expected_landmark_relative_orientation = expected_landmark_relative_orientation + T(2*M_PI);
 
 			// Residuals
-			_residuals[0] = (expected_landmark_relative_position(0) - T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0));
-			_residuals[1] = (expected_landmark_relative_position(1) - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1));
-			_residuals[2] = (expected_landmark_relative_orientation - T((*measurement_ptr_)(2))) / T(100*(*measurement_covariance_ptr_)(2,2));
+			_residuals[0] = (expected_landmark_relative_position(0) - T(measurement_(0))) / T(measurement_covariance_(0,0));
+			_residuals[1] = (expected_landmark_relative_position(1) - T(measurement_(1))) / T(measurement_covariance_(1,1));
+			_residuals[2] = (expected_landmark_relative_orientation - T(measurement_(2))) / T(100*measurement_covariance_(2,2));
 
 //			std::cout << "\nCONSTRAINT: " << nodeId() << std::endl;
 //			std::cout << "Feature: " << getFeaturePtr()->nodeId() << std::endl;
diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h
index 8d1648b2bcb30c4782db2281cb31694d21cfd44b..f3e82559f3e265cc53510e255cef8c1d832ee200 100644
--- a/src/constraint_gps_2D.h
+++ b/src/constraint_gps_2D.h
@@ -31,8 +31,8 @@ class ConstraintGPS2D: public ConstraintSparse<2,2>
 		template <typename T>
 		bool operator()(const T* const _x, T* _residuals) const
 		{
-			_residuals[0] = (T((*measurement_ptr_)(0)) - _x[0]) / T((*measurement_covariance_ptr_)(0,0));
-			_residuals[1] = (T((*measurement_ptr_)(1)) - _x[1]) / T((*measurement_covariance_ptr_)(1,1));
+			_residuals[0] = (T(measurement_(0)) - _x[0]) / T(measurement_covariance_(0,0));
+			_residuals[1] = (T(measurement_(1)) - _x[1]) / T(measurement_covariance_(1,1));
 			return true;
 		}
 };
diff --git a/src/constraint_odom_2D_complex_angle.h b/src/constraint_odom_2D_complex_angle.h
index 965393ba7e7b3fff4eb97b69066ac2848e8aa9f7..320e86f0ac4f54db8f1b4ddf76a9f4e5baf6da9b 100644
--- a/src/constraint_odom_2D_complex_angle.h
+++ b/src/constraint_odom_2D_complex_angle.h
@@ -36,8 +36,8 @@ class ConstraintOdom2DComplexAngle: public ConstraintSparse<2,2,2,2,2>
 			T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
 
 			// Residuals
-			_residuals[0] = (expected_range - T((*measurement_ptr_)(0))*T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0));
-			_residuals[1] = (expected_rotation - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1));
+			_residuals[0] = (expected_range - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0));
+			_residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1));
 
 			return true;
 		}
diff --git a/src/constraint_odom_2D_theta.h b/src/constraint_odom_2D_theta.h
index 97b55fe1c7041f9f6f193e934669b1941c093d37..f637169c3156d499f82f147c7f1f3259d21bbad8 100644
--- a/src/constraint_odom_2D_theta.h
+++ b/src/constraint_odom_2D_theta.h
@@ -36,8 +36,8 @@ class ConstraintOdom2DTheta: public ConstraintSparse<2,2,1,2,1>
 			T expected_rotation = _o2[0]-_o1[0];
 
 			// Residuals
-			_residuals[0] = (expected_range - T((*measurement_ptr_)(0))*T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0));
-			_residuals[1] = (expected_rotation - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1));
+			_residuals[0] = (expected_range - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0));
+			_residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1));
 
 			return true;
 		}
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 1d9713765430ad1a450822f4d8ae13f272b46e32..849bbf0537273a806af510f5c3d4b6b7f548280b 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -35,8 +35,8 @@ TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME})
 # 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})
+#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)
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 31bc5d1003c034d99e171a1f03f5d5705572ae4b..874d524f2fae4d097656f6901d442cf7d60f2842 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -50,6 +50,10 @@
 #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_)
 {
@@ -123,7 +127,6 @@ class WolfManager
 		{
         	createFrame(_init_frame, TimeStamp());
         	problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
-
 		}
 
         virtual ~WolfManager()
@@ -172,22 +175,8 @@ class WolfManager
         		// ODOMETRY SENSOR
         		if (new_capture->getSensorPtr() == sensor_prior_)
         		{
-        			// FIND PREVIOUS RELATIVE CAPTURE
-//					CaptureRelative* 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 = (CaptureRelative*)(*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((CaptureRelative*)(new_capture));
-//					Eigen::VectorXs prior = previous_relative_capture->computePrior();
         			last_capture_relative_->integrateCapture((CaptureRelative*)(new_capture));
 
         			// NEW KEY FRAME (if enough time from last frame)
@@ -434,14 +423,14 @@ int main(int argc, char** argv)
 		// DRAWING STUFF ---------------------------
 		t1=clock();
 		// draw detected corners
-		std::list<Eigen::Vector4s> corner_list;
+		std::list<laserscanutils::Corner> corner_list;
 		std::vector<double> corner_vector;
 		CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1_reading);
 		last_scan.extractCorners(corner_list);
-		for (std::list<Eigen::Vector4s>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
+		for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
 		{
-			corner_vector.push_back(corner_it->x());
-			corner_vector.push_back(corner_it->y());
+			corner_vector.push_back(corner_it->pt_(0));
+			corner_vector.push_back(corner_it->pt_(1));
 		}
 		myRender->drawCorners(laser1Pose,corner_vector);
 
diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp
index 93bec962d253f6a166c58876c513160a675c873e..49dce88ab0c9c7c3e7598750e004553bc70f41be 100644
--- a/src/examples/test_ceres_laser.cpp
+++ b/src/examples/test_ceres_laser.cpp
@@ -50,6 +50,10 @@
 #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_)
 {
@@ -436,14 +440,14 @@ int main(int argc, char** argv)
 		// DRAWING STUFF ---------------------------
 		t1=clock();
 		// draw detected corners
-		std::list<Eigen::Vector4s> corner_list;
+		std::list<laserscanutils::Corner> corner_list;
 		std::vector<double> corner_vector;
 		CaptureLaser2D last_scan(time_stamp, &laser_sensor, scan_reading);
 		last_scan.extractCorners(corner_list);
-		for (std::list<Eigen::Vector4s>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
+		for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
 		{
-			corner_vector.push_back(corner_it->x());
-			corner_vector.push_back(corner_it->y());
+			corner_vector.push_back(corner_it->pt_(0));
+			corner_vector.push_back(corner_it->pt_(1));
 		}
 		myRender->drawCorners(devicePose,corner_vector);
 
diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp
index 9ad87ed7267cb2bab2e46764859f3626990479b4..591267237367312e9d23517d989b0e8b373e1f03 100644
--- a/src/examples/test_ceres_manager.cpp
+++ b/src/examples/test_ceres_manager.cpp
@@ -22,6 +22,7 @@
 #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"
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index dca55969fb15fa1540b36849734b213cbc44b55b..fadc2ffaca6cba68a9a0fc804e8e59f05a705a97 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -46,14 +46,24 @@ void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list)
 		_ctr_list.push_back((*c_it));
 }
 
-Eigen::VectorXs * FeatureBase::getMeasurementPtr()
+//Eigen::VectorXs * FeatureBase::getMeasurementPtr()
+//{
+//    return &measurement_;
+//}
+//
+//Eigen::MatrixXs * FeatureBase::getMeasurementCovariancePtr()
+//{
+//    return &measurement_covariance_;
+//}
+
+const Eigen::VectorXs& FeatureBase::getMeasurement() const
 {
-    return &measurement_;
+    return measurement_;
 }
 
-Eigen::MatrixXs * FeatureBase::getMeasurementCovariancePtr()
+const Eigen::MatrixXs& FeatureBase::getMeasurementCovariance() const
 {
-    return &measurement_covariance_;
+    return measurement_covariance_;
 }
 
 void FeatureBase::setMeasurement(const Eigen::VectorXs & _meas)
diff --git a/src/feature_base.h b/src/feature_base.h
index fb0cd8ed03d397813ab4e7a68eeb6546e01cb5ae..59ffe6e31f026af54f550da703e63ef5ecb59a1a 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -48,9 +48,13 @@ class FeatureBase : public NodeLinked<CaptureBase,ConstraintBase>
         
         void getConstraintList(ConstraintBaseList & _ctr_list);
 
-        Eigen::VectorXs * getMeasurementPtr();
-        
-        Eigen::MatrixXs * getMeasurementCovariancePtr();
+//        Eigen::VectorXs * getMeasurementPtr();
+//
+//        Eigen::MatrixXs * getMeasurementCovariancePtr();
+
+        const Eigen::VectorXs & getMeasurement() const;
+
+        const Eigen::MatrixXs & getMeasurementCovariance() const;
 
         void setMeasurement(const Eigen::VectorXs & _meas);
         
diff --git a/src/feature_corner_2D.cpp b/src/feature_corner_2D.cpp
index 38bec15120a5807060b76b3d715dfee2901e39ed..af9b5f16f2cfce004ea0e60f9a0d6d30c648488e 100644
--- a/src/feature_corner_2D.cpp
+++ b/src/feature_corner_2D.cpp
@@ -11,8 +11,3 @@ FeatureCorner2D::~FeatureCorner2D()
 {
     //
 }
-
-//void FeatureCorner2D::findConstraints()
-//{
-//    //
-//}
diff --git a/src/feature_corner_2D.h b/src/feature_corner_2D.h
index d38fd57b615602895d0c4b78e6cf340397ccdf9e..770216a7a46e2ef28a9f032a25365f49c0653202 100644
--- a/src/feature_corner_2D.h
+++ b/src/feature_corner_2D.h
@@ -23,14 +23,6 @@ class FeatureCorner2D : public FeatureBase
          *
          */        
         virtual ~FeatureCorner2D();
-
-        //TODO: Encara que sigui brutforce jo la búsqueda la faria a nivell capture.
-        /** \brief Generic interface to find constraints
-         * 
-         * Generic interface to find constraints between this feature and a map (static/slam) or a previous feature
-         *
-         **/
-        //virtual void findConstraints();
         
 };
 #endif
diff --git a/src/feature_gps_fix.cpp b/src/feature_gps_fix.cpp
index 94ffd56f23d5bdbbce4c61d040d4d38b5997f07b..3106e34aab0682de69cb0de4de16ea97b7114fd3 100644
--- a/src/feature_gps_fix.cpp
+++ b/src/feature_gps_fix.cpp
@@ -16,9 +16,3 @@ FeatureGPSFix::~FeatureGPSFix()
 {
     //
 }
-
-void FeatureGPSFix::findConstraints()
-{
-	//ConstraintBase* gps_constraint(new ConstraintGPS2D(this, getCapturePtr()->getFramePtr()->getPPtr()));
-	addConstraint(new ConstraintGPS2D(this, getCapturePtr()->getFramePtr()->getPPtr()));
-}
diff --git a/src/feature_gps_fix.h b/src/feature_gps_fix.h
index 3a0445afa44352a68de9587d1dbc4d7a709e00e3..9e7228548470e2977f4750d87fee38e788abfc52 100644
--- a/src/feature_gps_fix.h
+++ b/src/feature_gps_fix.h
@@ -27,13 +27,5 @@ class FeatureGPSFix : public FeatureBase
         FeatureGPSFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
 
         virtual ~FeatureGPSFix();
-
-        /** \brief Generic interface to find constraints
-         * 
-         * Generic interface to find constraints between this feature and a map (static/slam) or a previous feature
-         *
-         **/
-        virtual void findConstraints();
-        
 };
 #endif
diff --git a/src/frame_base.h b/src/frame_base.h
index 60d80b9d410ccd50a094eaf4eb7a3861e506e008..c52a5d9e6ad008870859c11b9d0a146cff44d7e8 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -27,7 +27,6 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
         FrameType type_; //type of frame. Either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
         TimeStamp time_stamp_; //frame time stamp
         StateStatus status_; // status of the estimation of the frame state
-        //Eigen::Vector3s state_; //TBD: Instead , It could be a vector/list/map of pointers to state units
 		StateBase* p_ptr_; // Position state unit pointer
 		StateBase* o_ptr_; // Orientation state unit pointer
 		StateBase* v_ptr_; // Velocity state unit pointer
@@ -116,6 +115,8 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
 
         StateBase* getWPtr() const;
 
+        const Eigen::Matrix4s * getTransformationMatrix() const;
+
         virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
         
 };
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 2c89acdb4e4879067ccd2c0fe461033c1d617264..dd9b2076a77f8a8b08863ad2e9010bcf71b2329f 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -116,7 +116,7 @@ void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
 	descriptor_ = _descriptor;
 }
 
-const Eigen::VectorXs LandmarkBase::getDescriptor() const
+const Eigen::VectorXs& LandmarkBase::getDescriptor() const
 {
 	return descriptor_;
 }
diff --git a/src/landmark_base.h b/src/landmark_base.h
index c5723e88e61b130c214231e6b9264249785eddf4..ea4cfe7da9e0ea6eea45fd2bff90a4659dbd31b8 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -85,7 +85,7 @@ class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
 
 		void setDescriptor(const Eigen::VectorXs& _descriptor);
 
-		const Eigen::VectorXs getDescriptor() const;
+		const Eigen::VectorXs& getDescriptor() const;
 
         //const StateBase* getStatePtr() const;
 
diff --git a/src/landmark_corner_2D.cpp b/src/landmark_corner_2D.cpp
index 28d6a2b6249a4dfea0159e6066bbbe6db53dcb88..838d96159f5ff6605ddfac0c86b64e0a1e9480f4 100644
--- a/src/landmark_corner_2D.cpp
+++ b/src/landmark_corner_2D.cpp
@@ -4,8 +4,7 @@
 LandmarkCorner2D::LandmarkCorner2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _aperture) :
 	LandmarkBase(LANDMARK_CORNER, _p_ptr, _o_ptr)
 {
-    if (_aperture!=0)
-    	setDescriptor(Eigen::Vector1s(_aperture));
+  	setDescriptor(Eigen::VectorXs::Constant(1,_aperture));
 }
 
 LandmarkCorner2D::~LandmarkCorner2D()
diff --git a/src/sensor_base.h b/src/sensor_base.h
index c822a405c21c3f9907d0773bccb2760b55c508a5..0bab63e4115f67b010ce5bb78609bd448c474762 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -7,14 +7,19 @@
 //Wolf includes
 #include "wolf.h"
 #include "node_base.h"
+#include "state_point.h"
+#include "state_orientation.h"
 
 class SensorBase : public NodeBase
 {
     protected:
-        SensorType type_;//indicates sensor type. Enum defined at wolf.h
+        SensorType type_;			//indicates sensor type. Enum defined at wolf.h
         Eigen::Vector3s sensor_position_vehicle_;//sensor position in the vehicle frame
         Eigen::Matrix3s sensor_rotation_vehicle_;//sensor rotation in the vehicle frame
-        Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ... 
+        // TODO:
+//		StateBase* p_ptr_;			// sensor position state unit pointer
+//		StateOrientation* o_ptr_; 	// sensor orientation state unit pointer
+        Eigen::VectorXs params_;	//sensor intrinsic params: offsets, scale factors, sizes, ...
         //bool generate_prior_; //flag indicating if this sensor generates the prior or not
     
     public:
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index c8fb24ce7d7596d730159bab756d663f0cbd43f5..1ffc79c37ed3a24986cc11ef8ce32b09f24d9065 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -4,8 +4,8 @@
 #define SENSOR_LASER_2D_H_
 
 //laser_scan_utils
-#include "iri-algorithms/laser_scan_utils/scan_params.h"
-#include "iri-algorithms/laser_scan_utils/corners.h"
+#include "iri-algorithms/laser_scan_utils/scan_basics.h"
+#include "iri-algorithms/laser_scan_utils/corner_detector.h"
 
 //wolf 
 #include "sensor_base.h"
diff --git a/src/state_complex_angle.cpp b/src/state_complex_angle.cpp
index 8583434a7ab33ca46706c2acdda3553c707aa2b0..05a8853c7aa1815dc7304a3344d88311dff7881f 100644
--- a/src/state_complex_angle.cpp
+++ b/src/state_complex_angle.cpp
@@ -2,14 +2,13 @@
 #include "state_complex_angle.h"
 
 StateComplexAngle::StateComplexAngle(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
-	StateBase(_st_remote, _idx)
+		StateOrientation(_st_remote, _idx)
 {
 	//
-	std::cout << "state complex angle created\n";
 }
 
 StateComplexAngle::StateComplexAngle(WolfScalar* _st_ptr) :
-	StateBase(_st_ptr)
+		StateOrientation(_st_ptr)
 {
 	//
 }
@@ -29,10 +28,21 @@ unsigned int StateComplexAngle::getStateSize() const
 	return BLOCK_SIZE;
 }
 
+Eigen::Matrix3s StateComplexAngle::getRotationMatrix() const
+{
+	Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
+	R(0,0) = *state_ptr_;
+	R(1,1) = *state_ptr_;
+	R(0,1) = -*(state_ptr_+1);
+	R(1,0) = *(state_ptr_+1);
+
+	return R;
+}
+
 void StateComplexAngle::print(unsigned int _ntabs, std::ostream& _ost) const
 {
 	printTabs(_ntabs);
 	_ost << nodeLabel() << " " << nodeId() << std::endl;
 	printTabs(_ntabs);
-	_ost << *this->state_ptr_<< " " << *(this->state_ptr_+1) << std::endl;
+	_ost << *state_ptr_<< " " << *(state_ptr_+1) << std::endl;
 }
diff --git a/src/state_complex_angle.h b/src/state_complex_angle.h
index beef095022a45b8091b5acc84dc3ef6577538a7d..d4aab33c5b0b7fae84c26990077cded9367a581f 100644
--- a/src/state_complex_angle.h
+++ b/src/state_complex_angle.h
@@ -9,10 +9,10 @@
 
 //Wolf includes
 #include "wolf.h"
-#include "state_base.h"
+#include "state_orientation.h"
 
 //class StateComplexAngle
-class StateComplexAngle : public StateBase
+class StateComplexAngle : public StateOrientation
 {
     public:
 		static const unsigned int BLOCK_SIZE = 2;
@@ -55,6 +55,13 @@ class StateComplexAngle : public StateBase
 		 **/
 		virtual unsigned int getStateSize() const;
 
+        /** \brief Returns the 3x3 rotation matrix of the orientation
+		 *
+		 * Returns the 3x3 rotation matrix of the orientation
+		 *
+		 **/
+		virtual Eigen::Matrix3s getRotationMatrix() const;
+
         /** \brief Prints all the elements of the state unit
 		 *
 		 * Prints all the elements of the state unit
diff --git a/src/state_orientation.cpp b/src/state_orientation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e48be883c7ec96077515bbb7bb46c886e160a9ba
--- /dev/null
+++ b/src/state_orientation.cpp
@@ -0,0 +1,20 @@
+
+#include "state_orientation.h"
+
+StateOrientation::StateOrientation(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
+	StateBase(_st_remote, _idx)
+{
+	//
+}
+
+
+StateOrientation::StateOrientation(WolfScalar* _st_ptr) :
+	StateBase(_st_ptr)
+{
+	//
+}
+
+StateOrientation::~StateOrientation()
+{
+	//
+}
diff --git a/src/state_orientation.h b/src/state_orientation.h
new file mode 100644
index 0000000000000000000000000000000000000000..df5ee64272dbabd8b71f5d6ccb6a1b4cf1209599
--- /dev/null
+++ b/src/state_orientation.h
@@ -0,0 +1,50 @@
+
+#ifndef STATE_ORIENTATION_H_
+#define STATE_ORIENTATION_H_
+
+//std includes
+#include <iostream>
+#include <vector>
+#include <cmath>
+
+//Wolf includes
+#include "wolf.h"
+#include "state_base.h"
+
+//class StateOrientation
+class StateOrientation : public StateBase
+{
+	public:
+        
+        /** \brief Constructor with whole state storage and index where starts the state unit
+         * 
+         * Constructor with whole state storage and index where starts the state unit
+         * \param _st_remote is the whole state vector
+         * \param _idx is the index of the first element of the state in the whole state vector
+         * 
+         **/
+		StateOrientation(Eigen::VectorXs& _st_remote, const unsigned int _idx);
+
+        /** \brief Constructor with scalar pointer
+         * 
+         * Constructor with scalar pointer
+         * \param _st_ptr is the pointer of the first element of the state unit
+         * 
+         **/
+		StateOrientation(WolfScalar* _st_ptr);
+        
+        /** \brief Destructor
+         * 
+         * Destructor
+         * 
+         **/
+        virtual ~StateOrientation();
+
+        /** \brief Returns the 3x3 rotation matrix of the orientation
+		 *
+		 * Returns the 3x3 rotation matrix of the orientation
+		 *
+		 **/
+		virtual Eigen::Matrix3s getRotationMatrix() const = 0;
+};
+#endif
diff --git a/src/state_point.h b/src/state_point.h
index 0dfdc52280a1b8adb4f2b1fcc37d78da307b6966..7f515cc3f187ca3719b4365abd2a3c116ce46f4f 100644
--- a/src/state_point.h
+++ b/src/state_point.h
@@ -9,6 +9,7 @@
 
 //Wolf includes
 #include "wolf.h"
+#include "state_base.h"
 
 //class StateBase
 template <unsigned int SIZE>
@@ -80,6 +81,16 @@ class StatePoint : public StateBase
 			return BLOCK_SIZE;
 		}
 
+		/** \brief Returns the point in a vector
+		 *
+		 * Returns the point in a vector
+		 *
+		 **/
+		const Eigen::Matrix<WolfScalar, BLOCK_SIZE, 1>& getVector() const
+		{
+			return Eigen::Map<const Eigen::Matrix<WolfScalar, BLOCK_SIZE, 1>>(state_ptr_);
+		}
+
         /** \brief Prints all the elements of the state unit
 		 *
 		 * Prints all the elements of the state unit
diff --git a/src/state_theta.cpp b/src/state_theta.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b1e707b3e54f26d2b74a1c1bc756d20f3b411fc3
--- /dev/null
+++ b/src/state_theta.cpp
@@ -0,0 +1,50 @@
+
+#include "state_theta.h"
+
+StateTheta::StateTheta(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
+		StateOrientation(_st_remote, _idx)
+{
+	//
+}
+
+StateTheta::StateTheta(WolfScalar* _st_ptr) :
+		StateOrientation(_st_ptr)
+{
+	//
+}
+
+StateTheta::~StateTheta()
+{
+	//
+}
+
+StateType StateTheta::getStateType() const
+{
+	return ST_THETA;
+}
+
+unsigned int StateTheta::getStateSize() const
+{
+	return BLOCK_SIZE;
+}
+
+Eigen::Matrix3s StateTheta::getRotationMatrix() const
+{
+	Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
+	R(0,0) = cos(*state_ptr_);
+	R(1,1) = cos(*state_ptr_);
+	R(0,1) = -sin(*state_ptr_);
+	R(1,0) = sin(*state_ptr_);
+
+	return R;
+}
+
+void StateTheta::print(unsigned int _ntabs, std::ostream& _ost) const
+{
+	printTabs(_ntabs);
+	_ost << nodeLabel() << " " << nodeId() << std::endl;
+	printTabs(_ntabs);
+	for (uint i = 0; i < BLOCK_SIZE; i++)
+		_ost << *(this->state_ptr_+i) << " ";
+	_ost << std::endl;
+}
diff --git a/src/state_theta.h b/src/state_theta.h
new file mode 100644
index 0000000000000000000000000000000000000000..6c76502f2aec635eda799f4473662949e128eaee
--- /dev/null
+++ b/src/state_theta.h
@@ -0,0 +1,72 @@
+
+#ifndef STATE_THETA_H_
+#define STATE_THETA_H_
+
+//std includes
+#include <iostream>
+#include <vector>
+#include <cmath>
+
+//Wolf includes
+#include "wolf.h"
+#include "state_orientation.h"
+
+//class StateTheta
+class StateTheta : public StateOrientation
+{
+	public:
+		static const unsigned int BLOCK_SIZE = 1;
+        
+        /** \brief Constructor with whole state storage and index where starts the state unit
+         * 
+         * Constructor with whole state storage and index where starts the state unit
+         * \param _st_remote is the whole state vector
+         * \param _idx is the index of the first element of the state in the whole state vector
+         * 
+         **/
+		StateTheta(Eigen::VectorXs& _st_remote, const unsigned int _idx);
+
+        /** \brief Constructor with scalar pointer
+         * 
+         * Constructor with scalar pointer
+         * \param _st_ptr is the pointer of the first element of the state unit
+         * 
+         **/
+		StateTheta(WolfScalar* _st_ptr);
+        
+        /** \brief Destructor
+         * 
+         * Destructor
+         * 
+         **/
+        virtual ~StateTheta();
+        
+        /** \brief Returns the parametrization of the state unit
+		 *
+		 * Returns the parametrizationType (see wolf.h) of the state unit
+		 *
+		 **/
+		virtual StateType getStateType() const;
+
+		/** \brief Returns the state unit size
+		 *
+		 * Returns the parametrizationType (see wolf.h) of the state unit
+		 *
+		 **/
+		virtual unsigned int getStateSize() const;
+
+        /** \brief Returns the 3x3 rotation matrix of the orientation
+		 *
+		 * Returns the 3x3 rotation matrix of the orientation
+		 *
+		 **/
+		virtual Eigen::Matrix3s getRotationMatrix() const;
+
+        /** \brief Prints all the elements of the state unit
+		 *
+		 * Prints all the elements of the state unit
+		 *
+		 **/
+		virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
+};
+#endif
diff --git a/src/wolf.h b/src/wolf.h
index aee57e5d64369b8463533d66cfcef8b78f1eba1c..bcef332a035a3ecd2d965dc44cba6c08ff21ef51 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -126,12 +126,12 @@ typedef enum
  */
 typedef enum
 {
-	ST_POINT_1D=1,		///< A 1D point. No parametrization.
-	ST_POINT_2D=2,		///< A 2D point. No parametrization.
-	ST_POINT_3D=3,		///< A 3D point. No parametrization.
-	ST_THETA=1,			///< A 2D orientation represented by a single angle. No parametrization (equivalent to ST_POINT_1D).
-	ST_COMPLEX_ANGLE=4,	///< A 2D orientation represented by a complex number.
-	ST_QUATERNION=5		///< A 3D orientation represented by a quaternion.
+	ST_POINT_1D,		///< A 1D point. No parametrization.
+	ST_POINT_2D,		///< A 2D point. No parametrization.
+	ST_POINT_3D,		///< A 3D point. No parametrization.
+	ST_THETA,			///< A 2D orientation represented by a single angle. No parametrization.
+	ST_COMPLEX_ANGLE,	///< A 2D orientation represented by a complex number.
+	ST_QUATERNION		///< A 3D orientation represented by a quaternion.
 } StateType;
 
 /** \brief Enumeration of all possible state status
@@ -298,7 +298,6 @@ typedef TransSensorMap::iterator TransSensorIter;
 typedef std::list<StateBase*> StateBaseList;
 typedef StateBaseList::iterator StateBaseIter;
 
-typedef StatePoint<1> StateTheta;
 typedef StatePoint<1> StatePoint1D;
 typedef StatePoint<2> StatePoint2D;
 typedef StatePoint<3> StatePoint3D;