diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index fd50afab09..02e8b31f98 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -1403,6 +1403,17 @@ CV_EXPORTS_W void reprojectImageTo3D( InputArray disparity, bool handleMissingValues = false, int ddepth = -1 ); +/** @brief Calculates the Sampson Distance between two points. + +The function sampsonDistance calculates and returns the first order approximation of the geometric error as: +\f[sd( \texttt{pt1} , \texttt{pt2} )= \frac{(\texttt{pt2}^t \cdot \texttt{F} \cdot \texttt{pt1})^2}{(\texttt{F} \cdot \texttt{pt1})(0) + (\texttt{F} \cdot \texttt{pt1})(1) + (\texttt{F}^t \cdot \texttt{pt2})(0) + (\texttt{F}^t \cdot \texttt{pt2})(1)}\f] +The fundamental matrix may be calculated using the cv::findFundamentalMat function. See HZ 11.4.3 for details. +@param pt1 first homogeneous 2d point +@param pt2 second homogeneous 2d point +@param F fundamental matrix +*/ +CV_EXPORTS_W double sampsonDistance(InputArray pt1, InputArray pt2, InputArray F); + /** @brief Computes an optimal affine transformation between two 3D point sets. @param src First input 3D point set. diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 5d7e706a41..57e3699481 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -1039,4 +1039,24 @@ void cv::convertPointsHomogeneous( InputArray _src, OutputArray _dst ) convertPointsToHomogeneous(_src, _dst); } +double cv::sampsonDistance(InputArray _pt1, InputArray _pt2, InputArray _F) { + CV_Assert(_pt1.type() == CV_64F && _pt1.type() == CV_64F && _F.type() == CV_64F); + CV_DbgAssert(_pt1.rows() == 3 && _F.size() == Size(3, 3) && _pt1.rows() == _pt2.rows()); + + Mat pt1(_pt1.getMat()); + Mat pt2(_pt2.getMat()); + Mat F(_F.getMat()); + + Vec3d F_pt1 = *F.ptr() * *pt1.ptr(); + Vec3d Ft_pt2 = F.ptr()->t() * *pt2.ptr(); + + double v = pt2.ptr()->dot(F_pt1); + + // square + Ft_pt2 = Ft_pt2.mul(Ft_pt2); + F_pt1 = F_pt1.mul(F_pt1); + + return v*v / (F_pt1[0] + F_pt1[1] + Ft_pt2[0] + Ft_pt2[1]); +} + /* End of file. */