diff --git a/src/gpg/cloud_camera.cpp b/src/gpg/cloud_camera.cpp
index 252a17f..2852952 100644
--- a/src/gpg/cloud_camera.cpp
+++ b/src/gpg/cloud_camera.cpp
@@ -395,7 +395,7 @@ void CloudCamera::calculateNormalsOrganized()
   ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
   ne.setNormalSmoothingSize(20.0f);
   ne.compute(*cloud_normals);
-  normals_ = cloud_normals->getMatrixXfMap().cast<double>();
+  normals_ = cloud_normals->getMatrixXfMap(3, 4, 0).cast<double>();
 }