本文整理汇总了C++中eigen::MatrixXf::squaredNorm方法的典型用法代码示例。如果您正苦于以下问题：C++ MatrixXf::squaredNorm方法的具体用法？C++ MatrixXf::squaredNorm怎么用？C++ MatrixXf::squaredNorm使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::MatrixXf的用法示例。
在下文中一共展示了MatrixXf::squaredNorm方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: computeError
//uses cauchy as cost function instead of squared error
//observations is a matrix of nx1 where n is the number of landmarks observed
//each value in the matrix represents the angle at which the landmark was observed
//params is a matrix of nx2 where n is the number of landmarks
//for each landmark, the x and y pose of where it is
//pose is a matrix of 2x1 containing the initial guess of the robot pose
//error is a nx1 matrix for the difference between the measurement and the estimated angle
double LMAlgr::computeError(Eigen::MatrixXf observations, Eigen::MatrixXf params, Eigen::MatrixXf pose, Eigen::MatrixXf &error){
Eigen::MatrixXf estimated_angle;
estimated_angle.resize(observations.rows(), observations.cols());
for(int i = 0; i < observations.rows(); i++){
//compute the estimated angle for each landmark
estimated_angle(i, 0) = atan2(params(i, 1) - pose(1, 0), params(i, 0) - pose(0, 0));
//std::cout << params(i, 1) << " " << params(i, 0) << " " << pose(1, 0) << " " << pose(0, 0) << " " << estimated_angle(i, 0) << " " << observations(i, 0) << std::endl;
//compute the error for each landmark
error(i, 0) = atan2(sin(observations(i, 0) - estimated_angle(i, 0)), cos(observations(i, 0) - estimated_angle(i, 0)));//normalize_angle(observations(i, 0) - estimated_angle(i, 0));
}
/*std::cout << "estimated angle: \n" << estimated_angle << std::endl;*/
/*std::cout << "error matrix: \n " << error << std::endl;*/
//std::cout << "final error value:\n" << error << std::endl;
double cost = outlier_threshold * outlier_threshold * log10(1 + (error.squaredNorm() / (outlier_threshold * outlier_threshold)));
double weight = sqrt(cost) / error.norm();
error = weight * error;
return error.squaredNorm();
}