www.pudn.com > Face3DModel.zip > CornerMatch.cpp
#pragma once
#include "StdAfx.h"
#include "Matrix.h"
#include "CornerMatch.h"
MatchCornerAndIndex::MatchCornerAndIndex()
{
num = 0;
use = FALSE;
}
int MatchCornerAndIndex::getIndex()
{
return index;
}
int MatchCornerAndIndex::getNum()
{
return num;
}
MatchCornerAndIndex::addCorner(MatchCorner points, int index)
{
corners.push_back(points);
indexs.push_back(index);
num++;
}
MatchCorner MatchCornerAndIndex::getCorner(int i) //the ith couple of corner
{
return corners[i];
}
MatchCornerAndIndex::setUseState(bool state)
{
use = state;
}
bool MatchCornerAndIndex::getUseState()
{
return use;
}
void CornerDetection(int nWidth, int nHeight, BYTE *pbImage, BYTE *pbMask, vector& vecCorners)
{
int i, j;
BYTE *pbGray = new BYTE[nWidth * nHeight];
BYTE *pbEdge = new BYTE[nWidth * nHeight];
// Get gray image
for (i = 0; i < nHeight; i++)
{
for (j = 0; j < nWidth; j++)
{
int index = i * nWidth + j;
double dbGray = pbImage[index * 3] * 0.114 + pbImage[index * 3 + 1] * 0.587 + pbImage[index * 3 + 2] * 0.299;
if (dbGray < 0)
{
pbGray[index] = 0;
}
else if (dbGray > 255)
{
pbGray[index] = 255;
}
else
{
pbGray[index] = (BYTE)(int)(dbGray + 0.5);
}
}
}
// Moravec's Corner Detector
memset(pbEdge, 0, nWidth * nHeight);
CornerDetectOnGrayImage(nWidth, nHeight, pbGray, pbEdge);
POINT tempCorner;
for (i = 1; i < nHeight-1; i++)
{
for (j = 1; j < nWidth-1; j++)
{
if(pbEdge[i*nWidth+j] > 0)
{
tempCorner.x=j;
tempCorner.y=i;
vecCorners.push_back(tempCorner);
}
}
}
delete [] pbGray;
delete [] pbEdge;
}
void CornerMatch(int nWidth, int nHeight, BYTE *pbImage1, BYTE *pbImage2, vector& vecCorners1, vector& vecCorners2, vector &vecMatchCorners)
{
int i, j;
POINT correspondingPoint;
int correspondingPointIndex;
MatchCorner matchedPoints;
double tempCorrelation, maxCorrelation;
double threshold = 0.866 * 0.866; //this parameter can be adjusted;
double windowSize = 5; //this parameter can be adjusted;
BYTE *pbGray1 = new BYTE[nWidth * nHeight];
BYTE *pbGray2 = new BYTE[nWidth * nHeight];
// Get gray image
for (i = 0; i < nHeight; i++)
{
for (j = 0; j < nWidth; j++)
{
int index = i * nWidth + j;
double dbGray = pbImage1[index * 3] * 0.114 + pbImage1[index * 3 + 1] * 0.587 + pbImage1[index * 3 + 2] * 0.299;
if (dbGray < 0)
{
pbGray1[index] = 0;
}
else if (dbGray > 255)
{
pbGray1[index] = 255;
}
else
{
pbGray1[index] = (BYTE)(int)(dbGray + 0.5);
}
dbGray = pbImage2[index * 3] * 0.114 + pbImage2[index * 3 + 1] * 0.587 + pbImage2[index * 3 + 2] * 0.299;
if (dbGray < 0)
{
pbGray2[index] = 0;
}
else if (dbGray > 255)
{
pbGray2[index] = 255;
}
else
{
pbGray2[index] = (BYTE)(int)(dbGray + 0.5);
}
}
}
for (i = vecCorners1.size() - 1; i >= 0; i--)
{
if (OutRange(nWidth, nHeight, vecCorners1[i], windowSize)) continue;
maxCorrelation = 0;
for (j = vecCorners2.size() - 1; j >= 0; j--)
{
if (OutRange(nWidth, nHeight, vecCorners2[j], windowSize)) continue;
tempCorrelation = Correlation(nWidth, nHeight, pbGray1, pbGray2, vecCorners1[i], vecCorners2[j], windowSize);
if (tempCorrelation > maxCorrelation )
{
maxCorrelation = tempCorrelation;
correspondingPoint = vecCorners2[j];
correspondingPointIndex = j;
}
}
if (maxCorrelation > threshold)
{
matchedPoints.ptCorner1 = vecCorners1[i];
matchedPoints.ptCorner2 = correspondingPoint;
vecMatchCorners.push_back(matchedPoints);
vecCorners2.erase(vecCorners2.begin() + correspondingPointIndex);
}
}
delete [] pbGray2;
delete [] pbGray1;
}
void findbadMatch(double F11, double F12, double F13,
double F21, double F22, double F23,
double F31, double F32, double F33,
double G11, double G12, double G13,
double G21, double G22, double G23,
double G31, double G32, double G33,
double root, int matchCornerNumber,
double &minbadMatch,
vector &vecMatchCorners,
CMatrix &bestF)
{
int j;
CMatrix F(3, 3);
double tempDistance;
CMatrix m1(1, 3); //use m1, m2 to represent Mi, Mi' in the paper
CMatrix m2(1, 3);
CMatrix tempM1(1, 3);
CMatrix tempM2(1, 3);
m1.SetElement(0, 2, 1);
m2.SetElement(0, 2, 1);
F.SetElement(0, 0, root * F11 + (1 - root) * G11);
F.SetElement(1, 0, root * F12 + (1 - root) * G12);
F.SetElement(2, 0, root * F13 + (1 - root) * G13);
F.SetElement(0, 1, root * F21 + (1 - root) * G21);
F.SetElement(1, 1, root * F22 + (1 - root) * G22);
F.SetElement(2, 1, root * F23 + (1 - root) * G23);
F.SetElement(0, 2, root * F31 + (1 - root) * G31);
F.SetElement(1, 2, root * F32 + (1 - root) * G32);
F.SetElement(2, 2, root * F33 + (1 - root) * G33);
vector distanceSet;
for (j = 0; j < matchCornerNumber; j++)
{
m1.SetElement(0, 0, vecMatchCorners[j].ptCorner1.x);
m1.SetElement(0, 1, vecMatchCorners[j].ptCorner1.y);
m2.SetElement(0, 0, vecMatchCorners[j].ptCorner2.x);
m2.SetElement(0, 1, vecMatchCorners[j].ptCorner2.y);
tempM1 = F * m1;
tempM2 = F.GetTransposed() * m2;
tempDistance = distance(m2[0][0], m2[0][1], m2[0][2],
tempM1[0][0], tempM1[0][1], tempM1[0][2])
+ distance(m1[0][0], m1[0][1], m1[0][2],
tempM2[0][0], tempM2[0][1], tempM2[0][2]);
distanceSet.push_back(tempDistance);
}
vector ::iterator Iter1;
const double thresh1 = .1;
int badMatch = 0;
for (Iter1 = distanceSet.begin( ); Iter1 != distanceSet.end( ); Iter1++)
{
if (*Iter1 > thresh1) badMatch++;
}
if (badMatch < minbadMatch)
{
minbadMatch = badMatch;
bestF = F;
}
}
void MatchCancel(int nWidth, int nHeight, BYTE *pbImage1, BYTE *pbImage2, vector &vecMatchCorners, double thresholdLevel)
{
//use notations in "Determining the Epipolar Geometry and its Uncertainty:
//A review", Zhang Zhengyou, 1998
//use "Least Median of Squares"(P14 of the paper) technique to determine the fundamental matrix F
int bucketIndex; //use a bucketing technique
int loopTimes = 150; //the parameter can be adjusted
CMatrix Un(9, 7);
CMatrix f1(1, 7), f2(1, 2); //the first 7 elements and last 2 elements of f
int matchCornerNumber = vecMatchCorners.size();
int i, j;
int x1, y1, x2, y2;
CMatrix bestF(3, 3);
CMatrix zeroMatrix(3, 3);
zeroMatrix.SetElement(0, 0, 0);
zeroMatrix.SetElement(1, 0, 0);
zeroMatrix.SetElement(2, 0, 0);
zeroMatrix.SetElement(0, 1, 0);
zeroMatrix.SetElement(1, 1, 0);
zeroMatrix.SetElement(2, 1, 0);
zeroMatrix.SetElement(0, 2, 0);
zeroMatrix.SetElement(1, 2, 0);
zeroMatrix.SetElement(2, 2, 0);
double minbadMatch = 1000;
int bucketNumber = 8;
int bucketSize = bucketNumber * bucketNumber;
MatchCornerAndIndex bucketPoints[64]; //its dimension = bucketSize
for (i = 0; i < matchCornerNumber; i++)
{
int bucketIndex = vecMatchCorners[i].ptCorner1.x * bucketNumber / nWidth
+ (int)(vecMatchCorners[i].ptCorner2.y * bucketNumber / nHeight) * bucketNumber;
bucketPoints[bucketIndex].addCorner(vecMatchCorners[i], i);
}
for (i = 0; i < loopTimes; i++)
{
for (j = 0; j < 7; j++) //need 7 points to determine F
{
do
{
bucketIndex = rand() / 512; //* 64 / 32768;
}while (bucketPoints[bucketIndex].getUseState()
|| bucketPoints[bucketIndex].getNum() == 0);
bucketPoints[bucketIndex].setUseState(TRUE);
MatchCorner thisOne = bucketPoints[bucketIndex].getCorner(
rand() * bucketPoints[bucketIndex].getNum() / 32768);
//set elements in row j of matrix Un
x1 = thisOne.ptCorner1.x;
y1 = thisOne.ptCorner1.y;
x2 = thisOne.ptCorner2.x;
y2 = thisOne.ptCorner2.y;
if ( (x1 - x2) * (x1 - x2) > nWidth * nWidth / 64
|| (y1 - y2) * (y1 - y2) > nHeight * nHeight / 64)
{
bucketPoints[bucketIndex].setUseState(FALSE);
j--;
continue;
}
Un.SetElement(0, j, x1 * x2);
Un.SetElement(1, j, y1 * x2);
Un.SetElement(2, j, x2);
Un.SetElement(3, j, x1 * y2);
Un.SetElement(4, j, y1 * y2);
Un.SetElement(5, j, y2);
Un.SetElement(6, j, x1);
Un.SetElement(7, j, y1);
Un.SetElement(8, j, 1);
}
for (j = 0; j < 64; j++)
{
bucketPoints[j].setUseState(FALSE);
}
CMatrix Un1(7, 7), Un2(2, 7), Un1Inv(7, 7);
Un1 = Un.ExtractSubMatrix(0, 0, 7, 7);
Un2 = Un.ExtractSubMatrix(7, 0, 2, 7);
if (!Un1.Invert()) continue;
//F, G are two base solutions
double F11, F12, F13, F21, F22, F23, F31, F32, F33;
double G11, G12, G13, G21, G22, G23, G31, G32, G33;
f2.SetElement(0, 0, -1);
f2.SetElement(0, 1, 0);
f1 = Un1 * Un2 * f2;
F11 = f1[0][0];
F12 = f1[0][1];
F13 = f1[0][2];
F21 = f1[0][3];
F22 = f1[0][4];
F23 = f1[0][5];
F31 = f1[0][6];
F32 = 1;
F33 = 0;
f2.SetElement(0, 0, 0);
f2.SetElement(0, 1, -1);
f1 = Un1 * Un2 * f2;
G11 = f1[0][0];
G12 = f1[0][1];
G13 = f1[0][2];
G21 = f1[0][3];
G22 = f1[0][4];
G23 = f1[0][5];
G31 = f1[0][6];
G32 = 0;
G33 = 1;
//Solve the equation a0 x^3 + a1 x^2 + a2 x + a3 = 0
double a0, a1, a2, a3;
a0 = G11*F23*F32+G11*G22*F33+G11*F22*G33
-G11*F22*F33-F11*G23*G32+F11*G23*F32
+F11*F23*G32-F11*F23*F32-F11*F22*G33
+F11*G22*G33-F11*G22*F33+G31*G13*G22
-G11*G23*F32-G11*F23*G32+G21*F13*G32
-G21*F13*F32-G31*G12*G23-G21*G12*F33
-G21*F12*G33+G21*F12*F33-G31*F12*F23
+F21*G13*G32-F21*G13*F32-F21*F13*G32
+F21*F13*F32-F21*G12*G33+F21*G12*F33
+F21*F12*G33-F21*F12*F33-G31*G13*F22
-G31*F13*G22+G31*F13*F22+G31*G12*F23
+G31*F12*G23-F31*G13*G22+F31*G13*F22
+F31*F13*G22-F31*F13*F22+G11*G23*G32
+F31*G12*G23-F31*G12*F23-F31*F12*G23
+F31*F12*F23+G21*G13*F32+F11*F22*F33
-G11*G22*G33+G21*G12*G33-G21*G13*G32;
a1 = -G11*F23*F32-2*G11*G22*F33-2*G11*F22*G33
+G11*F22*F33+2*F11*G23*G32-F11*G23*F32
-F11*F23*G32+F11*F22*G33-2*F11*G22*G33
+F11*G22*F33-3*G31*G13*G22+2*G11*G23*F32
+2*G11*F23*G32-2*G21*F13*G32+G21*F13*F32
+3*G31*G12*G23+2*G21*G12*F33+2*G21*F12*G33
-G21*F12*F33+G31*F12*F23-2*F21*G13*G32
+F21*G13*F32+F21*F13*G32+2*F21*G12*G33
-F21*G12*F33-F21*F12*G33+2*G31*G13*F22
+2*G31*F13*G22-G31*F13*F22-2*G31*G12*F23
-2*G31*F12*G23+2*F31*G13*G22-F31*G13*F22
-F31*F13*G22-3*G11*G23*G32-2*F31*G12*G23
+F31*G12*F23+F31*F12*G23-2*G21*G13*F32
+3*G11*G22*G33-3*G21*G12*G33+3*G21*G13*G32;
a2 = -G21*G12*F33+F31*G12*G23-F21*G12*G33
-G31*F13*G22+F11*G22*G33+G21*F13*G32
-G11*F23*G32+G21*G13*F32+G31*F12*G23
-F31*G13*G22+F21*G13*G32-G21*F12*G33
-F11*G23*G32-G31*G13*F22+G11*G22*F33
-3*G11*G22*G33+3*G21*G12*G33-G11*G23*F32
+3*G11*G23*G32+3*G31*G13*G22+G11*F22*G33
-3*G31*G12*G23+G31*G12*F23-3*G21*G13*G32;
a3 = -G31*G13*G22+G31*G12*G23-G11*G23*G32
+G11*G22*G33-G21*G12*G33+G21*G13*G32;
double root1, root2, root3;
int numberOfRoots;
numberOfRoots = solve_equation_3(a0, a1, a2, a3, root1, root2, root3);
switch (numberOfRoots)
{
case 1:
findbadMatch(F11, F12, F13, F21, F22, F23, F31, F32, F33,
G11, G12, G13, G21, G22, G23, G31, G32, G33,
root1, matchCornerNumber, minbadMatch,
vecMatchCorners, bestF);
break;
case 3:
findbadMatch(F11, F12, F13, F21, F22, F23, F31, F32, F33,
G11, G12, G13, G21, G22, G23, G31, G32, G33,
root1, matchCornerNumber, minbadMatch,
vecMatchCorners, bestF);
findbadMatch(F11, F12, F13, F21, F22, F23, F31, F32, F33,
G11, G12, G13, G21, G22, G23, G31, G32, G33,
root2, matchCornerNumber, minbadMatch,
vecMatchCorners, bestF);
findbadMatch(F11, F12, F13, F21, F22, F23, F31, F32, F33,
G11, G12, G13, G21, G22, G23, G31, G32, G33,
root3, matchCornerNumber, minbadMatch,
vecMatchCorners, bestF);
}
}
//use gradient-based technique(P11 of the paper) to cancel the bad matches
vector fSquare;
CMatrix m1(1, 3); //use m1, m2 to represent Mi, Mi' in the paper
CMatrix m2(1, 3);
CMatrix l1(1, 3); //use m1, m2 to represent Mi, Mi' in the paper
CMatrix l2(1, 3);
CMatrix bestF_t(3,3); //the Transpose Matrix of bestF
bestF_t = bestF.GetTransposed();
m1.SetElement(0, 2, 1);
m2.SetElement(0, 2, 1);
for (j = 0; j < matchCornerNumber; j++)
{
m1.SetElement(0, 0, vecMatchCorners[j].ptCorner1.x);
m1.SetElement(0, 1, vecMatchCorners[j].ptCorner1.y);
m2.SetElement(0, 0, vecMatchCorners[j].ptCorner2.x);
m2.SetElement(0, 1, vecMatchCorners[j].ptCorner2.y);
l2 = bestF * m1;
l1 = bestF_t * m2;
double m2Fm1_product = (m2.GetTransposed() * bestF * m1).GetElement(0,0);
double tempDistance;
CMatrix tempM1(1, 3);
CMatrix tempM2(1, 3);
CMatrix tempM(1, 1);
m1.SetElement(0, 0, vecMatchCorners[j].ptCorner1.x);
m1.SetElement(0, 1, vecMatchCorners[j].ptCorner1.y);
m2.SetElement(0, 0, vecMatchCorners[j].ptCorner2.x);
m2.SetElement(0, 1, vecMatchCorners[j].ptCorner2.y);
tempM1 = bestF * m1;
tempM2 = bestF.GetTransposed() * m2;
tempM = m2.GetTransposed() * tempM1;
tempDistance = tempM[0][0] * tempM[0][0]
/ (tempM1[0][0] * tempM1[0][0]
+ tempM2[0][0] * tempM2[0][0]
+ tempM1[0][1] * tempM1[0][1]
+ tempM2[0][1] * tempM2[0][1]);
fSquare.push_back(tempDistance);
/*
tempDistance = distance(vecMatchCorners[j].ptCorner2.x,
vecMatchCorners[j].ptCorner2.y, 1,
tempM1[0][0], tempM1[0][1], tempM1[0][2])
+ distance(vecMatchCorners[j].ptCorner1.x,
vecMatchCorners[j].ptCorner1.y, 1,
tempM2[0][0], tempM2[0][1], tempM2[0][2]);
*/
}
//get a copy of fSquare to get in order to gain nth element of fSquare
vector pseudo_fSquare;
for (j = 0; j < matchCornerNumber; j++)
{
pseudo_fSquare.push_back(fSquare[j]);
}
double threshold2 = 1;
// threshold2 /= thresholdLevel;
for (j = matchCornerNumber - 1; j >= 0 ; j--)
{
if (fSquare[j] > threshold2
|| (vecMatchCorners[j].ptCorner1.x - vecMatchCorners[j].ptCorner2.x )
* (vecMatchCorners[j].ptCorner1.x - vecMatchCorners[j].ptCorner2.x )
> nWidth * nWidth / 64
|| (vecMatchCorners[j].ptCorner1.y - vecMatchCorners[j].ptCorner2.y )
* (vecMatchCorners[j].ptCorner1.y - vecMatchCorners[j].ptCorner2.y )
> nHeight * nHeight / 64
)
{
vecMatchCorners.erase(vecMatchCorners.begin() + j);
}
}
}