Add ICP support for 3d point clouds #465

NobleBumblebee · 2021-01-25T16:20:39Z

Reference issue

What does this implement/fix?

Adds an implementation of code for ICP to work both for 2d and 3d point clouds.

Additional information

Make functions icp_matching() and update_homogeneous_matrix() flexible. Depending on the current_points.shape[0] function svd_motion_estimation() automatically returns the correct size for Rt, Tt. Then, in update_homogeneous_matrix() depending on their size matrix H is formed to be 3x3 or 4x4.

Test for 3d point cloud is provided. Made the same manner as tests for 2d point cloud.

* icp_matching function returns R,T corresponding to 2D or 3D set of points * update_homogeneuous_matrix - general operations for translation and rotation matrixes

NobleBumblebee · 2021-01-25T16:49:17Z

Hello, I have a failed check caused by another module, I wasn't working on.

AtsushiSakai · 2021-01-26T11:45:37Z

Thank you for reporting. I will try to fix in #468. Please wait.

AtsushiSakai · 2021-01-26T12:17:56Z

Rerun CI.

NobleBumblebee · 2021-01-26T15:40:17Z

It's all good for the checks. Thank you for solving that issue.

AtsushiSakai

Thank you for great PR!!. I have a small request. PTAL.

AtsushiSakai · 2021-01-27T11:22:47Z

SLAM/iterative_closest_point/iterative_closest_point.py

@@ -152,6 +148,29 @@ def main():
        print("R:", R)
        print("T:", T)

+    # test for 3d point set


I think this test for 3d point test should be separated to main(), like main_3d_point() or something. And then iterative_closest_point.py should be updated.

NobleBumblebee · 2021-02-02T15:50:25Z

There is one thing that bothers me. Even if you pass 3d point cloud, the animation would be in 2d (for axis x,y). The code to visualize in 3d will be more complicated. Requiring to put if clause for 2d or 3d case, etc.
Is it ok, that we left 2d animation for both cases?

AtsushiSakai

Could you please add a new test here which call main_3d_points?

PythonRobotics/tests/test_iterative_closest_point.py

Line 8 in b3d39f5

AtsushiSakai · 2021-02-04T11:30:52Z

One idea is we can create a new plot function for 2d points to extract these codes:

PythonRobotics/SLAM/iterative_closest_point/iterative_closest_point.py

Lines 38 to 47 in b3d39f5

    
           plt.cla() 
        
           # for stopping simulation with the esc key. 
        
           plt.gcf().canvas.mpl_connect( 
        
               'key_release_event', 
        
               lambda event: [exit(0) if event.key == 'escape' else None]) 
        
           plt.plot(previous_points[0, :], previous_points[1, :], ".r") 
        
           plt.plot(current_points[0, :], current_points[1, :], ".b") 
        
           plt.plot(0.0, 0.0, "xr") 
        
           plt.axis("equal") 
        
           plt.pause(0.1)

And creating a new plot function for 3d points and dispatch to call which function using a if clause.

* Add space

* Add more spaces

* Delete spaces

NobleBumblebee · 2021-02-11T16:27:13Z

Can I ask you something? I am new to this style checking. How can I do these checks on my Spyder on Windows 10 before the commits? I always have some style issues.

NobleBumblebee · 2021-02-11T16:32:52Z

So for 3d plots I divided 2d case and 3d case by if-else operator. To my mind it is least dirty.
It is possible to put drawing code to separate functions, but I am afraid it will not look good among functions that implement functionality.
What do you think?

* remove space

lgtm-com · 2021-02-11T16:58:57Z

This pull request introduces 1 alert when merging 56b319c into b8f28cc - view on LGTM.com

new alerts:

1 for Unused import

NobleBumblebee · 2021-02-11T17:14:37Z

Unused import concerns from mpl_toolkits.mplot3d import Axes3D for 3D plotting. It is needed for 3d scatter drawing but it is never called explicitly.

AtsushiSakai · 2021-02-14T04:49:23Z

How can I do these checks on my Spyder on Windows 10 before the commits? I always have some style issues.

Unfortunately, there is not a way not. But I think the "runtests.sh" should do it locally.
I will try to add it in #481

AtsushiSakai

Hi. Thank you for your patience. Maybe this is the last review. After addressed my comments, I will merge this PR. PTAL.

AtsushiSakai · 2021-02-14T05:02:44Z

SLAM/iterative_closest_point/iterative_closest_point.py

+                fig = plt.figure()
+                ax = fig.add_subplot(111, projection='3d')


Current implementation create a new figure for every iteration. I think it is not good.
So, I propose the figure should be clean up after initialization(see my comment below):

Suggested change

fig = plt.figure()

ax = fig.add_subplot(111, projection='3d')

plt.cla()

SLAM/iterative_closest_point/iterative_closest_point.py

