isl-org / isl-org/Open3D

After using the pose graph to optimize the pose, the effect becomes worse

Open
#6,046 0 comments 0 reactions 0 assignees View on GitHub
question
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.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.