Skip to content

Commit d55bbff

Browse files
cbentejaccbentejac
authored
Merge pull request #1819 from alicevision/dev/cleanDistortion
Reduce intrication of distortion with intrinsics
2 parents 1334dbd + ac78b3e commit d55bbff

33 files changed

+549
-309
lines changed

‎pyTests/camera/test_equidistant.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -77,8 +77,8 @@
7777
# - void setDistortionParams(vector<double>& distortionParams)
7878
# - template<class F> void setDistortionParamsFn(F&& callback) / not binded
7979
# - template<class F> void setDistortionParamsFn(size_t count, F&& callback) / not binded
80-
# - vector<double> getParams() => DONE
81-
# - size_t getParamsSize() => DONE
80+
# - vector<double> getParameters() => DONE
81+
# - size_t getParametersSize() => DONE
8282
# - updateFromParams(vector<double>& params) => DONE
8383
# - float getMaximalDistortion(double min_radius, double max_radius)
8484
# - Eigen::Matrix<double, 2, 2> getDerivativeAddDistoWrtPt(Vec2& pt) / Matrix and Vec2 not binded
@@ -285,16 +285,16 @@ def test_equidistant_get_set_params():
285285
""" Test creating an Equidistant object, getting and setting its parameters with the
286286
parent's class getters and setters. """
287287
intrinsic = av.Equidistant()
288-
params = intrinsic.getParams()
288+
params = intrinsic.getParameters()
289289

290-
assert len(params) == intrinsic.getParamsSize()
290+
assert len(params) == intrinsic.getParametersSize()
291291
assert params == DEFAUT_PARAMETERS
292292

293293
params = (2.0, 2.0, 1.0, 1.0)
294294

295-
assert params != intrinsic.getParams()
295+
assert params != intrinsic.getParameters()
296296
intrinsic.updateFromParams(params)
297-
assert params == intrinsic.getParams()
297+
assert params == intrinsic.getParameters()
298298

299299

300300
def test_equidistant_lock_unlock():

‎pyTests/camera/test_pinhole.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -67,8 +67,8 @@
6767
# - void setDistortionParams(vector<double>& distortionParams)
6868
# - template<class F> void setDistortionParamsFn(F&& callback) / not binded
6969
# - template<class F> void setDistortionParamsFn(size_t count, F&& callback) / not binded
70-
# - vector<double> getParams() => DONE
71-
# - size_t getParamsSize() => DONE
70+
# - vector<double> getParameters() => DONE
71+
# - size_t getParametersSize() => DONE
7272
# - updateFromParams(vector<double>& params) => DONE
7373
# - float getMaximalDistortion(double min_radius, double max_radius)
7474
# - Eigen::Matrix<double, 2, 2> getDerivativeAddDistoWrtPt(Vec2& pt) / Matrix and Vec2 not binded
@@ -243,16 +243,16 @@ def test_pinhole_get_set_params():
243243
""" Test creating a Pinhole object, getting and setting its parameters with the
244244
parent's class getters and setters. """
245245
intrinsic = av.Pinhole()
246-
params = intrinsic.getParams()
246+
params = intrinsic.getParameters()
247247

248-
assert len(params) == intrinsic.getParamsSize()
248+
assert len(params) == intrinsic.getParametersSize()
249249
assert params == DEFAUT_PARAMETERS
250250

251251
params = (2.0, 2.0, 1.0, 1.0)
252252

253-
assert params != intrinsic.getParams()
253+
assert params != intrinsic.getParameters()
254254
intrinsic.updateFromParams(params)
255-
assert params == intrinsic.getParams()
255+
assert params == intrinsic.getParameters()
256256

257257

258258
def test_pinhole_lock_unlock():

‎src/aliceVision/camera/Equidistant.cpp‎

Lines changed: 16 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -116,7 +116,7 @@ Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeTransformProjectWrtPoint3(
116116
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_pt;
117117
}
118118

