@@ -69,18 +69,13 @@ private:
int applyControls(const Stream *stream, const V4L2Request *request);
void reinitRequest(V4L2Request *request);
- struct DewarpParms {
- Matrix<double, 3, 3> cm;
- std::vector<double> coeffs;
- };
-
struct VertexMapInfo {
Dw100VertexMap map;
bool update;
};
std::map<const Stream *, VertexMapInfo> vertexMaps_;
- std::optional<DewarpParms> dewarpParams_;
+ std::optional<Dw100VertexMap::DewarpParams> dewarpParams_;
unsigned int inputBufferCount_;
V4L2M2MConverter converter_;
Rectangle sensorCrop_;
@@ -9,6 +9,7 @@
#include <assert.h>
#include <cmath>
+#include <optional>
#include <stdint.h>
#include <vector>
@@ -30,6 +31,33 @@ public:
Crop = 1,
};
+ struct DewarpParams {
+ DewarpParams()
+ : cm(Matrix<double, 3, 3>::identity()),
+ coefficients({})
+ {
+ }
+
+ int setCoefficients(Span<const double> coeffs);
+
+ Matrix<double, 3, 3> cm;
+
+ struct {
+ double k1;
+ double k2;
+ double p1;
+ double p2;
+ double k3;
+ double k4;
+ double k5;
+ double k6;
+ double s1;
+ double s2;
+ double s3;
+ double s4;
+ } coefficients;
+ };
+
void applyLimits();
void setInputSize(const Size &size)
{
@@ -60,8 +88,7 @@ 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 setDewarpParams(const DewarpParams ¶ms) { dewarpParams_ = params; }
void setLensDewarpEnable(bool enable) { lensDewarpEnable_ = enable; }
bool lensDewarpEnable() { return lensDewarpEnable_; }
@@ -85,10 +112,8 @@ private:
Point effectiveOffset_;
Rectangle effectiveScalerCrop_;
- Matrix<double, 3, 3> dewarpM_ = Matrix<double, 3, 3>::identity();
- std::array<double, 12> dewarpCoeffs_;
+ std::optional<DewarpParams> dewarpParams_;
bool lensDewarpEnable_ = true;
- bool dewarpParamsValid_ = false;
};
} /* namespace libcamera */
@@ -15,6 +15,7 @@
#include <libcamera/stream.h>
#include "libcamera/internal/converter.h"
+#include "libcamera/internal/converter/converter_dw100_vertexmap.h"
#include "libcamera/internal/converter/converter_v4l2_m2m.h"
#include "libcamera/internal/media_device.h"
#include "libcamera/internal/v4l2_videodevice.h"
@@ -99,7 +100,7 @@ ConverterDW100Module::createModule(DeviceEnumerator *enumerator)
*/
int ConverterDW100Module::init(const ValueNode ¶ms)
{
- DewarpParms dp;
+ Dw100VertexMap::DewarpParams dp;
auto &cm = params["cm"];
auto &coefficients = params["coefficients"];
@@ -131,7 +132,13 @@ int ConverterDW100Module::init(const ValueNode ¶ms)
LOG(Converter, Error) << "Dewarp parameters 'coefficients' value is not a list";
return -EINVAL;
}
- dp.coeffs = std::move(*coeffs);
+
+ int ret = dp.setCoefficients(*coeffs);
+ if (ret) {
+ LOG(Converter, Error)
+ << "Dewarp 'coefficients' must have 4, 5, 8 or 12 values";
+ return -EINVAL;
+ }
dewarpParams_ = dp;
@@ -163,7 +170,7 @@ int ConverterDW100Module::configure(const StreamConfiguration &inputCfg,
vertexMap.setSensorCrop(sensorCrop_);
if (dewarpParams_)
- vertexMap.setDewarpParams(dewarpParams_->cm, dewarpParams_->coeffs);
+ vertexMap.setDewarpParams(*dewarpParams_);
info.update = true;
}
@@ -227,6 +227,65 @@ int dw100VerticesForLength(const int length)
* into account within the possible limits.
*/
+/**
+ * \struct Dw100VertexMap::DewarpParams
+ * \brief Structure combining all dewarp parameters
+ *
+ * \var Dw100VertexMap::DewarpParams::cm
+ * \brief The camera matrix
+ *
+ * \var Dw100VertexMap::DewarpParams::coefficients
+ * \brief Structure containing the lens dewarp coefficients
+
+ * See https://docs.opencv.org/4.12.0/d9/d0c/group__calib3d.html for further
+ * details on the model.
+ */
+
+/**
+ * \brief Fill the coefficients with a list of coefficients
+ * \param coeffs
+ *
+ * Fill the coefficients with the ones from the provided span. These data is
+ * used to fill the coefficients in the order k1, k2, p1, p2, k3, k4, k5, k6,
+ * s1, s2, s3, s4. The span must either contain 4, 5, 8 or 12 entries, the
+ * remaining coefficients are set to 0.
+ *
+ * \return 0 on success or -EINVAL if \a coeffs has the wrong size
+ */
+int Dw100VertexMap::DewarpParams::setCoefficients(Span<const double> coeffs)
+{
+ size_t s = coeffs.size();
+ if (!(s == 4 || s == 5 || s == 8 || s == 12))
+ return -EINVAL;
+
+ coefficients = {};
+
+ if (s >= 4) {
+ coefficients.k1 = coeffs[0];
+ coefficients.k2 = coeffs[1];
+ coefficients.p1 = coeffs[2];
+ coefficients.p2 = coeffs[3];
+ }
+
+ if (s >= 5)
+ coefficients.k3 = coeffs[4];
+
+ if (s >= 8) {
+ coefficients.k4 = coeffs[5];
+ coefficients.k5 = coeffs[6];
+ coefficients.k6 = coeffs[7];
+ }
+
+ if (s == 12) {
+ coefficients.s1 = coeffs[8];
+ coefficients.s2 = coeffs[9];
+ coefficients.s3 = coeffs[10];
+ coefficients.s4 = coeffs[11];
+ }
+
+ return 0;
+}
+
/**
* \brief Apply limits on scale and offset
*
@@ -549,7 +608,7 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap()
p = transformPoint(outputToSensor, p);
- if (dewarpParamsValid_ && lensDewarpEnable_)
+ if (lensDewarpEnable_)
p = dewarpPoint(p);
p = transformPoint(sensorToInput, p);
@@ -565,42 +624,15 @@ std::vector<uint32_t> Dw100VertexMap::getVertexMap()
}
/**
+ * \fn Dw100VertexMap::setDewarpParams()
* \brief Set the dewarp parameters
- * \param cm The camera matrix
- * \param coeffs The dewarp coefficients
+ * \param params The dewarp parameters
*
* 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
*/
/**
@@ -627,32 +659,28 @@ int Dw100VertexMap::setDewarpParams(const Matrix<double, 3, 3> &cm,
*/
Vector2d Dw100VertexMap::dewarpPoint(const Vector2d &p)
{
+ if (!dewarpParams_.has_value())
+ return p;
+
double x, y;
double xout, yout;
- 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];
+ auto &cm = dewarpParams_->cm;
+ auto &c = dewarpParams_->coefficients;
- y = (p.y() - dewarpM_[1][2]) / dewarpM_[1][1];
- x = (p.x() - dewarpM_[0][2] - y * dewarpM_[0][1]) / dewarpM_[0][0];
+ y = (p.y() - cm[1][2]) / cm[1][1];
+ x = (p.x() - cm[0][2] - y * cm[0][1]) / cm[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);
- xout = x * d + 2 * p1 * x * y + p2 * (r2 + 2 * x * x) + s1 * r2 + s2 * r2 * r2;
- yout = y * d + 2 * p2 * x * y + p1 * (r2 + 2 * y * y) + s3 * r2 + s4 * r2 * r2;
+ double r4 = r2 * r2;
+ double r6 = r2 * r2 * r2;
+ double d = (1 + c.k1 * r2 + c.k2 * r4 + c.k3 * r6) /
+ (1 + c.k4 * r2 + c.k5 * r4 + c.k6 * r6);
- return { { xout * dewarpM_[0][0] + yout * dewarpM_[0][1] + dewarpM_[0][2],
- yout * dewarpM_[1][1] + dewarpM_[1][2] } };
+ xout = x * d + 2 * c.p1 * x * y + c.p2 * (r2 + 2 * x * x) + c.s1 * r2 + c.s2 * r4;
+ yout = y * d + 2 * c.p2 * x * y + c.p1 * (r2 + 2 * y * y) + c.s3 * r2 + c.s4 * r4;
+
+ return { { xout * cm[0][0] + yout * cm[0][1] + cm[0][2],
+ yout * cm[1][1] + cm[1][2] } };
}
} /* namespace libcamera */