diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 042c1aa26036e3e5baf95c2081485447b356bb41..68673d2c79f40e69b19352c00788c457873f8e71 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -35,12 +35,15 @@ void CaptureLaser2D::processCapture()
     std::list<Eigen::Vector4s> corners;
     
     //extract corners from range data
+    //std::cout << "CaptureLaser2D::extractCorners..." << std::endl;
     extractCorners(corners);
     
     //generate a feature for each corner
+    //std::cout << "CaptureLaser2D::createFeatures..." << std::endl;
     createFeatures(corners);
     
     //Establish constraints for each feature
+    //std::cout << "CaptureLaser2D::establishConstraints..." << std::endl;
     establishConstraints();
 }
 
@@ -49,281 +52,6 @@ unsigned int CaptureLaser2D::extractCorners(std::list<Eigen::Vector4s> & _corner
     return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _corner_list);
 }
 
-/*
-unsigned int CaptureLaser2D::extractCorners_old(std::list<Eigen::Vector4s> & _corner_list) const
-{
-    //local variables
-    Eigen::MatrixXs points(3,data_.size());
-    double azimuth, cos_theta, theta;
-    Eigen::Vector3s corner;
-    Line line;
-    std::list<Line> line_list;
-    std::list<Line>::iterator line_it1, line_it2;
-    std::queue<unsigned int> jumps;
-
-    
-    //cast to specialized sensor. TODO: Could be done once at the constructor ?
-    SensorLaser2DPtr laser_ptr = (const SensorLaser2DPtr)sensor_ptr_;
-    
-    //init from sensor this->sensor_ptr_
-    //double aperture = laser_ptr->getAngleMax() - laser_ptr->getAngleMin();
-    double azimuth_step = laser_ptr->getAngleIncrement();
-    double range_std_dev = laser_ptr->getRangeStdDev();
-
-    std::default_random_engine generator(1);
-    std::uniform_int_distribution<int> rand_window_overlapping(1,segment_window_size);
-    
-    //convert range polar data to cartesian points.
-    for (unsigned int ii = 0; ii<ranges_.size(); ii++)
-	{
-    	azimuth = laser_ptr->getAngleMax() - azimuth_step * ii;
-    	//std::cout << "  azimuth: " << azimuth << std::endl;
-		points.col(ii) << ranges_(ii)*cos(azimuth), ranges_(ii)*sin(azimuth), 1; //points.row0-> x coordinate, points.row1->y coordinate
-
-		if (ii > 0 && fabs(ranges_(ii)-ranges_(ii-1)) > max_distance)
-		{
-			// store jumps
-			jumps.push(ii);
-			// consider jumps as a corners
-//			if (ranges_(ii) < ranges_(ii-1))
-//				_corner_list.push_back(Eigen::Vector4s(points(0,ii),points(1,ii),0,0)); // TODO: compute orientation
-//			else
-//				_corner_list.push_back(Eigen::Vector4s(points(0,ii-1),points(1,ii-1),0,0));// TODO: compute orientation
-		}
-	}
-
-    //find line segments running over the scan
-    for (unsigned int ii = segment_window_size-1; ii<ranges_.size(); ii=ii+rand_window_overlapping(generator) )
-    {
-    	unsigned int i_from = ii - segment_window_size + 1;
-
-    	// update the jump to be checked
-    	while (!jumps.empty() && i_from > jumps.front())
-    		jumps.pop();
-
-    	// check if there is a jump inside the window (not fitting if it is the case)
-    	if (jumps.front() > i_from && jumps.front() <= ii)
-    		continue;
-
-		//Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
-		fitLine(i_from, ii, points, line);
-
-		//if error below stdev, add line to result set
-		if ( line.error < range_std_dev*k_sigmas )
-			line_list.push_back(line);
-    }
-
-    //std::cout << "Lines fitted: " << line_list.size() << std::endl;
-
-    // concatenating and corners only if more than 1 line
-    if ( line_list.size() > 1 )
-	{
-    	// concatenate lines
-    	line_it1 = line_list.begin();
-		line_it2 = line_it1;
-		line_it2 ++;
-		while ( line_it1 != line_list.end() )
-		{
-			// last of current line and first of next line too far
-			if (line_it2->first > line_it1->last + max_beam_distance)
-			{
-				//std::cout << "lines too far:\nlast of current: " << line_it1->last << " first of next: " << line_it2->first << std::endl;
-				line_it1 ++;
-				line_it2 = line_it1;
-				line_it2 ++;
-			}
-			else
-			{
-				//compute angle between lines 1 and 2
-				cos_theta = (line_it1->vector).dot(line_it2->vector) / ( (line_it1->vector).norm()*(line_it2->vector).norm() );
-				theta = acos (cos_theta);
-
-				//TODO: fabs? acos returns [0 PI]
-				if (theta > M_PI/2)
-					theta -= M_PI;
-	//            std::cout << std::endl << "cos_theta: " << cos_theta << std::endl <<
-	//                                      "theta: " << theta << std::endl <<
-	//                                      "*index_it1: " << *index_it1 << std::endl <<
-	//                                      "*index_it2: " << *index_it2 << std::endl;
-	//                                       "   (*line_it1).dot(*line_it2): " << (*line_it1).dot(*line_it2) << std::endl <<
-	//                                       "   (*line_it1).norm()*(*line_it2).norm(): " << (*line_it1).norm()*(*line_it2).norm() << std::endl;
-
-
-				//Check angle threshold and consecutiveness of lines in the scan
-				if ( fabs(theta) < theta_max_parallel )
-				{
-					Line new_line;
-					fitLine(line_it1->first, line_it2->last, points, new_line);
-					if ( new_line.error < range_std_dev*k_sigmas )
-					{
-						*line_it1 = new_line;
-						line_it2 = line_list.erase(line_it2);
-
-//						std::cout << "lines concatenated" << std::endl;
-//						std::cout << "line 1: " << std::endl << line_it1->first << std::endl <<
-//							 line_it1->last << std::endl <<
-//							 line_it1->vector.transpose() << std::endl <<
-//							 line_it1->error << std::endl;
-//						std::cout << "line 2: " << std::endl << line_it2->first << std::endl <<
-//							 line_it2->last << std::endl <<
-//							 line_it2->vector.transpose() << std::endl <<
-//							 line_it2->error << std::endl;
-//						std::cout << "line resultant: "<< std::endl << new_line.first << std::endl <<
-//							new_line.last << std::endl <<
-//							new_line.vector.transpose() << std::endl <<
-//							new_line.error << std::endl;
-					}
-					else
-					{
-						//update iterators
-						line_it1 ++;
-						line_it2 = line_it1;
-						line_it2 ++;
-					}
-				}
-				else
-				{
-					//update iterators
-					line_it1 ++;
-					line_it2 = line_it1;
-					line_it2 ++;
-				}
-			}
-		}
-
-		//std::cout << "Lines after concatenation: " << line_list.size() << std::endl;
-
-		// find corners over the line list
-        line_it1 = line_list.begin();
-        line_it2 = line_it1;
-        line_it2 ++;
-        while ( line_it1 != line_list.end() )
-        {   
-        	// last of current line and first of next line too far
-        	if (line_it2->first > line_it1->last + max_beam_distance)
-			{
-        		//std::cout << "lines too far:\nlast of current: " << line_it1->last << " first of next: " << line_it2->first << std::endl;
-        		line_it1 ++;
-                line_it2 = line_it1;
-        		line_it2 ++;
-			}
-        	else
-        	{
-				//compute angle between lines 1 and 2
-				cos_theta = (line_it1->vector).dot(line_it2->vector) / ( (line_it1->vector).norm()*(line_it2->vector).norm() );
-				theta = acos (cos_theta);
-
-				//TODO: fabs? acos returns [0 PI]
-				if (theta > M_PI/2)
-					theta -= M_PI;
-
-//	            std::cout << std::endl << "cos_theta: " << cos_theta << std::endl <<
-//	                                      "theta: " << theta << std::endl <<
-//	                                      "line 1: " << line_it1->first << "-" << line_it1->last << std::endl <<
-//	                                      "line 2: " << line_it2->first << "-" << line_it2->last << std::endl <<
-//	                                      "   (*line_it1).dot(*line_it2): " << (line_it1->vector).dot(line_it2->vector) << std::endl <<
-//	                                      "   (*line_it1).norm()*(*line_it2).norm(): " << (line_it1->vector).norm()*(line_it2->vector).norm() << std::endl;
-
-
-				//Check angle threshold and consecutiveness of lines in the scan
-				if ( fabs(theta) > theta_min ) //TODO: fabs? acos returns [0 PI]
-				{
-					corner = (line_it1->vector).cross(line_it2->vector); //cross product between lines is the intersection
-					corner = corner / corner(2); //normalize point to set last component to 1
-
-					// Check if the corner is close to both lines ends
-					if ( (points.col(line_it1->last) - corner).head(2).norm() < max_distance && (points.col(line_it2->first) - corner).head(2).norm() < max_distance)
-					{
-						Eigen::Vector3s v1 = (points.col(line_it1->first) - corner);
-						Eigen::Vector3s v2 = (points.col(line_it2->last) - corner);
-						v1 /= v1.norm();
-						v2 /= v2.norm();
-
-						//corner orientation
-						if (v1(0) * v2(1) > v1(1) * v2(0))
-							corner(2) = asin (v1(1));
-						else
-							corner(2) = asin (v2(1));
-
-//						std::cout << "Detected corner!" << std::endl <<
-//									 "\tcorner: " << corner.transpose() << std::endl <<
-//									 "\tline 1 point: " << points.col(line_it1->first).transpose() << std::endl <<
-//									 "\tline 2 point: " << points.col(line_it2->last).transpose() << std::endl <<
-//									 "\tv1: " << v1.transpose() << std::endl <<
-//									 "\tv2: " << v2.transpose() << std::endl;
-
-						_corner_list.push_back(Eigen::Vector4s(corner(0),corner(1),corner(2),theta));
-					}
-				}
-
-				//update iterators
-				if (line_it2 != line_list.end() )
-					line_it2 ++;
-				else
-				{
-					line_it1 ++;
-					line_it2 = line_it1;
-					line_it2 ++;
-				}
-        	}
-        }
-    }
-
-    //std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
-
-    // Erase duplicated corners
-	if ( _corner_list.size() > 1 )
-	{
-		// concatenate lines
-		std::list<Eigen::Vector4s>::iterator corner_it1 = _corner_list.begin();
-		std::list<Eigen::Vector4s>::iterator corner_it2 = corner_it1;
-		corner_it2 ++;
-		while ( corner_it1 != _corner_list.end() )
-		{
-			// Check if two corners are close enough
-			if ( (*corner_it1 - *corner_it2).head(2).norm() < max_distance )
-			{
-				// TODO: keep the one with larger lines
-				// compute the mean
-				*corner_it1 = (*corner_it1 + *corner_it2) / 2;
-				corner_it2 = _corner_list.erase(corner_it2);
-			}
-			else
-			{
-				corner_it1 ++;
-				corner_it2 = corner_it1;
-				corner_it2 ++;
-			}
-		}
-	}
-
-	//std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
-
-    return _corner_list.size();
-}
-
-void CaptureLaser2D::fitLine(unsigned int _idx_from, unsigned int _idx_to, const Eigen::MatrixXs& _points, Line& line_) const
-{
-	line_.first = _idx_from;
-	line_.last = _idx_to;
-
-	//Found the best fitting line over points within the window. Build the system: A*line=[0 0 1]'. Matrix A = a_ij
-	Eigen::Matrix3s AA = _points.block(0, _idx_from, 3, _idx_to-_idx_from+1) * _points.block(0, _idx_from, 3, _idx_to-_idx_from+1).transpose();
-	AA.row(2) << 0,0,1;
-
-	//solve for line
-	line_.vector = AA.inverse().col(2);
-
-	// compute fitting error
-//	line_.error = 0;
-//    for(unsigned int jj = _idx_from; jj<=_idx_to; jj++)
-//    	line_.error += fabs( line_.vector(0)*_points(0,jj) + line_.vector(1)*_points(1,jj) + line_.vector(2)) / sqrt( line_.vector(0)*line_.vector(0) + line_.vector(1)*line_.vector(1) );
-//    line_.error /= (_idx_to-_idx_from+1);
-
-	line_.error = (_points.block(0, _idx_from, 3, _idx_to-_idx_from+1).transpose() * line_.vector).array().abs().sum()/(line_.vector.head(2).norm()*(_idx_to-_idx_from+1));
-}
-*/
-
 void CaptureLaser2D::createFeatures(std::list<Eigen::Vector4s> & _corner_list)
 {
     Eigen::Matrix4s cov_mat;
@@ -336,23 +64,30 @@ 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 ) ) );
+    }
 }
 
 void CaptureLaser2D::establishConstraints()
 {
 	// Global transformation TODO: Change by a function
-	Eigen::Vector2s t(*getFramePtr()->getPPtr()->getPtr(),*(getFramePtr()->getPPtr()->getPtr()+1));
+	Eigen::Vector2s t_robot(*getFramePtr()->getPPtr()->getPtr(),*(getFramePtr()->getPPtr()->getPtr()+1));
 	WolfScalar o = *(getFramePtr()->getOPtr()->getPtr());
-	Eigen::Matrix2s R;
+	Eigen::Matrix2s R_robot;
 	if (getFramePtr()->getOPtr()->getStateType() == ST_THETA)
-		R << cos(o),-sin(o),
+		R_robot << cos(o),-sin(o),
 			 sin(o), cos(o);
     else
     {
     	//TODO
     }
 
+	// Sensor transformation
+	Eigen::Vector2s t_sensor = getSensorPtr()->getSensorPosition()->head(2);
+	Eigen::Matrix2s R_sensor = getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>().transpose();
+
     //Brute force closest (xy and theta) landmark search
 //    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;
@@ -361,17 +96,18 @@ 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.1; //TODO: max_distance_matching depending on localization and landmarks uncertainty
-		double max_theta_matching = 0.02; //TODO: max_theta_matching depending on localization and landmarks uncertainty
+		double max_distance_matching2 = 0.5; //TODO: max_distance_matching depending on localization and landmarks uncertainty
+		double max_theta_matching = 0.2; //TODO: max_theta_matching depending on localization and landmarks uncertainty
 
 		//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);
 
-    	Eigen::Vector2s feature_global_position = R*feature_position + t;
+    	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;
+    	feature_global_orientation(0) = feature_orientation(0) + o + atan2(R_sensor(1,0),R_sensor(0,0));
+    	//feature_global_orientation(0) = 0;
     	double min_distance2 = max_distance_matching2;
 
 //    	std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl;
@@ -383,13 +119,28 @@ void CaptureLaser2D::establishConstraints()
     		WolfScalar landmark_orientation = *((*landmark_it)->getOPtr()->getPtr());
 
     		WolfScalar distance2 = (landmark_position-feature_global_position).transpose() * (landmark_position-feature_global_position);
-			if (distance2 < min_distance2 && fabs(landmark_orientation-feature_global_orientation(0) < max_theta_matching))
+    		WolfScalar theta_distance = fabs(landmark_orientation-feature_global_orientation(0));
+
+    		if (theta_distance > M_PI)
+    			theta_distance -= 2 * M_PI;
+			if (distance2 < min_distance2)
 			{
-//				std::cout << "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;
+				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;
+
+					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;
+				}
 			}
 		}
     	if (correspondent_landmark == nullptr)
