fixed gicp guess handling

[prclibo]

Denote final transformation result Y=X_guess. Gicp should either 1) optimize X from identity by comparing guess_source and target or 2) optimize Y starting from X by comparing source and target. Current GICP code messes these two approaches. In computeTransformation, it uses base_transformation_ = guess as the start of the optimization but use output = guess * source as the source cloud.

This PR fixed it to approach 1).

[VictorLamoine]

Does it fix #754 ?

[prclibo]

Yes it does

[VictorLamoine]

Then would you be so kind as to integrate the test unit as well?
It is located here.

[prclibo]

I took a look into the commit and it looks weird. The bunny test cloud is just 0.1m size. Why is the test giving a (20, -8, 1) to challenge it?

[taketwo]

@VictorLamoine any comments? I think you are the original author of that line.

[VictorLamoine]

I used a very big distance to make sure that one iteration could not get close to a perfect alignment.
Without using the guess transformation it is impossible that 1 iteration of GICP gets close to a good result.
With the guess transformation it is possible (especially in that case where the guess matrix is perfect)

[taketwo]

I tried this PR against that test case and it failed:

77: /media/Workspace/Libraries/pcl/test/registration/test_registration.cpp:536: Failure
77: Expected: (reg.getFitnessScore ()) < (0.0001), actual: 0.000809606 vs 0.0001

Looking inside the code, I found it rather strange how the final transformation matrix is computed:

final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3);
final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3);
final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3);
final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3);

Though if replaced with matrix multiplication, the final fitness score only increases.

[prclibo]

I did some experiments but still havn't get final conclusion yet. One un-confirmed observation is:

• Ignoring the guess, if I transform src to transform_src and tgt to transform_tgt by the same transform. The registration will fail though it can do between src and tgt. This case happens when transform has a relatively large translation, i.e. both clouds are moved far away from the origin.

I don't know why this happen. But it may be the reason for this test case to fail, which is not related to the input guess.

Also, I noticed that the second registration should not re-use the first gicp instance, as some member variables seem not re-initialized.

@taketwo I agree that the final_transformation_ computation is wrong.

[prclibo]

I got the testcase passed when by setting transform.translation () = Eigen::Vector3f (0.1, 0.2, 0.3);, which is already a large shift compared to the model size. Larger translation like (20, -8, 1) leads to test failure, probably due to numerial failure, I guess.

[VictorLamoine]

I think this deserves a little bit of cleaning but otherwise the problem is solved right?

[prclibo]

@VictorLamoine I think so

[prclibo]

#1147
This might be related to the following observation

I got the testcase passed when by setting transform.translation () = Eigen::Vector3f (0.1, 0.2, 0.3);, which is already a large shift compared to the model size. Larger translation like (20, -8, 1) leads to test failure, probably due to numerial failure, I guess.

[SergioRAgostinho]

Any particular reason for keeping these commented? Why not simply remove them.

[prclibo]

for some reason the static equivalent methode raises an error

I am not sure what this means. Maybe a Eigen template problem at some versions, but anyway I am removing these comments.

[SergioRAgostinho]

Note: incorporated changes from #972

[SergioRAgostinho]

@prclibo Can you clean up the commit so that we can incorporate it in 1.8.1? Thank you

[SergioRAgostinho]

Also, please squash your last 4 commits into a single one, basically only the commits in which you're the corresponding author.

[prclibo]

@SergioRAgostinho OK I am newbie to rebase and I think I messed it up=( Any suggestions?
What I did:

git rebase -i 0d66be09a91bb1a6f43fde111c6c0c1924b9356e 07cfa84b1b9261e5b17e827b96e12e70961cdfbd
// Reordered my commits to the bottom.
git commit --amend --author="prclibo<prclibo@gmail.com>"
git checkout tmp
git branch -d master
git checkout -b master 
git push origin +master

[SergioRAgostinho]

Whoops! This never happened to me before. Here's some options based on Google's first results.

[SergioRAgostinho]

Another idea:

• Add pcl official repo as a remote
• Pull the current HEAD it to a new branch named clone
• Cherry pick the original commits in this PR. The two original ones 5006575 and 1d8c7f9 were from Added improved test for guess matrix in GICP  #972 and then there were 4 or 5 which I think you squashed into a357ea8.
• Force push this into your pcl master on github.

[VictorLamoine]

Backup your work first (create an other branch) 😉

[prclibo]

@SergioRAgostinho @VictorLamoine thanks for the help! It seems fixed and I hope I didn't miss anything.