本文整理汇总了C++中MatrixXf::squaredNorm方法的典型用法代码示例。如果您正苦于以下问题：C++ MatrixXf::squaredNorm方法的具体用法？C++ MatrixXf::squaredNorm怎么用？C++ MatrixXf::squaredNorm使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类MatrixXf的用法示例。
在下文中一共展示了MatrixXf::squaredNorm方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: solveOneLevel
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;
}