Skip to content

Commit 303ff47

Browse files
cbentejaccbentejac
authored
Merge pull request #1778 from alicevision/dev/mergeSfm
Merge symbolic and automatic SfM
2 parents e6c666f + 086f596 commit 303ff47

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

48 files changed

+859
-3592
lines changed

‎pyTests/camera/test_equidistant.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -120,7 +120,7 @@
120120
# - Vec2 project(geometry::Pose3& pose, Vec4& pt3D, bool applyDistortion = true) /
121121
# Vec2, Pose3 and Vec4 not binded
122122
# - Vec2 project(Eigen::Matrix4d& pose, Vec4& pt3D, bool applyDistortion = true)
123-
# - Vec3 backproject(Vec2& pt2D, bool applyUndistortion = true,
123+
# - Vec3 backprojectTransform(Vec2& pt2D, bool applyUndistortion = true,
124124
# geometry::Pose3& pose = geometry::Pose3(),
125125
# double depth = 1.0) / Vec3, Vec2 and Pose3 not binded
126126
# - Vec4 getCartesianfromSphericalCoordinates(Vec3& pt) / Vec3 not binded

‎pyTests/camera/test_pinhole.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -110,7 +110,7 @@
110110
# - Vec2 project(geometry::Pose3& pose, Vec4& pt3D, bool applyDistortion = true) /
111111
# Vec2, Pose3 and Vec4 not binded
112112
# - Vec2 project(Eigen::Matrix4d& pose, Vec4& pt3D, bool applyDistortion = true)
113-
# - Vec3 backproject(Vec2& pt2D, bool applyUndistortion = true,
113+
# - Vec3 backprojectTransform(Vec2& pt2D, bool applyUndistortion = true,
114114
# geometry::Pose3& pose = geometry::Pose3(),
115115
# double depth = 1.0) / Vec3, Vec2 and Pose3 not binded
116116
# - Vec4 getCartesianfromSphericalCoordinates(Vec3& pt) / Vec3 not binded

‎src/aliceVision/camera/Equidistant.cpp‎