119-
Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const
119+
Eigen::Matrix<double, 2, Eigen::Dynamic> Equidistant::getDerivativeTransformProjectWrtDistortion(const Eigen::Matrix4d& pose, const Vec4& pt) const
120120
{
121121
Eigen::Matrix4d T = pose;
122122
const Vec4 X = T * pt; // apply pose
@@ -182,17 +182,11 @@ Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeTransformProjectWrtPrincip
182182

183183
Eigen::Matrix<double, 2, Eigen::Dynamic> Equidistant::getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
184184
{
185-
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParams().size());
185+
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParameters().size());
186186

187187
ret.block<2, 2>(0, 0) = getDerivativeTransformProjectWrtScale(pose, pt3D);
188188
ret.block<2, 2>(0, 2) = getDerivativeTransformProjectWrtPrincipalPoint(pose, pt3D);
189189

190-
if (_pDistortion != nullptr)
191-
{
192-
const size_t distortionSize = _pDistortion->getDistortionParametersCount();
193-
ret.block(0, 4, 2, distortionSize) = getDerivativeTransformProjectWrtDisto(pose, pt3D);
194-
}
195-
196190
return ret;
197191
}
198192

@@ -270,21 +264,28 @@ Eigen::Matrix<double, 3, 2> Equidistant::getDerivativetoUnitSphereWrtScale(const
270264

271265
Eigen::Matrix<double, 3, Eigen::Dynamic> Equidistant::getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const
272266
{
273-
size_t disto_size = getDistortionParamsSize();
274-
275267
const Vec2 ptMeters = ima2cam(pt2D);
276268
const Vec2 ptUndist = removeDistortion(ptMeters);
277269
const Vec3 ptSphere = toUnitSphere(ptUndist);
278270

279-
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, getParams().size());
271+
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, getParameters().size());
280272

281273
J.block<3, 2>(0, 0) = getDerivativetoUnitSphereWrtScale(ptUndist);
282274
J.block<3, 2>(0, 2) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtPt(ptMeters) * getDerivativeIma2CamWrtPrincipalPoint();
283275

284-
if (disto_size > 0)
285-
{
286-
J.block(0, 4, 3, disto_size) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtDisto(ptMeters);
287-
}
276+
return J;
277+
}
278+
279+
Eigen::Matrix<double, 3, Eigen::Dynamic> Equidistant::getDerivativeBackProjectUnitWrtDistortion(const Vec2& pt2D) const
280+
{
281+
size_t disto_size = getDistortionParamsSize();
282+
283+
const Vec2 ptMeters = ima2cam(pt2D);
284+
const Vec2 ptUndist = removeDistortion(ptMeters);
285+
const Vec3 ptSphere = toUnitSphere(ptUndist);
286+
287+
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, disto_size);
288+
J = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtDisto(ptMeters);
288289

289290
return J;
290291
}

‎src/aliceVision/camera/Equidistant.hpp‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -77,9 +77,7 @@ class Equidistant : public IntrinsicScaleOffsetDisto
7777
Vec2 project(const Vec4& pt, bool applyDistortion = true) const override;
7878

7979
Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
80-
81-
Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const;
82-
80+
8381
Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const;
8482

8583
Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const;
@@ -99,6 +97,10 @@ class Equidistant : public IntrinsicScaleOffsetDisto
9997
*/
10098
Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const override;
10199

100+
Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtDistortion(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
101+
102+
Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtDistortion(const Vec2& pt2D) const override;
103+
102104
double imagePlaneToCameraPlaneError(double value) const override;
103105

104106
// Transform a point from the camera plane to the image plane

‎src/aliceVision/camera/Equirectangular.cpp‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -107,7 +107,7 @@ Eigen::Matrix<double, 2, 2> Equirectangular::getDerivativeTransformProjectWrtPri
107107

108108
Eigen::Matrix<double, 2, Eigen::Dynamic> Equirectangular::getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
109109
{
110-
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParams().size());
110+
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParameters().size());
111111

