Program Listing for File evaluation_utils.h¶
↰ Return to documentation for file (voxblox/include/voxblox/utils/evaluation_utils.h
)
#ifndef VOXBLOX_UTILS_EVALUATION_UTILS_H_
#define VOXBLOX_UTILS_EVALUATION_UTILS_H_
#include <algorithm>
#include <string>
#include "voxblox/core/layer.h"
#include "voxblox/core/voxel.h"
namespace voxblox {
namespace utils {
enum class VoxelEvaluationResult { kNoOverlap, kIgnored, kEvaluated };
enum class VoxelEvaluationMode {
kEvaluateAllVoxels,
kIgnoreErrorBehindTestSurface,
kIgnoreErrorBehindGtSurface,
kIgnoreErrorBehindAllSurfaces
};
struct VoxelEvaluationDetails {
FloatingPoint rmse = 0.0;
// Max and min of absolute distance error.
FloatingPoint max_error = 0.0;
FloatingPoint min_error = 0.0;
size_t num_evaluated_voxels = 0u;
size_t num_ignored_voxels = 0u;
size_t num_overlapping_voxels = 0u;
size_t num_non_overlapping_voxels = 0u;
std::string toString() const {
std::stringstream ss;
ss << "\n\n======= Layer Evaluation Results =======\n"
<< " num evaluated voxels: " << num_evaluated_voxels << "\n"
<< " num overlapping voxels: " << num_overlapping_voxels << "\n"
<< " num non-overlapping voxels: " << num_non_overlapping_voxels << "\n"
<< " num ignored voxels: " << num_ignored_voxels << "\n"
<< " error min: " << min_error << "\n"
<< " error max: " << max_error << "\n"
<< " RMSE: " << rmse << "\n"
<< "========================================\n";
return ss.str();
}
};
template <typename VoxelType>
VoxelEvaluationResult computeVoxelError(
const VoxelType& voxel_gt, const VoxelType& voxel_test,
const VoxelEvaluationMode evaluation_mode, FloatingPoint* error);
template <typename VoxelType>
bool isObservedVoxel(const VoxelType& voxel);
template <typename VoxelType>
FloatingPoint getVoxelSdf(const VoxelType& voxel);
template <typename VoxelType>
void setVoxelSdf(const FloatingPoint sdf, VoxelType* voxel);
template <typename VoxelType>
void setVoxelWeight(const FloatingPoint weight, VoxelType* voxel);
template <typename VoxelType>
FloatingPoint evaluateLayersRmse(
const Layer<VoxelType>& layer_gt, const Layer<VoxelType>& layer_test,
const VoxelEvaluationMode& voxel_evaluation_mode,
VoxelEvaluationDetails* evaluation_result = nullptr,
Layer<VoxelType>* error_layer = nullptr) {
// Iterate over all voxels in the test layer and look them up in the ground
// truth layer. Then compute RMSE.
BlockIndexList block_list;
layer_test.getAllAllocatedBlocks(&block_list);
size_t vps = layer_test.voxels_per_side();
size_t num_voxels_per_block = vps * vps * vps;
VoxelEvaluationDetails evaluation_details;
double total_squared_error = 0.0;
for (const BlockIndex& block_index : block_list) {
const Block<VoxelType>& test_block =
layer_test.getBlockByIndex(block_index);
if (!layer_gt.hasBlock(block_index)) {
for (size_t linear_index = 0u; linear_index < num_voxels_per_block;
++linear_index) {
const VoxelType& voxel = test_block.getVoxelByLinearIndex(linear_index);
if (isObservedVoxel(voxel)) {
++evaluation_details.num_non_overlapping_voxels;
}
}
continue;
}
const Block<VoxelType>& gt_block = layer_gt.getBlockByIndex(block_index);
typename Block<VoxelType>::Ptr error_block;
if (error_layer != nullptr) {
error_block = error_layer->allocateBlockPtrByIndex(block_index);
}
for (size_t linear_index = 0u; linear_index < num_voxels_per_block;
++linear_index) {
FloatingPoint error = 0.0;
const VoxelEvaluationResult result =
computeVoxelError(gt_block.getVoxelByLinearIndex(linear_index),
test_block.getVoxelByLinearIndex(linear_index),
voxel_evaluation_mode, &error);
switch (result) {
case VoxelEvaluationResult::kEvaluated:
total_squared_error += error * error;
evaluation_details.min_error =
std::min(evaluation_details.min_error, std::abs(error));
evaluation_details.max_error =
std::max(evaluation_details.max_error, std::abs(error));
++evaluation_details.num_evaluated_voxels;
++evaluation_details.num_overlapping_voxels;
if (error_block) {
VoxelType& error_voxel =
error_block->getVoxelByLinearIndex(linear_index);
setVoxelSdf<VoxelType>(std::abs(error), &error_voxel);
setVoxelWeight<VoxelType>(1.0, &error_voxel);
}
break;
case VoxelEvaluationResult::kIgnored:
++evaluation_details.num_ignored_voxels;
++evaluation_details.num_overlapping_voxels;
break;
case VoxelEvaluationResult::kNoOverlap:
++evaluation_details.num_non_overlapping_voxels;
break;
default:
LOG(FATAL) << "Unkown voxel evaluation result: "
<< static_cast<int>(result);
}
}
}
// Iterate over all blocks in the grond truth layer and look them up in the
// test truth layer. This is only done to get the exact number of
// non-overlapping voxels.
BlockIndexList gt_block_list;
layer_gt.getAllAllocatedBlocks(>_block_list);
for (const BlockIndex& gt_block_index : gt_block_list) {
const Block<VoxelType>& gt_block = layer_gt.getBlockByIndex(gt_block_index);
if (!layer_test.hasBlock(gt_block_index)) {
for (size_t linear_index = 0u; linear_index < num_voxels_per_block;
++linear_index) {
const VoxelType& voxel = gt_block.getVoxelByLinearIndex(linear_index);
if (isObservedVoxel(voxel)) {
++evaluation_details.num_non_overlapping_voxels;
}
}
}
}
// Return the RMSE.
if (evaluation_details.num_evaluated_voxels == 0) {
evaluation_details.rmse = 0.0;
} else {
evaluation_details.rmse =
sqrt(total_squared_error / evaluation_details.num_evaluated_voxels);
}
// If the details are requested, output them.
if (evaluation_result != nullptr) {
*evaluation_result = evaluation_details;
}
VLOG(2) << evaluation_details.toString();
return evaluation_details.rmse;
}
template <typename VoxelType>
FloatingPoint evaluateLayersRmse(const Layer<VoxelType>& layer_gt,
const Layer<VoxelType>& layer_test) {
return evaluateLayersRmse<VoxelType>(
layer_gt, layer_test, VoxelEvaluationMode::kIgnoreErrorBehindTestSurface);
}
template <typename VoxelType>
VoxelEvaluationResult computeVoxelError(
const VoxelType& voxel_gt, const VoxelType& voxel_test,
const VoxelEvaluationMode evaluation_mode, FloatingPoint* error) {
CHECK_NOTNULL(error);
*error = 0.0;
// Ignore voxels that are not observed in both layers.
if (!isObservedVoxel(voxel_gt) || !isObservedVoxel(voxel_test)) {
return VoxelEvaluationResult::kNoOverlap;
}
const bool ignore_behind_test_surface =
(evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindTestSurface) ||
(evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindAllSurfaces);
const bool ignore_behind_gt_surface =
(evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindGtSurface) ||
(evaluation_mode == VoxelEvaluationMode::kIgnoreErrorBehindAllSurfaces);
if ((ignore_behind_test_surface && (voxel_test.distance) < 0.0) ||
(ignore_behind_gt_surface && (voxel_gt.distance) < 0.0)) {
return VoxelEvaluationResult::kIgnored;
}
*error = getVoxelSdf(voxel_test) - getVoxelSdf(voxel_gt);
return VoxelEvaluationResult::kEvaluated;
}
template <>
bool isObservedVoxel(const TsdfVoxel& voxel);
template <>
bool isObservedVoxel(const EsdfVoxel& voxel);
template <>
FloatingPoint getVoxelSdf(const TsdfVoxel& voxel);
template <>
FloatingPoint getVoxelSdf(const EsdfVoxel& voxel);
template <>
void setVoxelSdf(const FloatingPoint sdf, TsdfVoxel* voxel);
template <>
void setVoxelSdf(const FloatingPoint sdf, EsdfVoxel* voxel);
template <>
void setVoxelWeight(const FloatingPoint weight, TsdfVoxel* voxel);
template <>
void setVoxelWeight(const FloatingPoint weight, EsdfVoxel* voxel);
} // namespace utils
} // namespace voxblox
#endif // VOXBLOX_UTILS_EVALUATION_UTILS_H_