ICP_API float __stdcall ICP(Point3f *verts1, Point3f *verts2, int nVerts1, int nVerts2, float *R, float *t, int maxIter)
{
PointCloud cloud1;
cloud1.pts = vector<Point3f>(verts1, verts1 + nVerts1);
cv::Mat matR(3, 3, CV_32F, R);
cv::Mat matT(1, 3, CV_32F, t);
cv::Mat verts2Mat(nVerts2, 3, CV_32F, (float*)verts2);
float error = 1;
for (int iter = 0; iter < maxIter; iter++)
{
vector<Point3f> matched1, matched2;
vector<float> distances(nVerts2);
vector<size_t> indices(nVerts2);
FindClosestPointForEach(cloud1, verts2Mat, distances, indices);
vector<float> matchDistances;
vector<int> matchIdxs(nVerts1, -1);
for (int i = 0; i < nVerts2; i++)
{
int pos = matchIdxs[indices[i]];
if (pos != -1)
{
if (matchDistances[pos] < distances[i])
continue;
}
Point3f temp;
temp.X = verts2Mat.at<float>(i, 0);
temp.Y = verts2Mat.at<float>(i, 1);
temp.Z = verts2Mat.at<float>(i, 2);
if (pos == -1)
{
matched1.push_back(verts1[indices[i]]);
matched2.push_back(temp);
matchDistances.push_back(distances[i]);
matchIdxs[indices[i]] = matched1.size() - 1;
}
else
{
matched2[pos] = temp;
matchDistances[pos] = distances[i];
}
}
RejectOutlierMatches(matched1, matched2, matchDistances, 2.5);
//error = 0;
//for (int i = 0; i < matchDistances.size(); i++)
//{
// error += sqrt(matchDistances[i]);
//}
//error /= matchDistances.size();
//cout << error << endl;
cv::Mat matched1MatCv(matched1.size(), 3, CV_32F, matched1.data());
cv::Mat matched2MatCv(matched2.size(), 3, CV_32F, matched2.data());
cv::Mat tempT;
cv::reduce(matched1MatCv - matched2MatCv, tempT, 0, CV_REDUCE_AVG);
for (int i = 0; i < verts2Mat.rows; i++)
{
verts2Mat.row(i) += tempT;
}
for (int i = 0; i < matched2MatCv.rows; i++)
{
matched2MatCv.row(i) += tempT;
}
cv::Mat M = matched2MatCv.t() * matched1MatCv;
cv::SVD svd;
svd(M);
cv::Mat tempR = svd.u * svd.vt;
double det = cv::determinant(tempR);
if (det < 0)
{
cv::Mat temp = cv::Mat::eye(3, 3, CV_32F);
temp.at<float>(2, 2) = -1;
tempR = svd.u * temp * svd.vt;
}
verts2Mat = verts2Mat * tempR;
matT += tempT * matR.t();
matR = matR * tempR;
}
memcpy(verts2, verts2Mat.data, verts2Mat.rows * sizeof(float) * 3);
memcpy(R, matR.data, 9 * sizeof(float));
memcpy(t, matT.data, 3 * sizeof(float));
return error;
}
VectorN& leastSquaresApproximation(VectorN &x,
VectorN &y,
unsigned int deg)
{
unsigned int i, j;
unsigned int dim = x.dim();
deg++; // this is because we need an extra spot
if (x.dim() != y.dim()) {
// cerr << "leastSquaresApproximation: Dimensions do not match" << endl;
return y;
}
Matrix mat(dim, deg);
for (i = 0; i < dim; i++) {
mat[i][0] = 1;
for (j = 1; j < deg; j++) {
mat[i][j] = mat[i][j-1]*x[i];
}
}
Matrix matT(mat);
matT.trans();
// cout << mat << endl << endl;
// cout << matT << endl;
// cout << sol << endl << endl;
y *= matT;
// cout << sol << endl;
(matT*mat).rref(y);
return y;
}
