кумулятивное преобразование матрицы с ICP из PCL

Моя цель — выровнять трехмерные облака точек с помощью ICP. Почему-то у меня есть ошибка, я думаю, что это из-за кумулятивных преобразований матриц.

Для отладки я начинаю с 2D-облаков точек, которые я создал. Для создания облаков точек я создаю случайный угол и подписываю их с помощью cos () и sin () для значений x и y, поэтому у меня есть случайные точки на окружности. Чем я использую перевод и вращение, которое увеличивается итеративно для каждого нового созданного изображения.

Я создаю около 20 облаков точек и сохраняю их в этих 512 * 512 изображениях. Затем я хочу загрузить изображения, создать из них облака точек и выровнять их по ICP.

Теперь для кумулятивного преобразования матрицы. Изображение в момент времени 0 будет иметь матрицу идентичности. Но любое другое изображение получило бы в качестве преобразования матрицу, собранную из ICP (M), умноженную на матрицу преобразования, из последней известной позиции:
Ми = М * Ми-1

Я не уверен, является ли это способом записи, или мне нужно преобразовать обратно в Identity перед применением полного преобразования.

Мои результаты за 10 облаков точек:

В первом мы видим собранные облака точек без ICP, а во втором с ICP. Я проверял это раньше только с переводами, которые работали действительно хорошо. И затем я проверил это только вращениями, и там у меня был путь к большим ошибкам. Возможно, что вращение слишком велико, поэтому ICP выравнивает точки неправильно, а затем находит неправильные совпадения.

Но если я протестирую реальные данные, изображения, собранные с камеры Xbox Kinect, то, похоже, будет та же ошибка, что и в моем примере с 2D-облаками точек.

Так что я неправильно рассчитал преобразование совокупной матрицы? или есть другая проблема, которую я не вижу?

И как мне правильно настроить свой ICP? Я только использую настройку:

icp.setTransformationEpsilon (1e-9);

И есть ли другой способ проверить это правильно?