Merge branch 'publish/test_fixes' into 'public'

[public] Minor update with bug fixes and speeding up test performance on Jetson.

See merge request nvblox/nvblox!213

Jenkinsfile

@@ -26,7 +26,7 @@ pipeline {
     stage('Link Into External Project') {
       steps {
         dir("nvblox_lib_test") {
-          git credentialsId: 'isaac-git-master', url: 'ssh://[email protected]:12051/nvblox/nvblox_lib_test.git', branch: 'main'
+          git credentialsId: 'vault-svc-ssh', url: 'ssh://[email protected]:12051/nvblox/nvblox_lib_test.git', branch: 'main'
         }
         sh '''mkdir -p nvblox_lib_test/build'''
         sh '''cd nvblox_lib_test/build && cmake .. -DNVBLOX_INSTALL_PATH=${WORKSPACE}/nvblox/install && make'''

README.md

@@ -33,9 +33,10 @@ We depend on:
 - gflags (to run experiments)
 - CUDA 10.2 - 11.5 (others might work but are untested)
 - Eigen (no need to explicitly install, a recent version is built into the library)
+- SQLite 3 (for serialization)
 Please run
 ```
-sudo apt-get install -y libgoogle-glog-dev libgtest-dev libgflags-dev python3-dev
+sudo apt-get install -y libgoogle-glog-dev libgtest-dev libgflags-dev python3-dev libsqlite3-dev
 cd /usr/src/googletest && sudo cmake . && sudo cmake --build . --target install
 ```
 

nvblox/include/nvblox/core/camera.h

@@ -35,6 +35,12 @@ class Camera {
 
   __host__ __device__ inline float getDepth(const Vector3f& p_C) const;
 
+  // Back projection (image plane point to 3D point)
+  __host__ __device__ inline Vector3f unprojectFromImagePlaneCoordinates(
+      const Vector2f& u_C, const float depth) const;
+  __host__ __device__ inline Vector3f unprojectFromPixelIndices(
+      const Index2D& u_C, const float depth) const;
+
   /// Get the axis aligned bounding box of the view in the LAYER coordinate
   /// frame.
   __host__ AxisAlignedBoundingBox getViewAABB(const Transform& T_L_C,

nvblox/include/nvblox/core/impl/camera_impl.h

@@ -41,8 +41,16 @@ bool Camera::project(const Eigen::Vector3f& p_C, Eigen::Vector2f* u_C) const {
   return true;
 }
 
-float Camera::getDepth(const Vector3f& p_C) const {
-  return p_C.z();
+float Camera::getDepth(const Vector3f& p_C) const { return p_C.z(); }
+
+Vector3f Camera::unprojectFromImagePlaneCoordinates(const Vector2f& u_C,
+                                                    const float depth) const {
+  return depth * vectorFromImagePlaneCoordinates(u_C);
+}
+
+Vector3f Camera::unprojectFromPixelIndices(const Index2D& u_C,
+                                           const float depth) const {
+  return depth * vectorFromPixelIndices(u_C);
 }
 
 Vector3f Camera::vectorFromImagePlaneCoordinates(const Vector2f& u_C) const {

nvblox/include/nvblox/mesh/mesh_block.h

@@ -19,6 +19,7 @@ limitations under the License.
 
 #include "nvblox/core/blox.h"
 #include "nvblox/core/color.h"
+#include "nvblox/core/layer.h"
 #include "nvblox/core/types.h"
 #include "nvblox/core/unified_ptr.h"
 #include "nvblox/core/unified_vector.h"
@@ -34,6 +35,10 @@ struct MeshBlock {
 
   MeshBlock(MemoryType memory_type = MemoryType::kDevice);
 
+  // "Clone" copy constructor, with the possibility to change device type.
+  MeshBlock(const MeshBlock& mesh_block);
+  MeshBlock(const MeshBlock& mesh_block, MemoryType memory_type);
+
   // Mesh Data
   // These unified vectors contain the mesh data for this block. Note that
   // Colors and/or intensities are optional. The "triangles" vector is a vector
@@ -42,7 +47,6 @@ struct MeshBlock {
   unified_vector<Vector3f> vertices;
   unified_vector<Vector3f> normals;
   unified_vector<Color> colors;
-  unified_vector<float> intensities;
   unified_vector<int> triangles;
 
   void clear();
@@ -57,7 +61,6 @@ struct MeshBlock {
   // Resize colors/intensities such that:
   // `colors.size()/intensities.size() == vertices.size()`
   void expandColorsToMatchVertices();
-  void expandIntensitiesToMatchVertices();
 
   // Copy mesh data to the CPU.
   std::vector<Vector3f> getVertexVectorOnCPU() const;
@@ -84,4 +87,25 @@ struct CudaMeshBlock {
   int triangles_size = 0;
 };
 
+/// Specialization of BlockLayer clone just for MeshBlocks.
+template <>
+inline BlockLayer<MeshBlock>::BlockLayer(const BlockLayer& other,
+                                         MemoryType memory_type)
+    : BlockLayer(other.block_size_, memory_type) {
+  LOG(INFO) << "Deep copy of Mesh BlockLayer containing "
+            << other.numAllocatedBlocks() << " blocks.";
+  // Re-create all the blocks.
+  std::vector<Index3D> all_block_indices = other.getAllBlockIndices();
+
+  // Iterate over all blocks, clonin'.
+  for (const Index3D& block_index : all_block_indices) {
+    typename MeshBlock::ConstPtr block = other.getBlockAtIndex(block_index);
+    if (block == nullptr) {
+      continue;
+    }
+    blocks_.emplace(block_index,
+                    std::make_shared<MeshBlock>(*block, memory_type));
+  }
+}
+
 }  // namespace nvblox

nvblox/src/integrators/cuda/esdf_integrator.cu

@@ -125,79 +125,77 @@ __global__ void markAllSitesCombinedKernel(
     Index3D* updated_vec, int* updated_vec_size, Index3D* to_clear_vec,
     int* to_clear_vec_size) {
   dim3 voxel_index = threadIdx;
+  int block_idx = blockIdx.x;
 
   __shared__ TsdfBlock* tsdf_block;
   __shared__ EsdfBlock* esdf_block;
-  __shared__ bool updated, to_clear;
-  // This for loop allows us to have fewer threadblocks than there are
-  // blocks in this computation. We assume the threadblock size is constant
-  // though to make our lives easier.
-  for (int block_idx = blockIdx.x; block_idx < num_blocks;
-       block_idx += gridDim.x) {
-    if (threadIdx.x == 0 && threadIdx.y == 0 && threadIdx.z == 0) {
-      tsdf_block = nullptr;
-      esdf_block = nullptr;
-      updated = false;
-      to_clear = false;
-      auto tsdf_it = tsdf_block_hash.find(block_indices[block_idx]);
-      if (tsdf_it != tsdf_block_hash.end()) {
-        tsdf_block = tsdf_it->second;
-      }
-      auto esdf_it = esdf_block_hash.find(block_indices[block_idx]);
-      if (esdf_it != esdf_block_hash.end()) {
-        esdf_block = esdf_it->second;
-      }
+  __shared__ int updated;
+  __shared__ int to_clear;
+  __syncthreads();
+
+  if (threadIdx.x == 0 && threadIdx.y == 0 && threadIdx.z == 0) {
+    tsdf_block = nullptr;
+    esdf_block = nullptr;
+    updated = false;
+    to_clear = false;
+    auto tsdf_it = tsdf_block_hash.find(block_indices[block_idx]);
+    if (tsdf_it != tsdf_block_hash.end()) {
+      tsdf_block = tsdf_it->second;
     }
-    __syncthreads();
-    if (tsdf_block == nullptr || esdf_block == nullptr) {
-      continue;
+    auto esdf_it = esdf_block_hash.find(block_indices[block_idx]);
+    if (esdf_it != esdf_block_hash.end()) {
+      esdf_block = esdf_it->second;
     }
+  }
+  __syncthreads();
+  if (tsdf_block == nullptr || esdf_block == nullptr) {
+    return;
+  }
 
-    // Get the correct voxel for this index.
-    const TsdfVoxel* tsdf_voxel =
-        &tsdf_block->voxels[voxel_index.x][voxel_index.y][voxel_index.z];
-    EsdfVoxel* esdf_voxel =
-        &esdf_block->voxels[voxel_index.x][voxel_index.y][voxel_index.z];
-    if (tsdf_voxel->weight >= min_weight) {
-      // Mark as inside if the voxel distance is negative.
-      bool is_inside = tsdf_voxel->distance <= 0.0f;
-      if (esdf_voxel->is_inside && is_inside == false) {
+  // Get the correct voxel for this index.
+  const TsdfVoxel* tsdf_voxel =
+      &tsdf_block->voxels[voxel_index.x][voxel_index.y][voxel_index.z];
+  EsdfVoxel* esdf_voxel =
+      &esdf_block->voxels[voxel_index.x][voxel_index.y][voxel_index.z];
+  if (tsdf_voxel->weight >= min_weight) {
+    // Mark as inside if the voxel distance is negative.
+    bool is_inside = tsdf_voxel->distance <= 0.0f;
+    if (esdf_voxel->is_inside && is_inside == false) {
+      clearVoxelDevice(esdf_voxel, max_squared_distance_vox);
+      to_clear = true;
+    }
+    esdf_voxel->is_inside = is_inside;
+    if (is_inside && fabsf(tsdf_voxel->distance) <= max_site_distance_m) {
+      esdf_voxel->is_site = true;
+      esdf_voxel->squared_distance_vox = 0.0f;
+      esdf_voxel->parent_direction.setZero();
+      updated = true;
+    } else {
+      if (esdf_voxel->is_site) {
+        esdf_voxel->is_site = false;
+        // This voxel needs to be cleared.
+        clearVoxelDevice(esdf_voxel, max_squared_distance_vox);
+        to_clear = true;
+      } else if (!esdf_voxel->observed) {
+        // This is a brand new voxel.
+        clearVoxelDevice(esdf_voxel, max_squared_distance_vox);
+      } else if (esdf_voxel->squared_distance_vox <= 1e-4) {
+        // This is an invalid voxel that should be cleared.
         clearVoxelDevice(esdf_voxel, max_squared_distance_vox);
         to_clear = true;
       }
-      esdf_voxel->is_inside = is_inside;
-      if (is_inside && fabsf(tsdf_voxel->distance) <= max_site_distance_m) {
-        esdf_voxel->is_site = true;
-        esdf_voxel->squared_distance_vox = 0.0f;
-        esdf_voxel->parent_direction.setZero();
-        updated = true;
-      } else {
-        if (esdf_voxel->is_site) {
-          esdf_voxel->is_site = false;
-          // This voxel needs to be cleared.
-          clearVoxelDevice(esdf_voxel, max_squared_distance_vox);
-          to_clear = true;
-        } else if (!esdf_voxel->observed) {
-          // This is a brand new voxel.
-          clearVoxelDevice(esdf_voxel, max_squared_distance_vox);
-        } else if (esdf_voxel->squared_distance_vox <= 1e-4) {
-          // This is an invalid voxel that should be cleared.
-          clearVoxelDevice(esdf_voxel, max_squared_distance_vox);
-          to_clear = true;
-        }
-      }
-      esdf_voxel->observed = true;
     }
+    esdf_voxel->observed = true;
+  }
 
-    __syncthreads();
-    if (threadIdx.x == 0 && threadIdx.y == 0 && threadIdx.z == 0) {
-      if (updated) {
-        updated_vec[atomicAdd(updated_vec_size, 1)] = block_indices[block_idx];
-      }
-      if (to_clear) {
-        to_clear_vec[atomicAdd(to_clear_vec_size, 1)] =
-            block_indices[block_idx];
-      }
+  __syncthreads();
+
+  if (threadIdx.x == 1 && threadIdx.y == 1 && threadIdx.z == 1) {
+    if (updated) {
+      updated_vec[atomicAdd(updated_vec_size, 1)] = block_indices[block_idx];
+    }
+    if (to_clear) {
+      to_clear_vec[atomicAdd(to_clear_vec_size, 1)] = block_indices[block_idx];
     }
   }
 }
@@ -501,6 +499,10 @@ void EsdfIntegrator::markAllSitesCombined(
   CHECK_NOTNULL(esdf_layer);
   CHECK_NOTNULL(blocks_with_sites);
 
+  if (block_indices.empty()) {
+    return;
+  }
+
   // Caching.
   const float voxel_size = tsdf_layer.voxel_size();
   const float max_distance_vox = max_distance_m_ / voxel_size;
@@ -555,6 +557,10 @@ void EsdfIntegrator::markSitesInSliceCombined(
     float min_z, float max_z, float output_z, EsdfLayer* esdf_layer,
     device_vector<Index3D>* updated_blocks,
     device_vector<Index3D>* cleared_blocks) {
+  if (block_indices.empty()) {
+    return;
+  }
+
   // Caching.
   const float voxel_size = tsdf_layer.voxel_size();
   const float max_distance_vox = max_distance_m_ / voxel_size;
@@ -1023,6 +1029,10 @@ void EsdfIntegrator::sweepBlockBandCombined(
 void EsdfIntegrator::computeEsdfCombined(
     const device_vector<Index3D>& blocks_with_sites, EsdfLayer* esdf_layer) {
   CHECK_NOTNULL(esdf_layer);
+
+  if (blocks_with_sites.size() == 0) {
+    return;
+  }
   // Cache everything.
   constexpr int kVoxelsPerSide = VoxelBlock<bool>::kVoxelsPerSide;
   const float voxel_size = esdf_layer->block_size() / kVoxelsPerSide;
@@ -1077,7 +1087,7 @@ __global__ void clearAllInvalidKernel(
     Index3D* block_indices, Index3DDeviceHashMapType<EsdfBlock> block_hash,
     float max_squared_distance_vox, Index3D* output_vector, int* updated_size) {
   constexpr int kVoxelsPerSide = VoxelBlock<bool>::kVoxelsPerSide;
-  __shared__ bool block_updated;
+  __shared__ int block_updated;
   // Allow block size to be whatever.
   __shared__ EsdfBlock* block_ptr;
   // Get the current block for this... block.
@@ -1142,6 +1152,10 @@ __global__ void clearAllInvalidKernel(
 void EsdfIntegrator::clearAllInvalid(
     const std::vector<Index3D>& blocks_to_clear, EsdfLayer* esdf_layer,
     device_vector<Index3D>* updated_blocks) {
+  if (blocks_to_clear.size() == 0) {
+    return;
+  }
+
   // TODO: start out just getting all the blocks in the whole map.
   // Then replace with blocks within a radius of the cleared blocks.
   constexpr int kVoxelsPerSide = VoxelBlock<bool>::kVoxelsPerSide;

nvblox/src/mesh/mesh_block.cu

@@ -20,8 +20,17 @@ namespace nvblox {
 MeshBlock::MeshBlock(MemoryType memory_type)
     : vertices(memory_type),
       normals(memory_type),
-      triangles(memory_type),
-      colors(memory_type) {}
+      colors(memory_type),
+      triangles(memory_type) {}
+
+MeshBlock::MeshBlock(const MeshBlock& mesh_block)
+    : MeshBlock(mesh_block, mesh_block.vertices.memory_type()) {}
+
+MeshBlock::MeshBlock(const MeshBlock& mesh_block, MemoryType memory_type)
+    : vertices(mesh_block.vertices, memory_type),
+      normals(mesh_block.normals, memory_type),
+      colors(mesh_block.colors, memory_type),
+      triangles(mesh_block.triangles, memory_type) {}
 
 void MeshBlock::clear() {
   vertices.resize(0);
@@ -71,11 +80,6 @@ void MeshBlock::expandColorsToMatchVertices() {
   colors.resize(vertices.size());
 }
 
-void MeshBlock::expandIntensitiesToMatchVertices() {
-  intensities.reserve(vertices.capacity());
-  intensities.resize(vertices.size());
-}
-
 // Set the pointers to point to the mesh block.
 CudaMeshBlock::CudaMeshBlock(MeshBlock* block) {
   CHECK_NOTNULL(block);

nvblox/tests/include/nvblox/tests/utils.h

@@ -15,12 +15,16 @@ limitations under the License.
 */
 #pragma once
 
+#include <gflags/gflags.h>
+
 #include "nvblox/core/blox.h"
 #include "nvblox/core/color.h"
 #include "nvblox/core/layer.h"
 #include "nvblox/core/types.h"
 #include "nvblox/core/voxels.h"
 
+DECLARE_bool(nvblox_test_file_output);
+
 namespace nvblox {
 namespace test_utils {
 

nvblox/tests/lib/utils.cpp

@@ -21,6 +21,10 @@ limitations under the License.
 #include "nvblox/io/ply_writer.h"
 #include "nvblox/utils/timing.h"
 
+DEFINE_bool(
+    nvblox_test_file_output, false,
+    "Whether to output debug files from tests to disk or not. Off by default.");
+
 namespace nvblox {
 namespace test_utils {
 

nvblox/tests/test_camera.cpp

@@ -344,6 +344,30 @@ TEST(CameraTest, FrustumAtLeastOneValidVoxelTest) {
   EXPECT_LE(static_cast<float>(empty) / block_indices_in_frustum.size(), 0.03);
 }
 
+TEST(CameraTest, UnProjectionTest) {
+  Camera camera = getTestCamera();
+
+  constexpr int kNumPointsToTest = 1000;
+  for (int i = 0; i < kNumPointsToTest; i++) {
+    // Random point and depth
+    auto vector_image_point_pair = getRandomVisibleRayAndImagePoint(camera);
+    Vector2f u_C_in = vector_image_point_pair.second;
+    const float depth = test_utils::randomFloatInRange(0.1f, 10.0f);
+
+    // Unproject
+    const Vector3f p_C = camera.unprojectFromImagePlaneCoordinates(u_C_in, depth);
+    EXPECT_NEAR(p_C.z(), depth, kFloatEpsilon);
+
+    // Re-project  
+    Vector2f u_C_out;
+    EXPECT_TRUE(camera.project(p_C, &u_C_out));
+
+    // Check
+    EXPECT_NEAR(u_C_in.x(), u_C_out.x(), kFloatEpsilon);
+    EXPECT_NEAR(u_C_in.y(), u_C_out.y(), kFloatEpsilon);
+  }
+}
+
 int main(int argc, char** argv) {
   FLAGS_alsologtostderr = true;
   google::InitGoogleLogging(argv[0]);