zjudmd1015 / icp Goto Github PK

@zjudmd1015
I'm confused about the matrix H computed here.
H * A = B; --> H = A' * B, any reference?
Here A, B are corresponding point set, A' means A transpose, H is transform matrix;

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();
}
   .....................

}

The different point sizes of each frame can use its code?