add sampson distance error measure

the first order approximation of the geometric error

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.

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<Matx33d>() * *pt1.ptr<Vec3d>();
+    Vec3d Ft_pt2 = F.ptr<Matx33d>()->t() * *pt2.ptr<Vec3d>();
+
+    double v = pt2.ptr<Vec3d>()->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. */