Skip to content

Commit

Permalink
Merge pull request #1778 from alicevision/dev/mergeSfm
Browse files Browse the repository at this point in the history 
Merge symbolic and automatic SfM
  • Loading branch information
@cbentejac
cbentejac authored Nov 18, 2024
2 parents e6c666f + 086f596 commit 303ff47
Show file tree
Hide file tree
Showing 48 changed files with 859 additions and 3,592 deletions.
2 changes: 1 addition & 1 deletion pyTests/camera/test_equidistant.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@
# - Vec2 project(geometry::Pose3& pose, Vec4& pt3D, bool applyDistortion = true) /
# Vec2, Pose3 and Vec4 not binded
# - Vec2 project(Eigen::Matrix4d& pose, Vec4& pt3D, bool applyDistortion = true)
# - Vec3 backproject(Vec2& pt2D, bool applyUndistortion = true,
# - Vec3 backprojectTransform(Vec2& pt2D, bool applyUndistortion = true,
# geometry::Pose3& pose = geometry::Pose3(),
# double depth = 1.0) / Vec3, Vec2 and Pose3 not binded
# - Vec4 getCartesianfromSphericalCoordinates(Vec3& pt) / Vec3 not binded
Expand Down
2 changes: 1 addition & 1 deletion pyTests/camera/test_pinhole.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@
# - Vec2 project(geometry::Pose3& pose, Vec4& pt3D, bool applyDistortion = true) /
# Vec2, Pose3 and Vec4 not binded
# - Vec2 project(Eigen::Matrix4d& pose, Vec4& pt3D, bool applyDistortion = true)
# - Vec3 backproject(Vec2& pt2D, bool applyUndistortion = true,
# - Vec3 backprojectTransform(Vec2& pt2D, bool applyUndistortion = true,
# geometry::Pose3& pose = geometry::Pose3(),
# double depth = 1.0) / Vec3, Vec2 and Pose3 not binded
# - Vec4 getCartesianfromSphericalCoordinates(Vec3& pt) / Vec3 not binded
Expand Down
73 changes: 59 additions & 14 deletions src/aliceVision/camera/Equidistant.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
namespace aliceVision {
namespace camera {

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

Eigen::Matrix<double, 2, 9> Equidistant::getDerivativeProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const
Vec2 Equidistant::project(const Vec4& pt, bool applyDistortion) const
{
const double rsensor = std::min(sensorWidth(), sensorHeight());
const double rscale = sensorWidth() / std::max(w(), h());
const double fmm = _scale(0) * rscale;
const double fov = rsensor / fmm;

// Compute angle with optical center
const double angle_Z = std::atan2(sqrt(pt(0) * pt(0) + pt(1) * pt(1)), pt(2));

// Ignore depth component and compute radial angle
const double angle_radial = std::atan2(pt(1), pt(0));

const double radius = angle_Z / (0.5 * fov);

// radius = focal * angle_Z
const Vec2 P{cos(angle_radial) * radius, sin(angle_radial) * radius};

const Vec2 pt_disto = applyDistortion ? this->addDistortion(P) : P;
const Vec2 pt_ima = this->cam2ima(pt_disto);

return pt_ima;
}

Eigen::Matrix<double, 2, 9> Equidistant::getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const
{
Eigen::Matrix4d T = pose;
const Vec4 X = T * pt; // apply pose
Expand Down Expand Up @@ -87,7 +111,7 @@ Eigen::Matrix<double, 2, 9> Equidistant::getDerivativeProjectWrtRotation(const E
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_R;
}

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

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

Eigen::Matrix<double, 2, 4> Equidistant::getDerivativeProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
Eigen::Matrix<double, 2, 4> Equidistant::getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
{
Eigen::Matrix4d T = pose;
const Vec4 X = T * pt; // apply pose
Expand Down Expand Up @@ -235,7 +259,7 @@ Eigen::Matrix<double, 2, 4> Equidistant::getDerivativeProjectWrtPoint(const Eige
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_pt;
}

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

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

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

Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const
Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const
{
Eigen::Matrix4d T = pose;
const Vec4 X = T * pt; // apply pose
Expand Down Expand Up @@ -343,22 +367,22 @@ Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeProjectWrtScale(const Eige
return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_radius * d_radius_d_fov * d_fov_d_scale;
}

Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
Eigen::Matrix<double, 2, 2> Equidistant::getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
{
return getDerivativeCam2ImaWrtPrincipalPoint();
}

Eigen::Matrix<double, 2, Eigen::Dynamic> Equidistant::getDerivativeProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
Eigen::Matrix<double, 2, Eigen::Dynamic> Equidistant::getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
{
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParams().size());

ret.block<2, 2>(0, 0) = getDerivativeProjectWrtScale(pose, pt3D);
ret.block<2, 2>(0, 2) = getDerivativeProjectWrtPrincipalPoint(pose, pt3D);
ret.block<2, 2>(0, 0) = getDerivativeTransformProjectWrtScale(pose, pt3D);
ret.block<2, 2>(0, 2) = getDerivativeTransformProjectWrtPrincipalPoint(pose, pt3D);

if (_pDistortion != nullptr)
{
const size_t distortionSize = _pDistortion->getDistortionParametersCount();
ret.block(0, 4, 2, distortionSize) = getDerivativeProjectWrtDisto(pose, pt3D);
ret.block(0, 4, 2, distortionSize) = getDerivativeTransformProjectWrtDisto(pose, pt3D);
}

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

Eigen::Matrix<double, 3, Eigen::Dynamic> Equidistant::getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const
{
size_t disto_size = getDistortionParamsSize();

const Vec2 ptMeters = ima2cam(pt2D);
const Vec2 ptUndist = removeDistortion(ptMeters);
const Vec3 ptSphere = toUnitSphere(ptUndist);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> J(3, getParams().size());

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

if (disto_size > 0)
{
J.block(0, 4, 3, disto_size) = getDerivativetoUnitSphereWrtPoint(ptUndist) * getDerivativeRemoveDistoWrtDisto(ptMeters);
}

return J;
}

double Equidistant::imagePlaneToCameraPlaneError(double value) const { return value / _scale(0); }

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

const Vec2 proj = project(Eigen::Matrix4d::Identity(), ray.homogeneous(), true);
const Vec2 proj = transformProject(Eigen::Matrix4d::Identity(), ray.homogeneous(), true);
const Vec2 centered = proj - Vec2(_circleCenter(0), _circleCenter(1));
return centered.norm() <= _circleRadius;
}
Expand Down
33 changes: 21 additions & 12 deletions src/aliceVision/camera/Equidistant.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,37 +65,46 @@ class Equidistant : public IntrinsicScaleOffsetDisto

EINTRINSIC getType() const override;

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

Vec2 project(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
Vec2 transformProject(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
{
return project(pose.getHomogeneous(), pt3D, applyDistortion);
return transformProject(pose.getHomogeneous(), pt3D, applyDistortion);
}

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

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

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

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

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

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

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

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

Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const override;
Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const;

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

Vec3 toUnitSphere(const Vec2& pt) const override;

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

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

/**
* @brief Get the derivative of the unit sphere backprojection
* @param[in] pt2D The 2D point
* @return The backproject jacobian with respect to the pose
*/
Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const override;

double imagePlaneToCameraPlaneError(double value) const override;

// Transform a point from the camera plane to the image plane
Expand Down
11 changes: 10 additions & 1 deletion src/aliceVision/camera/IntrinsicBase.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ bool IntrinsicBase::operator==(const IntrinsicBase& other) const
_serialNumber == other._serialNumber && _initializationMode == other._initializationMode && getType() == other.getType();
}

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

Vec3 IntrinsicBase::backProjectUnit(const Vec2& pt2D) const
{
const Vec2 ptMeters = ima2cam(pt2D);
const Vec2 ptUndist = removeDistortion(ptMeters);
const Vec3 ptSphere = toUnitSphere(ptUndist);

return ptSphere;
}

Vec4 IntrinsicBase::getCartesianfromSphericalCoordinates(const Vec3& pt)
{
Vec4 rpt;
Expand Down
50 changes: 40 additions & 10 deletions src/aliceVision/camera/IntrinsicBase.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,9 +93,9 @@ class IntrinsicBase
* @param[in] applyDistortion If true, apply the distortion if there is any
* @return The 2D projection in the camera plane
*/
Vec2 project(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
Vec2 transformProject(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
{
return project(pose.getHomogeneous(), pt3D, applyDistortion);
return transformProject(pose.getHomogeneous(), pt3D, applyDistortion);
}

/**
Expand All @@ -105,7 +105,15 @@ class IntrinsicBase
* @param[in] applyDistortion If true, apply the distortion if there is any
* @return The 2D projection in the camera plane
*/
virtual Vec2 project(const Eigen::Matrix4d& pose, const Vec4& pt3D, bool applyDistortion = true) const = 0;
virtual Vec2 transformProject(const Eigen::Matrix4d& pose, const Vec4& pt3D, bool applyDistortion = true) const = 0;

/**
* @brief Projection of a 3D point into the camera plane (Apply disto (if any) and Intrinsics)
* @param[in] pt3D The 3D point
* @param[in] applyDistortion If true, apply the distortion if there is any
* @return The 2D projection in the camera plane
*/
virtual Vec2 project(const Vec4& pt3D, bool applyDistortion = true) const = 0;

/**
* @brief Back-projection of a 2D point at a specific depth into a 3D point
Expand All @@ -115,7 +123,14 @@ class IntrinsicBase
* @param[in] depth The depth
* @return The 3D point
*/
Vec3 backproject(const Vec2& pt2D, bool applyUndistortion = true, const geometry::Pose3& pose = geometry::Pose3(), double depth = 1.0) const;
Vec3 backprojectTransform(const Vec2& pt2D, bool applyUndistortion = true, const geometry::Pose3& pose = geometry::Pose3(), double depth = 1.0) const;

/**
* @brief Back-projection of a 2D point on a unitsphere
* @param[in] pt2D The 2D point
* @return The 3D point
*/
Vec3 backProjectUnit(const Vec2& pt2D) const;

Vec4 getCartesianfromSphericalCoordinates(const Vec3& pt);

Expand All @@ -127,39 +142,54 @@ class IntrinsicBase
* @param[in] pt3D The 3D point
* @return The projection jacobian with respect to the pose
*/
virtual Eigen::Matrix<double, 2, 16> getDerivativeProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
virtual Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;

/**
* @brief Get the derivative of a projection of a 3D point into the camera plane
* @param[in] pose The pose
* @param[in] pt3D The 3D point
* @return The projection jacobian with respect to the rotation
*/
virtual Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const = 0;

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

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

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

/**
* @brief Get the derivative of a projection of a 3D point into the camera plane
* @param[in] pose The pose
* @param[in] pt3D The 3D point
* @return The projection jacobian with respect to the params
*/
virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeProjectWrtParams(const Eigen::Matrix4d& pos, const Vec4& pt3D) const = 0;
virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pos, const Vec4& pt3D) const = 0;

/**
* @brief Get the derivative of the unit sphere backprojection
* @param[in] pt2D The 2D point
* @return The backproject jacobian with respect to the pose
*/
virtual Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const = 0;

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

return ((applyDistortion)?this->getUndistortedPixel(x):x) - proj;
}
Expand Down
Loading

0 comments on commit 303ff47

Please sign in to comment.