diff --git a/scancontext/include/scancontext/Scancontext.cpp b/scancontext/include/scancontext/Scancontext.cpp
index 0e874e1..bd321a5 100644
--- a/scancontext/include/scancontext/Scancontext.cpp
+++ b/scancontext/include/scancontext/Scancontext.cpp
@@ -146,7 +146,7 @@ MatrixXd SCManager::makeScancontext( Eigen::MatrixX3d & _scan_down )
         const auto &point = _scan_down.row(idx);
         const auto &ptx = point(0);
         const auto &pty = point(1);
-        const auto &ptz = point(2);
+        const auto ptz = point(2) + LIDAR_HEIGHT;
 
         // xyz to ring, sector
         azim_range = sqrt(ptx * ptx + pty * pty);