void pclbo::LBOEstimation<PointT, NormalT>::compute() { typename pcl::KdTreeFLANN<PointT>::Ptr kdt(new pcl::KdTreeFLANN<PointT>()); kdt->setInputCloud(_cloud); const double avg_dist = pclbo::avg_distance<PointT>(10, _cloud, kdt); const double h = 5 * avg_dist; std::cout << "Average distance between points: " << avg_dist << std::endl; int points_with_mass = 0; double avg_mass = 0.0; B.resize(_cloud->size()); std::cout << "Computing the Mass matrix..." << std::flush; // Compute the mass matrix diagonal B for (int i = 0; i < _cloud->size(); i++) { const auto& point = _cloud->at(i); const auto& normal = _normals->at(i); const auto& normal_vector = normal.getNormalVector3fMap().template cast<double>(); if (!pcl::isFinite(point)) continue; std::vector<int> indices; std::vector<float> distances; kdt->radiusSearch(point, h, indices, distances); if (indices.size() < 4) { B[i] = 0.0; continue; } // Project the neighbor points in the tangent plane at p_i with normal n_i std::vector<Eigen::Vector3d> projected_points; for (const auto& neighbor_index : indices) { if (neighbor_index != i) { const auto& neighbor_point = _cloud->at(neighbor_index); projected_points.push_back(project(point, normal, neighbor_point)); } } assert(projected_points.size() >= 3); // Use the first vector to create a 2D basis Eigen::Vector3d u = projected_points[0]; u.normalize(); Eigen::Vector3d v = (u.cross(normal_vector)); v.normalize(); // Add the points to a 2D plane std::vector<Eigen::Vector2d> plane; // Add the point at the center plane.push_back(Eigen::Vector2d::Zero()); // Add the rest of the points for (const auto& projected : projected_points) { double x = projected.dot(u); double y = projected.dot(v); // Add the 2D point to the vector plane.push_back(Eigen::Vector2d(x, y)); } assert(plane.size() >= 4); // Compute the voronoi cell area of the point double area = VoronoiDiagram::area(plane); B[i] = area; avg_mass += area; points_with_mass++; } // Average mass if (points_with_mass > 0) { avg_mass /= static_cast<double>(points_with_mass); } // Set border points to have average mass for (auto& b : B) { if (b == 0.0) { b = avg_mass; } } std::cout << "done" << std::endl; std::cout << "Computing the stiffness matrix..." << std::flush; std::vector<double> diag(_cloud->size(), 0.0); // Compute the stiffness matrix Q for (int i = 0; i < _cloud->size(); i++) { const auto& point = _cloud->at(i); if (!pcl::isFinite(point)) continue; std::vector<int> indices; std::vector<float> distances; kdt->radiusSearch(point, h, indices, distances); for (const auto& j : indices) { if (j != i) { const auto& neighbor = _cloud->at(j); double d = (neighbor.getVector3fMap() - point.getVector3fMap()).norm(); double w = B[i] * B[j] * (1.0 / (4.0 * M_PI * h * h)) * exp(-(d * d) / (4.0 * h)); I.push_back(i); J.push_back(j); S.push_back(w); diag[i] += w; } } } // Fill the diagonal as the negative sum of the rows for (int i = 0; i < diag.size(); i++) { I.push_back(i); J.push_back(i); S.push_back(-diag[i]); } // Compute the B^{-1}Q matrix Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(_cloud->size(), _cloud->size()); for (int i = 0; i < I.size(); i++) { const int row = I[i]; const int col = J[i]; Q(row, col) = S[i]; } std::cout << "done" << std::endl; std::cout << "Computing eigenvectors" << std::endl; Eigen::Map<Eigen::VectorXd> B_vec(B.data(), B.size()); Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> ges; ges.compute(Q, B_vec.asDiagonal()); eigenvalues = ges.eigenvalues(); eigenfunctions = ges.eigenvectors(); // Sort the eigenvalues by magnitude std::vector<std::pair<double, int> > map_vector(eigenvalues.size()); for (auto i = 0; i < eigenvalues.size(); i++) { map_vector[i].first = std::abs(eigenvalues(i)); map_vector[i].second = i; } std::sort(map_vector.begin(), map_vector.end()); // truncate the first 100 eigenfunctions Eigen::MatrixXd eigenvectors(eigenfunctions.rows(), eigenfunctions.cols()); Eigen::VectorXd eigenvals(eigenfunctions.cols()); eigenvalues.resize(map_vector.size()); for (auto i = 0; i < map_vector.size(); i++) { const auto& pair = map_vector[i]; eigenvectors.col(i) = eigenfunctions.col(pair.second); eigenvals(i) = pair.first; } eigenfunctions = eigenvectors; eigenvalues = eigenvals; }
void ColorLines::patchLine(double lambdas[3], double p1[3], double p2[3], double *img, int h, int w, int c, int min_y, int max_y, int min_x, int max_x, bool onlyPositive){ int numPx=(max_y-min_y+1)*(max_x-min_x+1); double d[numPx]; double p[c*numPx]; int ind=0; for(int i=min_y; i<=max_y; i++){ for(int j=min_x; j<=max_x; j++){ for(int k=0; k<c; k++){ p[c*ind+k]=img[c*(w*i+j)+k]; } ind++; } } ind=0; bool n_zero; for(int i=0; i<numPx; i++){ n_zero=false; for(int k=0; k<c; k++){ if(p[c*i+k]){ n_zero=true; } } if(n_zero){ for(int k=0; k<c; k++){ p[c*ind+k]=p[c*i+k]; } ind++; } } numPx=ind; double m[c]; for(int k=0; k<c; k++){ m[k]=0; } for(int i=0; i<numPx; i++){ for(int k=0; k<c; k++){ m[k]+=p[c*i+k]; } } for(int k=0; k<c; k++){ m[k]=m[k]/(double)numPx; } /*if(min_y==5 && min_x==5){ printf("%f %f %f\n", m[0], m[1], m[2]); }*/ double a[c*numPx]; for(int i=0; i<numPx; i++){ for(int k=0; k<c; k++){ a[c*i+k]=p[c*i+k]-m[k]; } } double a2[c*c]; for(int i=0; i<c; i++){ for(int j=0; j<c; j++){ a2[c*i+j]=0; for(int k=0; k<numPx; k++){ a2[c*i+j]+=a[c*k+i]*a[c*k+j]; } a2[c*i+j]=a2[c*i+j]/c; } } /*if(min_y==5 && min_x==5){ for(int i=0; i<3; i++){ for(int j=0; j<3; j++){ printf("%f ", a2[3*i+j]); } printf("\n"); } }*/ double diag[3]; double v[3]; Matrix3d aa(3,3); EigenSolver<Matrix3d> es(aa); EigenSolver<Eigen::MatrixXd>::EigenvalueType eigenvals; EigenSolver<Eigen::MatrixXd>::EigenvectorsType eigenvecs; aa << a2[0], a2[1], a2[2], a2[3], a2[4], a2[5],a2[6],a2[7],a2[8]; eigenvals=es.eigenvalues(); eigenvecs=es.eigenvectors(); diag[0]=eigenvals(0,0).real(); diag[1]=eigenvals(1,0).real(); diag[2]=eigenvals(2,0).real(); v[0]=eigenvecs(0,2).real()*0.3; v[1]=eigenvecs(1,2).real()*0.3; v[2]=eigenvecs(2,2).real()*0.3; /*if(min_x==5 && min_y==5){ std::cout << "matriz:" << std::endl << aa << std::endl; std::cout << "eigenvalues:" << std::endl << eigenvals << std::endl; std::cout << "eigenvectors:" << std::endl << eigenvecs << std::endl; //printf("vectors: %f %f %f\nvalues: %f %f %f\n", v[0], v[1], v[2], diag[0], diag[1], diag[2]); }*/ //printf("%f %f %f\n%f %f %f\n", diag[0],diag[1],diag[2], v[0], v[1], v[2]); //calculco dos eigenvalues /*double p_val1=a2[1]*a2[1] + a2[2]*a2[2] + a2[5]*a2[5]; if(p_val1==0){ diag[0] = a2[0]; diag[1] = a2[4]; diag[2] = a2[8]; } else{ double q=(a2[0]+a2[4]+a2[8])/3.0; double p_val2 = (a2[0]-q)*(a2[0]-q)+(a2[4]-q)*(a2[4]-q)+(a2[8]-q)*(a2[8]-q)+2*p_val1; double p_val=sqrt(p_val2/6.0); double b[9]; for(int i=0; i<9; i++){ b[i]=a2[i]; } for(int i=0; i<9; i+=4){ b[i]-=q; } for(int i=0; i<9; i++){ b[i]=b[i]/p_val; } double det_b=b[0]*b[4]*b[8]+b[2]*b[3]*b[7]+b[1]*b[5]*b[6]; det_b-=b[6]*b[4]*b[2]+b[3]*b[1]*b[8]+b[7]*b[5]*b[0]; double r=det_b/2.0; double phi; if (r<=-1){ phi=M_PI/3.0; } else if(r>=1){ phi=0; } else{ phi=acos(r)/3.0; } diag[2]=q+2.0*p_val*cos(phi); diag[0]=q+2.0*p_val*cos(phi+(2.0*M_PI/3.0)); diag[1]=3.0*q-diag[2]-diag[0]; }*/ for(int i=0; i<3; i++){ p1[i]=m[i]+v[i]; p2[i]=m[i]-v[i]; } int sig[3]; for(int i=0; i<3; i++){ if(v[i]>0){ sig[i]=1; } else if(v[i]<0){ sig[i]=-1; } else{ sig[i]=0; } } if(!onlyPositive || !((sig[1]-sig[0]) || sig[2]-sig[0])){ //printf("entrou\n"); /*for(int i=0; i<3; i++){ printf("%f ", p1[i]); } printf("\n"); for(int i=0; i<3; i++){ printf("%f ", p2[i]); } printf("\n");*/ for(int i=0; i<3; i++){ v[i]=v[i]*sig[0]; p1[i]=m[i]-v[i]; p2[i]=m[i]+v[i]; } double pq[3*numPx]; for(int i=0; i<numPx; i++){ pq[3*i]=p[3*i]-p1[0]; pq[3*i+1]=p[3*i+1]-p1[1]; pq[3*i+2]=p[3*i+2]-p1[2]; } double pqxv[3*numPx]; for(int i=0; i<numPx; i++){ pqxv[3*i]=pq[3*i+1]*v[2]-pq[3*i+2]*v[1]; pqxv[3*i+1]=pq[3*i+2]*v[0]-pq[3*i]*v[2]; pqxv[3*i+2]=pq[3*i]*v[1]-pq[3*i+1]*v[0]; } double norm_v=sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]); for(int i=0; i<numPx; i++){ d[i]=sqrt(pqxv[3*i]*pqxv[3*i]+pqxv[3*i+1]*pqxv[3*i+1]+pqxv[3*i+2]*pqxv[3*i+2])/norm_v; } int inds[numPx]; for(int i=0; i<numPx; i++){ inds[i]=i; } my_sort(d, inds, numPx); double p_sorted[3*numPx]; for(int i=0; i<numPx; i++){ p_sorted[3*i]=p[3*inds[i]]; p_sorted[3*i+1]=p[3*inds[i]+1]; p_sorted[3*i+2]=p[3*inds[i]+2]; } numPx = 0.8*numPx+0.5; for(int k=0; k<c; k++){ m[k]=0; } for(int i=0; i<numPx; i++){ for(int k=0; k<c; k++){ m[k]+=p_sorted[c*i+k]; } } for(int k=0; k<c; k++){ m[k]=m[k]/(double)numPx; } for(int i=0; i<numPx; i++){ for(int k=0; k<c; k++){ a[c*i+k]=p_sorted[c*i+k]-m[k]; } } for(int i=0; i<c; i++){ for(int j=0; j<c; j++){ a2[c*i+j]=0; for(int k=0; k<numPx; k++){ a2[c*i+j]+=a[c*k+i]*a[c*k+j]; } a2[c*i+j]=a2[c*i+j]/c; } } /*for(int i=0; i<9; i++){ aa[i/3][i%3]=a2[i]; }*/ //printf("entrou aqui!\n"); aa << a2[0], a2[1], a2[2], a2[3], a2[4], a2[5],a2[6],a2[7],a2[8]; es.compute(aa); eigenvals=es.eigenvalues(); eigenvecs=es.eigenvectors(); diag[0]=eigenvals(0,0).real(); diag[1]=eigenvals(1,0).real(); diag[2]=eigenvals(2,0).real(); v[0]=eigenvecs(0,2).real()*0.3; v[1]=eigenvecs(1,2).real()*0.3; v[2]=eigenvecs(2,2).real()*0.3; /*if(min_x==5 && min_y==5){ std::cout << "The eigenvalues of A are:" << std::endl << eigenvals << std::endl; std::cout << "The eigenvectors of A are:" << std::endl << eigenvecs << std::endl; printf("vectors: %f %f %f\nvalues: %f %f %f\n", v[0], v[1], v[2], diag[0], diag[1], diag[2]); }*/ //printf("%.20f %.20f %.20f\n%f %f %f\n", diag[0],diag[1],diag[2], v[0], v[1], v[2]); for(int i=0; i<3; i++){ p1[i]=m[i]+v[i]; p2[i]=m[i]-v[i]; } norm_v=sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]); double cross_vp1[3]; cross_vp1[0]=v[1]*(-p1[2])-v[2]*(-p1[1]); cross_vp1[1]=v[2]*(-p1[0])-v[0]*(-p1[2]); cross_vp1[2]=v[0]*(-p1[1])-v[1]*(-p1[0]); //printf("%f %f %f\n", cross_vp1[0], cross_vp1[1], cross_vp1[2]); double distFromOrigin = sqrt(cross_vp1[0]*cross_vp1[0]+cross_vp1[1]*cross_vp1[1]+cross_vp1[2]*cross_vp1[2])/norm_v; lambdas[0]=diag[2]; lambdas[1]=diag[2]/(diag[1]+0.00000001); lambdas[2]=distFromOrigin; //printf("%f %.20f %f %f %f\n", diag[2], diag[1], lambdas[0], lambdas[1], lambdas[2]); for(int i=0; i<3; i++){ if(v[i]>0){ sig[i]=1; } else if(v[i]<0){ sig[i]=-1; } else{ sig[i]=0; } } if(onlyPositive && (sig[1]-sig[0]+sig[2]-sig[0])!=0){ lambdas[0]=0; lambdas[1]=0; lambdas[2]=0; } //printf("%f\n",distFromOrigin); //printf("%f %f\n",norm_v, distFromOrigin); /*for(int i=0; i<3; i++){ printf("%f ", p1[i]); } printf("\n"); for(int i=0; i<3; i++){ printf("%f ", p2[i]); }*/ /*for(int i=0; i<numClose; i++){ printf("%f %f %f\n", p_sorted[3*i],p_sorted[3*i+1],p_sorted[3*i+2]); }*? /*printf("%f\n", norm_v); for(int i=0; i<numPx; i++){ printf("%f\n", d[i]); }*/ /*for(int i=0; i<3; i++){ printf("%f ", v[i]); } printf("\n"); for(int i=0; i<3; i++){ printf("%f ", p1[i]); } printf("\n"); for(int i=0; i<3; i++){ printf("%f ", p2[i]); }*/ } else { lambdas[0]=0; lambdas[1]=0; lambdas[2]=0; } /*for(int i=0; i<3; i++){ printf("%d\n", sig[i]); }*/ /*for(int i=0; i<c; i++){ printf("%f\n", m[i]); }*/ //printf("%d\n\n", numPx); /*for(int i=0; i<c; i++){ for(int k=0; k<c; k++){ printf("%f ", a2[c*i+k]); } printf("\n"); }*/ }