Update Simitary3 with Align feature

[Alexma3312]

This PR adds four functions into the Similarity3 class.
The code format is based on Google format and GTSAM::Pose3::Align().

1. transformFrom - Similarity transform of a Pose3 object
2. Align(point pairs) - Calculate Similarity transform from matched point pairs
3. rotationAveraging - Calculate average rotation from a list of rotations
4. Align(pose pairs) - Calculate Similarity transform from matched pose pairs

Visualization of unit tests for the Align functions:

1. Calculate Similarity transform from matched point pairs
   Input:
   r_point1 = Point3(0, 0, 0) g_point1 = Point3(6, 8, 10)
   r_point2 = Point3(3, 0, 0) g_point2 = Point3(6, 2, 10)
   r_point3 = Point3(3, 0, 4) g_point3 = Point3(6, 2, 18)
   

Output:
gRr = Rot3.Rz(math.radians(-90))
s = 2
gtr = Point3(6, 8, 10)

2. Calculate Similarity transform from matched pose pairs
   r_pose1 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(0, 0, 0))
   r_pose2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0))
   g_pose1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10))
   g_pose2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10))
   

Output:
gRr = Rot3.Ry(math.radians(180))
s = 2
gtr = Point3(4, 6, 10)

[varunagrawal]

This is awesome. I've been meaning to update the Sim(3) code for a while now so this makes life really good. Thanks @Alexma3312.

Please look at my comments and address them?

[dellaert]

Awesome! But I have a lot of advice :-)

[dellaert]

@Alexma3312 please don't push to repo until you resolved all comments in my review. Every push triggers a long CI process.

[dellaert]

Please merge in develop before pushing, as well. We changed CI system.

[dellaert]

Becoming beautiful! I'd just like to see one more big change, in addition to a few minor comments

[dellaert]

Confused. If my comment does not make sense maybe we should meet. But try to understand what I am suggesting first :-)

[dellaert]

Woohoo, so much clearer and faster. One more round which is mostly style but also some remaining performance improvements. Esp unpacking pairs is not needed in most cases, which will yield speed improvement and clarity (in case of centroids).

[dellaert]

Brilliant! This is high-quality code now!! super-factored :-)

There is one issue to be resolved before we can merge: the 2 checks with quaternion=ON are failing. Please compile with quaternions to see whether you can diagnose the issue?

[Alexma3312]

Brilliant! This is high-quality code now!! super-factored :-)

There is one issue to be resolved before we can merge: the 2 checks with quaternion=ON are failing. Please compile with quaternions to see whether you can diagnose the issue?

I found that the issue occurs on the Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1).

Rot3 R = Rot3::ClosestTo(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1).matrix());
R.print();

Output:

R: [
        1, 0, 0;
        0, 1, 0;
        0, 0, 1
]

Moreover, I created a new test for the compose function in testRot3.cpp:
Expected_1 passes the test when the quaternions flag is both on and off.
Expected_2 only passes the test when the quaternions flag is off.

TEST(Rot3, compose_2) {
  Rot3 R = Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1);
  // Create source poses
  Rot3 R1 = Rot3();
  Rot3 R2 = Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1);
  // Create destination poses
  Rot3 expected_1 = Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1);
  Rot3 expected_2 = Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1);

  EXPECT(assert_equal(expected_1, R.compose(R1)));
  EXPECT(assert_equal(expected_2, R.compose(R2)));
  EXPECT(assert_equal(expected_2, R * R2));
}

The error of expected_2 when the quaternions flag is on is:
The error is weird because the expected output (1,0,0,0,1,0,0,0,1) is different from what I declared (1, 0, 0, 0, 1, 0, 0, 0, -1).

Not equal:
expected:
 [
        1, 0, 0;
        0, 1, 0;
        0, 0, 1
]
actual:
 [
        -2.22045e-16, 0, 0;
        0, 1, 0;
        0, 0, -2.22045e-16
]
testRot3.cpp:404: Failure: "assert_equal(expected_2, R.compose(R2))"

The error still occurs and remains the same when I switch from

R.compose(R2)

to

R*R2

[dellaert]

That matrix is a reflection, not a rotation, I think. I have no idea where you copied this line from, though. Is that in a test, or where??

[Alexma3312]

That matrix is a reflection, not a rotation, I think. I have no idea where you copied this line from, though. Is that in a test, or where??

That line is not in a test. I can create a test in testRot3, but yes, you are right the matrix is a reflection. And that causes the quaternions issue, I will fix the unittests for Similarity3.

Checked response

[dellaert]

Congrats @Alexma3312 , and many thanks for your patience and diligence. I know I was demanding during the review :-)

[dellaert]

PS @Alexma3312 if you want to write a blog post about this (you have examples in the test) then I’ll post it on gtsam.org and tweet about it :-)

[Alexma3312]

PS @Alexma3312 if you want to write a blog post about this (you have examples in the test) then I’ll post it on gtsam.org and tweet about it :-)

Thank you @dellaert for spending your time helping me to improve my coding style. I will be happy to write a blog post about this.

[jlblancoc]

Throw an exception if n==0?
PS: Sorry for the late comment... :-S

[Alexma3312]

@jlblancoc Thanks for the catch. I will create a new PR to fix it.