After using the pose graph to optimize the pose, the effect becomes worse
- 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).
### My Question
I use open3d's pose graph library and found that the pose after using pose graph optimization becomes worse.
First, I somehow estimated the pose for N frames. I used the Open3d library to build a pose graph, then fixed the pose of the 0th frame, and updated the pose graph of the ground truth pose of the Nth frame. Finally, the pose graph optimization was performed and the results were found to be worse.
My code is as follows
```python
def create_pose_graph(poses,optimized_last_pose, additional_constraints=None):
pose_graph = o3d.pipelines.registration.PoseGraph()
num_poses = poses.shape[0]
for i in range(num_poses-1):
weight =(1- i / (num_poses + 1.0))*1000
information = np.identity(6) * weight
pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(poses[i]))
transformation = np.linalg.inv(poses[i])@poses[i + 1]
pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(i, i + 1, transformation, information=information, uncertain=False))
if additional_constraints is not None:
for constraint in additional_constraints:
i, j, transformation, information = constraint
pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(i, j, transformation, information, uncertain=False))
pose_graph.nodes[-1].pose = optimized_last_pose
fixed_edge_information = np.identity(6) * 1e6
fixed_edge_transformation = np.linalg.inv(poses[0])@optimized_last_pose
pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(0, num_poses - 1, fixed_edge_transformation, information=fixed_edge_information, uncertain=False))
return pose_graph
def optimize_pose_graph(pose_graph):
solver = o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt()
criteria = o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria()
criteria.max_iteration = 1000
option = o3d.pipelines.registration.GlobalOptimizationOption(
max_correspondence_distance=0.01, #
edge_prune_threshold=0.25,
reference_node=0)
o3d.pipelines.registration.global_optimization(pose_graph, solver, criteria, option)
pose_graph = create_pose_graph(poses,optimized_last_pose= gt_replica_poses[-1])
optimize_pose_graph(pose_graph)
```
Could you help me, please
Contributor guide
No contributing guide indexed for this repository
Assessment
This issue has not been assessed yet.