Program Listing for File sdf_ply.h

Return to documentation for file (voxblox/include/voxblox/io/sdf_ply.h)

#ifndef VOXBLOX_IO_SDF_PLY_H_
#define VOXBLOX_IO_SDF_PLY_H_

#include <algorithm>
#include <string>

#include "voxblox/core/layer.h"
#include "voxblox/io/mesh_ply.h"
#include "voxblox/mesh/mesh.h"
#include "voxblox/mesh/mesh_integrator.h"
#include "voxblox/mesh/mesh_layer.h"

namespace voxblox {

namespace io {

enum PlyOutputTypes {
  // The full SDF colorized by the distance in each voxel.
  kSdfColoredDistanceField,
  // Output isosurface, i.e. the mesh for sdf voxel types.
  kSdfIsosurface,
  // Output isosurface, i.e. the mesh for sdf voxel types.
  // Close vertices are connected and zero surface faces are removed.
  kSdfIsosurfaceConnected
};

template <typename VoxelType>
bool getColorFromVoxel(const VoxelType& voxel, const float sdf_color_range,
                       const float sdf_max_value, Color* color);

template <>
bool getColorFromVoxel(const TsdfVoxel& voxel, const float sdf_color_range,
                       const float sdf_max_value, Color* color);

template <>
bool getColorFromVoxel(const EsdfVoxel& voxel, const float sdf_color_range,
                       const float sdf_max_value, Color* color);

template <typename VoxelType>
bool convertVoxelGridToPointCloud(const Layer<VoxelType>& layer,
                                  const float sdf_color_range,
                                  const float sdf_max_value,
                                  voxblox::Mesh* point_cloud) {
  CHECK_NOTNULL(point_cloud);
  CHECK_GT(sdf_color_range, 0.0f);

  BlockIndexList blocks;
  layer.getAllAllocatedBlocks(&blocks);

  // Iterate over all blocks.
  for (const BlockIndex& index : blocks) {
    // Iterate over all voxels in said blocks.
    const Block<VoxelType>& block = layer.getBlockByIndex(index);

    const int vps = block.voxels_per_side();

    VoxelIndex voxel_index = VoxelIndex::Zero();
    for (voxel_index.x() = 0; voxel_index.x() < vps; ++voxel_index.x()) {
      for (voxel_index.y() = 0; voxel_index.y() < vps; ++voxel_index.y()) {
        for (voxel_index.z() = 0; voxel_index.z() < vps; ++voxel_index.z()) {
          const VoxelType& voxel = block.getVoxelByVoxelIndex(voxel_index);

          // Get back the original coordinate of this voxel.
          const Point coord =
              block.computeCoordinatesFromVoxelIndex(voxel_index);

          Color color;
          if (getColorFromVoxel(voxel, sdf_color_range, sdf_max_value,
                                &color)) {
            point_cloud->vertices.push_back(coord);
            point_cloud->colors.push_back(color);
          }
        }
      }
    }
  }
  return point_cloud->size() > 0u;
}

template <typename VoxelType>
bool convertVoxelGridToPointCloud(const Layer<VoxelType>& layer,
                                  const float sdf_color_range,
                                  voxblox::Mesh* point_cloud) {
  constexpr float kInvalidSdfMaxValue = -1.0f;
  return convertVoxelGridToPointCloud<VoxelType>(
      layer, sdf_color_range, kInvalidSdfMaxValue, point_cloud);
}

template <typename VoxelType>
bool convertLayerToMesh(
    const Layer<VoxelType>& layer, const MeshIntegratorConfig& mesh_config,
    voxblox::Mesh* mesh, const bool connected_mesh = true,
    const FloatingPoint vertex_proximity_threshold = 1e-10) {
  CHECK_NOTNULL(mesh);

  MeshLayer mesh_layer(layer.block_size());
  MeshIntegrator<VoxelType> mesh_integrator(mesh_config, layer, &mesh_layer);

  // Generate mesh layer.
  constexpr bool only_mesh_updated_blocks = false;
  constexpr bool clear_updated_flag = false;
  mesh_integrator.generateMesh(only_mesh_updated_blocks, clear_updated_flag);

  // Extract mesh from mesh layer, either by simply concatenating all meshes
  // (there is one per block) or by connecting them.
  if (connected_mesh) {
    mesh_layer.getConnectedMesh(mesh, vertex_proximity_threshold);
  } else {
    mesh_layer.getMesh(mesh);
  }
  return mesh->size() > 0u;
}
template <typename VoxelType>
bool convertLayerToMesh(
    const Layer<VoxelType>& layer, voxblox::Mesh* mesh,
    const bool connected_mesh = true,
    const FloatingPoint vertex_proximity_threshold = 1e-10) {
  MeshIntegratorConfig mesh_config;
  return convertLayerToMesh(layer, mesh_config, mesh, connected_mesh,
                            vertex_proximity_threshold);
}

template <typename VoxelType>
bool outputLayerAsPly(const Layer<VoxelType>& layer,
                      const std::string& filename, PlyOutputTypes type,
                      const float sdf_color_range = 0.3f,
                      const float max_sdf_value_to_output = 0.3f) {
  CHECK(!filename.empty());
  switch (type) {
    case PlyOutputTypes::kSdfColoredDistanceField: {
      voxblox::Mesh point_cloud;
      if (!convertVoxelGridToPointCloud(
              layer, sdf_color_range, max_sdf_value_to_output, &point_cloud)) {
        return false;
      }

      return outputMeshAsPly(filename, point_cloud);
    }
    case PlyOutputTypes::kSdfIsosurface: {
      constexpr bool kConnectedMesh = false;

      voxblox::Mesh mesh;
      if (!convertLayerToMesh(layer, &mesh, kConnectedMesh)) {
        return false;
      }
      return outputMeshAsPly(filename, mesh);
    }
    case PlyOutputTypes::kSdfIsosurfaceConnected: {
      constexpr bool kConnectedMesh = true;

      voxblox::Mesh mesh;
      if (!convertLayerToMesh(layer, &mesh, kConnectedMesh)) {
        return false;
      }
      return outputMeshAsPly(filename, mesh);
    }

    default:
      LOG(FATAL) << "Unknown layer to ply output type: "
                 << static_cast<int>(type);
  }
  return false;
}

}  // namespace io

}  // namespace voxblox

#endif  // VOXBLOX_IO_SDF_PLY_H_