diff --git a/corelib/include/rtabmap/core/Registration.h b/corelib/include/rtabmap/core/Registration.h
index 69b76091..4bfffd2c 100644
--- a/corelib/include/rtabmap/core/Registration.h
+++ b/corelib/include/rtabmap/core/Registration.h
@@ -85,6 +85,8 @@ public:
 			Transform guess = Transform::getIdentity(),
 			RegistrationInfo * info = 0) const;
 
+	void normalizeCovariance(cv::Mat & covariance, const Transform & transform) const;
+
 protected:
 	// take ownership of child
 	Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
diff --git a/corelib/src/Registration.cpp b/corelib/src/Registration.cpp
index 255fee6d..3f0d0f86 100644
--- a/corelib/src/Registration.cpp
+++ b/corelib/src/Registration.cpp
@@ -212,35 +212,8 @@ Transform Registration::computeTransformationMod(
 		}
 	}
 
-	if(covarianceNormalized_)
-	{
-		// normalize variance
-		UASSERT(info.covariance.cols == 6 && info.covariance.rows == 6);
-		float norm = t.getNorm();
-		if(norm > 0.0f)
-		{
-			cv::Mat(info.covariance, cv::Range(0,3), cv::Range(0,3)) *= norm;
-		}
-		float angle = t.getAngle();
-		if(angle > 0.0f)
-		{
-			cv::Mat(info.covariance, cv::Range(3,6), cv::Range(3,6)) *= angle;
-		}
-	}
 
-	double epsilon = 0.000001;
-	if(info.covariance.at<double>(0,0)<=0.0)
-		info.covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
-	if(info.covariance.at<double>(1,1)<=0.0)
-		info.covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
-	if(info.covariance.at<double>(2,2)<=0.0)
-		info.covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
-	if(info.covariance.at<double>(3,3)<=0.0)
-		info.covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
-	if(info.covariance.at<double>(4,4)<=0.0)
-		info.covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
-	if(info.covariance.at<double>(5,5)<=0.0)
-		info.covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
+	normalizeCovariance(info.covariance, t);
 
 	if(child_)
 	{
@@ -266,4 +239,36 @@ Transform Registration::computeTransformationMod(
 	return t;
 }
 
+void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & transform) const
+{
+	UASSERT(covariance.cols == 6 && covariance.rows == 6);
+
+	if(covarianceNormalized_)
+	{
+		// normalize variance
+		float norm = transform.getNorm();
+		covariance.at<double>(0,0) *= norm;
+		covariance.at<double>(1,1) *= norm;
+		covariance.at<double>(2,2) *= norm;
+		float angle = transform.getAngle()/10.0;
+		covariance.at<double>(3,3) *= angle;
+		covariance.at<double>(4,4) *= angle;
+		covariance.at<double>(5,5) *= angle;
+	}
+
+	double epsilon = 0.000001;
+	if(covariance.at<double>(0,0)<=0.0)
+		covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
+	if(covariance.at<double>(1,1)<=0.0)
+		covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
+	if(covariance.at<double>(2,2)<=0.0)
+		covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
+	if(covariance.at<double>(3,3)<=0.0)
+		covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
+	if(covariance.at<double>(4,4)<=0.0)
+		covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
+	if(covariance.at<double>(5,5)<=0.0)
+		covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
+}
+
 }
diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp
index 69aae5dd..ffec99c4 100644
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -1292,28 +1292,13 @@ Transform RegistrationVis::computeTransformationImpl(
 			poses.insert(std::make_pair(2, transforms[0]));
 
 			cv::Mat cov = covariances[0].clone();
-			if(covarianceNormalized())
-			{
-				cv::Mat(cov, cv::Range(0,3), cv::Range(0,3)) *= transform.getNorm();
-				cv::Mat(cov, cv::Range(3,6), cv::Range(3,6)) *= transform.getAngle();
-			}
-			if(cov.at<double>(0,0)<=0.0)
-			{
-				cov = cv::Mat::eye(6,6,CV_64FC1)*0.000001; // epsilon if exact transform
-			}
+			normalizeCovariance(cov, transform);
+
 			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
 			if(!transforms[1].isNull() && inliers[1].size())
 			{
 				cov = covariances[1].clone();
-				if(covarianceNormalized())
-				{
-					cv::Mat(cov, cv::Range(0,3), cv::Range(0,3)) *= transform.getNorm();
-					cv::Mat(cov, cv::Range(3,6), cv::Range(3,6)) *= transform.getAngle();
-				}
-				if(cov.at<double>(0,0)<=0.0)
-				{
-					cov = cv::Mat::eye(6,6,CV_64FC1)*0.000001; // epsilon if exact transform
-				}
+				normalizeCovariance(cov, transform);
 				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
 			}