#include <Base/Common/BIASpragma.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Geometry/PMatrix.hh>
#include <Geometry/RMatrix.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Base/Math/Random.hh>
#include <Utils/ThreeDOut.hh>
#include <vector>
#include <Geometry/PMatrixLinear.hh>
#include <MathAlgo/SVD.hh>
using namespace BIAS;
using namespace std;
int main()
{
unsigned int numPs = 50;
vector<PMatrix> P_origs;
vector<PMatrix> P_transformed;
K[0][0] = 400;
K[1][1] = 400;
K[0][2] = 200;
K[1][2] = 200;
double phiX, phiY, phiZ;
for(int i=0; i < 3; i++){
for(int j=0; j < 3; j++){
T[i][j] = R[i][j];
}
T[i][3] = C[i];
}
cout << "T " << T << endl;
for(unsigned int i = 0; i < numPs; i++){
P_origs.push_back(P);
P_transformed.push_back(
PMatrix(P*T));
R =
RMatrix(P_transformed[i].GetR());
}
for(int col = 0; col < 4; col++){
for(unsigned int camCount = 0; camCount < numPs; camCount++){
for(int i=0; i < 3; i++){
for(int j = 0; j < 4; j++){
A[i+camCount*3][j] = (P_origs[camCount])[i][j];
}
b[i+camCount*3] = (P_transformed[camCount])[i][col];
}
}
cout << T_col << endl;
T_leastSquares[0][col] = T_col[0];
T_leastSquares[1][col] = T_col[1];
T_leastSquares[2][col] = T_col[2];
T_leastSquares[3][col] = T_col[3];
}
cout << "Least squares T " << T_leastSquares << endl;
SVD svdT(T_leastSquares);
vector<PMatrix> P_leastSquares;
for(unsigned int i=0; i < numPs; i++){
P_leastSquare =
PMatrix(P_transformed[i] * T_invLS);
P_leastSquares.push_back(P_leastSquare);
}
for(unsigned int i = 0; i < numPs; i++){
}
return 0;
}