//
// Interpolates a scale-space extremum's location and scale to subpixel
// accuracy to form an image feature. Rejects features with low contrast.
// Based on Section 4 of Lowe's paper.
static bool adjustLocalExtrema( const vector<Mat>& dog_pyr, KeyPoint& kpt, int octv,
int& layer, int& r, int& c, int nOctaveLayers,
float contrastThreshold, float edgeThreshold, float sigma )
{
const float img_scale = 1.f/(255*SIFT_FIXPT_SCALE);
const float deriv_scale = img_scale*0.5f;
const float second_deriv_scale = img_scale;
const float cross_deriv_scale = img_scale*0.25f;
float xi=0, xr=0, xc=0, contr=0;
int i = 0;
for( ; i < SIFT_MAX_INTERP_STEPS; i++ )
{
int idx = octv*(nOctaveLayers+2) + layer;
const Mat& img = dog_pyr[idx];
const Mat& prev = dog_pyr[idx-1];
const Mat& next = dog_pyr[idx+1];
Vec3f dD((img.at<sift_wt>(r, c+1) - img.at<sift_wt>(r, c-1))*deriv_scale,
(img.at<sift_wt>(r+1, c) - img.at<sift_wt>(r-1, c))*deriv_scale,
(next.at<sift_wt>(r, c) - prev.at<sift_wt>(r, c))*deriv_scale);
float v2 = (float)img.at<sift_wt>(r, c)*2;
float dxx = (img.at<sift_wt>(r, c+1) + img.at<sift_wt>(r, c-1) - v2)*second_deriv_scale;
float dyy = (img.at<sift_wt>(r+1, c) + img.at<sift_wt>(r-1, c) - v2)*second_deriv_scale;
float dss = (next.at<sift_wt>(r, c) + prev.at<sift_wt>(r, c) - v2)*second_deriv_scale;
float dxy = (img.at<sift_wt>(r+1, c+1) - img.at<sift_wt>(r+1, c-1) -
img.at<sift_wt>(r-1, c+1) + img.at<sift_wt>(r-1, c-1))*cross_deriv_scale;
float dxs = (next.at<sift_wt>(r, c+1) - next.at<sift_wt>(r, c-1) -
prev.at<sift_wt>(r, c+1) + prev.at<sift_wt>(r, c-1))*cross_deriv_scale;
float dys = (next.at<sift_wt>(r+1, c) - next.at<sift_wt>(r-1, c) -
prev.at<sift_wt>(r+1, c) + prev.at<sift_wt>(r-1, c))*cross_deriv_scale;
Matx33f H(dxx, dxy, dxs,
dxy, dyy, dys,
dxs, dys, dss);
Vec3f X = H.solve(dD, DECOMP_LU);
xi = -X[2];
xr = -X[1];
xc = -X[0];
if( std::abs(xi) < 0.5f && std::abs(xr) < 0.5f && std::abs(xc) < 0.5f )
break;
if( std::abs(xi) > (float)(INT_MAX/3) ||
std::abs(xr) > (float)(INT_MAX/3) ||
std::abs(xc) > (float)(INT_MAX/3) )
return false;
c += cvRound(xc);
r += cvRound(xr);
layer += cvRound(xi);
if( layer < 1 || layer > nOctaveLayers ||
c < SIFT_IMG_BORDER || c >= img.cols - SIFT_IMG_BORDER ||
r < SIFT_IMG_BORDER || r >= img.rows - SIFT_IMG_BORDER )
return false;
}
// ensure convergence of interpolation
if( i >= SIFT_MAX_INTERP_STEPS )
return false;
{
int idx = octv*(nOctaveLayers+2) + layer;
const Mat& img = dog_pyr[idx];
const Mat& prev = dog_pyr[idx-1];
const Mat& next = dog_pyr[idx+1];
Matx31f dD((img.at<sift_wt>(r, c+1) - img.at<sift_wt>(r, c-1))*deriv_scale,
(img.at<sift_wt>(r+1, c) - img.at<sift_wt>(r-1, c))*deriv_scale,
(next.at<sift_wt>(r, c) - prev.at<sift_wt>(r, c))*deriv_scale);
float t = dD.dot(Matx31f(xc, xr, xi));
contr = img.at<sift_wt>(r, c)*img_scale + t * 0.5f;
if( std::abs( contr ) * nOctaveLayers < contrastThreshold )
return false;
// principal curvatures are computed using the trace and det of Hessian
float v2 = img.at<sift_wt>(r, c)*2.f;
float dxx = (img.at<sift_wt>(r, c+1) + img.at<sift_wt>(r, c-1) - v2)*second_deriv_scale;
float dyy = (img.at<sift_wt>(r+1, c) + img.at<sift_wt>(r-1, c) - v2)*second_deriv_scale;
float dxy = (img.at<sift_wt>(r+1, c+1) - img.at<sift_wt>(r+1, c-1) -
img.at<sift_wt>(r-1, c+1) + img.at<sift_wt>(r-1, c-1)) * cross_deriv_scale;
float tr = dxx + dyy;
float det = dxx * dyy - dxy * dxy;
if( det <= 0 || tr*tr*edgeThreshold >= (edgeThreshold + 1)*(edgeThreshold + 1)*det )
return false;
}
kpt.pt.x = (c + xc) * (1 << octv);
kpt.pt.y = (r + xr) * (1 << octv);
kpt.octave = octv + (layer << 8) + (cvRound((xi + 0.5)*255) << 16);
kpt.size = sigma*powf(2.f, (layer + xi) / nOctaveLayers)*(1 << octv)*2;
kpt.response = std::abs(contr);
return true;
}
Point3f Warp::transform(Point3f p)
{
Matx31f P = R * Matx31f(p.x, p.y, p.z);
return Point3f(P(0), P(1), P(2)) + t;
}
