int sampleSubPix(const cv::Mat &pSrc, const cv::Point2f &p) { int x = int(floorf(p.x)); int y = int(floorf(p.y)); if (x < 0 || x >= pSrc.cols - 1 || y < 0 || y >= pSrc.rows - 1) return 127; int dx = int(256 * (p.x - floorf(p.x))); int dy = int(256 * (p.y - floorf(p.y))); unsigned char* i = (unsigned char*)((pSrc.data + y * pSrc.step) + x); int a = i[0] + ((dx * (i[1] - i[0])) / 256); i += pSrc.step; int b = i[0] + ((dx * (i[1] - i[0])) / 256); return a + ((dy * (b - a)) / 256); } /* What does this code do? This function solves the following problem: The image is stored as an OpenCV matrix pSrc. Now, what we are doing is trying to get the intensity at a certain location p in the image. If p = (x,y) and x,y are natural numbers we can simply access the image pSrc.at(y,x). But since we are interested in non integer locations p, we can use this function. So what it does is bilinear interpolation (http://en.wikipedia.org/wiki/Bilinear_interpolation) of the image pSrc at location p. */