0.0.1

Author: kshitijgoel007

README.md

@@ -1,3 +1,9 @@
 # Python Wrapper for SOGMM
 
 Please see the meta-package https://github.com/gira3d/gira3d-reconstruction for detailed documentation.
+
+## Changelog
+
+### New in 0.0.1
+- Supporting scripts for incremental mapping with SOGMMs
+- Supporting scripts for the paper https://arxiv.org/pdf/2309.10900.pdf

paper_figures/gl-inference.py

@@ -0,0 +1,135 @@
+import os
+import json
+import argparse
+import numpy as np
+import open3d as o3d
+
+from sogmm_py.vis_open3d import VisOpen3D
+from sogmm_py.utils import np_to_o3d, o3d_to_np
+
+def vis_nvblox_mesh(path, view_string, target_file):
+    vis = VisOpen3D(visible=False)
+    render_ops = vis.get_render_option()
+    render_ops.point_size = 1
+    pcld = o3d.io.read_triangle_mesh(path, enable_post_processing=True)
+    vis.add_geometries([pcld])
+    vis.set_view_status(view_string)
+    vis.capture_screen_image(target_file)
+
+def vis_pcd(path, view_string, target_file):
+    vis = VisOpen3D(visible=True)
+    render_ops = vis.get_render_option()
+    render_ops.point_size = 1
+
+    pcld = o3d.io.read_point_cloud(path)
+    pcld_np = o3d_to_np(pcld)
+    pcld_np_cropped = pcld_np[pcld_np[:, 1] < 2.6, :]
+    pcld_cropped = np_to_o3d(pcld_np_cropped)
+
+    vis.add_geometries([pcld_cropped])
+    vis.set_view_status(view_string)
+    vis.run()
+
+def vis_gmm_pcd(path, view_string, target_file):
+    vis = VisOpen3D(visible=False)
+    render_ops = vis.get_render_option()
+    render_ops.point_size = 1
+
+    pcld = o3d.io.read_point_cloud(path)
+    pcld_np = o3d_to_np(pcld)
+    pcld_np_cropped = pcld_np[pcld_np[:, 1] < 2.6, :]
+    pcld_cropped = np_to_o3d(pcld_np_cropped)
+
+    vis.add_geometries([pcld_cropped])
+    vis.set_view_status(view_string)
+    vis.capture_screen_image(target_file)
+
+def vis_octomap_pcd(path, view_string, target_file, v):
+    vis = VisOpen3D(visible=False)
+    render_ops = vis.get_render_option()
+    render_ops.point_size = 1
+
+    pcld = o3d.io.read_point_cloud(path)
+    pcld_np = o3d_to_np(pcld)
+    pcld_np_cropped = pcld_np[pcld_np[:, 1] < 2.6, :]
+    pcld_cropped = np_to_o3d(pcld_np_cropped)
+
+    voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pcld_cropped, voxel_size=v)
+
+    vis.add_geometries([voxel_grid])
+    vis.set_view_status(view_string)
+    vis.capture_screen_image(target_file)
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="isogmm metrics")
+    parser.add_argument('--gt_path', type=str)
+    parser.add_argument('--results_path', type=str)
+    parser.add_argument('--dataset_name', type=str, default="livingroom1")
+
+    args = parser.parse_args()
+
+    livingroom_view = {
+	"class_name" : "ViewTrajectory",
+	"interval" : 29,
+	"is_loop" : False,
+	"trajectory" : 
+	[
+		{
+			"boundingbox_max" : [ 2.583730936050415, 2.5999975204467773, 4.2289800643920898 ],
+			"boundingbox_min" : [ -2.6225473880767822, 0.045030891895294189, -1.9846775531768799 ],
+			"field_of_view" : 60.0,
+			"front" : [ 0.42010601103967038, 0.8521088750580561, 0.3121240210807727 ],
+			"lookat" : [ -0.019408226013183594, 1.3225142061710358, 1.122151255607605 ],
+			"up" : [ -0.68270185199437294, 0.52336334468660561, -0.50991076741122343 ],
+			"zoom" : 2.0
+		}
+	],
+	"version_major" : 1,
+	"version_minor" : 0
+    }
+
+    lounge_view = {
+	"class_name" : "ViewTrajectory",
+	"interval" : 29,
+	"is_loop" : False,
+	"trajectory" : 
+	[
+		{
+			"boundingbox_max" : [ 2.583730936050415, 2.5999975204467773, 4.2289800643920898 ],
+			"boundingbox_min" : [ -2.6225473880767822, 0.045030891895294189, -1.9846775531768799 ],
+			"field_of_view" : 60.0,
+			"front" : [ -0.085948154700654367, -0.92242139092228226, -0.37649925932537326 ],
+			"lookat" : [ 2.0916572950719075, 1.2422129553584436, 1.1062828807923488 ],
+			"up" : [ -0.015645493914887437, -0.37660141707073896, 0.92624326781924338 ],
+			"zoom" : 0.51799999999999868
+		}
+	],
+	"version_major" : 1,
+	"version_minor" : 0
+    }
+
+    if args.dataset_name == "livingroom1":
+        view_string = json.dumps(livingroom_view)
+    if args.dataset_name == "lounge":
+        view_string = json.dumps(lounge_view)
+
+    vis_gmm_pcd(os.path.join(args.results_path, f'ground_truth/{args.dataset_name}_gt_voxelized.pcd'), view_string,
+                os.path.join(args.results_path, f'ground_truth/{args.dataset_name}_gt_voxelized.png'))
+
+    for s in ["0_02", "0_03", "0_04", "0_05"]:
+        vis_gmm_pcd(os.path.join(args.results_path, f'isogmm_results/{args.dataset_name}_bw_{s}_pr.pcd'), view_string,
+                    os.path.join(args.results_path, f'isogmm_results/{args.dataset_name}_bw_{s}_pr.png'))
+
+    for m in ["800", "400", "200", "100"]:
+        vis_gmm_pcd(os.path.join(args.results_path, f'fcgmm_results/{args.dataset_name}_{m}_pr_voxelized.pcd'), view_string,
+                    os.path.join(args.results_path, f'fcgmm_results/{args.dataset_name}_{m}_pr_voxelized.png'))
+
+    for r in ["0_02", "0_04", "0_06", "0_08"]:
+        vis_octomap_pcd(os.path.join(args.results_path, f'octomap_results/{args.dataset_name}_{r}_octomap.pcd'), view_string,
+                        os.path.join(args.results_path, f'octomap_results/{args.dataset_name}_{r}_octomap.png'), float(r.replace('_', '.')))
+
+    
+    for r in ["0.02", "0.04", "0.06", "0.08"]:
+        vis_nvblox_mesh(os.path.join(args.results_path, f'nvblox_results_2/{args.dataset_name}_{r}_nvblox.ply'), view_string,
+                        os.path.join(args.results_path, f'nvblox_results_2/{args.dataset_name}_{r}_nvblox.png'))

