message: implemented naive solution
diff: @@ -87,10 +87,16 @@ class MergeIntegrator {
  template <typename VoxelType>
  static void MergeLayerAintoLayerB(const Layer<VoxelType>& layer_A,
  const Transformation& T_A_B,
  - Layer<VoxelType>* layer_B) {
  + Layer<VoxelType>* layer_B,
  + bool use_naive_method = false) {
  Layer<VoxelType> layer_A_transformed(layer_B->voxel_size(),
  layer_B->voxels_per_side());
  +
  + if (use_naive_method) {
  + naiveTransformLayer(layer_A, T_A_B, &layer_A_transformed);
  + } else {
  transformLayer(layer_A, T_A_B, &layer_A_transformed);
  + }
  MergeLayerAintoLayerB(layer_A_transformed, layer_B);
  }
  @@ -104,6 +110,53 @@ class MergeIntegrator {
  transformLayer(layer_in, Transformation(), layer_out);
  }
  + // Similar to transformLayer in functionality, however the system only makes
  + // use of the forward transform and nearest neighbor interpolation. This will
  + // result in artifacts and other issues in the result, however it should be
  + // several orders of magnitude faster.
  + template <typename VoxelType>
  + static void naiveTransformLayer(const Layer<VoxelType>& layer_in,
  + const Transformation& T_in_out,
  + Layer<VoxelType>* layer_out) {
  + BlockIndexList block_idx_list_in;
  + layer_in.getAllAllocatedBlocks(&block_idx_list_in);
  +
  + Interpolator<VoxelType> interpolator(&layer_in);
  +
  + for (const BlockIndex& block_idx : block_idx_list_in) {
  + typename const Block<VoxelType>& input_block =
  + layer_in->getBlockByIndex(block_idx);
  +
  + for (IndexElement voxel_idx = 0; voxel_idx < block->num_voxels();
  + ++voxel_idx) {
  + const VoxelType& input_voxel = block->getVoxelByLinearIndex(voxel_idx);
  +
  + // find voxel centers location in the output
  + const Point voxel_center =
  + T_in_out * block->computeCoordinatesFromLinearIndex(voxel_idx);
  +
  + const VoxexlIndex global_output_voxel_idx =
  + getGridIndexFromPoint(voxel_center, layer_out->voxel_size_inv());
  +
  + // allocate it in the output
  + typename Block<VoxelType>::Ptr output_block =
  + layer_out->allocateBlockPtrByIndex(
  + getBlockIndexFromGlobalVoxelIndex(
  + global_output_voxel_idx, layer_out->voxels_per_side_inv()));
  +
  + // get the output voxel
  + VoxelType& output_voxel =
  + block->getVoxelByVoxelIndex(getLocalFromGlobalVoxelIndex(
  + global_output_voxel_idx, layer_out->voxels_per_side_inv()));
  +
  + // interpolate voxel
  + if (interpolator.getVoxel(voxel_center, &output_voxel, false)) {
  + block->has_data() = true;
  + }
  + }
  + }
  + }
  +
  // Performs a 3D transform on the input layer and writes the results to the
  // output layer. During the transformation resampling occurs so that the voxel
  // and block size of the input and output layer can differ.
  @@ -186,8 +239,7 @@ class MergeIntegrator {
  static const FloatingPoint kUnitCubeDiagonalLength;
  };
  -const FloatingPoint MergeIntegrator::kUnitCubeDiagonalLength =
  - std::sqrt(3.0);
  +const FloatingPoint MergeIntegrator::kUnitCubeDiagonalLength = std::sqrt(3.0);
  template <>
  void MergeIntegrator::mergeVoxelAIntoVoxelB(const TsdfVoxel& voxel_A,
  