-
Notifications
You must be signed in to change notification settings - Fork 0
/
normal.cpp
111 lines (102 loc) · 3.14 KB
/
normal.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
#include "normal.h"
Normal::Normal(PCD* pointcloud, float radius) {
if (!pointcloud || pointcloud->numPoints == 0)
return;
//set up data structures
cloud = pointcloud;
norm = new float[pointcloud->numPoints * 3];
curvature = new float[pointcloud->numPoints]();
if (!pointcloud->kdtree)
pointcloud->kdtree = new KdTree(pointcloud);
for (int i=0;i<pointcloud->numPoints;i++) {
float P[3] = {
pointcloud->float_data[i * 4],
pointcloud->float_data[i * 4 + 1],
pointcloud->float_data[i * 4 + 2]
};
//form covariance matrix
std::vector<int> neighbors;
pointcloud->kdtree->search(&neighbors,P[0],P[1],P[2],radius);
PCD::Quaternion center = pointcloud->getCentroid(&neighbors);
double cov[9] = {}; //column major
for (size_t j=0;j<neighbors.size();j++) {
float deltaP[3] = {
pointcloud->float_data[neighbors[j] * 4] - center.i,
pointcloud->float_data[neighbors[j] * 4 + 1] - center.j,
pointcloud->float_data[neighbors[j] * 4 + 2] - center.k,
};
cov[0] += deltaP[0] * deltaP[0];
cov[1] += deltaP[1] * deltaP[0];
cov[2] += deltaP[2] * deltaP[0];
cov[3] += deltaP[0] * deltaP[1];
cov[4] += deltaP[1] * deltaP[1];
cov[5] += deltaP[2] * deltaP[1];
cov[6] += deltaP[0] * deltaP[2];
cov[7] += deltaP[1] * deltaP[2];
cov[8] += deltaP[2] * deltaP[2];
}
//compute PCA
double lambda_real[3], lambda_imag[3], v[9];
eigenvalue(3,cov,lambda_real,lambda_imag,v);
//get normal and curvature
double minLambda;
int minIndex;
for (int j=0;j<3;j++) {
curvature[i] += lambda_real[j];
if (j==0 || lambda_real[j] < minLambda) {
minIndex = j;
minLambda = lambda_real[j];
}
}
curvature[i] = minLambda / curvature[i];
double N[3] = {
v[minIndex * 3],
v[minIndex * 3 + 1],
v[minIndex * 3 + 2]
};
//normal points to origin?
if (P[0] * N[0] + P[1] * N[1] + P[2] * N[2] < 0) {
norm[i * 3] = N[0];
norm[i * 3 + 1] = N[1];
norm[i * 3 + 2] = N[2];
} else {
norm[i * 3] = -N[0];
norm[i * 3 + 1] = -N[1];
norm[i * 3 + 2] = -N[2];
}
// printf("%4f,%4f,%4f: %lu neighbors %4f,%4f,%4f %4f\n",
// P[0],P[1],P[2],neighbors.size(),N[0],N[1],N[2],curvature[i]);
}
}
Normal::~Normal() {
if (norm) delete[] norm;
if (curvature) delete[] curvature;
}
//matrices are column major
void Normal::eigenvalue(int N,double* A,double* lambda_real,double* lambda_imag,double* v) {
int info,ldvl=1,ldvr=N,lwork=15*N;
double *work = new double[lwork]();
char jobvl = 'N', jobvr = 'V';
dgeev_(&jobvl,&jobvr, &N, A, &N, lambda_real, lambda_imag,
NULL,&ldvl, v, &ldvr ,work, &lwork, &info);
// printf("info: %d\n",info);
// printf("optimal: %f\n",work[0]);
if (info!=0) {
printf("Error in subroutine dgeev_ (info=%d)\n",info);
}
delete[] work;
}
//matrices are column major
void Normal::svd(int M,int N,double* A,double *U, double* S, double* VT) {
int info, lwork=5*(M>N?N:M);
double* work = new double[lwork];
char jobu = 'A', jobvt = 'A';
dgesvd_(&jobu, &jobvt, &M, &N, A, &M,
S, U, &M, VT, &N, work, &lwork, &info);
// printf("info: %d\n",info);
// printf("optimal: %f\n",work[0]);
if (info!=0) {
printf("Error in subroutine dgesvd_ (info=%d)\n",info);
}
delete[] work;
}