cgv
Loading...
Searching...
No Matches
normal_estimation.cxx
1#include "normal_estimation.h"
2
3#include <cgv/math/mat.h>
4#include <cgv/math/eig.h>
5#include <cgv/math/point_operations.h>
6
7namespace cgv {
8 namespace math {
9
10void estimate_normal_ls(unsigned nr_points, const float* _points, float* _normal, float* _evals, float* _mean, float* _evecs)
11{
12 cgv::math::mat<float> points;
13 points.set_extern_data(3, nr_points, const_cast<float*>(_points));
14
15 cgv::math::vec<float> normal;
16 normal.set_extern_data(3, _normal);
17
18 cgv::math::mat<float> covmat;
19 cgv::math::vec<float> mean;
20
21 cgv::math::covmat_and_mean(points,covmat,mean);
22 cgv::math::mat<double> dcovmat(covmat), v;
23 cgv::math::diag_mat<double> d;
24 cgv::math::eig_sym(dcovmat,v,d);
25
26 normal = cgv::math::vec<float>(normalize(v.col(2)));
27 if (_evals) {
28 _evals[0] = (float)d(0);
29 _evals[1] = (float)d(1);
30 _evals[2] = (float)d(2);
31 }
32 if (_mean) {
33 _mean[0] = (float)mean(0);
34 _mean[1] = (float)mean(1);
35 _mean[2] = (float)mean(2);
36 }
37 if (_evecs) {
38 _evecs[0] = (float)v(0, 0);
39 _evecs[1] = (float)v(1, 0);
40 _evecs[2] = (float)v(2, 0);
41 _evecs[3] = (float)v(0, 1);
42 _evecs[4] = (float)v(1, 1);
43 _evecs[5] = (float)v(2, 1);
44 _evecs[6] = (float)v(0, 2);
45 _evecs[7] = (float)v(1, 2);
46 _evecs[8] = (float)v(2, 2);
47 }
48}
49
50void estimate_normal_wls(unsigned nr_points, const float* _points, const float* _weights, float* _normal, float* _evals, float* _mean, float* _evecs)
51{
52 cgv::math::mat<float> points;
53 points.set_extern_data(3, nr_points, const_cast<float*>(_points));
54 cgv::math::vec<float> weights;
55 weights.set_extern_data(nr_points, const_cast<float*>(_weights));
56 cgv::math::vec<float> normal;
57 normal.set_extern_data(3, _normal);
58
59 cgv::math::mat<float> covmat;
60 cgv::math::vec<float> mean;
61
62
63
64 cgv::math::weighted_covmat_and_mean(weights,points,covmat,mean);
65 cgv::math::mat<double> dcovmat(covmat), v;
66 cgv::math::diag_mat<double> d;
67 cgv::math::eig_sym(dcovmat,v,d);
68
69 normal = cgv::math::vec<float>(normalize(v.col(2)));
70
71 if (_evals) {
72 _evals[0] = (float)d(0);
73 _evals[1] = (float)d(1);
74 _evals[2] = (float)d(2);
75 }
76 if (_mean) {
77 _mean[0] = (float)mean(0);
78 _mean[1] = (float)mean(1);
79 _mean[2] = (float)mean(2);
80 }
81 if (_evecs) {
82 _evecs[0] = (float)v(0, 0);
83 _evecs[1] = (float)v(1, 0);
84 _evecs[2] = (float)v(2, 0);
85 _evecs[3] = (float)v(0, 1);
86 _evecs[4] = (float)v(1, 1);
87 _evecs[5] = (float)v(2, 1);
88 _evecs[6] = (float)v(0, 2);
89 _evecs[7] = (float)v(1, 2);
90 _evecs[8] = (float)v(2, 2);
91 }
92}
93
94 }
95}
cgv::math::mat
A matrix type (full column major storage) The matrix can be loaded directly into OpenGL without need ...
Definition mat.h:208
cgv::math::mat::col
const vec< T > col(unsigned j) const
extract a column of the matrix as a vector
Definition mat.h:833
cgv::math::mat::set_extern_data
void set_extern_data(unsigned nrows, unsigned ncols, T *data)
set data pointer to an external data array
Definition mat.h:523
cgv::math::vec
A column vector class.
Definition vec.h:28
cgv::math::vec::set_extern_data
void set_extern_data(unsigned dim, T *data)
set data pointer to an external data array
Definition vec.h:168
cgv
the cgv namespace
Definition print.h:11
cgv::math::diag_mat
A diagonal matrix type which internally stores the elements on the main diagonal in a vector.
Definition diag_mat.h:16