236 lines
6.9 KiB
C++
236 lines
6.9 KiB
C++
/**********************************************************************
|
|
Finds the N_FEATURES best features in an image, and tracks these
|
|
features to the next image. Saves the feature
|
|
locations (before and after tracking) to text files and to PPM files,
|
|
and prints the features to the screen.
|
|
**********************************************************************/
|
|
|
|
#include "klt/pnmio.h"
|
|
#include "klt/klt.h"
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
#define N_FEATURES 100
|
|
#define DELTA_1 0.05
|
|
#define DELTA_2 0.01
|
|
#define RXY_LIMIT 0.6
|
|
#define CENTER_R 0.3
|
|
|
|
//#define DEBUG_IMG
|
|
|
|
#ifdef DEBUG_IMG
|
|
void drawDotXY(unsigned char* img, int ncols, int nrows, int x, int y, int color)
|
|
{
|
|
img[x+y*ncols] = color;
|
|
}
|
|
|
|
void drawDot(unsigned char* img, int ncols, int nrows, double r0, double r10, int color)
|
|
{
|
|
if (r0>=0 && r0<1 && r10 >=0.8 && r10 <1.2)
|
|
drawDotXY (img, ncols, nrows, (int)(r0*ncols), (int)((r10-0.8)*2.5*nrows), color);
|
|
}
|
|
#endif
|
|
|
|
int calcDistortion(unsigned char* img1, unsigned char* img2, int ncols, int nrows, int nfactor, double &distortion)
|
|
{
|
|
KLT_TrackingContext tc;
|
|
KLT_FeatureList fl;
|
|
KLT_FeatureTable ft;
|
|
int i,n;
|
|
double radius, wc, hc;
|
|
|
|
double r0[N_FEATURES*nfactor];
|
|
memset(r0,0,N_FEATURES*nfactor*sizeof(double));
|
|
double r10[N_FEATURES*nfactor];
|
|
memset(r10,0,N_FEATURES*nfactor*sizeof(double));
|
|
|
|
tc = KLTCreateTrackingContext();
|
|
//tc->mindist = 20;
|
|
tc->lighting_insensitive = TRUE;
|
|
tc->nSkippedPixels = 5;
|
|
tc->step_factor = 2.0;
|
|
tc->max_iterations = 20;
|
|
//KLTPrintTrackingContext(tc);
|
|
fl = KLTCreateFeatureList(N_FEATURES*nfactor);
|
|
ft = KLTCreateFeatureTable(2, N_FEATURES*nfactor);
|
|
|
|
radius = sqrt(ncols*ncols+nrows*nrows)/2.0;
|
|
wc=((double)ncols)/2.0-0.5;
|
|
hc=((double)nrows)/2.0-0.5;
|
|
|
|
KLTSelectGoodFeatures(tc, img1, ncols, nrows, fl);
|
|
KLTStoreFeatureList(fl, ft, 0);
|
|
|
|
KLTTrackFeatures(tc, img1, img2, ncols, nrows, fl);
|
|
KLTStoreFeatureList(fl, ft, 1);
|
|
|
|
// add a shade to img2, we want to draw something on top of it.
|
|
for (i=0;i<ncols*nrows;i++) {
|
|
img2[i]=(img2[i]/2)+16;
|
|
}
|
|
// find the best comp and scale when assume r1 = r0*(1.0-comp+(r0*comp))*scale;
|
|
n=0;
|
|
double total_r10=0.0, total_r0=0.0;
|
|
for (i=0;i<N_FEATURES*nfactor;i++) {
|
|
if (ft->feature[i][1]->val>=0) {
|
|
double x0,y0,x1,y1;
|
|
x0=ft->feature[i][0]->x;
|
|
y0=ft->feature[i][0]->y;
|
|
x1=ft->feature[i][1]->x;
|
|
y1=ft->feature[i][1]->y;
|
|
|
|
r0[n]=sqrt((x0-wc)*(x0-wc)+(y0-hc)*(y0-hc))/radius;
|
|
// dots too close to the center tends to have big diviation and create noise, extract them
|
|
if (r0[n]<CENTER_R)
|
|
continue;
|
|
r10[n]=(sqrt((x1-wc)*(x1-wc)+(y1-hc)*(y1-hc))/radius)/r0[n];
|
|
total_r10 += r10[n];
|
|
total_r0 += r0[n];
|
|
n++;
|
|
} else {
|
|
ft->feature[i][0]->x=-1.0;
|
|
ft->feature[i][0]->y=-1.0;
|
|
}
|
|
}
|
|
|
|
if (n < 5) {
|
|
printf ("Not sufficient features.\n");
|
|
distortion = 0.0;
|
|
return -1;
|
|
}
|
|
|
|
double avg_r10 = total_r10 / n;
|
|
double avg_r0 = total_r0 / n;
|
|
double Sxx = 0.0;
|
|
double Sxy = 0.0;
|
|
double Syy = 0.0;
|
|
|
|
for (i=0;i<n;i++) {
|
|
Sxx += (r0[i] - avg_r0) * (r0[i] - avg_r0);
|
|
Sxy += (r0[i] - avg_r0) * (r10[i] - avg_r10);
|
|
Syy += (r10[i] - avg_r10) * (r10[i] - avg_r10);
|
|
}
|
|
|
|
double u = Sxy/Sxx;
|
|
double v = avg_r10 - u * avg_r0;
|
|
double b = u + v;
|
|
double a = u / b;
|
|
double total_delta = 0.0;
|
|
double rxy = Sxy/sqrt(Sxx*Syy);
|
|
|
|
if (rxy<0) rxy= -rxy;
|
|
|
|
int new_n=n;
|
|
// calculate deviation
|
|
for (i=0;i<n;i++) {
|
|
double delta = r10[i] - (1.0-a+(r0[i]*a))*b;
|
|
delta = delta >= 0 ? delta : -delta;
|
|
|
|
#ifdef DEBUG_IMG
|
|
drawDot(img2, ncols, nrows, r0[i], r10[i], 255);
|
|
#endif
|
|
|
|
if (delta >= DELTA_1) {
|
|
total_r10 -= r10[i];
|
|
total_r0 -= r0[i];
|
|
r0[i] = -1.0;
|
|
new_n--;
|
|
}
|
|
|
|
total_delta += delta;
|
|
}
|
|
|
|
printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy);
|
|
|
|
if (new_n < 5) {
|
|
printf ("Not sufficient features.\n");
|
|
distortion = 0.0;
|
|
return -1;
|
|
}
|
|
|
|
printf ("Removed %d outstading data points\n", n-new_n);
|
|
avg_r10 = total_r10 / new_n;
|
|
avg_r0 = total_r0 / new_n;
|
|
Sxx = 0.0;
|
|
Sxy = 0.0;
|
|
Syy = 0.0;
|
|
|
|
for (i=0;i<n;i++) {
|
|
if (r0[i]<0) continue;
|
|
Sxx += (r0[i] - avg_r0) * (r0[i] - avg_r0);
|
|
Sxy += (r0[i] - avg_r0) * (r10[i] - avg_r10);
|
|
Syy += (r10[i] - avg_r10) * (r10[i] - avg_r10);
|
|
}
|
|
|
|
u = Sxy/Sxx;
|
|
v = avg_r10 - u * avg_r0;
|
|
b = u + v;
|
|
a = u / b;
|
|
total_delta = 0.0;
|
|
rxy = Sxy/sqrt(Sxx*Syy);
|
|
if (rxy<0) rxy= -rxy;
|
|
|
|
#ifdef DEBUG_IMG
|
|
// draw lines for curve and different deviation level, for debugging purpose
|
|
for (i=0; i< ncols; i++) {
|
|
double val= (1.0-a+((i/(double)ncols)*a))*b;
|
|
if (val >=0.8 && val <1.2) {
|
|
if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255)
|
|
img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 0;
|
|
}
|
|
val += DELTA_1;
|
|
if (val >=0.8 && val <1.2) {
|
|
if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255)
|
|
img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 8;
|
|
}
|
|
val -= DELTA_1*2;
|
|
if (val >=0.8 && val <1.2) {
|
|
if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255)
|
|
img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 8;
|
|
}
|
|
val += DELTA_1+DELTA_2;
|
|
if (val >=0.8 && val <1.2) {
|
|
if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255)
|
|
img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 16;
|
|
}
|
|
val -= DELTA_2*2;
|
|
if (val >=0.8 && val <1.2) {
|
|
if (img2[i+((int)((val-0.8)*2.5*nrows))*ncols] != 255)
|
|
img2[i+((int)((val-0.8)*2.5*nrows))*ncols] = 16;
|
|
}
|
|
}
|
|
|
|
KLTExtractFeatureList(fl, ft, 0);
|
|
KLTWriteFeatureListToPPM(fl,img1,ncols,nrows,"/tmp/feat0.ppm");
|
|
KLTExtractFeatureList(fl, ft, 1);
|
|
KLTWriteFeatureListToPPM(fl,img2,ncols,nrows,"/tmp/feat1.ppm");
|
|
#endif
|
|
|
|
// calculate deviation
|
|
for (i=0;i<n;i++) {
|
|
if (r0[i]<0) continue;
|
|
double delta = r10[i] - (1.0-a+(r0[i]*a))*b;
|
|
delta = delta >= 0 ? delta : -delta;
|
|
total_delta += delta;
|
|
}
|
|
|
|
printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy);
|
|
|
|
if (total_delta / new_n > DELTA_2) {
|
|
printf ("Deviation is too big.\n");
|
|
distortion = 0.0;
|
|
return -2;
|
|
}
|
|
|
|
if (rxy < RXY_LIMIT) {
|
|
printf ("Not linear enough\n");
|
|
distortion = 0.0;
|
|
return -3;
|
|
}
|
|
|
|
printf ("distortion amount=%lf scale=%lf deviation=%lf, rxy=%lf\n", a, b, total_delta/n, rxy);
|
|
distortion = a;
|
|
return 1;
|
|
}
|
|
|