Pose graph successfully aligns 9 point clouds out of 10.
- Dominant language
- C++
- Stars
- 14k
- Forks
- 2.6k
- Avg merge
- 5d 18h
- Merged PRs (30d)
- 6
Description
### Checklist
- [X] I have searched for [similar issues](https://github.com/isl-org/Open3D/issues).
- [X] For Python issues, I have tested with the [latest development wheel](http://www.open3d.org/docs/latest/getting_started.html#development-version-pip).
- [X] I have checked the [release documentation](http://www.open3d.org/docs/release/) and the [latest documentation](http://www.open3d.org/docs/latest/) (for `master` branch).
### Describe the issue
I am working on a program which aligns point clouds in real time using a pose graph. It also solves the pose graph after every alignment, and does some other processing on the result.
I sometimes get a bug where I add a point cloud to the pose graph, and then that point cloud is misaligned when the pose graph is solved. I discovered that if I transform the point cloud first, then change the transformation on the pose graph edge to the identity matrix, the problem is fixed. I don't know if this will always work though.
Here's some data to reproduce the bug. It contains point clouds, pose graph data, and a python script to solve the pose graph and display the result.
[data.zip](https://github.com/isl-org/Open3D/files/9028446/data.zip)
Before I added my fix:

After:

### Steps to reproduce the bug
```python
1. Download the zip file above.
2. Run bug.py
bug.py
import numpy as np
import open3d
import os
import re
COLORS = [
(1, 0, 0),
(0, 1, 0),
(0, 0, 1),
(1, 1, 0),
(1, 0, 1),
(0, 1, 1),
(0.5, 0, 0),
(0, 0.5, 0),
(0, 0, 0.5),
(0.5, 0.5, 0),
(0.5, 0, 0.5),
(0, 0.5, 0.5),
(0.5, 0.5, 0.5),
]
def load_pose_graph_and_pcds():
pcds = []
pose_graph = open3d.pipelines.registration.PoseGraph()
pcd_pattern = re.compile(r'(\d+).ply')
edge_pattern = re.compile(r'(\d+)_to_(\d+).npz')
for file_name in os.listdir(os.path.dirname(__file__)):
match = re.fullmatch(pcd_pattern, file_name)
if match is not None:
index = int(match.group(1))
pcd = open3d.io.read_point_cloud(file_name)
pcd.paint_uniform_color(COLORS[len(pcds)])
pcds.append((index, pcd))
else:
match = re.fullmatch(edge_pattern, file_name)
if match is not None:
source_node_id, target_node_id = int(match.group(1)), int(match.group(2))
npz = np.load(file_name)
pose_graph.edges.append(
open3d.pipelines.registration.PoseGraphEdge(source_node_id, target_node_id, npz['transformation'],
npz['information'])
)
pcds = sorted(pcds, key=lambda t: t[0])
pcds = [pcd for index, pcd in pcds]
for _ in range(len(pcds)):
pose_graph.nodes.append(open3d.pipelines.registration.PoseGraphNode())
return pose_graph, pcds
def main():
# This script loads point clouds and a pose graph, solves the pose graph, and displays the result
pose_graph, pcds = load_pose_graph_and_pcds()
# ****************** This fixes it ************************
for i, edge in enumerate(pose_graph.edges):
if edge.source_node_id == 9:
pcds[9].transform(edge.transformation)
edge.transformation = np.identity(4)
# *********************************************************
option = open3d.pipelines.registration.GlobalOptimizationOption(max_correspondence_distance=0.1, reference_node=0)
open3d.pipelines.registration.global_optimization(
pose_graph,
open3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
open3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
option
)
for pcd, node in zip(pcds, pose_graph.nodes):
pcd.transform(node.pose)
open3d.visualization.draw_geometries(pcds)
if __name__ == '__main__':
main()
```
### Error message
_No response_
### Expected behavior
All the point clouds should be aligned.
### Open3D, Python and System information
```markdown
- Operating system: Windows 10 64-bit
- Python version: Python 3.8.9
- Open3D version: output from python: 0.15.1
- System architecture: x64
- Is this a remote workstation?: no
- How did you install Open3D?: pip
```
### Additional information
_No response_
Contributor guide
No contributing guide indexed for this repository
Assessment
This issue has not been assessed yet.