//procustres inline void ICP::minimize(const Pair& init_f){ Eigen::Vector3d centroide_model(0.0,0.0,0.0), centroide_data(0.0,0.0,0.0); Eigen::Matrix3d M; unsigned int N = data_indices.size() + init_f.size(); Eigen::MatrixXd model(3,N); Eigen::MatrixXd data(3,N); //calcula os centroides int k=0; for(unsigned int i=0; i<N; i++){ if (i<data_indices.size()){ model(0,i) = cloud_m->points[ model_indices[i] ].x; model(1,i) = cloud_m->points[ model_indices[i] ].y; model(2,i) = cloud_m->points[ model_indices[i] ].z; data(0,i) = cloud_d->points[ data_indices[i] ].x*T(0,0) + cloud_d->points[ data_indices[i] ].y*T(0,1) + cloud_d->points[ data_indices[i] ].z*T(0,2) + T(0,3); data(1,i) = cloud_d->points[ data_indices[i] ].x*T(1,0) + cloud_d->points[ data_indices[i] ].y*T(1,1) + cloud_d->points[ data_indices[i] ].z*T(1,2) + T(1,3); data(2,i) = cloud_d->points[ data_indices[i] ].x*T(2,0) + cloud_d->points[ data_indices[i] ].y*T(2,1) + cloud_d->points[ data_indices[i] ].z*T(2,2) + T(2,3); }else{ model(0,i) = cloud_m->points[ init_f[k].first ].x; model(1,i) = cloud_m->points[ init_f[k].first ].y; model(2,i) = cloud_m->points[ init_f[k].first ].z; data(0,i) = cloud_d->points[ init_f[k].second ].x*T(0,0) + cloud_d->points[ init_f[k].second ].y*T(0,1) + cloud_d->points[ init_f[k].second ].z*T(0,2) + T(0,3); data(1,i) = cloud_d->points[ init_f[k].second ].x*T(1,0) + cloud_d->points[ init_f[k].second ].y*T(1,1) + cloud_d->points[ init_f[k].second ].z*T(1,2) + T(1,3); data(2,i) = cloud_d->points[ init_f[k].second ].x*T(2,0) + cloud_d->points[ init_f[k].second ].y*T(2,1) + cloud_d->points[ init_f[k].second ].z*T(2,2) + T(2,3); k++; } centroide_model += model.block(0,i,3,1); centroide_data += data.block(0,i,3,1); } centroide_data = centroide_data/N; centroide_model = centroide_model/N; //subtrai os centroides aos dados for (unsigned int i=0; i<N; i++){ model.block(0,i,3,1) -= centroide_model; data.block(0,i,3,1) -= centroide_data; } //Determina a transformacao M = data*model.transpose(); Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d U = svd.matrixU(); Eigen::Matrix3d V = svd.matrixV(); if (U.determinant()*V.determinant()<0) { for (int i=0; i<3; ++i) V(i,2) *=-1; } Eigen::Matrix3d r = V * U.transpose(); Eigen::Vector3d t = centroide_model - r * centroide_data; //~ T.block<3,3>(0,0) = r*T.block<3,3>(0,0); //~ T.block<3,1>(0,3) += t; T.block<3,1>(0,3) = T.block<3,3>(0,0)*t + T.block<3,1>(0,3); T.block<3,3>(0,0) = T.block<3,3>(0,0)*r; }
inline void ICP::UpdateAndReject(Pair& init_f){ double sigma=0.0; double mean=0.0; unsigned int N = data_indices.size() + init_f.size(); Eigen::Vector4d point_s(0.0,0.0,0.0,1.0); Eigen::Vector4d point_d(0.0,0.0,0.0,1.0); std::vector<double> dists(N); //compute mean unsigned int k=0; for (unsigned int i=0 ; i < N; i++){ if(i<data_indices.size()){ point_s(0) = cloud_m->points[ model_indices[i] ].x; point_s(1) = cloud_m->points[ model_indices[i] ].y; point_s(2) = cloud_m->points[ model_indices[i] ].z; point_d(0) = cloud_d->points[ data_indices[i] ].x; point_d(1) = cloud_d->points[ data_indices[i] ].y; point_d(2) = cloud_d->points[ data_indices[i] ].z; }else{ point_s(0) = cloud_m->points[ init_f[k].first ].x; point_s(1) = cloud_m->points[ init_f[k].first ].y; point_s(2) = cloud_m->points[ init_f[k].first ].z; point_d(0) = cloud_d->points[ init_f[k].second ].x; point_d(1) = cloud_d->points[ init_f[k].second ].y; point_d(2) = cloud_d->points[ init_f[k].second ].z; k++; } point_d = T*point_d; dists[i]= (point_d - point_s).norm(); mean = mean + dists[i]; } mean = mean/N; //compute standart diviation for (unsigned int i=0; i < N; i++){ sigma = sigma + (dists[i]-mean)*(dists[i]-mean); } sigma = sigma/N; sigma = sqrt(sigma); //How good is the registration if (mean<D) //very good Dmax = mean + 3*sigma; else if (mean<3*D) //good Dmax = mean + 2*sigma; else if (mean<6*D) //bad Dmax = mean + sigma; else { //very bad std::vector<double> dists2 = dists; sort (dists2.begin(), dists2.end()); if (dists2.size() % 2 == 0) { Dmax = (dists2[dists2.size()/2-1] + dists2[dists2.size()/2]) / 2.0; }else { Dmax = dists2[dists2.size()/2]; } } //Update the maching k=0; unsigned int i=0; for (i=0 ; i <data_indices.size() ; i++){ if (dists[i] < Dmax){ model_indices[k] = model_indices[i]; data_indices[k] = data_indices[i]; k++; } } model_indices.resize(k); data_indices.resize(k); k=0; unsigned int j,l; for (j=i, l=0; j<N ; j++,l++){ if (dists[j] < Dmax){ init_f[k]= init_f[l]; k++; } } if(k!=init_f.size()) init_f.resize(k); }