[PATCH v6 2/4] ipa: libipa: Copy pwl from rpi
Paul Elder
paul.elder at ideasonboard.com
Fri Jun 7 10:42:59 CEST 2024
Copy the piecewise linear function code from Raspberry Pi.
Signed-off-by: Paul Elder <paul.elder at ideasonboard.com>
Reviewed-by: Stefan Klug <stefan.klug at ideasonboard.com>
Acked-by: David Plowman <david.plowman at raspberrypi.com>
Reviewed-by: Kieran Bingham <kieran.bingham at ideasonboard.com>
---
No change in v6
Changes in v5:
- remove meson.build to prevent compilation this early in the merge
Changes in v4:
- update the copy
No change in v3
No change in v2
---
src/ipa/libipa/pwl.cpp | 269 +++++++++++++++++++++++++++++++++++++++++
src/ipa/libipa/pwl.h | 127 +++++++++++++++++++
2 files changed, 396 insertions(+)
create mode 100644 src/ipa/libipa/pwl.cpp
create mode 100644 src/ipa/libipa/pwl.h
diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp
new file mode 100644
index 000000000..e39123767
--- /dev/null
+++ b/src/ipa/libipa/pwl.cpp
@@ -0,0 +1,269 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ *
+ * piecewise linear functions
+ */
+
+#include <cassert>
+#include <cmath>
+#include <stdexcept>
+
+#include "pwl.h"
+
+using namespace RPiController;
+
+int Pwl::read(const libcamera::YamlObject ¶ms)
+{
+ if (!params.size() || params.size() % 2)
+ return -EINVAL;
+
+ const auto &list = params.asList();
+
+ for (auto it = list.begin(); it != list.end(); it++) {
+ auto x = it->get<double>();
+ if (!x)
+ return -EINVAL;
+ if (it != list.begin() && *x <= points_.back().x)
+ return -EINVAL;
+
+ auto y = (++it)->get<double>();
+ if (!y)
+ return -EINVAL;
+
+ points_.push_back(Point(*x, *y));
+ }
+
+ return 0;
+}
+
+void Pwl::append(double x, double y, const double eps)
+{
+ if (points_.empty() || points_.back().x + eps < x)
+ points_.push_back(Point(x, y));
+}
+
+void Pwl::prepend(double x, double y, const double eps)
+{
+ if (points_.empty() || points_.front().x - eps > x)
+ points_.insert(points_.begin(), Point(x, y));
+}
+
+Pwl::Interval Pwl::domain() const
+{
+ return Interval(points_[0].x, points_[points_.size() - 1].x);
+}
+
+Pwl::Interval Pwl::range() const
+{
+ double lo = points_[0].y, hi = lo;
+ for (auto &p : points_)
+ lo = std::min(lo, p.y), hi = std::max(hi, p.y);
+ return Interval(lo, hi);
+}
+
+bool Pwl::empty() const
+{
+ return points_.empty();
+}
+
+double Pwl::eval(double x, int *spanPtr, bool updateSpan) const
+{
+ int span = findSpan(x, spanPtr && *spanPtr != -1 ? *spanPtr : points_.size() / 2 - 1);
+ if (spanPtr && updateSpan)
+ *spanPtr = span;
+ return points_[span].y +
+ (x - points_[span].x) * (points_[span + 1].y - points_[span].y) /
+ (points_[span + 1].x - points_[span].x);
+}
+
+int Pwl::findSpan(double x, int span) const
+{
+ /*
+ * Pwls are generally small, so linear search may well be faster than
+ * binary, though could review this if large PWls start turning up.
+ */
+ int lastSpan = points_.size() - 2;
+ /*
+ * some algorithms may call us with span pointing directly at the last
+ * control point
+ */
+ span = std::max(0, std::min(lastSpan, span));
+ while (span < lastSpan && x >= points_[span + 1].x)
+ span++;
+ while (span && x < points_[span].x)
+ span--;
+ return span;
+}
+
+Pwl::PerpType Pwl::invert(Point const &xy, Point &perp, int &span,
+ const double eps) const
+{
+ assert(span >= -1);
+ bool prevOffEnd = false;
+ for (span = span + 1; span < (int)points_.size() - 1; span++) {
+ Point spanVec = points_[span + 1] - points_[span];
+ double t = ((xy - points_[span]) % spanVec) / spanVec.len2();
+ if (t < -eps) /* off the start of this span */
+ {
+ if (span == 0) {
+ perp = points_[span];
+ return PerpType::Start;
+ } else if (prevOffEnd) {
+ perp = points_[span];
+ return PerpType::Vertex;
+ }
+ } else if (t > 1 + eps) /* off the end of this span */
+ {
+ if (span == (int)points_.size() - 2) {
+ perp = points_[span + 1];
+ return PerpType::End;
+ }
+ prevOffEnd = true;
+ } else /* a true perpendicular */
+ {
+ perp = points_[span] + spanVec * t;
+ return PerpType::Perpendicular;
+ }
+ }
+ return PerpType::None;
+}
+
+Pwl Pwl::inverse(bool *trueInverse, const double eps) const
+{
+ bool appended = false, prepended = false, neither = false;
+ Pwl inverse;
+
+ for (Point const &p : points_) {
+ if (inverse.empty())
+ inverse.append(p.y, p.x, eps);
+ else if (std::abs(inverse.points_.back().x - p.y) <= eps ||
+ std::abs(inverse.points_.front().x - p.y) <= eps)
+ /* do nothing */;
+ else if (p.y > inverse.points_.back().x) {
+ inverse.append(p.y, p.x, eps);
+ appended = true;
+ } else if (p.y < inverse.points_.front().x) {
+ inverse.prepend(p.y, p.x, eps);
+ prepended = true;
+ } else
+ neither = true;
+ }
+
+ /*
+ * This is not a proper inverse if we found ourselves putting points
+ * onto both ends of the inverse, or if there were points that couldn't
+ * go on either.
+ */
+ if (trueInverse)
+ *trueInverse = !(neither || (appended && prepended));
+
+ return inverse;
+}
+
+Pwl Pwl::compose(Pwl const &other, const double eps) const
+{
+ double thisX = points_[0].x, thisY = points_[0].y;
+ int thisSpan = 0, otherSpan = other.findSpan(thisY, 0);
+ Pwl result({ { thisX, other.eval(thisY, &otherSpan, false) } });
+ while (thisSpan != (int)points_.size() - 1) {
+ double dx = points_[thisSpan + 1].x - points_[thisSpan].x,
+ dy = points_[thisSpan + 1].y - points_[thisSpan].y;
+ if (std::abs(dy) > eps &&
+ otherSpan + 1 < (int)other.points_.size() &&
+ points_[thisSpan + 1].y >=
+ other.points_[otherSpan + 1].x + eps) {
+ /*
+ * next control point in result will be where this
+ * function's y reaches the next span in other
+ */
+ thisX = points_[thisSpan].x +
+ (other.points_[otherSpan + 1].x -
+ points_[thisSpan].y) *
+ dx / dy;
+ thisY = other.points_[++otherSpan].x;
+ } else if (std::abs(dy) > eps && otherSpan > 0 &&
+ points_[thisSpan + 1].y <=
+ other.points_[otherSpan - 1].x - eps) {
+ /*
+ * next control point in result will be where this
+ * function's y reaches the previous span in other
+ */
+ thisX = points_[thisSpan].x +
+ (other.points_[otherSpan + 1].x -
+ points_[thisSpan].y) *
+ dx / dy;
+ thisY = other.points_[--otherSpan].x;
+ } else {
+ /* we stay in the same span in other */
+ thisSpan++;
+ thisX = points_[thisSpan].x,
+ thisY = points_[thisSpan].y;
+ }
+ result.append(thisX, other.eval(thisY, &otherSpan, false),
+ eps);
+ }
+ return result;
+}
+
+void Pwl::map(std::function<void(double x, double y)> f) const
+{
+ for (auto &pt : points_)
+ f(pt.x, pt.y);
+}
+
+void Pwl::map2(Pwl const &pwl0, Pwl const &pwl1,
+ std::function<void(double x, double y0, double y1)> f)
+{
+ int span0 = 0, span1 = 0;
+ double x = std::min(pwl0.points_[0].x, pwl1.points_[0].x);
+ f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false));
+ while (span0 < (int)pwl0.points_.size() - 1 ||
+ span1 < (int)pwl1.points_.size() - 1) {
+ if (span0 == (int)pwl0.points_.size() - 1)
+ x = pwl1.points_[++span1].x;
+ else if (span1 == (int)pwl1.points_.size() - 1)
+ x = pwl0.points_[++span0].x;
+ else if (pwl0.points_[span0 + 1].x > pwl1.points_[span1 + 1].x)
+ x = pwl1.points_[++span1].x;
+ else
+ x = pwl0.points_[++span0].x;
+ f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false));
+ }
+}
+
+Pwl Pwl::combine(Pwl const &pwl0, Pwl const &pwl1,
+ std::function<double(double x, double y0, double y1)> f,
+ const double eps)
+{
+ Pwl result;
+ map2(pwl0, pwl1, [&](double x, double y0, double y1) {
+ result.append(x, f(x, y0, y1), eps);
+ });
+ return result;
+}
+
+void Pwl::matchDomain(Interval const &domain, bool clip, const double eps)
+{
+ int span = 0;
+ prepend(domain.start, eval(clip ? points_[0].x : domain.start, &span),
+ eps);
+ span = points_.size() - 2;
+ append(domain.end, eval(clip ? points_.back().x : domain.end, &span),
+ eps);
+}
+
+Pwl &Pwl::operator*=(double d)
+{
+ for (auto &pt : points_)
+ pt.y *= d;
+ return *this;
+}
+
+void Pwl::debug(FILE *fp) const
+{
+ fprintf(fp, "Pwl {\n");
+ for (auto &p : points_)
+ fprintf(fp, "\t(%g, %g)\n", p.x, p.y);
+ fprintf(fp, "}\n");
+}
diff --git a/src/ipa/libipa/pwl.h b/src/ipa/libipa/pwl.h
new file mode 100644
index 000000000..7d5e7e4d3
--- /dev/null
+++ b/src/ipa/libipa/pwl.h
@@ -0,0 +1,127 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ *
+ * piecewise linear functions interface
+ */
+#pragma once
+
+#include <functional>
+#include <math.h>
+#include <vector>
+
+#include "libcamera/internal/yaml_parser.h"
+
+namespace RPiController {
+
+class Pwl
+{
+public:
+ struct Interval {
+ Interval(double _start, double _end)
+ : start(_start), end(_end)
+ {
+ }
+ double start, end;
+ bool contains(double value)
+ {
+ return value >= start && value <= end;
+ }
+ double clip(double value)
+ {
+ return value < start ? start
+ : (value > end ? end : value);
+ }
+ double len() const { return end - start; }
+ };
+ struct Point {
+ Point() : x(0), y(0) {}
+ Point(double _x, double _y)
+ : x(_x), y(_y) {}
+ double x, y;
+ Point operator-(Point const &p) const
+ {
+ return Point(x - p.x, y - p.y);
+ }
+ Point operator+(Point const &p) const
+ {
+ return Point(x + p.x, y + p.y);
+ }
+ double operator%(Point const &p) const
+ {
+ return x * p.x + y * p.y;
+ }
+ Point operator*(double f) const { return Point(x * f, y * f); }
+ Point operator/(double f) const { return Point(x / f, y / f); }
+ double len2() const { return x * x + y * y; }
+ double len() const { return sqrt(len2()); }
+ };
+ Pwl() {}
+ Pwl(std::vector<Point> const &points) : points_(points) {}
+ int read(const libcamera::YamlObject ¶ms);
+ void append(double x, double y, const double eps = 1e-6);
+ void prepend(double x, double y, const double eps = 1e-6);
+ Interval domain() const;
+ Interval range() const;
+ bool empty() const;
+ /*
+ * Evaluate Pwl, optionally supplying an initial guess for the
+ * "span". The "span" may be optionally be updated. If you want to know
+ * the "span" value but don't have an initial guess you can set it to
+ * -1.
+ */
+ double eval(double x, int *spanPtr = nullptr,
+ bool updateSpan = true) const;
+ /*
+ * Find perpendicular closest to xy, starting from span+1 so you can
+ * call it repeatedly to check for multiple closest points (set span to
+ * -1 on the first call). Also returns "pseudo" perpendiculars; see
+ * PerpType enum.
+ */
+ enum class PerpType {
+ None, /* no perpendicular found */
+ Start, /* start of Pwl is closest point */
+ End, /* end of Pwl is closest point */
+ Vertex, /* vertex of Pwl is closest point */
+ Perpendicular /* true perpendicular found */
+ };
+ PerpType invert(Point const &xy, Point &perp, int &span,
+ const double eps = 1e-6) const;
+ /*
+ * Compute the inverse function. Indicate if it is a proper (true)
+ * inverse, or only a best effort (e.g. input was non-monotonic).
+ */
+ Pwl inverse(bool *trueInverse = nullptr, const double eps = 1e-6) const;
+ /* Compose two Pwls together, doing "this" first and "other" after. */
+ Pwl compose(Pwl const &other, const double eps = 1e-6) const;
+ /* Apply function to (x,y) values at every control point. */
+ void map(std::function<void(double x, double y)> f) const;
+ /*
+ * Apply function to (x, y0, y1) values wherever either Pwl has a
+ * control point.
+ */
+ static void map2(Pwl const &pwl0, Pwl const &pwl1,
+ std::function<void(double x, double y0, double y1)> f);
+ /*
+ * Combine two Pwls, meaning we create a new Pwl where the y values are
+ * given by running f wherever either has a knot.
+ */
+ static Pwl
+ combine(Pwl const &pwl0, Pwl const &pwl1,
+ std::function<double(double x, double y0, double y1)> f,
+ const double eps = 1e-6);
+ /*
+ * Make "this" match (at least) the given domain. Any extension my be
+ * clipped or linear.
+ */
+ void matchDomain(Interval const &domain, bool clip = true,
+ const double eps = 1e-6);
+ Pwl &operator*=(double d);
+ void debug(FILE *fp = stdout) const;
+
+private:
+ int findSpan(double x, int span) const;
+ std::vector<Point> points_;
+};
+
+} /* namespace RPiController */
--
2.39.2
More information about the libcamera-devel
mailing list