paper_figures/spatial-hash-ablation.py

@@ -8,28 +8,26 @@
 
 def custom_draw_geometry_with_key_callback(pcd):
 
+    def rotate_view(vis):
+        ctr = vis.get_view_control()
+        ctr.rotate(0.75, 0.0)
+        return False
+
     def change_background_to_black(vis):
         opt = vis.get_render_option()
+        opt.point_size = 2
         opt.background_color = np.asarray([0, 0, 0])
         return False
 
     key_to_callback = {}
     key_to_callback[ord("K")] = change_background_to_black
+    key_to_callback[ord("R")] = rotate_view
     o3d.visualization.draw_geometries_with_key_callbacks([pcd], key_to_callback)
 
-results_path = '/Volumes/GoogleDrive/My Drive/phd/adaptive_perception/results'
-
-with open(os.path.join(results_path, 'cave/cave.pkl'), 'rb') as f:
-    loaded_model = pickle.load(f)
-
-pts = loaded_model.sample(3*loaded_model.support_size_, 1.8)
-np.savetxt('cave_pts.txt', pts)
-
-pcd = np_to_o3d(pts)
-
-# pcd = o3d.geometry.PointCloud()
-# pcd.points = o3d.utility.Vector3dVector(pts[:, :3])
-# colors = plt.get_cmap("inferno")(pts[:, 3])
-# pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
+pcld = o3d.io.read_point_cloud('/media/fractal/T7/rss2023-resub/results/isogmm_results/livingroom1_bw_0_02_pr.pcd')
+pcld_np = o3d_to_np(pcld)
+pcld_np_cropped = pcld_np[pcld_np[:, 1] < 2.6, :]
+pcld_np_cropped_2 = pcld_np_cropped[pcld_np_cropped[:, 2] < 3.1, :]
+pcld_cropped_2 = np_to_o3d(pcld_np_cropped_2)
 
