| Message ID | 20240405080259.1806453-3-paul.elder@ideasonboard.com |
|---|---|
| State | Superseded |
| Headers | show |
| Series |
|
| Related | show |
Hi Paul, On Fri, 5 Apr 2024 at 09:03, Paul Elder <paul.elder@ideasonboard.com> wrote: > > Copy the piecewise linear function code from Raspberry Pi. > This seems like a good time to mention something I was intending on doing for a while... Our PWL library is used internally in a few places (libcamera/libpisp/rpicam-apps/some internal tools). Right now each of these libraries has a copy of pretty much the same pwl.c/pwl.h file. I was intending to create a stand alone Raspberry Pi PWL library that could be linked with these so we only have one canonical copy of the source code. This could then be included into libcamera/libpipa as a subproject if needed. Do you think this would be helpful and suitable for you? We will need to look at possibly templating the Point type if needed as well to be more appropriate for libcamera's use. Regards, Naush > Signed-off-by: Paul Elder <paul.elder@ideasonboard.com> > --- > src/ipa/libipa/meson.build | 2 + > src/ipa/libipa/pwl.cpp | 267 +++++++++++++++++++++++++++++++++++++ > src/ipa/libipa/pwl.h | 123 +++++++++++++++++ > 3 files changed, 392 insertions(+) > create mode 100644 src/ipa/libipa/pwl.cpp > create mode 100644 src/ipa/libipa/pwl.h > > diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build > index 31cc8d70..8fba16e1 100644 > --- a/src/ipa/libipa/meson.build > +++ b/src/ipa/libipa/meson.build > @@ -8,6 +8,7 @@ libipa_headers = files([ > 'fc_queue.h', > 'histogram.h', > 'module.h', > + 'pwl.h', > ]) > > libipa_sources = files([ > @@ -18,6 +19,7 @@ libipa_sources = files([ > 'fc_queue.cpp', > 'histogram.cpp', > 'module.cpp', > + 'pwl.cpp' > ]) > > libipa_includes = include_directories('..') > diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp > new file mode 100644 > index 00000000..09f5d65c > --- /dev/null > +++ b/src/ipa/libipa/pwl.cpp > @@ -0,0 +1,267 @@ > +/* SPDX-License-Identifier: BSD-2-Clause */ > +/* > + * Copyright (C) 2019, Raspberry Pi Ltd > + * > + * pwl.cpp - piecewise linear functions > + */ > + > +#include <cassert> > +#include <cmath> > +#include <stdexcept> > + > +#include "pwl.h" > + > +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 00000000..7a6a6452 > --- /dev/null > +++ b/src/ipa/libipa/pwl.h > @@ -0,0 +1,123 @@ > +/* SPDX-License-Identifier: BSD-2-Clause */ > +/* > + * Copyright (C) 2019, Raspberry Pi Ltd > + * > + * pwl.h - piecewise linear functions interface > + */ > +#pragma once > + > +#include <functional> > +#include <math.h> > +#include <vector> > + > +#include "libcamera/internal/yaml_parser.h" > + > +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_; > +}; > -- > 2.39.2 >
Hi Paul, thanks for the patch. As this is only a copy of existing files, there is not much to review here. Reviewed-by: Stefan Klug <stefan.klug@ideasonboard.com> Cheers, Stefan On Fri, Apr 05, 2024 at 05:02:57PM +0900, Paul Elder wrote: > Copy the piecewise linear function code from Raspberry Pi. > > Signed-off-by: Paul Elder <paul.elder@ideasonboard.com> > --- > src/ipa/libipa/meson.build | 2 + > src/ipa/libipa/pwl.cpp | 267 +++++++++++++++++++++++++++++++++++++ > src/ipa/libipa/pwl.h | 123 +++++++++++++++++ > 3 files changed, 392 insertions(+) > create mode 100644 src/ipa/libipa/pwl.cpp > create mode 100644 src/ipa/libipa/pwl.h > > diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build > index 31cc8d70..8fba16e1 100644 > --- a/src/ipa/libipa/meson.build > +++ b/src/ipa/libipa/meson.build > @@ -8,6 +8,7 @@ libipa_headers = files([ > 'fc_queue.h', > 'histogram.h', > 'module.h', > + 'pwl.h', > ]) > > libipa_sources = files([ > @@ -18,6 +19,7 @@ libipa_sources = files([ > 'fc_queue.cpp', > 'histogram.cpp', > 'module.cpp', > + 'pwl.cpp' > ]) > > libipa_includes = include_directories('..') > diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp > new file mode 100644 > index 00000000..09f5d65c > --- /dev/null > +++ b/src/ipa/libipa/pwl.cpp > @@ -0,0 +1,267 @@ > +/* SPDX-License-Identifier: BSD-2-Clause */ > +/* > + * Copyright (C) 2019, Raspberry Pi Ltd > + * > + * pwl.cpp - piecewise linear functions > + */ > + > +#include <cassert> > +#include <cmath> > +#include <stdexcept> > + > +#include "pwl.h" > + > +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 00000000..7a6a6452 > --- /dev/null > +++ b/src/ipa/libipa/pwl.h > @@ -0,0 +1,123 @@ > +/* SPDX-License-Identifier: BSD-2-Clause */ > +/* > + * Copyright (C) 2019, Raspberry Pi Ltd > + * > + * pwl.h - piecewise linear functions interface > + */ > +#pragma once > + > +#include <functional> > +#include <math.h> > +#include <vector> > + > +#include "libcamera/internal/yaml_parser.h" > + > +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_; > +}; > -- > 2.39.2 >
Hi Naush, On Mon, Apr 08, 2024 at 09:14:41AM +0100, Naushir Patuck wrote: > Hi Paul, > > On Fri, 5 Apr 2024 at 09:03, Paul Elder <paul.elder@ideasonboard.com> wrote: > > > > Copy the piecewise linear function code from Raspberry Pi. > > > > This seems like a good time to mention something I was intending on > doing for a while... > > Our PWL library is used internally in a few places > (libcamera/libpisp/rpicam-apps/some internal tools). Right now each > of these libraries has a copy of pretty much the same pwl.c/pwl.h > file. I was intending to create a stand alone Raspberry Pi PWL > library that could be linked with these so we only have one canonical > copy of the source code. This could then be included into > libcamera/libpipa as a subproject if needed. Do you think this would > be helpful and suitable for you? We will need to look at possibly Yeah that might be a good idea. (although I'm not in the position to make these decisions) In any case we need this right now so for now we'll go with this :) Thanks, Paul > templating the Point type if needed as well to be more appropriate for > libcamera's use. > > Regards, > Naush > > > Signed-off-by: Paul Elder <paul.elder@ideasonboard.com> > > --- > > src/ipa/libipa/meson.build | 2 + > > src/ipa/libipa/pwl.cpp | 267 +++++++++++++++++++++++++++++++++++++ > > src/ipa/libipa/pwl.h | 123 +++++++++++++++++ > > 3 files changed, 392 insertions(+) > > create mode 100644 src/ipa/libipa/pwl.cpp > > create mode 100644 src/ipa/libipa/pwl.h > > > > diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build > > index 31cc8d70..8fba16e1 100644 > > --- a/src/ipa/libipa/meson.build > > +++ b/src/ipa/libipa/meson.build > > @@ -8,6 +8,7 @@ libipa_headers = files([ > > 'fc_queue.h', > > 'histogram.h', > > 'module.h', > > + 'pwl.h', > > ]) > > > > libipa_sources = files([ > > @@ -18,6 +19,7 @@ libipa_sources = files([ > > 'fc_queue.cpp', > > 'histogram.cpp', > > 'module.cpp', > > + 'pwl.cpp' > > ]) > > > > libipa_includes = include_directories('..') > > diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp > > new file mode 100644 > > index 00000000..09f5d65c > > --- /dev/null > > +++ b/src/ipa/libipa/pwl.cpp > > @@ -0,0 +1,267 @@ > > +/* SPDX-License-Identifier: BSD-2-Clause */ > > +/* > > + * Copyright (C) 2019, Raspberry Pi Ltd > > + * > > + * pwl.cpp - piecewise linear functions > > + */ > > + > > +#include <cassert> > > +#include <cmath> > > +#include <stdexcept> > > + > > +#include "pwl.h" > > + > > +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 00000000..7a6a6452 > > --- /dev/null > > +++ b/src/ipa/libipa/pwl.h > > @@ -0,0 +1,123 @@ > > +/* SPDX-License-Identifier: BSD-2-Clause */ > > +/* > > + * Copyright (C) 2019, Raspberry Pi Ltd > > + * > > + * pwl.h - piecewise linear functions interface > > + */ > > +#pragma once > > + > > +#include <functional> > > +#include <math.h> > > +#include <vector> > > + > > +#include "libcamera/internal/yaml_parser.h" > > + > > +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_; > > +}; > > -- > > 2.39.2 > >
Quoting Paul Elder (2024-04-26 08:39:39) > Hi Naush, > > On Mon, Apr 08, 2024 at 09:14:41AM +0100, Naushir Patuck wrote: > > Hi Paul, > > > > On Fri, 5 Apr 2024 at 09:03, Paul Elder <paul.elder@ideasonboard.com> wrote: > > > > > > Copy the piecewise linear function code from Raspberry Pi. > > > > > > > This seems like a good time to mention something I was intending on > > doing for a while... > > > > Our PWL library is used internally in a few places > > (libcamera/libpisp/rpicam-apps/some internal tools). Right now each > > of these libraries has a copy of pretty much the same pwl.c/pwl.h > > file. I was intending to create a stand alone Raspberry Pi PWL > > library that could be linked with these so we only have one canonical > > copy of the source code. This could then be included into > > libcamera/libpipa as a subproject if needed. Do you think this would > > be helpful and suitable for you? We will need to look at possibly > > Yeah that might be a good idea. (although I'm not in the position to > make these decisions) I don't think core libcamera can rely on a 'subproject' (in meson terms) as packagers/distributions will not accept a build that has to go and download more code to perform the build. But we should certainly find a way to reduce having 2, 3, 4 copies of the same code in different places... I think the Pwl is small enough (and stable enough) that it's not too painful for now. I wonder if we do need to break out more like we have with libcamera-base ... but I don't know where the bar is for that yet. > In any case we need this right now so for now we'll go with this :) > > > Thanks, > > Paul -- Kieran
On Fri, May 31, 2024 at 11:34:15AM +0100, Kieran Bingham wrote: > Quoting Paul Elder (2024-04-26 08:39:39) > > Hi Naush, > > > > On Mon, Apr 08, 2024 at 09:14:41AM +0100, Naushir Patuck wrote: > > > Hi Paul, > > > > > > On Fri, 5 Apr 2024 at 09:03, Paul Elder <paul.elder@ideasonboard.com> wrote: > > > > > > > > Copy the piecewise linear function code from Raspberry Pi. > > > > > > > > > > This seems like a good time to mention something I was intending on > > > doing for a while... > > > > > > Our PWL library is used internally in a few places > > > (libcamera/libpisp/rpicam-apps/some internal tools). Right now each > > > of these libraries has a copy of pretty much the same pwl.c/pwl.h > > > file. I was intending to create a stand alone Raspberry Pi PWL > > > library that could be linked with these so we only have one canonical > > > copy of the source code. This could then be included into > > > libcamera/libpipa as a subproject if needed. Do you think this would > > > be helpful and suitable for you? We will need to look at possibly > > > > Yeah that might be a good idea. (although I'm not in the position to > > make these decisions) > > I don't think core libcamera can rely on a 'subproject' (in meson terms) > as packagers/distributions will not accept a build that has to go and > download more code to perform the build. > > But we should certainly find a way to reduce having 2, 3, 4 copies of > the same code in different places... > > I think the Pwl is small enough (and stable enough) that it's not too > painful for now. I wonder if we do need to break out more like we have > with libcamera-base ... but I don't know where the bar is for that yet. I would vote for a standalone math library that would be optimized for camera usage, but I fear we would be reinventing the wheel :-) > > In any case we need this right now so for now we'll go with this :)
diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build index 31cc8d70..8fba16e1 100644 --- a/src/ipa/libipa/meson.build +++ b/src/ipa/libipa/meson.build @@ -8,6 +8,7 @@ libipa_headers = files([ 'fc_queue.h', 'histogram.h', 'module.h', + 'pwl.h', ]) libipa_sources = files([ @@ -18,6 +19,7 @@ libipa_sources = files([ 'fc_queue.cpp', 'histogram.cpp', 'module.cpp', + 'pwl.cpp' ]) libipa_includes = include_directories('..') diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp new file mode 100644 index 00000000..09f5d65c --- /dev/null +++ b/src/ipa/libipa/pwl.cpp @@ -0,0 +1,267 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * pwl.cpp - piecewise linear functions + */ + +#include <cassert> +#include <cmath> +#include <stdexcept> + +#include "pwl.h" + +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 00000000..7a6a6452 --- /dev/null +++ b/src/ipa/libipa/pwl.h @@ -0,0 +1,123 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * pwl.h - piecewise linear functions interface + */ +#pragma once + +#include <functional> +#include <math.h> +#include <vector> + +#include "libcamera/internal/yaml_parser.h" + +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_; +};
Copy the piecewise linear function code from Raspberry Pi. Signed-off-by: Paul Elder <paul.elder@ideasonboard.com> --- src/ipa/libipa/meson.build | 2 + src/ipa/libipa/pwl.cpp | 267 +++++++++++++++++++++++++++++++++++++ src/ipa/libipa/pwl.h | 123 +++++++++++++++++ 3 files changed, 392 insertions(+) create mode 100644 src/ipa/libipa/pwl.cpp create mode 100644 src/ipa/libipa/pwl.h