112112
ret.block<2, 2>(0, 0) = getDerivativeTransformProjectWrtScale(pose, pt3D);
113113
ret.block<2, 2>(0, 2) = getDerivativeTransformProjectWrtPrincipalPoint(pose, pt3D);
@@ -164,6 +164,16 @@ Eigen::Matrix<double, 3, Eigen::Dynamic> Equirectangular::getDerivativeBackProje
164164
return ret;
165165
}
166166

167+
Eigen::Matrix<double, 2, Eigen::Dynamic> Equirectangular::getDerivativeTransformProjectWrtDistortion(const Eigen::Matrix4d& pose, const Vec4& pt) const
168+
{
169+
return Eigen::Matrix<double, 2, 1>::Zero();
170+
}
171+
172+
Eigen::Matrix<double, 3, Eigen::Dynamic> Equirectangular::getDerivativeBackProjectUnitWrtDistortion(const Vec2& pt2D) const
173+
{
174+
return Eigen::Matrix<double, 3, 1>::Zero();
175+
}
176+
167177
double Equirectangular::imagePlaneToCameraPlaneError(double value) const { return 0.0; }
168178

169179
bool Equirectangular::isVisibleRay(const Vec3& ray) const

‎src/aliceVision/camera/Equirectangular.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,10 @@ class Equirectangular : public IntrinsicScaleOffsetDisto
7373

7474
Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const override;
7575

76+
Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtDistortion(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
77+
78+
Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtDistortion(const Vec2& pt2D) const override;
79+
7680
Vec3 toUnitSphere(const Vec2& pt) const override;
7781

7882
Eigen::Matrix<double, 3, 2> getDerivativetoUnitSphereWrtPoint(const Vec2& pt) const;

‎src/aliceVision/camera/IntrinsicBase.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -93,11 +93,13 @@ std::size_t IntrinsicBase::hashValue() const
9393
stl::hash_combine(seed, _sensorWidth);
9494
stl::hash_combine(seed, _sensorHeight);
9595
stl::hash_combine(seed, _serialNumber);
96-
const std::vector<double> params = this->getParams();
96+
97+
const std::vector<double> params = this->getParameters();
9798
for (double param : params)
9899
{
99100
stl::hash_combine(seed, param);
100101
}
102+
101103
return seed;
102104
}
103105

‎src/aliceVision/camera/IntrinsicBase.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -270,13 +270,13 @@ class IntrinsicBase
270270
* @brief Get the intrinsic parameters
271271
* @return Intrinsic parameters as a vector
272272
*/
273-
virtual std::vector<double> getParams() const = 0;
273+
virtual std::vector<double> getParameters() const = 0;
274274

275275
/**
276276
* @brief Get the count of intrinsic parameters
277277
* @return The number of intrinsic parameters
278278
*/
279-
virtual std::size_t getParamsSize() const = 0;
279+
virtual std::size_t getParametersSize() const = 0;
280280

