nvidia-isaac · gogojjh · Oct 28, 2022 · Oct 28, 2022 · Oct 30, 2022 · Oct 31, 2022
diff --git a/.gitignore b/.gitignore
@@ -51,3 +51,6 @@
 *docs/_build
 *docs/doxyoutput
 *docs/api
+
+# test 
+test*
diff --git a/README.md b/README.md
@@ -1,3 +1,87 @@
+# Nvblox-modify
+
+#### Code Pipeline of NVBlox
+
+1. Data loader
+
+2. Frame integration
+
+   1. After loading data (the code API): Fuser::integrateFrame(const int frame_number)
+
+   2. RgbdMapper::integrateOSLidarDepth -> ProjectiveTsdfIntegrator::integrateFrame -> ProjectiveTsdfIntegrator::integrateFrameTemplate 
+
+      > * set <code>voxel_size</code> and <code>truncation_distance</code>
+      > * set <code>truncation_distance_m = truncation_distance_vox * voxel_size</code>
+      > * Identify blocks given the camera view: <code>view_calculator_.getBlocksInImageViewRaycast</code>
+      >   * <code>getBlocksByRaycastingPixels</code>: Raycasts through (possibly subsampled) pixels in the image, use the kernal function
+      >   * <code>*void* combinedBlockIndicesInImageKernel</code>: retrieve visiable block by raycasting voxels, done in GPU
+      >
+      >
+      > * TSDF integration given block indices: <code>integrateBlocksTemplate</code>
+      >
+      >   * <code>ProjectiveTsdfIntegrator::integrateBlocks</code>: block integration for the OSLidar, use the kernal function
+      >   * <code>integrateBlocksKernel</code>: TSDF integration for each block, done in GPU
+      >     * <code>projectThreadVoxel</code>: convert blocks' indices into coordinates, retrieve voxels from the block, and project them onto the image to check whether they are visible or not
+      >     * <code>interpolateOSLidarImage</code>: linear interpolation of depth images given float coordinates
+      >       * ```const Index2D u_M_rounded = u_px.array().round().cast<int>();```
+      >       * ```u_M_rounded.x() < 0 || u_M_rounded.y() < 0 || u_M_rounded.x() >= cols || u_M_rounded.y() >= rows)```: check bounds
+      >     * <code>updateVoxel</code>: update the TSDF values of all visible voxels. 
+
+3. Weight averaging methods
+    ```
+    Projective distance:
+        1: constant weight, truncate the fused_distance
+        2: constant weight, truncate the voxel_distance_measured
+        3: linear weight, truncate the voxel_distance_measured
+        4: exponential weight, truncate the voxel_distance_measured
+    Non-Projective distance:
+        5: weight and distance derived from VoxField
+        6: linear weight, distance derived from VoxField
+    ```
+
+3. Output data
+    1. Mesh map
+    2. ESDF map
+    3. Obstacle map: points from the ESDF map whose distance is smaller than a threshold
+
+4. Global planning test
+
+--------------------------
+### Demo with [KITTI](https://www.cvlibs.net/datasets/kitti) dataset
+
+1. Prepare data: 
+* Download test data
+
+    * [2011_09_30_drive_0027_sync](http://gofile.me/72EEc/NGdCJrzA5)
+
+2. Run the NVBlox
+
+    ```../script/run_fuse_kitti.sh```
+
+* [Experiments on NVBlox with the KITTI dataset](docs/experiments_kitti.md)
+
+--------------------------
+### Demo with the [FusionPortable](https://ram-lab.com/file/site/multi-sensor-dataset) dataset
+
+##### Demo 
+
+1. Download test data
+
+    * [20220226_campus_road_day](http://gofile.me/72EEc/MDghPwECu)
+
+3. Run the NVBlox: 
+
+    ```../script/run_fuse_fusionportable.sh```
+
+4. We can view the output mesh using the Open3D viewer.
+
+    ```python3 ../../visualization/visualize_mesh.py 20220216_garden_day_mesh.ply```
+
+* [Tricks to preprocess OSLiDAR points](docs/preprocess_OSLiDAR.md)
+* [Experiments on NVBlox and VDBMapping](docs/experiments_fusionportable.md)
+
+--------------------------
+--------------------------
 # nvblox
 Signed Distance Functions (SDFs) on NVIDIA GPUs.
 
@@ -49,6 +133,7 @@ cmake .. && make && cd tests && ctest
 ```
 
 ## Run an example
+
 In this example we fuse data from the [3DMatch dataset](https://3dmatch.cs.princeton.edu/). First let's grab the dataset. Here I'm downloading it to my dataset folder `~/dataset/3dmatch`.
 ```
 wget http://vision.princeton.edu/projects/2016/3DMatch/downloads/rgbd-datasets/sun3d-mit_76_studyroom-76-1studyroom2.zip -P ~/datasets/3dmatch
@@ -158,3 +243,7 @@ export OPENBLAS_CORETYPE=ARMV8
 
 # License
 This code is under an [open-source license](LICENSE) (Apache 2.0). :)
+
+# Reference
+[1] Parallel Banding Algorithm to Compute Exact Distance Transform with the GPU
+> compute EDT with the GPU
diff --git a/docs/.~experiments.md b/docs/.~experiments.md
@@ -0,0 +1,84 @@
+### Experimental Results
+
+##### Reconstruction results (voxel_size = 0.1)
+
+* Sequence: 20220216_garden_day, *2000* frames
+  * Projective distance
+    * NVBlox (constant weight, truncate fused_distance): 
+    Point cloud distance [m]: 0.0713986; Coverage [%]: 0.648376
+    * NVBlox (constant weight, truncate measured_distance): 
+    Point cloud distance [m]: 0.070609; Coverage [%]: 0.716344
+    * NVBlox (linear weight, truncate fused_distance): 
+    Point cloud distance [m]: 0.0687769; Coverage [%]: 0.73421
+    * NVBlox (exp weight, truncate fused_distance): 
+    Point cloud distance [m]: 0.0690283; Coverage [%]: 0.714388
+
+  * Non-Projective distance
+    * NVBlox (non-projective distance, truncate fused_distance): (distance_th = 10.0m)
+    Point cloud distance [m]: 0.0480275; Coverage [%]: 0.388869
+    * NVBlox (non-projective distance, truncate fused_distance): (distance_th = 30.0m)
+    Point cloud distance [m]: 0.058146; Coverage [%]: 0.631758
+    * NVBlox (non-projective distance, truncate fused_distance): (distance_th = 50.0m)
+    Point cloud distance [m]: 0.057562; Coverage [%]: 0.651013
+
+  * VDBMapping: 
+  Point cloud distance [m]: 0.074576; Coverage [%]: 0.724301
+
+* Sequence: 20220216_canteen_day, *2600* frames
+    * Non-Projective distance
+      * NVBlox (non-projective distance, truncate fused_distance): (distance_th = 70.0m)
+        Point cloud distance [m]: 0.102588; Coverage [%]: 0.602803
+
+* Sequence: 20220225_building_day, *2300* frames
+  * Non-Projective distance
+    * NVBlox (non-projective distance, truncate fused_distance): (distance_th = 70.0m)
+      Point cloud distance [m]: 0.0704632; Coverage [%]: 0.574235
+
+* Sequence: 20220216_escalator_day, *3200* frames
+  * Projective distance
+    * NVBlox (linear weight, truncate fused_distance): 
+    Point cloud distance [m]: 0.615201; Coverage [%]: 0.649969
+
+  * Non-Projective distance
+    * NVBlox (non-projective distance, truncate fused_distance): (distance_th = 70.0m)
+      Point cloud distance [m]: 0.0526527; Coverage [%]: 0.629851
+    * NVBlox (non-projective distance, linear weight, truncate fused_distance): (distance_th = 70.0m)
+      Point cloud distance [m]: 0.0602039; Coverage [%]: 0.641247
+
+##### Computation time (voxel_size = 0.1)
+* Sequence: 20220216_garden_day
+  * Normal computation with a GPU: 0.7ms per frame
+  * NVBLox: 14.2ms per frame (total 2000)
+  * VDBMapping: 384.062 ms per frame (total 2500)
+
+* Sequence: 20220216_canteen_day
+  * Normal computation with a GPU: 0.01ms per frame
+  * NVBLox: 3ms per frame (total 2600)
+
+* Sequence: 20220226_campus_road_day
+  * NVBLox: 5ms per frame (total 2000)
+
+#### Appendix
+
+**Reconstruction results of NVBLox on 20220216_garden_day (voxel_size = 0.1)**
+<p align="center">
+	<center><img src="images/20220216_garden_day_mesh_img.png" width="450" /></center>
+  <br>
+  <center><img src="images/20220216_garden_day_mesh_img_eval_error.png" width="450" /></center>
+</p>
+
+**Reconstruction results of NVBLox on 20220225_building_day (voxel_size = 0.1)**
+<p align="center">
+	<center><img src="images/20220225_building_day_mesh_img.png" width="450" /></center>
+  <br>
+  <center><img src="images/20220225_building_day_mesh_img_eval_error.png" width="450" /></center>
+</p>
+
+**Reconstruction results of NVBLox on other sequences (voxel_size = 0.1)**
+<p align="center">
+	<center><img src="images/20220216_escalator_day.png" width="450" /></center>
+  <br>
+  <center><img src="images/20220226_campus_road_day.png" width="450" /></center>
+</p>
+
+
diff --git a/docs/.~experiments_kitti.md b/docs/.~experiments_kitti.md
@@ -0,0 +1,12 @@
+### Experimental Results
+
+#### Appendix
+
+**Reconstruction results of NVBLox on 2011_09_30_drive_0027_sync (voxel_size = 0.1)**
+<p align="center">
+  <center><img src="images/2011_09_30_drive_0027_sync_mesh_closeview.png" width="450" /></center>
+  <br>
+	<center><img src="images/2011_09_30_drive_0027_sync_mesh.png" width="450" /></center>
+  <br>
+	<center><img src="images/kitti_global_path.png" width="450" /></center>       
+</p>
diff --git a/docs/code_review_voxfield_panmap.md b/docs/code_review_voxfield_panmap.md
@@ -0,0 +1,14 @@
+## VoxField
+
+
+
+## Panmap
+
+Process semantic point cloud
+
+```c++
+conversions.h - inline void convertPointcloud
+1. define Label
+2. give label specific definitions
+```
+
diff --git a/docs/experiments_fusionportable.md b/docs/experiments_fusionportable.md
@@ -0,0 +1,89 @@
+## Experiments
+
+### Reconstruction results
+
+###### Weight averaging methods (TSDF integration):
+```
+Projective distance:
+  1: constant weight, truncate the fused_distance
+  2: constant weight, truncate the voxel_distance_measured
+  3: linear weight, truncate the voxel_distance_measured
+  4: exponential weight, truncate the voxel_distance_measured
+Non-Projective distance:
+  5: weight and distance derived from VoxField
+  6: linear weight, distance derived from VoxField
+```
+
+###### Weight averaging methods (Color integration):
+```
+  1: constant weight, truncate the voxel_distance_measured
+  2: linear weight, truncate the voxel_distance_measured
+  3: exponential weight, truncate the voxel_distance_measured
+  4: sensor distance weight
+  5: linear weight * sensor distance weight
+```
+
+###### Scene reconstruction results (voxel_size = 0.1)
+| Sequence | Algorithm | Method | Point cloud distance | Coverage [%] |
+| :------- | :-------- | :----- | :------------------- | :------------|
+| 20220216_garden_day (2000) | NVBlox | 1 | 0.0713986  | 0.648376 |
+| 20220216_garden_day (2000) | NVBlox | 2 | 0.070609   | 0.716344 |
+| 20220216_garden_day (2000) | NVBlox | 3 | 0.0687769  | 0.73421  |
+| 20220216_garden_day (2000) | NVBlox | 4 | 0.0690283  | 0.714388 |
+| 20220216_garden_day (2000) | NVBlox | 5 | 0.0480275  | 0.388869 |
+| 20220216_garden_day (2000) | NVBlox | 5 | 0.058146   | 0.631758 |
+| 20220216_garden_day (2000) | NVBlox | 5 | 0.057562   | 0.651013 |
+| 20220216_garden_day (2000) | NVBlox | 6 | 0.0607844  | 0.689476 |
+| 20220216_garden_day (2000) | VDBMapping | 3 | 0.074576 | 0.724301 |
+| 20220216_canteen_day (2600)   | NVBlox | 5 (70m)  | 0.102588  | 0.602803 |
+| 20220225_building_day (2300)  | NVBlox | 5 (70m)  | 0.0704632 | 0.574235 |
+| 20220216_escalator_day (3200) | NVBlox | 3             | 0.615201 | 0.649969  |
+| 20220216_escalator_day (3200) | NVBlox | 5 (70m)  | 0.0526527 | 0.629851 |
+| 20220216_escalator_day (3200) | NVBlox | 6 (50m)  | 0.0585957 | 0.641295 |
+
+###### Computation time (voxel_size = 0.1)
+| Sequence | Algorithm/ Module | Time per frame |
+| :------- | :-------- | :----- | 
+| 20220216_garden_day | Normal computation | 0.7ms |
+| 20220216_garden_day | NVBLox | 14.2ms (2000) |
+| 20220216_garden_day | VDBMapping | 384.062ms (2500) |
+| 20220216_canteen_day | Normal computation | 0.01ms |
+| 20220216_canteen_day | NVBLox | 3ms (2600) |
+| 20220226_campus_road_day | NVBLox | 5ms (2000) |
+
+###### Figures
+
+Reconstruction of NVBLox on 20220216_garden_day (voxel_size = 0.1)
+<p align="center">
+	<center><img src="images/20220216_garden_day_mesh_img.png" width="40%" /></center>
+  <br>
+  <center><img src="images/20220216_garden_day_mesh_img_eval_error.png" width="40%" /></center>
+</p>
+
+Reconstruction of NVBLox on 20220225_building_day (voxel_size = 0.1)
+<p align="center">
+	<center><img src="images/20220225_building_day_mesh_img.png" width="40%" /></center>
+  <br>
+  <center><img src="images/20220225_building_day_mesh_img_eval_error.png" width="40%" /></center>
+</p>
+
+Reconstruction of NVBLox on other sequences (voxel_size = 0.1)
+<p align="center">
+	<center><img src="images/20220216_escalator_day_mesh.png" width="40%" /></center>
+  <br>
+  <center><img src="images/20220226_campus_road_day_mesh.png" width="40%" /></center>
+</p>
+
+Obstacle information (voxel_size = 0.1)
+<p align="center">
+  <center><img src="images/20220226_campus_road_day_obs.png" width="40%" /></center>
+</p>
+
+Reconstruction of NVBLox on 20221126_lab_static (voxel_size = 0.05) with dynamic objcts
+<p align="center">
+  <center><img src="images/20221126_lab_static_1.png" width="40%" />100 frames</center>
+  <br>
+  <center><img src="images/20221126_lab_static_2.png" width="40%" />150 frames</center>
+  <br>
+  <center><img src="images/20221126_lab_static_3.png" width="40%" />600 frames</center> 
+</p>
diff --git a/docs/experiments_kitti.md b/docs/experiments_kitti.md
@@ -0,0 +1,15 @@
+### Experimental Results
+
+#### Appendix
+
+**Reconstruction results of NVBLox on 2011_09_30_drive_0027_sync (voxel_size = 0.1)**
+<p align="center">
+  <center><img src="images/2011_09_30_drive_0027_sync_mesh_closeview.png" width="450" /></center>
+  <br>
+	<center><img src="images/2011_09_30_drive_0027_sync_mesh.png" width="450" /></center>
+  <br>
+	<center><img src="images/kitti_global_path.png" width="450" /></center>
+  <br>
+	<center><img src="images/kitti_color_mesh.png" width="450" /></center>
+</p>
+
diff --git a/docs/images/2011_09_30_drive_0027_sync_mesh.png b/docs/images/2011_09_30_drive_0027_sync_mesh.png
diff --git a/docs/images/2011_09_30_drive_0027_sync_mesh_closeview.png b/docs/images/2011_09_30_drive_0027_sync_mesh_closeview.png
diff --git a/docs/images/20220216_escalator_day_mesh.png b/docs/images/20220216_escalator_day_mesh.png
diff --git a/docs/images/20220216_garden_day_mesh_img.png b/docs/images/20220216_garden_day_mesh_img.png
diff --git a/docs/images/20220216_garden_day_mesh_img_eval_error.png b/docs/images/20220216_garden_day_mesh_img_eval_error.png
diff --git a/docs/images/20220225_building_day_mesh_img.png b/docs/images/20220225_building_day_mesh_img.png
diff --git a/docs/images/20220225_building_day_mesh_img_eval_error.png b/docs/images/20220225_building_day_mesh_img_eval_error.png
diff --git a/docs/images/20220226_campus_road_day_mesh.png b/docs/images/20220226_campus_road_day_mesh.png
diff --git a/docs/images/20220226_campus_road_day_obs.png b/docs/images/20220226_campus_road_day_obs.png
diff --git a/docs/images/20221126_lab_static_1.png b/docs/images/20221126_lab_static_1.png
diff --git a/docs/images/20221126_lab_static_2.png b/docs/images/20221126_lab_static_2.png
diff --git a/docs/images/20221126_lab_static_3.png b/docs/images/20221126_lab_static_3.png
diff --git a/docs/images/kitti_color_mesh.png b/docs/images/kitti_color_mesh.png
diff --git a/docs/images/kitti_global_path.png b/docs/images/kitti_global_path.png
diff --git a/docs/images/ouster_angle_after_filter.jpg b/docs/images/ouster_angle_after_filter.jpg
diff --git a/docs/images/ouster_angle_before_filter.jpg b/docs/images/ouster_angle_before_filter.jpg
diff --git a/docs/images/voxfield_result.png b/docs/images/voxfield_result.png
diff --git a/docs/preprocess_OSLiDAR.md b/docs/preprocess_OSLiDAR.md
@@ -0,0 +1,67 @@
+### Tricks to preprocessing OSLiDAR points
+
+### Explanation
+
+The perfect intrinsic model of a LiDAR should be (like VLP, Hesai):
+
+* The elevation angle (angle between ray and +z) of each line increases evenly
+* The azimuth angle of each point at one line (2pi - angle between ray and +x) increases evenly
+
+But the intrinsics of the OSLiDAR are unstable. This means that the conversion between a point cloud and a range image is not lossless. Especially, near points may have large noise. We analyze the angle characters of OSLiDAR:
+
+<p align="center">
+	<center><img src="images/ouster_angle_before_filter.jpg" width="70%" /></center><br>
+</p>
+
+
+We need to preprocess the OSLiDAR points. For each line, we only keep points if their length is within <code>[scan_min, scan_max]</code>. And then we compute the mean elevation angle of the first line and last line as the starting and ending elevation angle. We analyze the angle characters of OSLiDAR with filtered points:
+
+<p align="center">
+	<center><img src="images/ouster_angle_after_filter.jpg" width="70%" /></center>
+</p>
+### Code to generate range images from point clouds of OSLiDAR
+
+```c++
+cv::Mat depth_img(num_elevation_divisions, num_azimuth_divisions, 
+                  CV_16UC1, cv::Scalar(0));
+for (const auto &pt : input_cloud) {
+	float r = sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z);
+  	if (r <= SCAN_MIN || r >= SCAN_MAX) continue;
+  	float elevation_angle_rad = acos(pt.z / r);
+  	float azimuth_angle_rad = M_PI - atan2(pt.y, pt.x);
+  	int row_id = round((elevation_angle_rad - start_elevation_rad) /
+     	                rads_per_pixel_elevation);
+  	if (row_id < 0 || row_id > num_azimuth_divisions - 1) continue;
+  	int col_id = round(azimuth_angle_rad / rads_per_pixel_azimuth);
+  	if (col_id >= 2048) col_id -= 2048;
+  	float dep = r * default_scale_factor;
+  	if (dep > std::numeric_limits<uint16_t>::max()) continue;
+  	if (dep < 0.0f) continue;
+  	depth_img.at<uint16_t>(row_id, col_id) = uint16_t(dep);
+}
+```
+
+### Code to generate point clouds from range images for OSLiDAR
+
+```c++
+pcl::PointCloud<pcl::PointXYZ> output_cloud;
+for (size_t row_id = 0; row_id < depth_img.rows; row_id++) {
+    for (size_t col_id = 0; col_id < height_img.cols; col_id++) {
+        float dep = depth_img.at<float>(row_id, col_id);
+        float elevation_angle_rad =
+            row_id * rads_per_pixel_elevation + start_elevation_rad;
+        float z = height_img.at<float>(row_id, col_id);
+        float r = sqrt(dep * dep - z * z);
+        float azimuth_angle_rad = M_PI - float(col_id) * rads_per_pixel_azimuth;
+        float x = r * cos(azimuth_angle_rad);
+        float y = r * sin(azimuth_angle_rad);
+        pcl::PointXYZ pt;
+        pt.x = x;
+        pt.y = y;
+        pt.z = z;
+        output_cloud.push_back(pt);
+    }
+}
+
+```
+