Closed mawenwuda closed 4 months ago
@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.
@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 previoustransformation does not seem to accumulate after multiple iterations, and it cannot achieve the intended purpose.
final_transformation_ = previous_transformation_ * guess;
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.
M
is a reference, so by assigning to it, we actually assign to mahalanobis_[i]
transformation_
is the total transformation matrix from all iterations (excluding the guess
I think). rigid_transformation_estimation_
updates the matrixtransformation_
is the accumulated transformation, why do you think it isn't?gicp.getFinalTransformation()
to get final_transformation_
and inspect its valueAs 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.
@mvieth I think I misunderstood this piece of code, thank you very much for your explanation!
You are welcome
`finaltransformation = 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(finaltransformation(i, j));
} //Computation of the final transformation matrix is not possible, because the previoustransformation variable does not implement the cumulative transformation matrix. finaltransformation = previoustransformation * guess; //delete`