void Masseuse::PrintErrorStatistics(const Values& values){
// calclate the error
Error err;
if(!CalculateError(err, values)){
std::cerr << "Unable to calculate error metrics." << std::endl;
return;
}
std::cerr << "======================ERROR REPORT=====================" <<
std::endl;
std::cerr << "Average trans error (m): " << err.GetAverageTransError() <<
std::endl;
std::cerr << "Average rot error (deg): " << err.GetAverageRotError() <<
std::endl;
std::cerr << "Total distance traveled (m): " << err.DistanceTraveled() <<
std::endl;
std::cerr << "% Avg. trans error: " << err.GetPercentAverageTansError()
* 100 << " %" << std::endl;
std::cerr << "Max trans error (m): " << err.MaxTransError() << std::endl;
std::cerr << "Max rot error (deg): " << err.MaxRotError() << std::endl;
if(options.enable_switchable_constraints){
int num_disabled_constraints = 0;
int num_lcc = 0;
for(const Factor& f : *graph){
if(f.isLCC){
num_lcc++;
if(f.switch_variable < 0.1){
num_disabled_constraints++;
}
}
}
fprintf(stderr, "Number of LCC that were disabled: %d ( %f%% )\n",
num_disabled_constraints,
(float)num_disabled_constraints/(float)num_lcc * 100.0f
);
}
std::cerr << "======================================================" <<
std::endl;
}
bool Masseuse::CalculateError(Error& error, const Values& values){
// First check if we have a ground truth to compare against
if(!gt_poses.size()){
std::cerr << "Unable to calculate error, no ground truth provided." <<
std::endl;
return false;
}
unsigned num_poses_to_compare = values.size();
if(gt_poses.size() != values.size()){
num_poses_to_compare = std::min(gt_poses.size(), values.size());
std::cerr << "There are " << gt_poses.size() << " ground truth poses"
<< " and " << values.size() << " optimized poses. Will only" <<
" compare the first " << num_poses_to_compare << " poses." <<
std::endl;
}
if(num_poses_to_compare > 0){
size_t index = 0;
for(const auto& kvp : values){
if(index >= num_poses_to_compare){
break;
}
Pose3 est_pose = kvp.second;
Pose3 gt_pose = gt_poses.at(index).Twp;
Eigen::Vector6d pose_error = (est_pose.inverse() * gt_pose).log();
Eigen::Vector3d trans_error = pose_error.head<3>().cwiseAbs();
Eigen::Vector3d rot_error = pose_error.tail<3>().cwiseAbs();
error.Translation()+= trans_error;
error.Rotation()+= rot_error;
error.NumPoses()++;
// Set the max trans and rotation errors
if(error.MaxTransError() < trans_error.norm()){
error.MaxTransError() = trans_error.norm();
}
if(error.MaxRotError() < rot_error.norm()){
error.MaxRotError() = rot_error.norm();
}
if(index > 0){
// add up the total distance traveled, based on the ground truth
error.DistanceTraveled()+= (gt_poses.at(index-1).Twp.inverse() *
gt_poses.at(index).Twp).translation().norm();
}
// calculate the % average translation error up to this point
if(error.DistanceTraveled() > 0){
error.PercentAvgTranslationError()+= (trans_error.norm() /
error.DistanceTraveled());
}
index++;
}
}else{
std::cerr << "No poses to compare." << std::endl;
return false;
}
return true;
}
