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 ; }