@@ -404,8 +155,8 @@ void CaptureLaser2D::establishConstraints()
     		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: " << *new_landmark->getPPtr()->getPtr() << " " << *(new_landmark->getPPtr()->getPtr()+1) << " orientation:" << *new_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() << std::endl;
     	}
     	else
     		correspondent_landmark->hit();
@@ -413,12 +164,6 @@ void CaptureLaser2D::establishConstraints()
     	//std::cout << "Creating new constraint: Landmark " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << " & feature " << (*feature_it)->nodeId() << std::endl;
 
     	// Add constraint to the correspondent landmark
-//    	ConstraintBase* landmark_constraint(new ConstraintCorner2DTheta(&(*feature_it),
-//    																		correspondent_landmark,
-//																			getFramePtr()->getPPtr(),//_robotPPtr,
-//																			getFramePtr()->getOPtr(),//_robotOPtr,
-//																			correspondent_landmark->getPPtr(), //_landmarkPPtr,
-//																			correspondent_landmark->getOPtr())); //_landmarkOPtr,
     	(*feature_it)->addConstraint(new ConstraintCorner2DTheta(*feature_it,
 																 correspondent_landmark,
 																 getFramePtr()->getPPtr(),//_robotPPtr,
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index 0626cd2a5a4433f736ee85ff395686bdebaddb1b..b4736054c366c367a1720e663b3273f53c2b8b8f 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -25,9 +25,11 @@ CaptureOdom2D::~CaptureOdom2D()
 
 inline void CaptureOdom2D::processCapture()
 {
+    //std::cout << "CaptureOdom2D::addFeature..." << std::endl;
 	// ADD FEATURE
     addFeature(new FeatureOdom2D(data_,data_covariance_));
 
+    //std::cout << "CaptureOdom2D::addConstraints..." << std::endl;
     // ADD CONSTRAINT
     addConstraints();
 }
diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h
index 1ab409b7689ff3dc462b2d44ff37f099e4aa0ca5..60d4a5fbe9f3fb0a16f174884e126464a135628c 100644
--- a/src/constraint_corner_2D_theta.h
+++ b/src/constraint_corner_2D_theta.h
@@ -1,4 +1,3 @@
-
 #ifndef CONSTRAINT_CORNER_2D_THETA_H_
 #define CONSTRAINT_CORNER_2D_THETA_H_
 
@@ -46,23 +45,39 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
 		template <typename T>
 		bool operator()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP, const T* const _landmarkO, T* _residuals) const
 		{
-			//TODO: Not compute every step InvRot
+			//TODO: Not computing the transformations each iteration
 			// Mapping
 			Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position(_landmarkP);
 			Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position(_robotP);
 
-			Eigen::Matrix<T,2,2> InvRot;
-			InvRot << cos(*_robotO), sin(*_robotO),
-					 -sin(*_robotO), cos(*_robotO);
+//			std::cout << "getSensorPosition: " << std::endl;
+//			std::cout << getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).transpose() << std::endl;
+//			std::cout << "getSensorRotation: " << std::endl;
+//			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>();
+
+			Eigen::Matrix<T,2,2> inverse_R_robot;
+			inverse_R_robot << cos(*_robotO), sin(*_robotO),
+					 	 	  -sin(*_robotO), cos(*_robotO);
 
 			// Expected measurement
-			Eigen::Matrix<T,2,1> expected_landmark_relative_position = InvRot * (landmark_position - robot_position);
-			T expected_landmark_relative_orientation = (*_landmarkO) - (*_robotO);
+			Eigen::Matrix<T,2,1> expected_landmark_relative_position = inverse_R_sensor * (inverse_R_robot * (landmark_position - robot_position) - sensor_position);
+			T expected_landmark_relative_orientation = (*_landmarkO) - (*_robotO) - atan2(inverse_R_sensor(0,1),inverse_R_sensor(0,0));
+
+			while (expected_landmark_relative_orientation > T(M_PI))
+				expected_landmark_relative_orientation = expected_landmark_relative_orientation - T(2*M_PI);
+			while (expected_landmark_relative_orientation <= T(-M_PI))
+				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((*measurement_covariance_ptr_)(2,2));
+			_residuals[2] = (expected_landmark_relative_orientation - T((*measurement_ptr_)(2))) / T(100*(*measurement_covariance_ptr_)(2,2));
 
 //			std::cout << "\nCONSTRAINT: " << nodeId() << std::endl;
 //			std::cout << "Feature: " << getFeaturePtr()->nodeId() << std::endl;
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 3032761c4f03d3683490047b446a0ea7cecd5051..1d9713765430ad1a450822f4d8ae13f272b46e32 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -51,7 +51,12 @@ IF(faramotics_FOUND)
     IF (laser_scan_utils_FOUND)
         ADD_EXECUTABLE(test_ceres_laser test_ceres_laser.cpp)
         TARGET_LINK_LIBRARIES(test_ceres_laser 
-#                                 ${GLUT_glut_LIBRARY} 
+                                ${pose_state_time_LIBRARIES} 
+                                ${faramotics_LIBRARIES} 
+                                ${PROJECT_NAME})
+                                
+        ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
+        TARGET_LINK_LIBRARIES(test_ceres_2_lasers 
                                 ${pose_state_time_LIBRARIES} 
                                 ${faramotics_LIBRARIES} 
                                 ${PROJECT_NAME})
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..165566c201af24871dbcbb4d80ea81ace5c3b4c8
--- /dev/null
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -0,0 +1,622 @@
+//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"
+
+//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_;
+		WolfProblem* 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 WolfProblem(_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
+        	StateBase* new_position = new StatePoint2D(problem_->getNewStatePtr());
+			problem_->addState(new_position, _frame_state.head(2));
+
+			StateBase* new_orientation;
+        	if (use_complex_angles_)
+        		new_orientation = new StateComplexAngle(problem_->getNewStatePtr());
+        	else
+        		new_orientation = new StateTheta(problem_->getNewStatePtr());
+
+			problem_->addState(new_orientation, _frame_state.tail(new_orientation->getStateSize()));
+
+			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
+					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();
+
+        			// 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)
+        			{
+						//std::cout << "store prev frame" << std::endl;
+        				auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
+
+        				// NEW FRAME
+        				//std::cout << "new frame" << std::endl;
+						createFrame(prior, new_capture->getTimeStamp());
+
+        				// COMPUTE PREVIOUS FRAME CAPTURES
+						//std::cout << "compute prev frame" << std::endl;
+						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)
+						//std::cout << "frame window" << std::endl;
+						if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_)
+						{
+							//std::cout << "frame fixing" << std::endl;
+							//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();
+        }
+
+        StateBaseList* getStateListPtr()
+		{
+        	return problem_->getStateListPtr();
+		}
+
+        Eigen::VectorXs getVehiclePose()
+		{
+        	return Eigen::Map<Eigen::VectorXs>(problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr()->getPtr(), use_complex_angles_ ? 4 : 3);
+		}
+
+        void getConstraintsList(ConstraintBaseList& corr_list)
+        {
+        	problem_->getTrajectoryPtr()->getConstraintList(corr_list);
+        }
+
+        WolfProblem* 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.0, 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, laser1Pose, laser2Pose;
+	vector<Cpose3d> devicePoses;
+	vector<float> scan1, scan2;
+	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);
+	viewPoint.setPose(devicePose);
+	viewPoint.moveForward(10);
+	viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/2, viewPoint.rt.pitch()+30.*M_PI/180., viewPoint.rt.roll() );
+	viewPoint.moveForward(-15);
+	//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::Vector6s laser_1_pose, laser_2_pose;
+	laser_1_pose << 1.2,0,0,0,0,0; //laser 1
+	laser_2_pose << -1.2,0,0,0,0,M_PI; //laser 2
+	SensorLaser2D laser_1_sensor(laser_1_pose);
+	SensorLaser2D laser_2_sensor(laser_2_pose);
+	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 (uint 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 scans
+		scan1.clear();
+		scan2.clear();
+		// scan 1
+		laser1Pose.setPose(devicePose);
+		laser1Pose.moveForward(laser_1_pose(0));
+		myScanner->computeScan(laser1Pose,scan1);
+		vector<double> scan1d(scan1.begin(), scan1.end());
+		Eigen::Map<Eigen::VectorXs> scan1_reading(scan1d.data(), 720);
+		// scan 2
+		laser2Pose.setPose(devicePose);
+		laser2Pose.moveForward(laser_2_pose(0));
+		laser2Pose.rt.setEuler( laser2Pose.rt.head()+M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll() );
+		myScanner->computeScan(laser2Pose,scan2);
+		vector<double> scan2d(scan2.begin(), scan2.end());
+		Eigen::Map<Eigen::VectorXs> scan2_reading(scan2d.data(), 720);
+
+		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_1_sensor, scan1_reading));
+		wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_2_sensor, scan2_reading));
+
+        // 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<Eigen::Vector4s> corner_list;
+		std::vector<double> corner_vector;
+		CaptureLaser2D last_scan(time_stamp, &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++ )
+		{
+			corner_vector.push_back(corner_it->x());
+			corner_vector.push_back(corner_it->y());
+		}
+		myRender->drawCorners(laser1Pose,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);
+
+		// draw localization and sensors
+		Cpose3d estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
+		estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0),wolf_manager->getVehiclePose()(1),0.2,wolf_manager->getVehiclePose()(2),0,0);
+		estimated_laser_1_pose.setPose(estimated_vehicle_pose);
+		estimated_laser_1_pose.moveForward(laser_1_pose(0));
+		estimated_laser_2_pose.setPose(estimated_vehicle_pose);
+		estimated_laser_2_pose.moveForward(laser_2_pose(0));
+		estimated_laser_2_pose.rt.setEuler( estimated_laser_2_pose.rt.head()+M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll() );
+		std::vector<Cpose3d> estimated_poses_vector{estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose};
+		myRender->drawPoseAxisVector(estimated_poses_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(laser1Pose,scan1,180.*M_PI/180.,90.*M_PI/180.); //draw scan
+		myRender->render();
+		mean_times(5) += ((double)clock()-t1)/CLOCKS_PER_SEC;
+
+		// TIME MANAGEMENT ---------------------------
+		double dt = ((double)clock()-t2)/CLOCKS_PER_SEC;
+		mean_times(6) += dt;
+
+		if (dt < 0.1)
+			usleep(100000-1e6*dt);
+
+//		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
index 7ed716701b3efef822494be5d9940c21b5971588..64e1a2f5f96dfdafbd16e8da825d9a8e6775fb72 100644
--- a/src/examples/test_ceres_manager.cpp
+++ b/src/examples/test_ceres_manager.cpp
@@ -709,8 +709,8 @@ int main(int argc, char** argv)
 	Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
 	std::queue<StateBase*> 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(3,1),0);
