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