diff --git a/slam3d/core/MeasurementStorage.cpp b/slam3d/core/MeasurementStorage.cpp
index 593dc10..88513e2 100644
--- a/slam3d/core/MeasurementStorage.cpp
+++ b/slam3d/core/MeasurementStorage.cpp
@@ -1,4 +1,4 @@
-#include "MeasurementStorage.hpp"
+#include "MeasurementStorage.hpp"
 
 #include <boost/uuid/uuid_io.hpp>
 #include <boost/lexical_cast.hpp>
@@ -7,12 +7,19 @@ using namespace slam3d;
 
 void MeasurementStorage::add(Measurement::Ptr measurement)
 {
-	mMeasurements[measurement->getUniqueId()] = measurement;
+	if (enabled) {
+		mMeasurements[measurement->getUniqueId()] = measurement;
+	}
+
 }
 
 Measurement::Ptr MeasurementStorage::get(const boost::uuids::uuid& uuid)
 {
-	return mMeasurements.at(uuid);
+	try {
+		return mMeasurements.at(uuid);
+	}catch (const std::out_of_range& e) {
+		return Measurement::Ptr();
+	}
 }
 
 Measurement::Ptr MeasurementStorage::get(const std::string& key)
@@ -24,3 +31,13 @@ bool MeasurementStorage::contains(const boost::uuids::uuid& key)
 {
 	return mMeasurements.count(key);
 }
+
+void MeasurementStorage::enable()
+{
+	enabled = true;
+}
+
+void MeasurementStorage::disable()
+{
+	enabled = false;
+}
diff --git a/slam3d/core/MeasurementStorage.hpp b/slam3d/core/MeasurementStorage.hpp
index 9b5454a..42ab3e2 100644
--- a/slam3d/core/MeasurementStorage.hpp
+++ b/slam3d/core/MeasurementStorage.hpp
@@ -16,7 +16,7 @@ namespace slam3d
 	class MeasurementStorage
 	{
 	public:
-
+		MeasurementStorage():enabled(true) {}
 		virtual ~MeasurementStorage() {}
 
 		/**
@@ -48,6 +48,12 @@ namespace slam3d
 		 */
 		Measurement::Ptr get(const std::string& key);
 
+		void enable();
+		void disable();
+
+	protected:
+		bool enabled;
+
 	private:
 		std::map<boost::uuids::uuid, Measurement::Ptr> mMeasurements;
 	};
diff --git a/slam3d/core/ScanSensor.cpp b/slam3d/core/ScanSensor.cpp
index 1dded9d..e43f976 100644
--- a/slam3d/core/ScanSensor.cpp
+++ b/slam3d/core/ScanSensor.cpp
@@ -55,6 +55,15 @@ bool ScanSensor::addMeasurement(const Measurement::Ptr& m)
 	}
 
 	Measurement::Ptr source = mMapper->getGraph()->getMeasurement(mLastVertex);