-	SensorBase* gps_sensor = new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1),0);
+	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;
diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp
index ebaba3b404d1452ae5ba2a929f7bc77b756c52d8..c475bc9dd348216332d651c0b55670da8ecd4d1e 100644
--- a/src/examples/test_ceres_manager_tree.cpp
+++ b/src/examples/test_ceres_manager_tree.cpp
@@ -120,22 +120,23 @@ class WolfManager
         		// TODO: accumulate odometries
         		if (new_capture->getSensorPtr() == sensor_prior_)
         		{
-					createFrame(VectorXs::Zero(use_complex_angles_ ? 4 : 3), new_capture->getTimeStamp());
-
-					// ADD CAPTURE TO THE NEW FRAME
+        			// ADD CAPTURE TO THE PREVIOUS FRAME
 					trajectory_->getFrameListPtr()->back()->addCapture(new_capture);
 
 					// COMPUTE PRIOR
-        			trajectory_->getFrameListPtr()->back()->setState(new_capture->computePrior());
+					trajectory_->getFrameListPtr()->back()->setState(new_capture->computePrior());
+
+					// CREATE NEW FRAME
+					createFrame(VectorXs::Zero(use_complex_angles_ ? 4 : 3), new_capture->getTimeStamp());
 
-					// TODO: Change by something like...
+					// 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 NEW FRAME
+        			// ADD CAPTURE TO THE LAST FRAME
 					trajectory_->getFrameListPtr()->back()->addCapture(new_capture);
         		}
 
