diff --git a/.gitignore b/.gitignore index eff634ad2..b2f6230d3 100644 --- a/.gitignore +++ b/.gitignore @@ -51,3 +51,6 @@ *docs/_build *docs/doxyoutput *docs/api + +# test +test* diff --git a/README.md b/README.md index b2b84c112..d80e99024 100644 --- 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 voxel_size and truncation_distance + > * set truncation_distance_m = truncation_distance_vox * voxel_size + > * Identify blocks given the camera view: view_calculator_.getBlocksInImageViewRaycast + > * getBlocksByRaycastingPixels: Raycasts through (possibly subsampled) pixels in the image, use the kernal function + > * *void* combinedBlockIndicesInImageKernel: retrieve visiable block by raycasting voxels, done in GPU + > + > + > * TSDF integration given block indices: integrateBlocksTemplate + > + > * ProjectiveTsdfIntegrator::integrateBlocks: block integration for the OSLidar, use the kernal function + > * integrateBlocksKernel: TSDF integration for each block, done in GPU + > * projectThreadVoxel: convert blocks' indices into coordinates, retrieve voxels from the block, and project them onto the image to check whether they are visible or not + > * interpolateOSLidarImage: linear interpolation of depth images given float coordinates + > * ```const Index2D u_M_rounded = u_px.array().round().cast();``` + > * ```u_M_rounded.x() < 0 || u_M_rounded.y() < 0 || u_M_rounded.x() >= cols || u_M_rounded.y() >= rows)```: check bounds + > * updateVoxel: 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 \ No newline at end of file diff --git a/docs/.~experiments.md b/docs/.~experiments.md new file mode 100644 index 000000000..5d5b8c025 --- /dev/null +++ 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)** +

+

+
+
+

+ +**Reconstruction results of NVBLox on 20220225_building_day (voxel_size = 0.1)** +

+

+
+
+

+ +**Reconstruction results of NVBLox on other sequences (voxel_size = 0.1)** +

+

+
+
+

+ + diff --git a/docs/.~experiments_kitti.md b/docs/.~experiments_kitti.md new file mode 100644 index 000000000..88d268711 --- /dev/null +++ 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)** +

+

+
+
+
+
+

diff --git a/docs/code_review_voxfield_panmap.md b/docs/code_review_voxfield_panmap.md new file mode 100755 index 000000000..fd1d4a705 --- /dev/null +++ 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 new file mode 100755 index 000000000..65ab7b679 --- /dev/null +++ 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) +

+

+
+
+

+ +Reconstruction of NVBLox on 20220225_building_day (voxel_size = 0.1) +

+

+
+
+

+ +Reconstruction of NVBLox on other sequences (voxel_size = 0.1) +

+

+
+
+

+ +Obstacle information (voxel_size = 0.1) +

+

+

+ +Reconstruction of NVBLox on 20221126_lab_static (voxel_size = 0.05) with dynamic objcts +

+

100 frames
+
+
150 frames
+
+
600 frames
+

\ No newline at end of file diff --git a/docs/experiments_kitti.md b/docs/experiments_kitti.md new file mode 100755 index 000000000..c0a2c7488 --- /dev/null +++ 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)** +

+

+
+
+
+
+
+
+

+ diff --git a/docs/images/2011_09_30_drive_0027_sync_mesh.png b/docs/images/2011_09_30_drive_0027_sync_mesh.png new file mode 100644 index 000000000..7b53831da Binary files /dev/null and b/docs/images/2011_09_30_drive_0027_sync_mesh.png differ 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 new file mode 100644 index 000000000..da841b0c7 Binary files /dev/null and b/docs/images/2011_09_30_drive_0027_sync_mesh_closeview.png differ diff --git a/docs/images/20220216_escalator_day_mesh.png b/docs/images/20220216_escalator_day_mesh.png new file mode 100644 index 000000000..5a9345902 Binary files /dev/null and b/docs/images/20220216_escalator_day_mesh.png differ diff --git a/docs/images/20220216_garden_day_mesh_img.png b/docs/images/20220216_garden_day_mesh_img.png new file mode 100644 index 000000000..6012319b3 Binary files /dev/null and b/docs/images/20220216_garden_day_mesh_img.png differ diff --git a/docs/images/20220216_garden_day_mesh_img_eval_error.png b/docs/images/20220216_garden_day_mesh_img_eval_error.png new file mode 100644 index 000000000..f847e892f Binary files /dev/null and b/docs/images/20220216_garden_day_mesh_img_eval_error.png differ diff --git a/docs/images/20220225_building_day_mesh_img.png b/docs/images/20220225_building_day_mesh_img.png new file mode 100644 index 000000000..d47d7a702 Binary files /dev/null and b/docs/images/20220225_building_day_mesh_img.png differ diff --git a/docs/images/20220225_building_day_mesh_img_eval_error.png b/docs/images/20220225_building_day_mesh_img_eval_error.png new file mode 100644 index 000000000..2d3b198e8 Binary files /dev/null and b/docs/images/20220225_building_day_mesh_img_eval_error.png differ diff --git a/docs/images/20220226_campus_road_day_mesh.png b/docs/images/20220226_campus_road_day_mesh.png new file mode 100644 index 000000000..372cfbf83 Binary files /dev/null and b/docs/images/20220226_campus_road_day_mesh.png differ diff --git a/docs/images/20220226_campus_road_day_obs.png b/docs/images/20220226_campus_road_day_obs.png new file mode 100644 index 000000000..1bae49049 Binary files /dev/null and b/docs/images/20220226_campus_road_day_obs.png differ diff --git a/docs/images/20221126_lab_static_1.png b/docs/images/20221126_lab_static_1.png new file mode 100644 index 000000000..b7569bc2f Binary files /dev/null and b/docs/images/20221126_lab_static_1.png differ diff --git a/docs/images/20221126_lab_static_2.png b/docs/images/20221126_lab_static_2.png new file mode 100644 index 000000000..92775c984 Binary files /dev/null and b/docs/images/20221126_lab_static_2.png differ diff --git a/docs/images/20221126_lab_static_3.png b/docs/images/20221126_lab_static_3.png new file mode 100644 index 000000000..0d5973b57 Binary files /dev/null and b/docs/images/20221126_lab_static_3.png differ diff --git a/docs/images/kitti_color_mesh.png b/docs/images/kitti_color_mesh.png new file mode 100644 index 000000000..440a990c4 Binary files /dev/null and b/docs/images/kitti_color_mesh.png differ diff --git a/docs/images/kitti_global_path.png b/docs/images/kitti_global_path.png new file mode 100644 index 000000000..6e11fcc33 Binary files /dev/null and b/docs/images/kitti_global_path.png differ diff --git a/docs/images/ouster_angle_after_filter.jpg b/docs/images/ouster_angle_after_filter.jpg new file mode 100644 index 000000000..0cacd3b3c Binary files /dev/null and b/docs/images/ouster_angle_after_filter.jpg differ diff --git a/docs/images/ouster_angle_before_filter.jpg b/docs/images/ouster_angle_before_filter.jpg new file mode 100644 index 000000000..d0fafe92d Binary files /dev/null and b/docs/images/ouster_angle_before_filter.jpg differ diff --git a/docs/images/voxfield_result.png b/docs/images/voxfield_result.png new file mode 100644 index 000000000..1847ea20f Binary files /dev/null and b/docs/images/voxfield_result.png differ diff --git a/docs/preprocess_OSLiDAR.md b/docs/preprocess_OSLiDAR.md new file mode 100755 index 000000000..05c6e11f1 --- /dev/null +++ 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: + +

+


+

+ + +We need to preprocess the OSLiDAR points. For each line, we only keep points if their length is within [scan_min, scan_max]. 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: + +

+

+

