small example demonstrating the ProjectionParametersPerspective by applying project and unproject
#include <Geometry/ProjectionParametersPerspective.hh>
#include <Base/Image/Image.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Math/Random.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Utils/ThreeDOut.hh>
using namespace BIAS;
using namespace std;
int main(int argc, char *argv[]) {
unsigned int numTests = 1;
for (unsigned int tests=0; tests<numTests; tests++) {
R.
SetXYZ(30.0*M_PI/180.0, 3.0*M_PI/180.0, 0.0);
double ImgWidth = 300;
double ImgHeight = 300;
K.
SetFy(focalX*aspectrat);
if (numTests == 1) {
cout << "K " << K << endl;
cout << "R " << R << endl;
cout << "C " << C << endl;
cout << "P " << P << endl;
}
int numPoints = 20;
vector<HomgPoint3D> points3D;
vector<double> zCorr;
for (int i = 0; i < numPoints; i++) {
zCorr.push_back(temp3D[2]);
points3D.push_back(temp3D);
}
vector<HomgPoint2D> points2D;
for (int i = 0; i < numPoints; i++) {
temp2D = ppp.
Project(points3D[i],
true);
points2D.push_back(temp2D);
}
vector<HomgPoint3D> unprojectRay, backprojectZDepth, backprojectWorldCoo;
vector<double> depth;
double errorSum = 0.0;
double errorSum2 = 0.0;
double zTemp, depthTemp;
for (int j = 0; j < numPoints; j++) {
ray = dir;
if (ray[2] != 0.0) {
ray[0] *= zCorr[j] / ray[2];
ray[1] *= zCorr[j] / ray[2];
ray[2] *= zCorr[j] / ray[2];
ray[0] += C[0];
ray[1] += C[1];
ray[2] += C[2];
unprojectRay.push_back(ray);
errorSum += (ray - points3D[j]).NormL2();
} else {
cout << "ray[2] == 0 for point " << j << " " << ray << endl;
}
camPointTemp = withoutK * points3D[j];
zTemp = camPointTemp[2];
backprojectZDepth.push_back(temp3D);
errorSum2 += (temp3D - points3D[j]).NormL2();
temp2D = points2D[j];
temp2D[0] -= principalX;
temp2D[1] = principalY - temp2D[1];
depthTemp = sqrt(sqrt(camPointTemp[0]*camPointTemp[0] - temp2D[1]*temp2D[1]) - temp2D[0]*temp2D[0]);
if(depthTemp!=depthTemp) depthTemp = 1.0;
if(temp3D[2] != 0){
backprojectWorldCoo.push_back(temp3D);
} else {
cout << "problem with temp3D[2] " << temp3D << endl;
}
cout << "point : " << j << backprojectWorldCoo[j] << endl;
}
cout << "error sum " << errorSum << endl;
cout << "error sum 2 " << errorSum2 << endl;
if (numTests==1) {
for (int i = 0; i < numPoints; i++) {
points3D[i].Homogenize();
unprojectRay[i].Homogenize();
backprojectZDepth[i].Homogenize();
backprojectWorldCoo[i].Homogenize();
threeDOut.
AddPoint(backprojectWorldCoo[i],
RGBAuc(255, 255, 255, 127));
}
}
}
return 0;
}