#include <Base/Image/Image.hh>
#include <Base/ImageUtils/ImageDraw.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Image/ImageConvert.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Debug/TimeMeasure.hh>
#include <Image/TextureTransformHomography.hh>
#include <Image/TextureTransformAffine.hh>
#include <Base/Math/Random.hh>
#include "MathAlgo/Lapack.hh"
#include <Matcher2D/ImageAlignment.hh>
#include <Image/HomographyMapping.hh>
#include <Geometry/RMatrix.hh>
#include <iostream>
#include <iomanip>
#include "Base/Common/FileHandling.hh"
#include "Utils/ThreeDOut.hh"
using namespace BIAS;
using namespace std;
{
BIASERR("error loading image "<<name);
return -1;
}
BIASERR("error converting image "<<name);
return -2;
}
BIASERR("error converting image to grey "<<name);
return -3;
}
}
return 0;
}
int main(int argc, char*argv[])
{
if (argc<2) {
cerr << argv[0] << " <image> "<< endl;
return -1;
}
if (LoadImage(argv[1], im[0]) != 0) {
BIASERR("error loading image " << argv[0]);
return -1;
}
im[1] = im[0];
cout<<"Testing AFFINITY"<<endl;
par.
newsize(6); par[0] = 0.0; par[1] = 0; par[2] = 0; par[3] = 0;
par[4] = 0; par[5] = 0;
cout<<"using parameters "<<par<<endl;
origpar = par;
(im[0].GetHeight()-1.0)/2.0);
cout<<"setting origin !"<<thepoint<<endl;
p[0][2] = p[1][2] = 1.0;
double d=20;
K[1][1] =K[0][0] = im[0].
GetWidth()*2.0;
cout << "K is "<<K<<endl;
for (unsigned int i=0;i<4;i++) PredictedCov[i][i] = 0.01;
for (unsigned int i=0; i<4; i++)
EmpiricCov[i][i] = 0.5/(d*d*2.0);
for (unsigned int i=4; i<6; i++)
EmpiricCov[i][i] = 0.5;
ofstream logfile("trackingerror.log");
logfile<<"# log written by "<<argv[0]<<" hws="<<d<<endl<<flush;
logfile<<"# angle empiricdistance preddistance distance cornercovdistance result"<<endl<<flush;
P1 = K * P1;
P1.InvalidateDecomposition();
vrml.
AddPMatrix(P1,im[0].GetWidth(), im[0].GetHeight(),
RGBAuc(0,255,0,255),0.1);
for (unsigned int angle=0; angle<80; angle+=2) {
cout<<"angle is now "<<angle<<" degree."<<endl;
Rot.
Set(axis,
double(angle)/180.0*M_PI);
cout<<"Rot is "<<Rot<<endl;
C = Rot * C - C;
cout <<"C is "<<C<<endl;
vrml.
AddPMatrix(P2,im[0].GetWidth(), im[0].GetHeight(),
RGBAuc(0,0,255,255),0.1);
double pixelnoise = 0.0;
if (pixelnoise>0.0) {
do {
} while (pData++ < pDataEnd);
}
stringstream ss;
cout<<"Setting alignment pixels "<<endl<<flush;
cout<<"Aligning ..."<<endl<<flush;
cov = EmpiricCov;
if (result!=0) {
BIASERR("alignment failed !!!"<<result);
}
cout<<"angle is "<<angle<<endl;
if (angle%10==0) getchar();
double linearizationerror = 0;
for (unsigned int corner=0; corner<5; corner++) {
switch (corner) {
case 0: cornerpoint =
HomgPoint2D(thepoint[0]-d,thepoint[1]-d);
break;
case 1: cornerpoint =
HomgPoint2D(thepoint[0]-d,thepoint[1]+d);
break;
case 2: cornerpoint =
HomgPoint2D(thepoint[0]+d,thepoint[1]-d);
break;
case 3: cornerpoint =
HomgPoint2D(thepoint[0]+d,thepoint[1]+d);
break;
default:;
}
linearizationerror +=
}
for (unsigned int i=0; i<cornerParams.size(); i++) {
meanCornerParam += cornerParams[i];
}
meanCornerParam *= 1.0/double(cornerParams.size());
for (unsigned int i=0; i<cornerParams.size(); i++) {
cornerCov +=
}
cornerCov *= 1.0/double(cornerParams.size()-1.0);
for (unsigned int i=0; i<6; i++) cornerCov[i][i] += 1e-8;
linearizationerror /= 4.0;
cout<<"average linearization error is "<<linearizationerror<<endl;
PredictedCov.SetIdentity();
PredictedCov *= 1e-8;
for (unsigned int i=0; i<4; i++)
PredictedCov[i][i] += linearizationerror*linearizationerror/(d*d*2.0);
for (unsigned int i=4; i<6; i++)
PredictedCov[i][i] += linearizationerror*linearizationerror;
gtpar -= par;
cout<<"empiric distance is "<<empiricdistance<<endl;
cout<<"compensated distance is "<<preddistance<<endl;
for (unsigned int i=0; i<6; i++) cov[i][i] += 1e-8;
cout<<"obtained data distance is "<<distance<<endl;
cout<<"corner cov distance is "<<cornercovdistance<<endl;
if (result>=0) {
logfile<<angle<<" "<<empiricdistance<<" "<<preddistance<<" "<<distance<<" "<<cornercovdistance<<" "<<result<<endl<<flush;
}
}
return 0;
}