I've noticed that in some places you used 1-based indices. I guess all of this should be 0-based? https://github.com/pglira/simpleICP/blob/1478851961c01eab677bd325f93dff9b85d7a069/c%2B%2B/src/simpleicp.cpp#L164 https://github.com/pglira/simpleICP/blob/1478851961c01eab677bd325f93dff9b85d7a069/c%2B%2B/src/simpleicp.cpp#L181 https://github.com/pglira/simpleICP/blob/1478851961c01eab677bd325f93dff9b85d7a069/c%2B%2B/src/pointcloud.cpp#L82
I've noticed that in some places you used 1-based indices. I guess all of this should be 0-based?
simpleICP/c++/src/simpleicp.cpp
Line 164 in 1478851
simpleICP/c++/src/simpleicp.cpp
Line 181 in 1478851
simpleICP/c++/src/pointcloud.cpp
Line 82 in 1478851