+
+	if (!source.get()) {
+		IdType newVertex = mMapper->addMeasurement(m);
+		mMapper->getGraph()->setCorrectedPose(newVertex, getCurrentPose());
+		mLastTransform = Transform::Identity();
+		mLastVertex = newVertex;
+		return true;
+	}
+
 	try
 	{
 		Constraint::Ptr c = createConstraint(source, m, mLastTransform, false);
@@ -100,10 +109,34 @@ bool ScanSensor::addMeasurement(const Measurement::Ptr& m, const Transform& odom
 		return true;
 	}
 	
+	Measurement::Ptr source = mMapper->getGraph()->getMeasurement(mLastVertex);
+
+
+
 	// Add measurement if sufficient movement is reported by the odometry
 	mLastTransform = mLastOdometry.inverse() * odom;
 	if(checkMinDistance(mLastTransform))
 	{
+		if (!source.get()) {
+			IdType newVertex = mMapper->addMeasurement(m);
+			mMapper->getGraph()->setCorrectedPose(newVertex, getCurrentPose());
+
+			Constraint::Ptr c(new SE3Constraint("paused", mLastTransform, Covariance<6>::Identity()));
+			SE3Constraint::Ptr se3 = boost::dynamic_pointer_cast<SE3Constraint>(c);
+			mMapper->getGraph()->addConstraint(mLastVertex, newVertex, c);
+			mLastTransform = se3->getRelativePose();
+			Transform last_pose = mMapper->getGraph()->getVertex(mLastVertex).correctedPose;
+			mMapper->getGraph()->setCorrectedPose(newVertex, getCurrentPose());
+
+
+			mLastOdometry = odom;
+			mLastVertex = newVertex;
+			mLastTransform = Transform::Identity();
+			return true;
+		}
+
+
+
 		IdType newVertex = mMapper->addMeasurement(m);
 		Measurement::Ptr source = mMapper->getGraph()->getMeasurement(mLastVertex);
 		if(mLinkPrevious)
@@ -119,7 +152,7 @@ bool ScanSensor::addMeasurement(const Measurement::Ptr& m, const Transform& odom
 				{
 					mLastTransform = se3->getRelativePose();
 				}
-				Transform last_pose = mMapper->getGraph()->getVertex(mLastVertex).correctedPose;
+				// Transform last_pose = mMapper->getGraph()->getVertex(mLastVertex).correctedPose;
 				mMapper->getGraph()->setCorrectedPose(newVertex, getCurrentPose());
 			}catch(std::exception &e)
 			{
diff --git a/slam3d/sensor/pcl/PointCloudSensor.cpp b/slam3d/sensor/pcl/PointCloudSensor.cpp
index 29c3eb4..d11897f 100644
--- a/slam3d/sensor/pcl/PointCloudSensor.cpp
+++ b/slam3d/sensor/pcl/PointCloudSensor.cpp
@@ -124,6 +124,11 @@ Transform align(PointCloudMeasurement::Ptr source,
 	// Downsample the scans
 	PointCloud::Ptr filtered_source = source->getPointCloud();
 	PointCloud::Ptr filtered_target = target->getPointCloud();
+
+	if (!filtered_source || !filtered_target || filtered_source->size() == 0 || filtered_target->size() == 0) {
+		return Transform::Identity();
+	}
+
 	if(config.point_cloud_density > 0)
 	{
 		filtered_source = PointCloudSensor::downsample(source->getPointCloud(), config.point_cloud_density);
@@ -241,10 +246,11 @@ PointCloud::Ptr PointCloudSensor::getAccumulatedCloud(const VertexObjectList& ve
 	{
 		Measurement::Ptr m = mMapper->getGraph()->getMeasurement(vertices[i].measurementUuid);
 		PointCloudMeasurement::Ptr pcl = boost::dynamic_pointer_cast<PointCloudMeasurement>(m);
-		if(!pcl)
+		if(!pcl || pcl->getPointCloud()->size() == 0)
 		{
-			mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not a point cloud!");
-			throw BadMeasurementType();
+			mLogger->message(ERROR, "Measurement in getAccumulatedCloud() is not available or not a point cloud!");
+			// throw BadMeasurementType();
+			continue;
 		}
 		
 		PointCloud::Ptr tempCloud = transform(pcl->getPointCloud(), (vertices[i].correctedPose * pcl->getSensorPose()));
@@ -276,10 +282,13 @@ Constraint::Ptr PointCloudSensor::createConstraint(const Measurement::Ptr& sourc
 	// Cast to this sensors measurement type
 	PointCloudMeasurement::Ptr sourceCloud = boost::dynamic_pointer_cast<PointCloudMeasurement>(source);
 	PointCloudMeasurement::Ptr targetCloud = boost::dynamic_pointer_cast<PointCloudMeasurement>(target);
-	if(!sourceCloud || !targetCloud)
+	if((!sourceCloud || !targetCloud) || sourceCloud->getPointCloud()->size() == 0 || targetCloud->getPointCloud()->size() == 0)
 	{
 		mLogger->message(ERROR, "Measurement given to createConstraint() is not a PointCloud!");
-		throw BadMeasurementType();
+		// throw BadMeasurementType();
+		// is there was an emptyx cloud, use the guess
+		Covariance<6> covariance = Covariance<6>::Identity() * mCovarianceScale;
+		return Constraint::Ptr(new SE3Constraint(mName, guess, covariance.inverse()));
 	}
 	
 	// For large loops, refine guess by a coarse ICP