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