Hi everyone,

I've been banging my head on this problem for some time now, and was hoping for some insight. Warning : wall of text

So, I am trying to apply an ICP(Iterative Closest Point) algorithm on a n number of point clouds. I am currently doing quite simple pairwise matching between two point clouds.

For each pair, I am thus calculating closest point ( with the use of a kd-tree), which allows me to create two new point clouds that I feed into my ComputeRigidTransform function that uses the Eigen JacobiSVD, which returns a Rotation and Translation, that I apply to the point cloud I want to move., and I do this for my n number of point clouds etc.

The ComputeRigidTransform is the same as this : https://gist.github.com/JiaxiangZheng/8168862 (I hope posting GitHub pages is fine).

Here is where it gets funky... And if anything seems off, please point it out :

I get the Euler Angles using this : Eigen::Vector3d euler = R.eulerAngles(0, 1, 2); (Noteworthy info : this is in 2D space, there is no Rx, or Ry, only Rz).

I then add my Rz rotation (euler[2]) as well as the x and y translation to my point cloud.

The x,y translation seem fine, but the Rz is clearly off and here's why : in a straight line, it seems fine, but whenever there is a big rotation, it clearly does not rotate my point clouds enough, and I end up with almost a straight line when there should be 90 degree angles.

So, first of all : in what format does eulerAngles return the Rz rotation ? In degrees or in Radians ?

While doing some tweaking to try to understand what was going on, I noticed that if I rotate my point cloud with the Rz I get multiplied by 5, I start to get something quite good on my 'big' rotations, but obviously it affects my 'straight' lines and makes them less accurate. For some reason 5 is the magical number, where less or more isn't quite as good.

This leads me to believe that my rotation matrix/euler angle calculation is off, and I can't seem to pinpoint it out. If anybody has a clue, it is more than welcome!

Thanks.

EDIT : Ok, I'll try to describe what exactly the issue is : My rotation value that I am getting from my SVD is a lot lower than it should be it feels like.
Could it be that : When I don't have a match, I add two points to the point cloud with 0,0,0 values, and so on a like 2k points point cloud, I end up with around 100 points at 0,0,0 ? I tried bypassing that but it did not seem to make a difference.
Could it be something else?

eulerAngles returns radian, and AngleAxis takes radian as well.

Have you tried to directly use the rotation matrix returned by ComputeRigidTransform instead of extracting the rotation angle?