@@ -253,8 +254,8 @@ int main(int argc, char** argv)
 	std::list<ConstraintBase*> new_constraints; // new constraints in wolf that must be added to ceres
 
 	// Wolf manager initialization
-	SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(3,1), odom_std, odom_std);
-	SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(3,1), gps_std);
+	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
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index cf687801feed8411010134cf19153836e3f8e81d..7c2bafe8e2bcfc494bf65706caf543723c29d30f 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -1,21 +1,26 @@
 #include "sensor_base.h"
 
-SensorBase::SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, const Eigen::VectorXs & _params) :
+SensorBase::SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, const Eigen::VectorXs & _params) :
 	NodeBase("SENSOR"),
     type_(_tp), 
-	sensor_pose_vehicle_(_pose), 
+	sensor_position_vehicle_(_pose.head(3)),
 	params_(_params.size())
 {
     params_ = _params;
+	sensor_rotation_vehicle_ = Eigen::AngleAxisd(_pose(3), Eigen::Vector3d::UnitX()) *
+							   Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) *
+							   Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ());
 }
 
-SensorBase::SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, unsigned int _params_size) : 
+SensorBase::SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, unsigned int _params_size) :
 	NodeBase("SENSOR"),
     type_(_tp), 
-    sensor_pose_vehicle_(_pose), 
+	sensor_position_vehicle_(_pose.head(3)),
     params_(_params_size)
 {
-    //
+	sensor_rotation_vehicle_ = Eigen::AngleAxisd(_pose(3), Eigen::Vector3d::UnitX()) *
+							   Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) *
+							   Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ());
 }
 
 SensorBase::~SensorBase()
