Sigma.at<float>(i,i)=1.0f/std::sqrt(sigma_squared_2D);// the higher the sigma_squared_2D, the smaller the diagonal entries of Sigma will be
// Sigma(i, i) = sqrt(sigma_squared_2D), but then Omega is Sigma.t() * Sigma (squares the diagonal) - so we just assign 1/sigma_squared_2D to Omega here:
Omega.at<float>(i,i)=1.0f/sigma_squared_2D;// the higher the sigma_squared_2D, the smaller the diagonal entries of Sigma will be
}
MatOmega=Sigma.t()*Sigma;// just squares the diagonal
// The landmarks in matrix notation (in homogeneous coordinates), $3N\times 1$