ERROR: function icp

[json87]

hi, the function icp has one error as following:

the point Eigen::MatrixXd dst should be the same as the input point B except for their eigen structure. Thus, the corrected version is as follows:

ICP_OUT icp(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
int max_iterations, double tolerance) {
const int row = A.rows();
Eigen::MatrixXd src = Eigen::MatrixXd::Ones(3 + 1, row);
Eigen::MatrixXd src3d = Eigen::MatrixXd::Ones(3, row);
Eigen::MatrixXd dst_chorder = Eigen::MatrixXd::Ones(3, row);

const int rowB = B.rows();
Eigen::MatrixXd dst = Eigen::MatrixXd::Ones(3 + 1, rowB);

NEIGHBOR neighbor;
Eigen::Matrix4d T;
ICP_OUT result;
int iter = 0;

for (int i = 0; i < row; ++i) {
	src.block<3, 1>(0, i) = A.block<1, 3>(i, 0).transpose();
	src3d.block<3, 1>(0, i) = A.block<1, 3>(i, 0).transpose();
}

for (int i = 0; i < rowB; ++i) {
	dst.block<3, 1>(0, i) = B.block<1, 3>(i, 0).transpose();
}
   .....................

}