-custom_draw_geometry_with_key_callback(pcd)
+custom_draw_geometry_with_key_callback(pcld_cropped_2)

paper_figures/video.py

@@ -0,0 +1,144 @@
+import argparse
+import os
+import numpy as np
+import glob
+import time
+from cprint import cprint
+
+from sogmm_py.vis_open3d import VisOpen3D
+from sogmm_py.utils import read_log_trajectory, np_to_o3d, o3d_to_np, ImageUtils
+from sogmm_py.gmm_spatial_hash import GMMSpatialHash
+
+from sogmm_gpu import SOGMMInference as GPUInference
+from sogmm_gpu import SOGMMLearner as GPUFit
+from sogmm_gpu import SOGMMf4Device as GPUContainerf4
+from sogmm_cpu import SOGMMf4Host as CPUContainerf4
+
+
+def extract_ms_data(X):
+    d = np.array([np.linalg.norm(x) for x in X[:, 0:3]])[:, np.newaxis]
+    g = X[:, 3][:, np.newaxis]
+    return np.concatenate((d, g), axis=1)
+
+
+parser = argparse.ArgumentParser(description="Incremental-SOGMM")
+parser.add_argument('--datasetroot', type=str)
+
+args = parser.parse_args()
+
+dfolder = args.datasetroot
+
+dname = 'lounge'
+deci = 2.0
+bandwidth = 0.015
+
+learner = GPUFit(bandwidth)
+inference = GPUInference()
+gsh = GMMSpatialHash(resolution=0.2)
+
+frame_1 = 675
+color_1 = [1.0, 0.0, 0.0]
+frame_2 = 700
+color_2 = [0.0, 0.0, 1.0]
+
+# paths to all rgb and depth images in the dataset
+rgb_paths = sorted(
+    glob.glob(os.path.join(dfolder, dname + '-color/*.png')))
+depth_paths = sorted(
+    glob.glob(os.path.join(dfolder, dname + '-depth/*.png')))
+
+# read the full camera trajectory
+traj = read_log_trajectory(os.path.join(dfolder, dname + '-traj.log'))
+
+K_d = np.eye(3)
+K_d[0, 0] = 525.0/deci
+K_d[1, 1] = 525.0/deci
+K_d[0, 2] = 319.5/deci
+K_d[1, 2] = 239.5/deci
+W_d = (int)(640/deci)
+H_d = (int)(480/deci)
+iu_d = ImageUtils(K_d, im_h=H_d, im_w=W_d)
+
+# first frame
+pose_1 = traj[frame_1].pose
+translation_1 = pose_1[:3, 3]
+rotation_1 = pose_1[:3, :3].flatten()
+pcld_1, im_1 = iu_d.generate_pcld_wf(pose_1, rgb_path=rgb_paths[frame_1],
+                                     depth_path=depth_paths[frame_1], size=(W_d, H_d))
+cprint.info(f"num points pcld_1: {pcld_1.shape[0]}")
+
+lsogmm_gpu = GPUContainerf4()
+learner.fit(extract_ms_data(pcld_1), pcld_1, lsogmm_gpu)
+cprint.info(f"num components lsogmm_gpu: {lsogmm_gpu.n_components_}")
+
+# second frame
+pose_2 = traj[frame_2].pose
+translation_2 = pose_2[:3, 3]
+pcld_2, im_2 = iu_d.generate_pcld_wf(pose_2, rgb_path=rgb_paths[frame_2],
+                                     depth_path=depth_paths[frame_2], size=(W_d, H_d))
+cprint.info(f"num points pcld_2: {pcld_2.shape[0]}")
+
+ts = time.time()
+scores_4d = inference.score_4d(pcld_2, lsogmm_gpu)
+te = time.time()
+cprint.info(f"4D time {te - ts} seconds")
+scores_4d = scores_4d.flatten()
+novel_4d_case = pcld_2[scores_4d < -3.14, :]
+
+ts = time.time()
+scores_3d = inference.score_3d(pcld_2[:, :3], lsogmm_gpu)
+te = time.time()
+cprint.info(f"3D time {te - ts} seconds")
+scores_3d = scores_3d.flatten()
+novel_3d_case = pcld_2[scores_3d < -3.14, :]
+
+lsogmm_cpu = CPUContainerf4(lsogmm_gpu.n_components_)
+lsogmm_gpu.to_host(lsogmm_cpu)
+gsh.add_points(lsogmm_cpu.means_, np.arange(0, lsogmm_cpu.n_components_, dtype=int))
+
+ts = time.time()
+fov_comp_indices = gsh.find_points(pcld_2)
+submap_cpu = lsogmm_cpu.submap_from_indices(fov_comp_indices)
+submap_gpu = GPUContainerf4(submap_cpu.n_components_)
+submap_gpu.from_host(submap_cpu)
+scores_3d_fov = inference.score_3d(pcld_2[:, :3], submap_gpu)
+te = time.time()
+cprint.info(f"3D FoV time {te - ts} seconds")
+cprint.info(f"num components submap_gpu: {submap_gpu.n_components_}")
+scores_3d_fov = scores_3d_fov.flatten()
+novel_3d_fov_case = pcld_2[scores_3d_fov < -3.14, :]
+
+vis0 = VisOpen3D(visible=True, window_name='a')
+vis0.add_geometries([np_to_o3d(pcld_1),
+                    np_to_o3d(pcld_2)])
+
+vis0.add_geometries([np_to_o3d(pcld_1[:, :3], color=color_1),
+                    np_to_o3d(pcld_2[:, :3], color=color_2)])
+vis0.frustrum(pose_1, K_d, W_d, H_d, scale=0.4, color=color_1)
+vis0.frustrum(pose_2, K_d, W_d, H_d, scale=0.4, color=color_2)
+
+vis1 = VisOpen3D(visible=True, window_name='b')
+vis1.add_geometries([np_to_o3d(pcld_1),
+                    np_to_o3d(novel_4d_case[:, :3], color=color_2)])
+vis1.update_view_point(extrinsic=np.linalg.inv(pose_2))
+vis1.poll_events()
+vis1.update_renderer()
+
+vis2 = VisOpen3D(visible=True, window_name='c')
+vis2.add_geometries([np_to_o3d(pcld_1),
+                    np_to_o3d(novel_3d_case[:, :3], color=color_2)])
+vis2.update_view_point(extrinsic=np.linalg.inv(pose_2))
+vis2.poll_events()
+vis2.update_renderer()
+
+vis3 = VisOpen3D(visible=True, window_name='d')
+vis3.add_geometries([np_to_o3d(pcld_1),
+                    np_to_o3d(novel_3d_fov_case[:, :3], color=color_2)])
+vis3.update_view_point(extrinsic=np.linalg.inv(pose_2))
+vis3.poll_events()
+vis3.update_renderer()
+
+vis0.run()
+vis1.run()
+vis2.run()
+vis3.run()

