In python 3.11.2, installed using pip install simpleicp. Simply importing the module results in the following error:

`

import simpleicp
Traceback (most recent call last):
File "", line 1, in
File "/Users/iremaltan/.pyenv/versions/3.11.2/lib/python3.11/site-packages/simpleicp/init.py", line 6, in
from .simpleicp import SimpleICP
File "/Users/iremaltan/.pyenv/versions/3.11.2/lib/python3.11/site-packages/simpleicp/simpleicp.py", line 17, in
from . import corrpts, mathutils, optimization, pointcloud
File "/Users/iremaltan/.pyenv/versions/3.11.2/lib/python3.11/site-packages/simpleicp/optimization.py", line 329, in
@DataClass
^^^^^^^^^
File "/Users/iremaltan/.pyenv/versions/3.11.2/lib/python3.11/dataclasses.py", line 1220, in dataclass
return wrap(cls)
^^^^^^^^^
File "/Users/iremaltan/.pyenv/versions/3.11.2/lib/python3.11/dataclasses.py", line 1210, in wrap
return _process_class(cls, init, repr, eq, order, unsafe_hash,
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/Users/iremaltan/.pyenv/versions/3.11.2/lib/python3.11/dataclasses.py", line 958, in _process_class
cls_fields.append(_get_field(cls, name, type, kw_only))
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/Users/iremaltan/.pyenv/versions/3.11.2/lib/python3.11/dataclasses.py", line 815, in _get_field
raise ValueError(f'mutable default {type(f.default)} for field '
ValueError: mutable default <class 'simpleicp.optimization.Parameter'> for field alpha1 is not allowed: use default_factory
`

The problem in the code seems to be in the RigidBodyParameters class definition, where alpha1 and others default to an instance of the Parameter class, which is a mutable variable.

Hi,

Is there anyway to extract which points correspond to eachother using this software?

Warnings substantially slow down binary.

I've noticed that in some places you used 1-based indices. I guess all of this should be 0-based?

I did some testing with the Python implementation and it seems that it fails with 2D point clouds. Is this expected? If so, it would be good if that was spelt out more clearly in the docs. If it was there I didn't see it. It might be a helpful feature to have.

A PointCloud must have three columns

In [35]: pc_zfix = PointCloud(x_f[:,0:2], columns=["x", "y"])
---------------------------------------------------------------------------
PointCloudException                       Traceback (most recent call last)
Cell In[35], line 1
----> 1 pc_zfix = PointCloud(x_f[:,0:2], columns=["x", "y"])

File ~/mambaforge/envs/icp/lib/python3.10/site-packages/simpleicp/pointcloud.py:42, in PointCloud.__init__(self, remapping, *args, **kwargs)
     40 for coordinate in ("x", "y", "z"):
     41     if coordinate not in self:
---> 42         raise PointCloudException(
     43             f'Column "{coordinate}" is missing in DataFrame.'
     44         )
     46 self._num_points = len(self)
     48 if "selected" not in self:

PointCloudException: Column "z" is missing in DataFrame.

Setting Z to zero causes errors when running ICP

I can just make a dummy Z column and set all the numbers in it to zero, but when I do that I see a lot of invalid value encountered in scalar divide messages and it not produce a meaningful answer. I tried this with both my own dataset and also the bunny dataset.

Hi, Im trying to use your code for point cloud of small point (about 10 -20 points per pointscloud ) in order to test that i use a .xyz containing 20 points of the the 20 vertices of a dodecahedron (fixed point cloud). i created a second files of this one where i moved each X coordinate by ading +10 (movable point cloud) .
Im feeding the algorithm with this two files and the algorithm give me back this matrix :
H =
[ 0.930487 0.366326 0.000000 -9.831626]
[ -0.366326 0.930487 0.000000 0.130998]
[ -0.000000 -0.000000 1.000000 0.000000]
[ 0.000000 0.000000 0.000000 1.000000]
i saw on the README file that to move a point to the position of the fixed, i have to do this operation to get the movable point tof fit the fixed point ;
Xt = H*X
im doing so :
X =[ 10.57735027; 0.57735027 ;0.57735027 ; 1] coordinate of first point in the movable point cloud
H =
[ 0.930487 0.366326 0.000000 -9.831626]
[ -0.366326 0.930487 0.000000 0.130998]
[ -0.000000 -0.000000 1.000000 0.000000]
[ 0.000000 0.000000 0.000000 1.000000]

but when i'm trying it, im not recovering the correct point :
Xt=X*A
Xt=[0.2220
-3.2065
0.5774
1.0000]

and if i understand well i should get the coordinate of this :
xt =0.57735027 0.57735027 0.57735027 1 wich is the coordinate of the first point in the fixed point cloud

