Vector2d leastSquareSolverCalib::optimize(pointpairVec &assoc){
int size=assoc.size();
MatrixXd P(size,2);
MatrixXd Pt(size,2);
MatrixXd Z(4,size);
lasercloudToEigenMatrixWithAssociations(P,Pt,assoc);
//UGLY
//--------------------------------------
for(int i=0;i<size;i++)
{
for(int j=0;j<2;j++)
{
Z(j,i)=P(i,j);
}
for(int j=2;j<4;j++)
{
Z(j,i)=Pt(i,j-2);
}
}
//--------------------------------------
Vector2d x=_startingParams;
_result=pointAlignerLoop(x,Z,_iterations,_result);
return _result;
}
void PointAlignerTest(){
MatrixXf P(2, 100);
for(int i=0; i<100; i++){
srand (time(NULL));
P(0, i) = rand()%100;
for(int j=0; j<100000; j++)
srand (time(NULL));
P(1, i) = rand()%100;
}
cout << "P : \n" << P << endl;
Vector3f x_ideal;
x_ideal << 20, 30, PI/2;
//cout << "x_ideal : \n" << x_ideal << endl;
Matrix3f X_ideal;
X_ideal = v2tRad(x_ideal);
//cout << "X_ideal : \n" << X_ideal << endl;
MatrixXf Pt(2, 100);
Pt = transformPoints(X_ideal.inverse(), P);
cout << "Pt : \n" << Pt << endl;
MatrixXf Z(4, P.cols());
for(int i=0; i<P.cols(); i++){
Z(0, i) = P(0, i);
Z(1, i) = P(1, i);
Z(2, i) = Pt(0, i);
Z(3, i) = Pt(1, i);
}
cout << "Z : \n" << Z << endl;
Vector3f x; //initial guess
x << 0,0,0;
cout << "x : \n" << x << endl;
cout << pointAlignerLoop(x, Z, 100);
return ;
}
