FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess) { unsigned int s_nr_data = src->data.cols();//std::min(int(src->data.cols()),int(500000)); unsigned int d_nr_data = dst->data.cols(); refinement->allow_regularization = true; //printf("s_nr_data: %i d_nr_data: %i\n",s_nr_data,d_nr_data); int stepy = std::max(1,int(d_nr_data)/100000); Eigen::Matrix<double, 3, Eigen::Dynamic> Y; Eigen::Matrix<double, 3, Eigen::Dynamic> N; Y.resize(Eigen::NoChange,d_nr_data/stepy); N.resize(Eigen::NoChange,d_nr_data/stepy); unsigned int ycols = Y.cols(); for(unsigned int i = 0; i < d_nr_data/stepy; i++) { Y(0,i) = dst->data(0,i*stepy); Y(1,i) = dst->data(1,i*stepy); Y(2,i) = dst->data(2,i*stepy); N(0,i) = dst->normals(0,i*stepy); N(1,i) = dst->normals(1,i*stepy); N(2,i) = dst->normals(2,i*stepy); } /// Build kd-tree //nanoflann::KDTreeAdaptor<Eigen::Matrix3Xd, 3, nanoflann::metric_L2_Simple> kdtree(Y); Eigen::VectorXd DST_INORMATION = Eigen::VectorXd::Zero(Y.cols()); for(unsigned int i = 0; i < d_nr_data/stepy; i++) { DST_INORMATION(i) = dst->information(0,i*stepy); } double s_mean_x = 0; double s_mean_y = 0; double s_mean_z = 0; for(unsigned int i = 0; i < s_nr_data; i++) { s_mean_x += src->data(0,i); s_mean_y += src->data(1,i); s_mean_z += src->data(2,i); } s_mean_x /= double(s_nr_data); s_mean_y /= double(s_nr_data); s_mean_z /= double(s_nr_data); double d_mean_x = 0; double d_mean_y = 0; double d_mean_z = 0; for(unsigned int i = 0; i < d_nr_data; i++) { d_mean_x += dst->data(0,i); d_mean_y += dst->data(1,i); d_mean_z += dst->data(2,i); } d_mean_x /= double(d_nr_data); d_mean_y /= double(d_nr_data); d_mean_z /= double(d_nr_data); double stop = 0.00001; Eigen::Affine3d Ymean = Eigen::Affine3d::Identity(); Ymean(0,3) = d_mean_x; Ymean(1,3) = d_mean_y; Ymean(2,3) = d_mean_z; Eigen::Affine3d Xmean = Eigen::Affine3d::Identity(); Xmean(0,3) = s_mean_x; Xmean(1,3) = s_mean_y; Xmean(2,3) = s_mean_z; std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > all_X; std::vector< Eigen::Affine3d > all_res; std::vector< int > count_X; std::vector< float > score_X; std::vector< std::vector< Eigen::VectorXd > > all_starts; int stepxsmall = std::max(1,int(s_nr_data)/250); Eigen::VectorXd Wsmall (s_nr_data/stepxsmall); for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++) { Wsmall(i) = src->information(0,i*stepxsmall); } double sumtime = 0; double sumtimeSum = 0; double sumtimeOK = 0; int r = 0; refinement->viewer = viewer; refinement->visualizationLvl = 0; //for(int r = 0; r < 1000; r++){ // while(true){ // double rx = 2.0*M_PI*0.0001*double(rand()%10000); // double ry = 2.0*M_PI*0.0001*double(rand()%10000); // double rz = 2.0*M_PI*0.0001*double(rand()%10000); //double stop = 0; double step = 0.1+2.0*M_PI/5; for(double rx = 0; rx < 2.0*M_PI; rx += step) { for(double ry = 0; ry < 2.0*M_PI; ry += step) for(double rz = 0; rz < 2.0*M_PI; rz += step) { printf("rx: %f ry: %f rz: %f\n",rx,ry,rz); double start = getTime(); double meantime = 999999999999; if(r != 0) { meantime = sumtimeSum/double(sumtimeOK+1.0); } refinement->maxtime = std::min(0.5,3*meantime); Eigen::VectorXd startparam = Eigen::VectorXd(3); startparam(0) = rx; startparam(1) = ry; startparam(2) = rz; Eigen::Affine3d randomrot = Eigen::Affine3d::Identity(); randomrot = Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()); Eigen::Affine3d current_guess = Ymean*randomrot*Xmean.inverse();//*Ymean; refinement->target_points = 250; FusionResults fr = refinement->getTransform(current_guess.matrix()); //fr.timeout = timestopped; double stoptime = getTime(); sumtime += stoptime-start; if(!fr.timeout) { sumtimeSum += stoptime-start; sumtimeOK++; } //printf("sumtime: %f\n",sumtime); stop = fr.score; Eigen::Matrix4d m = fr.guess; current_guess = m;//fr.guess; float m00 = current_guess(0,0); float m01 = current_guess(0,1); float m02 = current_guess(0,2); float m03 = current_guess(0,3); float m10 = current_guess(1,0); float m11 = current_guess(1,1); float m12 = current_guess(1,2); float m13 = current_guess(1,3); float m20 = current_guess(2,0); float m21 = current_guess(2,1); float m22 = current_guess(2,2); float m23 = current_guess(2,3); Eigen::Matrix<double, 3, Eigen::Dynamic> Xsmall; Xsmall.resize(Eigen::NoChange,s_nr_data/stepxsmall); for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++) { float x = src->data(0,i*stepxsmall); float y = src->data(1,i*stepxsmall); float z = src->data(2,i*stepxsmall); Xsmall(0,i) = m00*x + m01*y + m02*z + m03; Xsmall(1,i) = m10*x + m11*y + m12*z + m13; Xsmall(2,i) = m20*x + m21*y + m22*z + m23; } bool exists = false; for(unsigned int ax = 0; ax < all_X.size(); ax++) { Eigen::Matrix<double, 3, Eigen::Dynamic> axX = all_X[ax]; Eigen::Affine3d axT = all_res[ax]; double diff = (Xsmall-axX).colwise().norm().mean(); if(diff < 20*stop) { count_X[ax]++; all_starts[ax].push_back(startparam); int count = count_X[ax]; float score = score_X[ax]; std::vector< Eigen::VectorXd > starts = all_starts[ax]; for(int bx = ax-1; bx >= 0; bx--) { if(count_X[bx] < count_X[bx+1]) { count_X[bx+1] = count_X[bx]; score_X[bx+1] = score_X[bx]; all_X[bx+1] = all_X[bx]; all_starts[bx+1] = all_starts[bx]; all_res[bx+1] = all_res[bx]; all_X[bx] = axX; count_X[bx] = count; score_X[bx] = score; all_starts[bx] = starts; all_res[bx] = axT; } else { break; } } exists = true; break; } } if(!exists) { all_X.push_back(Xsmall); count_X.push_back(1); score_X.push_back(stop); all_starts.push_back(std::vector< Eigen::VectorXd >()); all_starts.back().push_back(startparam); all_res.push_back(current_guess); } r++; } } FusionResults fr = FusionResults(); refinement->allow_regularization = false; int tpbef = refinement->target_points; refinement->target_points = 2000; for(unsigned int ax = 0; ax < all_X.size(); ax++) { Eigen::Matrix4d np = all_res[ax].matrix(); refinement->visualizationLvl = visualizationLvl; if(ax < 25) { double start = getTime(); FusionResults fr1 = refinement->getTransform(np); double stop = getTime(); np = fr1.guess; } refinement->visualizationLvl = 0; fr.candidates.push_back(np); fr.counts.push_back(count_X[ax]); fr.scores.push_back(1.0/score_X[ax]); } refinement->target_points = tpbef; return fr; }
FusionResults RegistrationSICP::getTransform(Eigen::MatrixXd guess){ Eigen::Matrix<double, 3, Eigen::Dynamic> X;// = src->data; Eigen::Matrix<double, 3, Eigen::Dynamic> Y;// = dst->data; Eigen::Matrix<double, 3, Eigen::Dynamic> N;// = dst->data; float m00 = guess(0,0); float m01 = guess(0,1); float m02 = guess(0,2); float m03 = guess(0,3); float m10 = guess(1,0); float m11 = guess(1,1); float m12 = guess(1,2); float m13 = guess(1,3); float m20 = guess(2,0); float m21 = guess(2,1); float m22 = guess(2,2); float m23 = guess(2,3); unsigned int s_nr_data = src->data.cols(); X.resize(Eigen::NoChange,s_nr_data); for(unsigned int i = 0; i < s_nr_data; i++){ float x = src->data(0,i); float y = src->data(1,i); float z = src->data(2,i); X(0,i) = m00*x + m01*y + m02*z + m03; X(1,i) = m10*x + m11*y + m12*z + m13; X(2,i) = m20*x + m21*y + m22*z + m23; } unsigned int d_nr_data = dst->data.cols(); Y.resize(Eigen::NoChange,d_nr_data); N.resize(Eigen::NoChange,d_nr_data); for(unsigned int i = 0; i < d_nr_data; i++){ Y(0,i) = dst->data(0,i); Y(1,i) = dst->data(1,i); Y(2,i) = dst->data(2,i); N(0,i) = dst->normals(0,i); N(1,i) = dst->normals(1,i); N(2,i) = dst->normals(2,i); } SICP::Parameters pars; pars.p = 0.5; pars.max_icp = 5; pars.max_inner = 1; pars.max_outer = 30; pars.stop = 0.0001; pars.print_icpn = true; Eigen::Matrix4d t = SICP::point_to_plane(X,Y,N,pars); pcl::TransformationFromCorrespondences tfc; for(unsigned int i = 0; i < s_nr_data; i++){ Eigen::Vector3f a (X(0,i), X(1,i), X(2,i)); Eigen::Vector3f b (src->data(0,i), src->data(1,i), src->data(2,i)); tfc.add(b,a); } Eigen::Matrix4d np = tfc.getTransformation().matrix().cast<double>(); return FusionResults(np,0); /* ICP::Parameters ipars; ipars.f = ICP::PNORM; ipars.p = 0.5; //ipars.max_icp = 10; //ipars.max_outer = 100; //ipars.stop = 0.0001; pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr dcloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>); scloud->points.clear(); for(unsigned int i = 0; i < s_nr_data; i++){ pcl::PointXYZRGBNormal p;p.x = s(0,i);p.y = s(1,i);p.z = s(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p); } dcloud->points.clear(); for(unsigned int i = 0; i < d_nr_data; i++){ pcl::PointXYZRGBNormal p;p.x = d(0,i);p.y = d(1,i);p.z = d(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p); } viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud"); viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud"); viewer->spin(); viewer->removeAllPointClouds(); for(int k = 0;true;k++){ Eigen::Matrix4d t = SICP::point_to_plane(s,d,dn,pars); //Eigen::Matrix4d t = ICP::point_to_plane(s, d, dn,ipars);//, pars); scloud->points.clear(); for(unsigned int i = 0; i < s_nr_data; i++){ pcl::PointXYZRGBNormal p;p.x = s(0,i);p.y = s(1,i);p.z = s(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p); } dcloud->points.clear(); for(unsigned int i = 0; i < d_nr_data; i++){ pcl::PointXYZRGBNormal p;p.x = d(0,i);p.y = d(1,i);p.z = d(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p); } viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud"); viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "scloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "dcloud"); printf("%i\n",k); if(k % 3000 == 0){ viewer->spin();} else{ viewer->spinOnce();} viewer->removeAllPointClouds(); } //SICP::print(); */ /* float m00 = np(0,0); float m01 = np(0,1); float m02 = np(0,2); float m03 = np(0,3); float m10 = np(1,0); float m11 = np(1,1); float m12 = np(1,2); float m13 = np(1,3); float m20 = np(2,0); float m21 = np(2,1); float m22 = np(2,2); float m23 = np(2,3); dcloud->points.clear(); for(unsigned int i = 0; i < s_nr_data; i++){ pcl::PointXYZRGBNormal p; float x = s(0,i); float y = s(1,i); float z = s(2,i); p.x = x+0.0001; p.y = y; p.z = z; p.b = 255; p.g = 255; p.r = 0; dcloud->points.push_back(p); } scloud->points.clear(); for(unsigned int i = 0; i < s_nr_data; i++){ pcl::PointXYZRGBNormal p; float x = src->data(0,i); float y = src->data(1,i); float z = src->data(2,i); p.x = m00*x + m01*y + m02*z + m03; p.y = m10*x + m11*y + m12*z + m13; p.z = m20*x + m21*y + m22*z + m23; p.b = 0; p.g = 0; p.r = 255; scloud->points.push_back(p); } viewer->addPointCloud<pcl::PointXYZRGBNormal> (scloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(scloud), "scloud"); viewer->addPointCloud<pcl::PointXYZRGBNormal> (dcloud, pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>(dcloud), "dcloud"); viewer->spin(); viewer->removeAllPointClouds(); exit(0); return guess; */ }