@@ -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 */
@@ -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
+ * \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 */
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(-)