163 lines
4.3 KiB
C++
163 lines
4.3 KiB
C++
#ifndef FptPointMatcher_h
|
|
#define FptPointMatcher_h
|
|
|
|
|
|
|
|
#define FPT_MAX_FEATURE_POINTS_NUMBER 10000
|
|
|
|
|
|
class FptFeaturePoint_
|
|
{
|
|
public:
|
|
FptFeaturePoint_(void)
|
|
{
|
|
onBackground = false;
|
|
correspondentKF = NULL;
|
|
correspondentPF = NULL;
|
|
point3DKnown = false;
|
|
};
|
|
|
|
bool point3DKnown;
|
|
bool onBackground;
|
|
|
|
bool inlierKF;
|
|
bool inlierPF;
|
|
bool inlierNF;
|
|
|
|
double originalU, originalV;
|
|
double projectedUKF, projectedVKF;
|
|
double projectedUPF, projectedVPF;
|
|
double projectedUNF, projectedVNF;
|
|
double correspondentU, correspondentV;
|
|
double X, Y, Z;
|
|
|
|
// Tangent plane (with respect to the object coordinate system)
|
|
double normal[3], d;
|
|
|
|
// Correspondents:
|
|
FptFeaturePoint_ * correspondentKF;
|
|
FptFeaturePoint_ * correspondentPF;
|
|
FptFeaturePoint_ * correspondentNF;
|
|
|
|
FptFeaturePoint_ * tempCorrespondent;
|
|
|
|
// Used by the matching algorithm
|
|
short step1U, step1V;
|
|
short step2U, step2V;
|
|
|
|
short iBucket, jBucket;
|
|
|
|
double score;
|
|
float meanI;
|
|
float sigmaI;
|
|
};
|
|
|
|
|
|
|
|
class FptFeaturePointsArray_
|
|
{
|
|
public:
|
|
|
|
FptFeaturePointsArray_(void);
|
|
FptFeaturePointsArray_(int size);
|
|
|
|
~FptFeaturePointsArray_(void);
|
|
|
|
FptFeaturePoint_ *array;
|
|
int size;
|
|
|
|
int nbMatchedPoints;
|
|
int nbInliersKF;
|
|
int nbBackgroundPoints;
|
|
|
|
double minU, minV, maxU, maxV;
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
/*! Matching is done in two steps:
|
|
The first step generates matchs hypotheses based on points proximity (step 1)
|
|
The second step considers correlation between windows around the points (step 2)
|
|
|
|
*/
|
|
|
|
|
|
typedef struct appariementStruct{
|
|
FptFeaturePoint_ * p1;
|
|
FptFeaturePoint_ * p2;
|
|
int index1, index2;
|
|
double score;
|
|
int outlier;
|
|
} AppariementPDI;
|
|
|
|
|
|
class FptPointMatcher
|
|
{
|
|
public:
|
|
FptPointMatcher(void) { buckets1 = NULL; buckets2 = NULL;}
|
|
~FptPointMatcher();
|
|
|
|
void init(int width, int height,
|
|
int p_maxDisparityU, int p_maxDisparityV, int p_correlationWindowSize,
|
|
int maxPointNumberPerBucket);
|
|
|
|
int width, height;
|
|
int maxDisparityU, maxDisparityV, correlationWindowSize;
|
|
|
|
/*!********************************************************************
|
|
Appariement des deux ensembles de points d'interet entre les deux images
|
|
This is the main function to be called. Parameters:
|
|
|
|
PointDInteret * pointsDInteretPrev: The previous frame interest point array
|
|
int nbPointDInteretsPrev : The size of the array described in the line before this
|
|
PointDInteret * pointsDInteret : The current frame interest point array
|
|
int nbPointDInterets : The size of the array described in the line before this
|
|
IplImage * imagePrev : The previous IPL Image
|
|
IplImage * image : The current IPL Image
|
|
float correlationThreshold : A threshold (ex. 0.6, 0.8)
|
|
AppariementPDI * matches : OUTPUT: The matches!
|
|
int * nbMatches : OUTPUT: number of matches
|
|
*********************************************************************/
|
|
void FptPointMatcher::matchPoints(unsigned char *image1,unsigned char *image2, int imageWidth, int imageHeight, int imagePitch,
|
|
FptFeaturePointsArray_ *pointList1,FptFeaturePointsArray_ *pointList2,
|
|
float correlationThreshold, AppariementPDI * matches, int * nbMatches,
|
|
int method);
|
|
|
|
private:
|
|
|
|
|
|
void computeIntensityMeanAndSigma(FptFeaturePointsArray_ *points,
|
|
unsigned char * image, int imageWidth, int imageHeight, int imagePitch,
|
|
int correlationWindowSize);
|
|
|
|
double crossCorrelationScore(const FptFeaturePoint_ * pointPrev, const FptFeaturePoint_ * point,
|
|
const unsigned char * imagePrev, const unsigned char * image, int imageWidth, int imageHeight, int imagePitch,
|
|
const int correlationWindowSize);
|
|
void lookForCorrespondents(FptFeaturePointsArray_ *points1, FptFeaturePointsArray_ *points2,
|
|
unsigned char * image1, unsigned char * image2, int imageWidth, int imageHeight, int imagePitch,
|
|
FptFeaturePoint_ **** buckets1, int ** bucketsSizes1,
|
|
FptFeaturePoint_ **** buckets2, int ** bucketsSizes2,
|
|
int correlationWindowSize, float correlationThreshold,
|
|
int framesPairType);
|
|
|
|
|
|
void allocBuckets(FptFeaturePoint_ ***** buckets, int *** bucketsSizes,
|
|
int maxPointNumberPerBucket);
|
|
void freeBuckets(FptFeaturePoint_ ***** buckets, int *** bucketsSize);
|
|
void fillBuckets(FptFeaturePoint_ **** buckets, int ** bucketsSizes,
|
|
FptFeaturePointsArray_ * points);
|
|
|
|
|
|
FptFeaturePoint_* * ** buckets1;
|
|
int ** bucketsSizes1;
|
|
FptFeaturePoint_* * ** buckets2;
|
|
int ** bucketsSizes2;
|
|
int bucketsWidth, bucketsHeight;
|
|
int maxPointNumberPerBucket;
|
|
};
|
|
|
|
#endif
|
|
|