@@ -28,8 +33,16 @@ const SensorType SensorBase::getSensorType() const
     return type_;
 }
 
-const Eigen::VectorXs * SensorBase::getSensorPose() const
+const Eigen::Vector3s * SensorBase::getSensorPosition() const
+{
+	//std::cout << "getSensorPosition: " << sensor_position_vehicle_.transpose() << std::endl;
+    return & sensor_position_vehicle_;
+}
+
+const Eigen::Matrix3s * SensorBase::getSensorRotation() const
 {   
-    return & sensor_pose_vehicle_;
+	//std::cout << "getSensorRotation: " << sensor_rotation_vehicle_ << std::endl;
+    return & sensor_rotation_vehicle_;
 }
 
+
diff --git a/src/sensor_base.h b/src/sensor_base.h
index a0dcb2d16cfc4b9d4a1727425217969cf427ffba..c822a405c21c3f9907d0773bccb2760b55c508a5 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -1,7 +1,6 @@
 #ifndef SENSOR_BASE_H_
 #define SENSOR_BASE_H_
 
-
 //std includes
 #include <iostream>
 
@@ -13,20 +12,23 @@ class SensorBase : public NodeBase
 {
     protected:
         SensorType type_;//indicates sensor type. Enum defined at wolf.h
-        Eigen::VectorXs sensor_pose_vehicle_;//sensor pose in the vehicle frame
+        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, ... 
         //bool generate_prior_; //flag indicating if this sensor generates the prior or not
     
     public:
-        SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, const Eigen::VectorXs & _params);
+        SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, const Eigen::VectorXs & _params);
         
