diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 4c51f0ce3b02fae03e81bb97db758092e64e01c0..c37b61ab339ed274777d7a1f58bbf8a2c8ee0fb2 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -8,18 +8,31 @@
 // unsigned int CaptureLaser2D::max_beam_distance = 5;//max number of beams of distance between lines to consider corner or concatenation
 // double CaptureLaser2D::max_distance = 0.5;//max distance between line ends to consider corner or concatenation
 
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges):
-	CaptureBase(_ts, _sensor_ptr, _ranges),
-	ranges_(data_.data(), _ranges.size()),
-	intensities_(data_.data(), 0)
+//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges):
+//	CaptureBase(_ts, _sensor_ptr, _ranges),
+//	ranges_(data_.data(), _ranges.size()),
+//	intensities_(data_.data(), 0)
+//{
+//    laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
+//}
+CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges):
+  CaptureBase(_ts, _sensor_ptr),
+  ranges_(_ranges)
 {
     laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
 }
 
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities):
-		CaptureBase(_ts, _sensor_ptr, _ranges),
-		ranges_(data_.data(), _ranges.size()),
-		intensities_(data_.data(), _intensities.size())
+//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities):
+//    CaptureBase(_ts, _sensor_ptr, _ranges),
+//    ranges_(data_.data(), _ranges.size()),
+//    intensities_(data_.data(), _intensities.size())
+//{
+//      laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
+//}
+CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities):
+		CaptureBase(_ts, _sensor_ptr),
+		ranges_(_ranges),
+		intensities_(_intensities)
 {
     laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
 }
@@ -37,15 +50,15 @@ void CaptureLaser2D::processCapture()
     
     //extract corners from range data
     extractCorners(corners);
-    //std::cout << corners.size() << " corners extracted" << std::endl;
+    std::cout << corners.size() << " corners extracted" << std::endl;
     
     //generate a feature for each corner
     createFeatures(corners);
-    //std::cout << getFeatureListPtr()->size() << " Features created" << std::endl;
+    std::cout << getFeatureListPtr()->size() << " Features created" << std::endl;
     
     //Establish constraints for each feature
     establishConstraints();
-    //std::cout << "Constraints created" << std::endl;
+    std::cout << "Constraints created" << std::endl;
 }
 
 unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 8f4c537af9b9f54dd98f60a46a723086a8d5fdde..776fa0ba3bace880036c449b6e3e6d319441a69f 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -37,9 +37,11 @@ class CaptureLaser2D : public CaptureBase
 //         static double k_sigmas;//How many std_dev are tolerated to count that a point is supporting a line
 //         static unsigned int max_beam_distance;//max number of beams of distance between lines to consider corner or concatenation
 //         static double max_distance;//max distance between line ends to consider corner or concatenation
-        
-        Eigen::Map<Eigen::VectorXs> ranges_; // a map to the ranges inside de data vector
-        Eigen::Map<Eigen::VectorXs> intensities_; // a map to the intensities inside the data vector
+
+        //Eigen::Map<Eigen::VectorXs> ranges_; // a map to the ranges inside de data vector
+        std::vector<float> ranges_; // ranges vector
+        //Eigen::Map<Eigen::VectorXs> intensities_; // a map to the intensities inside the data vector
+        std::vector<float> intensities_; // intensities vector
         SensorLaser2D* laser_ptr_; //specific pointer to sensor laser 2D object
         
     public:
@@ -48,14 +50,16 @@ class CaptureLaser2D : public CaptureBase
          * Constructor with ranges
          * 
          **/
-        CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges);
+        //CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges);
+        CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges);
 
         /** \brief Constructor with ranges and intensities
          *
          * Constructor with ranges and intensities
          *
          **/
