Commit 465a3dc3 authored by Philip Trettner's avatar Philip Trettner
Browse files

improved quadric API

parent f1e5a051
...@@ -55,9 +55,9 @@ TG_NODISCARD constexpr pos<3, ScalarT> closest_point(quadric<3, ScalarT> const& ...@@ -55,9 +55,9 @@ TG_NODISCARD constexpr pos<3, ScalarT> closest_point(quadric<3, ScalarT> const&
auto d = q.A11; auto d = q.A11;
auto e = q.A12; auto e = q.A12;
auto f = q.A22; auto f = q.A22;
auto r0 = q.r.x; auto r0 = q.b0;
auto r1 = q.r.y; auto r1 = q.b1;
auto r2 = q.r.z; auto r2 = q.b2;
auto ad = a * d; auto ad = a * d;
auto ae = a * e; auto ae = a * e;
...@@ -92,8 +92,8 @@ TG_NODISCARD constexpr pos<2, ScalarT> closest_point(quadric<2, ScalarT> const& ...@@ -92,8 +92,8 @@ TG_NODISCARD constexpr pos<2, ScalarT> closest_point(quadric<2, ScalarT> const&
auto c = q.A11; auto c = q.A11;
auto denom = 1 / (a * c - b * b); auto denom = 1 / (a * c - b * b);
auto nom0 = q.r.x * c - q.r.y * b; auto nom0 = q.b0 * c - q.b1 * b;
auto nom1 = q.r.y * a - q.r.x * b; auto nom1 = q.b1 * a - q.b0 * b;
return {nom0 * denom, nom1 * denom}; return {nom0 * denom, nom1 * denom};
} }
......
...@@ -16,18 +16,15 @@ namespace tg ...@@ -16,18 +16,15 @@ namespace tg
template <int D, class ScalarT> template <int D, class ScalarT>
TG_NODISCARD constexpr quadric<D, ScalarT> point_quadric(pos<D, ScalarT> const& p) TG_NODISCARD constexpr quadric<D, ScalarT> point_quadric(pos<D, ScalarT> const& p)
{ {
quadric<D, ScalarT> Q; auto const v = tg::vec<D, ScalarT>(p);
Q.add(mat<D, D, ScalarT>::identity, vec<D, ScalarT>(p), ScalarT(0)); return quadric<D, ScalarT>::from_coefficients(mat<D, D, ScalarT>::identity, v, dot(v, v));
return Q;
} }
template <int D, class ScalarT> template <int D, class ScalarT>
TG_NODISCARD constexpr quadric<D, ScalarT> plane_quadric(pos<D, ScalarT> const& p, vec<D, ScalarT> const& n) TG_NODISCARD constexpr quadric<D, ScalarT> plane_quadric(pos<D, ScalarT> const& p, vec<D, ScalarT> const& n)
{ {
auto const d = dot(p, n); auto const d = dot(p, n);
quadric<D, ScalarT> Q; return quadric<D, ScalarT>::from_coefficients(self_outer_product(n), n * d, d * d);
Q.add(self_outer_product(n), n * d, d * d);
return Q;
} }
template <int D, class ScalarT> template <int D, class ScalarT>
...@@ -49,9 +46,7 @@ TG_NODISCARD constexpr quadric<D, ScalarT> probabilistic_plane_quadric(pos<D, Sc ...@@ -49,9 +46,7 @@ TG_NODISCARD constexpr quadric<D, ScalarT> probabilistic_plane_quadric(pos<D, Sc
auto const b = mean_n * d + sn2 * p; auto const b = mean_n * d + sn2 * p;
auto const c = d * d + sn2 * dot(p, p) + sp2 * dot(mean_n, mean_n) + sp2 * sn2; auto const c = d * d + sn2 * dot(p, p) + sp2 * dot(mean_n, mean_n) + sp2 * sn2;
quadric<D, ScalarT> Q; return quadric<D, ScalarT>::from_coefficients(A, b, c);
Q.add(A, b, c);
return Q;
} }
template <int D, class ScalarT> template <int D, class ScalarT>
...@@ -67,23 +62,19 @@ TG_NODISCARD constexpr quadric<D, ScalarT> probabilistic_plane_quadric(pos<D, Sc ...@@ -67,23 +62,19 @@ TG_NODISCARD constexpr quadric<D, ScalarT> probabilistic_plane_quadric(pos<D, Sc
auto const b = mean_n * d + sigma_n * p; auto const b = mean_n * d + sigma_n * p;
auto const c = d * d + dot(p, sigma_n * p) + dot(mean_n, sigma_p * mean_n) + trace_of_product(sigma_n, sigma_p); auto const c = d * d + dot(p, sigma_n * p) + dot(mean_n, sigma_p * mean_n) + trace_of_product(sigma_n, sigma_p);
quadric<D, ScalarT> Q; return quadric<D, ScalarT>::from_coefficients(A, b, c);
Q.add(A, b, c);
return Q;
} }
template <int D, class ScalarT> template <int D, class ScalarT>
TG_NODISCARD constexpr quadric<D, ScalarT> triangle_quadric(pos<D, ScalarT> const& p, pos<D, ScalarT> const& q, pos<D, ScalarT> const& r) TG_NODISCARD constexpr quadric<D, ScalarT> triangle_quadric(pos<D, ScalarT> const& p, pos<D, ScalarT> const& q, pos<D, ScalarT> const& r)
{ {
auto const pxq = cross(p, q); auto const pxq = cross(vec(p), vec(q));
auto const qxr = cross(q, r); auto const qxr = cross(vec(q), vec(r));
auto const rxp = cross(r, p); auto const rxp = cross(vec(r), vec(p));
auto const xsum = pxq + qxr + rxp; auto const xsum = pxq + qxr + rxp;
auto const det = dot(pxq, r); auto const det = dot(pxq, r);
quadric<D, ScalarT> Q; return quadric<D, ScalarT>::from_coefficients(self_outer_product(xsum), xsum * det, det * det);
Q.add(self_outer_product(xsum), xsum * det, det * det);
return Q;
} }
} }
...@@ -35,18 +35,19 @@ template <class ScalarT> ...@@ -35,18 +35,19 @@ template <class ScalarT>
struct quadric<2, ScalarT> struct quadric<2, ScalarT>
{ {
using scalar_t = ScalarT; using scalar_t = ScalarT;
using vec2_t = tg::vec<2, ScalarT>; using vec_t = tg::vec<2, ScalarT>;
using pos2_t = tg::pos<2, ScalarT>; using pos_t = tg::pos<2, ScalarT>;
using dir2_t = tg::dir<2, ScalarT>;
private: // x^T A x - 2 b^T x + c
scalar_t A00 = 0; public:
scalar_t A01 = 0; scalar_t A00 = scalar_t(0);
scalar_t A11 = 0; scalar_t A01 = scalar_t(0);
scalar_t A11 = scalar_t(0);
vec2_t r = vec2_t::zero; scalar_t b0 = scalar_t(0);
scalar_t b1 = scalar_t(0);
scalar_t d_sqr = 0; scalar_t c = scalar_t(0);
public: public:
constexpr quadric() = default; constexpr quadric() = default;
...@@ -61,19 +62,20 @@ public: ...@@ -61,19 +62,20 @@ public:
/// Adds a line (pos, normal) with a given uncertainty sigma /// Adds a line (pos, normal) with a given uncertainty sigma
/// `normal` should be normalized /// `normal` should be normalized
/// `sigma` is the standard deviation of the normal distribution added to `normal` /// `sigma` is the standard deviation of the normal distribution added to `normal`
void add_line(pos2_t pos, vec2_t normal, scalar_t sigma) void add_line(pos_t pos, vec_t normal, scalar_t sigma)
{ {
auto d = dot(pos - pos2_t::zero, normal); auto d = dot(pos - pos_t::zero, normal);
auto s2 = sigma * sigma; auto s2 = sigma * sigma;
A00 += normal.x * normal.x + s2; A00 += normal.x * normal.x + s2;
A01 += normal.x * normal.y; A01 += normal.x * normal.y;
A11 += normal.y * normal.y + s2; A11 += normal.y * normal.y + s2;
r += normal * d + vec2_t(pos) * s2; b0 += normal.x * d + pos.x * s2;
b1 += normal.y * d + pos.y * s2;
// TODO: maybe positional uncertainty // TODO: maybe positional uncertainty
d_sqr += d * d + distance_sqr_to_origin(pos) * s2; c += d * d + distance_sqr_to_origin(pos) * s2;
} }
// add two quadrics // add two quadrics
...@@ -83,9 +85,10 @@ public: ...@@ -83,9 +85,10 @@ public:
A01 += rhs.A01; A01 += rhs.A01;
A11 += rhs.A11; A11 += rhs.A11;
r += rhs.r; b0 += rhs.b0;
b1 += rhs.b1;
d_sqr += rhs.d_sqr; c += rhs.c;
} }
// Residual L2 error as given by x^T A x - 2 r^T x + c // Residual L2 error as given by x^T A x - 2 r^T x + c
...@@ -96,9 +99,9 @@ public: ...@@ -96,9 +99,9 @@ public:
A01 * p.x + A11 * p.y, // A01 * p.x + A11 * p.y, //
}; };
return dot(vec<2, ScalarT>(p), Ax) // x^T A x return dot(vec<2, ScalarT>(p), Ax) // x^T A x
- 2 * dot(vec<2, ScalarT>(p), r) // - 2 r^T x - 2 * (b0 * p.x + b1 * p.y) // - 2 r^T x
+ d_sqr; // + c + c; // + c
} }
// TODO: fix me // TODO: fix me
...@@ -112,21 +115,23 @@ template <class ScalarT> ...@@ -112,21 +115,23 @@ template <class ScalarT>
struct quadric<3, ScalarT> struct quadric<3, ScalarT>
{ {
using scalar_t = ScalarT; using scalar_t = ScalarT;
using vec3_t = tg::vec<3, ScalarT>; using vec_t = tg::vec<3, ScalarT>;
using pos3_t = tg::pos<3, ScalarT>; using pos_t = tg::pos<3, ScalarT>;
using dir3_t = tg::dir<3, ScalarT>;
private: // x^T A x - 2 b^T x + c
scalar_t A00 = 0; public:
scalar_t A01 = 0; scalar_t A00 = scalar_t(0);
scalar_t A02 = 0; scalar_t A01 = scalar_t(0);
scalar_t A11 = 0; scalar_t A02 = scalar_t(0);
scalar_t A12 = 0; scalar_t A11 = scalar_t(0);
scalar_t A22 = 0; scalar_t A12 = scalar_t(0);
scalar_t A22 = scalar_t(0);
vec3_t r = vec3_t::zero; scalar_t b0 = scalar_t(0);
scalar_t b1 = scalar_t(0);
scalar_t b2 = scalar_t(0);
scalar_t d_sqr = 0; scalar_t c = scalar_t(0);
public: public:
constexpr quadric() = default; constexpr quadric() = default;
...@@ -144,10 +149,10 @@ public: ...@@ -144,10 +149,10 @@ public:
q.A11 = A11; q.A11 = A11;
q.A12 = A12; q.A12 = A12;
q.A22 = A22; q.A22 = A22;
q.r.x = b0; q.b0 = b0;
q.r.y = b1; q.b1 = b1;
q.r.z = b2; q.b2 = b2;
q.d_sqr = c; q.c = c;
return q; return q;
} }
static constexpr quadric from_coefficients(tg::mat3 const& A, tg::vec3 const& b, float c) static constexpr quadric from_coefficients(tg::mat3 const& A, tg::vec3 const& b, float c)
...@@ -159,15 +164,15 @@ public: ...@@ -159,15 +164,15 @@ public:
q.A11 = A[1][1]; q.A11 = A[1][1];
q.A12 = A[1][2]; q.A12 = A[1][2];
q.A22 = A[2][2]; q.A22 = A[2][2];
q.r.x = b[0]; q.b0 = b[0];
q.r.y = b[1]; q.b1 = b[1];
q.r.z = b[2]; q.b2 = b[2];
q.d_sqr = c; q.c = c;
return q; return q;
} }
public: public:
constexpr mat<3, 3, ScalarT> A() const constexpr mat<3, 3, ScalarT> const A() const
{ {
mat<3, 3, ScalarT> m; mat<3, 3, ScalarT> m;
m[0][0] = A00; m[0][0] = A00;
...@@ -181,17 +186,16 @@ public: ...@@ -181,17 +186,16 @@ public:
m[2][2] = A22; m[2][2] = A22;
return m; return m;
} }
constexpr vec<3, ScalarT> b() const { return r; } constexpr vec<3, ScalarT> const b() const { return {b0, b1, b2}; }
constexpr scalar_t c() const { return d_sqr; }
// TODO: find better place for these? // TODO: find better place for these?
public: public:
/// Adds a plane (pos, normal) with a given uncertainty sigma /// Adds a plane (pos, normal) with a given uncertainty sigma
/// `normal` should be normalized /// `normal` should be normalized
/// `sigma` is the standard deviation of the normal distribution added to `normal` /// `sigma` is the standard deviation of the normal distribution added to `normal`
void add_plane(pos3_t pos, vec3_t normal, scalar_t sigma) void add_plane(pos_t pos, vec_t normal, scalar_t sigma)
{ {
auto d = dot(pos - pos3_t::zero, normal); auto d = dot(pos - pos_t::zero, normal);
auto s2 = sigma * sigma; auto s2 = sigma * sigma;
A00 += normal.x * normal.x + s2; A00 += normal.x * normal.x + s2;
...@@ -201,10 +205,12 @@ public: ...@@ -201,10 +205,12 @@ public:
A12 += normal.y * normal.z; A12 += normal.y * normal.z;
A22 += normal.z * normal.z + s2; A22 += normal.z * normal.z + s2;
r += normal * d + vec3_t(pos) * s2; b0 += normal.x * d + pos.x * s2;
b1 += normal.y * d + pos.y * s2;
b2 += normal.z * d + pos.z * s2;
// TODO: maybe positional uncertainty // TODO: maybe positional uncertainty
d_sqr += d * d + distance_sqr_to_origin(pos) * s2; c += d * d + distance_sqr_to_origin(pos) * s2;
} }
/// Adds two quadrics /// Adds two quadrics
...@@ -217,9 +223,11 @@ public: ...@@ -217,9 +223,11 @@ public:
A12 += rhs.A12; A12 += rhs.A12;
A22 += rhs.A22; A22 += rhs.A22;
r += rhs.r; b0 += rhs.b0;
b1 += rhs.b1;
b2 += rhs.b2;
d_sqr += rhs.d_sqr; c += rhs.c;
} }
// adds a quadric given in matrix form (x^T A x + b^T x + c) // adds a quadric given in matrix form (x^T A x + b^T x + c)
...@@ -233,9 +241,11 @@ public: ...@@ -233,9 +241,11 @@ public:
A12 += A[1][2]; A12 += A[1][2];
A22 += A[2][2]; A22 += A[2][2];
r += b; b0 += b.x;
b1 += b.y;
b2 += b.z;
d_sqr += c; c += c;
} }
// Residual L2 error as given by x^T A x - 2 r^T x + c // Residual L2 error as given by x^T A x - 2 r^T x + c
...@@ -247,9 +257,9 @@ public: ...@@ -247,9 +257,9 @@ public:
A02 * p.x + A12 * p.y + A22 * p.z, // A02 * p.x + A12 * p.y + A22 * p.z, //
}; };
return dot(vec<3, ScalarT>(p), Ax) // x^T A x return dot(vec<3, ScalarT>(p), Ax) // x^T A x
- 2 * dot(vec<3, ScalarT>(p), r) // - 2 r^T x - 2 * (p.x * b0 + p.y * b1 + p.z * b2) // - 2 r^T x
+ d_sqr; // + c + c; // + c
} }
// TODO: fix me // TODO: fix me
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment