Matlab fitgeotrans function convert C++

CSvMat CLBF3000::fitgeotrans(CSvMat& movingpoints, CSvMat& fixpoints)
{
	/*
	function tform = findNonreflectiveSimilarity(uv,xy)
	%
	% For a nonreflective similarity:
	%
	% let sc = s*cos(theta)
	% let ss = s*sin(theta)
	%
	%                   [ sc -ss
	% [u v] = [x y 1] *   ss  sc
	%                     tx  ty]
	%
	% There are 4 unknowns: sc,ss,tx,ty.
	%
	% Another way to write this is:
	%
	% u = [x y 1 0] * [sc
	%                  ss
	%                  tx
	%                  ty]
	%
	% v = [y -x 0 1] * [sc
	%                   ss
	%                   tx
	%                   ty]
	%
	% With 2 or more correspondence points we can combine the u equations and
	% the v equations for one linear system to solve for sc,ss,tx,ty.
	%
	% [ u1  ] = [ x1  y1  1  0 ] * [sc]
	% [ u2  ]   [ x2  y2  1  0 ]   [ss]
	% [ ... ]   [ ...          ]   [tx]
	% [ un  ]   [ xn  yn  1  0 ]   [ty]
	% [ v1  ]   [ y1 -x1  0  1 ]   
	% [ v2  ]   [ y2 -x2  0  1 ]    
	% [ ... ]   [ ...          ]
	% [ vn  ]   [ yn -xn  0  1 ]
	%
	% Or rewriting the above matrix equation:
	% U = X * r, where r = [sc ss tx ty]'
	% so r = X\U.
	%

	[uv,normMatrix1] = images.geotrans.internal.normalizeControlPoints(uv);
	[xy,normMatrix2] = images.geotrans.internal.normalizeControlPoints(xy);

	minRequiredNonCollinearPairs = 2;
	M = size(xy,1);

	x = xy(:,1);
	y = xy(:,2);
	X = [x   y  ones(M,1)   zeros(M,1);
	y  -x  zeros(M,1)  ones(M,1)  ]

	u = uv(:,1);
	v = uv(:,2);
	U = [u; v]

	% We know that X * r = U
	if rank(X) >= 2*minRequiredNonCollinearPairs 
	r = X \ U    
	else
	error(message('images:geotrans:requiredNonCollinearPoints', minRequiredNonCollinearPairs, 'nonreflectivesimilarity'))        
	end

	sc = r(1);
	ss = r(2);
	tx = r(3);
	ty = r(4);

	Tinv = [sc -ss 0;
	ss  sc 0;
	tx  ty 1]

	Tinv = normMatrix2 \ (Tinv * normMatrix1)

	T = inv(Tinv)
	T(:,3) = [0 0 1]';

	tform = affine2d(T)
	*/

	//movingpoint	
	
	CSvMat ddd;

	CSvMat normatrix1,normatrix2;
	CSvMat uv = normalizeControlPoints(movingpoints, normatrix1);
	CSvMat xy = normalizeControlPoints(fixpoints, normatrix2);
	
	CSvMat X(movingpoints.GetRows()*2, 4);

	for(int r=0; r< xy.GetRows(); r++)
	{
		X(r, 0) = xy(r, 0);
		X(r, 1) = xy(r, 1);
		X(r, 2) = 1;
		X(r, 3) = 0;
	}

	for(int r=xy.GetRows(); r< xy.GetRows()*2; r++)
	{
		X(r, 0) = xy(r-xy.GetRows(), 1);
		X(r, 1) = -xy(r-xy.GetRows(), 0);
		X(r, 2) = 0;
		X(r, 3) = 1;
	}
	
	CSvMat U(uv.GetRows()*2, 1);
	for(int r=0; r< uv.GetRows(); r++)
	{
		U(r,0) = uv(r,0);
		U(r+uv.GetRows(),0) = uv(r,1);
	}

	// (X^t * X)^-1 * X^t  
	CSvMat XtX = X.transpose() * X;
	CSvMat invmat = XtX.inverse();
	CSvMat r = invmat*X.transpose() * U;
	CSvMat Tinv(3,3);

	Tinv(0,0) = Tinv(1,1) =  r(0,0);
	Tinv(0,1) = -r(1,0);
	Tinv(1,0) = r(1,0);
	Tinv(2,0) = r(2,0);
	Tinv(2,1) = r(3,0);
	Tinv(2,2) = 1;
	

	Tinv = normatrix2.inverse()* (Tinv*normatrix1);
	CSvMat T = Tinv.inverse();
	
	T(0,2) = T(1,2) = 0;
	T(2,2) = 1.;
	
	return T;

}