libcamera: dw100_vertexmap: Implement parametric dewarping

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>
Reviewed-by: Paul Elder <paul.elder@ideasonboard.com>
Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com>

include/libcamera/internal/converter/converter_dw100_vertexmap.h

@@ -17,6 +17,9 @@
 #include <libcamera/geometry.h>
 #include <libcamera/transform.h>
 
+#include "libcamera/internal/matrix.h"
+#include "libcamera/internal/vector.h"
+
 namespace libcamera {
 
 class Dw100VertexMap
@@ -57,9 +60,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;
@@ -73,6 +84,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 */

src/libcamera/converter/converter_dw100_vertexmap.cpp

@@ -190,8 +190,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 constraints process works as follows:
@@ -551,10 +549,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);
 
@@ -568,4 +564,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, false otherwise
+ */
+
+/**
+ * \fn Dw100VertexMap::setLensDewarpEnable()
+ * \brief Enables or disables lens dewarping
+ * \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 */