+### 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::max()) continue; + if (dep < 0.0f) continue; + depth_img.at(row_id, col_id) = uint16_t(dep); +} +``` + +### Code to generate point clouds from range images for OSLiDAR + +```c++ +pcl::PointCloud 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(row_id, col_id); + float elevation_angle_rad = + row_id * rads_per_pixel_elevation + start_elevation_rad; + float z = height_img.at(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); + } +} + +``` + diff --git a/nvblox/CMakeLists.txt b/nvblox/CMakeLists.txt index fab37aa34..db587344c 100644 --- a/nvblox/CMakeLists.txt +++ b/nvblox/CMakeLists.txt @@ -100,10 +100,13 @@ add_library(nvblox_lib SHARED src/core/bounding_boxes.cpp src/core/bounding_spheres.cpp src/core/camera.cpp + src/core/camera_pinhole.cpp + src/core/frustum.cpp src/core/color.cpp src/core/cuda/blox.cu src/core/cuda/image_cuda.cu src/core/cuda/warmup.cu + src/core/cuda/image_operation.cu src/core/image.cpp src/core/interpolation_3d.cpp src/core/mapper.cpp @@ -201,6 +204,7 @@ target_link_libraries(nvblox_interface INTERFACE nvblox_lib nvblox_gpu_hash nvblox_cuda_check + nvblox_datasets ${GLOG_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen diff --git a/nvblox/executables/CMakeLists.txt b/nvblox/executables/CMakeLists.txt index 23cf07a9d..ccdeba211 100644 --- a/nvblox/executables/CMakeLists.txt +++ b/nvblox/executables/CMakeLists.txt @@ -1,33 +1,68 @@ +include_directories(include) + # Datasets library add_library(nvblox_datasets SHARED - src/datasets/3dmatch.cpp - src/datasets/image_loader.cpp - src/datasets/replica.cpp - src/fuser.cpp + src/datasets/image_loader.cpp ) target_include_directories(nvblox_datasets PUBLIC - $ - $ + $ + $ ) target_link_libraries(nvblox_datasets nvblox_lib) set_target_properties(nvblox_datasets PROPERTIES CUDA_SEPARABLE_COMPILATION ON) +#################################### +#### NOTE(gogojjh): compile libraries for datasets +add_library(nvblox_datasets_3dmatch SHARED + src/datasets/3dmatch.cpp + src/fuser_rgbd.cpp +) +target_link_libraries(nvblox_datasets_3dmatch nvblox_lib nvblox_datasets) -# 3Dmatch executable -add_executable(fuse_3dmatch - src/fuse_3dmatch.cpp +add_library(nvblox_datasets_replica SHARED + src/datasets/replica.cpp + src/fuser_rgbd.cpp +) +target_link_libraries(nvblox_datasets_replica nvblox_lib nvblox_datasets) + +add_library(nvblox_datasets_fusionportable SHARED + src/datasets/fusionportable.cpp + src/fuser_lidar.cpp +) +target_link_libraries(nvblox_datasets_fusionportable nvblox_lib nvblox_datasets) + +add_library(nvblox_datasets_kitti SHARED + src/datasets/kitti.cpp + src/fuser_lidar.cpp ) +target_link_libraries(nvblox_datasets_kitti nvblox_lib nvblox_datasets) + +#################################### +# 3Dmatch executable +add_executable(fuse_3dmatch src/fuse_3dmatch.cpp) target_link_libraries(fuse_3dmatch - nvblox_lib nvblox_datasets + nvblox_lib nvblox_datasets nvblox_datasets_3dmatch ) set_target_properties(fuse_3dmatch PROPERTIES CUDA_SEPARABLE_COMPILATION ON) # Replica executable -add_executable(fuse_replica - src/fuse_replica.cpp -) +add_executable(fuse_replica src/fuse_replica.cpp) target_link_libraries(fuse_replica - nvblox_lib nvblox_datasets + nvblox_lib nvblox_datasets nvblox_datasets_replica ) set_target_properties(fuse_replica PROPERTIES CUDA_SEPARABLE_COMPILATION ON) + +# FusionPortable executable +add_executable(fuse_fusionportable src/fuse_fusionportable.cpp) +target_link_libraries(fuse_fusionportable + nvblox_lib nvblox_datasets nvblox_datasets_fusionportable +) +set_target_properties(fuse_fusionportable PROPERTIES CUDA_SEPARABLE_COMPILATION ON) + +# KITTI executable +add_executable(fuse_kitti src/fuse_kitti.cpp) +target_link_libraries(fuse_kitti + nvblox_lib nvblox_datasets nvblox_datasets_kitti +) +set_target_properties(fuse_kitti PROPERTIES CUDA_SEPARABLE_COMPILATION ON) diff --git a/nvblox/executables/include/nvblox/datasets/3dmatch.h b/nvblox/executables/include/nvblox/datasets/3dmatch.h index dd68918d5..2b36966e7 100644 --- a/nvblox/executables/include/nvblox/datasets/3dmatch.h +++ b/nvblox/executables/include/nvblox/datasets/3dmatch.h @@ -21,21 +21,21 @@ limitations under the License. #include "nvblox/core/types.h" #include "nvblox/datasets/data_loader.h" #include "nvblox/datasets/image_loader.h" -#include "nvblox/executables/fuser.h" +#include "nvblox/executables/fuser_rgbd.h" namespace nvblox { namespace datasets { namespace threedmatch { // Build a Fuser for the 3DMatch dataset -std::unique_ptr createFuser(const std::string base_path, - const int seq_id); +std::unique_ptr createFuser(const std::string base_path, + const int seq_id); ///@brief A class for loading 3DMatch data class DataLoader : public RgbdDataLoaderInterface { public: DataLoader(const std::string& base_path, const int seq_id, - bool multithreaded = true); + bool multithreaded = true); /// Interface for a function that loads the next frames in a dataset ///@param[out] depth_frame_ptr The loaded depth frame. @@ -48,6 +48,21 @@ class DataLoader : public RgbdDataLoaderInterface { Camera* camera_ptr, // NOLINT ColorImage* color_frame_ptr = nullptr) override; + /// Interface for a function that loads the next frames in a dataset + ///@param[out] depth_frame_ptr The loaded depth frame. + ///@param[out] T_L_C_ptr Transform from Camera to the Layer frame. + ///@param[out] camera_ptr The intrinsic camera model. + ///@param[out] lidar_ptr The intrinsic oslidar model. + ///@param[out] height_frame_ptr The loaded z frame. + ///@param[out] color_frame_ptr Optional, load color frame. + ///@return Whether loading succeeded. + DataLoadResult loadNext(DepthImage* depth_frame_ptr, // NOLINT + Transform* T_L_C_ptr, // NOLINT + CameraPinhole* camera_ptr, // NOLINT + OSLidar* lidar_ptr, // NOLINT + DepthImage* height_frame_ptr, // NOLINT + ColorImage* color_frame_ptr = nullptr) override; + protected: const std::string base_path_; const int seq_id_; diff --git a/nvblox/executables/include/nvblox/datasets/data_loader.h b/nvblox/executables/include/nvblox/datasets/data_loader.h index ce3492d70..3b747eba6 100644 --- a/nvblox/executables/include/nvblox/datasets/data_loader.h +++ b/nvblox/executables/include/nvblox/datasets/data_loader.h @@ -16,7 +16,10 @@ limitations under the License. #pragma once #include "nvblox/core/camera.h" +#include "nvblox/core/camera_pinhole.h" #include "nvblox/core/image.h" +#include "nvblox/core/lidar.h" +#include "nvblox/core/oslidar.h" #include "nvblox/core/types.h" #include "nvblox/datasets/image_loader.h" @@ -27,10 +30,12 @@ enum class DataLoadResult { kSuccess, kBadFrame, kNoMoreData }; class RgbdDataLoaderInterface { public: - RgbdDataLoaderInterface(std::unique_ptr>&& depth_image_loader, - std::unique_ptr>&& color_image_loader) + RgbdDataLoaderInterface( + std::unique_ptr>&& depth_image_loader, + std::unique_ptr>&& color_image_loader) : depth_image_loader_(std::move(depth_image_loader)), color_image_loader_(std::move(color_image_loader)) {} + virtual ~RgbdDataLoaderInterface() = default; /// Interface for a function that loads the next frames in a dataset @@ -44,6 +49,22 @@ class RgbdDataLoaderInterface { Camera* camera_ptr, // NOLINT ColorImage* color_frame_ptr = nullptr) = 0; + /// Interface for a function that loads the next frames in a dataset + ///@param[out] depth_frame_ptr The loaded depth frame. + ///@param[out] T_L_C_ptr Transform from Camera to the Layer frame. + ///@param[out] camera_ptr The intrinsic camera model. + ///@param[out] lidar_ptr The intrinsic Ouster lidar model. + ///@param[out] height_frame_ptr The loaded z frame. + ///@param[out] color_frame_ptr Optional, load color frame. + ///@return Whether loading succeeded. + virtual DataLoadResult loadNext( + DepthImage* depth_frame_ptr, // NOLINT + Transform* T_L_C_ptr, // NOLINT + CameraPinhole* camera_ptr, // NOLINT + OSLidar* lidar_ptr, // NOLINT + DepthImage* height_frame_ptr = nullptr, // NOLINT + ColorImage* color_frame_ptr = nullptr) = 0; + protected: // Objects which do (multithreaded) image loading. std::unique_ptr> depth_image_loader_; diff --git a/nvblox/executables/include/nvblox/datasets/fusionportable.h b/nvblox/executables/include/nvblox/datasets/fusionportable.h new file mode 100644 index 000000000..83fc428f3 --- /dev/null +++ b/nvblox/executables/include/nvblox/datasets/fusionportable.h @@ -0,0 +1,108 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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 +#include + +#include "nvblox/core/types.h" +#include "nvblox/datasets/data_loader.h" +#include "nvblox/datasets/image_loader.h" +#include "nvblox/executables/fuser_lidar.h" + +namespace nvblox { +namespace datasets { +namespace fusionportable { + +// NOTE(gogojjh): the default settings of preprocess height_image +// offset +constexpr float kDefaultUintDepthScaleFactor = 1.0f / 1000.0f; +constexpr float kDefaultUintDepthScaleOffset = 10.0f; + +// Build a FuserLidar for the FusionPortable dataset +std::unique_ptr createFuser(const std::string base_path, + const int seq_id); + +///@brief A class for loading FusionPortable data +class DataLoader : public RgbdDataLoaderInterface { + public: + DataLoader(const std::string& base_path, const int seq_id, + bool multithreaded = true); + + DataLoadResult loadNext(DepthImage* depth_frame_ptr, // NOLINT + Transform* T_L_C_ptr, // NOLINT + Camera* camera_ptr, // NOLINT + ColorImage* color_frame_ptr = nullptr) override; + + /// Interface for a function that loads the next frames in a dataset + ///@param[out] depth_frame_ptr The loaded depth frame. + ///@param[out] T_L_C_ptr Transform from Camera to the Layer frame. + ///@param[out] camera_ptr The intrinsic camera model. + ///@param[out] lidar_ptr The intrinsic oslidar model. + ///@param[out] height_frame_ptr The loaded z frame. + ///@param[out] color_frame_ptr Optional, load color frame. + ///@return Whether loading succeeded. + DataLoadResult loadNext(DepthImage* depth_frame_ptr, // NOLINT + Transform* T_L_C_ptr, // NOLINT + CameraPinhole* camera_ptr, // NOLINT + OSLidar* lidar_ptr, // NOLINT + DepthImage* height_frame_ptr = nullptr, // NOLINT + ColorImage* color_frame_ptr = nullptr) override; + + protected: + std::unique_ptr> height_image_loader_; + + const std::string base_path_; + const int seq_id_; + + // The next frame to be loaded + int frame_number_ = 0; +}; + +namespace internal { + +bool parsePoseFromFile(const std::string& filename, Transform* transform); +bool parseCameraFromFile(const std::string& filename, + Eigen::Matrix3f* intrinsics); +bool parseLidarFromFile(const std::string& filename, + Eigen::Matrix* intrinsics); +std::string getPathForCameraIntrinsics(const std::string& base_path); +std::string getPathForLidarIntrinsics(const std::string& base_path, + const int seq_id, const int frame_id); +std::string getPathForFramePose(const std::string& base_path, const int seq_id, + const int frame_id); + +std::string getPathForDepthImage(const std::string& base_path, const int seq_id, + const int frame_id); +std::string getPathForHeightImage(const std::string& base_path, + const int seq_id, const int frame_id); +std::string getPathForColorImage(const std::string& base_path, const int seq_id, + const int frame_id); + +std::unique_ptr> createDepthImageLoader( + const std::string& base_path, const int seq_id, + const bool multithreaded = true); +std::unique_ptr> createHeightImageLoader( + const std::string& base_path, const int seq_id, + const bool multithreaded = true); +std::unique_ptr> createColorImageLoader( + const std::string& base_path, const int seq_id, + const bool multithreaded = true); + +} // namespace internal +} // namespace fusionportable +} // namespace datasets +} // namespace nvblox diff --git a/nvblox/executables/include/nvblox/datasets/image_loader.h b/nvblox/executables/include/nvblox/datasets/image_loader.h index d90b625d7..fde4f0aa0 100644 --- a/nvblox/executables/include/nvblox/datasets/image_loader.h +++ b/nvblox/executables/include/nvblox/datasets/image_loader.h @@ -29,10 +29,14 @@ namespace datasets { // depth is expressed in mm, and we converts it to a float with meters (through // a multiplication with 1.0/1000.0). constexpr float kDefaultUintDepthScaleFactor = 1.0f / 1000.0f; +constexpr float kDefaultUintDepthScaleOffset = 0.0f; + bool load16BitDepthImage( const std::string& filename, DepthImage* depth_frame_ptr, MemoryType memory_type = kDefaultImageMemoryType, - const float scaling_factor = kDefaultUintDepthScaleFactor); + const float scaling_factor = kDefaultUintDepthScaleFactor, + const float scaling_offset = 0.0f); + bool load8BitColorImage(const std::string& filename, ColorImage* color_image_ptr, MemoryType memory_type = kDefaultImageMemoryType); @@ -45,10 +49,12 @@ class ImageLoader { public: ImageLoader(IndexToFilepathFunction index_to_filepath, MemoryType memory_type = kDefaultImageMemoryType, - float depth_image_scaling_factor = kDefaultUintDepthScaleFactor) + float depth_image_scaling_factor = kDefaultUintDepthScaleFactor, + float depth_image_scaling_offset = kDefaultUintDepthScaleOffset) : index_to_filepath_(index_to_filepath), memory_type_(memory_type), - depth_image_scaling_factor_(depth_image_scaling_factor) {} + depth_image_scaling_factor_(depth_image_scaling_factor), + depth_image_scaling_offset_(depth_image_scaling_offset) {} virtual ~ImageLoader() {} virtual bool getNextImage(ImageType* image_ptr); @@ -62,6 +68,7 @@ class ImageLoader { // Note(alexmillane): Only used for depth image loading. Ignored for color; const float depth_image_scaling_factor_; + const float depth_image_scaling_offset_; }; // Multi-threaded image loader @@ -72,10 +79,11 @@ using ImageOptional = std::pair; template class MultiThreadedImageLoader : public ImageLoader { public: - MultiThreadedImageLoader(IndexToFilepathFunction index_to_filepath, - int num_threads, - MemoryType memory_type = kDefaultImageMemoryType, - float depth_image_scaling_factor = kDefaultUintDepthScaleFactor); + MultiThreadedImageLoader( + IndexToFilepathFunction index_to_filepath, int num_threads, + MemoryType memory_type = kDefaultImageMemoryType, + float depth_image_scaling_factor = kDefaultUintDepthScaleFactor, + float depth_image_scaling_offset = kDefaultUintDepthScaleOffset); ~MultiThreadedImageLoader(); bool getNextImage(ImageType* image_ptr) override; @@ -95,7 +103,8 @@ template std::unique_ptr> createImageLoader( IndexToFilepathFunction index_to_path_function, const bool multithreaded = true, - const float depth_image_scaling_factor = kDefaultUintDepthScaleFactor); + const float depth_image_scaling_factor = kDefaultUintDepthScaleFactor, + const float depth_image_scaling_offset = kDefaultUintDepthScaleOffset); } // namespace datasets } // namespace nvblox diff --git a/nvblox/executables/include/nvblox/datasets/impl/image_loader_impl.h b/nvblox/executables/include/nvblox/datasets/impl/image_loader_impl.h index d8eb427f4..ec448a4d2 100644 --- a/nvblox/executables/include/nvblox/datasets/impl/image_loader_impl.h +++ b/nvblox/executables/include/nvblox/datasets/impl/image_loader_impl.h @@ -28,9 +28,11 @@ bool ImageLoader::getNextImage(ImageType* image_ptr) { template MultiThreadedImageLoader::MultiThreadedImageLoader( IndexToFilepathFunction index_to_filepath, int num_threads, - MemoryType memory_type, float depth_image_scaling_factor) + MemoryType memory_type, float depth_image_scaling_factor, + float depth_image_scaling_offset) : ImageLoader(index_to_filepath, memory_type, - depth_image_scaling_factor), + depth_image_scaling_factor, + depth_image_scaling_offset), num_threads_(num_threads) { initLoadQueue(); } @@ -89,7 +91,8 @@ MultiThreadedImageLoader::getImageAsOptional(int image_idx) { template std::unique_ptr> createImageLoader( IndexToFilepathFunction index_to_path_function, const bool multithreaded, - const float depth_image_scaling_factor) { + const float depth_image_scaling_factor, + const float depth_image_scaling_offset) { if (multithreaded) { // NOTE(alexmillane): On my desktop the performance of threaded image // loading seems to saturate at around 6 threads. On machines with less @@ -103,10 +106,11 @@ std::unique_ptr> createImageLoader( << " threads for loading images."; return std::make_unique>( index_to_path_function, kMaxLoadingThreads, MemoryType::kDevice, - depth_image_scaling_factor); + depth_image_scaling_factor, depth_image_scaling_offset); } return std::make_unique>( - index_to_path_function, MemoryType::kDevice, depth_image_scaling_factor); + index_to_path_function, MemoryType::kDevice, depth_image_scaling_factor, + depth_image_scaling_offset); } } // namespace datasets diff --git a/nvblox/executables/include/nvblox/datasets/kitti.h b/nvblox/executables/include/nvblox/datasets/kitti.h new file mode 100644 index 000000000..5375496e5 --- /dev/null +++ b/nvblox/executables/include/nvblox/datasets/kitti.h @@ -0,0 +1,107 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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 +#include + +#include "nvblox/core/types.h" +#include "nvblox/datasets/data_loader.h" +#include "nvblox/datasets/image_loader.h" +#include "nvblox/executables/fuser_lidar.h" + +namespace nvblox { +namespace datasets { +namespace kitti { + +// NOTE(gogojjh): the default settings of preprocess height_image offset +constexpr float kDefaultUintDepthScaleFactor = 1.0f / 1000.0f; +constexpr float kDefaultUintDepthScaleOffset = 10.0f; + +// Build a FuserLidar for the FusionPortable dataset +std::unique_ptr createFuser(const std::string base_path, + const int seq_id); + +///@brief A class for loading FusionPortable data +class DataLoader : public RgbdDataLoaderInterface { + public: + DataLoader(const std::string& base_path, const int seq_id, + bool multithreaded = true); + + DataLoadResult loadNext(DepthImage* depth_frame_ptr, // NOLINT + Transform* T_L_C_ptr, // NOLINT + Camera* camera_ptr, // NOLINT + ColorImage* color_frame_ptr = nullptr) override; + + /// Interface for a function that loads the next frames in a dataset + ///@param[out] depth_frame_ptr The loaded depth frame. + ///@param[out] T_L_C_ptr Transform from Camera to the Layer frame. + ///@param[out] camera_ptr The intrinsic camera model. + ///@param[out] lidar_ptr The intrinsic oslidar model. + ///@param[out] height_frame_ptr The loaded z frame. + ///@param[out] color_frame_ptr Optional, load color frame. + ///@return Whether loading succeeded. + DataLoadResult loadNext(DepthImage* depth_frame_ptr, // NOLINT + Transform* T_L_C_ptr, // NOLINT + CameraPinhole* camera_ptr, // NOLINT + OSLidar* lidar_ptr, // NOLINT + DepthImage* height_frame_ptr = nullptr, // NOLINT + ColorImage* color_frame_ptr = nullptr) override; + + protected: + std::unique_ptr> height_image_loader_; + + const std::string base_path_; + const int seq_id_; + + // The next frame to be loaded + int frame_number_ = 0; +}; + +namespace internal { + +bool parsePoseFromFile(const std::string& filename, Transform* transform); +bool parseCameraFromFile(const std::string& filename, + Eigen::Matrix3f* intrinsics); +bool parseLidarFromFile(const std::string& filename, + Eigen::Matrix* intrinsics); +std::string getPathForCameraIntrinsics(const std::string& base_path); +std::string getPathForLidarIntrinsics(const std::string& base_path, + const int seq_id, const int frame_id); +std::string getPathForFramePose(const std::string& base_path, const int seq_id, + const int frame_id); + +std::string getPathForDepthImage(const std::string& base_path, const int seq_id, + const int frame_id); +std::string getPathForHeightImage(const std::string& base_path, + const int seq_id, const int frame_id); +std::string getPathForColorImage(const std::string& base_path, const int seq_id, + const int frame_id); + +std::unique_ptr> createDepthImageLoader( + const std::string& base_path, const int seq_id, + const bool multithreaded = true); +std::unique_ptr> createHeightImageLoader( + const std::string& base_path, const int seq_id, + const bool multithreaded = true); +std::unique_ptr> createColorImageLoader( + const std::string& base_path, const int seq_id, + const bool multithreaded = true); + +} // namespace internal +} // namespace kitti +} // namespace datasets +} // namespace nvblox diff --git a/nvblox/executables/include/nvblox/datasets/replica.h b/nvblox/executables/include/nvblox/datasets/replica.h index 7cc3f4be4..3d1c1adb0 100644 --- a/nvblox/executables/include/nvblox/datasets/replica.h +++ b/nvblox/executables/include/nvblox/datasets/replica.h @@ -21,14 +21,14 @@ limitations under the License. #include "nvblox/core/types.h" #include "nvblox/datasets/data_loader.h" #include "nvblox/datasets/image_loader.h" -#include "nvblox/executables/fuser.h" +#include "nvblox/executables/fuser_rgbd.h" namespace nvblox { namespace datasets { namespace replica { -// Build a Fuser for the Replica dataset -std::unique_ptr createFuser(const std::string base_path); +// Build a FuserRGBD for the Replica dataset +std::unique_ptr createFuser(const std::string base_path); ///@brief A class for loading Replica data class DataLoader : public RgbdDataLoaderInterface { @@ -46,6 +46,13 @@ class DataLoader : public RgbdDataLoaderInterface { Camera* camera_ptr, // NOLINT ColorImage* color_frame_ptr = nullptr) override; + DataLoadResult loadNext(DepthImage* depth_frame_ptr, // NOLINT + Transform* T_L_C_ptr, // NOLINT + CameraPinhole* camera_ptr, // NOLINT + OSLidar* lidar_ptr, // NOLINT + DepthImage* height_frame_ptr = nullptr, // NOLINT + ColorImage* color_frame_ptr = nullptr) override; + protected: const std::string base_path_; diff --git a/nvblox/executables/include/nvblox/executables/fuser_lidar.h b/nvblox/executables/include/nvblox/executables/fuser_lidar.h new file mode 100644 index 000000000..e8b472c5e --- /dev/null +++ b/nvblox/executables/include/nvblox/executables/fuser_lidar.h @@ -0,0 +1,112 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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 +#include + +#include + +#include "nvblox/core/blox.h" +#include "nvblox/core/layer.h" +#include "nvblox/core/layer_cake.h" +#include "nvblox/core/mapper.h" +#include "nvblox/core/voxels.h" +#include "nvblox/datasets/data_loader.h" +#include "nvblox/gpu_hash/gpu_layer_view.h" +#include "nvblox/integrators/esdf_integrator.h" +#include "nvblox/integrators/projective_color_integrator.h" +#include "nvblox/integrators/projective_tsdf_integrator.h" +#include "nvblox/mesh/mesh_block.h" +#include "nvblox/mesh/mesh_integrator.h" +#include "nvblox/rays/sphere_tracer.h" + +namespace nvblox { + +class FuserLidar { + public: + FuserLidar() = default; + FuserLidar(std::unique_ptr&& data_loader); + + // Loads parameters from command line flags + void readCommandLineFlags(); + + // Runs an experiment + int run(); + + // Set various settings. + void setVoxelSize(float voxel_size); + void setTsdfFrameSubsampling(int subsample); + void setColorFrameSubsampling(int subsample); + void setMeshFrameSubsampling(int subsample); + void setEsdfFrameSubsampling(int subsample); + void setEsdfMode(RgbdMapper::EsdfMode esdf_mode); + + // Integrate certain layers. + bool integrateFrame(const int frame_number); + bool integrateFrames(); + void updateEsdf(); + + // Output a pointcloud ESDF as PLY file. + bool outputPointcloudPly(); + // Output a file with the mesh. + bool outputMeshPly(); + // Output timings to a file + bool outputTimingsToFile(); + // Output the serialized map to a file + bool outputMapToFile(); + // Output an obstacle pointcloud based on the ESDF map as PLY file. + bool outputObstaclePointcloudPly(); + + // Get the mapper (useful for experiments where we modify mapper settings) + RgbdMapper& mapper(); + + // Dataset settings. + int num_frames_to_integrate_ = std::numeric_limits::max(); + std::unique_ptr data_loader_; + + // Params + float voxel_size_m_ = 0.05; + int tsdf_frame_subsampling_ = 1; + int color_frame_subsampling_ = 1; + // By default we just do the mesh and esdf once at the end (if output paths + // exist) + int mesh_frame_subsampling_ = -1; + int esdf_frame_subsampling_ = -1; + + // ESDF slice params + float z_min_ = 0.5f; + float z_max_ = 1.0f; + float z_slice_ = 0.75f; + + // ESDF mode + RgbdMapper::EsdfMode esdf_mode_ = RgbdMapper::EsdfMode::k3D; + + // Mapper - Contains map layers and integrators + std::unique_ptr mapper_; + + // Output paths + std::string timing_output_path_; + std::string esdf_output_path_; + std::string mesh_output_path_; + std::string map_output_path_; + std::string obs_output_path_; + + // NOTE(gogojjh): setting parameters for different datasets + Transform T_B_C_; +}; + +} // namespace nvblox diff --git a/nvblox/executables/include/nvblox/executables/fuser.h b/nvblox/executables/include/nvblox/executables/fuser_rgbd.h similarity index 95% rename from nvblox/executables/include/nvblox/executables/fuser.h rename to nvblox/executables/include/nvblox/executables/fuser_rgbd.h index 7ac2a6f2f..c5e27356b 100644 --- a/nvblox/executables/include/nvblox/executables/fuser.h +++ b/nvblox/executables/include/nvblox/executables/fuser_rgbd.h @@ -36,10 +36,10 @@ limitations under the License. namespace nvblox { -class Fuser { +class FuserRGBD { public: - Fuser() = default; - Fuser(std::unique_ptr&& data_loader); + FuserRGBD() = default; + FuserRGBD(std::unique_ptr&& data_loader); // Loads parameters from command line flags void readCommandLineFlags(); @@ -80,7 +80,8 @@ class Fuser { float voxel_size_m_ = 0.05; int tsdf_frame_subsampling_ = 1; int color_frame_subsampling_ = 1; - // By default we just do the mesh and esdf once at the end (if output paths exist) + // By default we just do the mesh and esdf once at the end (if output paths + // exist) int mesh_frame_subsampling_ = -1; int esdf_frame_subsampling_ = -1; diff --git a/nvblox/executables/src/datasets/3dmatch.cpp b/nvblox/executables/src/datasets/3dmatch.cpp index e48db361d..2ad098012 100644 --- a/nvblox/executables/src/datasets/3dmatch.cpp +++ b/nvblox/executables/src/datasets/3dmatch.cpp @@ -114,12 +114,12 @@ std::unique_ptr> createColorImageLoader( } // namespace internal -std::unique_ptr createFuser(const std::string base_path, - const int seq_id) { +std::unique_ptr createFuser(const std::string base_path, + const int seq_id) { // Object to load 3DMatch data auto data_loader = std::make_unique(base_path, seq_id); - // Fuser - return std::make_unique(std::move(data_loader)); + // FuserRGBD + return std::make_unique(std::move(data_loader)); } DataLoader::DataLoader(const std::string& base_path, const int seq_id, @@ -217,6 +217,15 @@ DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, return DataLoadResult::kSuccess; } +DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, + Transform* T_L_C_ptr, + CameraPinhole* camera_ptr, + OSLidar* lidar_ptr, + DepthImage* height_frame_ptr, + ColorImage* color_frame_ptr) { + return DataLoadResult::kNoMoreData; +} + } // namespace threedmatch } // namespace datasets } // namespace nvblox diff --git a/nvblox/executables/src/datasets/fusionportable.cpp b/nvblox/executables/src/datasets/fusionportable.cpp new file mode 100644 index 000000000..d4b419512 --- /dev/null +++ b/nvblox/executables/src/datasets/fusionportable.cpp @@ -0,0 +1,333 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ +#include "nvblox/datasets/fusionportable.h" + +#include + +#include +#include +#include +#include + +#include "nvblox/utils/timing.h" + +namespace nvblox { +namespace datasets { +namespace fusionportable { +namespace internal { + +bool parsePoseFromFile(const std::string& filename, Transform* transform) { + CHECK_NOTNULL(transform); + constexpr int kDimension = 4; + + std::ifstream fin(filename); + if (fin.is_open()) { + for (int row = 0; row < kDimension; row++) + for (int col = 0; col < kDimension; col++) { + float item = 0.0; + fin >> item; + (*transform)(row, col) = item; + } + fin.close(); + return true; + } + return false; +} + +bool parseCameraFromFile(const std::string& filename, + Eigen::Matrix3f* intrinsics) { + CHECK_NOTNULL(intrinsics); + constexpr int kDimension = 3; + + std::ifstream fin(filename); + if (fin.is_open()) { + for (int row = 0; row < kDimension; row++) + for (int col = 0; col < kDimension; col++) { + float item = 0.0; + fin >> item; + (*intrinsics)(row, col) = item; + } + fin.close(); + + return true; + } + return false; +} // namespace internal + +bool parseLidarFromFile(const std::string& filename, + Eigen::Matrix* intrinsics) { + CHECK_NOTNULL(intrinsics); + const int kDimension = intrinsics->rows(); + std::ifstream fin(filename); + if (fin.is_open()) { + for (int col = 0; col < kDimension; col++) { + float item = 0.0; + fin >> item; + (*intrinsics)(col) = item; + } + fin.close(); + return true; + } + return false; +} // namespace internal + +// ********************* +// ********************* +std::string getPathForCameraIntrinsics(const std::string& base_path) { + return base_path + "/camera-intrinsics.txt"; +} + +std::string getPathForLidarIntrinsics(const std::string& base_path, + const int seq_id, const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".lidar-intrinsics.txt"; + return ss.str(); +} + +std::string getPathForFramePose(const std::string& base_path, const int seq_id, + const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".pose.txt"; + return ss.str(); +} + +std::string getPathForDepthImage(const std::string& base_path, const int seq_id, + const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".depth.png"; + return ss.str(); +} + +std::string getPathForColorImage(const std::string& base_path, const int seq_id, + const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".color.png"; + return ss.str(); +} + +std::string getPathForHeightImage(const std::string& base_path, + const int seq_id, const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".height.png"; + return ss.str(); +} + +std::unique_ptr> createDepthImageLoader( + const std::string& base_path, const int seq_id, const bool multithreaded) { + return createImageLoader( + std::bind(getPathForDepthImage, base_path, seq_id, std::placeholders::_1), + multithreaded, kDefaultUintDepthScaleFactor, 0.0f); +} + +std::unique_ptr> createColorImageLoader( + const std::string& base_path, const int seq_id, const bool multithreaded) { + return createImageLoader( + std::bind(getPathForColorImage, base_path, seq_id, std::placeholders::_1), + multithreaded); +} + +// TODO: we need a more proper way to set kDefaultUintDepthScaleOffset +std::unique_ptr> createHeightImageLoader( + const std::string& base_path, const int seq_id, const bool multithreaded) { + return createImageLoader( + std::bind(getPathForHeightImage, base_path, seq_id, + std::placeholders::_1), + multithreaded, kDefaultUintDepthScaleFactor, + kDefaultUintDepthScaleOffset); +} + +} // namespace internal + +std::unique_ptr createFuser(const std::string base_path, + const int seq_id) { + bool multithreaded = false; + // Object to load FusionPortable data + auto data_loader = + std::make_unique(base_path, seq_id, multithreaded); + // FuserLidar + return std::make_unique(std::move(data_loader)); +} + +DataLoader::DataLoader(const std::string& base_path, const int seq_id, + bool multithreaded) + : RgbdDataLoaderInterface(fusionportable::internal::createDepthImageLoader( + base_path, seq_id, multithreaded), + fusionportable::internal::createColorImageLoader( + base_path, seq_id, multithreaded)), + height_image_loader_( + std::move(fusionportable::internal::createHeightImageLoader( + base_path, seq_id, multithreaded))), + base_path_(base_path), + seq_id_(seq_id) { + // +} + +/// Interface for a function that loads the next frames in a dataset +///@param[out] depth_frame_ptr The loaded depth frame. +///@param[out] T_L_C_ptr Transform from Camera to the Layer frame. +///@param[out] camera_ptr The intrinsic camera model. +///@param[out] height_frame_ptr The loaded z frame. +///@param[out] color_frame_ptr Optional, load color frame. +///@return Whether loading succeeded. +DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, + Transform* T_L_C_ptr, + CameraPinhole* camera_ptr, + OSLidar* lidar_ptr, + DepthImage* height_frame_ptr, + ColorImage* color_frame_ptr) { + CHECK_NOTNULL(depth_frame_ptr); + CHECK_NOTNULL(T_L_C_ptr); + CHECK_NOTNULL(camera_ptr); + CHECK_NOTNULL(lidar_ptr); + CHECK_NOTNULL(height_frame_ptr); + // CHECK_NOTNULL(color_frame_ptr); // can be null + + // Because we might fail along the way, increment the frame number before we + // start. + const int frame_number = frame_number_; + ++frame_number_; + + // ********************************************* + // ********************************************* + // ********************************************* + // Load the image into a Depth Frame. + CHECK(depth_image_loader_); + timing::Timer timer_file_depth("file_loading/depth_image"); + if (!depth_image_loader_->getNextImage(depth_frame_ptr)) { + return DataLoadResult::kNoMoreData; + } + timer_file_depth.Stop(); + + // Load the image into a Height Frame. + CHECK(height_image_loader_); + timing::Timer timer_file_coord("file_loading/height_image"); + if (!height_image_loader_->getNextImage(height_frame_ptr)) { + return DataLoadResult::kNoMoreData; + } + timer_file_coord.Stop(); + + // NOTE(gogojjh): Load lidar intrinsics: + // num_azimuth_divisions + // num_elevation_divisions + // horizontal_fov_rad + // vertical_fov_rad + // start_azimuth_angle_rad + // end_azimuth_angle_rad + // start_elevation_angle_rad + // end_elevation_angle_rad + timing::Timer timer_file_camera("file_loading/lidar"); + Eigen::Matrix lidar_intrinsics; + if (!fusionportable::internal::parseLidarFromFile( + fusionportable::internal::getPathForLidarIntrinsics( + base_path_, seq_id_, frame_number), + &lidar_intrinsics)) { + return DataLoadResult::kNoMoreData; + } + *lidar_ptr = + OSLidar(lidar_intrinsics(0), lidar_intrinsics(1), lidar_intrinsics(2), + lidar_intrinsics(3), lidar_intrinsics(4), lidar_intrinsics(5), + lidar_intrinsics(6), lidar_intrinsics(7)); + CHECK(depth_frame_ptr->rows() == lidar_ptr->num_elevation_divisions()); + CHECK(depth_frame_ptr->cols() == lidar_ptr->num_azimuth_divisions()); + CHECK(height_frame_ptr->rows() == lidar_ptr->num_elevation_divisions()); + CHECK(height_frame_ptr->cols() == lidar_ptr->num_azimuth_divisions()); + + // ********************************************* + // ********************************************* + // ********************************************* + // Load the color image into a ColorImage + if (color_frame_ptr) { + CHECK(color_image_loader_); + timing::Timer timer_file_color("file_loading/color_image"); + if (!color_image_loader_->getNextImage(color_frame_ptr)) { + return DataLoadResult::kNoMoreData; + } + timer_file_color.Stop(); + } + + // Get the camera for this frame. + if (color_frame_ptr) { + timing::Timer timer_file_camera("file_loading/camera"); + Eigen::Matrix3f K; + if (!fusionportable::internal::parseCameraFromFile( + fusionportable::internal::getPathForCameraIntrinsics(base_path_), + &K)) { + return DataLoadResult::kNoMoreData; + } + + // Create a camera object. + const int image_width = color_frame_ptr->cols(); + const int image_height = color_frame_ptr->rows(); + *camera_ptr = + CameraPinhole::fromIntrinsicsMatrix(K, image_width, image_height); + timer_file_camera.Stop(); + + if (!K.allFinite()) { + LOG(WARNING) << "Bad CSV data."; + return DataLoadResult::kBadFrame; // Bad data, but keep going. + } + } + + // ********************************************* + // ********************************************* + // ********************************************* + // Get the transform. + timing::Timer timer_file_pose("file_loading/pose"); + Transform T_O_C; + if (!fusionportable::internal::parsePoseFromFile( + fusionportable::internal::getPathForFramePose(base_path_, seq_id_, + frame_number), + &T_O_C)) { + return DataLoadResult::kNoMoreData; + } + *T_L_C_ptr = T_O_C; + + // Check that the loaded data doesn't contain NaNs or a faulty rotation + // matrix. This does occur. If we find one, skip that frame and move to the + // next. + constexpr float kRotationMatrixDetEpsilon = 1e-4; + if (!T_L_C_ptr->matrix().allFinite() || + std::abs(T_L_C_ptr->matrix().block<3, 3>(0, 0).determinant() - 1.0f) > + kRotationMatrixDetEpsilon) { + LOG(WARNING) << "Bad CSV data."; + return DataLoadResult::kBadFrame; // Bad data, but keep going. + } + timer_file_pose.Stop(); + + return DataLoadResult::kSuccess; +} + +// NOTE(gogojjh): need to define the virutal function (not used) here +/// Interface for a function that loads the next frames in a dataset +///@param[out] depth_frame_ptr The loaded depth frame. +///@param[out] T_L_C_ptr Transform from Camera to the Layer frame. +///@param[out] camera_ptr The intrinsic camera model. +///@param[out] color_frame_ptr Optional, load color frame. +///@return Whether loading succeeded. +DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, + Transform* T_L_C_ptr, Camera* camera_ptr, + ColorImage* color_frame_ptr) { + return DataLoadResult::kNoMoreData; +} + +} // namespace fusionportable +} // namespace datasets +} // namespace nvblox diff --git a/nvblox/executables/src/datasets/image_loader.cpp b/nvblox/executables/src/datasets/image_loader.cpp index b77e5559c..94a530de5 100644 --- a/nvblox/executables/src/datasets/image_loader.cpp +++ b/nvblox/executables/src/datasets/image_loader.cpp @@ -27,9 +27,16 @@ namespace datasets { bool load16BitDepthImage(const std::string& filename, DepthImage* depth_frame_ptr, MemoryType memory_type, - const float scale_factor) { + const float scale_factor, const float scale_offset) { CHECK_NOTNULL(depth_frame_ptr); - timing::Timer stbi_timer("file_loading/depth_image/stbi"); + std::string timer_name; + if (scale_offset == 0.0f) { + timer_name = "file_loading/depth_image/stbi"; + } else { + timer_name = "file_loading/height_image/stbi"; + } + timing::Timer stbi_timer(timer_name); + int width, height, num_channels; uint16_t* image_data = stbi_load_16(filename.c_str(), &width, &height, &num_channels, 0); @@ -47,8 +54,18 @@ bool load16BitDepthImage(const std::string& filename, // ~1ms. So only do this when 1ms is relevant. std::vector float_image_data(height * width); for (int lin_idx = 0; lin_idx < float_image_data.size(); lin_idx++) { - float_image_data[lin_idx] = - static_cast(image_data[lin_idx]) * scale_factor; + if (scale_offset == 0.0f) { + float_image_data[lin_idx] = + static_cast(image_data[lin_idx]) * scale_factor; + } else { + if (image_data[lin_idx] == 0) { + float_image_data[lin_idx] = static_cast(image_data[lin_idx]); + } else { + float_image_data[lin_idx] = + static_cast(image_data[lin_idx]) * scale_factor - + scale_offset; + } + } } *depth_frame_ptr = DepthImage::fromBuffer( @@ -86,7 +103,8 @@ template <> bool ImageLoader::getImage(int image_idx, DepthImage* image_ptr) { CHECK_NOTNULL(image_ptr); bool res = load16BitDepthImage(index_to_filepath_(image_idx), image_ptr, - memory_type_, depth_image_scaling_factor_); + memory_type_, depth_image_scaling_factor_, + depth_image_scaling_offset_); return res; } diff --git a/nvblox/executables/src/datasets/kitti.cpp b/nvblox/executables/src/datasets/kitti.cpp new file mode 100644 index 000000000..46e666b5a --- /dev/null +++ b/nvblox/executables/src/datasets/kitti.cpp @@ -0,0 +1,352 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ +#include "nvblox/datasets/kitti.h" + +#include + +#include +#include +#include +#include + +#include "nvblox/utils/timing.h" + +namespace nvblox { +namespace datasets { +namespace kitti { +namespace internal { + +bool parsePoseFromFile(const std::string& filename, Transform* transform) { + CHECK_NOTNULL(transform); + constexpr int kDimension = 4; + + std::ifstream fin(filename); + if (fin.is_open()) { + for (int row = 0; row < kDimension; row++) + for (int col = 0; col < kDimension; col++) { + float item = 0.0; + fin >> item; + (*transform)(row, col) = item; + } + fin.close(); + return true; + } + return false; +} + +bool parseCameraFromFile(const std::string& filename, Matrix3x4f* P_rect, + Matrix3f* R_rect, int* height, int* width) { + CHECK_NOTNULL(P_rect); + CHECK_NOTNULL(R_rect); + CHECK_NOTNULL(height); + CHECK_NOTNULL(width); + + std::ifstream fin(filename); + if (fin.is_open()) { + fin >> *height >> *width; + + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 4; col++) { + float item = 0.0; + fin >> item; + (*P_rect)(row, col) = item; + } + } + + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 3; col++) { + float item = 0.0; + fin >> item; + (*R_rect)(row, col) = item; + } + } + + fin.close(); + return true; + } + return false; +} // namespace internal + +bool parseLidarFromFile(const std::string& filename, + Eigen::Matrix* intrinsics) { + CHECK_NOTNULL(intrinsics); + const int kDimension = intrinsics->rows(); + std::ifstream fin(filename); + if (fin.is_open()) { + for (int col = 0; col < kDimension; col++) { + float item = 0.0; + fin >> item; + (*intrinsics)(col) = item; + } + fin.close(); + return true; + } + return false; +} // namespace internal + +// ********************* +// ********************* +std::string getPathForCameraIntrinsics(const std::string& base_path) { + return base_path + "/camera-intrinsics.txt"; +} + +std::string getPathForLidarIntrinsics(const std::string& base_path, + const int seq_id, const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".lidar-intrinsics.txt"; + return ss.str(); +} + +std::string getPathForFramePose(const std::string& base_path, const int seq_id, + const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".pose.txt"; + return ss.str(); +} + +std::string getPathForDepthImage(const std::string& base_path, const int seq_id, + const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".depth.png"; + return ss.str(); +} + +std::string getPathForColorImage(const std::string& base_path, const int seq_id, + const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".color.png"; + return ss.str(); +} + +std::string getPathForHeightImage(const std::string& base_path, + const int seq_id, const int frame_id) { + std::stringstream ss; + ss << base_path << "/seq-" << std::setfill('0') << std::setw(2) << seq_id + << "/frame-" << std::setw(6) << frame_id << ".height.png"; + return ss.str(); +} + +std::unique_ptr> createDepthImageLoader( + const std::string& base_path, const int seq_id, const bool multithreaded) { + return createImageLoader( + std::bind(getPathForDepthImage, base_path, seq_id, std::placeholders::_1), + multithreaded, kDefaultUintDepthScaleFactor, 0.0f); +} + +std::unique_ptr> createColorImageLoader( + const std::string& base_path, const int seq_id, const bool multithreaded) { + return createImageLoader( + std::bind(getPathForColorImage, base_path, seq_id, std::placeholders::_1), + multithreaded); +} + +std::unique_ptr> createHeightImageLoader( + const std::string& base_path, const int seq_id, const bool multithreaded) { + return createImageLoader( + std::bind(getPathForHeightImage, base_path, seq_id, + std::placeholders::_1), + multithreaded, kDefaultUintDepthScaleFactor, + kDefaultUintDepthScaleOffset); +} + +} // namespace internal + +std::unique_ptr createFuser(const std::string base_path, + const int seq_id) { + bool multithreaded = false; + // Object to load FusionPortable data + auto data_loader = + std::make_unique(base_path, seq_id, multithreaded); + // FuserLidar + return std::make_unique(std::move(data_loader)); +} + +DataLoader::DataLoader(const std::string& base_path, const int seq_id, + bool multithreaded) + : RgbdDataLoaderInterface(kitti::internal::createDepthImageLoader( + base_path, seq_id, multithreaded), + kitti::internal::createColorImageLoader( + base_path, seq_id, multithreaded)), + height_image_loader_(std::move(kitti::internal::createHeightImageLoader( + base_path, seq_id, multithreaded))), + base_path_(base_path), + seq_id_(seq_id) { + // +} + +/// Interface for a function that loads the next frames in a dataset +///@param[out] depth_frame_ptr The loaded depth frame. +///@param[out] T_L_C_ptr Transform from the Base camera to the Layer frame. +///@param[out] camera_ptr The intrinsic camera model. +///@param[out] height_frame_ptr The loaded z frame. +///@param[out] color_frame_ptr Optional, load color frame. +///@return Whether loading succeeded. +DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, + Transform* T_L_C_ptr, + CameraPinhole* camera_ptr, + OSLidar* lidar_ptr, + DepthImage* height_frame_ptr, + ColorImage* color_frame_ptr) { + CHECK_NOTNULL(depth_frame_ptr); + CHECK_NOTNULL(T_L_C_ptr); + CHECK_NOTNULL(camera_ptr); + CHECK_NOTNULL(lidar_ptr); + CHECK_NOTNULL(height_frame_ptr); + // CHECK_NOTNULL(color_frame_ptr); // can be null + + // Because we might fail along the way, increment the frame number before we + // start. + const int frame_number = frame_number_; + ++frame_number_; + + // ********************************************* + // ********************************************* + // ********************************************* + // Load the image into a Depth Frame. + CHECK(depth_image_loader_); + timing::Timer timer_file_depth("file_loading/depth_image"); + if (!depth_image_loader_->getNextImage(depth_frame_ptr)) { + return DataLoadResult::kNoMoreData; + } + timer_file_depth.Stop(); + + // Load the image into a Height Frame. + CHECK(height_image_loader_); + timing::Timer timer_file_coord("file_loading/height_image"); + if (!height_image_loader_->getNextImage(height_frame_ptr)) { + return DataLoadResult::kNoMoreData; + } + timer_file_coord.Stop(); + + // NOTE(gogojjh): Load lidar intrinsics: + // num_azimuth_divisions + // num_elevation_divisions + // horizontal_fov_rad + // vertical_fov_rad + // start_azimuth_angle_rad + // end_azimuth_angle_rad + // start_elevation_angle_rad + // end_elevation_angle_rad + timing::Timer timer_file_camera("file_loading/lidar"); + Eigen::Matrix lidar_intrinsics; + if (!kitti::internal::parseLidarFromFile( + kitti::internal::getPathForLidarIntrinsics(base_path_, seq_id_, + frame_number), + &lidar_intrinsics)) { + return DataLoadResult::kNoMoreData; + } + *lidar_ptr = + OSLidar(lidar_intrinsics(0), lidar_intrinsics(1), lidar_intrinsics(2), + lidar_intrinsics(3), lidar_intrinsics(4), lidar_intrinsics(5), + lidar_intrinsics(6), lidar_intrinsics(7)); + CHECK(depth_frame_ptr->rows() == lidar_ptr->num_elevation_divisions()); + CHECK(depth_frame_ptr->cols() == lidar_ptr->num_azimuth_divisions()); + CHECK(height_frame_ptr->rows() == lidar_ptr->num_elevation_divisions()); + CHECK(height_frame_ptr->cols() == lidar_ptr->num_azimuth_divisions()); + + // ********************************************* + // ********************************************* + // ********************************************* + // Load the color image into a ColorImage + if (color_frame_ptr) { + CHECK(color_image_loader_); + timing::Timer timer_file_color("file_loading/color_image"); + if (!color_image_loader_->getNextImage(color_frame_ptr)) { + return DataLoadResult::kNoMoreData; + } + timer_file_color.Stop(); + } + + // Get the camera for this frame. + if (color_frame_ptr) { + timing::Timer timer_file_camera("file_loading/camera"); + Matrix3f R_rect; + Matrix3x4f P_rect; + int image_width; + int image_height; + if (!kitti::internal::parseCameraFromFile( + kitti::internal::getPathForCameraIntrinsics(base_path_), &P_rect, + &R_rect, &image_height, &image_width)) { + return DataLoadResult::kNoMoreData; + } + // std::cout << image_height << " " << image_width << std::endl + // << P_rect << std::endl + // << R_rect << std::endl; + + // Create a camera object. + *camera_ptr = CameraPinhole::fromIntrinsicsMatrix( + P_rect, R_rect, image_width, image_height); + timer_file_camera.Stop(); + + if (!P_rect.allFinite()) { + LOG(WARNING) << "[P_rect] Bad CSV data."; + return DataLoadResult::kBadFrame; // Bad data, but keep going. + } + if (!R_rect.allFinite()) { + LOG(WARNING) << "[R_rect] Bad CSV data."; + return DataLoadResult::kBadFrame; // Bad data, but keep going. + } + } + + // ********************************************* + // ********************************************* + // ********************************************* + // Get the transform. + timing::Timer timer_file_pose("file_loading/pose"); + Transform T_O_C; + if (!kitti::internal::parsePoseFromFile( + kitti::internal::getPathForFramePose(base_path_, seq_id_, + frame_number), + &T_O_C)) { + return DataLoadResult::kNoMoreData; + } + *T_L_C_ptr = T_O_C; + + // Check that the loaded data doesn't contain NaNs or a faulty rotation + // matrix. This does occur. If we find one, skip that frame and move to the + // next. + constexpr float kRotationMatrixDetEpsilon = 1e-4; + if (!T_L_C_ptr->matrix().allFinite() || + std::abs(T_L_C_ptr->matrix().block<3, 3>(0, 0).determinant() - 1.0f) > + kRotationMatrixDetEpsilon) { + LOG(WARNING) << "Bad CSV data."; + return DataLoadResult::kBadFrame; // Bad data, but keep going. + } + timer_file_pose.Stop(); + + return DataLoadResult::kSuccess; +} + +// NOTE(gogojjh): need to define the virutal function (not used) here +/// Interface for a function that loads the next frames in a dataset +///@param[out] depth_frame_ptr The loaded depth frame. +///@param[out] T_L_C_ptr Transform from Camera to the Layer frame. +///@param[out] camera_ptr The intrinsic camera model. +///@param[out] color_frame_ptr Optional, load color frame. +///@return Whether loading succeeded. +DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, + Transform* T_L_C_ptr, Camera* camera_ptr, + ColorImage* color_frame_ptr) { + return DataLoadResult::kNoMoreData; +} + +} // namespace kitti +} // namespace datasets +} // namespace nvblox diff --git a/nvblox/executables/src/datasets/replica.cpp b/nvblox/executables/src/datasets/replica.cpp index 07f2c3e09..4f7a9109b 100644 --- a/nvblox/executables/src/datasets/replica.cpp +++ b/nvblox/executables/src/datasets/replica.cpp @@ -141,11 +141,11 @@ std::unique_ptr> createColorImageLoader( } // namespace internal -std::unique_ptr createFuser(const std::string base_path) { +std::unique_ptr createFuser(const std::string base_path) { // Object to load 3DMatch data auto data_loader = std::make_unique(base_path); - // Fuser - return std::make_unique(std::move(data_loader)); + // FuserRGBD + return std::make_unique(std::move(data_loader)); } DataLoader::DataLoader(const std::string& base_path, bool multithreaded) @@ -214,7 +214,7 @@ DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, } else { if (!replica::internal::parseCameraFromFile( replica::internal::getPathForCameraIntrinsics(base_path_), &camera_, - &scale)) { + &scale)) { LOG(INFO) << "Couldn't find camera params file"; return DataLoadResult::kNoMoreData; } @@ -251,6 +251,15 @@ DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, return DataLoadResult::kSuccess; } +DataLoadResult DataLoader::loadNext(DepthImage* depth_frame_ptr, + Transform* T_L_C_ptr, + CameraPinhole* camera_ptr, + OSLidar* lidar_ptr, + DepthImage* height_frame_ptr, + ColorImage* color_frame_ptr) { + return DataLoadResult::kNoMoreData; +} + } // namespace replica } // namespace datasets } // namespace nvblox diff --git a/nvblox/executables/src/fuse_3dmatch.cpp b/nvblox/executables/src/fuse_3dmatch.cpp index b5edc03af..83a3f770e 100644 --- a/nvblox/executables/src/fuse_3dmatch.cpp +++ b/nvblox/executables/src/fuse_3dmatch.cpp @@ -17,7 +17,7 @@ limitations under the License. #include #include "nvblox/datasets/3dmatch.h" -#include "nvblox/executables/fuser.h" +#include "nvblox/executables/fuser_rgbd.h" DECLARE_bool(alsologtostderr); @@ -44,7 +44,7 @@ int main(int argc, char* argv[]) { // Fuser // NOTE(alexmillane): Hardcode the sequence ID. constexpr int seq_id = 1; - std::unique_ptr fuser = + std::unique_ptr fuser = datasets::threedmatch::createFuser(base_path, seq_id); // Mesh location (optional) diff --git a/nvblox/executables/src/fuse_fusionportable.cpp b/nvblox/executables/src/fuse_fusionportable.cpp new file mode 100644 index 000000000..70d0bc438 --- /dev/null +++ b/nvblox/executables/src/fuse_fusionportable.cpp @@ -0,0 +1,74 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ +#include +#include + +#include "nvblox/datasets/fusionportable.h" +#include "nvblox/executables/fuser_lidar.h" + +DECLARE_bool(alsologtostderr); + +using namespace nvblox; + +int main(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + FLAGS_alsologtostderr = true; + google::InstallFailureSignalHandler(); + + // Get the dataset + std::string base_path; + if (argc < 2) { + // Try out running on the test datasets. + base_path = "../tests/data/fusionportable"; + LOG(WARNING) + << "No FusionProtable file path specified; defaulting to the test " + "directory."; + } else { + base_path = argv[1]; + LOG(INFO) << "Loading FusionPortable files from " << base_path; + } + + // Fuser + // NOTE(alexmillane): Hardcode the sequence ID. + constexpr int seq_id = 1; + std::unique_ptr fuser_lidar = + datasets::fusionportable::createFuser(base_path, seq_id); + + // Mesh location (optional) + if (argc >= 3) { + fuser_lidar->mesh_output_path_ = argv[2]; + LOG(INFO) << "Mesh location:" << fuser_lidar->mesh_output_path_; + } + + // NOTE(gogojjh): set extrinsics from the base_link to the camera + Eigen::Quaternionf Qcb(0.500292, 0.490181, -0.508467, 0.500889); + Eigen::Vector3f tcb(0.067436, -0.022029, -0.078333); + Eigen::Quaternionf Qbc = Qcb.inverse(); + Eigen::Vector3f tbc = -(Qbc * tcb); + + Eigen::Matrix4f Tbc = Eigen::Matrix4f::Identity(); + Tbc.block<3, 3>(0, 0) = Qbc.toRotationMatrix(); + Tbc.block<3, 1>(0, 3) = tbc; + fuser_lidar->T_B_C_ = Transform(Tbc); + + // std::cout << fuser_lidar->T_B_C_.matrix() << std::endl; + // std::cout << std::endl; + // std::cout << Tbc << std::endl; + + // Make sure the layers are the correct resolution. + return fuser_lidar->run(); +} diff --git a/nvblox/executables/src/fuse_kitti.cpp b/nvblox/executables/src/fuse_kitti.cpp new file mode 100644 index 000000000..d5efdd7e9 --- /dev/null +++ b/nvblox/executables/src/fuse_kitti.cpp @@ -0,0 +1,74 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ +#include +#include + +#include "nvblox/datasets/kitti.h" +#include "nvblox/executables/fuser_lidar.h" + +DECLARE_bool(alsologtostderr); + +using namespace nvblox; + +int main(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + FLAGS_alsologtostderr = true; + google::InstallFailureSignalHandler(); + + // Get the dataset + std::string base_path; + if (argc < 2) { + // Try out running on the test datasets. + base_path = "../tests/data/kitti"; + LOG(WARNING) + << "No FusionProtable file path specified; defaulting to the test " + "directory."; + } else { + base_path = argv[1]; + LOG(INFO) << "Loading FusionPortable files from " << base_path; + } + + // Fuser + // NOTE(alexmillane): Hardcode the sequence ID. + constexpr int seq_id = 1; + std::unique_ptr fuser_lidar = + datasets::kitti::createFuser(base_path, seq_id); + + // Mesh location (optional) + if (argc >= 3) { + fuser_lidar->mesh_output_path_ = argv[2]; + LOG(INFO) << "Mesh location:" << fuser_lidar->mesh_output_path_; + } + + // NOTE(gogojjh): set extrinsics from the base_link to the camera + Eigen::Quaternionf Qcb(0.500292, 0.490181, -0.508467, 0.500889); + Eigen::Vector3f tcb(0.067436, -0.022029, -0.078333); + Eigen::Quaternionf Qbc = Qcb.inverse(); + Eigen::Vector3f tbc = -(Qbc * tcb); + + Eigen::Matrix4f Tbc = Eigen::Matrix4f::Identity(); + Tbc.block<3, 3>(0, 0) = Qbc.toRotationMatrix(); + Tbc.block<3, 1>(0, 3) = tbc; + fuser_lidar->T_B_C_ = Transform(Tbc); + + // std::cout << fuser_lidar->T_B_C_.matrix() << std::endl; + // std::cout << std::endl; + // std::cout << Tbc << std::endl; + + // Make sure the layers are the correct resolution. + return fuser_lidar->run(); +} diff --git a/nvblox/executables/src/fuse_replica.cpp b/nvblox/executables/src/fuse_replica.cpp index 2a9029aea..90491b89c 100644 --- a/nvblox/executables/src/fuse_replica.cpp +++ b/nvblox/executables/src/fuse_replica.cpp @@ -14,7 +14,6 @@ See the License for the specific language governing permissions and limitations under the License. */ - #include #include #include @@ -29,7 +28,7 @@ limitations under the License. #include "nvblox/core/types.h" #include "nvblox/datasets/image_loader.h" #include "nvblox/datasets/replica.h" -#include "nvblox/executables/fuser.h" +#include "nvblox/executables/fuser_rgbd.h" using namespace nvblox; @@ -48,7 +47,7 @@ int main(int argc, char* argv[]) { LOG(INFO) << "Loading Replica files from " << base_path; // Fuser - std::unique_ptr fuser = datasets::replica::createFuser(base_path); + std::unique_ptr fuser = datasets::replica::createFuser(base_path); // Mesh location (optional) if (argc >= 3) { diff --git a/nvblox/executables/src/fuser_lidar.cpp b/nvblox/executables/src/fuser_lidar.cpp new file mode 100644 index 000000000..fbe96e350 --- /dev/null +++ b/nvblox/executables/src/fuser_lidar.cpp @@ -0,0 +1,471 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ + +#include +#include + +#include "nvblox/executables/fuser_lidar.h" +#include "nvblox/io/mesh_io.h" +#include "nvblox/io/ply_writer.h" +#include "nvblox/io/pointcloud_io.h" +#include "nvblox/utils/timing.h" + +#include "nvblox/core/cuda/image_operation.h" + +// Layer params +DEFINE_double(voxel_size, 0.0f, "Voxel resolution in meters."); + +// Dataset flags +DEFINE_int32(num_frames, -1, + "Number of frames to process. Empty means process all."); + +// The output paths +DEFINE_string(timing_output_path, "", + "File in which to save the timing results."); +DEFINE_string(esdf_output_path, "", + "File in which to save the ESDF pointcloud."); +DEFINE_string(mesh_output_path, "", "File in which to save the surface mesh."); +DEFINE_string(map_output_path, "", "File in which to save the serialize map."); +DEFINE_string(obstacle_output_path, "", + "File in which to save the obstacle pointcloud map."); + +// Subsampling +DEFINE_int32(tsdf_frame_subsampling, 0, + "By what amount to subsample the TSDF frames. A subsample of 3 " + "means only every 3rd frame is taken."); +DEFINE_int32(color_frame_subsampling, 0, + "How much to subsample the color integration by."); +DEFINE_int32(mesh_frame_subsampling, 0, + "How much to subsample the meshing by."); +DEFINE_int32(esdf_frame_subsampling, 0, + "How much to subsample the ESDF integration by."); + +// TSDF Integrator settings +DEFINE_double(tsdf_integrator_max_integration_distance_m, -1.0, + "Maximum distance (in meters) from the camera at which to " + "integrate data into the TSDF."); +DEFINE_double(tsdf_integrator_truncation_distance_vox, -1.0, + "Truncation band (in voxels)."); +DEFINE_double( + tsdf_integrator_max_weight, -1.0, + "The maximum weight that a tsdf voxel can accumulate through integration."); + +// Mesh integrator settings +DEFINE_double(mesh_integrator_min_weight, -1.0, + "The minimum weight a tsdf voxel must have before it is meshed."); +DEFINE_bool(mesh_integrator_weld_vertices, true, + "Whether or not to weld duplicate vertices in the mesh."); + +// Color integrator settings +DEFINE_double(color_integrator_max_integration_distance_m, -1.0, + "Maximum distance (in meters) from the camera at which to " + "integrate color into the voxel grid."); + +// ESDF Integrator settings +DEFINE_double(esdf_integrator_min_weight, -1.0, + "The minimum weight at which to consider a voxel a site."); +DEFINE_double(esdf_integrator_max_site_distance_vox, -1.0, + "The maximum distance at which we consider a TSDF voxel a site."); +DEFINE_double(esdf_integrator_max_distance_m, -1.0, + "The maximum distance which we integrate ESDF distances out to."); +DEFINE_int32(esdf_mode, 0, "The ESDF mode. 0: k3D, 1: k2D, 2: kUnset"); +DEFINE_double(esdf_zmin, 0.5, "zmin of the 2D ESDF map"); +DEFINE_double(esdf_zmax, 1.0, "zmax of the 2D ESDF map"); +DEFINE_double(esdf_z_slice, 0.75, "z_slice of the 2D ESDF map"); + +namespace nvblox { +FuserLidar::FuserLidar( + std::unique_ptr&& data_loader) + : data_loader_(std::move(data_loader)) { + // NOTE(alexmillane): We require the voxel size before we construct the + // mapper, so we grab this parameter first and separately. + if (!gflags::GetCommandLineFlagInfoOrDie("voxel_size").is_default) { + LOG(INFO) << "Command line parameter found: voxel_size = " + << FLAGS_voxel_size; + setVoxelSize(static_cast(FLAGS_voxel_size)); + } + + // Initialize the mapper + mapper_ = std::make_unique(voxel_size_m_); + + // Default parameters + mapper_->mesh_integrator().min_weight(2.0f); + mapper_->color_integrator().max_integration_distance_m(5.0f); + mapper_->lidar_tsdf_integrator().max_integration_distance_m(1.0f); + mapper_->lidar_tsdf_integrator().view_calculator().raycast_subsampling_factor( + 4); + mapper_->esdf_integrator().max_distance_m(4.0f); + mapper_->esdf_integrator().min_weight(2.0f); + + // Pick commands off the command line + readCommandLineFlags(); +}; + +void FuserLidar::readCommandLineFlags() { + // Dataset flags + if (!gflags::GetCommandLineFlagInfoOrDie("num_frames").is_default) { + LOG(INFO) << "Command line parameter found: num_frames = " + << FLAGS_num_frames; + num_frames_to_integrate_ = FLAGS_num_frames; + } + // Output paths + if (!gflags::GetCommandLineFlagInfoOrDie("timing_output_path").is_default) { + LOG(INFO) << "Command line parameter found: timing_output_path = " + << FLAGS_timing_output_path; + timing_output_path_ = FLAGS_timing_output_path; + } + if (!gflags::GetCommandLineFlagInfoOrDie("esdf_output_path").is_default) { + LOG(INFO) << "Command line parameter found: esdf_output_path = " + << FLAGS_esdf_output_path; + esdf_output_path_ = FLAGS_esdf_output_path; + } + if (!gflags::GetCommandLineFlagInfoOrDie("mesh_output_path").is_default) { + LOG(INFO) << "Command line parameter found: mesh_output_path = " + << FLAGS_mesh_output_path; + mesh_output_path_ = FLAGS_mesh_output_path; + } + if (!gflags::GetCommandLineFlagInfoOrDie("map_output_path").is_default) { + LOG(INFO) << "Command line parameter found: map_output_path = " + << FLAGS_map_output_path; + map_output_path_ = FLAGS_map_output_path; + } + if (!gflags::GetCommandLineFlagInfoOrDie("obstacle_output_path").is_default) { + LOG(INFO) << "Command line parameter found: obstacle_output_path = " + << FLAGS_obstacle_output_path; + obs_output_path_ = FLAGS_obstacle_output_path; + } + + // Subsampling flags + if (!gflags::GetCommandLineFlagInfoOrDie("tsdf_frame_subsampling") + .is_default) { + LOG(INFO) << "Command line parameter found: tsdf_frame_subsampling = " + << FLAGS_tsdf_frame_subsampling; + setTsdfFrameSubsampling(FLAGS_tsdf_frame_subsampling); + } + if (!gflags::GetCommandLineFlagInfoOrDie("color_frame_subsampling") + .is_default) { + LOG(INFO) << "Command line parameter found: color_frame_subsampling = " + << FLAGS_color_frame_subsampling; + setColorFrameSubsampling(FLAGS_color_frame_subsampling); + } + if (!gflags::GetCommandLineFlagInfoOrDie("mesh_frame_subsampling") + .is_default) { + LOG(INFO) << "Command line parameter found: mesh_frame_subsampling = " + << FLAGS_mesh_frame_subsampling; + setMeshFrameSubsampling(FLAGS_mesh_frame_subsampling); + } + if (!gflags::GetCommandLineFlagInfoOrDie("esdf_frame_subsampling") + .is_default) { + LOG(INFO) << "Command line parameter found: esdf_frame_subsampling = " + << FLAGS_esdf_frame_subsampling; + setEsdfFrameSubsampling(FLAGS_esdf_frame_subsampling); + } + + // TSDF integrator + if (!gflags::GetCommandLineFlagInfoOrDie( + "tsdf_integrator_max_integration_distance_m") + .is_default) { + LOG(INFO) << "Command line parameter found: " + "tsdf_integrator_max_integration_distance_m= " + << FLAGS_tsdf_integrator_max_integration_distance_m; + mapper_->lidar_tsdf_integrator().max_integration_distance_m( + FLAGS_tsdf_integrator_max_integration_distance_m); + } + if (!gflags::GetCommandLineFlagInfoOrDie( + "tsdf_integrator_truncation_distance_vox") + .is_default) { + LOG(INFO) << "Command line parameter found: " + "tsdf_integrator_truncation_distance_vox = " + << FLAGS_tsdf_integrator_truncation_distance_vox; + mapper_->lidar_tsdf_integrator().truncation_distance_vox( + FLAGS_tsdf_integrator_truncation_distance_vox); + } + if (!gflags::GetCommandLineFlagInfoOrDie("tsdf_integrator_max_weight") + .is_default) { + LOG(INFO) << "Command line parameter found: tsdf_integrator_max_weight = " + << FLAGS_tsdf_integrator_max_weight; + mapper_->lidar_tsdf_integrator().max_weight( + FLAGS_tsdf_integrator_max_weight); + } + + // Mesh integrator + if (!gflags::GetCommandLineFlagInfoOrDie("mesh_integrator_min_weight") + .is_default) { + LOG(INFO) << "Command line parameter found: mesh_integrator_min_weight = " + << FLAGS_mesh_integrator_min_weight; + mapper_->mesh_integrator().min_weight(FLAGS_mesh_integrator_min_weight); + } + if (!gflags::GetCommandLineFlagInfoOrDie("mesh_integrator_weld_vertices") + .is_default) { + LOG(INFO) + << "Command line parameter found: mesh_integrator_weld_vertices = " + << FLAGS_mesh_integrator_weld_vertices; + mapper_->mesh_integrator().weld_vertices( + FLAGS_mesh_integrator_weld_vertices); + } + + // Color integrator + if (!gflags::GetCommandLineFlagInfoOrDie( + "color_integrator_max_integration_distance_m") + .is_default) { + LOG(INFO) << "Command line parameter found: " + "color_integrator_max_integration_distance_m = " + << FLAGS_color_integrator_max_integration_distance_m; + mapper_->color_integrator().max_integration_distance_m( + FLAGS_color_integrator_max_integration_distance_m); + } + + // ESDF integrator + if (!gflags::GetCommandLineFlagInfoOrDie("esdf_integrator_min_weight") + .is_default) { + LOG(INFO) << "Command line parameter found: esdf_integrator_min_weight = " + << FLAGS_esdf_integrator_min_weight; + mapper_->esdf_integrator().min_weight(FLAGS_esdf_integrator_min_weight); + } + if (!gflags::GetCommandLineFlagInfoOrDie( + "esdf_integrator_max_site_distance_vox") + .is_default) { + LOG(INFO) << "Command line parameter found: " + "esdf_integrator_max_site_distance_vox = " + << FLAGS_esdf_integrator_max_site_distance_vox; + mapper_->esdf_integrator().max_site_distance_vox( + FLAGS_esdf_integrator_max_site_distance_vox); + } + + if (!gflags::GetCommandLineFlagInfoOrDie("esdf_integrator_max_distance_m") + .is_default) { + LOG(INFO) + << "Command line parameter found: esdf_integrator_max_distance_m = " + << FLAGS_esdf_integrator_max_distance_m; + mapper_->esdf_integrator().max_distance_m( + FLAGS_esdf_integrator_max_distance_m); + } + + if (!gflags::GetCommandLineFlagInfoOrDie("esdf_mode").is_default) { + LOG(INFO) << "Command line parameter found: esdf_mode = " + << FLAGS_esdf_mode; + if (FLAGS_esdf_mode == 0) { + setEsdfMode(RgbdMapper::EsdfMode::k3D); + } else if (FLAGS_esdf_mode == 1) { + setEsdfMode(RgbdMapper::EsdfMode::k2D); + z_min_ = FLAGS_esdf_zmin; + z_max_ = FLAGS_esdf_zmax; + z_slice_ = FLAGS_esdf_z_slice; + } else if (FLAGS_esdf_mode == 2) { + setEsdfMode(RgbdMapper::EsdfMode::kUnset); + } + } +} + +// NOTE(gogojjh): the overall procedures running function +int FuserLidar::run() { + LOG(INFO) << "Trying to integrate the first frame: "; + if (!integrateFrames()) { + LOG(FATAL) + << "Failed to integrate first frame. Please check the file path."; + return 1; + } + + if (!mesh_output_path_.empty()) { + LOG(INFO) << "Generating the mesh."; + mapper_->updateMesh(); + LOG(INFO) << "Outputting mesh ply file to " << mesh_output_path_; + outputMeshPly(); + } + + if (!esdf_output_path_.empty()) { + LOG(INFO) << "Generating the ESDF."; + updateEsdf(); + LOG(INFO) << "Outputting ESDF pointcloud ply file to " << esdf_output_path_; + outputPointcloudPly(); + } + + if (!map_output_path_.empty()) { + LOG(INFO) << "Outputting the serialized map to " << map_output_path_; + outputMapToFile(); + } + + if (!obs_output_path_.empty()) { + LOG(INFO) << "Outputting Obstacle based on the ESDF map ply file to " + << obs_output_path_; + outputObstaclePointcloudPly(); + } + + // std::vector keywords = { + // std::string("integrate"), std::string("normal"), std::string("write")}; + std::vector keywords = {std::string("fuser"), + std::string("write")}; + LOG(INFO) << nvblox::timing::Timing::Print(keywords); + + LOG(INFO) << "Writing timings to file."; + outputTimingsToFile(); + + return 0; +} + +RgbdMapper& FuserLidar::mapper() { return *mapper_; } + +void FuserLidar::setVoxelSize(float voxel_size) { voxel_size_m_ = voxel_size; } + +void FuserLidar::setTsdfFrameSubsampling(int subsample) { + tsdf_frame_subsampling_ = subsample; +} + +void FuserLidar::setColorFrameSubsampling(int subsample) { + color_frame_subsampling_ = subsample; +} + +void FuserLidar::setMeshFrameSubsampling(int subsample) { + mesh_frame_subsampling_ = subsample; +} + +void FuserLidar::setEsdfFrameSubsampling(int subsample) { + esdf_frame_subsampling_ = subsample; +} + +void FuserLidar::setEsdfMode(RgbdMapper::EsdfMode esdf_mode) { + if (esdf_mode_ != RgbdMapper::EsdfMode::kUnset) { + LOG(WARNING) << "EsdfMode already set. Cannot change once set once. Not " + "doing anything."; + } + esdf_mode_ = esdf_mode; +} + +// NOTE(gogojjh): this function will run the tsdf, mesh, and esdf integration +// for each incoming frame +bool FuserLidar::integrateFrame(const int frame_number) { + timing::Timer timer_file("fuser/file_loading"); + DepthImage depth_frame; + DepthImage height_frame; + ColorImage color_frame; + Transform T_W_B; + CameraPinhole camera; + OSLidar oslidar; + const datasets::DataLoadResult load_result = data_loader_->loadNext( + &depth_frame, &T_W_B, &camera, &oslidar, &height_frame, &color_frame); + timer_file.Stop(); + + if (load_result == datasets::DataLoadResult::kBadFrame) { + LOG(INFO) << "Bad frame: wrong parameters of intrinsics or extrinsics"; + return true; // Bad data but keep going + } + if (load_result == datasets::DataLoadResult::kNoMoreData) { + LOG(INFO) << "No more data: lack of depth_frame, height_frame, " + "or color_frame"; + return false; // Shows over folks + } + + timing::Timer per_frame_timer("fuser/time_per_frame"); + + if ((frame_number + 1) % tsdf_frame_subsampling_ == 0) { + oslidar.setDepthFrameCUDA(depth_frame.dataPtr()); + oslidar.setHeightFrameCUDA(height_frame.dataPtr()); + + timing::Timer timer_normal("fuser/compute_normal_image"); // 0.7ms + nvblox::cuda::getNormalImageOSLidar(oslidar); + timer_normal.Stop(); + + timing::Timer timer_integrate("fuser/integrate_tsdf"); + mapper_->integrateOSLidarDepth(depth_frame, T_W_B, oslidar); + timer_integrate.Stop(); + + nvblox::cuda::freeNormalImageOSLidar(oslidar); + } + + Transform T_W_C = T_W_B * T_B_C_; + if (color_frame_subsampling_ > 0) { + if ((frame_number + 1) % color_frame_subsampling_ == 0) { + timing::Timer timer_integrate_color("fuser/integrate_color"); + mapper_->integrateColor(color_frame, T_W_C, camera); + timer_integrate_color.Stop(); + } + } + + if (mesh_frame_subsampling_ > 0) { + if ((frame_number + 1) % mesh_frame_subsampling_ == 0) { + timing::Timer timer_mesh("fuser/mesh"); + mapper_->updateMesh(); + } + } + + if (esdf_frame_subsampling_ > 0) { + if ((frame_number + 1) % esdf_frame_subsampling_ == 0) { + timing::Timer timer_integrate_esdf("fuser/integrate_esdf"); + updateEsdf(); + timer_integrate_esdf.Stop(); + } + } + + per_frame_timer.Stop(); + return true; +} + +// NOTE(gogojjh): Running all TSDF integrations +bool FuserLidar::integrateFrames() { + int frame_number = 0; + while (frame_number < num_frames_to_integrate_ && + integrateFrame(frame_number++)) { + timing::mark("Frame " + std::to_string(frame_number - 1), Color::Red()); + LOG(INFO) << "Integrating frame " << frame_number - 1; + } + LOG(INFO) << "Ran out of data at frame: " << frame_number - 1; + return true; +} + +void FuserLidar::updateEsdf() { + switch (esdf_mode_) { + case RgbdMapper::EsdfMode::kUnset: + break; + case RgbdMapper::EsdfMode::k3D: + mapper_->updateEsdf(); + break; + case RgbdMapper::EsdfMode::k2D: + mapper_->updateEsdfSlice(z_min_, z_max_, z_slice_); + break; + } +} + +bool FuserLidar::outputPointcloudPly() { + timing::Timer timer_write("fuser/esdf/write"); + return io::outputVoxelLayerToPly(mapper_->esdf_layer(), esdf_output_path_); +} + +bool FuserLidar::outputMeshPly() { + timing::Timer timer_write("fuser/mesh/write"); + return io::outputMeshLayerToPly(mapper_->mesh_layer(), mesh_output_path_); +} + +bool FuserLidar::outputObstaclePointcloudPly() { + timing::Timer timer_write("fuser/obstacle/write"); + LOG(INFO) + << "[NOTE] The output of the obstacle point cloud is under construction"; + return io::outputObstacleToPly(mapper_->esdf_layer(), obs_output_path_); +} + +bool FuserLidar::outputTimingsToFile() { + LOG(INFO) << "Writing timing to: " << timing_output_path_; + std::ofstream timing_file(timing_output_path_); + timing_file << nvblox::timing::Timing::Print(); + timing_file.close(); + return true; +} + +bool FuserLidar::outputMapToFile() { + timing::Timer timer_serialize("fuser/map/write"); + return mapper_->saveMap(map_output_path_); +} + +} // namespace nvblox diff --git a/nvblox/executables/src/fuser.cpp b/nvblox/executables/src/fuser_rgbd.cpp similarity index 90% rename from nvblox/executables/src/fuser.cpp rename to nvblox/executables/src/fuser_rgbd.cpp index 0d2f16de9..9a80a3640 100644 --- a/nvblox/executables/src/fuser.cpp +++ b/nvblox/executables/src/fuser_rgbd.cpp @@ -13,12 +13,11 @@ 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. */ -#include "nvblox/executables/fuser.h" #include #include -#include "nvblox/executables/fuser.h" +#include "nvblox/executables/fuser_rgbd.h" #include "nvblox/io/mesh_io.h" #include "nvblox/io/ply_writer.h" #include "nvblox/io/pointcloud_io.h" @@ -81,7 +80,8 @@ DEFINE_double(esdf_integrator_max_distance_m, -1.0, namespace nvblox { -Fuser::Fuser(std::unique_ptr&& data_loader) +FuserRGBD::FuserRGBD( + std::unique_ptr&& data_loader) : data_loader_(std::move(data_loader)) { // NOTE(alexmillane): We require the voxel size before we construct the // mapper, so we grab this parameter first and separately. @@ -106,7 +106,7 @@ Fuser::Fuser(std::unique_ptr&& data_loader) readCommandLineFlags(); }; -void Fuser::readCommandLineFlags() { +void FuserRGBD::readCommandLineFlags() { // Dataset flags if (!gflags::GetCommandLineFlagInfoOrDie("num_frames").is_default) { LOG(INFO) << "Command line parameter found: num_frames = " @@ -160,6 +160,7 @@ void Fuser::readCommandLineFlags() { << FLAGS_esdf_frame_subsampling; setEsdfFrameSubsampling(FLAGS_esdf_frame_subsampling); } + // TSDF integrator if (!gflags::GetCommandLineFlagInfoOrDie( "tsdf_integrator_max_integration_distance_m") @@ -185,6 +186,7 @@ void Fuser::readCommandLineFlags() { << FLAGS_tsdf_integrator_max_weight; mapper_->tsdf_integrator().max_weight(FLAGS_tsdf_integrator_max_weight); } + // Mesh integrator if (!gflags::GetCommandLineFlagInfoOrDie("mesh_integrator_min_weight") .is_default) { @@ -200,6 +202,7 @@ void Fuser::readCommandLineFlags() { mapper_->mesh_integrator().weld_vertices( FLAGS_mesh_integrator_weld_vertices); } + // Color integrator if (!gflags::GetCommandLineFlagInfoOrDie( "color_integrator_max_integration_distance_m") @@ -210,6 +213,7 @@ void Fuser::readCommandLineFlags() { mapper_->color_integrator().max_integration_distance_m( FLAGS_color_integrator_max_integration_distance_m); } + // ESDF integrator if (!gflags::GetCommandLineFlagInfoOrDie("esdf_integrator_min_weight") .is_default) { @@ -226,6 +230,7 @@ void Fuser::readCommandLineFlags() { mapper_->esdf_integrator().max_site_distance_vox( FLAGS_esdf_integrator_max_site_distance_vox); } + if (!gflags::GetCommandLineFlagInfoOrDie("esdf_integrator_max_distance_m") .is_default) { LOG(INFO) @@ -236,7 +241,7 @@ void Fuser::readCommandLineFlags() { } } -int Fuser::run() { +int FuserRGBD::run() { LOG(INFO) << "Trying to integrate the first frame: "; if (!integrateFrames()) { LOG(FATAL) @@ -271,27 +276,27 @@ int Fuser::run() { return 0; } -RgbdMapper& Fuser::mapper() { return *mapper_; } +RgbdMapper& FuserRGBD::mapper() { return *mapper_; } -void Fuser::setVoxelSize(float voxel_size) { voxel_size_m_ = voxel_size; } +void FuserRGBD::setVoxelSize(float voxel_size) { voxel_size_m_ = voxel_size; } -void Fuser::setTsdfFrameSubsampling(int subsample) { +void FuserRGBD::setTsdfFrameSubsampling(int subsample) { tsdf_frame_subsampling_ = subsample; } -void Fuser::setColorFrameSubsampling(int subsample) { +void FuserRGBD::setColorFrameSubsampling(int subsample) { color_frame_subsampling_ = subsample; } -void Fuser::setMeshFrameSubsampling(int subsample) { +void FuserRGBD::setMeshFrameSubsampling(int subsample) { mesh_frame_subsampling_ = subsample; } -void Fuser::setEsdfFrameSubsampling(int subsample) { +void FuserRGBD::setEsdfFrameSubsampling(int subsample) { esdf_frame_subsampling_ = subsample; } -void Fuser::setEsdfMode(RgbdMapper::EsdfMode esdf_mode) { +void FuserRGBD::setEsdfMode(RgbdMapper::EsdfMode esdf_mode) { if (esdf_mode_ != RgbdMapper::EsdfMode::kUnset) { LOG(WARNING) << "EsdfMode already set. Cannot change once set once. Not " "doing anything."; @@ -299,8 +304,8 @@ void Fuser::setEsdfMode(RgbdMapper::EsdfMode esdf_mode) { esdf_mode_ = esdf_mode; } -bool Fuser::integrateFrame(const int frame_number) { - timing::Timer timer_file("fuser/file_loading"); +bool FuserRGBD::integrateFrame(const int frame_number) { + timing::Timer timer_file("FuserRGBD/file_loading"); DepthImage depth_frame; ColorImage color_frame; Transform T_L_C; @@ -316,51 +321,50 @@ bool Fuser::integrateFrame(const int frame_number) { return false; // Shows over folks } - timing::Timer per_frame_timer("fuser/time_per_frame"); + timing::Timer per_frame_timer("FuserRGBD/time_per_frame"); if ((frame_number + 1) % tsdf_frame_subsampling_ == 0) { - timing::Timer timer_integrate("fuser/integrate_tsdf"); + timing::Timer timer_integrate("FuserRGBD/integrate_tsdf"); mapper_->integrateDepth(depth_frame, T_L_C, camera); timer_integrate.Stop(); } if ((frame_number + 1) % color_frame_subsampling_ == 0) { - timing::Timer timer_integrate_color("fuser/integrate_color"); + timing::Timer timer_integrate_color("FuserRGBD/integrate_color"); mapper_->integrateColor(color_frame, T_L_C, camera); timer_integrate_color.Stop(); } if (mesh_frame_subsampling_ > 0) { if ((frame_number + 1) % mesh_frame_subsampling_ == 0) { - timing::Timer timer_mesh("fuser/mesh"); + timing::Timer timer_mesh("FuserRGBD/mesh"); mapper_->updateMesh(); } } if (esdf_frame_subsampling_ > 0) { if ((frame_number + 1) % esdf_frame_subsampling_ == 0) { - timing::Timer timer_integrate_esdf("fuser/integrate_esdf"); + timing::Timer timer_integrate_esdf("FuserRGBD/integrate_esdf"); updateEsdf(); timer_integrate_esdf.Stop(); } } - per_frame_timer.Stop(); - return true; -} +} // namespace nvblox -bool Fuser::integrateFrames() { +bool FuserRGBD::integrateFrames() { int frame_number = 0; while (frame_number < num_frames_to_integrate_ && integrateFrame(frame_number++)) { timing::mark("Frame " + std::to_string(frame_number - 1), Color::Red()); LOG(INFO) << "Integrating frame " << frame_number - 1; + std::cout << std::endl; } LOG(INFO) << "Ran out of data at frame: " << frame_number - 1; return true; } -void Fuser::updateEsdf() { +void FuserRGBD::updateEsdf() { switch (esdf_mode_) { case RgbdMapper::EsdfMode::kUnset: break; @@ -373,17 +377,17 @@ void Fuser::updateEsdf() { } } -bool Fuser::outputPointcloudPly() { - timing::Timer timer_write("fuser/esdf/write"); +bool FuserRGBD::outputPointcloudPly() { + timing::Timer timer_write("FuserRGBD/esdf/write"); return io::outputVoxelLayerToPly(mapper_->esdf_layer(), esdf_output_path_); } -bool Fuser::outputMeshPly() { - timing::Timer timer_write("fuser/mesh/write"); +bool FuserRGBD::outputMeshPly() { + timing::Timer timer_write("FuserRGBD/mesh/write"); return io::outputMeshLayerToPly(mapper_->mesh_layer(), mesh_output_path_); } -bool Fuser::outputTimingsToFile() { +bool FuserRGBD::outputTimingsToFile() { LOG(INFO) << "Writing timing to: " << timing_output_path_; std::ofstream timing_file(timing_output_path_); timing_file << nvblox::timing::Timing::Print(); @@ -391,8 +395,8 @@ bool Fuser::outputTimingsToFile() { return true; } -bool Fuser::outputMapToFile() { - timing::Timer timer_serialize("fuser/map/write"); +bool FuserRGBD::outputMapToFile() { + timing::Timer timer_serialize("FuserRGBD/map/write"); return mapper_->saveMap(map_output_path_); } diff --git a/nvblox/experiments/experiments/threaded_image_loading/main.cpp b/nvblox/experiments/experiments/threaded_image_loading/main.cpp index b68ce2276..015eeb7ef 100644 --- a/nvblox/experiments/experiments/threaded_image_loading/main.cpp +++ b/nvblox/experiments/experiments/threaded_image_loading/main.cpp @@ -25,7 +25,7 @@ limitations under the License. #include "nvblox/datasets/3dmatch.h" #include "nvblox/datasets/3dmatch.h" -#include "nvblox/executables/fuser.h" +#include "nvblox/executables/fuser_rgbd.h" DEFINE_bool(single_thread, false, "Load images using a single thread."); DEFINE_bool(multi_thread, false, "Load images using multiple threads"); @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) { } constexpr int seq_id = 1; - std::unique_ptr fuser = + std::unique_ptr fuser = datasets::threedmatch::createFuser(base_path, seq_id); // Replacing the image loader with the one we are asked to test. diff --git a/nvblox/include/nvblox/core/camera.h b/nvblox/include/nvblox/core/camera.h index 01dc72144..3a1a89068 100644 --- a/nvblox/include/nvblox/core/camera.h +++ b/nvblox/include/nvblox/core/camera.h @@ -15,6 +15,7 @@ limitations under the License. */ #pragma once +#include "nvblox/core/frustum.h" #include "nvblox/core/types.h" namespace nvblox { @@ -90,53 +91,6 @@ class Camera { // Stream Camera as text std::ostream& operator<<(std::ostream& os, const Camera& camera); -/// A bounding plane which has one "inside" direction and the other direction is -/// "outside." Quick tests for which side of the plane you are on. -class BoundingPlane { - public: - BoundingPlane() : normal_(Vector3f::Identity()), distance_(0.0f) {} - - void setFromPoints(const Vector3f& p1, const Vector3f& p2, - const Vector3f& p3); - void setFromDistanceNormal(const Vector3f& normal, float distance); - - /// Is the point on the correct side of the bounding plane? - bool isPointInside(const Vector3f& point) const; - - const Vector3f& normal() const { return normal_; } - float distance() const { return distance_; } - - private: - Vector3f normal_; - float distance_; -}; - -/// Class that allows checking for whether objects are within the field of view -/// of a camera or not. -class Frustum { - public: - // Frustum must be initialized with a camera and min and max depth and pose. - Frustum(const Camera& camera, const Transform& T_L_C, float min_depth, - float max_depth); - - AxisAlignedBoundingBox getAABB() const { return aabb_; } - - bool isPointInView(const Vector3f& point) const; - bool isAABBInView(const AxisAlignedBoundingBox& aabb) const; - - private: - // Helper functions to do the actual computations. - void computeBoundingPlanes(const Eigen::Matrix& corners_C, - const Transform& T_L_C); - - /// Bounding planes containing around the frustum. Expressed in the layer - /// coordinate frame. - std::array bounding_planes_L_; - - /// Cached AABB of the - AxisAlignedBoundingBox aabb_; -}; - } // namespace nvblox #include "nvblox/core/impl/camera_impl.h" diff --git a/nvblox/include/nvblox/core/camera_pinhole.h b/nvblox/include/nvblox/core/camera_pinhole.h new file mode 100644 index 000000000..b0106d79d --- /dev/null +++ b/nvblox/include/nvblox/core/camera_pinhole.h @@ -0,0 +1,103 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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/core/frustum.h" +#include "nvblox/core/types.h" + +namespace nvblox { + +/// Class that describes the parameters and FoV of a camera. +class CameraPinhole { + public: + __host__ __device__ inline CameraPinhole() = default; + __host__ __device__ inline CameraPinhole(const Matrix3f& K, const int width, + const int height); + __host__ __device__ inline CameraPinhole(const Matrix3x4f& P, + const Matrix3f& rect, + const int width, const int height); + + __host__ __device__ inline bool project(const Vector3f& p_C, + Vector2f* u_C) const; + + __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, + const float min_depth, + const float max_depth) const; + + __host__ Frustum getViewFrustum(const Transform& T_L_C, const float min_depth, + const float max_depth) const; + + /// Gets the view corners in the CAMERA coordinate frame. + __host__ Eigen::Matrix getViewCorners( + const float min_depth, const float max_depth) const; + + // Returns an unnormalized ray direction in the camera frame corresponding to + // the passed pixel. + // Two functions, one for (floating point) image-plane coordinates, another + // function for pixel indices. + __host__ __device__ inline Vector3f vectorFromImagePlaneCoordinates( + const Vector2f& u_C) const; + __host__ __device__ inline Vector3f vectorFromPixelIndices( + const Index2D& u_C) const; + + // Accessors + __host__ __device__ inline Matrix3f K() const { return K_; } + __host__ __device__ inline Matrix3x4f P() const { return P_; } + __host__ __device__ inline Matrix3f Rect() const { return R_rect_; } + __host__ __device__ inline int width() const { return width_; } + __host__ __device__ inline int height() const { return height_; } + __host__ __device__ inline int cols() const { return width_; } + __host__ __device__ inline int rows() const { return height_; } + __host__ __device__ inline bool isRectified() const { return rectified_; } + + // Factories + inline static CameraPinhole fromIntrinsicsMatrix(const Matrix3f& mat, + int width, int height); + + inline static CameraPinhole fromIntrinsicsMatrix(const Matrix3x4f& P, + const Matrix3f& rect, + int width, int height); + + private: + Matrix3f K_; + /// NOTE(gogojjh): + /// KITTI dataset: P_ = P_rect_00, we first transform the point into a camera + /// P_(0:2, 3) = 0, P_(3, 3) = 1 + Matrix3x4f P_; + /// KITTI dataset: R_rect_ = R_rect_0x + Matrix3f R_rect_; + + int width_; + int height_; + bool rectified_; +}; + +// Stream Camera as text +std::ostream& operator<<(std::ostream& os, const CameraPinhole& camera); + +} // namespace nvblox + +#include "nvblox/core/impl/camera_pinhole_impl.h" diff --git a/nvblox/include/nvblox/core/cuda/image_operation.h b/nvblox/include/nvblox/core/cuda/image_operation.h new file mode 100644 index 000000000..c8df43019 --- /dev/null +++ b/nvblox/include/nvblox/core/cuda/image_operation.h @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include + +#include "nvblox/core/oslidar.h" + +namespace nvblox { +namespace cuda { + +__host__ __device__ inline int idivup(int a, int b) { + return ((a % b) != 0) ? (a / b + 1) : (a / b); +} + +// NOTE(gogojjh): we cannot directly give the value from CPU and avoid +// dangerous operations, like bug1: int w_ = lidar.num_azimuth_divisions(); +// bug2: int h_ = lidar.num_elevation_divisions(); +// bug3: cannot print in the loop +// we cannot correctly use values of w_ and h_ +__global__ void computeNormalImageOSLidar(const float* depth_image, + const float* height_image, + float* normal_image, const int w, + const int h, + const float rads_per_pixel_azimuth, + const float rads_per_pixel_elevation); +/** + * Get and compute the normal image with the CUDA + * @param {OSLidar} lidar : + */ +void getNormalImageOSLidar(OSLidar& lidar); + +/** + * Free the CUDA memory allocated + * @param {OSLidar} lidar : + */ +void freeNormalImageOSLidar(OSLidar& lidar); + +} // namespace cuda +} // namespace nvblox diff --git a/nvblox/include/nvblox/core/frustum.h b/nvblox/include/nvblox/core/frustum.h new file mode 100644 index 000000000..2e14ef311 --- /dev/null +++ b/nvblox/include/nvblox/core/frustum.h @@ -0,0 +1,55 @@ +#pragma once + +#include "nvblox/core/types.h" + +namespace nvblox { + +/// A bounding plane which has one "inside" direction and the other direction is +/// "outside." Quick tests for which side of the plane you are on. +class BoundingPlane { + public: + BoundingPlane() : normal_(Vector3f::Identity()), distance_(0.0f) {} + + void setFromPoints(const Vector3f& p1, const Vector3f& p2, + const Vector3f& p3); + void setFromDistanceNormal(const Vector3f& normal, float distance); + + /// Is the point on the correct side of the bounding plane? + bool isPointInside(const Vector3f& point) const; + + const Vector3f& normal() const { return normal_; } + float distance() const { return distance_; } + + private: + Vector3f normal_; + float distance_; +}; + +/// Class that allows checking for whether objects are within the field of view +/// of a camera or not. +class Frustum { + public: + // Frustum must be initialized with a camera and min and max depth and pose. + template + Frustum(const CameraType& camera, const Transform& T_L_C, float min_depth, + float max_depth); + + AxisAlignedBoundingBox getAABB() const { return aabb_; } + + bool isPointInView(const Vector3f& point) const; + bool isAABBInView(const AxisAlignedBoundingBox& aabb) const; + + private: + // Helper functions to do the actual computations. + void computeBoundingPlanes(const Eigen::Matrix& corners_C, + const Transform& T_L_C); + + /// Bounding planes containing around the frustum. Expressed in the layer + /// coordinate frame. + std::array bounding_planes_L_; + + /// Cached AABB of the + AxisAlignedBoundingBox aabb_; +}; + +} // namespace nvblox \ No newline at end of file diff --git a/nvblox/include/nvblox/core/image.h b/nvblox/include/nvblox/core/image.h index 453c1f47e..cec0297d3 100644 --- a/nvblox/include/nvblox/core/image.h +++ b/nvblox/include/nvblox/core/image.h @@ -17,6 +17,7 @@ limitations under the License. #include +#include #include "nvblox/core/color.h" #include "nvblox/core/types.h" #include "nvblox/core/unified_ptr.h" @@ -133,7 +134,9 @@ class Image { }; using DepthImage = Image; +using NormalImage = Image; using ColorImage = Image; +using CoorImage = Image; // Image Reductions namespace image { diff --git a/nvblox/include/nvblox/core/impl/camera_pinhole_impl.h b/nvblox/include/nvblox/core/impl/camera_pinhole_impl.h new file mode 100644 index 000000000..bed66ee53 --- /dev/null +++ b/nvblox/include/nvblox/core/impl/camera_pinhole_impl.h @@ -0,0 +1,110 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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 + +namespace nvblox { + +CameraPinhole::CameraPinhole(const Matrix3f& K, const int width, + const int height) + : K_(K), width_(width), height_(height), rectified_(false) { + P_.setIdentity(); + R_rect_.setIdentity(); +} + +CameraPinhole::CameraPinhole(const Matrix3x4f& P, const Matrix3f& rect, + const int width, const int height) + : P_(P), R_rect_(rect), width_(width), height_(height), rectified_(true) { + K_.setIdentity(); +} + +bool CameraPinhole::project(const Eigen::Vector3f& p_C, + Eigen::Vector2f* u_C) const { + // Point is behind the camera. + if (p_C.z() <= 0.0f) { + return false; + } + if (rectified_) { + Vector3f p = P_.block<3, 3>(0, 0) * (R_rect_ * p_C) + P_.block<3, 1>(0, 3); + *u_C = p.head<2>() / p(2); + if (u_C->x() > width_ || u_C->y() > height_ || u_C->x() < 0 || + u_C->y() < 0) { + return false; + } + } else { + Vector3f p = K_ * p_C; + *u_C = p.head<2>() / p(2); + if (u_C->x() > width_ || u_C->y() > height_ || u_C->x() < 0 || + u_C->y() < 0) { + return false; + } + } + return true; +} + +float CameraPinhole::getDepth(const Vector3f& p_C) const { return p_C.z(); } + +Vector3f CameraPinhole::unprojectFromImagePlaneCoordinates( + const Vector2f& u_C, const float depth) const { + return depth * vectorFromImagePlaneCoordinates(u_C); +} + +Vector3f CameraPinhole::unprojectFromPixelIndices(const Index2D& u_C, + const float depth) const { + return depth * vectorFromPixelIndices(u_C); +} + +Vector3f CameraPinhole::vectorFromImagePlaneCoordinates( + const Vector2f& u_C) const { + // NOTE(alexmillane): We allow u_C values up to the outer edges of pixels, + // such that: + // 0.0f < u_C[0] <= width + // 0.0f < u_C[1] <= height + if (rectified_) { + Matrix3f P_tmp = P_.block<3, 3>(0, 0); + Vector3f x(u_C[0], u_C[1], 1.0f); + Vector3f p = P_tmp.inverse() * x; + p /= p(2); + return p; + } else { + Vector3f x(u_C[0], u_C[1], 1.0f); + Vector3f p = K_.inverse() * x; + p /= p(2); + return p; + } +} + +Vector3f CameraPinhole::vectorFromPixelIndices(const Index2D& u_C) const { + // NOTE(alexmillane): The +0.5 here takes us from image plane indices, which + // are equal to the coordinates of the lower pixel corner, to the pixel + // center. + return vectorFromImagePlaneCoordinates(u_C.cast() + + Vector2f(0.5, 0.5)); +} + +CameraPinhole CameraPinhole::fromIntrinsicsMatrix(const Eigen::Matrix3f& mat, + int width, int height) { + return CameraPinhole(mat, width, height); +} + +CameraPinhole CameraPinhole::fromIntrinsicsMatrix(const Matrix3x4f& P, + const Matrix3f& rect, + int width, int height) { + return CameraPinhole(P, rect, width, height); +} + +} // namespace nvblox diff --git a/nvblox/include/nvblox/core/impl/interpolation_2d_impl.h b/nvblox/include/nvblox/core/impl/interpolation_2d_impl.h index 705438eb2..4e6acde31 100644 --- a/nvblox/include/nvblox/core/impl/interpolation_2d_impl.h +++ b/nvblox/include/nvblox/core/impl/interpolation_2d_impl.h @@ -119,7 +119,8 @@ bool interpolate2DLinear(const Image& frame, const Vector2f& u_px, template bool interpolate2DClosest(const ElementType* frame, const Vector2f& u_px, const int rows, const int cols, - ElementType* value_interpolated_ptr, Index2D* u_px_closest_ptr) { + ElementType* value_interpolated_ptr, + Index2D* u_px_closest_ptr) { // Closest pixel const Index2D u_M_rounded = u_px.array().round().cast(); // Check bounds: @@ -135,17 +136,18 @@ bool interpolate2DClosest(const ElementType* frame, const Vector2f& u_px, return false; } *value_interpolated_ptr = pixel_value; - if(u_px_closest_ptr) { + if (u_px_closest_ptr) { *u_px_closest_ptr = u_M_rounded; } return true; } +/// @brief interpolate the pixel value given float coordinates template -bool interpolate2DLinear(const ElementType* frame, const Vector2f& u_px, - const int rows, const int cols, - ElementType* value_interpolated_ptr, - Interpolation2DNeighbours* neighbours_ptr) { +bool interpolate2DLinear( + const ElementType* frame, const Vector2f& u_px, const int rows, + const int cols, ElementType* value_interpolated_ptr, + Interpolation2DNeighbours* neighbours_ptr) { // Subtraction of Vector2f(0.5, 0.5) takes our coordinates from // corner-referenced to center-referenced. const Vector2f u_center_referenced_px = u_px - Vector2f(0.5, 0.5); diff --git a/nvblox/include/nvblox/core/impl/lidar_impl.h b/nvblox/include/nvblox/core/impl/lidar_impl.h index 4e0b9ddf3..51d155eeb 100644 --- a/nvblox/include/nvblox/core/impl/lidar_impl.h +++ b/nvblox/include/nvblox/core/impl/lidar_impl.h @@ -14,6 +14,14 @@ See the License for the specific language governing permissions and limitations under the License. */ +/* +NOTE(gogojjh): +This implements the operation that project lidar points onto a depth image +This implementation is only valid for lidars that have average elevation angle + i.e., VLP16, otherwise, the lidar_impl.h should be rewritten for a specific + lidar type +*/ + #pragma once #include "math.h" @@ -23,9 +31,10 @@ limitations under the License. namespace nvblox { Lidar::Lidar(int num_azimuth_divisions, int num_elevation_divisions, - float vertical_fov_rad) + float horizontal_fov_rad, float vertical_fov_rad) : num_azimuth_divisions_(num_azimuth_divisions), num_elevation_divisions_(num_elevation_divisions), + horizontal_fov_rad_(horizontal_fov_rad), vertical_fov_rad_(vertical_fov_rad) { // Even numbers of beams allowed CHECK(num_azimuth_divisions_ % 2 == 0); @@ -35,9 +44,9 @@ Lidar::Lidar(int num_azimuth_divisions, int num_elevation_divisions, // This is because in the azimuth direction there's a wrapping around. The // point at pi/-pi is not double sampled, generating this difference. rads_per_pixel_elevation_ = - vertical_fov_rad / static_cast(num_elevation_divisions_ - 1); + vertical_fov_rad_ / static_cast(num_elevation_divisions_ - 1); rads_per_pixel_azimuth_ = - 2.0f * M_PI / static_cast(num_azimuth_divisions_); + horizontal_fov_rad_ / static_cast(num_azimuth_divisions_); // Inverse of the above elevation_pixels_per_rad_ = 1.0f / rads_per_pixel_elevation_; @@ -49,7 +58,13 @@ Lidar::Lidar(int num_azimuth_divisions, int num_elevation_divisions, // below this. // Note(alexmillane): Note that we use polar angle here, not elevation. // Polar is from the top of the sphere down, elevation, the middle up. - start_polar_angle_rad_ = M_PI / 2.0f - (vertical_fov_rad / 2.0f + + // ********************* polar_angle + // ****** the start polar_angle indicate the direction: x=0, +z + // ****** the end polar_angle indicate the direction: x=0, -z + // ********************* azimuth_angle + // ****** the start and end azimuth_angle: counterclockwise + // -x, y=0 -> +x, y=0 + start_polar_angle_rad_ = M_PI / 2.0f - (vertical_fov_rad_ / 2.0f + rads_per_pixel_elevation_ / 2.0f); start_azimuth_angle_rad_ = -M_PI - rads_per_pixel_azimuth_ / 2.0f; } diff --git a/nvblox/include/nvblox/core/impl/oslidar_impl.h b/nvblox/include/nvblox/core/impl/oslidar_impl.h new file mode 100644 index 000000000..b0173d15a --- /dev/null +++ b/nvblox/include/nvblox/core/impl/oslidar_impl.h @@ -0,0 +1,257 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ + +/* +NOTE(gogojjh): +This implements the operation that project OSLidar points onto a depth image +*/ + +#pragma once + +#include "math.h" + +#include + +namespace nvblox { + +// given initial intrinsics from the intrisics file, but need to be refined for +// each new scan +OSLidar::OSLidar(int num_azimuth_divisions, int num_elevation_divisions, + float horizontal_fov_rad, float vertical_fov_rad, + float start_azimuth_angle_rad, float end_azimuth_angle_rad, + float start_elevation_angle_rad, float end_elevation_angle_rad) + : num_azimuth_divisions_(num_azimuth_divisions), + num_elevation_divisions_(num_elevation_divisions), + horizontal_fov_rad_(horizontal_fov_rad), + vertical_fov_rad_(vertical_fov_rad), + start_azimuth_angle_rad_(start_azimuth_angle_rad), + end_azimuth_angle_rad_(end_azimuth_angle_rad), + start_elevation_angle_rad_(start_elevation_angle_rad), + end_elevation_angle_rad_(end_elevation_angle_rad) { + // Even numbers of beams allowed + CHECK(num_azimuth_divisions_ % 2 == 0); + + rads_per_pixel_elevation_ = + vertical_fov_rad_ / static_cast(num_elevation_divisions_ - 1); + rads_per_pixel_azimuth_ = + horizontal_fov_rad_ / static_cast(num_azimuth_divisions_ - 1); + + // Inverse of the above + elevation_pixels_per_rad_ = 1.0f / rads_per_pixel_elevation_; + azimuth_pixels_per_rad_ = 1.0f / rads_per_pixel_azimuth_; + + // printIntrinsics(); + + depth_image_ptr_cuda_ = nullptr; + height_image_ptr_cuda_ = nullptr; + normal_image_ptr_cuda_ = nullptr; +} + +OSLidar::~OSLidar() {} + +void OSLidar::printIntrinsics() const { + printf("OSLidar intrinsics--------------------\n"); + printf("horizontal_fov_rad: %f\n", horizontal_fov_rad_); + printf("vertical_fov_rad: %f\n", vertical_fov_rad_); + printf("start_elevation: %f\n", start_elevation_angle_rad_); + printf("end_elevation: %f\n", end_elevation_angle_rad_); + printf("rads_per_pixel_elevation: %f\n", rads_per_pixel_elevation_); + printf("rads_per_pixel_azimuth: %f\n", rads_per_pixel_azimuth_); +} + +/********************************************** + * get the parameters of OSLidar + **********************************************/ +int OSLidar::num_azimuth_divisions() const { return num_azimuth_divisions_; } + +int OSLidar::num_elevation_divisions() const { + return num_elevation_divisions_; +} + +float OSLidar::vertical_fov_rad() const { return vertical_fov_rad_; } + +float OSLidar::horizontal_fov_rad() const { return horizontal_fov_rad_; } + +float OSLidar::rads_per_pixel_elevation() const { + return rads_per_pixel_elevation_; +} + +float OSLidar::rads_per_pixel_azimuth() const { + return rads_per_pixel_azimuth_; +} + +int OSLidar::numel() const { + return num_azimuth_divisions_ * num_elevation_divisions_; +} + +int OSLidar::cols() const { return num_azimuth_divisions_; } + +int OSLidar::rows() const { return num_elevation_divisions_; } + +float OSLidar::getDepth(const Vector3f& p_C) const { return p_C.norm(); } + +// NOTE(gogojjh): this function is added by gogojjh +Vector3f OSLidar::getNormalVector(const Index2D& u_C) const { + if (normal_image_ptr_cuda_) { + float x = normal_image_ptr_cuda_[3 * (u_C.y() * num_azimuth_divisions_ + + u_C.x())]; + float y = normal_image_ptr_cuda_[3 * (u_C.y() * num_azimuth_divisions_ + + u_C.x()) + + 1]; + float z = normal_image_ptr_cuda_[3 * (u_C.y() * num_azimuth_divisions_ + + u_C.x()) + + 2]; + return Vector3f(x, y, z); + } else { + return Vector3f(0.0f, 0.0f, 0.0f); + } +} + +/********************************************** + * Project a 3D point p_C to get the image coordinates u_C + **********************************************/ +bool OSLidar::project(const Vector3f& p_C, Vector2f* u_C) const { + const float r = p_C.norm(); + constexpr float kMinProjectionEps = 0.01; + if (r < kMinProjectionEps) return false; + + const float elevation_angle_rad = acos(p_C.z() / r); + const float azimuth_angle_rad = M_PI - atan2(p_C.y(), p_C.x()); + float v_float = (elevation_angle_rad - start_elevation_angle_rad_) / + rads_per_pixel_elevation_; + float u_float = + (azimuth_angle_rad - start_azimuth_angle_rad_) / rads_per_pixel_azimuth_; + + // Catch wrap around issues. + if (u_float >= num_azimuth_divisions_) { + u_float -= num_azimuth_divisions_; + } + + // Points out of FOV + // NOTE(alexmillane): It should be impossible to escape the -pi-to-pi range in + // azimuth due to wrap around this. Therefore we don't check. + if ((round(v_float) < 0) || (round(v_float) > num_elevation_divisions_ - 1)) { + return false; + } + + // Write output + *u_C = Vector2f(u_float, v_float); + return true; +} + +bool OSLidar::project(const Vector3f& p_C, Index2D* u_C) const { + Vector2f u_C_float; + bool res = project(p_C, &u_C_float); + *u_C = u_C_float.array().round().matrix().cast(); + return res; +} + +/********************************************** + * transformation between the pixel index (int) and image coordinates (float) + **********************************************/ +Vector2f OSLidar::pixelIndexToImagePlaneCoordsOfCenter( + const Index2D& u_C) const { + // The index cast to a float is the coordinates of the lower corner of the + // pixel. + return u_C.cast(); +} + +Index2D OSLidar::imagePlaneCoordsToPixelIndex(const Vector2f& u_C) const { + // NOTE(alexmillane): We do round rather than a straight truncation such that + // we handle negative image plane coordinates. + return u_C.array().round().cast(); +} + +/********************************************** + * Unproject a 2D image coordinates u_C to a 3D point p_C + **********************************************/ +Vector3f OSLidar::unprojectFromImagePlaneCoordinates(const Vector2f& u_C, + const float depth) const { + return depth * vectorFromImagePlaneCoordinates(u_C); +} + +Vector3f OSLidar::unprojectFromPixelIndices(const Index2D& u_C, + const float depth) const { + return depth * vectorFromPixelIndices(u_C); +} + +Vector3f OSLidar::unprojectFromImageIndex(const Index2D& u_C) const { + float height = image::access(u_C.y(), u_C.x(), num_azimuth_divisions_, + height_image_ptr_cuda_); + float depth = image::access(u_C.y(), u_C.x(), num_azimuth_divisions_, + depth_image_ptr_cuda_); + float r = sqrt(depth * depth - height * height); + float azimuth_angle_rad = M_PI - u_C.x() * rads_per_pixel_azimuth_; + Vector3f p(r * cos(azimuth_angle_rad), r * sin(azimuth_angle_rad), height); + return p; +} + +Vector3f OSLidar::vectorFromImagePlaneCoordinates(const Vector2f& u_C) const { + float height = + image::access(round(u_C.y()), round(u_C.x()), + num_azimuth_divisions_, height_image_ptr_cuda_); + float depth = + image::access(round(u_C.y()), round(u_C.x()), + num_azimuth_divisions_, depth_image_ptr_cuda_); + float r = sqrt(depth * depth - height * height); + float azimuth_angle_rad = M_PI - u_C.x() * rads_per_pixel_azimuth_; + Vector3f p(r * cos(azimuth_angle_rad), r * sin(azimuth_angle_rad), height); + return p / depth; +} + +Vector3f OSLidar::vectorFromPixelIndices(const Index2D& u_C) const { + return vectorFromImagePlaneCoordinates( + pixelIndexToImagePlaneCoordsOfCenter(u_C)); +} + +/********************************************** + * Project a 3D point p_C to get the image coordinates u_C + **********************************************/ +AxisAlignedBoundingBox OSLidar::getViewAABB(const Transform& T_L_C, + const float min_depth, + const float max_depth) const { + // The AABB is a square centered at the OSLidars location where the height is + // determined by the OSLidar FoV. + // NOTE(alexmillane): The min depth is ignored in this function, it is a + // parameter so it matches with camera's getViewAABB() + AxisAlignedBoundingBox box( + Vector3f(-max_depth, -max_depth, + -max_depth * sin(vertical_fov_rad_ / 2.0f)), + Vector3f(max_depth, max_depth, + max_depth * sin(vertical_fov_rad_ / 2.0f))); + + // Translate the box to the sensor's location (note that orientation doesn't + // matter as the OSLidar sees in the circle) + box.translate(T_L_C.translation()); + return box; +} + +size_t OSLidar::Hash::operator()(const OSLidar& OSLidar) const { + // Taken from: + // https://stackoverflow.com/questions/17016175/c-unordered-map-using-a-custom-class-type-as-the-key + size_t az_hash = std::hash()(OSLidar.num_azimuth_divisions_); + size_t el_hash = std::hash()(OSLidar.num_elevation_divisions_); + size_t fov_hash = std::hash()(OSLidar.vertical_fov_rad_); + return ((az_hash ^ (el_hash << 1)) >> 1) ^ (fov_hash << 1); +} + +bool operator==(const OSLidar& lhs, const OSLidar& rhs) { + return (lhs.num_azimuth_divisions_ == rhs.num_azimuth_divisions_) && + (lhs.num_elevation_divisions_ == rhs.num_elevation_divisions_) && + (std::fabs(lhs.vertical_fov_rad_ - rhs.vertical_fov_rad_) < + std::numeric_limits::epsilon()); +} +} // namespace nvblox diff --git a/nvblox/include/nvblox/core/lidar.h b/nvblox/include/nvblox/core/lidar.h index 7c07e0c3d..ddb00604f 100644 --- a/nvblox/include/nvblox/core/lidar.h +++ b/nvblox/include/nvblox/core/lidar.h @@ -24,6 +24,7 @@ class Lidar { Lidar() = delete; __host__ __device__ inline Lidar(int num_azimuth_divisions, int num_elevation_divisions, + float horizontal_fov_rad, float vertical_fov_rad); __host__ __device__ inline ~Lidar() = default; @@ -81,6 +82,7 @@ class Lidar { // Core parameters int num_azimuth_divisions_; int num_elevation_divisions_; + float horizontal_fov_rad_; float vertical_fov_rad_; // Dependent parameters diff --git a/nvblox/include/nvblox/core/mapper.h b/nvblox/include/nvblox/core/mapper.h index ab4e4cc21..ee4ea08d3 100644 --- a/nvblox/include/nvblox/core/mapper.h +++ b/nvblox/include/nvblox/core/mapper.h @@ -19,11 +19,13 @@ limitations under the License. #include "nvblox/core/blox.h" #include "nvblox/core/camera.h" +#include "nvblox/core/camera_pinhole.h" #include "nvblox/core/common_names.h" #include "nvblox/core/hash.h" #include "nvblox/core/layer.h" #include "nvblox/core/layer_cake.h" #include "nvblox/core/lidar.h" +#include "nvblox/core/oslidar.h" #include "nvblox/core/voxels.h" #include "nvblox/integrators/esdf_integrator.h" #include "nvblox/integrators/projective_color_integrator.h" @@ -94,8 +96,9 @@ class RgbdMapper : public MapperBase { ///@param T_L_C Pose of the camera, specified as a transform from Camera-frame /// to Layer-frame transform. ///@param camera Intrinsics model of the camera. + template void integrateColor(const ColorImage& color_frame, const Transform& T_L_C, - const Camera& camera); + const CameraType& camera); /// Integrates a 3D LiDAR scan into the reconstruction. ///@param depth_frame Depth image representing the LiDAR scan. To convert a @@ -106,6 +109,15 @@ class RgbdMapper : public MapperBase { void integrateLidarDepth(const DepthImage& depth_frame, const Transform& T_L_C, const Lidar& lidar); + /// Integrates a 3D LiDAR scan into the reconstruction. + ///@param depth_frame Depth image representing the LiDAR scan. To convert a + /// lidar scan to a DepthImage see TODOOO. + ///@param T_L_C Pose of the LiDAR, specified as a transform from LiDAR-frame + /// to Layer-frame transform. + ///@param lidar Intrinsics model of the Ouster LiDAR. + void integrateOSLidarDepth(DepthImage& depth_frame, const Transform& T_L_C, + OSLidar& oslidar); + /// Updates the mesh blocks which require an update /// @return The indices of the blocks that were updated in this call. std::vector updateMesh(); @@ -251,6 +263,7 @@ class RgbdMapper : public MapperBase { /// updateEsdfSlice(). This member tracks which mode we're in. EsdfMode esdf_mode_ = EsdfMode::kUnset; + /// TODO(gogojjh): how to modify these integrators for large-scale mapping? /// Integrators ProjectiveTsdfIntegrator tsdf_integrator_; ProjectiveTsdfIntegrator lidar_tsdf_integrator_; diff --git a/nvblox/include/nvblox/core/oslidar.h b/nvblox/include/nvblox/core/oslidar.h new file mode 100644 index 000000000..a7b95b1f7 --- /dev/null +++ b/nvblox/include/nvblox/core/oslidar.h @@ -0,0 +1,166 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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/core/image.h" +#include "nvblox/core/types.h" + +namespace nvblox { +// NOTE(gogojjh): +// image coordinates to pixel indices +// return u_C.array().floor().cast(); +// image angle order: +// from left to right: 0 -> 2pi +// fron bottom to top: phi_min -> phi_max +class OSLidar { + public: + __host__ __device__ inline OSLidar() = default; + __host__ __device__ inline OSLidar( + int num_azimuth_divisions, int num_elevation_divisions, + float horizontal_fov_rad, float vertical_fov_rad, + float start_azimuth_angle_rad, float end_azimuth_angle_rad, + float start_elevation_angle_rad, float end_elevation_angle_rad); + __host__ __device__ inline ~OSLidar(); + + // __host__ __device__ inline void setIntrinsics(); + __host__ __device__ inline void printIntrinsics() const; + + // NOTE(gogojjh): This function is used to check whether p_C (in the camera + // coordinate) is projected on the the image plane, or outside the + // OSLidar's FOV Projects a 3D point to the (floating-point) image plane + __host__ __device__ inline bool project(const Vector3f& p_C, + Vector2f* u_C) const; + + // Projects a 3D point to the (index-based) image plane + __host__ __device__ inline bool project(const Vector3f& p_C, + Index2D* u_C) const; + + // Gets the depth of a point + __host__ __device__ inline float getDepth(const Vector3f& p_C) const; + + // Gets the normal vector of a point + __host__ __device__ inline Vector3f getNormalVector(const Index2D& u_C) const; + + // NOTE(gogojjh): This function is used to unproject a pixel to a 3D point + // given the 2D coordinate of an image Back projection (image plane point to + // 3D point) represented in the lidar coordinate system + __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; + __host__ __device__ inline Vector3f unprojectFromImageIndex( + const Index2D& u_C) const; + + // Back projection (image plane point to ray) + __host__ __device__ inline Vector3f vectorFromImagePlaneCoordinates( + const Vector2f& u_C) const; + __host__ __device__ inline Vector3f vectorFromPixelIndices( + const Index2D& u_C) const; + + // Conversions between pixel indices and image plane coordinates + __host__ __device__ inline Vector2f pixelIndexToImagePlaneCoordsOfCenter( + const Index2D& u_C) const; + __host__ __device__ inline Index2D imagePlaneCoordsToPixelIndex( + const Vector2f& u_C) const; + + __host__ __device__ void setDepthFrameCUDA(float* depth_image_ptr_cuda) { + depth_image_ptr_cuda_ = depth_image_ptr_cuda; + } + + __host__ __device__ void setHeightFrameCUDA(float* height_image_ptr_cuda) { + height_image_ptr_cuda_ = height_image_ptr_cuda; + } + + __host__ __device__ void setNormalFrameCUDA(float* normal_image_ptr_cuda) { + normal_image_ptr_cuda_ = normal_image_ptr_cuda; + } + + __host__ __device__ inline float* getDepthFrameCUDA() const { + return depth_image_ptr_cuda_; + } + + __host__ __device__ inline float* getHeightFrameCUDA() const { + return height_image_ptr_cuda_; + } + + __host__ __device__ inline float* getNormalFrameCUDA() const { + return normal_image_ptr_cuda_; + } + + // View + __host__ inline AxisAlignedBoundingBox getViewAABB( + const Transform& T_L_C, const float min_depth, + const float max_depth) const; + + __host__ __device__ inline int num_azimuth_divisions() const; + __host__ __device__ inline int num_elevation_divisions() const; + __host__ __device__ inline float horizontal_fov_rad() const; + __host__ __device__ inline float vertical_fov_rad() const; + __host__ __device__ inline float rads_per_pixel_elevation() const; + __host__ __device__ inline float rads_per_pixel_azimuth() const; + __host__ __device__ inline int numel() const; + __host__ __device__ inline int rows() const; + __host__ __device__ inline int cols() const; + + // Equality + __host__ inline friend bool operator==(const OSLidar& lhs, + const OSLidar& rhs); + + // Hash + struct Hash { + __host__ inline size_t operator()(const OSLidar& OSLidar) const; + }; + + private: + float* depth_image_ptr_cuda_; + float* height_image_ptr_cuda_; + float* normal_image_ptr_cuda_; + + // Core parameters + int num_azimuth_divisions_; + int num_elevation_divisions_; + float horizontal_fov_rad_; + float vertical_fov_rad_; + + // Angular distance between pixels + // Note(alexmillane): Note the difference in division by N vs. (N-1) below. + // This is because in the azimuth direction there's a wrapping around. The + // point at pi/-pi is not double sampled, generating this difference. + float start_azimuth_angle_rad_; + float end_azimuth_angle_rad_; + + // ********************* elevation_angle + // ****** the start elevation_angle indicate the direction: x=0, +z + // ****** the end elevation_angle indicate the direction: x=0, -z + // ********************* azimuth_angle + // ****** the start and end azimuth_angle: clockwise + // -x, y=0 -> +x, y=0 + float start_elevation_angle_rad_; + float end_elevation_angle_rad_; + + // Dependent parameters + float elevation_pixels_per_rad_; + float azimuth_pixels_per_rad_; + float rads_per_pixel_elevation_; + float rads_per_pixel_azimuth_; +}; + +// Equality +__host__ inline bool operator==(const OSLidar& lhs, const OSLidar& rhs); + +} // namespace nvblox + +#include "nvblox/core/impl/oslidar_impl.h" diff --git a/nvblox/include/nvblox/core/types.h b/nvblox/include/nvblox/core/types.h index 2ac3d1037..295e087f4 100644 --- a/nvblox/include/nvblox/core/types.h +++ b/nvblox/include/nvblox/core/types.h @@ -34,9 +34,15 @@ enum class DeviceType { kCPU, kGPU }; enum class MemoryType { kDevice, kUnified, kHost }; inline std::string toString(MemoryType memory_type) { switch (memory_type) { - case MemoryType::kDevice: return "kDevice"; break; - case MemoryType::kUnified: return "kUnified"; break; - default: return "kHost"; break; + case MemoryType::kDevice: + return "kDevice"; + break; + case MemoryType::kUnified: + return "kUnified"; + break; + default: + return "kHost"; + break; } } @@ -50,14 +56,20 @@ typedef Eigen::AlignedBox3f AxisAlignedBoundingBox; typedef Eigen::Isometry3f Transform; +// NOTE(gogojjh): add other types of vectors/matrices +typedef Eigen::Matrix Vector4f; +typedef Eigen::Matrix3f Matrix3f; +typedef Eigen::Matrix4f Matrix4f; +typedef Eigen::Matrix Matrix3x4f; + // This can be replaced with std::byte once we go to C++17. -typedef uint8_t Byte; +typedef uint8_t Byte; // Aligned Eigen containers template using AlignedVector = std::vector>; -enum class InterpolationType { kNearestNeighbor, kLinear}; +enum class InterpolationType { kNearestNeighbor, kLinear }; typedef Eigen::ParametrizedLine Ray; diff --git a/nvblox/include/nvblox/core/voxels.h b/nvblox/include/nvblox/core/voxels.h index 02fc8b3a3..007a45ebb 100644 --- a/nvblox/include/nvblox/core/voxels.h +++ b/nvblox/include/nvblox/core/voxels.h @@ -26,6 +26,9 @@ struct TsdfVoxel { float distance = 0.0f; // How many observations/how confident we are in this observation. float weight = 0.0f; + // ADD(gogojjh): for the implementation of signed distance gradient, its + // direction is from the surface toward the sensor // NOLINT + Eigen::Vector3f gradient = Eigen::Vector3f::Zero(); }; struct EsdfVoxel { diff --git a/nvblox/include/nvblox/integrators/internal/cuda/impl/projective_integrators_common_impl.cuh b/nvblox/include/nvblox/integrators/internal/cuda/impl/projective_integrators_common_impl.cuh index 5352d111e..d825f2243 100644 --- a/nvblox/include/nvblox/integrators/internal/cuda/impl/projective_integrators_common_impl.cuh +++ b/nvblox/include/nvblox/integrators/internal/cuda/impl/projective_integrators_common_impl.cuh @@ -18,6 +18,19 @@ limitations under the License. namespace nvblox { template +// NOTE(gogojjh): this function project a voxel in the world onto the camera +// plane to check whether the voxel is visible or not +/* + * This function projects a voxel in the world onto the camera plane to check + whether the voxel is visible or not + * @param [in] block_indices_device_ptr: + * @param [in] sensor: the sensor object, like the camera, lidar, etc + * @param [in] T_C_L: the transformation from the camera to the world + * @param [in] block_size: + * @param [out] u_px_ptr: the image coordinates projected from the voxel + * @param [out] u_depth_ptr: the depth of the 3D points + * @param [out] p_voxel_center_C_ptr: the voxel in the world coordinate system +*/ __device__ inline bool projectThreadVoxel( const Index3D* block_indices_device_ptr, const SensorType& sensor, const Transform& T_C_L, const float block_size, Eigen::Vector2f* u_px_ptr, diff --git a/nvblox/include/nvblox/integrators/internal/cuda/projective_integrators_common.cuh b/nvblox/include/nvblox/integrators/internal/cuda/projective_integrators_common.cuh index 558ab1503..5e4f6def5 100644 --- a/nvblox/include/nvblox/integrators/internal/cuda/projective_integrators_common.cuh +++ b/nvblox/include/nvblox/integrators/internal/cuda/projective_integrators_common.cuh @@ -21,7 +21,6 @@ limitations under the License. #include "nvblox/core/types.h" namespace nvblox { - /// Camera projection of a voxel onto the image plane. /// Projects the center of the voxel associated with this GPU block/thread into /// the image plane. Internally uses threadIdx and blockIdx to select the diff --git a/nvblox/include/nvblox/integrators/projective_color_integrator.h b/nvblox/include/nvblox/integrators/projective_color_integrator.h index 2d0227fb7..87eb28161 100644 --- a/nvblox/include/nvblox/integrators/projective_color_integrator.h +++ b/nvblox/include/nvblox/integrators/projective_color_integrator.h @@ -49,8 +49,9 @@ class ProjectiveColorIntegrator : public ProjectiveIntegratorBase { /// intergrated. /// @param updated_blocks Optional pointer to a vector which will contain the /// 3D indices of blocks affected by the integration. + template void integrateFrame(const ColorImage& color_frame, const Transform& T_L_C, - const Camera& camera, const TsdfLayer& tsdf_layer, + const CameraType& camera, const TsdfLayer& tsdf_layer, ColorLayer* color_layer, std::vector* updated_blocks = nullptr); @@ -82,10 +83,11 @@ class ProjectiveColorIntegrator : public ProjectiveIntegratorBase { protected: // Given a set of blocks in view (block_indices) perform color updates on all // voxels within these blocks on the GPU. + template void updateBlocks(const std::vector& block_indices, const ColorImage& color_frame, const DepthImage& depth_frame, const Transform& T_L_C, - const Camera& camera, const float truncation_distance_m, + const CameraType& camera, const float truncation_distance_m, ColorLayer* layer); // Takes a list of block indices and returns a subset containing the block diff --git a/nvblox/include/nvblox/integrators/projective_integrator_base.h b/nvblox/include/nvblox/integrators/projective_integrator_base.h index 71c3b5903..32e4e695e 100644 --- a/nvblox/include/nvblox/integrators/projective_integrator_base.h +++ b/nvblox/include/nvblox/integrators/projective_integrator_base.h @@ -18,6 +18,7 @@ limitations under the License. #include #include "nvblox/core/camera.h" +#include "nvblox/core/camera_pinhole.h" #include "nvblox/core/image.h" #include "nvblox/core/types.h" #include "nvblox/integrators/view_calculator.h" @@ -82,7 +83,6 @@ class ProjectiveIntegratorBase { ViewCalculator& view_calculator(); protected: - // Truncation distance in meters for this block size. float truncation_distance_m(float block_size) const; diff --git a/nvblox/include/nvblox/integrators/projective_tsdf_integrator.h b/nvblox/include/nvblox/integrators/projective_tsdf_integrator.h index 4546cff4b..e2970ed2f 100644 --- a/nvblox/include/nvblox/integrators/projective_tsdf_integrator.h +++ b/nvblox/include/nvblox/integrators/projective_tsdf_integrator.h @@ -21,6 +21,7 @@ limitations under the License. #include "nvblox/core/image.h" #include "nvblox/core/layer.h" #include "nvblox/core/lidar.h" +#include "nvblox/core/oslidar.h" #include "nvblox/core/types.h" #include "nvblox/core/voxels.h" #include "nvblox/gpu_hash/gpu_layer_view.h" @@ -65,6 +66,19 @@ class ProjectiveTsdfIntegrator : public ProjectiveIntegratorBase { const Lidar& lidar, TsdfLayer* layer, std::vector* updated_blocks = nullptr); + /// Integrates a depth image in to the passed TSDF layer. + /// @param depth_frame A depth image. + /// @param T_L_C The pose of the camera. Supplied as a Transform mapping + /// points in the camera frame (C) to the layer frame (L). + /// @param lidar A the Ouster LiDAR model. + /// @param layer A pointer to the layer into which this observation will be + /// intergrated. + /// @param updated_blocks Optional pointer to a vector which will contain the + /// 3D indices of blocks affected by the integration. + void integrateFrame(DepthImage& depth_frame, const Transform& T_L_C, + OSLidar& oslidar, TsdfLayer* layer, + std::vector* updated_blocks = nullptr); + /// Blocks until GPU operations are complete /// Ensure outstanding operations are finished (relevant for integrators /// launching asynchronous work) @@ -102,12 +116,16 @@ class ProjectiveTsdfIntegrator : public ProjectiveIntegratorBase { float lidar_linear_interpolation_max_allowable_difference_vox_ = 2.0f; float lidar_nearest_interpolation_max_allowable_dist_to_ray_vox_ = 0.5f; + // NOTE(gogojjh): the main function to implement the GPU-based integration // Given a set of blocks in view (block_indices) perform TSDF updates on all // voxels within these blocks on the GPU. void integrateBlocks(const DepthImage& depth_frame, const Transform& T_C_L, const Camera& camera, TsdfLayer* layer_ptr); void integrateBlocks(const DepthImage& depth_frame, const Transform& T_C_L, const Lidar& lidar, TsdfLayer* layer_ptr); + void integrateBlocks(const DepthImage& depth_frame, const Transform& T_C_L, + const OSLidar& lidar, TsdfLayer* layer_ptr); + template void integrateBlocksTemplate(const std::vector& block_indices, const DepthImage& depth_frame, diff --git a/nvblox/include/nvblox/integrators/view_calculator.h b/nvblox/include/nvblox/integrators/view_calculator.h index c6afdbd58..abe6a5f78 100644 --- a/nvblox/include/nvblox/integrators/view_calculator.h +++ b/nvblox/include/nvblox/integrators/view_calculator.h @@ -18,8 +18,10 @@ limitations under the License. #include #include "nvblox/core/camera.h" +#include "nvblox/core/camera_pinhole.h" #include "nvblox/core/image.h" #include "nvblox/core/lidar.h" +#include "nvblox/core/oslidar.h" #include "nvblox/core/types.h" #include "nvblox/core/unified_vector.h" @@ -43,8 +45,9 @@ class ViewCalculator { /// @param block_size The size of the blocks in the layer. /// @param max_distance The maximum distance of blocks considered. /// @return a vector of the 3D indices of the blocks in view. + template static std::vector getBlocksInViewPlanes(const Transform& T_L_C, - const Camera& camera, + const CameraType& camera, const float block_size, const float max_distance); @@ -61,9 +64,10 @@ class ViewCalculator { /// @param truncation_distance_m The truncation distance. /// @param max_integration_distance_m The max integration distance. /// @return a vector of the 3D indices of the blocks in view. + template static std::vector getBlocksInImageViewPlanes( const DepthImage& depth_frame, const Transform& T_L_C, - const Camera& camera, const float block_size, + const CameraType& camera, const float block_size, const float truncation_distance_m, const float max_integration_distance_m); @@ -80,9 +84,10 @@ class ViewCalculator { /// @param truncation_distance_m The truncation distance. /// @param max_integration_distance_m The max integration distance. /// @return a vector of the 3D indices of the blocks in view. + template std::vector getBlocksInImageViewRaycast( const DepthImage& depth_frame, const Transform& T_L_C, - const Camera& camera, const float block_size, + const CameraType& camera, const float block_size, const float truncation_distance_m, const float max_integration_distance_m); @@ -104,6 +109,25 @@ class ViewCalculator { const float block_size, const float truncation_distance_m, const float max_integration_distance_m); + /// Gets blocks which fall into the lidar view (using a depth image) + /// Performs ray casting to get the blocks in view + /// Operates by ray through the grid returning the blocks traversed in the ray + /// casting process. The number of pixels on the image plane raycast is + /// determined by the class parameter raycast_subsampling_factor. + /// @param depth_frame the depth image. + /// @param T_L_C The pose of the camera. Supplied as a Transform mapping + /// points in the camera frame (C) to the layer frame (L). + /// @param Lidar The Ouster lidar (intrinsics) model. + /// @param block_size The size of the blocks in the layer. + /// @param truncation_distance_m The truncation distance. + /// @param max_integration_distance_m The max integration distance. + /// @return a vector of the 3D indices of the blocks in view. + std::vector getBlocksInImageViewRaycast( + const DepthImage& depth_frame, const Transform& T_L_C, + const OSLidar& lidar, const float block_size, + const float truncation_distance_m, + const float max_integration_distance_m); + /// A parameter getter /// The rate at which we subsample pixels to raycast. Note that we always /// raycast the edges of the frame, no matter the subsample rate. For example, @@ -145,6 +169,7 @@ class ViewCalculator { bool* aabb_updated_cuda); // Raycasts through (possibly subsampled) pixels in the image. + // aabb_updated_cuda is the updated variables template void getBlocksByRaycastingPixels( const Transform& T_L_C, // NOLINT diff --git a/nvblox/include/nvblox/io/impl/pointcloud_io_impl.h b/nvblox/include/nvblox/io/impl/pointcloud_io_impl.h index ff8a40186..56d4b5a35 100644 --- a/nvblox/include/nvblox/io/impl/pointcloud_io_impl.h +++ b/nvblox/include/nvblox/io/impl/pointcloud_io_impl.h @@ -21,6 +21,7 @@ limitations under the License. namespace nvblox { namespace io { +/// NOTE(gogojjh): This function outputs TSDF and ESDF map to a file /// Outputs a voxel layer as a pointcloud with the lambda function deciding the /// intensity. template @@ -32,7 +33,7 @@ bool outputVoxelLayerToPly( // Combine all the voxels in the mesh into a pointcloud. std::vector points; - std::vector intensities; + std::vector intensities; // NOTE(gogojjh): record the distance constexpr int kVoxelsPerSide = VoxelBlock::kVoxelsPerSide; const float block_size = layer.block_size(); @@ -87,5 +88,64 @@ bool outputVoxelLayerToPly(const EsdfLayer& layer, return outputVoxelLayerToPly(layer, filename, lambda); } +/// NOTE(gogojjh): This function outputs obstance information to a file +template +bool outputObstacleToPly( + const VoxelBlockLayer& layer, const std::string& filename, + std::function lambda) { + // Create a ply writer object. + io::PlyWriter writer(filename); + + // Combine all the voxels in the mesh into a pointcloud. + std::vector points; + std::vector intensities; + + constexpr int kVoxelsPerSide = VoxelBlock::kVoxelsPerSide; + const float block_size = layer.block_size(); + const float voxel_size = layer.voxel_size(); + + auto new_lambda = [&points, &intensities, &block_size, &voxel_size, &lambda]( + const Index3D& block_index, const Index3D& voxel_index, + const VoxelType* voxel) { + float intensity = 0.0f; + if (lambda(voxel, &intensity)) { + points.push_back(getCenterPostionFromBlockIndexAndVoxelIndex( + block_size, block_index, voxel_index)); + intensities.push_back(intensity); + } + }; + + // Call above lambda on every voxel in the layer. + callFunctionOnAllVoxels(layer, new_lambda); + + // Add the obstacle pointcloud to the ply writer. + const float OBSTACLE_DISTANCE_TH = 0.02f; + std::vector obs_points; + obs_points.reserve(points.size() / 10); + for (size_t i = 0; i < points.size(); i++) { + if (abs(intensities[i]) <= OBSTACLE_DISTANCE_TH) { + obs_points.push_back(points[i]); + } + } + writer.setPoints(&obs_points); + + // Write out the ply. + return writer.write(); +} + +/// Specialization for the obstacle information. +// template <> +bool outputObstacleToPly(const EsdfLayer& layer, const std::string& filename) { + const float voxel_size = layer.voxel_size(); + auto lambda = [&voxel_size](const EsdfVoxel* voxel, float* distance) -> bool { + *distance = voxel_size * std::sqrt(voxel->squared_distance_vox); + if (voxel->is_inside) { + *distance = -*distance; + } + return voxel->observed; + }; + return outputObstacleToPly(layer, filename, lambda); +} + } // namespace io } // namespace nvblox \ No newline at end of file diff --git a/nvblox/include/nvblox/io/pointcloud_io.h b/nvblox/include/nvblox/io/pointcloud_io.h index 01318728e..098d86b12 100644 --- a/nvblox/include/nvblox/io/pointcloud_io.h +++ b/nvblox/include/nvblox/io/pointcloud_io.h @@ -38,6 +38,12 @@ template bool outputVoxelLayerToPly(const VoxelBlockLayer& layer, const std::string& filename); +/// Without specifying a lambda, this outputs the point with smaller distance. +template +bool outputObstacleToPly( + const VoxelBlockLayer& layer, const std::string& filename, + std::function lambda); + /// Specializations for the TSDF type. template <> bool outputVoxelLayerToPly(const TsdfLayer& layer, const std::string& filename); @@ -46,6 +52,10 @@ bool outputVoxelLayerToPly(const TsdfLayer& layer, const std::string& filename); template <> bool outputVoxelLayerToPly(const EsdfLayer& layer, const std::string& filename); +/// Specialization for the obstacle information. +// template <> +bool outputObstacleToPly(const EsdfLayer& layer, const std::string& filename); + } // namespace io } // namespace nvblox diff --git a/nvblox/include/nvblox/rays/sphere_tracer.h b/nvblox/include/nvblox/rays/sphere_tracer.h index 5fd12e00d..ba6a16832 100644 --- a/nvblox/include/nvblox/rays/sphere_tracer.h +++ b/nvblox/include/nvblox/rays/sphere_tracer.h @@ -19,6 +19,7 @@ limitations under the License. #include #include "nvblox/core/camera.h" +#include "nvblox/core/camera_pinhole.h" #include "nvblox/core/common_names.h" #include "nvblox/core/image.h" #include "nvblox/gpu_hash/gpu_layer_view.h" @@ -49,9 +50,10 @@ class SphereTracer { /// 100x100 pixels, we trace 50x50 pixels and return a syntheric depth image /// of that size. /// @returns A pointer to the internal (GPU) buffer where the image is stored. + template std::shared_ptr renderImageOnGPU( - const Camera& camera, const Transform& T_L_C, const TsdfLayer& tsdf_layer, - const float truncation_distance_m, + const CameraType& camera, const Transform& T_L_C, + const TsdfLayer& tsdf_layer, const float truncation_distance_m, const MemoryType output_image_memory_type = MemoryType::kDevice, const int ray_subsampling_factor = 1); diff --git a/nvblox/include/nvblox/utils/timing.h b/nvblox/include/nvblox/utils/timing.h index 8286e66e3..e7c957e1b 100644 --- a/nvblox/include/nvblox/utils/timing.h +++ b/nvblox/include/nvblox/utils/timing.h @@ -201,6 +201,9 @@ class Timing { static double GetHz(std::string const& tag); static void Print(std::ostream& out); static std::string Print(); + static void Print(std::ostream& out, + std::vector const& keywords); + static std::string Print(std::vector const& keywords); static std::string SecondsToTimeString(double seconds); static void Reset(); static const map_t& GetTimers() { return Instance().tagMap_; } diff --git a/nvblox/include/nvblox/utils/weight_function.h b/nvblox/include/nvblox/utils/weight_function.h new file mode 100644 index 000000000..907528424 --- /dev/null +++ b/nvblox/include/nvblox/utils/weight_function.h @@ -0,0 +1,56 @@ +#pragma once + +#include + +namespace nvblox { + +__host__ __device__ inline float tsdf_constant_weight(const float& sdf) { + return 1.0f; +} + +__host__ __device__ inline float tsdf_linear_weight(const float& sdf, + const float& trunc) { + float epsilon = trunc * 0.5f; + float a = 0.5f * trunc / (trunc - epsilon); + float b = 0.5f / (trunc - epsilon); + if (sdf < epsilon) return 1.0f; + if ((epsilon <= sdf) && (sdf <= trunc)) return (a - b * sdf + 0.5f); + return 0.5f; +} + +__host__ __device__ inline float tsdf_exp_weight(const float& sdf, + const float& trunc) { + float epsilon = trunc * 0.5f; + if (sdf < epsilon) return 1.0f; + if ((epsilon <= sdf) && (sdf <= trunc)) { + float d = -trunc * (sdf - epsilon) * (sdf - epsilon); + return exp(d); + } else { + float d = -trunc * (trunc - epsilon) * (trunc - epsilon); + return exp(d); + } +} + +__host__ __device__ inline float tsdf_sensor_weight(const float& dis, + const int m, + const float dis_near) { + if (dis <= dis_near) { + return 1.0f; + } else { + return 1.0f / pow(dis, m); + } +} + +__host__ __device__ inline float tsdf_dropoff_weight(const float& sdf, + const float& trunc) { + float epsilon = trunc * 0.333f; + if (sdf <= -trunc) { + return 0.0f; + } else if (sdf > -trunc && sdf < -epsilon) { + return (trunc + sdf) / (trunc - epsilon); + } else { + return 1.0f; + } +} + +} // namespace nvblox diff --git a/nvblox/script/run_fuse_3dmatch.sh b/nvblox/script/run_fuse_3dmatch.sh new file mode 100755 index 000000000..ba225d899 --- /dev/null +++ b/nvblox/script/run_fuse_3dmatch.sh @@ -0,0 +1,9 @@ +# /bin/bash +cd build; +make && \ +./executables/fuse_3dmatch \ + /Spy/dataset/3DMatch/sun3d-mit_76_studyroom-76-1studyroom2 \ + --voxel_size 0.05 \ + --mesh_output_path /Spy/dataset/3DMatch/sun3d-mit_76_studyroom-76-1studyroom2/mesh_test.ply \ + --esdf_frame_subsampling 3000 \ + diff --git a/nvblox/script/run_fuse_fusionportable.sh b/nvblox/script/run_fuse_fusionportable.sh new file mode 100755 index 000000000..8a1a0c74d --- /dev/null +++ b/nvblox/script/run_fuse_fusionportable.sh @@ -0,0 +1,22 @@ +# /bin/bash +cd build; +make && \ +./executables/fuse_fusionportable \ + /Spy/dataset/mapping_results/nvblox/20220216_garden_day/ \ + --tsdf_integrator_max_integration_distance_m 70.0 \ + --color_integrator_max_integration_distance_m 30.0 \ + --num_frames 10 \ + --voxel_size 0.1 \ + --mesh_frame_subsampling 20 \ + --color_frame_subsampling -1 \ + --esdf_frame_subsampling 10 \ + --esdf_mode 1 \ + --esdf_zmin 0.5 \ + --esdf_zmax 1.0 \ + --mesh_output_path \ + /Spy/dataset/mapping_results/nvblox/20220216_garden_day_mesh_test.ply \ + --esdf_output_path \ + /Spy/dataset/mapping_results/nvblox/20220216_garden_day_esdf_test.ply \ + --obstacle_output_path \ + /Spy/dataset/mapping_results/nvblox/20220216_garden_day_obs_test.ply \ + diff --git a/nvblox/script/run_fuse_kitti.sh b/nvblox/script/run_fuse_kitti.sh new file mode 100755 index 000000000..5de1250a0 --- /dev/null +++ b/nvblox/script/run_fuse_kitti.sh @@ -0,0 +1,22 @@ +# /bin/bash +cd build; +make && \ +./executables/fuse_kitti \ + /Spy/dataset/mapping_results/nvblox/2011_09_30_drive_0027_sync/ \ + --tsdf_integrator_max_integration_distance_m 70.0 \ + --color_integrator_max_integration_distance_m 30.0 \ + --num_frames 100 \ + --voxel_size 0.2 \ + --mesh_frame_subsampling 20 \ + --color_frame_subsampling 1 \ + --esdf_frame_subsampling 10 \ + --esdf_mode 1 \ + --esdf_zmin 0.5 \ + --esdf_zmax 1.0 \ + --mesh_output_path \ + /Spy/dataset/mapping_results/nvblox/2011_09_30_drive_0027_sync_mesh_100_weightmethod6.ply \ + --esdf_output_path \ + /Spy/dataset/mapping_results/nvblox/2011_09_30_drive_0027_sync_esdf_100_weightmethod6.ply \ + --obstacle_output_path \ + /Spy/dataset/mapping_results/nvblox/2011_09_30_drive_0027_sync_obs_100_weightmethod6.ply \ + diff --git a/nvblox/src/core/camera.cpp b/nvblox/src/core/camera.cpp index 566fa85c6..4ba9a971c 100644 --- a/nvblox/src/core/camera.cpp +++ b/nvblox/src/core/camera.cpp @@ -80,113 +80,4 @@ Eigen::Matrix Camera::getViewCorners(const float min_depth, corners_C.row(7) = max_depth * ray_3_C; return corners_C; } - -// Frustum definitions. -Frustum::Frustum(const Camera& camera, const Transform& T_L_C, float min_depth, - float max_depth) { - Eigen::Matrix corners_C = - camera.getViewCorners(min_depth, max_depth); - computeBoundingPlanes(corners_C, T_L_C); -} - -void Frustum::computeBoundingPlanes(const Eigen::Matrix& corners_C, - const Transform& T_L_C) { - // Transform the corners. - const Eigen::Matrix corners_L = - (T_L_C * corners_C.transpose()).transpose(); - - // Near plane first. - bounding_planes_L_[0].setFromPoints(corners_L.row(0), corners_L.row(2), - corners_L.row(1)); - // Far plane. - bounding_planes_L_[1].setFromPoints(corners_L.row(4), corners_L.row(5), - corners_L.row(6)); - // Left. - bounding_planes_L_[2].setFromPoints(corners_L.row(3), corners_L.row(7), - corners_L.row(6)); - // Right. - bounding_planes_L_[3].setFromPoints(corners_L.row(0), corners_L.row(5), - corners_L.row(4)); - // Top. - bounding_planes_L_[4].setFromPoints(corners_L.row(3), corners_L.row(4), - corners_L.row(7)); - // Bottom. - bounding_planes_L_[5].setFromPoints(corners_L.row(2), corners_L.row(6), - corners_L.row(5)); - - // Calculate AABB. - Vector3f aabb_min, aabb_max; - - aabb_min.setConstant(std::numeric_limits::max()); - aabb_max.setConstant(std::numeric_limits::lowest()); - - for (int i = 0; i < corners_L.cols(); i++) { - for (size_t j = 0; j < corners_L.rows(); j++) { - aabb_min(i) = std::min(aabb_min(i), corners_L(j, i)); - aabb_max(i) = std::max(aabb_max(i), corners_L(j, i)); - } - } - - aabb_ = AxisAlignedBoundingBox(aabb_min, aabb_max); -} - -bool Frustum::isPointInView(const Vector3f& point) const { - // Skip the AABB check, assume already been done. - for (size_t i = 0; i < bounding_planes_L_.size(); i++) { - if (!bounding_planes_L_[i].isPointInside(point)) { - return false; - } - } - return true; -} - -bool Frustum::isAABBInView(const AxisAlignedBoundingBox& aabb) const { - // If we're not even close, don't bother checking the planes. - if (!aabb_.intersects(aabb)) { - return false; - } - constexpr int kNumCorners = 8; - - // Check the center of the bounding box to see if it's within the AABB. - // This covers a corner case where the given AABB is larger than the - // frustum. - if (isPointInView(aabb.center())) { - return true; - } - - // Iterate over all the corners of the bounding box and see if any are - // within the view frustum. - for (int i = 0; i < kNumCorners; i++) { - if (isPointInView( - aabb.corner(static_cast(i)))) { - return true; - } - } - return false; -} - -// Bounding plane definitions. -void BoundingPlane::setFromPoints(const Vector3f& p1, const Vector3f& p2, - const Vector3f& p3) { - Vector3f p1p2 = p2 - p1; - Vector3f p1p3 = p3 - p1; - - Vector3f cross = p1p2.cross(p1p3); - normal_ = cross.normalized(); - distance_ = normal_.dot(p1); -} - -void BoundingPlane::setFromDistanceNormal(const Vector3f& normal, - float distance) { - normal_ = normal; - distance_ = distance; -} - -bool BoundingPlane::isPointInside(const Vector3f& point) const { - if (point.dot(normal_) >= distance_) { - return true; - } - return false; -} - } // namespace nvblox diff --git a/nvblox/src/core/camera_pinhole.cpp b/nvblox/src/core/camera_pinhole.cpp new file mode 100644 index 000000000..5dd86d6bc --- /dev/null +++ b/nvblox/src/core/camera_pinhole.cpp @@ -0,0 +1,93 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ +#include "nvblox/core/camera_pinhole.h" + +namespace nvblox { + +std::ostream& operator<<(std::ostream& os, const CameraPinhole& camera) { + if (camera.isRectified()) { + os << "camera with intrinsics:\n\t" + << "\trectified: " << camera.isRectified() << "\n" + << "\tP: " << camera.P() << "\n" + << "\tRect: " << camera.Rect() << "\n" + << "\twidith: " << camera.width() << "\n" + << "\theight: " << camera.height() << "\n"; + } else { + os << "camera with intrinsics:\n\t" + << "\trectified: " << camera.isRectified() << "\n" + << "\tK: " << camera.K() << "\n" + << "\twidith: " << camera.width() << "\n" + << "\theight: " << camera.height() << "\n"; + } + return os; +} + +AxisAlignedBoundingBox CameraPinhole::getViewAABB(const Transform& T_L_C, + const float min_depth, + const float max_depth) const { + // Get the bounding corners of this view. + Eigen::Matrix corners_C = getViewCorners(min_depth, max_depth); + + Vector3f aabb_min, aabb_max; + aabb_min.setConstant(std::numeric_limits::max()); + aabb_max.setConstant(std::numeric_limits::lowest()); + + // Transform it into the layer coordinate frame. + for (size_t i = 0; i < corners_C.rows(); i++) { + const Vector3f& corner_C = corners_C.row(i); + Vector3f corner_L = T_L_C * corner_C; + for (int i = 0; i < 3; i++) { + aabb_min(i) = std::min(aabb_min(i), corner_L(i)); + aabb_max(i) = std::max(aabb_max(i), corner_L(i)); + } + } + + return AxisAlignedBoundingBox(aabb_min, aabb_max); +} + +Frustum CameraPinhole::getViewFrustum(const Transform& T_L_C, + const float min_depth, + const float max_depth) const { + return Frustum(*this, T_L_C, min_depth, max_depth); +} + +Eigen::Matrix CameraPinhole::getViewCorners( + const float min_depth, const float max_depth) const { + // Rays through the corners of the image plane + // Clockwise from the top left corner of the image. + const Vector3f ray_0_C = + vectorFromImagePlaneCoordinates(Vector2f(0.0f, 0.0f)); // NOLINT + const Vector3f ray_1_C = + vectorFromImagePlaneCoordinates(Vector2f(width_, 0.0f)); // NOLINT + const Vector3f ray_2_C = + vectorFromImagePlaneCoordinates(Vector2f(width_, height_)); // NOLINT + const Vector3f ray_3_C = + vectorFromImagePlaneCoordinates(Vector2f(0.0f, height_)); // NOLINT + + // True bounding box from the 3D points + Eigen::Matrix corners_C; + corners_C.row(0) = min_depth * ray_2_C; + corners_C.row(1) = min_depth * ray_1_C; + corners_C.row(2) = min_depth * ray_0_C, + corners_C.row(3) = min_depth * ray_3_C; + corners_C.row(4) = max_depth * ray_2_C; + corners_C.row(5) = max_depth * ray_1_C; + corners_C.row(6) = max_depth * ray_0_C; + corners_C.row(7) = max_depth * ray_3_C; + return corners_C; +} + +} // namespace nvblox diff --git a/nvblox/src/core/cuda/image_operation.cu b/nvblox/src/core/cuda/image_operation.cu new file mode 100644 index 000000000..c1fcfe8e9 --- /dev/null +++ b/nvblox/src/core/cuda/image_operation.cu @@ -0,0 +1,119 @@ +#include "nvblox/core/cuda/error_check.cuh" +#include "nvblox/core/cuda/image_operation.h" + +namespace nvblox { +namespace cuda { +__global__ void computeNormalImageOSLidar( + const float* depth_image, const float* height_image, float* normal_image, + const int w, const int h, const float rads_per_pixel_azimuth, + const float rads_per_pixel_elevation) { + const float end_azimuth_rad = 3.1415926535f; + const int tid = blockIdx.x * blockDim.x + threadIdx.x; + int u_stride = blockDim.x; + int v_stride = 1; + for (int u = tid; u < w; u += u_stride) { + for (int v = 0; v < h; v += v_stride) { + normal_image[3 * (v * w + u)] = 0.0f; + normal_image[3 * (v * w + u) + 1] = 0.0f; + normal_image[3 * (v * w + u) + 2] = 0.0f; + + float sign = 1.0f; + int uu, vv; + if (u == w - 1) { + uu = 0; + } else { + uu = u + 1; + } + if (v == h - 1) { + vv = 0; + sign *= -1.0f; + } else { + vv = v + 1; + } + + float d = image::access(v, u, w, depth_image); + float d1 = image::access(v, uu, w, depth_image); + float d2 = image::access(vv, u, w, depth_image); + // on the boundary, not continous + if (abs(d - d1) > 0.9 * d) continue; + // on the boundary, not continous + if (abs(d - d2) > 0.9 * d) continue; + + float px, py, pz; + float px1, py1, pz1; + float px2, py2, pz2; + + { + float depth = image::access(v, u, w, depth_image); + float height = image::access(v, u, w, height_image); + float r = sqrt(depth * depth - height * height); + float azimuth_angle_rad = end_azimuth_rad - u * rads_per_pixel_azimuth; + px = r * cos(azimuth_angle_rad); + py = r * sin(azimuth_angle_rad); + pz = height; + } + + { + float depth = image::access(v, uu, w, depth_image); + float height = image::access(v, uu, w, height_image); + float r = sqrt(depth * depth - height * height); + float azimuth_angle_rad = end_azimuth_rad - uu * rads_per_pixel_azimuth; + px1 = r * cos(azimuth_angle_rad); + py1 = r * sin(azimuth_angle_rad); + pz1 = height; + } + + { + float depth = image::access(vv, u, w, depth_image); + float height = image::access(vv, u, w, height_image); + float r = sqrt(depth * depth - height * height); + float azimuth_angle_rad = end_azimuth_rad - u * rads_per_pixel_azimuth; + px2 = r * cos(azimuth_angle_rad); + py2 = r * sin(azimuth_angle_rad); + pz2 = height; + } + + float nx, ny, nz; + { + nx = sign * (py - py2) * (pz - pz1) - (py - py1) * (pz - pz2); + ny = sign * (px - px1) * (pz - pz2) - (px - px2) * (pz - pz1); + nz = sign * (px - px2) * (py - py1) - (px - px1) * (py - py2); + float l = sqrt(nx * nx + ny * ny + nz * nz); + if (l == 0.0f) { + continue; + } else { + nx /= l; + ny /= l; + nz /= l; + } + // printf("%f %f, %f, %f\n", l, nx, ny, nz); + } + normal_image[3 * (v * w + u)] = nx; + normal_image[3 * (v * w + u) + 1] = ny; + normal_image[3 * (v * w + u) + 2] = nz; + } + } +} + +// OSLidar +void getNormalImageOSLidar(OSLidar& lidar) { + int w = lidar.num_azimuth_divisions(); + int h = lidar.num_elevation_divisions(); + float* normal_frame_cuda; + checkCudaErrors(cudaMalloc(&normal_frame_cuda, sizeof(float) * w * h * 3)); + int block_size = idivup(w, 2); + int grid_size = 1; + computeNormalImageOSLidar<<>>( + lidar.getDepthFrameCUDA(), lidar.getHeightFrameCUDA(), normal_frame_cuda, + lidar.num_azimuth_divisions(), lidar.num_elevation_divisions(), + lidar.rads_per_pixel_azimuth(), lidar.rads_per_pixel_elevation()); + lidar.setNormalFrameCUDA(normal_frame_cuda); +} + +void freeNormalImageOSLidar(OSLidar& lidar) { + float* normal_image = lidar.getNormalFrameCUDA(); + checkCudaErrors(cudaFree(normal_image)); +} + +} // namespace cuda +} // namespace nvblox \ No newline at end of file diff --git a/nvblox/src/core/frustum.cpp b/nvblox/src/core/frustum.cpp new file mode 100644 index 000000000..6df5c8a5f --- /dev/null +++ b/nvblox/src/core/frustum.cpp @@ -0,0 +1,120 @@ +#include "nvblox/core/frustum.h" +#include "nvblox/core/camera.h" +#include "nvblox/core/camera_pinhole.h" + +namespace nvblox { + +template Frustum::Frustum(const Camera& camera, const Transform& T_L_C, + float min_depth, float max_depth); +template Frustum::Frustum(const CameraPinhole& camera, const Transform& T_L_C, + float min_depth, float max_depth); + +// Frustum definitions. +template +Frustum::Frustum(const CameraType& camera, const Transform& T_L_C, + float min_depth, float max_depth) { + Eigen::Matrix corners_C = + camera.getViewCorners(min_depth, max_depth); + computeBoundingPlanes(corners_C, T_L_C); +} + +void Frustum::computeBoundingPlanes(const Eigen::Matrix& corners_C, + const Transform& T_L_C) { + // Transform the corners. + const Eigen::Matrix corners_L = + (T_L_C * corners_C.transpose()).transpose(); + + // Near plane first. + bounding_planes_L_[0].setFromPoints(corners_L.row(0), corners_L.row(2), + corners_L.row(1)); + // Far plane. + bounding_planes_L_[1].setFromPoints(corners_L.row(4), corners_L.row(5), + corners_L.row(6)); + // Left. + bounding_planes_L_[2].setFromPoints(corners_L.row(3), corners_L.row(7), + corners_L.row(6)); + // Right. + bounding_planes_L_[3].setFromPoints(corners_L.row(0), corners_L.row(5), + corners_L.row(4)); + // Top. + bounding_planes_L_[4].setFromPoints(corners_L.row(3), corners_L.row(4), + corners_L.row(7)); + // Bottom. + bounding_planes_L_[5].setFromPoints(corners_L.row(2), corners_L.row(6), + corners_L.row(5)); + + // Calculate AABB. + Vector3f aabb_min, aabb_max; + + aabb_min.setConstant(std::numeric_limits::max()); + aabb_max.setConstant(std::numeric_limits::lowest()); + + for (int i = 0; i < corners_L.cols(); i++) { + for (size_t j = 0; j < corners_L.rows(); j++) { + aabb_min(i) = std::min(aabb_min(i), corners_L(j, i)); + aabb_max(i) = std::max(aabb_max(i), corners_L(j, i)); + } + } + + aabb_ = AxisAlignedBoundingBox(aabb_min, aabb_max); +} + +bool Frustum::isPointInView(const Vector3f& point) const { + // Skip the AABB check, assume already been done. + for (size_t i = 0; i < bounding_planes_L_.size(); i++) { + if (!bounding_planes_L_[i].isPointInside(point)) { + return false; + } + } + return true; +} + +bool Frustum::isAABBInView(const AxisAlignedBoundingBox& aabb) const { + // If we're not even close, don't bother checking the planes. + if (!aabb_.intersects(aabb)) { + return false; + } + constexpr int kNumCorners = 8; + + // Check the center of the bounding box to see if it's within the AABB. + // This covers a corner case where the given AABB is larger than the + // frustum. + if (isPointInView(aabb.center())) { + return true; + } + + // Iterate over all the corners of the bounding box and see if any are + // within the view frustum. + for (int i = 0; i < kNumCorners; i++) { + if (isPointInView( + aabb.corner(static_cast(i)))) { + return true; + } + } + return false; +} + +// Bounding plane definitions. +void BoundingPlane::setFromPoints(const Vector3f& p1, const Vector3f& p2, + const Vector3f& p3) { + Vector3f p1p2 = p2 - p1; + Vector3f p1p3 = p3 - p1; + + Vector3f cross = p1p2.cross(p1p3); + normal_ = cross.normalized(); + distance_ = normal_.dot(p1); +} + +void BoundingPlane::setFromDistanceNormal(const Vector3f& normal, + float distance) { + normal_ = normal; + distance_ = distance; +} + +bool BoundingPlane::isPointInside(const Vector3f& point) const { + if (point.dot(normal_) >= distance_) { + return true; + } + return false; +} +} // namespace nvblox \ No newline at end of file diff --git a/nvblox/src/core/mapper.cpp b/nvblox/src/core/mapper.cpp index d5351fe81..1c318a5d1 100644 --- a/nvblox/src/core/mapper.cpp +++ b/nvblox/src/core/mapper.cpp @@ -20,6 +20,15 @@ limitations under the License. namespace nvblox { +// NOTE(gogojjh): Define the template function +template void RgbdMapper::integrateColor(const ColorImage& color_frame, + const Transform& T_L_C, + const Camera& camera); +template void RgbdMapper::integrateColor(const ColorImage& color_frame, + const Transform& T_L_C, + const CameraPinhole& camera); + +////////////////////////////////////////////////////////////////////// RgbdMapper::RgbdMapper(float voxel_size_m, MemoryType memory_type) : voxel_size_m_(voxel_size_m), memory_type_(memory_type) { layers_ = LayerCake::create( @@ -56,8 +65,25 @@ void RgbdMapper::integrateLidarDepth(const DepthImage& depth_frame, esdf_blocks_to_update_.insert(updated_blocks.begin(), updated_blocks.end()); } +void RgbdMapper::integrateOSLidarDepth(DepthImage& depth_frame, + const Transform& T_L_C, + OSLidar& oslidar) { + // Call the integrator. + std::vector updated_blocks; + lidar_tsdf_integrator_.integrateFrame(depth_frame, T_L_C, oslidar, + layers_.getPtr(), + &updated_blocks); + LOG(INFO) << "Integrated TSDF block: " << updated_blocks.size(); + + // Update all the relevant queues. + mesh_blocks_to_update_.insert(updated_blocks.begin(), updated_blocks.end()); + esdf_blocks_to_update_.insert(updated_blocks.begin(), updated_blocks.end()); +} + +template void RgbdMapper::integrateColor(const ColorImage& color_frame, - const Transform& T_L_C, const Camera& camera) { + const Transform& T_L_C, + const CameraType& camera) { color_integrator_.integrateFrame(color_frame, T_L_C, camera, layers_.get(), layers_.getPtr()); @@ -67,6 +93,8 @@ std::vector RgbdMapper::updateMesh() { // Convert the set of MeshBlocks needing an update to a vector std::vector mesh_blocks_to_update_vector( mesh_blocks_to_update_.begin(), mesh_blocks_to_update_.end()); + LOG(INFO) << "Size of mesh blocks to be updated: " + << mesh_blocks_to_update_vector.size(); // Call the integrator. mesh_integrator_.integrateBlocksGPU(layers_.get(), diff --git a/nvblox/src/integrators/cuda/projective_color_integrator.cu b/nvblox/src/integrators/cuda/projective_color_integrator.cu index 684531eff..235f29a50 100644 --- a/nvblox/src/integrators/cuda/projective_color_integrator.cu +++ b/nvblox/src/integrators/cuda/projective_color_integrator.cu @@ -18,9 +18,21 @@ limitations under the License. #include "nvblox/integrators/internal/cuda/projective_integrators_common.cuh" #include "nvblox/integrators/internal/integrators_common.h" #include "nvblox/utils/timing.h" +#include "nvblox/utils/weight_function.h" namespace nvblox { +/// NOTE(gogojjh): Define the template class +template void ProjectiveColorIntegrator::integrateFrame( + const ColorImage& color_frame, const Transform& T_L_C, const Camera& camera, + const TsdfLayer& tsdf_layer, ColorLayer* color_layer, + std::vector* updated_blocks); +template void ProjectiveColorIntegrator::integrateFrame( + const ColorImage& color_frame, const Transform& T_L_C, + const CameraPinhole& camera, const TsdfLayer& tsdf_layer, + ColorLayer* color_layer, std::vector* updated_blocks); + +////////////////////////////////////////////////////////////////////// ProjectiveColorIntegrator::ProjectiveColorIntegrator() : ProjectiveIntegratorBase() { sphere_tracer_.maximum_ray_length_m(max_integration_distance_m_); @@ -36,10 +48,12 @@ void ProjectiveColorIntegrator::finish() const { cudaStreamSynchronize(integration_stream_); } +// NOTE(gogojjh): the API function +template void ProjectiveColorIntegrator::integrateFrame( - const ColorImage& color_frame, const Transform& T_L_C, const Camera& camera, - const TsdfLayer& tsdf_layer, ColorLayer* color_layer, - std::vector* updated_blocks) { + const ColorImage& color_frame, const Transform& T_L_C, + const CameraType& camera, const TsdfLayer& tsdf_layer, + ColorLayer* color_layer, std::vector* updated_blocks) { timing::Timer color_timer("color/integrate"); CHECK_NOTNULL(color_layer); CHECK_EQ(tsdf_layer.block_size(), color_layer->block_size()); @@ -49,6 +63,7 @@ void ProjectiveColorIntegrator::integrateFrame( color_layer->block_size() / VoxelBlock::kVoxelsPerSide; const float truncation_distance_m = truncation_distance_vox_ * voxel_size; + // Get visible blocks timing::Timer blocks_in_view_timer("color/integrate/get_blocks_in_view"); std::vector block_indices = view_calculator_.getBlocksInViewPlanes( T_L_C, camera, color_layer->block_size(), @@ -126,6 +141,92 @@ __device__ inline Color blendTwoColors(const Color& first_color, return new_color; } +/// NOTE(gogojjh): This function implement different weighting functions to +/// update the color of a voxel +__device__ inline bool updateVoxelMultiWeightComp( + const Color color_measured, ColorVoxel* voxel_ptr, + const float voxel_depth_m, const float voxel_distance_measured, + const float truncation_distance_m, const float max_weight, + const int voxel_weight_method) { + // NOTE(alexmillane): We integrate all voxels passed to this function, We + // should probably not do this. We should no update some based on occlusion + // and their distance in the distance field.... + // TODO(alexmillane): The above. + + if (voxel_weight_method == 1) { + // Read CURRENT voxel values (from global GPU memory) + const Color voxel_color_current = voxel_ptr->color; + const float voxel_weight_current = voxel_ptr->weight; + + // Fuse + constexpr float measurement_weight = 1.0f; + const Color fused_color = + blendTwoColors(voxel_color_current, voxel_weight_current, + color_measured, measurement_weight); + const float weight = + fmin(measurement_weight + voxel_weight_current, max_weight); + // Write NEW voxel values (to global GPU memory) + voxel_ptr->color = fused_color; + voxel_ptr->weight = weight; + } else if (voxel_weight_method == 2) { + const Color voxel_color_current = voxel_ptr->color; + const float voxel_weight_current = voxel_ptr->weight; + + const float measurement_weight = + tsdf_linear_weight(voxel_distance_measured, truncation_distance_m); + Color fused_color = + blendTwoColors(voxel_color_current, voxel_weight_current, + color_measured, measurement_weight); + const float weight = + fmin(measurement_weight + voxel_weight_current, max_weight); + voxel_ptr->color = fused_color; + voxel_ptr->weight = weight; + } else if (voxel_weight_method == 3) { + const Color voxel_color_current = voxel_ptr->color; + const float voxel_weight_current = voxel_ptr->weight; + + const float measurement_weight = + tsdf_exp_weight(voxel_distance_measured, truncation_distance_m); + Color fused_color = + blendTwoColors(voxel_color_current, voxel_weight_current, + color_measured, measurement_weight); + const float weight = + fmin(measurement_weight + voxel_weight_current, max_weight); + voxel_ptr->color = fused_color; + voxel_ptr->weight = weight; + } else if (voxel_weight_method == 4) { + const Color voxel_color_current = voxel_ptr->color; + const float voxel_weight_current = voxel_ptr->weight; + + const float COLOR_WEIGHT_DISTANCE_TH = 10.0f; + const float measurement_weight = + tsdf_sensor_weight(voxel_depth_m, 2, COLOR_WEIGHT_DISTANCE_TH); + Color fused_color = + blendTwoColors(voxel_color_current, voxel_weight_current, + color_measured, measurement_weight); + const float weight = + fmin(measurement_weight + voxel_weight_current, max_weight); + voxel_ptr->color = fused_color; + voxel_ptr->weight = weight; + } else if (voxel_weight_method == 5) { + const Color voxel_color_current = voxel_ptr->color; + const float voxel_weight_current = voxel_ptr->weight; + + const float COLOR_WEIGHT_DISTANCE_TH = 10.0f; + const float measurement_weight = + tsdf_linear_weight(voxel_distance_measured, truncation_distance_m) * + tsdf_sensor_weight(voxel_depth_m, 2, COLOR_WEIGHT_DISTANCE_TH); + Color fused_color = + blendTwoColors(voxel_color_current, voxel_weight_current, + color_measured, measurement_weight); + const float weight = + fmin(measurement_weight + voxel_weight_current, max_weight); + voxel_ptr->color = fused_color; + voxel_ptr->weight = weight; + } + return true; +} + __device__ inline bool updateVoxel(const Color color_measured, ColorVoxel* voxel_ptr, const float voxel_depth_m, @@ -152,8 +253,9 @@ __device__ inline bool updateVoxel(const Color color_measured, return true; } +template __global__ void integrateBlocks( - const Index3D* block_indices_device_ptr, const Camera camera, + const Index3D* block_indices_device_ptr, const CameraType camera, const Color* color_image, const int color_rows, const int color_cols, const float* depth_image, const int depth_rows, const int depth_cols, const Transform T_C_L, const float block_size, @@ -165,9 +267,8 @@ __global__ void integrateBlocks( Eigen::Vector2f u_px; float voxel_depth_m; Vector3f p_voxel_center_C; - if (!projectThreadVoxel(block_indices_device_ptr, camera, T_C_L, - block_size, &u_px, &voxel_depth_m, - &p_voxel_center_C)) { + if (!projectThreadVoxel(block_indices_device_ptr, camera, T_C_L, block_size, + &u_px, &voxel_depth_m, &p_voxel_center_C)) { return; } @@ -201,25 +302,42 @@ __global__ void integrateBlocks( } // Get the Voxel we'll update in this thread - // NOTE(alexmillane): Note that we've reverse the voxel indexing order such - // that adjacent threads (x-major) access adjacent memory locations in the - // block (z-major). + // NOTE(alexmillane): Note that we've reverse the voxel indexing order + // such that adjacent threads (x-major) access adjacent memory locations + // in the block (z-major). ColorVoxel* voxel_ptr = &(block_device_ptrs[blockIdx.x] ->voxels[threadIdx.z][threadIdx.y][threadIdx.x]); // Update the voxel using the update rule for this layer type - updateVoxel(image_value, voxel_ptr, voxel_depth_m, truncation_distance_m, - max_weight); + // Weight method + // 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 + const int voxel_weight_method = 5; + if (voxel_weight_method == 1) { + updateVoxel(image_value, voxel_ptr, voxel_depth_m, truncation_distance_m, + max_weight); + } else { + updateVoxelMultiWeightComp( + image_value, voxel_ptr, voxel_depth_m, voxel_distance_from_surface, + truncation_distance_m, max_weight, voxel_weight_method); + } } +template void ProjectiveColorIntegrator::updateBlocks( const std::vector& block_indices, const ColorImage& color_frame, - const DepthImage& depth_frame, const Transform& T_L_C, const Camera& camera, - const float truncation_distance_m, ColorLayer* layer_ptr) { + const DepthImage& depth_frame, const Transform& T_L_C, + const CameraType& camera, const float truncation_distance_m, + ColorLayer* layer_ptr) { CHECK_NOTNULL(layer_ptr); CHECK_EQ(color_frame.rows() % depth_frame.rows(), 0); CHECK_EQ(color_frame.cols() % depth_frame.cols(), 0); + std::cout << "[updateBlocks]: update " << block_indices.size() << " blocks" + << std::endl; if (block_indices.empty()) { return; diff --git a/nvblox/src/integrators/cuda/projective_tsdf_integrator.cu b/nvblox/src/integrators/cuda/projective_tsdf_integrator.cu index f4e12dece..077798d7f 100644 --- a/nvblox/src/integrators/cuda/projective_tsdf_integrator.cu +++ b/nvblox/src/integrators/cuda/projective_tsdf_integrator.cu @@ -21,16 +21,17 @@ limitations under the License. #include "nvblox/integrators/internal/cuda/projective_integrators_common.cuh" #include "nvblox/integrators/internal/integrators_common.h" #include "nvblox/utils/timing.h" +#include "nvblox/utils/weight_function.h" namespace nvblox { - +// NOTE(gogojjh): the original nvblox implementation __device__ inline bool updateVoxel(const float surface_depth_measured, TsdfVoxel* voxel_ptr, const float voxel_depth_m, const float truncation_distance_m, const float max_weight) { // Get the MEASURED depth of the VOXEL - const float voxel_distance_measured = surface_depth_measured - voxel_depth_m; + float voxel_distance_measured = surface_depth_measured - voxel_depth_m; // If we're behind the negative truncation distance, just continue. if (voxel_distance_measured < -truncation_distance_m) { @@ -44,28 +45,211 @@ __device__ inline bool updateVoxel(const float surface_depth_measured, // NOTE(alexmillane): We could try to use CUDA math functions to speed up // below // https://docs.nvidia.com/cuda/cuda-math-api/group__CUDA__MATH__SINGLE.html#group__CUDA__MATH__SINGLE - // Fuse constexpr float measurement_weight = 1.0f; float fused_distance = (voxel_distance_measured * measurement_weight + voxel_distance_current * voxel_weight_current) / (measurement_weight + voxel_weight_current); - // Clip if (fused_distance > 0.0f) { - fused_distance = fmin(truncation_distance_m, fused_distance); + fused_distance = fminf(truncation_distance_m, fused_distance); } else { - fused_distance = fmax(-truncation_distance_m, fused_distance); + fused_distance = fmaxf(-truncation_distance_m, fused_distance); } const float weight = - fmin(measurement_weight + voxel_weight_current, max_weight); - + fminf(measurement_weight + voxel_weight_current, max_weight); // Write NEW voxel values (to global GPU memory) voxel_ptr->distance = fused_distance; voxel_ptr->weight = weight; return true; } +__device__ inline bool updateVoxelMultiWeightComp( + const float surface_depth_measured, TsdfVoxel* voxel_ptr, + const float voxel_depth_m, const float truncation_distance_m, + const float max_weight, const int voxel_weight_method, + const Vector3f& measurement_point, const Vector3f& measurement_normal, + const Transform& T_C_L) { + // NOTE(gogojjh): need to externally set parameters in both host and device + const float kEpsilon = 1e-6; // Used for coordinates + const float kFloatEpsilon = 1e-8; // Used for weights + const float TSDF_NORMAL_RATIO_TH = 0.05f; + const float TSDF_WEIGHT_DISTANCE_TH = 50.0f; + + // Get the MEASURED depth of the VOXEL + float voxel_distance_measured = surface_depth_measured - voxel_depth_m; + // If we're behind the negative truncation distance, just continue. + if (voxel_distance_measured < -truncation_distance_m) { + return false; + } + + // Read CURRENT voxel values (from global GPU memory) + const float voxel_distance_current = voxel_ptr->distance; + const float voxel_weight_current = voxel_ptr->weight; + const Vector3f voxel_gradient_current = voxel_ptr->gradient; + + // NOTE(alexmillane): We could try to use CUDA math functions to speed up + // below + // https://docs.nvidia.com/cuda/cuda-math-api/group__CUDA__MATH__SINGLE.html#group__CUDA__MATH__SINGLE + if (voxel_weight_method == 1) { + // Fuse + constexpr float measurement_weight = 1.0f; + float fused_distance = (voxel_distance_measured * measurement_weight + + voxel_distance_current * voxel_weight_current) / + (measurement_weight + voxel_weight_current); + // Clip + if (fused_distance > 0.0f) { + fused_distance = fminf(truncation_distance_m, fused_distance); + } else { + fused_distance = fmaxf(-truncation_distance_m, fused_distance); + } + const float weight = + fminf(measurement_weight + voxel_weight_current, max_weight); + // Write NEW voxel values (to global GPU memory) + voxel_ptr->distance = fused_distance; + voxel_ptr->weight = weight; + } else if (voxel_weight_method == 2) { + voxel_distance_measured = + fminf(voxel_distance_measured, truncation_distance_m); + float measurement_weight = tsdf_constant_weight(voxel_distance_measured); + float fused_distance = (voxel_distance_measured * measurement_weight + + voxel_distance_current * voxel_weight_current) / + (measurement_weight + voxel_weight_current); + if (fused_distance > 0.0f) { + fused_distance = fminf(truncation_distance_m, fused_distance); + } else { + fused_distance = fmaxf(-truncation_distance_m, fused_distance); + } + const float fused_weight = + fminf(measurement_weight + voxel_weight_current, max_weight); + voxel_ptr->distance = fused_distance; + voxel_ptr->weight = fused_weight; + } else if (voxel_weight_method == 3) { + voxel_distance_measured = + fminf(voxel_distance_measured, truncation_distance_m); + float measurement_weight = + tsdf_linear_weight(voxel_distance_measured, truncation_distance_m); + float fused_distance = (voxel_distance_measured * measurement_weight + + voxel_distance_current * voxel_weight_current) / + (measurement_weight + voxel_weight_current); + if (fused_distance > 0.0f) { + fused_distance = fminf(truncation_distance_m, fused_distance); + } else { + fused_distance = fmaxf(-truncation_distance_m, fused_distance); + } + const float weight = + fminf(measurement_weight + voxel_weight_current, max_weight); + voxel_ptr->distance = fused_distance; + voxel_ptr->weight = weight; + } else if (voxel_weight_method == 4) { + voxel_distance_measured = + fminf(voxel_distance_measured, truncation_distance_m); + float measurement_weight = + tsdf_exp_weight(voxel_distance_measured, truncation_distance_m); + float fused_distance = (voxel_distance_measured * measurement_weight + + voxel_distance_current * voxel_weight_current) / + (measurement_weight + voxel_weight_current); + if (fused_distance > 0.0f) { + fused_distance = fminf(truncation_distance_m, fused_distance); + } else { + fused_distance = fmaxf(-truncation_distance_m, fused_distance); + } + const float weight = + fminf(measurement_weight + voxel_weight_current, max_weight); + voxel_ptr->distance = fused_distance; + voxel_ptr->weight = weight; + } else if (voxel_weight_method == 5 || voxel_weight_method == 6) { + float normal_ratio = 1.0f; + + // case 1: existing gradient, use the gradient to compute the ratio + if (voxel_gradient_current.norm() > kFloatEpsilon) { + Vector3f gradient_C; + // transform the gradient into the camera coordinate system + gradient_C = T_C_L.rotation() * voxel_gradient_current; + + // case 1.1: existing gradient, existing normal + if (measurement_normal.norm() > kFloatEpsilon) { + // alpha: the angle between the normal and gradient + float cos_alpha = + abs(gradient_C.dot(measurement_normal) / measurement_normal.norm()); + float sin_alpha = sqrt(1 - cos_alpha * cos_alpha); + + // theta: the angle between the ray and gradient + float cos_theta = + abs(gradient_C.dot(measurement_point) / measurement_point.norm()); + float sin_theta = sqrt(1 - cos_theta * cos_theta); + + // condition 1: flat surface, alpha is approximate to zero + if (abs(1.0f - cos_alpha) < kFloatEpsilon) { + normal_ratio = cos_theta; + } + // condition 2: curve surface + else { + normal_ratio = + abs((cos_alpha - 1) * sin_theta / sin_alpha + cos_theta); + if (isnan(normal_ratio)) normal_ratio = cos_theta; + } + } + // case 1.2: existing gradient, no normal + else { + normal_ratio = + abs(measurement_point.dot(gradient_C) / measurement_point.norm()); + } + } + // case 2: no gradient + else { + // case 2.1: no gradient, existing normal + if (measurement_normal.norm() > kFloatEpsilon) { + normal_ratio = abs(measurement_point.dot(measurement_normal) / + measurement_point.norm()); + } + } + // ruling out extremely large incidence angle + if (normal_ratio < TSDF_NORMAL_RATIO_TH) return false; + + float measurement_distance = normal_ratio * voxel_distance_measured; + measurement_distance = fminf(measurement_distance, truncation_distance_m); + + float measurement_weight; + if (voxel_weight_method == 5) { + float weight_sensor = tsdf_sensor_weight(surface_depth_measured, 2, + TSDF_WEIGHT_DISTANCE_TH); + float weight_dropoff = + tsdf_dropoff_weight(voxel_distance_measured, truncation_distance_m); + measurement_weight = weight_sensor * weight_dropoff; + } else if (voxel_weight_method == 6) { + measurement_weight = + tsdf_linear_weight(measurement_distance, truncation_distance_m); + } + + // NOTE(gogojjh): it is possible to have weights very close to zero, due + // to the limited precision of floating points dividing by this small + // value can cause nans + if (measurement_weight < kFloatEpsilon) return false; + + float fused_distance = (measurement_distance * measurement_weight + + voxel_distance_current * voxel_weight_current) / + (measurement_weight + voxel_weight_current); + const float fused_weight = + fminf(voxel_weight_current + measurement_weight, max_weight); + voxel_ptr->distance = fused_distance; + voxel_ptr->weight = fused_weight; + + // existing normal, update the gradient + if (measurement_normal.norm() > kFloatEpsilon) { + // transform the normal into the world coordinate system + Vector3f mea_normal_world = + T_C_L.rotation().transpose() * measurement_normal; + Vector3f fused_gradient = (voxel_weight_current * voxel_gradient_current + + measurement_weight * mea_normal_world) / + (measurement_weight + voxel_weight_current); + fused_gradient.normalize(); + voxel_ptr->gradient = fused_gradient; + } + } + return true; +} + __device__ inline bool interpolateLidarImage( const Lidar& lidar, const Vector3f& p_voxel_center_C, const float* image, const Vector2f& u_px, const int rows, const int cols, @@ -123,6 +307,100 @@ __device__ inline bool interpolateLidarImage( return true; } +// nearest_interpolation_max_allowable_squared_dist_to_ray_m, default: 0.125**2 +__device__ inline bool interpolateOSLidarImage( + const OSLidar& lidar, const Vector3f& p_voxel_center_C, const float* image, + const Vector2f& u_px, const int rows, const int cols, + const float linear_interpolation_max_allowable_difference_m, + const float nearest_interpolation_max_allowable_squared_dist_to_ray_m, + float* image_value) { + // Try linear interpolation first + interpolation::Interpolation2DNeighbours neighbours; + bool linear_interpolation_success = interpolation::interpolate2DLinear< + float, interpolation::checkers::FloatPixelGreaterThanZero>( + image, u_px, rows, cols, image_value, &neighbours); + + // Additional check + // Check that we're not interpolating over a discontinuity + // NOTE(alexmillane): This prevents smearing are object edges. + if (linear_interpolation_success) { + const float d00 = fabsf(neighbours.p00 - *image_value); + const float d01 = fabsf(neighbours.p01 - *image_value); + const float d10 = fabsf(neighbours.p10 - *image_value); + const float d11 = fabsf(neighbours.p11 - *image_value); + float maximum_depth_difference_to_neighbours = + fmax(fmax(d00, d01), fmax(d10, d11)); + if (maximum_depth_difference_to_neighbours > + linear_interpolation_max_allowable_difference_m) { + linear_interpolation_success = false; + } + } + + // If linear didn't work - try nearest neighbour interpolation + if (!linear_interpolation_success) { + Index2D u_neighbour_px; + if (!interpolation::interpolate2DClosest< + float, interpolation::checkers::FloatPixelGreaterThanZero>( + image, u_px, rows, cols, image_value, &u_neighbour_px)) { + // If we can't successfully do closest, fail to intgrate this voxel. + return false; + } + + // Additional check + // Check that this voxel is close to the ray passing through the pixel. + // Note(alexmillane): This is to prevent large numbers of voxels + // being integrated by a single pixel at long ranges. + const Vector3f closest_ray = lidar.vectorFromPixelIndices(u_neighbour_px); + const float off_ray_squared_distance = + (p_voxel_center_C - p_voxel_center_C.dot(closest_ray) * closest_ray) + .squaredNorm(); + if (off_ray_squared_distance > + nearest_interpolation_max_allowable_squared_dist_to_ray_m) { + return false; + } + } + + // TODO(alexmillane): We should add clearing rays, even in the case both + // interpolations fail. + return true; +} + +// NOTE(gogojjh): +__device__ inline bool getPointVectorOSLidar(const OSLidar& lidar, + const Index2D& u_C, const int rows, + const int cols, + Vector3f& point_vector) { + const float kFloatEpsilon = 1e-8; // Used for weights + if (u_C.x() < 0 || u_C.y() < 0 || u_C.x() >= cols || u_C.y() >= rows) { + return false; + } else { + point_vector = lidar.unprojectFromImageIndex(u_C); + if (point_vector.norm() < kFloatEpsilon) { + return false; + } else { + return true; + } + } +} + +// NOTE(gogojjh): +__device__ inline bool getNormalVectorOSLidar(const OSLidar& lidar, + const Index2D& u_C, + const int rows, const int cols, + Vector3f& normal_vector) { + const float kFloatEpsilon = 1e-8; // Used for weights + if (u_C.x() < 0 || u_C.y() < 0 || u_C.x() >= cols || u_C.y() >= rows) { + return false; + } else { + normal_vector = lidar.getNormalVector(u_C); + if (normal_vector.norm() < kFloatEpsilon) { + return false; + } else { + return true; + } + } +} + // CAMERA __global__ void integrateBlocksKernel(const Index3D* block_indices_device_ptr, const Camera camera, const float* image, @@ -132,7 +410,8 @@ __global__ void integrateBlocksKernel(const Index3D* block_indices_device_ptr, const float max_weight, const float max_integration_distance, TsdfBlock** block_device_ptrs) { - // Get - the image-space projection of the voxel associated with this thread + // Get - the image-space projection of the voxel associated with this + // thread // - the depth associated with the projection. Eigen::Vector2f u_px; float voxel_depth_m; @@ -178,7 +457,8 @@ __global__ void integrateBlocksKernel( const float linear_interpolation_max_allowable_difference_m, const float nearest_interpolation_max_allowable_squared_dist_to_ray_m, TsdfBlock** block_device_ptrs) { - // Get - the image-space projection of the voxel associated with this thread + // Get - the image-space projection of the voxel associated with this + // thread // - the depth associated with the projection. Eigen::Vector2f u_px; float voxel_depth_m; @@ -217,6 +497,91 @@ __global__ void integrateBlocksKernel( max_weight); } +// OSLiDAR +// NOTE(gogojjh): main function to integrate blocks in GPU +__global__ void integrateBlocksKernel( + const Index3D* block_indices_device_ptr, const OSLidar lidar, + const float* image, int rows, int cols, const Transform T_C_L, + const float block_size, const float truncation_distance_m, + const float max_weight, const float max_integration_distance, + const float linear_interpolation_max_allowable_difference_m, + const float nearest_interpolation_max_allowable_squared_dist_to_ray_m, + TsdfBlock** block_device_ptrs) { + // function 1 + // Get - the image-space projection of the voxel associated with this + // thread + // - the depth associated with the projection. + // - the projected image coordinate of the voxel + Eigen::Vector2f u_px; + float voxel_depth_m; + Vector3f p_voxel_center_C; + if (!projectThreadVoxel(block_indices_device_ptr, lidar, T_C_L, block_size, + &u_px, &voxel_depth_m, &p_voxel_center_C)) { + return; // false: the voxel is not visible + } + + // If voxel further away than the limit, skip this voxel + if (max_integration_distance > 0.0f) { + if (voxel_depth_m > max_integration_distance) { + return; + } + } + + // function 2 + // Interpolate on the image plane + float image_value; + if (!interpolateOSLidarImage( + lidar, p_voxel_center_C, image, u_px, rows, cols, + linear_interpolation_max_allowable_difference_m, + nearest_interpolation_max_allowable_squared_dist_to_ray_m, + &image_value)) { + return; + } + + // Get the Voxel we'll update in this thread + // NOTE(alexmillane): Note that we've reverse the voxel indexing order + // such that adjacent threads (x-major) access adjacent memory locations + // in the block (z-major). + TsdfVoxel* voxel_ptr = &(block_device_ptrs[blockIdx.x] + ->voxels[threadIdx.z][threadIdx.y][threadIdx.x]); + + // NOTE(gogojjh): retrive the normal vector given u_px + const Index2D u_C = u_px.array().round().cast(); + Vector3f point_vector = Vector3f::Zero(); + Vector3f normal_vector = Vector3f::Zero(); + if (!getPointVectorOSLidar(lidar, u_C, rows, cols, point_vector)) return; + if (!getNormalVectorOSLidar(lidar, u_C, rows, cols, normal_vector)) return; + // printf("(%f, %f, %f - %f, %f, %f) ", point_vector.x(), point_vector.y(), + // point_vector.z(), normal_vector.x(), normal_vector.y(), + // normal_vector.z()); + + // function 3 + // Update the voxel using the update rule for this layer type + // NOTE(gogojjh): + // setting the voxel update method + // 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 + const int voxel_weight_method = 6; + if (voxel_weight_method == 1) { + // the original nvblox impelentation + // not use normal vector + updateVoxel(image_value, voxel_ptr, voxel_depth_m, truncation_distance_m, + max_weight); + } else { + // the improved weight computation + // use normal vector + updateVoxelMultiWeightComp( + image_value, voxel_ptr, voxel_depth_m, truncation_distance_m, + max_weight, voxel_weight_method, point_vector, normal_vector, T_C_L); + } +} + ProjectiveTsdfIntegrator::ProjectiveTsdfIntegrator() : ProjectiveIntegratorBase() { checkCudaErrors(cudaStreamCreate(&integration_stream_)); @@ -260,6 +625,7 @@ void ProjectiveTsdfIntegrator::integrateFrameTemplate( std::vector* updated_blocks) { CHECK_NOTNULL(layer); timing::Timer tsdf_timer("tsdf/integrate"); + // Metric truncation distance for this layer const float voxel_size = layer->block_size() / VoxelBlock::kVoxelsPerSide; @@ -271,6 +637,7 @@ void ProjectiveTsdfIntegrator::integrateFrameTemplate( view_calculator_.getBlocksInImageViewRaycast( depth_frame, T_L_C, sensor, layer->block_size(), truncation_distance_m, max_integration_distance_m_); + // LOG(INFO) << "block_indices size: " << block_indices.size(); blocks_in_view_timer.Stop(); // Allocate blocks (CPU) @@ -303,6 +670,13 @@ void ProjectiveTsdfIntegrator::integrateFrame( integrateFrameTemplate(depth_frame, T_L_C, lidar, layer, updated_blocks); } +// OSLidar +void ProjectiveTsdfIntegrator::integrateFrame( + DepthImage& depth_frame, const Transform& T_L_C, OSLidar& oslidar, + TsdfLayer* layer, std::vector* updated_blocks) { + integrateFrameTemplate(depth_frame, T_L_C, oslidar, layer, updated_blocks); +} + // Camera void ProjectiveTsdfIntegrator::integrateBlocks(const DepthImage& depth_frame, const Transform& T_C_L, @@ -383,6 +757,55 @@ void ProjectiveTsdfIntegrator::integrateBlocks(const DepthImage& depth_frame, checkCudaErrors(cudaPeekAtLastError()); } +// OSLidar +void ProjectiveTsdfIntegrator::integrateBlocks(const DepthImage& depth_frame, + const Transform& T_C_L, + const OSLidar& lidar, + TsdfLayer* layer_ptr) { + // Kernel call - One ThreadBlock launched per VoxelBlock + constexpr int kVoxelsPerSide = VoxelBlock::kVoxelsPerSide; + const dim3 kThreadsPerBlock(kVoxelsPerSide, kVoxelsPerSide, kVoxelsPerSide); + // NOTE(gogojjh): the number of visible blocks + const int num_thread_blocks = block_indices_device_.size(); + + // Metric truncation distance for this layer + const float voxel_size = + layer_ptr->block_size() / VoxelBlock::kVoxelsPerSide; + // default: 4.0 * 0.1 + const float truncation_distance_m = truncation_distance_vox_ * voxel_size; + + // Metric params + const float linear_interpolation_max_allowable_difference_m = + lidar_linear_interpolation_max_allowable_difference_vox_ * voxel_size; + const float nearest_interpolation_max_allowable_squared_dist_to_ray_m = + std::pow(lidar_nearest_interpolation_max_allowable_dist_to_ray_vox_ * + voxel_size, + 2); + + // Kernel + // std::cout << "num_thread_blocks: " << num_thread_blocks << std::endl; + // std::cout << "kVoxelsPerSide: " << kVoxelsPerSide << std::endl; + integrateBlocksKernel<<>>( + block_indices_device_.data(), // NOLINT + lidar, // NOLINT + depth_frame.dataConstPtr(), // NOLINT + depth_frame.rows(), // NOLINT + depth_frame.cols(), // NOLINT + T_C_L, // NOLINT + layer_ptr->block_size(), // NOLINT + truncation_distance_m, // NOLINT + max_weight_, // NOLINT + max_integration_distance_m_, // NOLINT + linear_interpolation_max_allowable_difference_m, // NOLINT + nearest_interpolation_max_allowable_squared_dist_to_ray_m, // NOLINT + block_ptrs_device_.data()); // NOLINT + + // Finish processing of the frame before returning control + finish(); + checkCudaErrors(cudaPeekAtLastError()); +} + template void ProjectiveTsdfIntegrator::integrateBlocksTemplate( const std::vector& block_indices, const DepthImage& depth_frame, diff --git a/nvblox/src/integrators/cuda/view_calculator.cu b/nvblox/src/integrators/cuda/view_calculator.cu index 08bc589ea..a962b2122 100644 --- a/nvblox/src/integrators/cuda/view_calculator.cu +++ b/nvblox/src/integrators/cuda/view_calculator.cu @@ -24,6 +24,18 @@ limitations under the License. namespace nvblox { +/// NOTE(gogojjh): define template function +template std::vector ViewCalculator::getBlocksInImageViewRaycast( + const DepthImage& depth_frame, const Transform& T_L_C, const Camera& camera, + const float block_size, const float truncation_distance_m, + const float max_integration_distance_m); + +template std::vector ViewCalculator::getBlocksInImageViewRaycast( + const DepthImage& depth_frame, const Transform& T_L_C, + const CameraPinhole& camera, const float block_size, + const float truncation_distance_m, const float max_integration_distance_m); + +/////////////////////////////////////////////////////////////// ViewCalculator::ViewCalculator() { cudaStreamCreate(&cuda_stream_); } ViewCalculator::~ViewCalculator() { cudaStreamDestroy(cuda_stream_); } @@ -191,6 +203,7 @@ __global__ void combinedBlockIndicesInImageKernel( } // Ok now project this thing into space. + // in the camera coordinate Vector3f p_C = (depth + truncation_distance_m) * camera.vectorFromPixelIndices(Index2D(pixel_col, pixel_row)); Vector3f p_L = T_L_C * p_C; @@ -227,8 +240,9 @@ std::vector ViewCalculator::getBlocksInImageViewRaycastTemplate( const Index3D aabb_size = max_index - min_index + Index3D::Ones(); const size_t aabb_linear_size = aabb_size.x() * aabb_size.y() * aabb_size.z(); - // A 3D grid of bools, one for each block in the AABB, which indicates if it - // is in the view. The 3D grid is represented as a flat vector. + // A 3D grid of bools, one for each block in the + // AABB, which indicates if it is in the view. The 3D grid is represented + // as a flat vector. if (aabb_linear_size > aabb_device_buffer_.size()) { constexpr float kBufferExpansionFactor = 1.5f; const int new_size = @@ -236,6 +250,7 @@ std::vector ViewCalculator::getBlocksInImageViewRaycastTemplate( aabb_device_buffer_.reserve(new_size); aabb_host_buffer_.reserve(new_size); } + checkCudaErrors(cudaMemsetAsync(aabb_device_buffer_.data(), 0, sizeof(bool) * aabb_linear_size)); aabb_device_buffer_.resize(aabb_linear_size); @@ -244,6 +259,7 @@ std::vector ViewCalculator::getBlocksInImageViewRaycastTemplate( setup_timer.Stop(); // Raycast + // default: true if (raycast_to_pixels_) { getBlocksByRaycastingPixels(T_L_C, camera, depth_frame, block_size, truncation_distance_m, @@ -277,10 +293,11 @@ std::vector ViewCalculator::getBlocksInImageViewRaycastTemplate( } // Camera +template std::vector ViewCalculator::getBlocksInImageViewRaycast( - const DepthImage& depth_frame, const Transform& T_L_C, const Camera& camera, - const float block_size, const float truncation_distance_m, - const float max_integration_distance_m) { + const DepthImage& depth_frame, const Transform& T_L_C, + const CameraType& camera, const float block_size, + const float truncation_distance_m, const float max_integration_distance_m) { return getBlocksInImageViewRaycastTemplate(depth_frame, T_L_C, camera, block_size, truncation_distance_m, max_integration_distance_m); @@ -296,6 +313,16 @@ std::vector ViewCalculator::getBlocksInImageViewRaycast( max_integration_distance_m); } +// OSLidar +std::vector ViewCalculator::getBlocksInImageViewRaycast( + const DepthImage& depth_frame, const Transform& T_L_C, const OSLidar& lidar, + const float block_size, const float truncation_distance_m, + const float max_integration_distance_m) { + return getBlocksInImageViewRaycastTemplate(depth_frame, T_L_C, lidar, + block_size, truncation_distance_m, + max_integration_distance_m); +} + template void ViewCalculator::getBlocksByRaycastingCorners( const Transform& T_L_C, const SensorType& camera, @@ -383,9 +410,13 @@ void ViewCalculator::getBlocksByRaycastingPixels( depth_frame.cols(), block_size, max_integration_distance_m, truncation_distance_m, raycast_subsampling_factor_, min_index, aabb_size, aabb_updated_cuda); + checkCudaErrors(cudaStreamSynchronize(cuda_stream_)); checkCudaErrors(cudaPeekAtLastError()); combined_kernel_timer.Stop(); } -} // namespace nvblox \ No newline at end of file +} // namespace nvblox + +// .cpp +// lidar.depth_frame_ptr[i] -> segmentation fault \ No newline at end of file diff --git a/nvblox/src/integrators/view_calculator.cpp b/nvblox/src/integrators/view_calculator.cpp index ffb86007e..33691b3be 100644 --- a/nvblox/src/integrators/view_calculator.cpp +++ b/nvblox/src/integrators/view_calculator.cpp @@ -19,8 +19,21 @@ limitations under the License. namespace nvblox { +/// NOTE(gogojjh): define template function +template std::vector ViewCalculator::getBlocksInImageViewPlanes( + const DepthImage& depth_frame, const Transform& T_L_C, const Camera& camera, + const float block_size, const float truncation_distance_m, + const float max_integration_distance_m); + +template std::vector ViewCalculator::getBlocksInImageViewPlanes( + const DepthImage& depth_frame, const Transform& T_L_C, + const CameraPinhole& camera, const float block_size, + const float truncation_distance_m, const float max_integration_distance_m); + +////////////////////////////////////////////////////////////////// +template std::vector ViewCalculator::getBlocksInViewPlanes( - const Transform& T_L_C, const Camera& camera, const float block_size, + const Transform& T_L_C, const CameraType& camera, const float block_size, const float max_distance) { CHECK_GT(max_distance, 0.0f); @@ -46,10 +59,11 @@ std::vector ViewCalculator::getBlocksInViewPlanes( return block_indices_in_frustum; } +template std::vector ViewCalculator::getBlocksInImageViewPlanes( - const DepthImage& depth_frame, const Transform& T_L_C, const Camera& camera, - const float block_size, const float truncation_distance_m, - const float max_integration_distance_m) { + const DepthImage& depth_frame, const Transform& T_L_C, + const CameraType& camera, const float block_size, + const float truncation_distance_m, const float max_integration_distance_m) { float min_depth, max_depth; std::tie(min_depth, max_depth) = image::minmaxGPU(depth_frame); float max_depth_plus_trunc = max_depth + truncation_distance_m; diff --git a/nvblox/src/rays/cuda/sphere_tracer.cu b/nvblox/src/rays/cuda/sphere_tracer.cu index fb4cd0d66..811b85e73 100644 --- a/nvblox/src/rays/cuda/sphere_tracer.cu +++ b/nvblox/src/rays/cuda/sphere_tracer.cu @@ -25,6 +25,20 @@ limitations under the License. namespace nvblox { +/// NOTE(gogojjh): define the template functions +template std::shared_ptr SphereTracer::renderImageOnGPU( + const Camera& camera, const Transform& T_L_C, const TsdfLayer& tsdf_layer, + const float truncation_distance_m, + const MemoryType output_image_memory_type, + const int ray_subsampling_factor); + +template std::shared_ptr SphereTracer::renderImageOnGPU( + const CameraPinhole& camera, const Transform& T_L_C, + const TsdfLayer& tsdf_layer, const float truncation_distance_m, + const MemoryType output_image_memory_type, + const int ray_subsampling_factor); + +///////////////////////////////////////////////////// __device__ inline bool isTsdfVoxelValid(const TsdfVoxel& voxel) { constexpr float kMinWeight = 1e-4; return voxel.weight > kMinWeight; @@ -190,8 +204,9 @@ __global__ void sphereTracingKernel( success_flags[ray_idx] = result.second; } +template __global__ void sphereTracingKernel( - const Camera camera, // NOLINT + const CameraType camera, // NOLINT const Transform T_L_C, // NOLINT Index3DDeviceHashMapType block_hash, // NOLINT float* image, // NOLINT @@ -318,9 +333,10 @@ bool SphereTracer::castOnGPU(const Ray& ray, const TsdfLayer& tsdf_layer, return success_flag; } +template std::shared_ptr SphereTracer::renderImageOnGPU( - const Camera& camera, const Transform& T_L_C, const TsdfLayer& tsdf_layer, - const float truncation_distance_m, + const CameraType& camera, const Transform& T_L_C, + const TsdfLayer& tsdf_layer, const float truncation_distance_m, const MemoryType output_image_memory_type, const int ray_subsampling_factor) { CHECK_EQ(camera.width() % ray_subsampling_factor, 0); diff --git a/nvblox/src/utils/timing.cpp b/nvblox/src/utils/timing.cpp index fafa535c7..8da5ec245 100644 --- a/nvblox/src/utils/timing.cpp +++ b/nvblox/src/utils/timing.cpp @@ -209,7 +209,7 @@ std::string Timing::SecondsToTimeString(double seconds) { } void Timing::Print(std::ostream& out) { - map_t& tagMap = Instance().tagMap_; + map_t& tagMap = Instance().tagMap_; // std::map if (tagMap.empty()) { return; @@ -217,6 +217,9 @@ void Timing::Print(std::ostream& out) { out << "NVBlox Timing\n"; out << "-----------\n"; + out << "operation\tsample_number\ttotal_second\tmean_second\tmin_second\tmax_" + "second\n"; + out << "-----------\n"; for (typename map_t::value_type t : tagMap) { size_t i = t.second; out.width((std::streamsize)Instance().maxTagLength_); @@ -243,12 +246,69 @@ void Timing::Print(std::ostream& out) { out << std::endl; } } + std::string Timing::Print() { std::stringstream ss; Print(ss); return ss.str(); } +// NOTE(gogojjh): added to only print timeing of key steps (given keywords) +void Timing::Print(std::ostream& out, + std::vector const& keywords) { + map_t& tagMap = Instance().tagMap_; // std::map + + if (tagMap.empty()) { + return; + } + + out << "NVBlox Timing\n"; + out << "-----------\n"; + out << "operation\tsample_number\ttotal_second\tmean_second\tmin_second\tmax_" + "second\n"; + out << "-----------\n"; + for (typename map_t::value_type t : tagMap) { + bool contain_word = false; + for (const auto& word : keywords) { + if (t.first.find(word) != std::string::npos) { + contain_word = true; + break; + } + } + if (!contain_word) continue; + + size_t i = t.second; + out.width((std::streamsize)Instance().maxTagLength_); + out.setf(std::ios::left, std::ios::adjustfield); + out << t.first << "\t"; + out.width(7); + + out.setf(std::ios::right, std::ios::adjustfield); + out << GetNumSamples(i) << "\t"; + if (GetNumSamples(i) > 0) { + out << SecondsToTimeString(GetTotalSeconds(i)) << "\t"; + double meansec = GetMeanSeconds(i); + double stddev = sqrt(GetVarianceSeconds(i)); + out << "(" << SecondsToTimeString(meansec) << " +- "; + out << SecondsToTimeString(stddev) << ")\t"; + + double minsec = GetMinSeconds(i); + double maxsec = GetMaxSeconds(i); + + // The min or max are out of bounds. + out << "[" << SecondsToTimeString(minsec) << "," + << SecondsToTimeString(maxsec) << "]"; + } + out << std::endl; + } +} + +std::string Timing::Print(std::vector const& keywords) { + std::stringstream ss; + Print(ss, keywords); + return ss.str(); +} + void Timing::Reset() { std::lock_guard lock(Instance().mutex_); Instance().tagMap_.clear(); diff --git a/nvblox/tests/CMakeLists.txt b/nvblox/tests/CMakeLists.txt index 8d9e1740d..416a422ac 100644 --- a/nvblox/tests/CMakeLists.txt +++ b/nvblox/tests/CMakeLists.txt @@ -61,7 +61,7 @@ target_link_libraries(test_tsdf_integrator nvblox_test_utils) gtest_discover_tests(test_tsdf_integrator ${TEST_OPTIONS}) add_executable(test_3dmatch test_3dmatch.cpp) -target_link_libraries(test_3dmatch nvblox_test_utils nvblox_lib) +target_link_libraries(test_3dmatch nvblox_test_utils nvblox_lib nvblox_datasets_3dmatch) gtest_discover_tests(test_3dmatch ${TEST_OPTIONS}) set_target_properties(test_3dmatch PROPERTIES CUDA_SEPARABLE_COMPILATION ON) @@ -70,7 +70,7 @@ target_link_libraries(test_unified_ptr nvblox_test_utils) gtest_discover_tests(test_unified_ptr ${TEST_OPTIONS}) add_executable(test_mesh test_mesh.cpp) -target_link_libraries(test_mesh nvblox_test_utils) +target_link_libraries(test_mesh nvblox_test_utils nvblox_datasets_3dmatch) gtest_discover_tests(test_mesh ${TEST_OPTIONS}) add_executable(test_scene test_scene.cpp) @@ -94,7 +94,7 @@ target_link_libraries(test_esdf_integrator nvblox_test_utils) gtest_discover_tests(test_esdf_integrator ${TEST_OPTIONS}) add_executable(test_color_image test_color_image.cpp) -target_link_libraries(test_color_image nvblox_test_utils) +target_link_libraries(test_color_image nvblox_test_utils nvblox_datasets_3dmatch) gtest_discover_tests(test_color_image ${TEST_OPTIONS}) add_executable(test_color_integrator test_color_integrator.cpp) @@ -102,15 +102,15 @@ target_link_libraries(test_color_integrator nvblox_test_utils) gtest_discover_tests(test_color_integrator ${TEST_OPTIONS}) add_executable(test_mesh_coloring test_mesh_coloring.cpp) -target_link_libraries(test_mesh_coloring nvblox_test_utils) +target_link_libraries(test_mesh_coloring nvblox_test_utils nvblox_datasets_3dmatch) gtest_discover_tests(test_mesh_coloring ${TEST_OPTIONS}) add_executable(test_for_memory_leaks test_for_memory_leaks.cpp) -target_link_libraries(test_for_memory_leaks nvblox_test_utils) +target_link_libraries(test_for_memory_leaks nvblox_test_utils nvblox_datasets_3dmatch) gtest_discover_tests(test_for_memory_leaks ${TEST_OPTIONS}) add_executable(test_frustum test_frustum.cpp) -target_link_libraries(test_frustum nvblox_test_utils) +target_link_libraries(test_frustum nvblox_test_utils nvblox_datasets_3dmatch) gtest_discover_tests(test_frustum ${TEST_OPTIONS}) add_executable(test_gpu_layer_view test_gpu_layer_view.cpp) @@ -141,9 +141,14 @@ add_executable(test_lidar_integration test_lidar_integration.cpp) target_link_libraries(test_lidar_integration nvblox_test_utils) gtest_discover_tests(test_lidar_integration ${TEST_OPTIONS}) -add_executable(test_fuser test_fuser.cpp) -target_link_libraries(test_fuser nvblox_test_utils) -gtest_discover_tests(test_fuser ${TEST_OPTIONS}) +### NOTE(gogojjh): fuser is already replaced by the fuser_rgbd, no need to test_fuser +# add_executable(test_fuser test_fuser.cpp) +# target_link_libraries(test_fuser nvblox_test_utils) +# gtest_discover_tests(test_fuser ${TEST_OPTIONS}) + +add_executable(test_fuser_rgbd test_fuser_rgbd.cpp) +target_link_libraries(test_fuser_rgbd nvblox_test_utils nvblox_datasets_3dmatch) +gtest_discover_tests(test_fuser_rgbd ${TEST_OPTIONS}) add_executable(test_bounding_spheres test_bounding_spheres.cpp) target_link_libraries(test_bounding_spheres nvblox_test_utils) @@ -152,3 +157,13 @@ gtest_discover_tests(test_bounding_spheres ${TEST_OPTIONS}) add_executable(test_mapper test_mapper.cpp) target_link_libraries(test_mapper nvblox_test_utils) gtest_discover_tests(test_mapper ${TEST_OPTIONS}) + +### NOTE(gogojjh): need to rewrite the test_oslidar to adjust the new oslidar model +# add_executable(test_oslidar test_oslidar.cpp) +# target_link_libraries(test_oslidar nvblox_test_utils) +# gtest_discover_tests(test_oslidar ${TEST_OPTIONS}) + +#### camera_pinhole +add_executable(test_camera_pinhole test_camera_pinhole.cpp) +target_link_libraries(test_camera_pinhole nvblox_test_utils) +gtest_discover_tests(test_camera_pinhole ${TEST_OPTIONS}) diff --git a/nvblox/tests/test_camera_pinhole.cpp b/nvblox/tests/test_camera_pinhole.cpp new file mode 100644 index 000000000..19a96a574 --- /dev/null +++ b/nvblox/tests/test_camera_pinhole.cpp @@ -0,0 +1,386 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ +#include + +#include "nvblox/core/bounding_boxes.h" +#include "nvblox/core/camera_pinhole.h" +#include "nvblox/core/types.h" + +#include "nvblox/tests/utils.h" + +using namespace nvblox; + +// TODO: Decide where to put test epsilons +// NOTE(alexmillane): I had to crank this up slightly to get things to pass... I +// guess this is just floating point errors accumulating? +constexpr float kFloatEpsilon = 1e-4; + +std::pair getRandomVisibleRayAndImagePoint( + const CameraPinhole& camera) { + // Random point on image plane + const Vector2f u_C(test_utils::randomFloatInRange( + 0.0f, static_cast(camera.width() - 1)), + test_utils::randomFloatInRange( + 0.0f, static_cast(camera.height() - 1))); + // Normalized ray + return {camera.vectorFromImagePlaneCoordinates(u_C).normalized(), u_C}; +} + +CameraPinhole getTestCamera() { + Matrix3f K; + K << 300.0f, 0.0f, 320.0f, 0.0f, 300.0f, 240.0f, 0.0f, 0.0f, 1.0f; + constexpr int width = 640; + constexpr int height = 480; + return CameraPinhole(K, width, height); +} + +TEST(CameraTest, PointProjection) { + const CameraPinhole camera = getTestCamera(); + Vector3f P(0.0, 0.0f, 5.0f); + Vector2f u; + EXPECT_TRUE(camera.project(P, &u)); + std::cout << "u: " << u.transpose() << std::endl; +} + +TEST(CameraTest, PointsInView) { + // Make sure this is deterministic. + std::srand(0); + + const CameraPinhole camera = getTestCamera(); + + // Generate some random points (in view) and project them back + constexpr int kNumPoints = 1000; + for (int i = 0; i < kNumPoints; i++) { + Vector3f ray_C; + Vector2f u_C; + std::tie(ray_C, u_C) = getRandomVisibleRayAndImagePoint(camera); + const Vector3f p_C = test_utils::randomFloatInRange(1.0, 1000.0) * ray_C; + Vector2f u_reprojection_C; + EXPECT_TRUE(camera.project(p_C, &u_reprojection_C)); + EXPECT_TRUE(((u_reprojection_C - u_C).array().abs() < kFloatEpsilon).all()); + } +} + +TEST(CameraTest, CenterPixel) { + // Make sure this is deterministic. + std::srand(0); + + const CameraPinhole camera = getTestCamera(); + + // Center + const Vector3f center_ray = Vector3f(0.0f, 0.0f, 1.0f); + const Vector3f p_C = test_utils::randomFloatInRange(1.0, 1000.0) * center_ray; + Eigen::Vector2f u; + EXPECT_TRUE(camera.project(p_C, &u)); + EXPECT_TRUE( + ((u - Vector2f(camera.K()(0, 2), camera.K()(1, 2))).array().abs() < + kFloatEpsilon) + .all()); +} + +TEST(CameraTest, BehindCamera) { + // Make sure this is deterministic. + std::srand(0); + + const CameraPinhole camera = getTestCamera(); + + constexpr int kNumPoints = 1000; + for (int i = 0; i < kNumPoints; i++) { + Vector3f ray_C; + Vector2f u_C; + std::tie(ray_C, u_C) = getRandomVisibleRayAndImagePoint(camera); + Vector3f p_C = test_utils::randomFloatInRange(1.0, 1000.0) * ray_C; + // The negative here puts the point behind the camera + p_C.z() = -1.0f * p_C.z(); + Vector2f u_reprojection_C; + EXPECT_FALSE(camera.project(p_C, &u_reprojection_C)); + } +} + +TEST(CameraTest, OutsideImagePlane) { + // Make sure this is deterministic. + std::srand(0); + + const CameraPinhole camera = getTestCamera(); + + constexpr int kNumPoints = 1000; + for (int i = 0; i < kNumPoints; i++) { + // Random point off image plane + // Add a random offset to the center pixel with sufficient magnitude to take + // it off the plane. + constexpr float kOffImagePlaneFactor = 5.0; + const Vector2f u_perturbation_C( + test_utils::randomSign() * + test_utils::randomFloatInRange( + camera.K()(0, 2), kOffImagePlaneFactor * camera.width()), + test_utils::randomSign() * + test_utils::randomFloatInRange( + camera.K()(1, 2), kOffImagePlaneFactor * camera.height())); + const Vector2f u_C = + Vector2f(camera.K()(0, 2), camera.K()(1, 2)) + u_perturbation_C; + + // NOTE(alexmillane): My own ray-from-pixel function to not trigger checks + // because the pixel is off the image plane. + const auto rayFromPixelNoChecks = [camera](const auto& u_C) { + return Vector3f((u_C[0] - camera.K()(0, 2)) / camera.K()(0, 0), + (u_C[1] - camera.K()(1, 2)) / camera.K()(1, 1), 1.0f); + }; + const Vector3f ray_C = rayFromPixelNoChecks(u_C); + const Vector3f p_C = test_utils::randomFloatInRange(1.0, 1000.0) * ray_C; + Vector2f u_reprojection_C; + EXPECT_FALSE(camera.project(p_C, &u_reprojection_C)); + } +} + +TEST(CameraTest, AxisAlignedBoundingBox) { + // Make sure this is deterministic. + std::srand(0); + + const CameraPinhole camera = getTestCamera(); + + // Rays through the corners of the image plane + const Vector3f ray_0_C = + camera.vectorFromImagePlaneCoordinates(Vector2f(0.0f, 0.0f)); + const Vector3f ray_2_C = + camera.vectorFromImagePlaneCoordinates(Vector2f(0.0f, camera.height())); + const Vector3f ray_1_C = + camera.vectorFromImagePlaneCoordinates(Vector2f(camera.width(), 0.0f)); + const Vector3f ray_3_C = camera.vectorFromImagePlaneCoordinates( + Vector2f(camera.width(), camera.height())); + + // Generate a random depths + constexpr float kMinimumDepthPx = 1.0; + constexpr float kMaximumDepthPx = 1000.0; + const float min_depth = + test_utils::randomFloatInRange(kMinimumDepthPx, kMaximumDepthPx); + const float max_depth = + test_utils::randomFloatInRange(kMinimumDepthPx, kMaximumDepthPx); + + // True bounding box from the 3D points + AlignedVector view_corners_C = { + min_depth * ray_0_C, max_depth * ray_0_C, // NOLINT + min_depth * ray_1_C, max_depth * ray_1_C, // NOLINT + min_depth * ray_2_C, max_depth * ray_2_C, // NOLINT + min_depth * ray_3_C, max_depth * ray_3_C // NOLINT + }; + AxisAlignedBoundingBox aabb_true; + std::for_each(view_corners_C.begin(), view_corners_C.end(), + [&aabb_true](const Vector3f& p) { aabb_true.extend(p); }); + + // Bounding box approximated by the camera model. + // TODO(alexmillane): Only tested with identity transform at the moment. + const Transform T_L_C = Transform::Identity(); + const AxisAlignedBoundingBox aabb_test = + camera.getViewAABB(T_L_C, min_depth, max_depth); + + EXPECT_TRUE(aabb_true.isApprox(aabb_test)) + << "AABB true: " << aabb_true.min().transpose() << " " + << aabb_true.max().transpose() + << " AABB test: " << aabb_test.min().transpose() << " " + << aabb_test.max().transpose(); +} + +TEST(CameraTest, FrustumTest) { + constexpr float kMinDist = 1.0f; + constexpr float kMaxDist = 10.0f; + + const CameraPinhole camera = getTestCamera(); + + Frustum frustum(camera, Transform::Identity(), kMinDist, kMaxDist); + + // Project a point into the camera. + Vector3f point_C(0.5, 0.5, 5.0); + Vector2f u_C; + ASSERT_TRUE(camera.project(point_C, &u_C)); + std::cout << u_C.transpose() << std::endl; + + // Check that the point is within the frustum. + EXPECT_TRUE(frustum.isPointInView(point_C)); + + // Check a point further than the max dist. + point_C << 0.5, 0.5, kMaxDist + 10.0f; + ASSERT_TRUE(camera.project(point_C, &u_C)); + EXPECT_FALSE(frustum.isPointInView(point_C)); + + // Check a point closer than the max dist. + point_C << 0.0, 0.0, 0.3f; + ASSERT_TRUE(camera.project(point_C, &u_C)); + EXPECT_FALSE(frustum.isPointInView(point_C)); +} + +TEST(CameraTest, FrustumAABBTest) { + constexpr float kMinDist = 1.0f; + constexpr float kMaxDist = 10.0f; + constexpr int kVoxelsPerSide = VoxelBlock::kVoxelsPerSide; + const CameraPinhole camera = getTestCamera(); + + Frustum frustum(camera, Transform::Identity(), kMinDist, kMaxDist); + AxisAlignedBoundingBox view_aabb = + camera.getViewAABB(Transform::Identity(), kMinDist, kMaxDist); + + // Double-check that the camera and the frustum AABB match. + EXPECT_TRUE(frustum.isAABBInView(view_aabb)); + EXPECT_TRUE(view_aabb.isApprox(frustum.getAABB())); + + // Get all blocks in the view AABB and make sure that some of them are + // actually in the view. + const float block_size = 1.0f; + std::vector block_indices_in_aabb = + getBlockIndicesTouchedByBoundingBox(block_size, view_aabb); + std::vector block_indices_in_frustum; + for (const Index3D& block_index : block_indices_in_aabb) { + const AxisAlignedBoundingBox& aabb_block = + getAABBOfBlock(block_size, block_index); + if (frustum.isAABBInView(aabb_block)) { + block_indices_in_frustum.push_back(block_index); + } + } + + EXPECT_GT(block_indices_in_aabb.size(), block_indices_in_frustum.size()); + EXPECT_GT(block_indices_in_frustum.size(), 0); + + // Check all voxels within the view and make sure that they're correctly + // marked. + for (const Index3D& block_index : block_indices_in_aabb) { + Index3D voxel_index; + + // Iterate over all the voxels: + for (voxel_index.x() = 0; voxel_index.x() < kVoxelsPerSide; + voxel_index.x()++) { + for (voxel_index.y() = 0; voxel_index.y() < kVoxelsPerSide; + voxel_index.y()++) { + for (voxel_index.z() = 0; voxel_index.z() < kVoxelsPerSide; + voxel_index.z()++) { + Vector3f position = getCenterPostionFromBlockIndexAndVoxelIndex( + block_size, block_index, voxel_index); + Eigen::Vector2f u_C; + bool in_frustum = frustum.isPointInView(position); + bool in_camera = camera.project(position, &u_C); + if (position.z() <= kMaxDist && position.z() >= kMinDist) { + EXPECT_EQ(in_frustum, in_camera); + } else { + // Doesn't matter if we're within the camera view if it's false. + EXPECT_FALSE(in_frustum); + } + } + } + } + } +} + +TEST(CameraTest, FrustumAtLeastOneValidVoxelTest) { + constexpr float kMinDist = 0.0f; + constexpr float kMaxDist = 10.0f; + constexpr int kVoxelsPerSide = VoxelBlock::kVoxelsPerSide; + const CameraPinhole camera = getTestCamera(); + + Frustum frustum(camera, Transform::Identity(), kMinDist, kMaxDist); + AxisAlignedBoundingBox view_aabb = + camera.getViewAABB(Transform::Identity(), kMinDist, kMaxDist); + + // Double-check that the camera and the frustum AABB match. + EXPECT_TRUE(frustum.isAABBInView(view_aabb)); + EXPECT_TRUE(view_aabb.isApprox(frustum.getAABB())); + + // Get all blocks in the view AABB and make sure that some of them are + // actually in the view. + const float block_size = 0.5f; + std::vector block_indices_in_aabb = + getBlockIndicesTouchedByBoundingBox(block_size, view_aabb); + std::vector block_indices_in_frustum; + for (const Index3D& block_index : block_indices_in_aabb) { + const AxisAlignedBoundingBox& aabb_block = + getAABBOfBlock(block_size, block_index); + if (frustum.isAABBInView(aabb_block)) { + block_indices_in_frustum.push_back(block_index); + } + } + + EXPECT_GT(block_indices_in_aabb.size(), block_indices_in_frustum.size()); + EXPECT_GT(block_indices_in_frustum.size(), 0); + + // Check that for any given block in the frustum, there's AT LEAST one valid + // voxel. + int empty = 0; + for (const Index3D& block_index : block_indices_in_frustum) { + Index3D voxel_index; + bool any_valid = false; + // Iterate over all the voxels: + for (voxel_index.x() = 0; voxel_index.x() < kVoxelsPerSide; + voxel_index.x()++) { + for (voxel_index.y() = 0; voxel_index.y() < kVoxelsPerSide; + voxel_index.y()++) { + for (voxel_index.z() = 0; voxel_index.z() < kVoxelsPerSide; + voxel_index.z()++) { + Vector3f position = getCenterPostionFromBlockIndexAndVoxelIndex( + block_size, block_index, voxel_index); + Eigen::Vector2f u_C; + bool in_frustum = frustum.isPointInView(position); + bool in_camera = camera.project(position, &u_C); + any_valid = in_camera || any_valid; + if (position.z() >= 0.0f && position.z() < 1e-4f) { + // Nothing. + } else if (position.z() <= kMaxDist && position.z() > kMinDist) { + EXPECT_EQ(in_frustum, in_camera); + } else { + // Doesn't matter if we're within the camera view if it's false. + EXPECT_FALSE(in_frustum); + } + } + } + } + const AxisAlignedBoundingBox& aabb_block = + getAABBOfBlock(block_size, block_index); + if (!any_valid) { + empty++; + } + } + // At MOST 3% empty on the corners. + EXPECT_LE(static_cast(empty) / block_indices_in_frustum.size(), 0.03); +} + +TEST(CameraTest, UnProjectionTest) { + CameraPinhole 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]); + google::InstallFailureSignalHandler(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/nvblox/tests/test_depth_image.cpp b/nvblox/tests/test_depth_image.cpp index 4b80f951c..ff3f2c52d 100644 --- a/nvblox/tests/test_depth_image.cpp +++ b/nvblox/tests/test_depth_image.cpp @@ -245,4 +245,4 @@ int main(int argc, char** argv) { google::InstallFailureSignalHandler(); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/nvblox/tests/test_fuser_rgbd.cpp b/nvblox/tests/test_fuser_rgbd.cpp new file mode 100644 index 000000000..b81767268 --- /dev/null +++ b/nvblox/tests/test_fuser_rgbd.cpp @@ -0,0 +1,106 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ + +#include + +#include "nvblox/datasets/3dmatch.h" +#include "nvblox/executables/fuser_rgbd.h" + +using namespace nvblox; + +TEST(FuserTest, CommandLineFlags) { + // Fake flags + char* argv[] = { + (char*)"CommandLineFlags_test", + (char*)"--voxel_size=1.0", + (char*)"--num_frames=2", + (char*)"--timing_output_path=3", + (char*)"--esdf_output_path=4", + (char*)"--mesh_output_path=5", + (char*)"--map_output_path=6", + (char*)"--tsdf_frame_subsampling=7", + (char*)"--color_frame_subsampling=8", + (char*)"--mesh_frame_subsampling=9", + (char*)"--esdf_frame_subsampling=10", + (char*)"--tsdf_integrator_max_integration_distance_m=11.0", + (char*)"--tsdf_integrator_truncation_distance_vox=12.0", + (char*)"--tsdf_integrator_max_weight=13.0", + (char*)"--mesh_integrator_min_weight=14.0", + (char*)"--mesh_integrator_weld_vertices=false", + (char*)"--color_integrator_max_integration_distance_m=15.0", + (char*)"--esdf_integrator_min_weight=16.0", + (char*)"--esdf_integrator_max_site_distance_vox=17.0", + (char*)"--esdf_integrator_max_distance_m=18.0", + NULL, + }; + int argc = (sizeof(argv) / sizeof(*(argv))) - 1; + char** argv_ptr = argv; + gflags::ParseCommandLineFlags(&argc, &argv_ptr, true); + + std::unique_ptr fuser = + datasets::threedmatch::createFuser("./data/3dmatch", 1); + + // Check that the params made it in + constexpr float kEps = 1.0e-6; + + // Layer params + CHECK_NEAR(fuser->voxel_size_m_, 1.0f, kEps); + CHECK_NEAR(fuser->mapper().tsdf_layer().voxel_size(), 1.0f, kEps); + + // Dataset + CHECK_EQ(fuser->num_frames_to_integrate_, 2); + + // Output paths + CHECK_EQ(fuser->timing_output_path_, "3"); + CHECK_EQ(fuser->esdf_output_path_, "4"); + CHECK_EQ(fuser->mesh_output_path_, "5"); + CHECK_EQ(fuser->map_output_path_, "6"); + + // Subsampling + CHECK_EQ(fuser->tsdf_frame_subsampling_, 7); + CHECK_EQ(fuser->color_frame_subsampling_, 8); + CHECK_EQ(fuser->mesh_frame_subsampling_, 9); + CHECK_EQ(fuser->esdf_frame_subsampling_, 10); + + // TSDF integrator + CHECK_NEAR(fuser->mapper().tsdf_integrator().max_integration_distance_m(), + 11.0f, kEps); + CHECK_NEAR(fuser->mapper().tsdf_integrator().truncation_distance_vox(), 12.0f, + kEps); + CHECK_NEAR(fuser->mapper().tsdf_integrator().max_weight(), 13.0f, kEps); + + // Mesh integrator + CHECK_NEAR(fuser->mapper().mesh_integrator().min_weight(), 14.0f, kEps); + CHECK_EQ(fuser->mapper().mesh_integrator().weld_vertices(), false); + + // Color integrator + CHECK_NEAR(fuser->mapper().color_integrator().max_integration_distance_m(), + 15.0f, kEps); + + // ESDF integrator + CHECK_NEAR(fuser->mapper().esdf_integrator().min_weight(), 16.0f, kEps); + CHECK_NEAR(fuser->mapper().esdf_integrator().max_site_distance_vox(), 17.0f, + kEps); + CHECK_NEAR(fuser->mapper().esdf_integrator().max_distance_m(), 18.0f, kEps); +} + +int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/nvblox/tests/test_lidar.cpp b/nvblox/tests/test_lidar.cpp index 83b0197fc..8c7e926f7 100644 --- a/nvblox/tests/test_lidar.cpp +++ b/nvblox/tests/test_lidar.cpp @@ -23,8 +23,7 @@ limitations under the License. using namespace nvblox; constexpr float kFloatEpsilon = 1e-4; -class LidarTest - : public ::testing::Test { +class LidarTest : public ::testing::Test { protected: LidarTest() {} }; @@ -44,7 +43,8 @@ TEST_P(ParameterizedLidarTest, Extremes) { const float vertical_fov_deg = std::get<2>(params); const float vertical_fov_rad = vertical_fov_deg * M_PI / 180.0f; - Lidar lidar(num_azimuth_divisions, num_elevation_divisions, vertical_fov_rad); + Lidar lidar(num_azimuth_divisions, num_elevation_divisions, 2.0f * M_PI, + vertical_fov_rad); //------------------- // Elevation extremes @@ -123,7 +123,8 @@ TEST_P(ParameterizedLidarTest, SphereTest) { const float vertical_fov_deg = std::get<2>(params); const float vertical_fov_rad = vertical_fov_deg * M_PI / 180.0f; - Lidar lidar(num_azimuth_divisions, num_elevation_divisions, vertical_fov_rad); + Lidar lidar(num_azimuth_divisions, num_elevation_divisions, 2.0f * M_PI, + vertical_fov_rad); // Pointcloud Eigen::MatrixX3f pointcloud(num_azimuth_divisions * num_elevation_divisions, @@ -196,7 +197,8 @@ TEST_P(ParameterizedLidarTest, OutOfBoundsTest) { const float vertical_fov_deg = std::get<2>(params); const float vertical_fov_rad = vertical_fov_deg * M_PI / 180.0f; - Lidar lidar(num_azimuth_divisions, num_elevation_divisions, vertical_fov_rad); + Lidar lidar(num_azimuth_divisions, num_elevation_divisions, 2.0f * M_PI, + vertical_fov_rad); // Outside on top and bottom const float rads_per_pixel_elevation = @@ -234,7 +236,8 @@ TEST_P(ParameterizedLidarTest, PixelToRayExtremes) { const float vertical_fov_rad = vertical_fov_deg * M_PI / 180.0f; const float half_vertical_fov_rad = vertical_fov_rad / 2.0; - Lidar lidar(num_azimuth_divisions, num_elevation_divisions, vertical_fov_rad); + Lidar lidar(num_azimuth_divisions, num_elevation_divisions, 2.0f * M_PI, + vertical_fov_rad); // Special pixels to use const float middle_elevation_pixel = (num_elevation_divisions - 1) / 2; @@ -316,7 +319,8 @@ TEST_P(ParameterizedLidarTest, RandomPixelRoundTrips) { const float vertical_fov_rad = vertical_fov_deg * M_PI / 180.0f; const float half_vertical_fov_rad = vertical_fov_rad / 2.0; - Lidar lidar(num_azimuth_divisions, num_elevation_divisions, vertical_fov_rad); + Lidar lidar(num_azimuth_divisions, num_elevation_divisions, 2.0f * M_PI, + vertical_fov_rad); // Test a large number of points const int kNumberOfPointsToTest = 10000; diff --git a/nvblox/tests/test_lidar_integration.cpp b/nvblox/tests/test_lidar_integration.cpp index 3ebca6cd4..e3af5fac5 100644 --- a/nvblox/tests/test_lidar_integration.cpp +++ b/nvblox/tests/test_lidar_integration.cpp @@ -35,8 +35,8 @@ constexpr float kFloatEpsilon = 1e-4; class LidarIntegrationTest : public ::testing::Test { protected: LidarIntegrationTest() - : lidar(num_azimuth_divisions, num_elevation_divisions, - vertical_fov_rad) { + : lidar(num_azimuth_divisions, num_elevation_divisions, vertical_fov_rad, + 2.0 * M_PI) { // } diff --git a/nvblox/tests/test_oslidar.cpp b/nvblox/tests/test_oslidar.cpp new file mode 100644 index 000000000..923182532 --- /dev/null +++ b/nvblox/tests/test_oslidar.cpp @@ -0,0 +1,261 @@ +/* +Copyright 2022 NVIDIA CORPORATION + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +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. +*/ +#include + +#include "nvblox/core/image.h" +#include "nvblox/core/oslidar.h" +#include "nvblox/core/types.h" + +#include +#include "nvblox/tests/utils.h" + +using namespace nvblox; + +constexpr float kFloatEpsilon = 1e-4; +class oslidarTest : public ::testing::Test { + protected: + oslidarTest() {} +}; + +class ParameterizedoslidarTest + : public oslidarTest, + public ::testing::WithParamInterface> { + protected: + // Yo dawg I heard you like params +}; + +TEST_P(ParameterizedoslidarTest, Extremes) { + // oslidar params + const auto params = GetParam(); + const int num_azimuth_divisions = std::get<0>(params); + const int num_elevation_divisions = std::get<1>(params); + const float horizontal_fov_deg = std::get<2>(params); + const float horizontal_fov_rad = horizontal_fov_deg / 180.0 * M_PI; + const float vertical_fov_deg = std::get<3>(params); + const float vertical_fov_rad = vertical_fov_deg / 180.0 * M_PI; + + DepthImage depth_image(num_elevation_divisions, num_azimuth_divisions); + DepthImage height_image(num_elevation_divisions, num_azimuth_divisions); + OSLidar oslidar(num_azimuth_divisions, num_elevation_divisions, + horizontal_fov_rad, vertical_fov_rad, 1.1781f, 1.9635f, 0.0f, + 2.0f * M_PI); + + //------------------- + // Elevation extremes + //------------------- + float azimuth_center_pixel = + static_cast(num_azimuth_divisions - 1) / 2.0f; + float elevation_center_pixel = + static_cast(num_elevation_divisions - 1) / 2.0f; + + float elevation_top_pixel = 0.0f; + float elevation_bottom_pixel = + static_cast(num_elevation_divisions) - 1; + + const float x_dist = 10; + const float z_dist = x_dist * tan(vertical_fov_rad / 2.0f); + + // Top beam + Vector2f u_C; + Vector3f p = Vector3f(x_dist, 0.0, z_dist); + EXPECT_TRUE(oslidar.project(p, &u_C)); + EXPECT_NEAR(u_C.x(), azimuth_center_pixel, kFloatEpsilon); + EXPECT_NEAR(u_C.y(), elevation_top_pixel, kFloatEpsilon); + + // Center + p = Vector3f(x_dist, 0.0f, 0.0f); + EXPECT_TRUE(oslidar.project(p, &u_C)); + EXPECT_NEAR(u_C.x(), azimuth_center_pixel, kFloatEpsilon); + EXPECT_NEAR(u_C.y(), elevation_center_pixel, kFloatEpsilon); + + // Bottom beam + p = Vector3f(x_dist, 0.0f, -z_dist); + EXPECT_TRUE(oslidar.project(p, &u_C)); + EXPECT_NEAR(u_C.x(), azimuth_center_pixel, kFloatEpsilon); + EXPECT_NEAR(u_C.y(), elevation_bottom_pixel, kFloatEpsilon); + + //----------------- + // Azimuth extremes + //----------------- + + float azimuth_left_pixel = 0.0; + float azimuth_quarter_pixel = + (azimuth_center_pixel - azimuth_left_pixel) / 2.0f + azimuth_left_pixel; + float azimuth_three_quarter_pixel = + (azimuth_center_pixel - azimuth_left_pixel) / 2.0f + azimuth_center_pixel; + + // Backwards + p = Vector3f(-1.0, 0.0, 0.0); + EXPECT_TRUE(oslidar.project(p, &u_C)); + EXPECT_NEAR(u_C.x(), azimuth_left_pixel, kFloatEpsilon); + EXPECT_NEAR(u_C.y(), elevation_center_pixel, kFloatEpsilon); + + // Right + p = Vector3f(0.0, -1.0, 0.0); + EXPECT_TRUE(oslidar.project(p, &u_C)); + EXPECT_NEAR(u_C.x(), azimuth_three_quarter_pixel, kFloatEpsilon); + EXPECT_NEAR(u_C.y(), elevation_center_pixel, kFloatEpsilon); + + // Forwards + p = Vector3f(1.0, 0.0, 0.0); + EXPECT_TRUE(oslidar.project(p, &u_C)); + EXPECT_NEAR(u_C.x(), azimuth_center_pixel, kFloatEpsilon); + EXPECT_NEAR(u_C.y(), elevation_center_pixel, kFloatEpsilon); + + // Left + p = Vector3f(0.0, 1.0, 0.0); + EXPECT_TRUE(oslidar.project(p, &u_C)); + EXPECT_NEAR(u_C.x(), azimuth_quarter_pixel, kFloatEpsilon); + EXPECT_NEAR(u_C.y(), elevation_center_pixel, kFloatEpsilon); +} + +TEST_P(ParameterizedoslidarTest, SphereTest) { + // oslidar params + const auto params = GetParam(); + const int num_azimuth_divisions = std::get<0>(params); + const int num_elevation_divisions = std::get<1>(params); + const float horizontal_fov_deg = std::get<2>(params); + const float horizontal_fov_rad = horizontal_fov_deg / 180.0 * M_PI; + const float vertical_fov_deg = std::get<3>(params); + const float vertical_fov_rad = vertical_fov_deg / 180.0 * M_PI; + + DepthImage depth_image(num_elevation_divisions, num_azimuth_divisions); + DepthImage height_image(num_elevation_divisions, num_azimuth_divisions); + OSLidar oslidar(num_azimuth_divisions, num_elevation_divisions, + horizontal_fov_rad, vertical_fov_rad, 1.1781f, 1.9635f, 0.0f, + 2.0f * M_PI); + + // Pointcloud + Eigen::MatrixX3f pointcloud(num_azimuth_divisions * num_elevation_divisions, + 3); + Eigen::MatrixXf desired_image(num_elevation_divisions, num_azimuth_divisions); + + // Construct a pointcloud of a points at random distances. + const float azimuth_increments_rad = 2 * M_PI / (num_azimuth_divisions - 1); + const float polar_increments_rad = + vertical_fov_rad / (num_elevation_divisions - 1); + const float half_vertical_fov_rad = vertical_fov_rad / 2.0f; + + int point_idx = 0; + for (int az_idx = 0; az_idx < num_azimuth_divisions; az_idx++) { + for (int el_idx = 0; el_idx < num_elevation_divisions; el_idx++) { + const float azimuth_rad = M_PI - az_idx * azimuth_increments_rad; + const float polar_rad = + el_idx * polar_increments_rad - half_vertical_fov_rad; + + constexpr float max_depth = 10.0; + constexpr float min_depth = 1.0; + const float distance = + test_utils::randomFloatInRange(min_depth, max_depth); + + const float x = distance * cos(polar_rad) * cos(azimuth_rad); + const float y = distance * cos(polar_rad) * sin(azimuth_rad); + const float z = distance * sin(polar_rad); + + pointcloud(point_idx, 0) = x; + pointcloud(point_idx, 1) = y; + pointcloud(point_idx, 2) = z; + + // from bottom to top + desired_image(num_elevation_divisions - el_idx - 1, az_idx) = distance; + + point_idx++; + } + } + + // Project the pointcloud to a depth image + Eigen::MatrixXf reprojected_image(num_elevation_divisions, + num_azimuth_divisions); + for (int point_idx = 0; point_idx < pointcloud.rows(); point_idx++) { + // Projection + Vector2f u_C_float; + EXPECT_TRUE(oslidar.project(pointcloud.row(point_idx), &u_C_float)); + Index2D u_C_int; + EXPECT_TRUE(oslidar.project(pointcloud.row(point_idx), &u_C_int)); + + // Check that this is at the center of a pixel + Vector2f corner_dist = u_C_float - u_C_float.array().round().matrix(); + constexpr float kReprojectionEpsilon = 0.001; + EXPECT_NEAR(corner_dist.x(), 0.0f, kReprojectionEpsilon); + EXPECT_NEAR(corner_dist.y(), 0.0f, kReprojectionEpsilon); + + // Add to depth image + reprojected_image(u_C_int.y(), u_C_int.x()) = + pointcloud.row(point_idx).norm(); + } + + const Eigen::MatrixXf error_image = + desired_image.array() - reprojected_image.array(); + float max_error = error_image.rowwise().mean().mean(); + EXPECT_NEAR(max_error, 0.0, kFloatEpsilon); +} + +TEST_P(ParameterizedoslidarTest, OutOfBoundsTest) { + // oslidar params + const auto params = GetParam(); + const int num_azimuth_divisions = std::get<0>(params); + const int num_elevation_divisions = std::get<1>(params); + const float horizontal_fov_deg = std::get<2>(params); + const float horizontal_fov_rad = horizontal_fov_deg / 180.0 * M_PI; + const float vertical_fov_deg = std::get<3>(params); + const float vertical_fov_rad = vertical_fov_deg / 180.0 * M_PI; + + DepthImage depth_image(num_elevation_divisions, num_azimuth_divisions); + DepthImage height_image(num_elevation_divisions, num_azimuth_divisions); + OSLidar oslidar(num_azimuth_divisions, num_elevation_divisions, + horizontal_fov_rad, vertical_fov_rad, 1.1781f, 1.9635f, 0.0f, + 2.0f * M_PI); + + // Outside on top and bottom + const float rads_per_pixel_elevation = + vertical_fov_rad / static_cast(num_elevation_divisions - 1); + const float x_dist = 10; + const float z_dist = x_dist * tan(vertical_fov_rad / 2.0f + + rads_per_pixel_elevation / 2.0f + 0.1); + + Vector2f u_C_float; + EXPECT_FALSE(oslidar.project(Vector3f(x_dist, 0.0f, z_dist), &u_C_float)); + EXPECT_FALSE(oslidar.project(Vector3f(x_dist, 0.0f, -z_dist), &u_C_float)); + Index2D u_C_int; + EXPECT_FALSE(oslidar.project(Vector3f(x_dist, 0.0f, z_dist), &u_C_int)); + EXPECT_FALSE(oslidar.project(Vector3f(x_dist, 0.0f, -z_dist), &u_C_int)); + + // Make a bunch of points with only azimuth and check they all pass + for (int i = 0; i < 1000; i++) { + const float theta = test_utils::randomFloatInRange(-M_PI - 0.1, M_PI + 0.1); + const float radius = 10.0f; + const float x = radius * cos(theta); + const float y = radius * cos(theta); + const float z = 0; + + EXPECT_TRUE(oslidar.project(Vector3f(x, y, z), &u_C_float)); + EXPECT_TRUE(oslidar.project(Vector3f(x, y, z), &u_C_int)); + } +} + +// clang-format off +INSTANTIATE_TEST_CASE_P( + ParameterizedoslidarTests, ParameterizedoslidarTest, ::testing::Values( + std::tuple(2048, 128, 360.0f, 45.0f))); +// clang-format on + +int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nvblox/visualization/visualize_ptcloud.py b/nvblox/visualization/visualize_ptcloud.py new file mode 100755 index 000000000..24ece8842 --- /dev/null +++ b/nvblox/visualization/visualize_ptcloud.py @@ -0,0 +1,47 @@ +#!/usr/bin/python3 + +# +# Copyright 2022 NVIDIA CORPORATION +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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. +# + +import os +import sys +import argparse + +import numpy as np +import open3d as o3d + + +def visualize_pcd(pcd_path: str): + # Load the mesh. + ptcloud = o3d.io.read_point_cloud(pcd_path) + print(ptcloud) + + # Create a window. + vis = o3d.visualization.Visualizer() + vis.create_window() + vis.add_geometry(ptcloud) + vis.run() + vis.destroy_window() + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="Visualize a PCD mesh.") + parser.add_argument("path", metavar="path", type=str, + help="Path to the .pcd file or file to visualize.") + + args = parser.parse_args() + if args.path: + visualize_pcd(args.path)