paper_figures/zr-likelihood.py

@@ -0,0 +1,27 @@
+import os
+import configargparse
+
+
+class ConfigParser(configargparse.ArgParser):
+    def __init__(self):
+        super().__init__(default_config_files=[
+            os.path.join(os.path.dirname(__file__),
+                         'livingroom1.yaml')
+        ], conflict_handler='resolve')
+
+        # for 4D point cloud dataset
+        self.add_argument('--path_datasets', type=str)
+        self.add_argument('--path_results', type=str)
+        self.add_argument('--dataset_name', type=str)
+        self.add_argument('--color_ext', type=str)
+        self.add_argument('--nframes', type=int)
+        self.add_argument('--bandwidth', type=float)
+        self.add_argument('--deci', type=float)
+        self.add_argument('--machine', type=str)
+        self.add_argument('--zfill', type=int)
+        self.add_argument('--l_thres', type=float)
+        self.add_argument('--bw_list', nargs='+', type=float)
+
+    def get_config(self):
+        config = self.parse_args()
+        return config

run_scripts/config_parser.py

@@ -0,0 +1,11 @@
+path_datasets: "/media/fractal/T7/datasets"
+path_results: "/media/fractal/T7/rss2023-resub/results"
+dataset_name: "copyroom"
+color_ext: "png"
+nframes: 5490
+bandwidth: 0.03
+deci: 5.0
+machine: "fractal"
+zfill: 6
+l_thres: -1.0
+bw_list: [0.05, 0.04, 0.03, 0.02]