import%20marimo%0A%0A__generated_with%20%3D%20%220.24.0%22%0Aapp%20%3D%20marimo.App(width%3D%22medium%22)%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20base64%0A%20%20%20%20import%20copy%0A%20%20%20%20import%20importlib%0A%20%20%20%20import%20sys%0A%20%20%20%20from%20pathlib%20import%20Path%0A%0A%20%20%20%20import%20marimo%20as%20mo%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt%0A%20%20%20%20import%20numpy%20as%20np%0A%0A%20%20%20%20notebook_dir%20%3D%20Path(__file__).resolve().parent%20if%20%22__file__%22%20in%20globals()%20else%20Path.cwd()%0A%20%20%20%20repo_root%20%3D%20notebook_dir.parents%5B1%5D%0A%20%20%20%20output_dir%20%3D%20repo_root%20%2F%20%22data%22%20%2F%20%22others%22%20%2F%20%22open3d%22%0A%20%20%20%20output_dir.mkdir(parents%3DTrue%2C%20exist_ok%3DTrue)%0A%0A%20%20%20%20original_sys_path%20%3D%20list(sys.path)%0A%20%20%20%20sys.path%20%3D%20%5B%0A%20%20%20%20%20%20%20%20entry%0A%20%20%20%20%20%20%20%20for%20entry%20in%20sys.path%0A%20%20%20%20%20%20%20%20if%20Path(entry%20or%20%22.%22).resolve()%20!%3D%20notebook_dir.resolve()%0A%20%20%20%20%5D%0A%20%20%20%20try%3A%0A%20%20%20%20%20%20%20%20o3d%20%3D%20importlib.import_module(%22open3d%22)%0A%20%20%20%20finally%3A%0A%20%20%20%20%20%20%20%20sys.path%20%3D%20original_sys_path%0A%20%20%20%20return%20base64%2C%20copy%2C%20mo%2C%20np%2C%20o3d%2C%20output_dir%2C%20plt%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%20Open3D%20%E3%81%AB%E3%82%88%E3%82%8B%E7%82%B9%E7%BE%A4%E3%83%AC%E3%82%B8%E3%82%B9%E3%83%88%E3%83%AC%E3%83%BC%E3%82%B7%E3%83%A7%E3%83%B3%0A%0A%20%20%20%20Open3D%20%E3%81%AE%20GUI%20%E3%81%AB%E4%BE%9D%E5%AD%98%E3%81%9B%E3%81%9A%E3%80%81%E5%90%88%E6%88%90%E7%82%B9%E7%BE%A4%E3%82%92%E4%BD%BF%E3%81%A3%E3%81%A6%20headless%20%E3%81%AB%E9%9D%99%E6%AD%A2%E7%94%BB%E3%82%92%E6%9B%B8%E3%81%8D%E5%87%BA%E3%81%99%E3%80%82%0A%20%20%20%20offscreen%20renderer%20%E3%81%8C%E4%BD%BF%E3%81%88%E3%81%AA%E3%81%84%E5%A0%B4%E5%90%88%E3%81%AF%E3%80%81%E7%90%86%E7%94%B1%E3%82%92%E8%A1%A8%E7%A4%BA%E3%81%97%E3%81%9F%E3%81%86%E3%81%88%E3%81%A7%20matplotlib%20%E3%81%AB%E5%88%87%E3%82%8A%E6%9B%BF%E3%81%88%E3%82%8B%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np%2C%20o3d)%3A%0A%20%20%20%20rng_points%20%3D%20np.random.default_rng(7)%0A%20%20%20%20theta%20%3D%20np.linspace(0.0%2C%202.0%20*%20np.pi%2C%20700)%0A%20%20%20%20spiral%20%3D%20np.column_stack(%5B%0A%20%20%20%20%20%20%20%200.6%20*%20np.cos(theta)%2C%0A%20%20%20%20%20%20%20%200.6%20*%20np.sin(theta)%2C%0A%20%20%20%20%20%20%20%20np.linspace(-0.8%2C%200.8%2C%20theta.size)%2C%0A%20%20%20%20%5D)%0A%20%20%20%20plane%20%3D%20rng_points.uniform(-0.8%2C%200.8%2C%20size%3D(900%2C%203))%0A%20%20%20%20plane%5B%3A%2C%202%5D%20%3D%200.15%20*%20plane%5B%3A%2C%200%5D%20-%200.1%20*%20plane%5B%3A%2C%201%5D%0A%20%20%20%20sphere%20%3D%20rng_points.normal(size%3D(700%2C%203))%0A%20%20%20%20sphere%20%3D%20sphere%20%2F%20np.linalg.norm(sphere%2C%20axis%3D1%2C%20keepdims%3DTrue)%20*%200.35%0A%20%20%20%20sphere%20%2B%3D%20np.array(%5B0.9%2C%200.2%2C%20-0.2%5D)%0A%0A%20%20%20%20source_points%20%3D%20np.vstack(%5Bspiral%2C%20plane%2C%20sphere%5D)%0A%20%20%20%20source_points%20%2B%3D%200.01%20*%20rng_points.normal(size%3Dsource_points.shape)%0A%0A%20%20%20%20ground_truth%20%3D%20np.eye(4)%0A%20%20%20%20ground_truth%5B%3A3%2C%20%3A3%5D%20%3D%20o3d.geometry.get_rotation_matrix_from_xyz((0.35%2C%20-0.2%2C%200.45))%0A%20%20%20%20ground_truth%5B%3A3%2C%203%5D%20%3D%20np.array(%5B0.35%2C%20-0.18%2C%200.12%5D)%0A%0A%20%20%20%20target_points%20%3D%20source_points%20%40%20ground_truth%5B%3A3%2C%20%3A3%5D.T%20%2B%20ground_truth%5B%3A3%2C%203%5D%0A%20%20%20%20target_points%20%2B%3D%200.008%20*%20rng_points.normal(size%3Dtarget_points.shape)%0A%0A%20%20%20%20source_cloud%20%3D%20o3d.geometry.PointCloud(o3d.utility.Vector3dVector(source_points))%0A%20%20%20%20target_cloud%20%3D%20o3d.geometry.PointCloud(o3d.utility.Vector3dVector(target_points))%0A%20%20%20%20return%20ground_truth%2C%20source_cloud%2C%20target_cloud%0A%0A%0A%40app.cell%0Adef%20_(o3d)%3A%0A%20%20%20%20def%20preprocess_point_cloud(point_cloud%2C%20voxel_size%3A%20float)%3A%0A%20%20%20%20%20%20%20%20point_cloud_down%20%3D%20point_cloud.voxel_down_sample(voxel_size)%0A%20%20%20%20%20%20%20%20point_cloud_down.estimate_normals(%0A%20%20%20%20%20%20%20%20%20%20%20%20o3d.geometry.KDTreeSearchParamHybrid(radius%3Dvoxel_size%20*%202.0%2C%20max_nn%3D30)%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20point_cloud_fpfh%20%3D%20o3d.pipelines.registration.compute_fpfh_feature(%0A%20%20%20%20%20%20%20%20%20%20%20%20point_cloud_down%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20o3d.geometry.KDTreeSearchParamHybrid(radius%3Dvoxel_size%20*%205.0%2C%20max_nn%3D100)%2C%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20return%20point_cloud_down%2C%20point_cloud_fpfh%0A%0A%20%20%20%20return%20(preprocess_point_cloud%2C)%0A%0A%0A%40app.cell%0Adef%20_(o3d%2C%20preprocess_point_cloud%2C%20source_cloud%2C%20target_cloud)%3A%0A%20%20%20%20voxel_size%20%3D%200.08%0A%20%20%20%20source_down%2C%20source_fpfh%20%3D%20preprocess_point_cloud(source_cloud%2C%20voxel_size)%0A%20%20%20%20target_down%2C%20target_fpfh%20%3D%20preprocess_point_cloud(target_cloud%2C%20voxel_size)%0A%20%20%20%20distance_threshold%20%3D%20voxel_size%20*%201.5%0A%20%20%20%20ransac_result%20%3D%20o3d.pipelines.registration.registration_ransac_based_on_feature_matching(%0A%20%20%20%20%20%20%20%20source_down%2C%0A%20%20%20%20%20%20%20%20target_down%2C%0A%20%20%20%20%20%20%20%20source_fpfh%2C%0A%20%20%20%20%20%20%20%20target_fpfh%2C%0A%20%20%20%20%20%20%20%20True%2C%0A%20%20%20%20%20%20%20%20distance_threshold%2C%0A%20%20%20%20%20%20%20%20o3d.pipelines.registration.TransformationEstimationPointToPoint(False)%2C%0A%20%20%20%20%20%20%20%203%2C%0A%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)%2C%0A%20%20%20%20%20%20%20%20%5D%2C%0A%20%20%20%20%20%20%20%20o3d.pipelines.registration.RANSACConvergenceCriteria(100000%2C%200.999)%2C%0A%20%20%20%20)%0A%20%20%20%20return%20ransac_result%2C%20voxel_size%0A%0A%0A%40app.cell%0Adef%20_(o3d%2C%20ransac_result%2C%20source_cloud%2C%20target_cloud%2C%20voxel_size)%3A%0A%20%20%20%20source_cloud.estimate_normals(%0A%20%20%20%20%20%20%20%20o3d.geometry.KDTreeSearchParamHybrid(radius%3Dvoxel_size%20*%202.0%2C%20max_nn%3D30)%0A%20%20%20%20)%0A%20%20%20%20target_cloud.estimate_normals(%0A%20%20%20%20%20%20%20%20o3d.geometry.KDTreeSearchParamHybrid(radius%3Dvoxel_size%20*%202.0%2C%20max_nn%3D30)%0A%20%20%20%20)%0A%20%20%20%20icp_result%20%3D%20o3d.pipelines.registration.registration_icp(%0A%20%20%20%20%20%20%20%20source_cloud%2C%0A%20%20%20%20%20%20%20%20target_cloud%2C%0A%20%20%20%20%20%20%20%20voxel_size%2C%0A%20%20%20%20%20%20%20%20ransac_result.transformation%2C%0A%20%20%20%20%20%20%20%20o3d.pipelines.registration.TransformationEstimationPointToPlane()%2C%0A%20%20%20%20)%0A%20%20%20%20return%20(icp_result%2C)%0A%0A%0A%40app.cell%0Adef%20_(base64%2C%20copy%2C%20mo%2C%20np%2C%20o3d%2C%20output_dir%2C%20plt)%3A%0A%20%20%20%20def%20render_point_clouds(title%3A%20str%2C%20filename%3A%20str%2C%20point_clouds%2C%20colors)%3A%0A%20%20%20%20%20%20%20%20colored_clouds%20%3D%20%5B%5D%0A%20%20%20%20%20%20%20%20for%20point_cloud%2C%20color%20in%20zip(point_clouds%2C%20colors%2C%20strict%3DTrue)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20painted%20%3D%20copy.deepcopy(point_cloud)%0A%20%20%20%20%20%20%20%20%20%20%20%20painted.paint_uniform_color(color)%0A%20%20%20%20%20%20%20%20%20%20%20%20colored_clouds.append(painted)%0A%0A%20%20%20%20%20%20%20%20image_path%20%3D%20output_dir%20%2F%20filename%0A%20%20%20%20%20%20%20%20backend_name%20%3D%20%22open3d-offscreen%22%0A%20%20%20%20%20%20%20%20try%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20renderer%20%3D%20o3d.visualization.rendering.OffscreenRenderer(960%2C%20720)%0A%20%20%20%20%20%20%20%20%20%20%20%20renderer.scene.set_background(%5B1.0%2C%201.0%2C%201.0%2C%201.0%5D)%0A%20%20%20%20%20%20%20%20%20%20%20%20material%20%3D%20o3d.visualization.rendering.MaterialRecord()%0A%20%20%20%20%20%20%20%20%20%20%20%20material.shader%20%3D%20%22defaultUnlit%22%0A%20%20%20%20%20%20%20%20%20%20%20%20material.point_size%20%3D%205.0%0A%20%20%20%20%20%20%20%20%20%20%20%20for%20geometry_index%2C%20point_cloud%20in%20enumerate(colored_clouds)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20renderer.scene.add_geometry(f%22pcd-%7Bgeometry_index%7D%22%2C%20point_cloud%2C%20material)%0A%20%20%20%20%20%20%20%20%20%20%20%20bbox%20%3D%20renderer.scene.bounding_box%0A%20%20%20%20%20%20%20%20%20%20%20%20center%20%3D%20bbox.get_center()%0A%20%20%20%20%20%20%20%20%20%20%20%20extent%20%3D%20max(float(np.linalg.norm(bbox.get_extent()))%2C%201.0)%0A%20%20%20%20%20%20%20%20%20%20%20%20eye%20%3D%20center%20%2B%20np.array(%5B1.3%2C%20-1.5%2C%201.0%5D)%20*%20extent%0A%20%20%20%20%20%20%20%20%20%20%20%20renderer.setup_camera(55.0%2C%20center%2C%20eye%2C%20%5B0.0%2C%200.0%2C%201.0%5D)%0A%20%20%20%20%20%20%20%20%20%20%20%20image%20%3D%20renderer.render_to_image()%0A%20%20%20%20%20%20%20%20%20%20%20%20o3d.io.write_image(str(image_path)%2C%20image)%0A%20%20%20%20%20%20%20%20%20%20%20%20del%20renderer%0A%20%20%20%20%20%20%20%20except%20(OSError%2C%20RuntimeError)%20as%20exc%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20backend_name%20%3D%20f%22matplotlib%20fallback%20(%7Bexc.__class__.__name__%7D%3A%20%7Bexc%7D)%22%0A%20%20%20%20%20%20%20%20%20%20%20%20figure%20%3D%20plt.figure(figsize%3D(8%2C%206))%0A%20%20%20%20%20%20%20%20%20%20%20%20axis%20%3D%20figure.add_subplot(111%2C%20projection%3D%223d%22)%0A%20%20%20%20%20%20%20%20%20%20%20%20for%20point_cloud%2C%20color%20in%20zip(colored_clouds%2C%20colors%2C%20strict%3DTrue)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20points%20%3D%20np.asarray(point_cloud.points)%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20axis.scatter(points%5B%3A%2C%200%5D%2C%20points%5B%3A%2C%201%5D%2C%20points%5B%3A%2C%202%5D%2C%20s%3D1.5%2C%20c%3D%5Bcolor%5D%2C%20alpha%3D0.8)%0A%20%20%20%20%20%20%20%20%20%20%20%20axis.set_title(title)%0A%20%20%20%20%20%20%20%20%20%20%20%20axis.set_xlabel(%22x%22)%0A%20%20%20%20%20%20%20%20%20%20%20%20axis.set_ylabel(%22y%22)%0A%20%20%20%20%20%20%20%20%20%20%20%20axis.set_zlabel(%22z%22)%0A%20%20%20%20%20%20%20%20%20%20%20%20figure.tight_layout()%0A%20%20%20%20%20%20%20%20%20%20%20%20figure.savefig(image_path%2C%20dpi%3D200%2C%20bbox_inches%3D%22tight%22)%0A%20%20%20%20%20%20%20%20%20%20%20%20plt.close(figure)%0A%20%20%20%20%20%20%20%20encoded%20%3D%20base64.b64encode(image_path.read_bytes()).decode(%22ascii%22)%0A%20%20%20%20%20%20%20%20return%20mo.md(%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%22%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%23%23%23%20%7Btitle%7D%0A%20%20%20%20%20%20%20%20%20%20%20%20-%20renderer%3A%20%60%7Bbackend_name%7D%60%0A%20%20%20%20%20%20%20%20%20%20%20%20%3Cimg%20src%3D%22data%3Aimage%2Fpng%3Bbase64%2C%7Bencoded%7D%22%20alt%3D%22%7Btitle%7D%22%20%2F%3E%0A%20%20%20%20%20%20%20%20%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20)%0A%0A%20%20%20%20return%20(render_point_clouds%2C)%0A%0A%0A%40app.cell%0Adef%20_(render_point_clouds%2C%20source_cloud)%3A%0A%20%20%20%20render_point_clouds(%22%E3%82%BD%E3%83%BC%E3%82%B9%E7%82%B9%E7%BE%A4%22%2C%20%22source_cloud.png%22%2C%20%5Bsource_cloud%5D%2C%20%5B%5B0.95%2C%200.55%2C%200.15%5D%5D)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(render_point_clouds%2C%20source_cloud%2C%20target_cloud)%3A%0A%20%20%20%20render_point_clouds(%0A%20%20%20%20%20%20%20%20%22%E5%88%9D%E6%9C%9F%E4%BD%8D%E7%BD%AE%E3%81%AE%202%20%E7%82%B9%E7%BE%A4%22%2C%0A%20%20%20%20%20%20%20%20%22initial_alignment.png%22%2C%0A%20%20%20%20%20%20%20%20%5Bsource_cloud%2C%20target_cloud%5D%2C%0A%20%20%20%20%20%20%20%20%5B%5B0.95%2C%200.55%2C%200.15%5D%2C%20%5B0.15%2C%200.55%2C%200.95%5D%5D%2C%0A%20%20%20%20)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(copy%2C%20icp_result%2C%20render_point_clouds%2C%20source_cloud%2C%20target_cloud)%3A%0A%20%20%20%20aligned_source%20%3D%20copy.deepcopy(source_cloud)%0A%20%20%20%20aligned_source.transform(icp_result.transformation)%0A%20%20%20%20render_point_clouds(%0A%20%20%20%20%20%20%20%20%22%E3%83%AC%E3%82%B8%E3%82%B9%E3%83%88%E3%83%AC%E3%83%BC%E3%82%B7%E3%83%A7%E3%83%B3%E5%BE%8C%22%2C%0A%20%20%20%20%20%20%20%20%22registered_alignment.png%22%2C%0A%20%20%20%20%20%20%20%20%5Baligned_source%2C%20target_cloud%5D%2C%0A%20%20%20%20%20%20%20%20%5B%5B0.95%2C%200.55%2C%200.15%5D%2C%20%5B0.15%2C%200.55%2C%200.95%5D%5D%2C%0A%20%20%20%20)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(ground_truth%2C%20icp_result%2C%20np)%3A%0A%20%20%20%20print(%22Ground%20truth%20transformation%22)%0A%20%20%20%20print(np.array2string(ground_truth%2C%20precision%3D4))%0A%20%20%20%20print(%22Estimated%20transformation%22)%0A%20%20%20%20print(np.array2string(icp_result.transformation%2C%20precision%3D4))%0A%20%20%20%20print(f%22fitness%3D%7Bicp_result.fitness%3A.4f%7D%2C%20inlier_rmse%3D%7Bicp_result.inlier_rmse%3A.4f%7D%22)%0A%20%20%20%20return%0A%0A%0Aif%20__name__%20%3D%3D%20%22__main__%22%3A%0A%20%20%20%20app.run()%0A
a36ff59f500cc7a65c0a63d36f609658