281281
/**
282282
* @brief Update intrinsic parameters

‎src/aliceVision/camera/IntrinsicScaleOffsetDisto.hpp‎

Lines changed: 7 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -186,26 +186,16 @@ class IntrinsicScaleOffsetDisto : public IntrinsicScaleOffset
186186
}
187187

188188
// Data wrapper for non linear optimization (get data)
189-
std::vector<double> getParams() const override
189+
std::vector<double> getParameters() const override
190190
{
191191
std::vector<double> params = {_scale(0), _scale(1), _offset(0), _offset(1)};
192192

193-
if (_pDistortion)
194-
{
195-
params.insert(params.end(), _pDistortion->getParameters().begin(), _pDistortion->getParameters().end());
196-
}
197-
198193
return params;
199194
}
200195

201-
std::size_t getParamsSize() const override
196+
std::size_t getParametersSize() const override
202197
{
203-
std::size_t size = 4;
204-
if (_pDistortion)
205-
{
206-
size += _pDistortion->getParameters().size();
207-
}
208-
return size;
198+
return 4;
209199
}
210200

211201
// Data wrapper for non linear optimization (update from data)
@@ -260,6 +250,10 @@ class IntrinsicScaleOffsetDisto : public IntrinsicScaleOffset
260250
return this->_pDistortion->getDerivativeRemoveDistoWrtDisto(pt);
261251
}
262252

253+
virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtDistortion(const Eigen::Matrix4d& pose, const Vec4& pt) const = 0;
254+
255+
virtual Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtDistortion(const Vec2& pt2D) const = 0;
256+
263257
/**
264258
* @brief Set The intrinsic disto initialization mode
265259
* @param[in] distortionInitializationMode The intrintrinsic distortion initialization mode enum

‎src/aliceVision/camera/Pinhole.cpp‎

Lines changed: 17 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,7 @@ Eigen::Matrix<double, 2, 3> Pinhole::getDerivativeTransformProjectWrtPoint3(cons
9898
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_Pt;
9999
}
100100

101-
Eigen::Matrix<double, 2, Eigen::Dynamic> Pinhole::getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const
101+
Eigen::Matrix<double, 2, Eigen::Dynamic> Pinhole::getDerivativeTransformProjectWrtDistortion(const Eigen::Matrix4d& pose, const Vec4& pt) const
102102
{
103103
const Vec4 X = pose * pt; // apply pose
104104
const Vec2 P = X.head<2>() / X(2);
@@ -123,17 +123,11 @@ Eigen::Matrix<double, 2, 2> Pinhole::getDerivativeTransformProjectWrtScale(const
123123

124124
Eigen::Matrix<double, 2, Eigen::Dynamic> Pinhole::getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
125125
{
126-
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParams().size());
126+
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParameters().size());
127127

128128
ret.block<2, 2>(0, 0) = getDerivativeTransformProjectWrtScale(pose, pt3D);
129129
ret.block<2, 2>(0, 2) = getDerivativeTransformProjectWrtPrincipalPoint(pose, pt3D);
130130

131-
if (_pDistortion != nullptr)
132-
{
133-
const size_t distortionSize = _pDistortion->getDistortionParametersCount();
134-
ret.block(0, 4, 2, distortionSize) = getDerivativeTransformProjectWrtDisto(pose, pt3D);
135-
}
136-
137131
return ret;
138132
}
139133

@@ -170,15 +164,25 @@ Eigen::Matrix<double, 3, Eigen::Dynamic> Pinhole::getDerivativeBackProjectUnitWr
170164
const Vec3 ptSphere = toUnitSphere(ptUndist);
171165

172166

173-
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, getParams().size());
167+
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, getParameters().size());
174168

175169
J.block<3, 2>(0, 0) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtPt(ptMeters) * getDerivativeIma2CamWrtScale(pt2D);
176170
J.block<3, 2>(0, 2) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtPt(ptMeters) * getDerivativeIma2CamWrtPrincipalPoint();
177171

178-
if (disto_size > 0)
179-
{
180-
J.block(0, 4, 3, disto_size) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtDisto(ptMeters);
181-
}
172+
return J;
173+
}
174+
175+
Eigen::Matrix<double, 3, Eigen::Dynamic> Pinhole::getDerivativeBackProjectUnitWrtDistortion(const Vec2& pt2D) const
176+
{
177+
size_t disto_size = getDistortionParamsSize();
178+
179+
const Vec2 ptMeters = ima2cam(pt2D);
180+
const Vec2 ptUndist = removeDistortion(ptMeters);
181+
const Vec3 ptSphere = toUnitSphere(ptUndist);
182+
183+
184+
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, disto_size);
185+
J = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtDisto(ptMeters);
182186

183187

184188
return J;

0 commit comments

Comments
 (0)