fix integrateDepth with pointcloud in mapper (#117)

Co-authored-by: Remo Steiner <remos@nvidia.com>

Author: david-tingdahl-nvidia

nvblox/include/nvblox/mapper/internal/impl/mapper_impl.h

@@ -15,6 +15,8 @@ limitations under the License.
 */
 #pragma once
 
+#include "nvblox/sensors/pointcloud_to_depth_conversion.h"
+
 namespace nvblox {
 template <typename SensorType>
 void Mapper::integrateDepth(const DepthImage& depth_frame,
@@ -96,8 +98,7 @@ void Mapper::integrateDepth(const Pointcloud& pointcloud,
                               *cuda_stream_);
 
   // Integrate the depth image.
-  integrateDepth(depth_frame_from_pointcloud_, T_L_S_scanStart, lidar_sensor,
-                 T_L_S_scanEnd, scan_duration_ms);
+  integrateDepth(depth_frame_from_pointcloud_, T_L_S_scanStart, lidar_sensor);
 }
 
 template <typename SensorType>

nvblox/include/nvblox/mapper/internal/impl/multi_mapper_impl.h

@@ -13,6 +13,7 @@ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.
 */
+#pragma once
 
 #include "nvblox/sensors/pointcloud_to_depth_conversion.h"
 

nvblox/tests/test_mapper.cpp

@@ -26,6 +26,7 @@ limitations under the License.
 #include "nvblox/mapper/mapper.h"
 #include "nvblox/mesh/mesh.h"
 #include "nvblox/primitives/scene.h"
+#include "nvblox/sensors/lidar.h"
 #include "nvblox/tests/utils.h"
 
 using namespace nvblox;
@@ -403,6 +404,64 @@ TEST(MapperTest, SaveAndLoad) {
             mapper2.freespace_layer().numBlocks());
 }
 
+TEST(MapperTest, IntegratePointcloud) {
+  // Create a simple pointcloud
+  Pointcloud pointcloud(MemoryType::kUnified);
+  std::vector<Vector3f> points_host = {
+      Vector3f(1.0f, 0.0f, 0.0f),  Vector3f(2.0f, 0.0f, 0.0f),
+      Vector3f(3.0f, 0.0f, 0.0f),  Vector3f(1.5f, 0.5f, 0.0f),
+      Vector3f(2.5f, -0.5f, 0.0f), Vector3f(2.0f, 1.0f, 0.0f),
+      Vector3f(2.0f, -1.0f, 0.0f), Vector3f(3.0f, 0.5f, 0.5f)};
+  pointcloud.copyPointsFromAsync(points_host, CudaStreamOwning());
+
+  // Create a Lidar sensor with valid dimensions
+  const int num_azimuth_divisions = 1024;
+  const int num_elevation_divisions = 16;
+  const float min_valid_range_m = 0.1f;
+  const float vertical_fov_rad = 30.0f * M_PI / 180.0f;
+  Lidar lidar(num_azimuth_divisions, num_elevation_divisions, min_valid_range_m,
+              vertical_fov_rad);
+
+  // Create Mapper
+  const float voxel_size_m = 0.1f;
+  Mapper mapper(voxel_size_m, MemoryType::kDevice, ProjectiveLayerType::kTsdf);
+
+  // Integrate pointcloud without motion compensation
+  Transform T_L_S = Transform::Identity();
+  bool use_lidar_motion_compensation = false;
+  mapper.integrateDepth(pointcloud, T_L_S, lidar,
+                        use_lidar_motion_compensation);
+
+  // Verify integration worked - should have allocated some blocks
+  const int num_blocks_after_first = mapper.tsdf_layer().numBlocks();
+  EXPECT_GT(num_blocks_after_first, 0);
+
+  // Integrate pointcloud again with motion compensation from a different
+  // location Motion compensation requires timestamps for each point
+  const int64_t scan_duration_ms_value = 100;
+  Time scan_duration_ms(scan_duration_ms_value);
+  std::vector<Time> timestamps_ms(points_host.size());
+  for (size_t i = 0; i < points_host.size(); i++) {
+    // Spread timestamps evenly across scan duration
+    timestamps_ms[i] = Time(i * scan_duration_ms_value / points_host.size());
+  }
+  pointcloud.copyTimestampsFromAsync(timestamps_ms, CudaStreamOwning());
+
+  // Start from a different position to cover new space
+  Transform T_L_S_scanStart2 = Transform::Identity();
+  T_L_S_scanStart2.translate(Vector3f(0.0f, 3.0f, 0.0f));  // Start 3m away in y
+  Transform T_L_S_scanEnd2 = Transform::Identity();
+  T_L_S_scanEnd2.translate(Vector3f(0.0f, 4.0f, 0.0f));  // End 4m away in y
+  use_lidar_motion_compensation = true;
+  mapper.integrateDepth(pointcloud, T_L_S_scanStart2, lidar,
+                        use_lidar_motion_compensation, T_L_S_scanEnd2,
+                        scan_duration_ms);
+
+  // Verify more blocks were allocated after second integration
+  const int num_blocks_after_second = mapper.tsdf_layer().numBlocks();
+  EXPECT_GT(num_blocks_after_second, num_blocks_after_first);
+}
+
 int main(int argc, char** argv) {
   FLAGS_alsologtostderr = true;
   google::InitGoogleLogging(argv[0]);

nvblox/tests/test_multi_mapper.cpp

@@ -19,6 +19,7 @@ limitations under the License.
 
 #include "nvblox/datasets/3dmatch.h"
 #include "nvblox/mapper/multi_mapper.h"
+#include "nvblox/sensors/lidar.h"
 
 using namespace nvblox;
 
@@ -191,6 +192,63 @@ TEST_F(MultiMapperTest, MaskOnAndOff) {
   LOG(INFO) << "num_non_zero_weight_voxels: ";
 }
 
+TEST_F(MultiMapperTest, IntegratePointcloud) {
+  // Create a simple pointcloud
+  Pointcloud pointcloud(MemoryType::kUnified);
+  std::vector<Vector3f> points_host = {
+      Vector3f(1.0f, 0.0f, 0.0f),  Vector3f(2.0f, 0.0f, 0.0f),
+      Vector3f(3.0f, 0.0f, 0.0f),  Vector3f(1.5f, 0.5f, 0.0f),
+      Vector3f(2.5f, -0.5f, 0.0f), Vector3f(2.0f, 1.0f, 0.0f),
+      Vector3f(2.0f, -1.0f, 0.0f), Vector3f(3.0f, 0.5f, 0.5f)};
+  pointcloud.copyPointsFromAsync(points_host, CudaStreamOwning());
+
+  // Create a Lidar sensor with valid dimensions
+  const int num_azimuth_divisions = 1024;
+  const int num_elevation_divisions = 16;
+  const float min_valid_range_m = 0.1f;
+  const float vertical_fov_rad = 30.0f * M_PI / 180.0f;
+  Lidar lidar(num_azimuth_divisions, num_elevation_divisions, min_valid_range_m,
+              vertical_fov_rad);
+
+  // Integrate pointcloud using MultiMapper without motion compensation
+  Transform T_L_S = Transform::Identity();
+  bool use_lidar_motion_compensation = false;
+  multi_mapper_.integrateDepth(pointcloud, T_L_S, lidar,
+                               use_lidar_motion_compensation);
+
+  // Verify integration worked - should have allocated some blocks
+  // MultiMapper integrates into the background mapper by default
+  const int num_blocks_after_first =
+      multi_mapper_.background_mapper()->tsdf_layer().numBlocks();
+  EXPECT_GT(num_blocks_after_first, 0);
+
+  // Integrate pointcloud again with motion compensation from a different
+  // location Motion compensation requires timestamps for each point
+  const int64_t scan_duration_ms_value = 100;
+  Time scan_duration_ms(scan_duration_ms_value);
+  std::vector<Time> timestamps_ms(points_host.size());
+  for (size_t i = 0; i < points_host.size(); i++) {
+    // Spread timestamps evenly across scan duration
+    timestamps_ms[i] = Time(i * scan_duration_ms_value / points_host.size());
+  }
+  pointcloud.copyTimestampsFromAsync(timestamps_ms, CudaStreamOwning());
+
+  // Start from a different position to cover new space
+  Transform T_L_S_scanStart2 = Transform::Identity();
+  T_L_S_scanStart2.translate(Vector3f(0.0f, 3.0f, 0.0f));  // Start 3m away in y
+  Transform T_L_S_scanEnd2 = Transform::Identity();
+  T_L_S_scanEnd2.translate(Vector3f(0.0f, 4.0f, 0.0f));  // End 4m away in y
+  use_lidar_motion_compensation = true;
+  multi_mapper_.integrateDepth(pointcloud, T_L_S_scanStart2, lidar,
+                               use_lidar_motion_compensation, T_L_S_scanEnd2,
+                               scan_duration_ms);
+
+  // Verify more blocks were allocated after second integration
+  const int num_blocks_after_second =
+      multi_mapper_.background_mapper()->tsdf_layer().numBlocks();
+  EXPECT_GT(num_blocks_after_second, num_blocks_after_first);
+}
+
 int main(int argc, char** argv) {
   FLAGS_alsologtostderr = true;
   google::InitGoogleLogging(argv[0]);