so im asking myself if the algorithm is working properly . or im not using it correctly

here is the two points cloud and some screenshot .
dode1.xyz (fixed pointcloud) :
0.57735027 0.57735027 0.57735027
0.57735027 0.57735027 -0.57735027
0.57735027 -0.57735027 0.57735027
0.57735027 -0.57735027 -0.57735027
-0.57735027 0.57735027 0.57735027
-0.57735027 0.57735027 -0.57735027
-0.57735027 -0.57735027 0.57735027
-0.57735027 -0.57735027 -0.57735027
0.35682209 0.93417236 0.00000000
-0.35682209 0.93417236 0.00000000
0.35682209 -0.93417236 0.00000000
-0.35682209 -0.93417236 0.00000000
0.93417236 0.00000000 0.35682209
0.93417236 0.00000000 -0.35682209
-0.93417236 0.00000000 0.35682209
-0.93417236 0.00000000 -0.35682209
0.00000000 0.35682209 0.93417236
0.00000000 -0.35682209 0.93417236
0.00000000 0.35682209 -0.93417236
0.00000000 -0.35682209 -0.93417236

dode2.xyz (movable pointcloud)
10.57735027 0.57735027 0.57735027
10.57735027 0.57735027 -0.57735027
10.57735027 -0.57735027 0.57735027
10.57735027 -0.57735027 -0.57735027
9,42264973 0.57735027 0.57735027
9,42264973 0.57735027 -0.57735027
9,42264973 -0.57735027 0.57735027
9,42264973 -0.57735027 -0.57735027
10.35682209 0.93417236 0.00000000
9,64317791 0.93417236 0.00000000
10.35682209 -0.93417236 0.00000000
9,64317791 -0.93417236 0.00000000
10.93417236 0.00000000 0.35682209
10.93417236 0.00000000 -0.35682209
9,06582764 0.00000000 0.35682209
9,06582764 0.00000000 -0.35682209
10.00000000 0.35682209 0.93417236
10.00000000 -0.35682209 0.93417236
10.00000000 0.35682209 -0.93417236
10.00000000 -0.35682209 -0.93417236

here is the image of the result of the multiplication of the matrix (i did on Matlab online )

and here the algorithm :

if you have any clue about it would help me !

Currently it exists only for the python implementation.

Hi!
I am attempting to detect differences between multiple clouds. I am afraid that I am a little confused...does your [python] implementation generate any products that would allow me to calculate the RMSE between the final estimation given by the algo and my target cloud. What I am looking for are the inlier correspondences and I am unsure of how to generate them/find them from the outputs.

Apologies if it obvious, I would be very grateful if you could point it out to me, or if there is a process involved, give me a run down of what I need to do.

Cheers and thanks in advance,
J

Hello! Does this line miss the absolute value operation for distance calculation?
Thx!

I am feeding the algorithm 2 point clouds where all the points are contained in the same plane. FOr example these are a subset of both:

Cloud1:

79.9322  -29.1766         0
  79.0491  -29.9871         0
  77.9883  -30.5455         0
  76.8435   -30.901         0
  75.6688  -31.1422         0
   74.497  -31.3972         0
   73.326  -31.6563         0
  72.1549  -31.9149         0
  70.9831  -32.1699         0
  69.8104   -32.421         0
  68.6367  -32.6678         0
  67.4619  -32.9089         0
  66.2842  -33.1353         0
  65.1034  -33.3449         0

Cloud2:

 -53.1773    -46.1855           0
 -52.0631    -46.0045           0
 -50.9379    -45.8592           0
  -49.806    -45.7634           0
 -48.6745    -45.6633           0
 -47.5434    -45.5648           0
 -46.4184    -45.4074           0
 -45.2863      -45.32           0
 -44.1639    -45.1455           0
 -44.1639    -45.1455           0
 -42.9911    -45.0785           0
 -41.8164    -45.0597           0
 -40.6471    -44.9518           0
 -39.4825    -44.7987           0
  -38.313    -44.6947           0

The icp method for this data is returning the identity matrix, i.e.

Estimated transformation matrix H:
[    1.000000     0.000000     0.000000     0.000000]
[    0.000000     1.000000     0.000000     0.000000]
[    0.000000     0.000000     1.000000     0.000000]
[    0.000000     0.000000     0.000000     1.000000]

With iterations:

Iteration | correspondences | mean(residuals) |  std(residuals)
   orig:0 |            1582 |          0.0000 |          0.0000
        1 |            1582 |          0.0000 |          0.0000
        2 |            1582 |          0.0000 |          0.0000
        3 |            1582 |          0.0000 |          0.0000
        4 |            1582 |          0.0000 |          0.0000
        5 |            1582 |          0.0000 |          0.0000
        6 |            1582 |          0.0000 |          0.0000
        7 |            1582 |          0.0000 |          0.0000
        8 |            1582 |          0.0000 |          0.0000
        9 |            1582 |          0.0000 |          0.0000
       10 |            1582 |          0.0000 |          0.0000
       ```

I tried multiple different parameters and I always get the identity despite the point clouds being clearly different. I was wondering if you could help me identify the issue.

Thank you for your work in simpleICP, I have used this method to register 3D point cloud in the given DataSet and the result is good. However, when it is used to register the 2D point cloud, this method can't get the right result. I have extended the 2D point (x, y) to 3D point (x, y, z) with z=0, but it still can't calculate the true planarity, which cause all selected correspondences is rejected in the condition (P_i = (ev2-ev3)/ev1 and P_i < min_planarity).
I want to know what is the definition of planarity when the dimension decrease to 2D, and what is the rejection criteria of planarity.

It should be possible to directly observe the transformation parameters of the rigid body transformation.

Hi, I tried out SimpleICP and it seems to work very well for my problem. The only issue with SimpleICP for me is that it's very slow due to not being batched (as I need a distribution of ICP results). I was wondering if you knew any pytorch implementations of point to plane ICP? pytorch3d's implementation I believe is point to point and produces much worse fits than SimpleICP. If not, I am interested in porting this python implementation to pytorch to support batching.

Hi, thanks for offering this sort of convenient implementation. However, I randomly made some inputs, and often found that it reported the error of singular matrix. See below.

If would be great to have a possibility to choose of being verbose or not using python.

We should replace print by logger python functionality

In some cases the algorithm is crashing peforming ICP for two similar meshes. The same two point clouds (set of vertices) can converge to a result and sometimes can crash when the mesh have another set of world transforms.

Error: Python: Traceback (most recent call last):
  File "/Users/__USER__/model.blend/Text", line 163, in <module>
  File "/Applications/Blender.app/Contents/Resources/3.2/python/lib/python3.10/site-packages/simpleicp/simpleicp.py", line 211, in run
    distance_residuals_new = optim.estimate_parameters()
  File "/Applications/Blender.app/Contents/Resources/3.2/python/lib/python3.10/site-packages/simpleicp/optimization.py", line 120, in estimate_parameters
    weighted_distance_residuals = self._optim_results.residual[
TypeError: 'float' object is not subscriptable

Dear pglira,

Your work is amazingly concise and neat!

Could I also find out how do you display the point clouds after each iteration? (Do you use open3d to visualise the point clouds?)

Thank you for you time!

Regards,
rllyryan

Use PDAL to read/write point clouds. However, PDAL shouldn't become a dependency of simpleICP.

Hello again,
this may be
planarity_.fill(NAN);
?

Use pdal naming for point coordinates and attributes in PointCloud class.

Export after each iteration:

• movable point cloud
• correspondences

Hi, thanks for the implementation!

I hope to adopt your algorithm in a deep learning framework, which requires backpropagation in Pyotrch. Nevertheless, your algorithm is written in numpy. I suppose it does not accumulate the gradients and cannot be used in deep learning projects?

When building transformation matrix from 'alpha1', 'alpha2', 'alpha3', rotation part is not correct. 'alpha1', 'alpha2', 'alpha3' are Euler angles and can not be just inserted in matrix like this:

Rotation part of the matrix should be calculated the same way it is done in python version:

hello, i want to aggregate depth scans from a game camera in Unity.

Do you have an implementation in C# i could use or is there any way I could use your code? I reaaaally need to do this and I don't understand other tutorials out there.

Every 10 frames I would make a "picture" and get the depth from the camera, then I would turn that into a point cloud and use for example your implementation to make a point cloud of my scene.

I hope you can help me @pglira

Remove the timestamp from the output of all implementations.

I installed the SimpleICP package with pip (python), and tested the code presented in the documentation. And now I had this problem. Can you help me please ?

At least:

Select points for correspondences within overlap area of fixed point cloud ... --> Select points for correspondences in fixed point cloud ...
0 --> orig:0

I am attempting to use this ICP algorithm as part of my SLAM model but every H found states my robot is not moving (H = I).

Setup: I have 2D Lidar with a 180 degree FOV that publishes a 150 point pointcloud @ 10Hz. I would like to find some H between each Lidar Scan (or at least between some) to correct my dynamics model.

Attached are 2 lidar scans taken consecutively (I had to change them to txt for upload). I have noticed that after the reject() function there are no differences left so pehaps I just need to tune the parameters? I have tried messing with the parameters a bit (like setting min_planarity arbitrarily high) but have had no luck.

I appreciate the help!

test_point_cloud (copy).txt
test_point_cloud_minus_one (copy).txt