-        CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities);
+        //CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities);
+        CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities);
 
         /** \brief Destructor
          * 
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index 884d68c457b19a941bd9e31b14a576feb6ce7e12..5c7b366cf74c8e300bdaff56688abf2e303fbaa3 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -10,9 +10,9 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, cons
 	CaptureRelative(_ts, _sensor_ptr, _data)
 {
 	data_covariance_ = Eigen::Matrix2s::Zero();
-  data_covariance_(0,0) = _data(0)*((SensorOdom2D*)_sensor_ptr)->getDisplacementNoiseFactor();
-  data_covariance_(1,1) = _data(1)*((SensorOdom2D*)_sensor_ptr)->getRotationNoiseFactor();
-  std::cout << data_covariance_ << std::endl;
+  data_covariance_(0,0) = (_data(0) == 0 ? 1e-6 : _data(0))*((SensorOdom2D*)_sensor_ptr)->getDisplacementNoiseFactor();
+  data_covariance_(1,1) = (_data(1) == 0 ? 1e-6 : _data(1))*((SensorOdom2D*)_sensor_ptr)->getRotationNoiseFactor();
+//  std::cout << data_covariance_ << std::endl;
 }
 
 CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 5530fb4bd2c9cafcdd5a96eff4edd6bb2c285170..bf0a8a8cc349257129574792818b0fb3f9ca7ec3 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -198,8 +198,8 @@ int main(int argc, char** argv)
 		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(0)+1e-3);
-		odom_reading(1) += distribution_odom(generator)*(odom_reading(1)+1e-3);
+		odom_reading(0) += distribution_odom(generator)*(odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
+		odom_reading(1) += distribution_odom(generator)*(odom_reading(1) == 0 ? 1e-6 : odom_reading(1));
 
 		// odometry integration
 		pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2));
@@ -219,15 +219,11 @@ int main(int argc, char** argv)
 		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;
 
@@ -238,8 +234,8 @@ int main(int argc, char** argv)
 		// adding new sensor captures
 		wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading));//, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
 //		wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
-		wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1_reading));
-		wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2_reading));
+		wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
+		wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
         // updating problem
 		wolf_manager->update();
 		mean_times(1) += ((double)clock()-t1)/CLOCKS_PER_SEC;
@@ -271,7 +267,7 @@ int main(int argc, char** argv)
 		// draw detected corners
 		std::list<laserscanutils::Corner> corner_list;
 		std::vector<double> corner_vector;
-		CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1_reading);
+		CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
 		last_scan.extractCorners(corner_list);
 		for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
 		{
diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp
index 49dce88ab0c9c7c3e7598750e004553bc70f41be..424b5c535bd63d648ed6682eaecb34c98bc068d2 100644
--- a/src/examples/test_ceres_laser.cpp
+++ b/src/examples/test_ceres_laser.cpp
@@ -396,8 +396,6 @@ int main(int argc, char** argv)
 		//compute scan
 		myScan.clear();
 		myScanner->computeScan(devicePose,myScan);
-		vector<double> myScanDoubles(myScan.begin(), myScan.end());
-		Eigen::Map<Eigen::VectorXs> scan_reading(myScanDoubles.data(), 720);
 		mean_times(0) += ((double)clock()-t1)/CLOCKS_PER_SEC;
 
 
@@ -408,7 +406,7 @@ int main(int argc, char** argv)
 	    time_stamp.setToNow();
 		wolf_manager->addCapture(new CaptureOdom2D(time_stamp, &odom_sensor, odom_reading, odom_std * Eigen::MatrixXs::Identity(2,2)));
 		//wolf_manager->addCapture(new CaptureGPSFix(time_stamp, &gps_sensor, gps_fix_reading, gps_std * MatrixXs::Identity(3,3)));
-		wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_sensor, scan_reading));
+		wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_sensor, myScan));
 
         // updating problem
 		wolf_manager->update();
@@ -442,7 +440,7 @@ int main(int argc, char** argv)
 		// draw detected corners
 		std::list<laserscanutils::Corner> corner_list;
 		std::vector<double> corner_vector;
-		CaptureLaser2D last_scan(time_stamp, &laser_sensor, scan_reading);
+		CaptureLaser2D last_scan(time_stamp, &laser_sensor, myScan);
 		last_scan.extractCorners(corner_list);
 		for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
 		{
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index cb75f37b599032344a2da50e87e4af889a435c80..2068eb45f079f296abfda6951367b38358c717fe 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -119,7 +119,7 @@ void FrameBase::setState(const Eigen::VectorXs& _st)
 						  (!v_ptr_ ? 0 : v_ptr_->getStateSize()) +
 						  (!w_ptr_ ? 0 : w_ptr_->getStateSize())) &&
 						  "In FrameBase::setState wrong state size");
-	assert(!!p_ptr_ && "in FrameBase::setState(), p_ptr_ is nullptr");
+	assert(p_ptr_!=nullptr && "in FrameBase::setState(), p_ptr_ is nullptr");
 
 	Eigen::Map<Eigen::VectorXs> state_map(p_ptr_->getPtr(), _st.size());
 	state_map = _st;
@@ -132,7 +132,7 @@ void FrameBase::addCapture(CaptureBase* _capt_ptr)
 
 void FrameBase::removeCapture(CaptureBaseIter& _capt_iter)
 {
-	std::cout << "removing capture " << (*_capt_iter)->nodeId() << " from Frame " << nodeId() << std::endl;
+	//std::cout << "removing capture " << (*_capt_iter)->nodeId() << " from Frame " << nodeId() << std::endl;
 	removeDownNode(_capt_iter);
 }
 
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index 0e1e0be9d05feb444c37d22ea3c06c451791d601..7529d336da23ab91a25f485373b781c3c0db6ce9 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -30,9 +30,9 @@ void SensorLaser2D::setDefaultScanParams()
 {
     //TODO: Decide who holds intrinsic parameters, either SensorBase::params_ or scan_params_, but NOTH BOTH!!
     
-    scan_params_.angle_min_ = -M_PI/2;
-    scan_params_.angle_max_ = M_PI/2;
-    scan_params_.angle_step_ = M_PI/720;
+    scan_params_.angle_min_ = M_PI/2;
+    scan_params_.angle_max_ = -M_PI/2;
+    scan_params_.angle_step_ = -M_PI/720;
     scan_params_.scan_time_ = 0.01;//not relevant
     scan_params_.range_min_ = 0.2;
     scan_params_.range_max_ = 100;
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
index f8f1b62b7410c30eb42938da7d5cc4ff4c8db618..4743190f0578120706d7057d38aa0dac8d29db12 100644
--- a/src/time_stamp.cpp
+++ b/src/time_stamp.cpp
@@ -14,7 +14,7 @@ TimeStamp::TimeStamp(const WolfScalar _ts) :
 }
 
 TimeStamp::TimeStamp(const unsigned long int _sec, const unsigned long int _nsec) :
-        time_stamp_((WolfScalar)(_sec + _nsec / 1e9))
+        time_stamp_((WolfScalar)_sec + (WolfScalar)_nsec / 1e9)
 {
     //
 }
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index a1ec474316014bff7fc0ab9167debd642852fac0..076bbbbcb408f75af306469a018c497c593d65fa 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -59,7 +59,7 @@ class WolfManager
         window_size_(_w_size),
         new_frame_elapsed_time_(_new_frame_elapsed_time)
     {
-      createFrame(_init_frame, TimeStamp());
+      createFrame(_init_frame, TimeStamp(0));
       problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
     }
 
@@ -95,7 +95,7 @@ class WolfManager
     void addCapture(CaptureBase* _capture)
     {
       new_captures_.push(_capture);
-      //std::cout << "added new capture: " << _capture->nodeId() << std::endl;
+      //std::cout << "added new capture: " << _capture->nodeId() << "stamp: " << _capture->getTimeStamp().get() << std::endl;
     }
 
     void update()
@@ -110,12 +110,14 @@ class WolfManager
         if (new_capture->getSensorPtr() == sensor_prior_)
         {
           // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR
-          //std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl;
+          //std::cout << "integrating captures " << last_capture_relative_->nodeId() << " " << new_capture->nodeId() << std::endl;
           last_capture_relative_->integrateCapture((CaptureRelative*)(new_capture));
 
           // 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() > new_frame_elapsed_time_)
+          if (problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() == 0)
+            problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setTimeStamp(new_capture->getTimeStamp());
+          else if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > new_frame_elapsed_time_)
           {
             //std::cout << "store prev frame" << std::endl;
             auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();