//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();
}
