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