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;
}