-        SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, unsigned int _params_size);
+        SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, unsigned int _params_size);
 
         ~SensorBase();
         
         const SensorType getSensorType() const;
         
-        const Eigen::VectorXs * getSensorPose() const;
+        const Eigen::Vector3s * getSensorPosition() const;
+
+        const Eigen::Matrix3s * getSensorRotation() const;
         
 };
 #endif
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
index f12a2cd2933c3e183f2d0d8d688fa7e59abf4867..f9a52cd490e55ad95d74767b06c891aa028ef003 100644
--- a/src/sensor_gps_fix.cpp
+++ b/src/sensor_gps_fix.cpp
@@ -1,6 +1,6 @@
 #include "sensor_gps_fix.h"
 
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _sp, const double& _noise) :
+SensorGPSFix::SensorGPSFix(const Eigen::Vector6s & _sp, const double& _noise) :
         SensorBase(GPS_FIX, _sp, Eigen::VectorXs::Constant(1,_noise))
 {
     //
diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h
index 65a3ae03846d0340d0c8407179346b1153db6cca..d7fe447249c6d52554073703a3c30ddf50dc0e87 100644
--- a/src/sensor_gps_fix.h
+++ b/src/sensor_gps_fix.h
@@ -15,7 +15,7 @@ class SensorGPSFix : public SensorBase
          * \param _noise noise standard deviation
          * 
          **/
-		SensorGPSFix(const Eigen::VectorXs & _sp, const double& _noise);
+		SensorGPSFix(const Eigen::Vector6s & _sp, const double& _noise);
         
         /** \brief Destructor
          * 
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index cd282f28130d3f0fc81cff6418500d1ec3109516..0e1e0be9d05feb444c37d22ea3c06c451791d601 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -7,7 +7,7 @@
 //     params_ << _angle_min, _angle_max, _angle_increment, _range_min, _range_max, _range_stdev, _time_increment, _scan_time;
 // }
 
-SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp) :
+SensorLaser2D::SensorLaser2D(const Eigen::Vector6s & _sp) :
     SensorBase(LIDAR, _sp, 8)
 {
     setDefaultScanParams();
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index 67b3db094e49504413630abbe2d23a74088653fb..c8fb24ce7d7596d730159bab756d663f0cbd43f5 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -44,7 +44,7 @@ class SensorLaser2D : public SensorBase
          * \param _params struct with scan parameters. See laser_scan_utils library API for reference
          * 
          **/        
-        SensorLaser2D(const Eigen::VectorXs & _sp);
+        SensorLaser2D(const Eigen::Vector6s & _sp);
         //SensorLaser2D(const Eigen::VectorXs & _sp, const laserscanutils::ScanParams & _params);
 
         /** \brief Destructor
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index 5bf66af15b572d34313918e926db2994b5f5b211..c395464610d05cb3d24ad620cf2353a4b7ab831c 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -1,6 +1,6 @@
 #include "sensor_odom_2D.h"
 
-SensorOdom2D::SensorOdom2D(const Eigen::VectorXs & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar&  _rot_noise_factor) :
+SensorOdom2D::SensorOdom2D(const Eigen::Vector6s & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar&  _rot_noise_factor) :
         SensorBase(ODOM_2D, _sp, 2)
 {
     params_ << _disp_noise_factor, _rot_noise_factor;
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index c81628ba3e4604c6a56a82427b36458371206dd2..84491a92899a88a76c9cc6b5bb7085b2fb149a64 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -16,7 +16,7 @@ class SensorOdom2D : public SensorBase
          * \param _rot_noise_factor rotation noise factor
          * 
          **/
-		SensorOdom2D(const Eigen::VectorXs & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar&  _rot_noise_factor);
+		SensorOdom2D(const Eigen::Vector6s & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar&  _rot_noise_factor);
         
         /** \brief Destructor
          * 
diff --git a/src/wolf.h b/src/wolf.h
index 5583edd5aa0df28c7864e1d74d8b9ebaefa196a4..aee57e5d64369b8463533d66cfcef8b78f1eba1c 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -62,6 +62,7 @@ typedef Matrix<WolfScalar, 1, 1> Vector1s;                ///< 1-vector of real
 typedef Matrix<WolfScalar, 2, 1> Vector2s;                ///< 2-vector of real scalar_t type
 typedef Matrix<WolfScalar, 3, 1> Vector3s;                ///< 3-vector of real scalar_t type
 typedef Matrix<WolfScalar, 4, 1> Vector4s;                ///< 4-vector of real scalar_t type
+typedef Matrix<WolfScalar, 6, 1> Vector6s;                ///< 6-vector of real scalar_t type
 typedef Matrix<WolfScalar, Dynamic, 1> VectorXs;          ///< variable size vector of real scalar_t type
 typedef Matrix<WolfScalar, 1, 2> RowVector2s;             ///< 2-row-vector of real scalar_t type
 typedef Matrix<WolfScalar, 1, 3> RowVector3s;             ///< 3-row-vector of real scalar_t type