Add ICP support for 3d point clouds (#465)

* Add 3d support ICP

* 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)

* Separate test for 3d points to main_3d_points

* Add test for ICP 3d

* Correct style

* Add space

* Style correction

* Add more spaces

* Add 3d visualizing for ICP

* Style corrections

* Delete spaces

* Style correction

* remove space

* Separate plot drawing

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

* Style correction

* Comment 3d plot drawing

Co-authored-by: Shamil GEMUEV <https://github.maf-roda.com/>

tests/test_iterative_closest_point.py

@@ -7,5 +7,10 @@ def test_1():
     m.main()
 
 
+def test_2():
+    m.show_animation = False
+    m.main_3d_points()
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)