Lines changed: 59 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
namespace aliceVision {
1313
namespace camera {
1414

15-
Vec2 Equidistant::project(const Eigen::Matrix4d& pose, const Vec4& pt, bool applyDistortion) const
15+
Vec2 Equidistant::transformProject(const Eigen::Matrix4d& pose, const Vec4& pt, bool applyDistortion) const
1616
{
1717
const double rsensor = std::min(sensorWidth(), sensorHeight());
1818
const double rscale = sensorWidth() / std::max(w(), h());
@@ -38,7 +38,31 @@ Vec2 Equidistant::project(const Eigen::Matrix4d& pose, const Vec4& pt, bool appl
3838
return pt_ima;
3939
}
4040

41-
Eigen::Matrix<double, 2, 9> Equidistant::getDerivativeProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const
41+
Vec2 Equidistant::project(const Vec4& pt, bool applyDistortion) const
42+
{
43+
const double rsensor = std::min(sensorWidth(), sensorHeight());
44+
const double rscale = sensorWidth() / std::max(w(), h());
45+
const double fmm = _scale(0) * rscale;
46+
const double fov = rsensor / fmm;
47+
48+
// Compute angle with optical center
49+
const double angle_Z = std::atan2(sqrt(pt(0) * pt(0) + pt(1) * pt(1)), pt(2));
50+
51+
// Ignore depth component and compute radial angle
52+
const double angle_radial = std::atan2(pt(1), pt(0));
53+
54+
const double radius = angle_Z / (0.5 * fov);
55+
56+
// radius = focal * angle_Z
57+
const Vec2 P{cos(angle_radial) * radius, sin(angle_radial) * radius};
58+
59+
const Vec2 pt_disto = applyDistortion ? this->addDistortion(P) : P;
60+
const Vec2 pt_ima = this->cam2ima(pt_disto);
61+
62+
return pt_ima;
63+
}
64+
65+
Eigen::Matrix<double, 2, 9> Equidistant::getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const
4266
{
4367
Eigen::Matrix4d T = pose;
4468
const Vec4 X = T * pt; // apply pose
@@ -87,7 +111,7 @@ Eigen::Matrix<double, 2, 9> Equidistant::getDerivativeProjectWrtRotation(const E
87111
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_R;
88112
}
89113

90-
Eigen::Matrix<double, 2, 16> Equidistant::getDerivativeProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const
114+
Eigen::Matrix<double, 2, 16> Equidistant::getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const
91115
{
92116
Eigen::Matrix4d T = pose;
93117
const Vec4 X = T * pt; // apply pose
@@ -136,7 +160,7 @@ Eigen::Matrix<double, 2, 16> Equidistant::getDerivativeProjectWrtPose(const Eige
136160
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_T.block<3, 16>(0, 0);
137161
}
138162

139-
Eigen::Matrix<double, 2, 16> Equidistant::getDerivativeProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const
163+
Eigen::Matrix<double, 2, 16> Equidistant::getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const
140164
{
141165
Eigen::Matrix4d T = pose;
142166
const Vec4 X = T * pt; // apply pose
@@ -185,7 +209,7 @@ Eigen::Matrix<double, 2, 16> Equidistant::getDerivativeProjectWrtPoseLeft(const
185209
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_T.block<3, 16>(0, 0);
186210
}
187211

188-
Eigen::Matrix<double, 2, 4> Equidistant::getDerivativeProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
212+
Eigen::Matrix<double, 2, 4> Equidistant::getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
189213
{
190214
Eigen::Matrix4d T = pose;
191215
const Vec4 X = T * pt; // apply pose
@@ -235,7 +259,7 @@ Eigen::Matrix<double, 2, 4> Equidistant::getDerivativeProjectWrtPoint(const Eige
235259
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_pt;
236260
}
237261

238-
Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeProjectWrtPoint3(const Eigen::Matrix4d& T, const Vec4& pt) const
262+
Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& T, const Vec4& pt) const
239263
{
240264
const Vec4 X = T * pt; // apply pose
241265

@@ -284,7 +308,7 @@ Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeProjectWrtPoint3(const Eig
284308
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_pt;
285309
}
286310

287-
Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const
311+
Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const
288312
{
289313
Eigen::Matrix4d T = pose;
290314
const Vec4 X = T * pt; // apply pose
@@ -308,7 +332,7 @@ Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeProjectWrtDisto(const Eige
308332
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtDisto(P);
309333
}
310334

311-
Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const
335+
Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const
312336
{
313337
Eigen::Matrix4d T = pose;
314338
const Vec4 X = T * pt; // apply pose
@@ -343,22 +367,22 @@ Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeProjectWrtScale(const Eige
343367
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_radius * d_radius_d_fov * d_fov_d_scale;
344368
}
345369

346-
Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
370+
Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
347371
{
348372
return getDerivativeCam2ImaWrtPrincipalPoint();
349373
}
350374

351-
Eigen::Matrix<double, 2, Eigen::Dynamic> Equidistant::getDerivativeProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
375+
Eigen::Matrix<double, 2, Eigen::Dynamic> Equidistant::getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
352376
{
353377
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParams().size());
354378

355-
ret.block<2, 2>(0, 0) = getDerivativeProjectWrtScale(pose, pt3D);
356-
ret.block<2, 2>(0, 2) = getDerivativeProjectWrtPrincipalPoint(pose, pt3D);
379+
ret.block<2, 2>(0, 0) = getDerivativeTransformProjectWrtScale(pose, pt3D);
380+
ret.block<2, 2>(0, 2) = getDerivativeTransformProjectWrtPrincipalPoint(pose, pt3D);
357381

358382
if (_pDistortion != nullptr)
359383
{
360384
const size_t distortionSize = _pDistortion->getDistortionParametersCount();
361-
ret.block(0, 4, 2, distortionSize) = getDerivativeProjectWrtDisto(pose, pt3D);
385+
ret.block(0, 4, 2, distortionSize) = getDerivativeTransformProjectWrtDisto(pose, pt3D);
362386
}
363387

364388
return ret;
@@ -436,6 +460,27 @@ Eigen::Matrix<double, 3, 2> Equidistant::getDerivativetoUnitSphereWrtScale(const
436460
return d_ret_d_angles * d_angles_d_fov * d_fov_d_scale;
437461
}
438462

463+
Eigen::Matrix<double, 3, Eigen::Dynamic> Equidistant::getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const
464+
{
465+
size_t disto_size = getDistortionParamsSize();
466+
467+
const Vec2 ptMeters = ima2cam(pt2D);
468+
const Vec2 ptUndist = removeDistortion(ptMeters);
469+
const Vec3 ptSphere = toUnitSphere(ptUndist);
470+
471+
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, getParams().size());
472+
473+
J.block<3, 2>(0, 0) = getDerivativetoUnitSphereWrtScale(ptUndist);
474+
J.block<3, 2>(0, 2) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtPt(ptMeters) * getDerivativeIma2CamWrtPrincipalPoint();
475+
476+
if (disto_size > 0)
477+
{
478+
J.block(0, 4, 3, disto_size) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtDisto(ptMeters);
479+
}
480+
481+
return J;
482+
}
483+
439484
double Equidistant::imagePlaneToCameraPlaneError(double value) const { return value / _scale(0); }
440485

441486
Vec2 Equidistant::cam2ima(const Vec2& p) const { return _circleRadius * p + getPrincipalPoint(); }
@@ -459,7 +504,7 @@ bool Equidistant::isVisibleRay(const Vec3& ray) const
459504
if (std::abs(angle) > 1.2 * (0.5 * fov))
460505
return false;
461506

462-
const Vec2 proj = project(Eigen::Matrix4d::Identity(), ray.homogeneous(), true);
507+
const Vec2 proj = transformProject(Eigen::Matrix4d::Identity(), ray.homogeneous(), true);
463508
const Vec2 centered = proj - Vec2(_circleCenter(0), _circleCenter(1));
464509
return centered.norm() <= _circleRadius;
465510
}

‎src/aliceVision/camera/Equidistant.hpp‎

Lines changed: 21 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -65,37 +65,46 @@ class Equidistant : public IntrinsicScaleOffsetDisto
6565

6666
EINTRINSIC getType() const override;
6767

68-
Vec2 project(const Eigen::Matrix4d& pose, const Vec4& pt, bool applyDistortion = true) const override;
68+
Vec2 transformProject(const Eigen::Matrix4d& pose, const Vec4& pt, bool applyDistortion = true) const override;
6969

70-
Vec2 project(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
70+
Vec2 transformProject(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
7171
{
72-
return project(pose.getHomogeneous(), pt3D, applyDistortion);
72+
return transformProject(pose.getHomogeneous(), pt3D, applyDistortion);
7373
}
7474

75-
Eigen::Matrix<double, 2, 9> getDerivativeProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const;
75+
Vec2 project(const Vec4& pt, bool applyDistortion = true) const override;
7676

77-
Eigen::Matrix<double, 2, 16> getDerivativeProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
77+
Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
7878

79-
Eigen::Matrix<double, 2, 16> getDerivativeProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
79+
Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
8080

81-
Eigen::Matrix<double, 2, 4> getDerivativeProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
81+
Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
8282

83-
Eigen::Matrix<double, 2, 3> getDerivativeProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
83+
Eigen::Matrix<double, 2, 4> getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
8484

85-
Eigen::Matrix<double, 2, 3> getDerivativeProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const;
85+
Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
8686

87-
Eigen::Matrix<double, 2, 2> getDerivativeProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const;
87+
Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const;
8888

89-
Eigen::Matrix<double, 2, 2> getDerivativeProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const;
89+
Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const;
9090

91-
Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const override;
91+
Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const;
92+
93+
Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const override;
9294

9395
Vec3 toUnitSphere(const Vec2& pt) const override;
9496

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

9799
Eigen::Matrix<double, 3, 2> getDerivativetoUnitSphereWrtScale(const Vec2& pt) const;
98100

101+
/**
102+
* @brief Get the derivative of the unit sphere backprojection
103+
* @param[in] pt2D The 2D point
104+
* @return The backproject jacobian with respect to the pose
105+
*/
106+
Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const override;
107+
99108
double imagePlaneToCameraPlaneError(double value) const override;
100109

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

‎src/aliceVision/camera/IntrinsicBase.cpp‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ bool IntrinsicBase::operator==(const IntrinsicBase& other) const
1717
_serialNumber == other._serialNumber && _initializationMode == other._initializationMode && getType() == other.getType();
1818
}
1919

20-
Vec3 IntrinsicBase::backproject(const Vec2& pt2D, bool applyUndistortion, const geometry::Pose3& pose, double depth) const
20+
Vec3 IntrinsicBase::backprojectTransform(const Vec2& pt2D, bool applyUndistortion, const geometry::Pose3& pose, double depth) const
2121
{
2222
const Vec2 pt2D_cam = ima2cam(pt2D);
2323
const Vec2 pt2D_undist = applyUndistortion ? removeDistortion(pt2D_cam) : pt2D_cam;
@@ -27,6 +27,15 @@ Vec3 IntrinsicBase::backproject(const Vec2& pt2D, bool applyUndistortion, const
2727
return output;
2828
}
2929

30+
Vec3 IntrinsicBase::backProjectUnit(const Vec2& pt2D) const
31+
{
32+
const Vec2 ptMeters = ima2cam(pt2D);
33+
const Vec2 ptUndist = removeDistortion(ptMeters);
34+
const Vec3 ptSphere = toUnitSphere(ptUndist);
35+
36+
return ptSphere;
37+
}
38+
3039
Vec4 IntrinsicBase::getCartesianfromSphericalCoordinates(const Vec3& pt)
3140
{
3241
Vec4 rpt;

‎src/aliceVision/camera/IntrinsicBase.hpp‎

Lines changed: 40 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -93,9 +93,9 @@ class IntrinsicBase
9393
* @param[in] applyDistortion If true, apply the distortion if there is any
9494
* @return The 2D projection in the camera plane
9595
*/
96-
Vec2 project(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
96+
Vec2 transformProject(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
9797
{
98-
return project(pose.getHomogeneous(), pt3D, applyDistortion);
98+
return transformProject(pose.getHomogeneous(), pt3D, applyDistortion);
9999
}
100100

101101
/**
@@ -105,7 +105,15 @@ class IntrinsicBase
105105
* @param[in] applyDistortion If true, apply the distortion if there is any
106106
* @return The 2D projection in the camera plane
107107
*/
108-
virtual Vec2 project(const Eigen::Matrix4d& pose, const Vec4& pt3D, bool applyDistortion = true) const = 0;
108+
virtual Vec2 transformProject(const Eigen::Matrix4d& pose, const Vec4& pt3D, bool applyDistortion = true) const = 0;
109+
110+
/**
111+
* @brief Projection of a 3D point into the camera plane (Apply disto (if any) and Intrinsics)
112+
* @param[in] pt3D The 3D point
113+
* @param[in] applyDistortion If true, apply the distortion if there is any
114+
* @return The 2D projection in the camera plane
115+
*/
116+
virtual Vec2 project(const Vec4& pt3D, bool applyDistortion = true) const = 0;
109117

110118
/**
111119
* @brief Back-projection of a 2D point at a specific depth into a 3D point
@@ -115,7 +123,14 @@ class IntrinsicBase
115123
* @param[in] depth The depth
116124
* @return The 3D point
117125
*/
118-
Vec3 backproject(const Vec2& pt2D, bool applyUndistortion = true, const geometry::Pose3& pose = geometry::Pose3(), double depth = 1.0) const;
126+
Vec3 backprojectTransform(const Vec2& pt2D, bool applyUndistortion = true, const geometry::Pose3& pose = geometry::Pose3(), double depth = 1.0) const;
127+
128+
/**
129+
* @brief Back-projection of a 2D point on a unitsphere
130+
* @param[in] pt2D The 2D point
131+
* @return The 3D point
132+
*/
133+
Vec3 backProjectUnit(const Vec2& pt2D) const;
119134

120135
Vec4 getCartesianfromSphericalCoordinates(const Vec3& pt);
121136

@@ -127,39 +142,54 @@ class IntrinsicBase
127142
* @param[in] pt3D The 3D point
128143
* @return The projection jacobian with respect to the pose
129144
*/
130-
virtual Eigen::Matrix<double, 2, 16> getDerivativeProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
145+
virtual Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
146+
147+
/**
148+
* @brief Get the derivative of a projection of a 3D point into the camera plane
149+
* @param[in] pose The pose
150+
* @param[in] pt3D The 3D point
151+
* @return The projection jacobian with respect to the rotation
152+
*/
153+
virtual Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const = 0;
131154

132155
/**
133156
* @brief Get the derivative of a projection of a 3D point into the camera plane
134157
* @param[in] pose The pose
135158
* @param[in] pt3D The 3D point
136159
* @return The projection jacobian with respect to the pose
137160
*/
138-
virtual Eigen::Matrix<double, 2, 16> getDerivativeProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
161+
virtual Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
139162

140163
/**
141164
* @brief Get the derivative of a projection of a 3D point into the camera plane
142165
* @param[in] pose The pose
143166
* @param[in] pt3D The 3D point
144167
* @return The projection jacobian with respect to the point
145168
*/
146-
virtual Eigen::Matrix<double, 2, 4> getDerivativeProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
169+
virtual Eigen::Matrix<double, 2, 4> getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
147170

148171
/**
149172
* @brief Get the derivative of a projection of a 3D point into the camera plane
150173
* @param[in] pose The pose
151174
* @param[in] pt3D The 3D point
152175
* @return The projection jacobian with respect to the point
153176
*/
154-
virtual Eigen::Matrix<double, 2, 3> getDerivativeProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
177+
virtual Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
155178

156179
/**
157180
* @brief Get the derivative of a projection of a 3D point into the camera plane
158181
* @param[in] pose The pose
159182
* @param[in] pt3D The 3D point
160183
* @return The projection jacobian with respect to the params
161184
*/
162-
virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeProjectWrtParams(const Eigen::Matrix4d& pos, const Vec4& pt3D) const = 0;
185+
virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pos, const Vec4& pt3D) const = 0;
186+
187+
/**
188+
* @brief Get the derivative of the unit sphere backprojection
189+
* @param[in] pt2D The 2D point
190+
* @return The backproject jacobian with respect to the pose
191+
*/
192+
virtual Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const = 0;
163193

164194
/**
165195
* @brief Compute the residual between the 3D projected point X and an image observation x
@@ -172,7 +202,7 @@ class IntrinsicBase
172202
inline Vec2 residual(const geometry::Pose3& pose, const Vec4& X, const Vec2& x, bool applyDistortion = true) const
173203
{
174204
// We will compare to an undistorted point, so always ignore the distortion when computing coordinates
175-
const Vec2 proj = this->project(pose, X, false);
205+
const Vec2 proj = this->transformProject(pose, X, false);
176206

177207
return ((applyDistortion)?this->getUndistortedPixel(x):x) - proj;
178208
}

0 commit comments

Comments
 (0)