There is a logic error in the code of the computeTransformation function in the gicp.hpp file when calculating the transformation matrix T

[mawenwuda]

`final_transformation_ = guess; //New code

while (!converged_) {
for (std::size_t i = 0; i < 4; i++)
for (std::size_t j = 0; j < 4; j++) //modified
transform_R(i, j) = static_cast(final_transformation_(i, j));

query.getVector4fMap() =        
final_transformation_.template cast<float>() * query.getVector4fMap();    //**modified**

final_transformation_ = transformation_ * final_transformation_;    //**modified**
nr_iterations_++;    //**Add the above modified statement before this statement**

}
//Computation of the final transformation matrix is not possible, because the previous_transformation_ variable does not implement the cumulative transformation matrix.
final_transformation_ = previous_transformation_ * guess; //delete`

[Markus Vieth]

@mawenwuda Sorry, I am having difficulties interpreting your modifications. Could you explain, what exactly does not work currently? If you could provide an example where you call GICP, and show how the result does not match your expectations, that would be very helpful.

[mawenwuda]

@mvieth While reading the source code of the computeTransformation function in the gicp.hpp file, I encountered some issues that I couldn't understand, as follows:
1.The defined matrix M is a local variable and is not used later. Why is it defined then?
if (nn_dists[0] < dist_threshold) { Eigen::Matrix3d& C1 = (*input_covariances_)[i]; Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]]; Eigen::Matrix3d& M = mahalanobis_[i]; M = R * C1; Eigen::Matrix3d temp = M * R.transpose(); temp += C2; M = temp.inverse(); source_indices[cnt] = static_cast<int>(i); target_indices[cnt] = nn_indices[0]; cnt++; }
2.Is the transformation_ obtained from the function the current transformation matrix?If so, it will lead to subsequent issues.
rigid_transformation_estimation_( output, source_indices, *target_, target_indices, transformation_);
3.When the while loop starts for the second time, the variable transformation_ only represents the current transformation matrix, not the accumulated transformation matrix after multiple iterations. The following two lines of code cannot achieve their intended functionality
transform_R(i, j) += static_cast<double>(transformation_(i, k)) * static_cast<double>(guess(k, j));
query.getVector4fMap() = transformation_.template cast<float>() * query.getVector4fMap();
4.The purpose of this line of code is obviously to obtain the final transformation matrix. However, the variable previous_transformation_ does not seem to accumulate after multiple iterations, and it cannot achieve the intended purpose.
final_transformation_ = previous_transformation_ * guess;

[Markus Vieth]

While reading the source code of the computeTransformation function in the gicp.hpp file, I encountered some issues that I couldn't understand

@mawenwuda But have you tried running GICP? I have successfully used GICP several times, and as far as I can tell, the transformation it returned was always correct.

1. M is a reference, so by assigning to it, we actually assign to mahalanobis_[i]
2. transformation_ is the total transformation matrix from all iterations (excluding the guess I think). rigid_transformation_estimation_ updates the matrix
3. transformation_ is the accumulated transformation, why do you think it isn't?
4. I would suggest that you try running GICP yourself, and at the end, use gicp.getFinalTransformation() to get final_transformation_ and inspect its value

As I said in my last comment, if you have a test case where you run GICP and the behaviour is different from what you expected (so you observe a bug in practice), I am happy to investigate, but so far it seems to me that you simply misunderstand the code and assume it is a logic error.

[mawenwuda]

@mvieth I think I misunderstood this piece of code, thank you very much for your explanation!