Method matchTemplates

recognition/src/linemod.cpp:116–360

/////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::LINEMOD::matchTemplates (const std::vector<QuantizableModality*> & modalities, std::vector<LINEMODDetection> & detections) const
{
  // create energy maps
  std::vector<EnergyMaps> modality_energy_maps;
  const std::size_t nr_modalities = modalities.size();
  for (std::size_t modality_index = 0; modality_index < nr_modalities; ++modality_index)
  {
    const QuantizedMap & quantized_map = modalities[modality_index]->getSpreadedQuantizedMap ();

    const std::size_t width = quantized_map.getWidth ();
    const std::size_t height = quantized_map.getHeight ();

    const unsigned char * quantized_data = quantized_map.getData ();

    const unsigned char nr_bins = 8;
    EnergyMaps energy_maps;
    energy_maps.initialize (width, height, nr_bins);
    //std::vector< unsigned char* > energy_maps(nr_bins);
    for (unsigned char bin_index = 0; bin_index < nr_bins; ++bin_index)
    {
      //energy_maps[bin_index] = new unsigned char[width*height];
      //memset (energy_maps[bin_index], 0, width*height);

      const auto base_bit = static_cast<unsigned char> (0x1);
      auto val0 = static_cast<unsigned char> (base_bit << bin_index); // e.g. 00100000
      auto val1 = static_cast<unsigned char> (val0 | (base_bit << (bin_index+1)&7) | (base_bit << (bin_index+7)&7)); // e.g. 01110000
      auto val2 = static_cast<unsigned char> (val1 | (base_bit << (bin_index+2)&7) | (base_bit << (bin_index+6)&7)); // e.g. 11111000
      auto val3 = static_cast<unsigned char> (val2 | (base_bit << (bin_index+3)&7) | (base_bit << (bin_index+5)&7)); // e.g. 11111101
      for (std::size_t index = 0; index < width*height; ++index)
      {
        if ((val0 & quantized_data[index]) != 0)
          ++energy_maps (bin_index, index);
        if ((val1 & quantized_data[index]) != 0)
          ++energy_maps (bin_index, index);
        if ((val2 & quantized_data[index]) != 0)
          ++energy_maps (bin_index, index);
        if ((val3 & quantized_data[index]) != 0)
          ++energy_maps (bin_index, index);
      }
    }

    modality_energy_maps.push_back (energy_maps);
  }

  // create linearized maps
  const std::size_t step_size = 8;
  std::vector<std::vector<LinearizedMaps> > modality_linearized_maps;
  for (std::size_t modality_index = 0; modality_index < nr_modalities; ++modality_index)
  {
    const std::size_t width = modality_energy_maps[modality_index].getWidth ();
    const std::size_t height = modality_energy_maps[modality_index].getHeight ();

    std::vector<LinearizedMaps> linearized_maps;
    const std::size_t nr_bins = modality_energy_maps[modality_index].getNumOfBins ();
    for (std::size_t bin_index = 0; bin_index < nr_bins; ++bin_index)
    {
      unsigned char * energy_map = modality_energy_maps[modality_index] (bin_index);