Example #1
0
void CDifodo::solveOneLevel()
{
	MatrixXf A(num_valid_points,6);
	MatrixXf B(num_valid_points,1);
	unsigned int cont = 0;

	//Fill the matrix A and the vector B
	//The order of the unknowns is (vz, vx, vy, wz, wx, wy)
	//The points order will be (1,1), (1,2)...(1,cols-1), (2,1), (2,2)...(row-1,cols-1).

	const float f_inv = float(cols_i)/(2.f*tan(0.5f*fovh));

	for (unsigned int u = 1; u < cols_i-1; u++)
		for (unsigned int v = 1; v < rows_i-1; v++)
			if (null(v,u) == false)
			{
				// Precomputed expressions
				const float d = depth_inter[image_level](v,u);
				const float inv_d = 1.f/d;
				const float x = xx_inter[image_level](v,u);
				const float y = yy_inter[image_level](v,u);
				const float dycomp = du(v,u)*f_inv*inv_d;
				const float dzcomp = dv(v,u)*f_inv*inv_d;
				const float tw = weights(v,u);

				//Fill the matrix A
				A(cont, 0) = tw*(1.f + dycomp*x*inv_d + dzcomp*y*inv_d);
				A(cont, 1) = tw*(-dycomp);
				A(cont, 2) = tw*(-dzcomp);
				A(cont, 3) = tw*(dycomp*y - dzcomp*x);
				A(cont, 4) = tw*(y + dycomp*inv_d*y*x + dzcomp*(y*y*inv_d + d));
				A(cont, 5) = tw*(-x - dycomp*(x*x*inv_d + d) - dzcomp*inv_d*y*x);
				B(cont,0) = tw*(-dt(v,u));

				cont++;
			}
	
	//Solve the linear system of equations using weighted least squares
	MatrixXf AtA, AtB;
	AtA.multiply_AtA(A);
	AtB.multiply_AtB(A,B);
	MatrixXf Var = AtA.ldlt().solve(AtB);

	//Covariance matrix calculation 
	MatrixXf res = -B;
	for (unsigned int k = 0; k<6; k++)
		res += Var(k)*A.col(k);

	est_cov = (1.f/float(num_valid_points-6))*AtA.inverse()*res.squaredNorm();

	//Update last velocity in local coordinates
	kai_loc_level = Var;
}