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
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
|
/* This file is part of the Gudhi Library. The Gudhi library
* (Geometric Understanding in Higher Dimensions) is a generic C++
* library for computational topology.
*
* Author(s): David Salinas
*
* Copyright (C) 2014 INRIA Sophia Antipolis-M�diterran�e (France)
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#ifndef GARLAND_HECKBERT_ERROR_QUADRIC_H_
#define GARLAND_HECKBERT_ERROR_QUADRIC_H_
#include <boost/optional/optional.hpp>
#include <vector>
#include <utility>
template <typename Point> class Error_quadric {
private:
double coeff[10];
public:
Error_quadric() {
clear();
}
/**
* Quadric corresponding to the L2 distance to the plane.
*
* According to the notation of Garland Heckbert, they
* denote a quadric symetric matrix as :
* Q = [ q11 q12 q13 q14]
* [ q12 q22 q23 q24]
* [ q13 q23 q33 q34]
* [ q14 q24 q34 q44]
*
* which is represented by a vector with 10 elts that
* are denoted ci for clarity with :
* Q = [ c0 c1 c2 c3 ]
* [ c1 c4 c5 c6 ]
* [ c2 c5 c7 c8 ]
* [ c3 c6 c8 c9 ]
*
* The constructor return the quadrics that represents
* the squared distance to the plane defined by triangle p0,p1,p2
* times the area of triangle p0,p1,p2.
*/
Error_quadric(const Point & p0, const Point & p1, const Point & p2) {
Point normal(unit_normal(p0, p1, p2));
double a = normal[0];
double b = normal[1];
double c = normal[2];
double d = -a * p0[0] - b * p0[1] - c * p0[2];
coeff[0] = a*a;
coeff[1] = a*b;
coeff[2] = a*c;
coeff[3] = a*d;
coeff[4] = b*b;
coeff[5] = b*c;
coeff[6] = b*d;
coeff[7] = c*c;
coeff[8] = c*d;
coeff[9] = d*d;
double area_p0p1p2 = std::sqrt(squared_area(p0, p1, p2));
for (auto& x : coeff)
x *= area_p0p1p2;
}
inline double squared_area(const Point& p0, const Point& p1, const Point& p2) {
// if (x1,x2,x3) = p1-p0 and (y1,y2,y3) = p2-p0
// then the squared area is = (u^2+v^2+w^2)/4
// with: u = x2 * y3 - x3 * y2;
// v = x3 * y1 - x1 * y3;
// w = x1 * y2 - x2 * y1;
Point p0p1(p1 - p0);
Point p0p2(p2 - p0);
double A = p0p1[1] * p0p2[2] - p0p1[2] * p0p2[1];
double B = p0p1[2] * p0p2[0] - p0p1[0] * p0p2[2];
double C = p0p1[0] * p0p2[1] - p0p1[1] * p0p2[0];
return 1. / 4. * (A * A + B * B + C * C);
}
void clear() {
for (auto& x : coeff)
x = 0;
}
Error_quadric& operator+=(const Error_quadric& other) {
if (this != &other) {
for (int i = 0; i < 10; ++i)
coeff[i] += other.coeff[i];
}
return *this;
}
/**
* @return The quadric quost defined by the scalar product v^T Q v where Q is the quadratic form of Garland/Heckbert
*/
inline double cost(const Point& point) const {
double cost =
coeff[0] * point.x() * point.x() + coeff[4] * point.y() * point.y() + coeff[7] * point.z() * point.z()
+ 2 * (coeff[1] * point.x() * point.y() + coeff[5] * point.y() * point.z() + coeff[2] * point.z() * point.x())
+ 2 * (coeff[3] * point.x() + coeff[6] * point.y() + coeff[8] * point.z())
+ coeff[9];
if (cost < 0) {
return 0;
} else {
return cost;
}
}
inline double grad_determinant() const {
return
coeff[0] * coeff[4] * coeff[7]
- coeff[0] * coeff[5] * coeff[5]
- coeff[1] * coeff[1] * coeff[7]
+ 2 * coeff[1] * coeff[5] * coeff[2]
- coeff[4] * coeff[2] * coeff[2];
}
/**
* Return the point such that it minimizes the gradient of the quadric.
* Det must be passed with the determinant value of the gradient (should be non zero).
*/
inline Point solve_linear_gradient(double det) const {
return Point({
(-coeff[1] * coeff[5] * coeff[8] + coeff[1] * coeff[7] * coeff[6] + coeff[2] * coeff[8] * coeff[4] -
coeff[2] * coeff[5] * coeff[6] - coeff[3] * coeff[4] * coeff[7] + coeff[3] * coeff[5] * coeff[5])
/ det,
(coeff[0] * coeff[5] * coeff[8] - coeff[0] * coeff[7] * coeff[6] - coeff[5] * coeff[2] * coeff[3] -
coeff[1] * coeff[2] * coeff[8] + coeff[6] * coeff[2] * coeff[2] + coeff[1] * coeff[3] * coeff[7])
/ det,
(-coeff[8] * coeff[0] * coeff[4] + coeff[8] * coeff[1] * coeff[1] + coeff[2] * coeff[3] * coeff[4] +
coeff[5] * coeff[0] * coeff[6] - coeff[5] * coeff[1] * coeff[3] - coeff[1] * coeff[2] * coeff[6])
/ det
});
}
/**
* returns the point that minimizes the quadric.
* It inverses the quadric if its determinant is higher that a given threshold .
* If the determinant is lower than this value the returned value is uninitialized.
*/
boost::optional<Point> min_cost(double scale = 1) const {
// const double min_determinant = 1e-4 * scale*scale;
const double min_determinant = 1e-5;
boost::optional<Point> pt_res;
double det = grad_determinant();
if (std::abs(det) > min_determinant)
pt_res = solve_linear_gradient(det);
return pt_res;
}
friend std::ostream& operator<<(std::ostream& stream, const Error_quadric& quadric) {
stream << "\n[ " << quadric.coeff[0] << "," << quadric.coeff[1] << "," << quadric.coeff[2] << "," <<
quadric.coeff[3] << ";\n";
stream << " " << quadric.coeff[1] << "," << quadric.coeff[4] << "," << quadric.coeff[5] << "," <<
quadric.coeff[6] << ";\n";
stream << " " << quadric.coeff[2] << "," << quadric.coeff[5] << "," << quadric.coeff[7] << "," <<
quadric.coeff[8] << ";\n";
stream << " " << quadric.coeff[3] << "," << quadric.coeff[6] << "," << quadric.coeff[8] << "," <<
quadric.coeff[9] << "]";
return stream;
}
};
#endif // GARLAND_HECKBERT_ERROR_QUADRIC_H_
|
