From a58ec494d1a1b424c9104ad492af805f16276d8c Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 28 Mar 2021 09:28:55 -0400
Subject: [PATCH] icpCC: when force3DoF is true, set z to 0 in data conversion

---
 corelib/src/icp/cccorelib.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/corelib/src/icp/cccorelib.h b/corelib/src/icp/cccorelib.h
index 1ca0574f..f1515ed2 100644
--- a/corelib/src/icp/cccorelib.h
+++ b/corelib/src/icp/cccorelib.h
@@ -91,7 +91,7 @@ rtabmap::Transform icpCC(
 		CCVector3 P;
 		P.x = fromCloud->points[nIndex].x;
 		P.y = fromCloud->points[nIndex].y;
-		P.z = fromCloud->points[nIndex].z;
+		P.z = force3DoF?0:fromCloud->points[nIndex].z;
 		fromPointCloud.addPoint(P);
 	}
 	toPointCloud.reserve(toCloud->points.size());
@@ -100,7 +100,7 @@ rtabmap::Transform icpCC(
 		CCVector3 P;
 		P.x = toCloud->points[nIndex].x;
 		P.y = toCloud->points[nIndex].y;
-		P.z = toCloud->points[nIndex].z;
+		P.z = force3DoF?0:toCloud->points[nIndex].z;
 		toPointCloud.addPoint(P);
 	}