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;
}
double Solver4234::solve(ResidualFunction *f, double *X, GaussNewtonParams param, GaussNewtonReport *report){
int itn = 0;
while (1) {
double *R = new double[f->nR()];
double *J = new double[f->nR()*f->nX()];
f->eval(R, J, X);
std::cout<<"X: "<<X[0]<<" "<<X[1]<<" "<<X[2]<<std::endl;
cv::Mat matJr(f->nR(),f->nX(),CV_64FC1,J);
cv::Mat matR(f->nR(),1,CV_64FC1,R);
std::cout<<matR<<std::endl;
//array2mat(f->nR(), f->nX(), matJr, J);
//array2mat(f->nR(), 1, matR, R);
cv::Mat detX = -((matJr.t())*(matJr)).inv()*(matJr.t())*(matR);
std::cout<<detX<<std::endl;
//write report.
if (itn >= param.max_iter) {
if (!checknum(X,f->nX())) {
report->stop_type = GaussNewtonReport::STOP_NUMERIC_FAILURE;
}
else{
std::cout<<"final error: "<<err(R, f->nR())<<std::endl;
report->stop_type = GaussNewtonReport::STOP_NO_CONVERGE;
}
break;
}
else if (cv::norm(matJr, cv::NORM_INF) < param.gradient_tolerance) {
if (!checknum(X,f->nX())) {
report->stop_type = GaussNewtonReport::STOP_NUMERIC_FAILURE;
}
else{
std::cout<<"final error: "<<err(R, f->nR())<<std::endl;
report->stop_type = GaussNewtonReport::STOP_GRAD_TOL;
}
break;
}
else if(cv::norm(matR, cv::NORM_INF) < param.residual_tolerance){
if (!checknum(X,f->nX())) {
report->stop_type = GaussNewtonReport::STOP_NUMERIC_FAILURE;
}
else{
std::cout<<"final error: "<<err(R, f->nR())<<std::endl;
report->stop_type = GaussNewtonReport::STOP_RESIDUAL_TOL;
}
break;
}
// cv::solve(matJr, -matR, detX, cv::DECOMP_SVD);
std::cout<<"detX: "<<detX<<std::endl;
std::cout<<"error: "<<err(R, f->nR())<<std::endl;
int a = 1;
for (int i = 0; i < f->nR(); i++) {
X[i] = X[i] + a*detX.at<double>(i,0);
}
itn++;
report->n_iter = itn;
delete [] R;
delete [] J;
}
return 0;
}
FbxAMatrix NotDecomposedMultiply(const FbxAMatrix& lhs, const FbxAMatrix& rhs){
FbxMatrix matL(lhs);
FbxMatrix matR(rhs);
auto result = matL*matR;
return *(FbxAMatrix*) (double*) &result;
}
