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