[v2,30/35] libcamera: dw100_vertexmap: Implement parametric dewarping

24757 diff mbox series

Implement functions to allow lens dewarping based on the common lens
dewarp model used e.g. by OpenCV.

See https://docs.opencv.org/4.12.0/d9/d0c/group__calib3d.html for an
in depth explanation of the parameters.

Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com>

---

Changes in v2:
- Adapt to vertex map implementation based on affine transforms
- Improved documentation
- dropped loadDewarpParams function as it was never used
---
 .../converter/converter_dw100_vertexmap.h     | 16 +++
 .../converter/converter_dw100_vertexmap.cpp   | 98 +++++++++++++++++--
 2 files changed, 108 insertions(+), 6 deletions(-)

Quoting Stefan Klug (2025-10-23 23:48:31)
> Implement functions to allow lens dewarping based on the common lens
> dewarp model used e.g. by OpenCV.
> 
> See https://docs.opencv.org/4.12.0/d9/d0c/group__calib3d.html for an
> in depth explanation of the parameters.
> 
> Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com>
> 
> ---
> 
> Changes in v2:
> - Adapt to vertex map implementation based on affine transforms
> - Improved documentation
> - dropped loadDewarpParams function as it was never used
> ---
>  .../converter/converter_dw100_vertexmap.h     | 16 +++
>  .../converter/converter_dw100_vertexmap.cpp   | 98 +++++++++++++++++--
>  2 files changed, 108 insertions(+), 6 deletions(-)
> 
> diff --git a/include/libcamera/internal/converter/converter_dw100_vertexmap.h b/include/libcamera/internal/converter/converter_dw100_vertexmap.h
> index e72cb72bb9f1..b05ed8338a23 100644
> --- a/include/libcamera/internal/converter/converter_dw100_vertexmap.h
> +++ b/include/libcamera/internal/converter/converter_dw100_vertexmap.h
> @@ -10,6 +10,9 @@
>  #include <libcamera/geometry.h>
>  #include <libcamera/transform.h>
>  
> +#include "libcamera/internal/matrix.h"
> +#include "libcamera/internal/vector.h"
> +
>  namespace libcamera {
>  
>  class Dw100VertexMap
> @@ -55,9 +58,17 @@ public:
>         void setMode(const ScaleMode mode) { mode_ = mode; }
>         ScaleMode mode() const { return mode_; }
>  
> +       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:
> +       Vector<double, 2> dewarpPoint(const Vector<double, 2> &p);
> +
>         Rectangle scalerCrop_;
>         Rectangle sensorCrop_;
>         Transform transform_ = Transform::Identity;
> @@ -71,6 +82,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 0e930479b6f7..f2be9a697ff3 100644
> --- a/src/libcamera/converter/converter_dw100_vertexmap.cpp
> +++ b/src/libcamera/converter/converter_dw100_vertexmap.cpp
> @@ -184,8 +184,6 @@ int dw100VerticesForLength(const int length)
>   * |             |    |             |    | Transpose) |    | Rotate)         |
>   * +-------------+    +-------------+    +------------+    +-----------------+
>   *
> - * \todo Lens dewarp is not yet implemented. An identity map is used instead.
> - *
>   * All parameters are clamped to valid values before creating the vertex map.
>   *
>   * The constrains process works as follows:
> @@ -546,10 +544,8 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap()
>  
>                         p = transformPoint(outputToSensor, p);
>  
> -                       /*
> -                        * \todo: Transformations in sensor space to be added
> -                        * here.
> -                        */
> +                       if (dewarpParamsValid_ && lensDewarpEnable_)
> +                               p = dewarpPoint(p);
>  
>                         p = transformPoint(sensorToInput, p);
>  
> @@ -563,4 +559,94 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap()
>         return res;
>  }
>  
> +/**
> + * \brief Set the dewarp parameters
> + * \param cm The camera matrix
> + * \param coeffs The dewarp coefficients
> + *
> + * Sets the dewarp parameters according to the commonly used dewarp model. See
> + * https://docs.opencv.org/4.12.0/d9/d0c/group__calib3d.html for further details
> + * on the model. The parameter \a coeffs must either hold 4,5,8 or 12 values.
> + * They represent the parameters k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4]]] in
> + * the model.
> + *
> + * \return A negative number on error, 0 otherwise
> + */
> +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;
> +}
> +
> +/**
> + * \fn Dw100VertexMap::dewarpParamsValid()
> + * \brief Returns if the dewarp parameters are valid
> + *
> + * \return True if the dewarp parameters are valid, fals otherwise
> + */
> +
> +/**
> + * \fn Dw100VertexMap::setLensDewarpEnable()
> + * \brief Enables or disabled lens dewarping

s/disabled/disables/

Reviewed-by: Paul Elder <paul.elder@ideasonboard.com>

> + * \param[in] enable Enable or disable lens dewarping
> + */
> +
> +/**
> + * \fn Dw100VertexMap::lensDewarpEnable()
> + * \brief Returns if lens dewarping is enabled
> + */
> +
> +/**
> + * \brief Apply dewarp calculation to a point
> + * \param p The point to dewarp
> + *
> + * Applies the dewarp transformation to point \a p according to the commonly
> + * used dewarp model. See
> + * https://docs.opencv.org/4.12.0/d9/d0c/group__calib3d.html for further details
> + * on the model.
> + *
> + * \return The dewarped point
> + */
> +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;
> +
> +       return { { x * dewarpM_[0][0] + y * dewarpM_[0][1] + dewarpM_[0][2],
> +                  y * dewarpM_[1][1] + dewarpM_[1][2] } };
> +}
> +
>  } /* namespace libcamera */
> -- 
> 2.48.1
>