-
-
Notifications
You must be signed in to change notification settings - Fork 941
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We鈥檒l occasionally send you account related emails.
Already on GitHub? Sign in to your account
Vanilla ICP #2875
base: main
Are you sure you want to change the base?
Vanilla ICP #2875
Conversation
@edgarriba I have created an initial version. Can you please take a look at it ? |
|
||
|
||
def iterative_closest_point( | ||
points_in_a: torch.Tensor, points_in_b: torch.Tensor, max_iterations: int = 20, tolerance: float = 1e-4 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
points_in_a: torch.Tensor, points_in_b: torch.Tensor, max_iterations: int = 20, tolerance: float = 1e-4 | |
points_in_a: torch.Tensor, points_in_b: torch.Tensor, max_iterations: int = 20, tolerance: float = 1e-4, verbose: bool = False |
The relative transformation between the two pointcloud with shape 3x4 | ||
""" | ||
|
||
src = points_in_a.clone() |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
add some basic shape and type check
KORNIA_CHECK_SHAPE(points_in_a, ["B", "3"])
|
||
t = b_mean.T - R @ a_mean.T | ||
|
||
points_in_a = (points_in_a.T @ R + t.unsqueeze(-1)).T |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
points_in_a = (points_in_a.T @ R + t.unsqueeze(-1)).T | |
points_in_a = points_in_a @ R + t |
|
||
src = points_in_a.clone() | ||
dst = points_in_b.clone() | ||
prev_error = 0 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
prev_error = 0 | |
prev_error = 0 | |
R = torch.eye(3) | |
t = torch.zeros(3) |
|
||
# print(points_in_a) | ||
|
||
return 0 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
return 0 | |
return torch.cat([R, t[:, None]], dim=1) |
prev_error = mean_error | ||
print(mean_error, iter) | ||
|
||
# print(points_in_a) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
# print(points_in_a) |
if torch.abs(prev_error - mean_error) < tolerance: | ||
break | ||
prev_error = mean_error | ||
print(mean_error, iter) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
print(mean_error, iter) | |
if verbose: | |
print(f"ICP Mean error: {mean_error}, iter: {iter}") |
|
||
Args: | ||
points_in_a: The point cloud in the source coordinates frame A with shape Nx3 | ||
points_in_b: The point cloud in the source coordinates frame A with shape Nx |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
points_in_b: The point cloud in the source coordinates frame A with shape Nx | |
points_in_b: The point cloud in the source coordinates frame A with shape Nx3 |
@jeffin07 can you write a test to verify the functionality? |
Changes
Added Vanilla ICP
Fixes # (issue)
#2649
Type of change
Checklist