Compensation of point cloud point depth by water level reference point.

Axel13fr · Jan 29, 2021 · 9e94375 · 9e94375
1 parent 20c82a5
commit 9e94375
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/em_driver_library/EM_driver/kongsberg_em2040_util.h b/src/em_driver_library/EM_driver/kongsberg_em2040_util.h
@@ -162,7 +162,7 @@ static sensor_msgs::PointCloud2 mrz_to_pointcloud2(const EMdgmMRZ_S& msg,
   {
     pt.x = msg.sounding[i].x_reRefPoint_m;
     pt.y = msg.sounding[i].y_reRefPoint_m;
-    pt.z = msg.sounding[i].z_reRefPoint_m;
+    pt.z = msg.sounding[i].z_reRefPoint_m - msg.pingInfo.z_waterLevelReRefPoint_m;
     pt.intensity = msg.sounding[i].reflectivity1_dB;
     pcl.push_back(pt);
   }