* plot drawing in a separate function for both 2D and 3D versions * figure creating before while loop

NobleBumblebee · 2021-02-19T16:23:03Z

I tried different options to make a 3d scatter be drawn on the same figure. None of them worked for me. PTAL.
Options I tried:

just plt.cla() before drawing scatter
no clearing at all
axes.clear()
All of the above with recreating of axes axes = figure.add_subplot(111, projection='3d')
Using figure.show() at the end
Using figure.canvas.draw() figure.canvas.flush_events()
None of if was working for 3d plot. I left some code commented. That may help you. So I left working solution with recreating figure for 3d.

lgtm-com · 2021-02-19T16:51:58Z

This pull request introduces 1 alert when merging 7aa502c into 58390e7 - view on LGTM.com

new alerts:

1 for Unused import

NobleBumblebee · 2021-02-19T16:55:42Z

We also need to do something with import of from mpl_toolkits.mplot3d import Axes3D as it will continue to produce alerts.

AtsushiSakai · 2021-02-20T12:03:06Z

SLAM/iterative_closest_point/iterative_closest_point.py

+    if previous_points.shape[0] == 3:
+        # axes = figure.axes[0]
+        # axes.clear()
+        figure = plt.figure()


Does this work without figure creation in your environment?

Suggested change

figure = plt.figure()

plt.clf()

For 3d case in Spyder I can see only the first plot with initial point position. And it is not updating.
(Need to mention that for 2d case every single frame of optimization is displayed as a separate image, and not an updated previous)
May be this is the case for Spyder. Does it work for you without creating figure for 3d?

Yeah. In my environment (just run the script from a terminal), it works. I think creating a lot of figures is annoying. Could you try to run the script from terminal?

AtsushiSakai · 2021-02-20T12:40:10Z

One more thing. I think you can run style checker locally with:

pip install pycodestyle
run ././rundiffstylecheck.sh
Please try it.

NobleBumblebee · 2021-02-22T14:35:09Z

bash rundiffstylecheck.sh didn't worked for me:

rundiffstylecheck.sh start!
--2021-02-22` 15:27:06-- https://github.com/AtsushiSakai/DiffSentinel/archive/v0.1.6%0D.zip%0D

Resolving github.com (github.com)... 140.82.121.4
Connecting to github.com (github.com)|140.82.121.4|:443... connected.
HTTP request sent, awaiting response... 404 Not Found
2021-02-22 15:27:06 ERROR 404: Not Found.

.ZIP.or v0.1.6 find or open v0.1.6
rundiffstylecheck.sh: line 6: ./DiffSentinel*/starter.sh: No such file or directory
rundiffstylecheck.sh: line 16: syntax error: unexpected end of file

Calling pycodestyle could be used directly for every single file instead. #481
pycodestyle --show-source --show-pep8 SLAM/iterative_closest_point/iterative_closest_point.py

NobleBumblebee · 2021-03-19T14:17:44Z

Hello @AtsushiSakai , still no answer from you.

There is one thing that bothers me. Even if you pass 3d point cloud, the animation would be in 2d (for axis x,y). The code to visualize in 3d will be more complicated. Requiring to put if clause for 2d or 3d case, etc.
Is it ok, that we left 2d animation for both cases?

As I said at the beginning 3d plots are complicated because of a lot of non evident moments.
I suggest to keep 2d plots for both cases.

AtsushiSakai · 2021-03-20T05:00:24Z

I'm sorry I missed your comments.

I suggest to keep 2d plots for both cases.

I think 3d plot is very useful for understanding what is happening in the script and other many examples are already using 3d plot.
If you don't mind, once this PR is merged, I will fix the 3d plot related code. Is it OK for you?

NobleBumblebee · 2021-04-01T15:05:34Z

I commented the part of 3d plots, so that now it works good with 2d plots. I leave the 3d part for you to modify.

AtsushiSakai

LGTM. Thank you for your patience!!

NobleBumblebee · 2021-04-02T13:04:24Z

Thank you too. Please keep me in touch with when you will add 3d plot drawing. I want to see a solution that works for you.

Shamil GEMUEV added 2 commits January 25, 2021 17:03

Add 3d support ICP

96fe31e

* icp_matching function returns R,T corresponding to 2D or 3D set of points * update_homogeneuous_matrix - general operations for translation and rotation matrixes

Add test for 3d point cloud (with 2d visualization)

2c59fb9

AtsushiSakai closed this Jan 26, 2021

AtsushiSakai reopened this Jan 26, 2021

AtsushiSakai requested changes Jan 27, 2021

View reviewed changes

Separate test for 3d points to main_3d_points

7aded2f

AtsushiSakai requested changes Feb 4, 2021

View reviewed changes

Shamil GEMUEV added 5 commits February 11, 2021 16:15

Add test for ICP 3d

3715a07

Correct style

5437994

* Add space

Style correction

6287b9a

* Add more spaces

Add 3d visualizing for ICP

74162e0

Style corrections

bf50a06

* Delete spaces

Style correction

56b319c

* remove space

AtsushiSakai requested changes Feb 14, 2021

View reviewed changes

Separate plot drawing

e3d23a2

* plot drawing in a separate function for both 2D and 3D versions * figure creating before while loop

Style correction

7aa502c

AtsushiSakai requested changes Feb 20, 2021

View reviewed changes

Comment 3d plot drawing

b72b6f5

NobleBumblebee requested a review from AtsushiSakai April 1, 2021 15:05

AtsushiSakai approved these changes Apr 2, 2021

View reviewed changes

AtsushiSakai merged commit bf2d9df into AtsushiSakai:master Apr 2, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add ICP support for 3d point clouds #465

Add ICP support for 3d point clouds #465

NobleBumblebee commented Jan 25, 2021

NobleBumblebee commented Jan 25, 2021

AtsushiSakai commented Jan 26, 2021

AtsushiSakai commented Jan 26, 2021

NobleBumblebee commented Jan 26, 2021

AtsushiSakai left a comment

AtsushiSakai Jan 27, 2021

NobleBumblebee commented Feb 2, 2021

AtsushiSakai left a comment

AtsushiSakai commented Feb 4, 2021

NobleBumblebee commented Feb 11, 2021

NobleBumblebee commented Feb 11, 2021

lgtm-com bot commented Feb 11, 2021

NobleBumblebee commented Feb 11, 2021

AtsushiSakai commented Feb 14, 2021

AtsushiSakai left a comment

AtsushiSakai Feb 14, 2021 •

edited

Loading

NobleBumblebee commented Feb 19, 2021

lgtm-com bot commented Feb 19, 2021

NobleBumblebee commented Feb 19, 2021

AtsushiSakai Feb 20, 2021 •

edited

Loading

NobleBumblebee Feb 22, 2021

AtsushiSakai Mar 20, 2021

AtsushiSakai commented Feb 20, 2021

NobleBumblebee commented Feb 22, 2021

NobleBumblebee commented Mar 19, 2021

AtsushiSakai commented Mar 20, 2021 •

edited

Loading

NobleBumblebee commented Apr 1, 2021

AtsushiSakai left a comment

NobleBumblebee commented Apr 2, 2021

		fig = plt.figure()
		ax = fig.add_subplot(111, projection='3d')

	fig = plt.figure()
	ax = fig.add_subplot(111, projection='3d')
	plt.cla()

Add ICP support for 3d point clouds #465

Add ICP support for 3d point clouds #465

Conversation

NobleBumblebee commented Jan 25, 2021

Reference issue

What does this implement/fix?

Additional information

NobleBumblebee commented Jan 25, 2021

AtsushiSakai commented Jan 26, 2021

AtsushiSakai commented Jan 26, 2021

NobleBumblebee commented Jan 26, 2021

AtsushiSakai left a comment

Choose a reason for hiding this comment

AtsushiSakai Jan 27, 2021

Choose a reason for hiding this comment

NobleBumblebee commented Feb 2, 2021

AtsushiSakai left a comment

Choose a reason for hiding this comment

AtsushiSakai commented Feb 4, 2021

NobleBumblebee commented Feb 11, 2021

NobleBumblebee commented Feb 11, 2021

lgtm-com bot commented Feb 11, 2021

NobleBumblebee commented Feb 11, 2021

AtsushiSakai commented Feb 14, 2021

AtsushiSakai left a comment

Choose a reason for hiding this comment

AtsushiSakai Feb 14, 2021 • edited Loading

Choose a reason for hiding this comment

NobleBumblebee commented Feb 19, 2021

lgtm-com bot commented Feb 19, 2021

NobleBumblebee commented Feb 19, 2021

AtsushiSakai Feb 20, 2021 • edited Loading

Choose a reason for hiding this comment

NobleBumblebee Feb 22, 2021

Choose a reason for hiding this comment

AtsushiSakai Mar 20, 2021

Choose a reason for hiding this comment

AtsushiSakai commented Feb 20, 2021

NobleBumblebee commented Feb 22, 2021

NobleBumblebee commented Mar 19, 2021

AtsushiSakai commented Mar 20, 2021 • edited Loading

NobleBumblebee commented Apr 1, 2021

AtsushiSakai left a comment

Choose a reason for hiding this comment

NobleBumblebee commented Apr 2, 2021

AtsushiSakai Feb 14, 2021 •

edited

Loading

AtsushiSakai Feb 20, 2021 •

edited

Loading

AtsushiSakai commented Mar 20, 2021 •

edited

Loading