diff --git a/applications/reconstruct/src/scene_rep_hash_sdf.cu b/applications/reconstruct/src/scene_rep_hash_sdf.cu
index 521a34601e5a2071ee9e24d79f12ff6ce4dbfa8e..9c568280d24014a37aa8bdeb527b4569aa5267b1 100644
--- a/applications/reconstruct/src/scene_rep_hash_sdf.cu
+++ b/applications/reconstruct/src/scene_rep_hash_sdf.cu
@@ -624,7 +624,7 @@ __global__ void starveVoxelsKernel(HashData hashData) {
 	//is typically exectued only every n'th frame
 	int weight = hashData.d_SDFBlocks[entry.ptr + threadIdx.x].weight;
 	weight = max(0, weight-1);	
-	hashData.d_SDFBlocks[entry.ptr + threadIdx.x].weight = weight;  //CHECK Remove to totally clear previous frame (Nick)
+	hashData.d_SDFBlocks[entry.ptr + threadIdx.x].weight = 1;  //CHECK Remove to totally clear previous frame (Nick)
 }
 
 extern "C" void starveVoxelsKernelCUDA(HashData& hashData, const HashParams& hashParams)
diff --git a/applications/registration/src/aruco.cpp b/applications/registration/src/aruco.cpp
index 28cddabea859cdef754c80d9303099eea03dbc45..8ff84854603123f0799455f223bcd98506b0b2e0 100644
--- a/applications/registration/src/aruco.cpp
+++ b/applications/registration/src/aruco.cpp
@@ -165,8 +165,9 @@ void ftl::registration::aruco(ftl::Configurable *root) {
 			}
 
 			Eigen::Matrix4d t;
-			lastScore = current->estimateTransform(t, srcfeat, targfeat);
+			lastScore = current->estimateTransform(t, srcfeat, targfeat, true);
 			current->setTransform(t);
+			//lastScore = current->icp();
 		} else if (key == 'f') {
 			Mat f1,f2;
 			current->capture(f1,f2);
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index f40eaca97242d51ada14428b2293e562a4df10c4..a05cddfa8bbf3931f1a9df1a3f236ea4ff28afe6 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -310,7 +310,7 @@ double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
 	return validate.validateTransformation(src_cloud, targ_cloud, T);
 }
 
-double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat) {
+double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat, bool doicp) {
 	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
 	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
 
@@ -337,8 +337,27 @@ double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<
 
 	pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
 
-	svd.estimateRigidTransformation(*src_cloud, idx, *targ_cloud, idx, T);
-	return validate.validateTransformation(src_cloud, targ_cloud, T);
+	svd.estimateRigidTransformation(*tsrc_cloud, idx, *targ_cloud, idx, T);
+
+	if (doicp) {
+		pcl::transformPointCloud(*tsrc_cloud, *src_cloud, T);
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
+
+		icp.setInputSource(src_cloud);
+		icp.setInputTarget(targ_cloud);
+		icp.align(*final_cloud);
+		LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
+		//transform_ *= icp.getFinalTransformation();
+
+		T = icp.getFinalTransformation() * T * transform_;
+		//uptodate_ = true;
+
+		return icp.getFitnessScore();
+	} else {
+		return validate.validateTransformation(src_cloud, targ_cloud, T);
+	}
 }
 
 double Correspondances::estimateTransform(Eigen::Matrix4d &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index 53f50a155ba4921c4b14f65571ee775473d738fb..72f39cf86640834f4ee49855f19ed237b4883d1c 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -65,7 +65,7 @@ class Correspondances {
 	 */
 	double estimateTransform(Eigen::Matrix4d &);
 	double estimateTransform(Eigen::Matrix4d &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
-	double estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat);
+	double estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat, bool doicp=false);
 
 	double findBestSubset(Eigen::Matrix4d &tr, int K, int N);