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");
}*/
}
