| Message ID | 20250930122726.1837524-29-stefan.klug@ideasonboard.com |
|---|---|
| State | New |
| Headers | show |
| Series |
|
| Related | show |
Quoting Stefan Klug (2025-09-30 13:26:49) > Implement functions to allow lens dewarping based on the common lens > dewarp model used e.g. by OpenCV > > See https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga7dfb72c9cf9780a347fbe3d1c47e5d5a > > Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com> > --- > .../converter/converter_dw100_vertexmap.h | 17 ++++ > .../converter/converter_dw100_vertexmap.cpp | 87 +++++++++++++++++++ > 2 files changed, 104 insertions(+) > > diff --git a/include/libcamera/internal/converter/converter_dw100_vertexmap.h b/include/libcamera/internal/converter/converter_dw100_vertexmap.h > index 9c6e0ffa4513..7a12c4cb7a50 100644 > --- a/include/libcamera/internal/converter/converter_dw100_vertexmap.h > +++ b/include/libcamera/internal/converter/converter_dw100_vertexmap.h > @@ -12,6 +12,9 @@ > #include <libcamera/geometry.h> > #include <libcamera/transform.h> > > +#include "libcamera/internal/matrix.h" > +#include "libcamera/internal/vector.h" > + > namespace libcamera { > > class Dw100VertexMap > @@ -57,11 +60,20 @@ public: > void setMode(const ScaleMode mode) { mode_ = mode; } > ScaleMode mode() const { return mode_; } > > + int loadDewarpParams(const YamlObject &dict); > + int setDewarpParams(const Matrix<double, 3, 3> &cm, const Span<const double> &coeffs); > + bool dewarpParamsValid() { return dewarpParamsValid_; } > + > + void setLensDewarpEnable(bool enable) { lensDewarpEnable_ = enable; } > + bool lensDewarpEnable() { return lensDewarpEnable_; } > + > std::vector<uint32_t> getVertexMap(); > > private: > int getVerticesForLength(const int length); > > + Vector<double, 2> dewarpPoint(const Vector<double, 2> &p); > + > Rectangle scalerCrop_; > Rectangle sensorCrop_; > Transform transform_ = Transform::Identity; > @@ -75,6 +87,11 @@ private: > double effectiveScaleY_; > Point effectiveOffset_; > Rectangle effectiveScalerCrop_; > + > + Matrix<double, 3, 3> dewarpM_ = Matrix<double, 3, 3>::identity(); > + std::array<double, 12> dewarpCoeffs_; > + bool lensDewarpEnable_ = true; > + bool dewarpParamsValid_ = false; > }; > > } /* namespace libcamera */ > diff --git a/src/libcamera/converter/converter_dw100_vertexmap.cpp b/src/libcamera/converter/converter_dw100_vertexmap.cpp > index 10d9e34d98c5..5581d53e57c7 100644 > --- a/src/libcamera/converter/converter_dw100_vertexmap.cpp > +++ b/src/libcamera/converter/converter_dw100_vertexmap.cpp > @@ -66,6 +66,14 @@ Vector2d point2Vec2d(const Point &p) > return { { static_cast<double>(p.x), static_cast<double>(p.y) } }; > } > > +void transformPoint(const Rectangle &from, const Rectangle &to, Vector2d &p) I wonder if this ends up going to geometry in the future. No need now without another user. > +{ > + double sx = to.width / static_cast<double>(from.width); > + double sy = to.height / static_cast<double>(from.height); > + p.x() = (p.x() - from.x) * sx + to.x; > + p.y() = (p.y() - from.y) * sy + to.y; > +} > + > } /* namespace */ > > /** > @@ -344,6 +352,17 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap() > p.x() += localScalerCrop.x; > p.y() += localScalerCrop.y; > > + if (dewarpParamsValid_ && lensDewarpEnable_) { > + /* > + * We are in localScalerCrop coordinates. > + * Tranform to sensor coordinates. Transform > + */ > + transformPoint(localScalerCrop, effectiveScalerCrop_, p); > + p = dewarpPoint(p); > + /* tarnsform back to localScalerCrop */ Transform > + transformPoint(effectiveScalerCrop_, localScalerCrop, p); > + } > + > /* Convert to fixed point */ > uint32_t v = static_cast<uint32_t>(p.y() * 16) << 16 | > (static_cast<uint32_t>(p.x() * 16) & 0xffff); > @@ -354,6 +373,74 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap() > return res; > } > > +int Dw100VertexMap::loadDewarpParams(const YamlObject &dict) > +{ > + Matrix<double, 3, 3> m; > + const auto &cm = dict["cm"].get<Matrix<double, 3, 3>>(); > + if (!cm) { > + LOG(Converter, Error) << "Dewarp parameters are missing 'cm' value"; > + return -EINVAL; > + } > + > + m = *cm; > + > + const auto &coeffs = dict["coefficients"].getList<double>(); > + if (!coeffs) { > + LOG(Converter, Error) << "Dewarp parameters 'coefficients' value is not a list"; > + return -EINVAL; > + } > + > + const Span<const double> span{ *coeffs }; > + return setDewarpParams(m, span); > +} > + > +int Dw100VertexMap::setDewarpParams(const Matrix<double, 3, 3> &cm, const Span<const double> &coeffs) > +{ > + dewarpM_ = cm; > + dewarpCoeffs_.fill(0.0); > + > + if (coeffs.size() != 4 && coeffs.size() != 5 && > + coeffs.size() != 8 && coeffs.size() != 12) { > + LOG(Converter, Error) << "Dewarp 'coefficients' must have 4, 5, 8 or 12 values"; > + dewarpParamsValid_ = false; > + return -EINVAL; > + } > + std::copy(coeffs.begin(), coeffs.end(), dewarpCoeffs_.begin()); > + > + dewarpParamsValid_ = true; > + return 0; > +} > + > +Vector2d Dw100VertexMap::dewarpPoint(const Vector2d &p) > +{ > + double x, y; > + double k1 = dewarpCoeffs_[0]; > + double k2 = dewarpCoeffs_[1]; > + double p1 = dewarpCoeffs_[2]; > + double p2 = dewarpCoeffs_[3]; > + double k3 = dewarpCoeffs_[4]; > + double k4 = dewarpCoeffs_[5]; > + double k5 = dewarpCoeffs_[6]; > + double k6 = dewarpCoeffs_[7]; > + double s1 = dewarpCoeffs_[8]; > + double s2 = dewarpCoeffs_[9]; > + double s3 = dewarpCoeffs_[10]; > + double s4 = dewarpCoeffs_[11]; I'm scared to ask - but how could we document the coefficients. Perhaps we need a top level dewarp doxygen page sometime. > + > + y = (p.y() - dewarpM_[1][2]) / dewarpM_[1][1]; > + x = (p.x() - dewarpM_[0][2] - y * dewarpM_[0][1]) / dewarpM_[0][0]; > + > + double r2 = x * x + y * y; > + double d = (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) / > + (1 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2); > + x = x * d + 2 * p1 * x * y + p2 * (r2 + 2 * x * x) + s1 * r2 + s2 * r2 * r2; > + y = y * d + 2 * p2 * x * y + p1 * (r2 + 2 * y * y) + s3 * r2 + s4 * r2 * r2; > + > + Vector2d ret{ { x * dewarpM_[0][0] + y * dewarpM_[0][1] + dewarpM_[0][2], > + y * dewarpM_[1][1] + dewarpM_[1][2] } }; > + return ret; Can this be directly returned? return { { x * dewarpM_[0][0] + y * dewarpM_[0][1] + dewarpM_[0][2], y * dewarpM_[1][1] + dewarpM_[1][2] } }; Or does the construction get more difficult ? But actually for this patch - I think I'll already add with the typos fixed: Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com> > +} > + > int Dw100VertexMap::getVerticesForLength(const int length) > { > return (length + kDw100BlockSize - 1) / kDw100BlockSize + 1; > -- > 2.48.1 >
Hi 2025. 10. 02. 23:57 keltezéssel, Kieran Bingham írta: > Quoting Stefan Klug (2025-09-30 13:26:49) >> Implement functions to allow lens dewarping based on the common lens >> dewarp model used e.g. by OpenCV >> >> See https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga7dfb72c9cf9780a347fbe3d1c47e5d5a >> >> Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com> >> --- >> .../converter/converter_dw100_vertexmap.h | 17 ++++ >> .../converter/converter_dw100_vertexmap.cpp | 87 +++++++++++++++++++ >> 2 files changed, 104 insertions(+) >> >> diff --git a/include/libcamera/internal/converter/converter_dw100_vertexmap.h b/include/libcamera/internal/converter/converter_dw100_vertexmap.h >> index 9c6e0ffa4513..7a12c4cb7a50 100644 >> --- a/include/libcamera/internal/converter/converter_dw100_vertexmap.h >> +++ b/include/libcamera/internal/converter/converter_dw100_vertexmap.h >> @@ -12,6 +12,9 @@ >> #include <libcamera/geometry.h> >> #include <libcamera/transform.h> >> >> +#include "libcamera/internal/matrix.h" >> +#include "libcamera/internal/vector.h" >> + >> namespace libcamera { >> >> class Dw100VertexMap >> @@ -57,11 +60,20 @@ public: >> void setMode(const ScaleMode mode) { mode_ = mode; } >> ScaleMode mode() const { return mode_; } >> >> + int loadDewarpParams(const YamlObject &dict); >> + int setDewarpParams(const Matrix<double, 3, 3> &cm, const Span<const double> &coeffs); >> + bool dewarpParamsValid() { return dewarpParamsValid_; } >> + >> + void setLensDewarpEnable(bool enable) { lensDewarpEnable_ = enable; } >> + bool lensDewarpEnable() { return lensDewarpEnable_; } >> + >> std::vector<uint32_t> getVertexMap(); >> >> private: >> int getVerticesForLength(const int length); >> >> + Vector<double, 2> dewarpPoint(const Vector<double, 2> &p); >> + >> Rectangle scalerCrop_; >> Rectangle sensorCrop_; >> Transform transform_ = Transform::Identity; >> @@ -75,6 +87,11 @@ private: >> double effectiveScaleY_; >> Point effectiveOffset_; >> Rectangle effectiveScalerCrop_; >> + >> + Matrix<double, 3, 3> dewarpM_ = Matrix<double, 3, 3>::identity(); >> + std::array<double, 12> dewarpCoeffs_; >> + bool lensDewarpEnable_ = true; >> + bool dewarpParamsValid_ = false; >> }; >> >> } /* namespace libcamera */ >> diff --git a/src/libcamera/converter/converter_dw100_vertexmap.cpp b/src/libcamera/converter/converter_dw100_vertexmap.cpp >> index 10d9e34d98c5..5581d53e57c7 100644 >> --- a/src/libcamera/converter/converter_dw100_vertexmap.cpp >> +++ b/src/libcamera/converter/converter_dw100_vertexmap.cpp >> @@ -66,6 +66,14 @@ Vector2d point2Vec2d(const Point &p) >> return { { static_cast<double>(p.x), static_cast<double>(p.y) } }; >> } >> >> +void transformPoint(const Rectangle &from, const Rectangle &to, Vector2d &p) > > I wonder if this ends up going to geometry in the future. No need now > without another user. This is already implemented in `Rectangle::transformedBetween()`. That works on rectangles, but to transform a point, one can use the top left corner and ignore the sizes. Regards, Barnabás Pőcze > >> +{ >> + double sx = to.width / static_cast<double>(from.width); >> + double sy = to.height / static_cast<double>(from.height); >> + p.x() = (p.x() - from.x) * sx + to.x; >> + p.y() = (p.y() - from.y) * sy + to.y; >> +} >> + >> } /* namespace */ >> >> /** >> @@ -344,6 +352,17 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap() >> p.x() += localScalerCrop.x; >> p.y() += localScalerCrop.y; >> >> + if (dewarpParamsValid_ && lensDewarpEnable_) { >> + /* >> + * We are in localScalerCrop coordinates. >> + * Tranform to sensor coordinates. > > Transform > >> + */ >> + transformPoint(localScalerCrop, effectiveScalerCrop_, p); >> + p = dewarpPoint(p); >> + /* tarnsform back to localScalerCrop */ > > Transform > >> + transformPoint(effectiveScalerCrop_, localScalerCrop, p); >> + } >> + >> /* Convert to fixed point */ >> uint32_t v = static_cast<uint32_t>(p.y() * 16) << 16 | >> (static_cast<uint32_t>(p.x() * 16) & 0xffff); >> @@ -354,6 +373,74 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap() >> return res; >> } >> >> +int Dw100VertexMap::loadDewarpParams(const YamlObject &dict) >> +{ >> + Matrix<double, 3, 3> m; >> + const auto &cm = dict["cm"].get<Matrix<double, 3, 3>>(); >> + if (!cm) { >> + LOG(Converter, Error) << "Dewarp parameters are missing 'cm' value"; >> + return -EINVAL; >> + } >> + >> + m = *cm; >> + >> + const auto &coeffs = dict["coefficients"].getList<double>(); >> + if (!coeffs) { >> + LOG(Converter, Error) << "Dewarp parameters 'coefficients' value is not a list"; >> + return -EINVAL; >> + } >> + >> + const Span<const double> span{ *coeffs }; >> + return setDewarpParams(m, span); >> +} >> + >> +int Dw100VertexMap::setDewarpParams(const Matrix<double, 3, 3> &cm, const Span<const double> &coeffs) >> +{ >> + dewarpM_ = cm; >> + dewarpCoeffs_.fill(0.0); >> + >> + if (coeffs.size() != 4 && coeffs.size() != 5 && >> + coeffs.size() != 8 && coeffs.size() != 12) { >> + LOG(Converter, Error) << "Dewarp 'coefficients' must have 4, 5, 8 or 12 values"; >> + dewarpParamsValid_ = false; >> + return -EINVAL; >> + } >> + std::copy(coeffs.begin(), coeffs.end(), dewarpCoeffs_.begin()); >> + >> + dewarpParamsValid_ = true; >> + return 0; >> +} >> + >> +Vector2d Dw100VertexMap::dewarpPoint(const Vector2d &p) >> +{ >> + double x, y; >> + double k1 = dewarpCoeffs_[0]; >> + double k2 = dewarpCoeffs_[1]; >> + double p1 = dewarpCoeffs_[2]; >> + double p2 = dewarpCoeffs_[3]; >> + double k3 = dewarpCoeffs_[4]; >> + double k4 = dewarpCoeffs_[5]; >> + double k5 = dewarpCoeffs_[6]; >> + double k6 = dewarpCoeffs_[7]; >> + double s1 = dewarpCoeffs_[8]; >> + double s2 = dewarpCoeffs_[9]; >> + double s3 = dewarpCoeffs_[10]; >> + double s4 = dewarpCoeffs_[11]; > > I'm scared to ask - but how could we document the coefficients. Perhaps > we need a top level dewarp doxygen page sometime. > >> + >> + y = (p.y() - dewarpM_[1][2]) / dewarpM_[1][1]; >> + x = (p.x() - dewarpM_[0][2] - y * dewarpM_[0][1]) / dewarpM_[0][0]; >> + >> + double r2 = x * x + y * y; >> + double d = (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) / >> + (1 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2); >> + x = x * d + 2 * p1 * x * y + p2 * (r2 + 2 * x * x) + s1 * r2 + s2 * r2 * r2; >> + y = y * d + 2 * p2 * x * y + p1 * (r2 + 2 * y * y) + s3 * r2 + s4 * r2 * r2; >> + >> + Vector2d ret{ { x * dewarpM_[0][0] + y * dewarpM_[0][1] + dewarpM_[0][2], >> + y * dewarpM_[1][1] + dewarpM_[1][2] } }; >> + return ret; > > Can this be directly returned? > > return { { x * dewarpM_[0][0] + y * dewarpM_[0][1] + dewarpM_[0][2], > y * dewarpM_[1][1] + dewarpM_[1][2] } }; > > Or does the construction get more difficult ? > > > But actually for this patch - I think I'll already add with the typos > fixed: > > Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com> > >> +} >> + >> int Dw100VertexMap::getVerticesForLength(const int length) >> { >> return (length + kDw100BlockSize - 1) / kDw100BlockSize + 1; >> -- >> 2.48.1 >>
Quoting Barnabás Pőcze (2025-10-03 10:09:32) > Hi > > > 2025. 10. 02. 23:57 keltezéssel, Kieran Bingham írta: > > Quoting Stefan Klug (2025-09-30 13:26:49) > >> Implement functions to allow lens dewarping based on the common lens > >> dewarp model used e.g. by OpenCV > >> > >> See https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga7dfb72c9cf9780a347fbe3d1c47e5d5a > >> > >> Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com> > >> --- > >> .../converter/converter_dw100_vertexmap.h | 17 ++++ > >> .../converter/converter_dw100_vertexmap.cpp | 87 +++++++++++++++++++ > >> 2 files changed, 104 insertions(+) > >> > >> diff --git a/include/libcamera/internal/converter/converter_dw100_vertexmap.h b/include/libcamera/internal/converter/converter_dw100_vertexmap.h > >> index 9c6e0ffa4513..7a12c4cb7a50 100644 > >> --- a/include/libcamera/internal/converter/converter_dw100_vertexmap.h > >> +++ b/include/libcamera/internal/converter/converter_dw100_vertexmap.h > >> @@ -12,6 +12,9 @@ > >> #include <libcamera/geometry.h> > >> #include <libcamera/transform.h> > >> > >> +#include "libcamera/internal/matrix.h" > >> +#include "libcamera/internal/vector.h" > >> + > >> namespace libcamera { > >> > >> class Dw100VertexMap > >> @@ -57,11 +60,20 @@ public: > >> void setMode(const ScaleMode mode) { mode_ = mode; } > >> ScaleMode mode() const { return mode_; } > >> > >> + int loadDewarpParams(const YamlObject &dict); > >> + int setDewarpParams(const Matrix<double, 3, 3> &cm, const Span<const double> &coeffs); > >> + bool dewarpParamsValid() { return dewarpParamsValid_; } > >> + > >> + void setLensDewarpEnable(bool enable) { lensDewarpEnable_ = enable; } > >> + bool lensDewarpEnable() { return lensDewarpEnable_; } > >> + > >> std::vector<uint32_t> getVertexMap(); > >> > >> private: > >> int getVerticesForLength(const int length); > >> > >> + Vector<double, 2> dewarpPoint(const Vector<double, 2> &p); > >> + > >> Rectangle scalerCrop_; > >> Rectangle sensorCrop_; > >> Transform transform_ = Transform::Identity; > >> @@ -75,6 +87,11 @@ private: > >> double effectiveScaleY_; > >> Point effectiveOffset_; > >> Rectangle effectiveScalerCrop_; > >> + > >> + Matrix<double, 3, 3> dewarpM_ = Matrix<double, 3, 3>::identity(); > >> + std::array<double, 12> dewarpCoeffs_; > >> + bool lensDewarpEnable_ = true; > >> + bool dewarpParamsValid_ = false; > >> }; > >> > >> } /* namespace libcamera */ > >> diff --git a/src/libcamera/converter/converter_dw100_vertexmap.cpp b/src/libcamera/converter/converter_dw100_vertexmap.cpp > >> index 10d9e34d98c5..5581d53e57c7 100644 > >> --- a/src/libcamera/converter/converter_dw100_vertexmap.cpp > >> +++ b/src/libcamera/converter/converter_dw100_vertexmap.cpp > >> @@ -66,6 +66,14 @@ Vector2d point2Vec2d(const Point &p) > >> return { { static_cast<double>(p.x), static_cast<double>(p.y) } }; > >> } > >> > >> +void transformPoint(const Rectangle &from, const Rectangle &to, Vector2d &p) > > > > I wonder if this ends up going to geometry in the future. No need now > > without another user. > > This is already implemented in `Rectangle::transformedBetween()`. > That works on rectangles, but to transform a point, one can use > the top left corner and ignore the sizes. Yes, but then you'd loose the subpixel accuracy because Rectangle is int only. That would lead to visible artifacts. Best regards, Stefan > > > Regards, > Barnabás Pőcze > > > > > >> +{ > >> + double sx = to.width / static_cast<double>(from.width); > >> + double sy = to.height / static_cast<double>(from.height); > >> + p.x() = (p.x() - from.x) * sx + to.x; > >> + p.y() = (p.y() - from.y) * sy + to.y; > >> +} > >> + > >> } /* namespace */ > >> > >> /** > >> @@ -344,6 +352,17 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap() > >> p.x() += localScalerCrop.x; > >> p.y() += localScalerCrop.y; > >> > >> + if (dewarpParamsValid_ && lensDewarpEnable_) { > >> + /* > >> + * We are in localScalerCrop coordinates. > >> + * Tranform to sensor coordinates. > > > > Transform > > > >> + */ > >> + transformPoint(localScalerCrop, effectiveScalerCrop_, p); > >> + p = dewarpPoint(p); > >> + /* tarnsform back to localScalerCrop */ > > > > Transform > > > >> + transformPoint(effectiveScalerCrop_, localScalerCrop, p); > >> + } > >> + > >> /* Convert to fixed point */ > >> uint32_t v = static_cast<uint32_t>(p.y() * 16) << 16 | > >> (static_cast<uint32_t>(p.x() * 16) & 0xffff); > >> @@ -354,6 +373,74 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap() > >> return res; > >> } > >> > >> +int Dw100VertexMap::loadDewarpParams(const YamlObject &dict) > >> +{ > >> + Matrix<double, 3, 3> m; > >> + const auto &cm = dict["cm"].get<Matrix<double, 3, 3>>(); > >> + if (!cm) { > >> + LOG(Converter, Error) << "Dewarp parameters are missing 'cm' value"; > >> + return -EINVAL; > >> + } > >> + > >> + m = *cm; > >> + > >> + const auto &coeffs = dict["coefficients"].getList<double>(); > >> + if (!coeffs) { > >> + LOG(Converter, Error) << "Dewarp parameters 'coefficients' value is not a list"; > >> + return -EINVAL; > >> + } > >> + > >> + const Span<const double> span{ *coeffs }; > >> + return setDewarpParams(m, span); > >> +} > >> + > >> +int Dw100VertexMap::setDewarpParams(const Matrix<double, 3, 3> &cm, const Span<const double> &coeffs) > >> +{ > >> + dewarpM_ = cm; > >> + dewarpCoeffs_.fill(0.0); > >> + > >> + if (coeffs.size() != 4 && coeffs.size() != 5 && > >> + coeffs.size() != 8 && coeffs.size() != 12) { > >> + LOG(Converter, Error) << "Dewarp 'coefficients' must have 4, 5, 8 or 12 values"; > >> + dewarpParamsValid_ = false; > >> + return -EINVAL; > >> + } > >> + std::copy(coeffs.begin(), coeffs.end(), dewarpCoeffs_.begin()); > >> + > >> + dewarpParamsValid_ = true; > >> + return 0; > >> +} > >> + > >> +Vector2d Dw100VertexMap::dewarpPoint(const Vector2d &p) > >> +{ > >> + double x, y; > >> + double k1 = dewarpCoeffs_[0]; > >> + double k2 = dewarpCoeffs_[1]; > >> + double p1 = dewarpCoeffs_[2]; > >> + double p2 = dewarpCoeffs_[3]; > >> + double k3 = dewarpCoeffs_[4]; > >> + double k4 = dewarpCoeffs_[5]; > >> + double k5 = dewarpCoeffs_[6]; > >> + double k6 = dewarpCoeffs_[7]; > >> + double s1 = dewarpCoeffs_[8]; > >> + double s2 = dewarpCoeffs_[9]; > >> + double s3 = dewarpCoeffs_[10]; > >> + double s4 = dewarpCoeffs_[11]; > > > > I'm scared to ask - but how could we document the coefficients. Perhaps > > we need a top level dewarp doxygen page sometime. > > > >> + > >> + y = (p.y() - dewarpM_[1][2]) / dewarpM_[1][1]; > >> + x = (p.x() - dewarpM_[0][2] - y * dewarpM_[0][1]) / dewarpM_[0][0]; > >> + > >> + double r2 = x * x + y * y; > >> + double d = (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) / > >> + (1 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2); > >> + x = x * d + 2 * p1 * x * y + p2 * (r2 + 2 * x * x) + s1 * r2 + s2 * r2 * r2; > >> + y = y * d + 2 * p2 * x * y + p1 * (r2 + 2 * y * y) + s3 * r2 + s4 * r2 * r2; > >> + > >> + Vector2d ret{ { x * dewarpM_[0][0] + y * dewarpM_[0][1] + dewarpM_[0][2], > >> + y * dewarpM_[1][1] + dewarpM_[1][2] } }; > >> + return ret; > > > > Can this be directly returned? > > > > return { { x * dewarpM_[0][0] + y * dewarpM_[0][1] + dewarpM_[0][2], > > y * dewarpM_[1][1] + dewarpM_[1][2] } }; > > > > Or does the construction get more difficult ? > > > > > > But actually for this patch - I think I'll already add with the typos > > fixed: > > > > Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com> > > > >> +} > >> + > >> int Dw100VertexMap::getVerticesForLength(const int length) > >> { > >> return (length + kDw100BlockSize - 1) / kDw100BlockSize + 1; > >> -- > >> 2.48.1 > >> >
diff --git a/include/libcamera/internal/converter/converter_dw100_vertexmap.h b/include/libcamera/internal/converter/converter_dw100_vertexmap.h index 9c6e0ffa4513..7a12c4cb7a50 100644 --- a/include/libcamera/internal/converter/converter_dw100_vertexmap.h +++ b/include/libcamera/internal/converter/converter_dw100_vertexmap.h @@ -12,6 +12,9 @@ #include <libcamera/geometry.h> #include <libcamera/transform.h> +#include "libcamera/internal/matrix.h" +#include "libcamera/internal/vector.h" + namespace libcamera { class Dw100VertexMap @@ -57,11 +60,20 @@ public: void setMode(const ScaleMode mode) { mode_ = mode; } ScaleMode mode() const { return mode_; } + int loadDewarpParams(const YamlObject &dict); + int setDewarpParams(const Matrix<double, 3, 3> &cm, const Span<const double> &coeffs); + bool dewarpParamsValid() { return dewarpParamsValid_; } + + void setLensDewarpEnable(bool enable) { lensDewarpEnable_ = enable; } + bool lensDewarpEnable() { return lensDewarpEnable_; } + std::vector<uint32_t> getVertexMap(); private: int getVerticesForLength(const int length); + Vector<double, 2> dewarpPoint(const Vector<double, 2> &p); + Rectangle scalerCrop_; Rectangle sensorCrop_; Transform transform_ = Transform::Identity; @@ -75,6 +87,11 @@ private: double effectiveScaleY_; Point effectiveOffset_; Rectangle effectiveScalerCrop_; + + Matrix<double, 3, 3> dewarpM_ = Matrix<double, 3, 3>::identity(); + std::array<double, 12> dewarpCoeffs_; + bool lensDewarpEnable_ = true; + bool dewarpParamsValid_ = false; }; } /* namespace libcamera */ diff --git a/src/libcamera/converter/converter_dw100_vertexmap.cpp b/src/libcamera/converter/converter_dw100_vertexmap.cpp index 10d9e34d98c5..5581d53e57c7 100644 --- a/src/libcamera/converter/converter_dw100_vertexmap.cpp +++ b/src/libcamera/converter/converter_dw100_vertexmap.cpp @@ -66,6 +66,14 @@ Vector2d point2Vec2d(const Point &p) return { { static_cast<double>(p.x), static_cast<double>(p.y) } }; } +void transformPoint(const Rectangle &from, const Rectangle &to, Vector2d &p) +{ + double sx = to.width / static_cast<double>(from.width); + double sy = to.height / static_cast<double>(from.height); + p.x() = (p.x() - from.x) * sx + to.x; + p.y() = (p.y() - from.y) * sy + to.y; +} + } /* namespace */ /** @@ -344,6 +352,17 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap() p.x() += localScalerCrop.x; p.y() += localScalerCrop.y; + if (dewarpParamsValid_ && lensDewarpEnable_) { + /* + * We are in localScalerCrop coordinates. + * Tranform to sensor coordinates. + */ + transformPoint(localScalerCrop, effectiveScalerCrop_, p); + p = dewarpPoint(p); + /* tarnsform back to localScalerCrop */ + transformPoint(effectiveScalerCrop_, localScalerCrop, p); + } + /* Convert to fixed point */ uint32_t v = static_cast<uint32_t>(p.y() * 16) << 16 | (static_cast<uint32_t>(p.x() * 16) & 0xffff); @@ -354,6 +373,74 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap() return res; } +int Dw100VertexMap::loadDewarpParams(const YamlObject &dict) +{ + Matrix<double, 3, 3> m; + const auto &cm = dict["cm"].get<Matrix<double, 3, 3>>(); + if (!cm) { + LOG(Converter, Error) << "Dewarp parameters are missing 'cm' value"; + return -EINVAL; + } + + m = *cm; + + const auto &coeffs = dict["coefficients"].getList<double>(); + if (!coeffs) { + LOG(Converter, Error) << "Dewarp parameters 'coefficients' value is not a list"; + return -EINVAL; + } + + const Span<const double> span{ *coeffs }; + return setDewarpParams(m, span); +} + +int Dw100VertexMap::setDewarpParams(const Matrix<double, 3, 3> &cm, const Span<const double> &coeffs) +{ + dewarpM_ = cm; + dewarpCoeffs_.fill(0.0); + + if (coeffs.size() != 4 && coeffs.size() != 5 && + coeffs.size() != 8 && coeffs.size() != 12) { + LOG(Converter, Error) << "Dewarp 'coefficients' must have 4, 5, 8 or 12 values"; + dewarpParamsValid_ = false; + return -EINVAL; + } + std::copy(coeffs.begin(), coeffs.end(), dewarpCoeffs_.begin()); + + dewarpParamsValid_ = true; + return 0; +} + +Vector2d Dw100VertexMap::dewarpPoint(const Vector2d &p) +{ + double x, y; + double k1 = dewarpCoeffs_[0]; + double k2 = dewarpCoeffs_[1]; + double p1 = dewarpCoeffs_[2]; + double p2 = dewarpCoeffs_[3]; + double k3 = dewarpCoeffs_[4]; + double k4 = dewarpCoeffs_[5]; + double k5 = dewarpCoeffs_[6]; + double k6 = dewarpCoeffs_[7]; + double s1 = dewarpCoeffs_[8]; + double s2 = dewarpCoeffs_[9]; + double s3 = dewarpCoeffs_[10]; + double s4 = dewarpCoeffs_[11]; + + y = (p.y() - dewarpM_[1][2]) / dewarpM_[1][1]; + x = (p.x() - dewarpM_[0][2] - y * dewarpM_[0][1]) / dewarpM_[0][0]; + + double r2 = x * x + y * y; + double d = (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) / + (1 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2); + x = x * d + 2 * p1 * x * y + p2 * (r2 + 2 * x * x) + s1 * r2 + s2 * r2 * r2; + y = y * d + 2 * p2 * x * y + p1 * (r2 + 2 * y * y) + s3 * r2 + s4 * r2 * r2; + + Vector2d ret{ { x * dewarpM_[0][0] + y * dewarpM_[0][1] + dewarpM_[0][2], + y * dewarpM_[1][1] + dewarpM_[1][2] } }; + return ret; +} + int Dw100VertexMap::getVerticesForLength(const int length) { return (length + kDw100BlockSize - 1) / kDw100BlockSize + 1;
Implement functions to allow lens dewarping based on the common lens dewarp model used e.g. by OpenCV See https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga7dfb72c9cf9780a347fbe3d1c47e5d5a Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com> --- .../converter/converter_dw100_vertexmap.h | 17 ++++ .../converter/converter_dw100_vertexmap.cpp | 87 +++++++++++++++++++ 2 files changed, 104 insertions(+)