<commit_before>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

//#define MBILOG_ENABLE_DEBUG

#include "mitkITKDICOMSeriesReaderHelper.h"
#include "mitkITKDICOMSeriesReaderHelper.txx"

#define switch3DCase(IOType, T) \
  case IOType: return LoadDICOMByITK< T >(filenames, correctTilt, tiltInfo, io);

bool
mitk::ITKDICOMSeriesReaderHelper
::CanHandleFile(const std::string& filename)
{
  MITK_DEBUG << "ITKDICOMSeriesReaderHelper::CanHandleFile " << filename;
  itk::GDCMImageIO::Pointer tester = itk::GDCMImageIO::New();
  if ( tester->CanReadFile(filename.c_str()) )
  {
    tester->SetFileName( filename.c_str() );
    tester->ReadImageInformation();

    std::string numberOfFrames;
    if (tester->GetValueFromTag("0028|0008", numberOfFrames))
    {
      std::istringstream converter(numberOfFrames);
      int i;
      if (converter >> i)
      {
        MITK_DEBUG << "Number of Frames for " << filename << ": " << numberOfFrames;
        return (i <= 1); // cannot handle multi-frame
      }
      else
      {
        return true; // we assume single-frame
      }
    }
    else
    {
      MITK_DEBUG << "No Number of Frames tag for " << filename;
      // friendly old single-frame file
      return true;
    }
  }

  MITK_DEBUG << "GDCMImageIO found: No DICOM in " << filename;
  // does not seem to be DICOM
  return false;
}

mitk::Image::Pointer
mitk::ITKDICOMSeriesReaderHelper
::Load( const StringContainer& filenames, bool correctTilt, const GantryTiltInformation& tiltInfo )
{
  if( filenames.empty() )
  {
    MITK_DEBUG << "Calling LoadDicomSeries with empty filename string container. Probably invalid application logic.";
    return NULL; // this is not actually an error but the result is very simple
  }

  typedef itk::GDCMImageIO DcmIoType;
  DcmIoType::Pointer io = DcmIoType::New();

  try
  {
    if (io->CanReadFile(filenames.front().c_str()))
    {
      io->SetFileName(filenames.front().c_str());
      io->ReadImageInformation();

      if (io->GetPixelType() == itk::ImageIOBase::SCALAR)
      {
        switch (io->GetComponentType())
        {
          switch3DCase(DcmIoType::UCHAR, unsigned char)
          switch3DCase(DcmIoType::CHAR, char)
          switch3DCase(DcmIoType::USHORT, unsigned short)
          switch3DCase(DcmIoType::SHORT, short)
          switch3DCase(DcmIoType::UINT, unsigned int)
          switch3DCase(DcmIoType::INT, int)
          switch3DCase(DcmIoType::ULONG, long unsigned int)
          switch3DCase(DcmIoType::LONG, long int)
          switch3DCase(DcmIoType::FLOAT, float)
          switch3DCase(DcmIoType::DOUBLE, double)
          default:
            MITK_ERROR << "Found unsupported DICOM scalar pixel type: (enum value) " << io->GetComponentType();
        }
      }
      else if (io->GetPixelType() == itk::ImageIOBase::RGB)
      {
        switch (io->GetComponentType())
        {
          switch3DCase(DcmIoType::UCHAR, itk::RGBPixel<unsigned char>)
          switch3DCase(DcmIoType::CHAR, itk::RGBPixel<char>)
          switch3DCase(DcmIoType::USHORT, itk::RGBPixel<unsigned short>)
          switch3DCase(DcmIoType::SHORT, itk::RGBPixel<short>)
          switch3DCase(DcmIoType::UINT, itk::RGBPixel<unsigned int>)
          switch3DCase(DcmIoType::INT, itk::RGBPixel<int>)
          switch3DCase(DcmIoType::ULONG, itk::RGBPixel<long unsigned int>)
          switch3DCase(DcmIoType::LONG, itk::RGBPixel<long int>)
          switch3DCase(DcmIoType::FLOAT, itk::RGBPixel<float>)
          switch3DCase(DcmIoType::DOUBLE, itk::RGBPixel<double>)
          default:
            MITK_ERROR << "Found unsupported DICOM scalar pixel type: (enum value) " << io->GetComponentType();
        }
      }

      MITK_ERROR << "Unsupported DICOM pixel type";
      return NULL;
    }
  }
  catch(itk::MemoryAllocationError& e)
  {
    MITK_ERROR << "Out of memory. Cannot load DICOM series: " << e.what();
  }
  catch(std::exception& e)
  {
    MITK_ERROR << "Error encountered when loading DICOM series:" << e.what();
  }
  catch(...)
  {
    MITK_ERROR << "Unspecified error encountered when loading DICOM series.";
  }

  return NULL;
}

#define switch3DnTCase(IOType, T) \
  case IOType: return LoadDICOMByITK3DnT< T >(filenamesLists, correctTilt, tiltInfo, io);

mitk::Image::Pointer
mitk::ITKDICOMSeriesReaderHelper
::Load3DnT( const StringContainerList& filenamesLists, bool correctTilt, const GantryTiltInformation& tiltInfo )
{
  if( filenamesLists.empty() || filenamesLists.front().empty() )
  {
    MITK_DEBUG << "Calling LoadDicomSeries with empty filename string container. Probably invalid application logic.";
    return NULL; // this is not actually an error but the result is very simple
  }

  typedef itk::GDCMImageIO DcmIoType;
  DcmIoType::Pointer io = DcmIoType::New();

  try
  {
    if (io->CanReadFile(filenamesLists.front().front().c_str()))
    {
      io->SetFileName(filenamesLists.front().front().c_str());
      io->ReadImageInformation();

      if (io->GetPixelType() == itk::ImageIOBase::SCALAR)
      {
        switch (io->GetComponentType())
        {
          switch3DnTCase(DcmIoType::UCHAR, unsigned char)
          switch3DnTCase(DcmIoType::CHAR, char)
          switch3DnTCase(DcmIoType::USHORT, unsigned short)
          switch3DnTCase(DcmIoType::SHORT, short)
          switch3DnTCase(DcmIoType::UINT, unsigned int)
          switch3DnTCase(DcmIoType::INT, int)
          switch3DnTCase(DcmIoType::ULONG, long unsigned int)
          switch3DnTCase(DcmIoType::LONG, long int)
          switch3DnTCase(DcmIoType::FLOAT, float)
          switch3DnTCase(DcmIoType::DOUBLE, double)
          default:
            MITK_ERROR << "Found unsupported DICOM scalar pixel type: (enum value) " << io->GetComponentType();
        }
      }
      else if (io->GetPixelType() == itk::ImageIOBase::RGB)
      {
        switch (io->GetComponentType())
        {
          switch3DnTCase(DcmIoType::UCHAR, itk::RGBPixel<unsigned char>)
          switch3DnTCase(DcmIoType::CHAR, itk::RGBPixel<char>)
          switch3DnTCase(DcmIoType::USHORT, itk::RGBPixel<unsigned short>)
          switch3DnTCase(DcmIoType::SHORT, itk::RGBPixel<short>)
          switch3DnTCase(DcmIoType::UINT, itk::RGBPixel<unsigned int>)
          switch3DnTCase(DcmIoType::INT, itk::RGBPixel<int>)
          switch3DnTCase(DcmIoType::ULONG, itk::RGBPixel<long unsigned int>)
          switch3DnTCase(DcmIoType::LONG, itk::RGBPixel<long int>)
          switch3DnTCase(DcmIoType::FLOAT, itk::RGBPixel<float>)
          switch3DnTCase(DcmIoType::DOUBLE, itk::RGBPixel<double>)
          default:
            MITK_ERROR << "Found unsupported DICOM scalar pixel type: (enum value) " << io->GetComponentType();
        }
      }

      MITK_ERROR << "Unsupported DICOM pixel type";
      return NULL;
    }
  }
  catch(itk::MemoryAllocationError& e)
  {
    MITK_ERROR << "Out of memory. Cannot load DICOM series: " << e.what();
  }
  catch(std::exception& e)
  {
    MITK_ERROR << "Error encountered when loading DICOM series:" << e.what();
  }
  catch(...)
  {
    MITK_ERROR << "Unspecified error encountered when loading DICOM series.";
  }

  return NULL;
}
<commit_msg>removed unnecessary check that prevented multiframe support<commit_after>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

//#define MBILOG_ENABLE_DEBUG

#include "mitkITKDICOMSeriesReaderHelper.h"
#include "mitkITKDICOMSeriesReaderHelper.txx"

#define switch3DCase(IOType, T) \
  case IOType: return LoadDICOMByITK< T >(filenames, correctTilt, tiltInfo, io);

bool
mitk::ITKDICOMSeriesReaderHelper
::CanHandleFile(const std::string& filename)
{
  MITK_DEBUG << "ITKDICOMSeriesReaderHelper::CanHandleFile " << filename;
  itk::GDCMImageIO::Pointer tester = itk::GDCMImageIO::New();
  return tester->CanReadFile(filename.c_str());
}

mitk::Image::Pointer
mitk::ITKDICOMSeriesReaderHelper
::Load( const StringContainer& filenames, bool correctTilt, const GantryTiltInformation& tiltInfo )
{
  if( filenames.empty() )
  {
    MITK_DEBUG << "Calling LoadDicomSeries with empty filename string container. Probably invalid application logic.";
    return NULL; // this is not actually an error but the result is very simple
  }

  typedef itk::GDCMImageIO DcmIoType;
  DcmIoType::Pointer io = DcmIoType::New();

  try
  {
    if (io->CanReadFile(filenames.front().c_str()))
    {
      io->SetFileName(filenames.front().c_str());
      io->ReadImageInformation();

      if (io->GetPixelType() == itk::ImageIOBase::SCALAR)
      {
        switch (io->GetComponentType())
        {
          switch3DCase(DcmIoType::UCHAR, unsigned char)
          switch3DCase(DcmIoType::CHAR, char)
          switch3DCase(DcmIoType::USHORT, unsigned short)
          switch3DCase(DcmIoType::SHORT, short)
          switch3DCase(DcmIoType::UINT, unsigned int)
          switch3DCase(DcmIoType::INT, int)
          switch3DCase(DcmIoType::ULONG, long unsigned int)
          switch3DCase(DcmIoType::LONG, long int)
          switch3DCase(DcmIoType::FLOAT, float)
          switch3DCase(DcmIoType::DOUBLE, double)
          default:
            MITK_ERROR << "Found unsupported DICOM scalar pixel type: (enum value) " << io->GetComponentType();
        }
      }
      else if (io->GetPixelType() == itk::ImageIOBase::RGB)
      {
        switch (io->GetComponentType())
        {
          switch3DCase(DcmIoType::UCHAR, itk::RGBPixel<unsigned char>)
          switch3DCase(DcmIoType::CHAR, itk::RGBPixel<char>)
          switch3DCase(DcmIoType::USHORT, itk::RGBPixel<unsigned short>)
          switch3DCase(DcmIoType::SHORT, itk::RGBPixel<short>)
          switch3DCase(DcmIoType::UINT, itk::RGBPixel<unsigned int>)
          switch3DCase(DcmIoType::INT, itk::RGBPixel<int>)
          switch3DCase(DcmIoType::ULONG, itk::RGBPixel<long unsigned int>)
          switch3DCase(DcmIoType::LONG, itk::RGBPixel<long int>)
          switch3DCase(DcmIoType::FLOAT, itk::RGBPixel<float>)
          switch3DCase(DcmIoType::DOUBLE, itk::RGBPixel<double>)
          default:
            MITK_ERROR << "Found unsupported DICOM scalar pixel type: (enum value) " << io->GetComponentType();
        }
      }

      MITK_ERROR << "Unsupported DICOM pixel type";
      return NULL;
    }
  }
  catch(itk::MemoryAllocationError& e)
  {
    MITK_ERROR << "Out of memory. Cannot load DICOM series: " << e.what();
  }
  catch(std::exception& e)
  {
    MITK_ERROR << "Error encountered when loading DICOM series:" << e.what();
  }
  catch(...)
  {
    MITK_ERROR << "Unspecified error encountered when loading DICOM series.";
  }

  return NULL;
}

#define switch3DnTCase(IOType, T) \
  case IOType: return LoadDICOMByITK3DnT< T >(filenamesLists, correctTilt, tiltInfo, io);

mitk::Image::Pointer
mitk::ITKDICOMSeriesReaderHelper
::Load3DnT( const StringContainerList& filenamesLists, bool correctTilt, const GantryTiltInformation& tiltInfo )
{
  if( filenamesLists.empty() || filenamesLists.front().empty() )
  {
    MITK_DEBUG << "Calling LoadDicomSeries with empty filename string container. Probably invalid application logic.";
    return NULL; // this is not actually an error but the result is very simple
  }

  typedef itk::GDCMImageIO DcmIoType;
  DcmIoType::Pointer io = DcmIoType::New();

  try
  {
    if (io->CanReadFile(filenamesLists.front().front().c_str()))
    {
      io->SetFileName(filenamesLists.front().front().c_str());
      io->ReadImageInformation();

      if (io->GetPixelType() == itk::ImageIOBase::SCALAR)
      {
        switch (io->GetComponentType())
        {
          switch3DnTCase(DcmIoType::UCHAR, unsigned char)
          switch3DnTCase(DcmIoType::CHAR, char)
          switch3DnTCase(DcmIoType::USHORT, unsigned short)
          switch3DnTCase(DcmIoType::SHORT, short)
          switch3DnTCase(DcmIoType::UINT, unsigned int)
          switch3DnTCase(DcmIoType::INT, int)
          switch3DnTCase(DcmIoType::ULONG, long unsigned int)
          switch3DnTCase(DcmIoType::LONG, long int)
          switch3DnTCase(DcmIoType::FLOAT, float)
          switch3DnTCase(DcmIoType::DOUBLE, double)
          default:
            MITK_ERROR << "Found unsupported DICOM scalar pixel type: (enum value) " << io->GetComponentType();
        }
      }
      else if (io->GetPixelType() == itk::ImageIOBase::RGB)
      {
        switch (io->GetComponentType())
        {
          switch3DnTCase(DcmIoType::UCHAR, itk::RGBPixel<unsigned char>)
          switch3DnTCase(DcmIoType::CHAR, itk::RGBPixel<char>)
          switch3DnTCase(DcmIoType::USHORT, itk::RGBPixel<unsigned short>)
          switch3DnTCase(DcmIoType::SHORT, itk::RGBPixel<short>)
          switch3DnTCase(DcmIoType::UINT, itk::RGBPixel<unsigned int>)
          switch3DnTCase(DcmIoType::INT, itk::RGBPixel<int>)
          switch3DnTCase(DcmIoType::ULONG, itk::RGBPixel<long unsigned int>)
          switch3DnTCase(DcmIoType::LONG, itk::RGBPixel<long int>)
          switch3DnTCase(DcmIoType::FLOAT, itk::RGBPixel<float>)
          switch3DnTCase(DcmIoType::DOUBLE, itk::RGBPixel<double>)
          default:
            MITK_ERROR << "Found unsupported DICOM scalar pixel type: (enum value) " << io->GetComponentType();
        }
      }

      MITK_ERROR << "Unsupported DICOM pixel type";
      return NULL;
    }
  }
  catch(itk::MemoryAllocationError& e)
  {
    MITK_ERROR << "Out of memory. Cannot load DICOM series: " << e.what();
  }
  catch(std::exception& e)
  {
    MITK_ERROR << "Error encountered when loading DICOM series:" << e.what();
  }
  catch(...)
  {
    MITK_ERROR << "Unspecified error encountered when loading DICOM series.";
  }

  return NULL;
}
<|endoftext|>


<commit_before>#include <chrono>
#include <iostream>
#include <vector>

#include <armadillo>

using std::chrono::high_resolution_clock;
using std::chrono::duration;

namespace {
	uint32_t num = 1;
	uint32_t fft_size = 48000;
	uint32_t conv_ir_size = 72000;
	uint32_t conv_sig_size = 5760000;
}

duration<double> Convolution(uint32_t ir_size, uint32_t sig_size) {
	arma::fvec sig {arma::randn<arma::fvec>(sig_size+ir_size-1)};
	sig.subvec(sig_size,sig_size+ir_size-2) = arma::zeros<arma::fvec>(ir_size-1);
	arma::fvec ir {arma::randn<arma::fvec>(ir_size)};
	arma::fvec output (sig_size+ir_size-1);

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		for (uint32_t sample_cnt=0;sample_cnt<sig_size;++sample_cnt) {
			for (uint32_t ir_cnt=0;ir_cnt<ir_size;++ir_cnt) {
				output[sample_cnt] += sig[sample_cnt+ir_cnt] * ir[ir_cnt];
			}
		}
	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(output);
	auto end = high_resolution_clock::now();

	return end - begin;
}

duration<double> ArmadilloConv(uint32_t ir_size, uint32_t sig_size) {
	arma::fvec sig {arma::randn<arma::fvec>(sig_size+ir_size-1)};
	sig.subvec(sig_size,sig_size+ir_size-2) = arma::zeros<arma::fvec>(ir_size-1);
	arma::fvec ir {arma::randn<arma::fvec>(ir_size)};
	arma::fvec output;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::conv(sig, ir);
	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(output);
	auto end = high_resolution_clock::now();

	return end - begin;
}

duration<double> ArmadilloFftConv(uint32_t ir_size, uint32_t sig_size) {
	arma::fvec sig {arma::randn<arma::fvec>(sig_size+ir_size-1)};
	sig.subvec(sig_size,sig_size+ir_size-2) = arma::zeros<arma::fvec>(ir_size-1);
	arma::fvec ir {arma::randn<arma::fvec>(ir_size)};
	arma::cx_fvec output;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::ifft(arma::fft(sig) % arma::fft(ir,sig_size+ir_size-1));
	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(arma::real(output));
	auto end = high_resolution_clock::now();

	return end - begin;
}

duration<double> ArmadilloFftPow2Conv(uint32_t ir_size, uint32_t sig_size) {
	uint32_t size = pow(2,ceil(log2(sig_size+ir_size-1)));
	arma::fvec sig {arma::randn<arma::fvec>(sig_size)};
	arma::fvec ir {arma::randn<arma::fvec>(ir_size)};
	arma::cx_fvec output;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::ifft(arma::fft(sig,size) % arma::fft(ir,size));
	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(arma::real(output));
	auto end = high_resolution_clock::now();

	return end - begin;
}

duration<double> ArmadilloFft(uint32_t fft_size) {
	arma::fvec input {arma::randn<arma::fvec>(fft_size)};
	arma::cx_fvec output_fd;
	arma::cx_fvec output_td;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output_fd = arma::fft(input);
		output_td = arma::ifft(output_fd);

	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(arma::real(output_td));
	auto end = high_resolution_clock::now();

	return end - begin;
}

int main(int argc, char* argv[]) {
	auto arma_fft_time = ArmadilloFft(::fft_size);
	std::cout << "Armadillo FFT: " << arma_fft_time.count() << std::endl;

	auto arma_fft_pow2_conv_time = ArmadilloFftPow2Conv(::conv_ir_size, ::conv_sig_size);
	std::cout << "Armadillo FFT-Pow2-convolution: " << arma_fft_pow2_conv_time.count() << std::endl;

	auto arma_fft_conv_time = ArmadilloFftConv(::conv_ir_size, ::conv_sig_size);
	std::cout << "Armadillo FFT-convolution: " << arma_fft_conv_time.count() << std::endl;

	auto arma_conv_time = ArmadilloConv(::conv_ir_size, ::conv_sig_size);
	std::cout << "Armadillo convolution: " << arma_conv_time.count() << std::endl;

	auto conv_time = Convolution(::conv_ir_size, ::conv_sig_size);
	std::cout << "convolution: " << conv_time.count() << std::endl;
}
<commit_msg>fix convolution calculation, compare results<commit_after>#include <chrono>
#include <iostream>
#include <vector>
#include <utility> // std::pair

#include <armadillo>

using std::chrono::high_resolution_clock;
using std::chrono::duration;

namespace {
	uint32_t num = 1;
	uint32_t fft_size = 48000;
	uint32_t conv_ir_size = 72000;
	uint32_t conv_sig_size = 5760000;
}

std::pair<duration<double>, arma::fvec> Convolution(arma::fvec sig, arma::fvec ir) {
	size_t size = (::conv_sig_size+::conv_ir_size-1);
	ir = arma::flipud(ir);
	arma::fvec sig_new = arma::zeros<arma::fvec>(::conv_sig_size + 2*(::conv_ir_size-1));
	sig_new.subvec(::conv_ir_size - 1, ::conv_sig_size + ::conv_ir_size -2) = sig;
	arma::fvec output (::conv_sig_size + ::conv_ir_size - 1, arma::fill::zeros);

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		for (uint32_t sample_cnt=0;sample_cnt<size;++sample_cnt) {
			for (uint32_t ir_cnt=0;ir_cnt<::conv_ir_size;++ir_cnt) {
				output[sample_cnt] += sig_new[sample_cnt+ir_cnt] * ir[ir_cnt];
			}
		}
	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(output);
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin, output);
}

std::pair<duration<double>, arma::fvec> ArmadilloConv(arma::fvec sig, arma::fvec ir) {
	arma::fvec output;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::conv(sig, ir);
	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(output);
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin, output);
}

std::pair<duration<double>, arma::fvec> ArmadilloFftConv(arma::fvec sig, arma::fvec ir) {
	arma::cx_fvec output;
	size_t size = sig.size() + ir.size() - 1;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::ifft(arma::fft(sig, size) % arma::fft(ir, size));
	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(arma::real(output));
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin, arma::real(output));
}

std::pair<duration<double>, arma::fvec> ArmadilloFftPow2Conv(arma::fvec sig, arma::fvec ir) {
	uint32_t size = pow(2,ceil(log2(::conv_sig_size + ::conv_ir_size - 1)));
	arma::cx_fvec output;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output = arma::ifft(arma::fft(sig,size) % arma::fft(ir,size));
	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(arma::real(output));
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin, arma::real(output.subvec(0, ::conv_sig_size + ::conv_ir_size - 2)));
}

std::pair<duration<double>,arma::fvec> ArmadilloFft(arma::fvec input) {
	arma::cx_fvec output_fd;
	arma::cx_fvec output_td;

	auto begin = high_resolution_clock::now();
	for (uint32_t cnt=0;cnt<::num;++cnt) {
		output_fd = arma::fft(input);
		output_td = arma::ifft(output_fd);

	}
	std::vector<float> output_copy = arma::conv_to<std::vector<float>>::from(arma::real(output_td));
	auto end = high_resolution_clock::now();

	return std::make_pair(end - begin,arma::real(output_td));
}

int main(int argc, char* argv[]) {

	// generate input signals
	arma::fvec sig {arma::randn<arma::fvec>(::conv_sig_size)};
	arma::fvec ir {arma::randn<arma::fvec>(::conv_ir_size)};

	auto result_arma_fft = ArmadilloFft(sig);
	std::cout << "Armadillo FFT: " << result_arma_fft.first.count() << std::endl;

	// normal convolution, this is our reference output
	auto result_conv = Convolution(sig, ir);
	std::cout << "convolution: " << result_conv.first.count()
			  << std::endl;

	auto result_arma_fft_pow2_conv = ArmadilloFftPow2Conv(sig, ir);
	std::cout << "Armadillo FFT-Pow2-convolution: "
		      << result_arma_fft_pow2_conv.first.count()
			  << "\n\tmaximum difference of result: "
			  << arma::abs(result_conv.second - result_arma_fft_pow2_conv.second).max()
			  << std::endl;

	auto result_arma_fft_conv = ArmadilloFftConv(sig, ir);
	std::cout << "Armadillo FFT-convolution: "
		      << result_arma_fft_conv.first.count()
			  << "\n\tmaximum difference of result: "
			  << arma::abs(result_conv.second - result_arma_fft_conv.second).max()
			  << std::endl;

	auto result_arma_conv = ArmadilloConv(sig, ir);
	std::cout << "Armadillo convolution: "
		      << result_arma_conv.first.count()
			  << "\n\tmaximum difference of result: "
			  << arma::abs(result_conv.second - result_arma_conv.second).max()
			  << std::endl;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <queue>
#include <sys/wait.h>
#include <sys/types.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include "Shell.h"
#include "Input.h"

using namespace std;

int main() {
    Input in;

//    while(1) {

    in.getInput();
    queue<string> tasks = in.Parse();

    char** c;
    while (tasks.size() != 0) {
        c = in.toChar(tasks.front());
        tasks.pop();
    }

    pid_t pid = fork();
    if (pid == 0) {
        // cout << "child: " << pid << endl;
        if (execvp(c[0], c) == -1) {
            perror("exec");
            exit(0);
        }
    }

    if (pid > 0) {
        if ( wait(0) == 1 ) {
            perror("wait");
        }
        // cout << "parent: " << pid << endl;
    }

//    int i = 0;
//    while (c[i] != '\0') {
//        cout << c[i] << endl;
//        ++i;
//    }

    // in.getInput();

    cout << flush;
    delete c;

 //   }

    return 0;

}
<commit_msg>moved execute into loop<commit_after>#include <iostream>
#include <queue>
#include <sys/wait.h>
#include <sys/types.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include "Shell.h"
#include "Input.h"

using namespace std;

int main() {
    Input in;

//    while(1) {

    in.getInput();
    queue<string> tasks = in.Parse();

    char** c;
    Execute ex;
    while (tasks.size() != 0) {
        c = in.toChar(tasks.front());
        ex.execute(c);
        tasks.pop();
    }

    // Execute ex;
    // ex.execute(c);

//    int i = 0;
//    while (c[i] != '\0') {
//        cout << c[i] << endl;
//        ++i;
//    }

    // in.getInput();

    cout << flush;
    delete c;

//    }

    return 0;

}
<|endoftext|>


<commit_before>/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
/*=========================================================================
 *
 *  Portions of this file are subject to the VTK Toolkit Version 3 copyright.
 *
 *  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
 *
 *  For complete copyright, license and disclaimer of warranty information
 *  please refer to the NOTICE file at the top of the ITK source tree.
 *
 *=========================================================================*/
#include "itkProcessObject.h"
#include "itkSmartPointerForwardReference.txx"
#include "itkRealTimeClock.h"
#include "itkTemporalDataObject.h"

// Manual instantiation is necessary to prevent link errors
template class itk::SmartPointerForwardReference< itk::ProcessObject >;

namespace itk
{

//----------------------------------------------------------------------------
TemporalDataObject::TemporalDataObject()
  : m_LargestPossibleTemporalRegion(),
    m_RequestedTemporalRegion(),
    m_BufferedTemporalRegion(),
    m_TemporalUnit(Frame)
{
  m_DataObjectBuffer = BufferType::New();
}

//----------------------------------------------------------------------------
TemporalDataObject
::~TemporalDataObject()
{}

//----------------------------------------------------------------------------
const TemporalDataObject::TemporalRegionType
TemporalDataObject
::GetUnbufferedRequestedTemporalRegion()
{
  // Get the start and end of the buffered and requested temporal regions
  unsigned long reqStart = m_RequestedTemporalRegion.GetFrameStart();
  unsigned long reqEnd = m_RequestedTemporalRegion.GetFrameStart() +
                          m_RequestedTemporalRegion.GetFrameDuration() - 1;
  unsigned long bufStart = m_BufferedTemporalRegion.GetFrameStart();
  unsigned long bufEnd = m_BufferedTemporalRegion.GetFrameStart() +
                          m_BufferedTemporalRegion.GetFrameDuration() - 1;

  //DEBUG
  std::cout << "req start: " << reqStart << " req end: " << reqEnd << std::endl;
  std::cout << "buf start: " << bufStart << " buf end: " << bufEnd << std::endl;

  // Handle case with unbuffered frames at beginning and end
  if (reqStart < bufStart && reqEnd > bufEnd)
    {
    itkDebugMacro(<< "Unbuffered frames at beginning and end. Returning entire "
                  << "requested region as unbuffered");
    return this->m_RequestedTemporalRegion;
    }

  // Handle case with unbuffered frames at end -- TODO: FIX FOR REAL TIME!!!!!
  else if(reqEnd > bufEnd)
    {
    TemporalRegionType out;
    out.SetFrameStart(bufEnd + 1);
    out.SetFrameDuration(reqEnd - bufEnd);
    return out;
    }

  // Handle case with unbuffered frames at beginning -- TODO: FIX FOR REAL TIME!!!!!
  else if(reqStart < bufStart)
    {
    TemporalRegionType out;
    out.SetFrameStart(reqStart);
    out.SetFrameDuration(bufStart - reqStart);
    return out;
    }

  // Otherwise, nothing unbuffered
  else
    {
    TemporalRegionType out;
    out.SetFrameStart(0);
    out.SetFrameDuration(0);
    return out;
    }

}

//----------------------------------------------------------------------------
void
TemporalDataObject
::SetRequestedRegionToLargestPossibleRegion()
{
  this->SetRequestedTemporalRegion( this->GetLargestPossibleTemporalRegion() );
}

//----------------------------------------------------------------------------
bool
TemporalDataObject
::RequestedRegionIsOutsideOfTheBufferedRegion()
{
  bool frameFlag = m_RequestedTemporalRegion.GetFrameStart() <
    m_BufferedTemporalRegion.GetFrameStart();
  frameFlag |= m_RequestedTemporalRegion.GetFrameDuration() >
    m_BufferedTemporalRegion.GetFrameDuration();
  bool realTimeFlag = m_RequestedTemporalRegion.GetRealStart() <
    m_BufferedTemporalRegion.GetRealStart();
  realTimeFlag |= m_RequestedTemporalRegion.GetRealDuration() >
    m_BufferedTemporalRegion.GetRealDuration();
  switch( m_TemporalUnit )
    {
    case Frame:
      {
      return frameFlag;
      }
    case RealTime:
      {
      return realTimeFlag;
      }
    case FrameAndRealTime:
      {
      return frameFlag || realTimeFlag;
      }
    default:
      itkExceptionMacro( << "itk::TemporalDataObject::"
                         << "RequestedRegionIsOutsideOfTheBufferedRegion() "
                         << "Invalid Temporal Unit" );
      return true;
    }
}

//----------------------------------------------------------------------------
bool
TemporalDataObject
::VerifyRequestedRegion()
{
  bool frameFlag = m_RequestedTemporalRegion.GetFrameStart() >=
    m_LargestPossibleTemporalRegion.GetFrameStart();
  frameFlag &= m_RequestedTemporalRegion.GetFrameDuration() <=
    m_LargestPossibleTemporalRegion.GetFrameDuration();
  bool realTimeFlag = m_RequestedTemporalRegion.GetRealStart() >=
    m_LargestPossibleTemporalRegion.GetRealStart();
  realTimeFlag &= m_RequestedTemporalRegion.GetRealDuration() <=
    m_LargestPossibleTemporalRegion.GetRealDuration();
  switch( m_TemporalUnit )
    {
    case Frame:
      {
      return frameFlag;
      }
    case RealTime:
      {
      return realTimeFlag;
      }
    case FrameAndRealTime:
      {
      return frameFlag && realTimeFlag;
      }
    default:
      itkExceptionMacro( << "itk::TemporalDataObject::VerifyRequestedRegion() "
                         << "Invalid Temporal Unit" );
      return false;
    }
}

//----------------------------------------------------------------------------
void
TemporalDataObject
::CopyInformation(const DataObject *data)
{
  // Standard call to the superclass' method
  Superclass::CopyInformation(data);

  const TemporalDataObject* temporalData;
  temporalData = dynamic_cast< const TemporalDataObject* >( data );

  if ( temporalData )
    {
    // Copy the meta data for this data type
    this->SetLargestPossibleTemporalRegion(
      temporalData->GetLargestPossibleTemporalRegion() );
    for( unsigned int i = 0;
         i < this->m_DataObjectBuffer->GetNumberOfBuffers();
         ++i )
      {
      if( this->m_DataObjectBuffer->BufferIsFull(i) )
        {
        m_DataObjectBuffer->GetBufferContents(i)->CopyInformation(
          temporalData->m_DataObjectBuffer->GetBufferContents(i) );
        }
      }
    }
  else
    {
    // pointer could not be cast back down
    itkExceptionMacro( << "itk::TemporalDataObject::CopyInformation() "
                       << "cannot cast " << typeid( data ).name() << " to "
                       << typeid( const TemporalDataObject* ).name() );
    }
}

//----------------------------------------------------------------------------
void
TemporalDataObject
::Graft(const DataObject *data)
{
  const TemporalDataObject* temporalData;

  temporalData = dynamic_cast< const TemporalDataObject* >( data );

  if( temporalData )
    {
    // Copy the meta-information
    this->CopyInformation( temporalData );

    this->SetBufferedTemporalRegion(
      temporalData->GetBufferedTemporalRegion() );
    this->SetRequestedTemporalRegion(
      temporalData->GetRequestedTemporalRegion() );

    for( unsigned int i = 0;
         i < this->m_DataObjectBuffer->GetNumberOfBuffers();
         ++i )
      {
      if( this->m_DataObjectBuffer->BufferIsFull(i) )
        {
        m_DataObjectBuffer->GetBufferContents(i)->Graft(
          temporalData->m_DataObjectBuffer->GetBufferContents(i) );
        }
      }
    }
  else
    {
    // pointer could not be cast back down
    itkExceptionMacro( << "itk::TemporalDataObject::Graft() "
                       << "cannot cast " << typeid( data ).name() << " to "
                       << typeid( const TemporalDataObject* ).name() );
    }
}

//----------------------------------------------------------------------------
void
TemporalDataObject
::SetRequestedRegion(DataObject *data)
{
  TemporalDataObject *temporalData;

  temporalData = dynamic_cast< TemporalDataObject * >( data );

  if ( temporalData )
    {
    // only copy the RequestedTemporalRegion if the parameter object is
    // a temporal data object
    this->SetRequestedTemporalRegion(
      temporalData->GetRequestedTemporalRegion() );
    for( unsigned int i = 0;
         i < this->m_DataObjectBuffer->GetNumberOfBuffers();
         ++i )
      {
      if( this->m_DataObjectBuffer->BufferIsFull(i) )
        {
        m_DataObjectBuffer->GetBufferContents(i)->SetRequestedRegion(
          temporalData->m_DataObjectBuffer->GetBufferContents(i) );
        }
      }
    }
  else
    {
    // pointer could not be cast back down
    itkExceptionMacro( << "itk::TemporalDataObject:SetRequestedRegion() "
                       << "cannot cast " << typeid( data ).name() << " to "
                       << typeid( const TemporalDataObject* ).name() );
    }
}

//----------------------------------------------------------------------------
void
TemporalDataObject
::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "Data Object Buffer: " << m_DataObjectBuffer.GetPointer()
     << std::endl;
  os << indent << "LargestPossibleTemporalRegion: " << std::endl;
  this->GetLargestPossibleTemporalRegion().Print( os,
                                                  indent.GetNextIndent() );

  os << indent << "BufferedTemporalRegion: " << std::endl;
  this->GetBufferedTemporalRegion().Print( os, indent.GetNextIndent() );

  os << indent << "RequestedTemporalRegion: " << std::endl;
  this->GetRequestedTemporalRegion().Print( os, indent.GetNextIndent() );
}

} // end namespace itk
<commit_msg>ENH: Fix calculations for unbuffered region<commit_after>/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
/*=========================================================================
 *
 *  Portions of this file are subject to the VTK Toolkit Version 3 copyright.
 *
 *  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
 *
 *  For complete copyright, license and disclaimer of warranty information
 *  please refer to the NOTICE file at the top of the ITK source tree.
 *
 *=========================================================================*/
#include "itkProcessObject.h"
#include "itkSmartPointerForwardReference.txx"
#include "itkRealTimeClock.h"
#include "itkTemporalDataObject.h"

// Manual instantiation is necessary to prevent link errors
template class itk::SmartPointerForwardReference< itk::ProcessObject >;

namespace itk
{

//----------------------------------------------------------------------------
TemporalDataObject::TemporalDataObject()
  : m_LargestPossibleTemporalRegion(),
    m_RequestedTemporalRegion(),
    m_BufferedTemporalRegion(),
    m_TemporalUnit(Frame)
{
  m_DataObjectBuffer = BufferType::New();
}

//----------------------------------------------------------------------------
TemporalDataObject
::~TemporalDataObject()
{}

//----------------------------------------------------------------------------
const TemporalDataObject::TemporalRegionType
TemporalDataObject
::GetUnbufferedRequestedTemporalRegion()
{
  // If nothing is buffered or nothing is requested, just return the entire request
  if (m_BufferedTemporalRegion.GetFrameDuration() == 0 ||
      m_RequestedTemporalRegion.GetFrameDuration() == 0)
    {
    return m_RequestedTemporalRegion;
    }

  // Get the start and end of the buffered and requested temporal regions
  unsigned long reqStart = m_RequestedTemporalRegion.GetFrameStart();
  unsigned long reqEnd = m_RequestedTemporalRegion.GetFrameStart() +
                          m_RequestedTemporalRegion.GetFrameDuration() - 1;

  unsigned long bufStart = m_BufferedTemporalRegion.GetFrameStart();
  unsigned long bufEnd = m_BufferedTemporalRegion.GetFrameStart() +
                          m_BufferedTemporalRegion.GetFrameDuration() - 1;

  // If the request starts after the buffered region, return the whole request
  if (reqStart > bufEnd)
    {
    return m_RequestedTemporalRegion;
    }

  // Handle case with unbuffered frames at beginning and end
  if (reqStart < bufStart && reqEnd > bufEnd)
    {
    itkDebugMacro(<< "Unbuffered frames at beginning and end. Returning entire "
                  << "requested region as unbuffered");
    return this->m_RequestedTemporalRegion;
    }

  // Handle case with unbuffered frames at end -- TODO: FIX FOR REAL TIME!!!!!
  else if(reqEnd > bufEnd)
    {
    TemporalRegionType out;
    out.SetFrameStart(bufEnd + 1);
    out.SetFrameDuration(reqEnd - bufEnd);
    return out;
    }

  // Handle case with unbuffered frames at beginning -- TODO: FIX FOR REAL TIME!!!!!
  else if(reqStart < bufStart)
    {
    TemporalRegionType out;
    out.SetFrameStart(reqStart);
    out.SetFrameDuration(bufStart - reqStart);
    return out;
    }

  // Otherwise, nothing unbuffered
  else
    {
    TemporalRegionType out;
    out.SetFrameStart(0);
    out.SetFrameDuration(0);
    return out;
    }

}

//----------------------------------------------------------------------------
void
TemporalDataObject
::SetRequestedRegionToLargestPossibleRegion()
{
  this->SetRequestedTemporalRegion( this->GetLargestPossibleTemporalRegion() );
}

//----------------------------------------------------------------------------
bool
TemporalDataObject
::RequestedRegionIsOutsideOfTheBufferedRegion()
{
  bool frameFlag = m_RequestedTemporalRegion.GetFrameStart() <
    m_BufferedTemporalRegion.GetFrameStart();
  frameFlag |= m_RequestedTemporalRegion.GetFrameDuration() >
    m_BufferedTemporalRegion.GetFrameDuration();
  bool realTimeFlag = m_RequestedTemporalRegion.GetRealStart() <
    m_BufferedTemporalRegion.GetRealStart();
  realTimeFlag |= m_RequestedTemporalRegion.GetRealDuration() >
    m_BufferedTemporalRegion.GetRealDuration();
  switch( m_TemporalUnit )
    {
    case Frame:
      {
      return frameFlag;
      }
    case RealTime:
      {
      return realTimeFlag;
      }
    case FrameAndRealTime:
      {
      return frameFlag || realTimeFlag;
      }
    default:
      itkExceptionMacro( << "itk::TemporalDataObject::"
                         << "RequestedRegionIsOutsideOfTheBufferedRegion() "
                         << "Invalid Temporal Unit" );
      return true;
    }
}

//----------------------------------------------------------------------------
bool
TemporalDataObject
::VerifyRequestedRegion()
{
  bool frameFlag = m_RequestedTemporalRegion.GetFrameStart() >=
    m_LargestPossibleTemporalRegion.GetFrameStart();
  frameFlag &= m_RequestedTemporalRegion.GetFrameDuration() <=
    m_LargestPossibleTemporalRegion.GetFrameDuration();
  bool realTimeFlag = m_RequestedTemporalRegion.GetRealStart() >=
    m_LargestPossibleTemporalRegion.GetRealStart();
  realTimeFlag &= m_RequestedTemporalRegion.GetRealDuration() <=
    m_LargestPossibleTemporalRegion.GetRealDuration();
  switch( m_TemporalUnit )
    {
    case Frame:
      {
      return frameFlag;
      }
    case RealTime:
      {
      return realTimeFlag;
      }
    case FrameAndRealTime:
      {
      return frameFlag && realTimeFlag;
      }
    default:
      itkExceptionMacro( << "itk::TemporalDataObject::VerifyRequestedRegion() "
                         << "Invalid Temporal Unit" );
      return false;
    }
}

//----------------------------------------------------------------------------
void
TemporalDataObject
::CopyInformation(const DataObject *data)
{
  // Standard call to the superclass' method
  Superclass::CopyInformation(data);

  const TemporalDataObject* temporalData;
  temporalData = dynamic_cast< const TemporalDataObject* >( data );

  if ( temporalData )
    {
    // Copy the meta data for this data type
    this->SetLargestPossibleTemporalRegion(
      temporalData->GetLargestPossibleTemporalRegion() );
    for( unsigned int i = 0;
         i < this->m_DataObjectBuffer->GetNumberOfBuffers();
         ++i )
      {
      if( this->m_DataObjectBuffer->BufferIsFull(i) )
        {
        m_DataObjectBuffer->GetBufferContents(i)->CopyInformation(
          temporalData->m_DataObjectBuffer->GetBufferContents(i) );
        }
      }
    }
  else
    {
    // pointer could not be cast back down
    itkExceptionMacro( << "itk::TemporalDataObject::CopyInformation() "
                       << "cannot cast " << typeid( data ).name() << " to "
                       << typeid( const TemporalDataObject* ).name() );
    }
}

//----------------------------------------------------------------------------
void
TemporalDataObject
::Graft(const DataObject *data)
{
  const TemporalDataObject* temporalData;

  temporalData = dynamic_cast< const TemporalDataObject* >( data );

  if( temporalData )
    {
    // Copy the meta-information
    this->CopyInformation( temporalData );

    this->SetBufferedTemporalRegion(
      temporalData->GetBufferedTemporalRegion() );
    this->SetRequestedTemporalRegion(
      temporalData->GetRequestedTemporalRegion() );

    for( unsigned int i = 0;
         i < this->m_DataObjectBuffer->GetNumberOfBuffers();
         ++i )
      {
      if( this->m_DataObjectBuffer->BufferIsFull(i) )
        {
        m_DataObjectBuffer->GetBufferContents(i)->Graft(
          temporalData->m_DataObjectBuffer->GetBufferContents(i) );
        }
      }
    }
  else
    {
    // pointer could not be cast back down
    itkExceptionMacro( << "itk::TemporalDataObject::Graft() "
                       << "cannot cast " << typeid( data ).name() << " to "
                       << typeid( const TemporalDataObject* ).name() );
    }
}

//----------------------------------------------------------------------------
void
TemporalDataObject
::SetRequestedRegion(DataObject *data)
{
  TemporalDataObject *temporalData;

  temporalData = dynamic_cast< TemporalDataObject * >( data );

  if ( temporalData )
    {
    // only copy the RequestedTemporalRegion if the parameter object is
    // a temporal data object
    this->SetRequestedTemporalRegion(
      temporalData->GetRequestedTemporalRegion() );
    for( unsigned int i = 0;
         i < this->m_DataObjectBuffer->GetNumberOfBuffers();
         ++i )
      {
      if( this->m_DataObjectBuffer->BufferIsFull(i) )
        {
        m_DataObjectBuffer->GetBufferContents(i)->SetRequestedRegion(
          temporalData->m_DataObjectBuffer->GetBufferContents(i) );
        }
      }
    }
  else
    {
    // pointer could not be cast back down
    itkExceptionMacro( << "itk::TemporalDataObject:SetRequestedRegion() "
                       << "cannot cast " << typeid( data ).name() << " to "
                       << typeid( const TemporalDataObject* ).name() );
    }
}

//----------------------------------------------------------------------------
void
TemporalDataObject
::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "Data Object Buffer: " << m_DataObjectBuffer.GetPointer()
     << std::endl;
  os << indent << "LargestPossibleTemporalRegion: " << std::endl;
  this->GetLargestPossibleTemporalRegion().Print( os,
                                                  indent.GetNextIndent() );

  os << indent << "BufferedTemporalRegion: " << std::endl;
  this->GetBufferedTemporalRegion().Print( os, indent.GetNextIndent() );

  os << indent << "RequestedTemporalRegion: " << std::endl;
  this->GetRequestedTemporalRegion().Print( os, indent.GetNextIndent() );
}

} // end namespace itk
<|endoftext|>


<commit_before>#include <thread>
#include <condition_variable>
#include <mutex>
#include <sstream>

#include <lcm/lcm-cpp.hpp>
#include <ConciseArgs>

#include <drc_utils/LcmWrapper.hpp>
#include <drc_utils/BotWrapper.hpp>

#include <bot_lcmgl_client/lcmgl.h>

#include <lcmtypes/drc/map_scans_t.hpp>
#include <lcmtypes/drc/affordance_collection_t.hpp>
#include <lcmtypes/drc/block_fit_request_t.hpp>

#include <maps/ScanBundleView.hpp>
#include <maps/LcmTranslator.hpp>

#include <pcl/common/io.h>
#include <pcl/common/transforms.h>

#include "BlockFitter.hpp"

struct State {
  drc::BotWrapper::Ptr mBotWrapper;
  drc::LcmWrapper::Ptr mLcmWrapper;
  bool mRunContinuously;
  bool mDoFilter;
  bool mRemoveGround;
  bool mGrabExactPoses;
  bool mDebug;
  Eigen::Vector3f mBlockSize;
  int mAlgorithm;
  bool mDoTrigger;
  std::string mNamePrefix;
  bool mTriggered;

  drc::map_scans_t mData;
  int64_t mLastDataTime;
  Eigen::Isometry3f mSensorPose;
  Eigen::Isometry3f mGroundPose;

  std::thread mWorkerThread;
  std::condition_variable mCondition;
  std::mutex mProcessMutex;
  std::mutex mDataMutex;

  State() {
    mRunContinuously = false;
    mDoFilter = true;
    mRemoveGround = true;
    mGrabExactPoses = false;
    mDebug = false;
    mAlgorithm = planeseg::BlockFitter::RectangleFitAlgorithm::MinimumArea;
    mBlockSize << 15+3/8.0, 15+5/8.0, 5+5/8.0;
    mBlockSize *=0.0254;
    mDoTrigger = false;
    mNamePrefix = "cinderblock";
    mTriggered = true;
  }

  void start() {
    mLastDataTime = 0;
    mData.utime = 0;
    mLcmWrapper->get()->subscribe("MAP_SCANS", &State::onScans, this);
    mWorkerThread = std::thread(std::ref(*this));
    mLcmWrapper->startHandleThread(true);
  }

  void stop() {
    mLcmWrapper->stopHandleThread();
    if (mWorkerThread.joinable()) mWorkerThread.join();
  }

  void operator()() {
    while (true) {
      // wait for data
      std::unique_lock<std::mutex> lock(mProcessMutex);
      mCondition.wait_for(lock, std::chrono::milliseconds(100));

      // grab data
      drc::map_scans_t data;
      Eigen::Isometry3f sensorPose;
      Eigen::Isometry3f groundPose;
      {
        std::unique_lock<std::mutex> dataLock(mDataMutex);
        if (mData.utime == mLastDataTime) continue;
        data = mData;
        sensorPose = mSensorPose;
        groundPose = mGroundPose;
        mLastDataTime = mData.utime;
      }

      // convert scans to point cloud
      maps::ScanBundleView view;
      maps::LcmTranslator::fromLcm(data, view);
      maps::PointCloud rawCloud, curCloud;
      std::vector<float> allDeltas;
      for (const auto& scan : view.getScans()) {

        // compute range deltas
        int numRanges = scan->getNumRanges();
        std::vector<float> deltas;
        const auto& ranges = scan->getRanges();
        float prevRange = -1;
        int curIndex = 0;
        for (int i = 0; i < numRanges; ++i, ++curIndex) {
          if (ranges[i] <= 0) continue;
          prevRange = ranges[i];
          deltas.push_back(0);
          break;
        }
        for (int i = curIndex+1; i < numRanges; ++i) {
          float range = ranges[i];
          if (range <= 0) continue;
          deltas.push_back(range-prevRange);
          prevRange = range;
        }

        // add this scan to cloud
        scan->get(curCloud, true);
        rawCloud += curCloud;
        allDeltas.insert(allDeltas.end(), deltas.begin(), deltas.end());
      }
      pcl::transformPointCloud
        (rawCloud, rawCloud,
         Eigen::Affine3f(view.getTransform().matrix()).inverse());
      planeseg::LabeledCloud::Ptr cloud(new planeseg::LabeledCloud());
      pcl::copyPointCloud(rawCloud, *cloud);
      /* TODO: change point type
      for (int i = 0; i < (int)cloud->size(); ++i) {
        cloud->points[i].label = 1000*allDeltas[i];
      }
      */

      // remove points outside max radius
      const float kValidRadius = 5;  // meters; TODO: could make this a param
      const float kValidRadius2 = kValidRadius*kValidRadius;
      planeseg::LabeledCloud::Ptr tempCloud(new planeseg::LabeledCloud());
      for (int i = 0; i < (int)cloud->size(); ++i) {
        Eigen::Vector3f p = cloud->points[i].getVector3fMap();
        float dist2 = (p-sensorPose.translation()).squaredNorm();
        if (dist2 > kValidRadius2) continue;
        tempCloud->push_back(cloud->points[i]);
      }
      std::swap(cloud, tempCloud);

      // process
      planeseg::BlockFitter fitter;
      fitter.setSensorPose(sensorPose.translation(),
                           sensorPose.rotation().col(2));
      fitter.setGroundBand(groundPose.translation()[2]-1.0,
                           groundPose.translation()[2]+0.5);
      if (mDoFilter) fitter.setAreaThresholds(0.8, 1.2);
      else fitter.setAreaThresholds(0, 1000);
      fitter.setBlockDimensions(mBlockSize);
      fitter.setRemoveGround(mRemoveGround);
      fitter.setRectangleFitAlgorithm
        ((planeseg::BlockFitter::RectangleFitAlgorithm)mAlgorithm);
      fitter.setDebug(mDebug);
      fitter.setCloud(cloud);
      auto result = fitter.go();
      if (!result.mSuccess) {
        std::cout << "error: could not detect blocks" << std::endl;
        continue;
      }

      //
      // construct json string
      //

      // header
      std::string json;
      json += "{\n";
      json += "  \"command\": \"echo_response\",\n";
      json += "  \"descriptions\": {\n";
      std::string timeString = std::to_string(mBotWrapper->getCurrentTime());

      // blocks
      for (int i = 0; i < (int)result.mBlocks.size(); ++i) {
        const auto& block = result.mBlocks[i];
        std::string dimensionsString, positionString, quaternionString;
        {
          std::ostringstream oss;
          Eigen::Vector3f size = block.mSize;
          oss << size[0] << ", " << size[1] << ", " << size[2];
          dimensionsString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Vector3f p = block.mPose.translation();
          oss << p[0] << ", " << p[1] << ", " << p[2];
          positionString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Quaternionf q(block.mPose.rotation());
          oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z();
          quaternionString = oss.str();
        }
        std::string uuid = timeString + "_" + std::to_string(i+1);

        json += "    \"" + uuid + "\": {\n";
        json += "      \"classname\": \"BoxAffordanceItem\",\n";
        json += "      \"pose\": [[" + positionString + "], [" +
          quaternionString + "]],\n";
        json += "      \"uuid\": \"" + uuid + "\",\n";
        json += "      \"Dimensions\": [" + dimensionsString + "],\n";
        json += "      \"Name\": \"" + mNamePrefix + " " +
          std::to_string(i) + "\"\n";
        json += "    },\n";
      }

      // ground
      {
        std::string positionString, quaternionString;
        Eigen::Vector3f groundNormal = result.mGroundPlane.head<3>();
        {
          std::ostringstream oss;
          Eigen::Vector3f p = groundPose.translation();
          p -= (groundNormal.dot(p)+result.mGroundPlane[3])*groundNormal;
          p -= 0.005*groundNormal;  // account for thickness of slab
          oss << p[0] << ", " << p[1] << ", " << p[2];
          positionString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Matrix3f rot = Eigen::Matrix3f::Identity();
          rot.col(2) = groundNormal.normalized();
          rot.col(1) = rot.col(2).cross(Eigen::Vector3f::UnitX()).normalized();
          rot.col(0) = rot.col(1).cross(rot.col(2)).normalized();
          Eigen::Quaternionf q(rot);
          oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z();
          quaternionString = oss.str();
        }

        json += "    \"ground affordance\": {\n";
        json += "      \"classname\": \"BoxAffordanceItem\",\n";
        json += "      \"pose\": [[" + positionString + "], [" +
          quaternionString + "]],\n";
        json += "      \"uuid\": \"ground affordance\",\n";
        json += "      \"Dimensions\": [100, 100, 0.01],\n";
        json += "      \"Name\": \"ground affordance\",\n";
        json += "      \"Visible\": 0\n";
        json += "    }\n";
      }

      // footer
      json += "  },\n";
      json += "  \"commandId\": \"" + timeString + "\",\n";
      json += "  \"collectionId\": \"block-fitter\"\n";
      json += "}\n";

      // publish result
      drc::affordance_collection_t msg;
      msg.utime = data.utime;
      msg.name = json;
      msg.naffs = 0;
      mLcmWrapper->get()->publish("AFFORDANCE_COLLECTION_COMMAND", &msg);
      std::cout << "Published affordance collection" << std::endl;

      // publish lcmgl
      if (mDebug) {
        bot_lcmgl_t* lcmgl;
        lcmgl = bot_lcmgl_init(mLcmWrapper->get()->getUnderlyingLCM(),
                              "block-fitter");
        for (const auto& block : result.mBlocks) {
          bot_lcmgl_color3f(lcmgl, 1, 0, 0);
          bot_lcmgl_line_width(lcmgl, 4);
          bot_lcmgl_begin(lcmgl, LCMGL_LINE_LOOP);
          for (const auto& pt : block.mHull) {
            bot_lcmgl_vertex3f(lcmgl, pt[0], pt[1], pt[2]);
          }
          bot_lcmgl_end(lcmgl);
        }

        bot_lcmgl_color3f(lcmgl, 0, 1, 0);
        bot_lcmgl_begin(lcmgl, LCMGL_LINE_LOOP);
        for (const auto& pt : result.mGroundPolygon) {
          bot_lcmgl_vertex3f(lcmgl, pt[0], pt[1], pt[2]);
        }
        bot_lcmgl_end(lcmgl);

        bot_lcmgl_switch_buffer(lcmgl);
        bot_lcmgl_destroy(lcmgl);
      }

      if (!mRunContinuously) break;
    }
    mLcmWrapper->stopHandleThread();
  }

  void onScans(const lcm::ReceiveBuffer* iBuf,
               const std::string& iChannel,
               const drc::map_scans_t* iMessage) {
    std::unique_lock<std::mutex> lock(mDataMutex);
    if (!mTriggered) return;
    mData = *iMessage;
    int64_t scanTime = iMessage->utime;
    int64_t headPoseTime = mBotWrapper->getLatestTime("head", "local");
    int64_t groundPoseTime = mBotWrapper->getLatestTime("ground", "local");
    if ((groundPoseTime == 0) || (headPoseTime == 0) ||
        (mGrabExactPoses && ((std::abs(headPoseTime-scanTime) > 1e6) ||
                             (std::abs(groundPoseTime-scanTime) > 1e6)))) {
      std::cout << "warning: got scans but no valid pose found" << std::endl;
      return;
    }
    mBotWrapper->getTransform("head", "local", mSensorPose, iMessage->utime);
    mBotWrapper->getTransform("ground", "local", mGroundPose, iMessage->utime);
    mCondition.notify_one();
    if (mDoTrigger) mTriggered = false;
  }

  void onTrigger(const lcm::ReceiveBuffer* iBuf, const std::string& iChannel,
                 const drc::block_fit_request_t* iMessage) {
    mNamePrefix = iMessage->name_prefix;
    mBlockSize << iMessage->dimensions[0], iMessage->dimensions[1],
      iMessage->dimensions[2];
    mAlgorithm = iMessage->algorithm;
    mLastDataTime = 0;  // force processing of new data
    mTriggered = true;
    std::cout << "received trigger" << std::endl;
  }
};

int main(const int iArgc, const char** iArgv) {

  std::string sizeString("");
  std::string triggerChannel;
  State state;

  ConciseArgs opt(iArgc, (char**)iArgv);
  opt.add(state.mRunContinuously, "c", "continuous", "run continuously");
  opt.add(state.mDoFilter, "f", "filter", "filter blocks based on size");
  opt.add(sizeString, "s", "blocksize", "prior size for blocks \"x y z\"");
  opt.add(state.mRemoveGround, "g", "remove-ground",
          "whether to remove ground before processing");
  opt.add(state.mAlgorithm, "a", "algorithm",
          "0=min_area, 1=closest_size, 2=closest_hull");
  opt.add(state.mGrabExactPoses, "p", "exact-poses",
          "wait for synchronized poses");
  opt.add(triggerChannel, "t", "trigger-channel",
          "perform block fit only when trigger is received");
  opt.add(state.mDebug, "d", "debug", "debug flag");
  opt.parse();

  if (sizeString.length() > 0) {
    std::istringstream iss(sizeString);
    float x, y, z;
    if (iss >> x) {
      if (iss >> y) {
        if (iss >> z) {
          state.mBlockSize << x,y,z;
          std::cout << "using block size " << state.mBlockSize.transpose() <<
            std::endl;
        }
      }
    }
  }

  state.mBotWrapper.reset(new drc::BotWrapper());
  state.mLcmWrapper.reset(new drc::LcmWrapper(state.mBotWrapper->getLcm()));

  if (triggerChannel.length() > 0) {
    state.mDoTrigger = true;
    state.mTriggered = false;
    state.mLcmWrapper->get()->subscribe(triggerChannel,
                                        &State::onTrigger, &state);
  }

  state.start();
  state.stop();

  return 1;
}
<commit_msg>named ground affordance dimensions<commit_after>#include <thread>
#include <condition_variable>
#include <mutex>
#include <sstream>

#include <lcm/lcm-cpp.hpp>
#include <ConciseArgs>

#include <drc_utils/LcmWrapper.hpp>
#include <drc_utils/BotWrapper.hpp>

#include <bot_lcmgl_client/lcmgl.h>

#include <lcmtypes/drc/map_scans_t.hpp>
#include <lcmtypes/drc/affordance_collection_t.hpp>
#include <lcmtypes/drc/block_fit_request_t.hpp>

#include <maps/ScanBundleView.hpp>
#include <maps/LcmTranslator.hpp>

#include <pcl/common/io.h>
#include <pcl/common/transforms.h>

#include "BlockFitter.hpp"

struct State {
  drc::BotWrapper::Ptr mBotWrapper;
  drc::LcmWrapper::Ptr mLcmWrapper;
  bool mRunContinuously;
  bool mDoFilter;
  bool mRemoveGround;
  bool mGrabExactPoses;
  bool mDebug;
  Eigen::Vector3f mBlockSize;
  int mAlgorithm;
  bool mDoTrigger;
  std::string mNamePrefix;
  bool mTriggered;

  drc::map_scans_t mData;
  int64_t mLastDataTime;
  Eigen::Isometry3f mSensorPose;
  Eigen::Isometry3f mGroundPose;

  std::thread mWorkerThread;
  std::condition_variable mCondition;
  std::mutex mProcessMutex;
  std::mutex mDataMutex;

  State() {
    mRunContinuously = false;
    mDoFilter = true;
    mRemoveGround = true;
    mGrabExactPoses = false;
    mDebug = false;
    mAlgorithm = planeseg::BlockFitter::RectangleFitAlgorithm::MinimumArea;
    mBlockSize << 15+3/8.0, 15+5/8.0, 5+5/8.0;
    mBlockSize *=0.0254;
    mDoTrigger = false;
    mNamePrefix = "cinderblock";
    mTriggered = true;
  }

  void start() {
    mLastDataTime = 0;
    mData.utime = 0;
    mLcmWrapper->get()->subscribe("MAP_SCANS", &State::onScans, this);
    mWorkerThread = std::thread(std::ref(*this));
    mLcmWrapper->startHandleThread(true);
  }

  void stop() {
    mLcmWrapper->stopHandleThread();
    if (mWorkerThread.joinable()) mWorkerThread.join();
  }

  void operator()() {
    while (true) {
      // wait for data
      std::unique_lock<std::mutex> lock(mProcessMutex);
      mCondition.wait_for(lock, std::chrono::milliseconds(100));

      // grab data
      drc::map_scans_t data;
      Eigen::Isometry3f sensorPose;
      Eigen::Isometry3f groundPose;
      {
        std::unique_lock<std::mutex> dataLock(mDataMutex);
        if (mData.utime == mLastDataTime) continue;
        data = mData;
        sensorPose = mSensorPose;
        groundPose = mGroundPose;
        mLastDataTime = mData.utime;
      }

      // convert scans to point cloud
      maps::ScanBundleView view;
      maps::LcmTranslator::fromLcm(data, view);
      maps::PointCloud rawCloud, curCloud;
      std::vector<float> allDeltas;
      for (const auto& scan : view.getScans()) {

        // compute range deltas
        int numRanges = scan->getNumRanges();
        std::vector<float> deltas;
        const auto& ranges = scan->getRanges();
        float prevRange = -1;
        int curIndex = 0;
        for (int i = 0; i < numRanges; ++i, ++curIndex) {
          if (ranges[i] <= 0) continue;
          prevRange = ranges[i];
          deltas.push_back(0);
          break;
        }
        for (int i = curIndex+1; i < numRanges; ++i) {
          float range = ranges[i];
          if (range <= 0) continue;
          deltas.push_back(range-prevRange);
          prevRange = range;
        }

        // add this scan to cloud
        scan->get(curCloud, true);
        rawCloud += curCloud;
        allDeltas.insert(allDeltas.end(), deltas.begin(), deltas.end());
      }
      pcl::transformPointCloud
        (rawCloud, rawCloud,
         Eigen::Affine3f(view.getTransform().matrix()).inverse());
      planeseg::LabeledCloud::Ptr cloud(new planeseg::LabeledCloud());
      pcl::copyPointCloud(rawCloud, *cloud);
      /* TODO: change point type
      for (int i = 0; i < (int)cloud->size(); ++i) {
        cloud->points[i].label = 1000*allDeltas[i];
      }
      */

      // remove points outside max radius
      const float kValidRadius = 5;  // meters; TODO: could make this a param
      const float kValidRadius2 = kValidRadius*kValidRadius;
      planeseg::LabeledCloud::Ptr tempCloud(new planeseg::LabeledCloud());
      for (int i = 0; i < (int)cloud->size(); ++i) {
        Eigen::Vector3f p = cloud->points[i].getVector3fMap();
        float dist2 = (p-sensorPose.translation()).squaredNorm();
        if (dist2 > kValidRadius2) continue;
        tempCloud->push_back(cloud->points[i]);
      }
      std::swap(cloud, tempCloud);

      // process
      planeseg::BlockFitter fitter;
      fitter.setSensorPose(sensorPose.translation(),
                           sensorPose.rotation().col(2));
      fitter.setGroundBand(groundPose.translation()[2]-1.0,
                           groundPose.translation()[2]+0.5);
      if (mDoFilter) fitter.setAreaThresholds(0.8, 1.2);
      else fitter.setAreaThresholds(0, 1000);
      fitter.setBlockDimensions(mBlockSize);
      fitter.setRemoveGround(mRemoveGround);
      fitter.setRectangleFitAlgorithm
        ((planeseg::BlockFitter::RectangleFitAlgorithm)mAlgorithm);
      fitter.setDebug(mDebug);
      fitter.setCloud(cloud);
      auto result = fitter.go();
      if (!result.mSuccess) {
        std::cout << "error: could not detect blocks" << std::endl;
        continue;
      }

      //
      // construct json string
      //

      // header
      std::string json;
      json += "{\n";
      json += "  \"command\": \"echo_response\",\n";
      json += "  \"descriptions\": {\n";
      std::string timeString = std::to_string(mBotWrapper->getCurrentTime());

      // blocks
      for (int i = 0; i < (int)result.mBlocks.size(); ++i) {
        const auto& block = result.mBlocks[i];
        std::string dimensionsString, positionString, quaternionString;
        {
          std::ostringstream oss;
          Eigen::Vector3f size = block.mSize;
          oss << size[0] << ", " << size[1] << ", " << size[2];
          dimensionsString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Vector3f p = block.mPose.translation();
          oss << p[0] << ", " << p[1] << ", " << p[2];
          positionString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Quaternionf q(block.mPose.rotation());
          oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z();
          quaternionString = oss.str();
        }
        std::string uuid = timeString + "_" + std::to_string(i+1);

        json += "    \"" + uuid + "\": {\n";
        json += "      \"classname\": \"BoxAffordanceItem\",\n";
        json += "      \"pose\": [[" + positionString + "], [" +
          quaternionString + "]],\n";
        json += "      \"uuid\": \"" + uuid + "\",\n";
        json += "      \"Dimensions\": [" + dimensionsString + "],\n";
        json += "      \"Name\": \"" + mNamePrefix + " " +
          std::to_string(i) + "\"\n";
        json += "    },\n";
      }

      // ground
      {
        std::string positionString, quaternionString, dimensionsString;
        Eigen::Vector3f groundNormal = result.mGroundPlane.head<3>();
        Eigen::Vector3f groundSize(100,100,0.01);
        {
          std::ostringstream oss;
          Eigen::Vector3f p = groundPose.translation();
          p -= (groundNormal.dot(p)+result.mGroundPlane[3])*groundNormal;
          p -= (groundSize[2]/2)*groundNormal;
          oss << p[0] << ", " << p[1] << ", " << p[2];
          positionString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Matrix3f rot = Eigen::Matrix3f::Identity();
          rot.col(2) = groundNormal.normalized();
          rot.col(1) = rot.col(2).cross(Eigen::Vector3f::UnitX()).normalized();
          rot.col(0) = rot.col(1).cross(rot.col(2)).normalized();
          Eigen::Quaternionf q(rot);
          oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z();
          quaternionString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Vector3f size = groundSize;
          oss << size[0] << ", " << size[1] << ", " << size[2];
          dimensionsString = oss.str();
        }

        json += "    \"ground affordance\": {\n";
        json += "      \"classname\": \"BoxAffordanceItem\",\n";
        json += "      \"pose\": [[" + positionString + "], [" +
          quaternionString + "]],\n";
        json += "      \"uuid\": \"ground affordance\",\n";
        json += "      \"Dimensions\": [" + dimensionsString+ "],\n";
        json += "      \"Name\": \"ground affordance\",\n";
        json += "      \"Visible\": 0\n";
        json += "    }\n";
      }

      // footer
      json += "  },\n";
      json += "  \"commandId\": \"" + timeString + "\",\n";
      json += "  \"collectionId\": \"block-fitter\"\n";
      json += "}\n";

      // publish result
      drc::affordance_collection_t msg;
      msg.utime = data.utime;
      msg.name = json;
      msg.naffs = 0;
      mLcmWrapper->get()->publish("AFFORDANCE_COLLECTION_COMMAND", &msg);
      std::cout << "Published affordance collection" << std::endl;

      // publish lcmgl
      if (mDebug) {
        bot_lcmgl_t* lcmgl;
        lcmgl = bot_lcmgl_init(mLcmWrapper->get()->getUnderlyingLCM(),
                              "block-fitter");
        for (const auto& block : result.mBlocks) {
          bot_lcmgl_color3f(lcmgl, 1, 0, 0);
          bot_lcmgl_line_width(lcmgl, 4);
          bot_lcmgl_begin(lcmgl, LCMGL_LINE_LOOP);
          for (const auto& pt : block.mHull) {
            bot_lcmgl_vertex3f(lcmgl, pt[0], pt[1], pt[2]);
          }
          bot_lcmgl_end(lcmgl);
        }

        bot_lcmgl_color3f(lcmgl, 0, 1, 0);
        bot_lcmgl_begin(lcmgl, LCMGL_LINE_LOOP);
        for (const auto& pt : result.mGroundPolygon) {
          bot_lcmgl_vertex3f(lcmgl, pt[0], pt[1], pt[2]);
        }
        bot_lcmgl_end(lcmgl);

        bot_lcmgl_switch_buffer(lcmgl);
        bot_lcmgl_destroy(lcmgl);
      }

      if (!mRunContinuously) break;
    }
    mLcmWrapper->stopHandleThread();
  }

  void onScans(const lcm::ReceiveBuffer* iBuf,
               const std::string& iChannel,
               const drc::map_scans_t* iMessage) {
    std::unique_lock<std::mutex> lock(mDataMutex);
    if (!mTriggered) return;
    mData = *iMessage;
    int64_t scanTime = iMessage->utime;
    int64_t headPoseTime = mBotWrapper->getLatestTime("head", "local");
    int64_t groundPoseTime = mBotWrapper->getLatestTime("ground", "local");
    if ((groundPoseTime == 0) || (headPoseTime == 0) ||
        (mGrabExactPoses && ((std::abs(headPoseTime-scanTime) > 1e6) ||
                             (std::abs(groundPoseTime-scanTime) > 1e6)))) {
      std::cout << "warning: got scans but no valid pose found" << std::endl;
      return;
    }
    mBotWrapper->getTransform("head", "local", mSensorPose, iMessage->utime);
    mBotWrapper->getTransform("ground", "local", mGroundPose, iMessage->utime);
    mCondition.notify_one();
    if (mDoTrigger) mTriggered = false;
  }

  void onTrigger(const lcm::ReceiveBuffer* iBuf, const std::string& iChannel,
                 const drc::block_fit_request_t* iMessage) {
    mNamePrefix = iMessage->name_prefix;
    mBlockSize << iMessage->dimensions[0], iMessage->dimensions[1],
      iMessage->dimensions[2];
    mAlgorithm = iMessage->algorithm;
    mLastDataTime = 0;  // force processing of new data
    mTriggered = true;
    std::cout << "received trigger" << std::endl;
  }
};

int main(const int iArgc, const char** iArgv) {

  std::string sizeString("");
  std::string triggerChannel;
  State state;

  ConciseArgs opt(iArgc, (char**)iArgv);
  opt.add(state.mRunContinuously, "c", "continuous", "run continuously");
  opt.add(state.mDoFilter, "f", "filter", "filter blocks based on size");
  opt.add(sizeString, "s", "blocksize", "prior size for blocks \"x y z\"");
  opt.add(state.mRemoveGround, "g", "remove-ground",
          "whether to remove ground before processing");
  opt.add(state.mAlgorithm, "a", "algorithm",
          "0=min_area, 1=closest_size, 2=closest_hull");
  opt.add(state.mGrabExactPoses, "p", "exact-poses",
          "wait for synchronized poses");
  opt.add(triggerChannel, "t", "trigger-channel",
          "perform block fit only when trigger is received");
  opt.add(state.mDebug, "d", "debug", "debug flag");
  opt.parse();

  if (sizeString.length() > 0) {
    std::istringstream iss(sizeString);
    float x, y, z;
    if (iss >> x) {
      if (iss >> y) {
        if (iss >> z) {
          state.mBlockSize << x,y,z;
          std::cout << "using block size " << state.mBlockSize.transpose() <<
            std::endl;
        }
      }
    }
  }

  state.mBotWrapper.reset(new drc::BotWrapper());
  state.mLcmWrapper.reset(new drc::LcmWrapper(state.mBotWrapper->getLcm()));

  if (triggerChannel.length() > 0) {
    state.mDoTrigger = true;
    state.mTriggered = false;
    state.mLcmWrapper->get()->subscribe(triggerChannel,
                                        &State::onTrigger, &state);
  }

  state.start();
  state.stop();

  return 1;
}
<|endoftext|>


<commit_before>#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>
#include <sys/stat.h>
#include <unistd.h>
#include <thread>
#include <mutex>
#include <cmath>
#include "concurrentqueue.h"

using namespace std;
using namespace cv;
using namespace moodycamel;

class Utils {
	private:
		static void extractHistogram(Mat frame, int num, vector< pair<int, Mat> > &hTemp);
		static std::mutex mutex;

	public:
		static void writeOutputFile(string outFile, vector< pair<int,int> > shots);
		static bool checkFile(string name);
		static bool checkOutputFile(string name);
		static vector<Mat> extractVideoHistograms(string videoPath);
		static vector<Mat> extractVideoHistograms2(string videoPath);
		static bool pairCompare(const pair<int, Mat> &fElem, const pair<int, Mat> &sElem);
};<commit_msg>Some garbage got though the last commit<commit_after>#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>
#include <sys/stat.h>
#include <unistd.h>
#include <thread>
#include <mutex>
#include <cmath>

using namespace std;
using namespace cv;

class Utils {
	private:
		static void extractHistogram(Mat frame, int num, vector< pair<int, Mat> > &hTemp);
		static std::mutex mutex;

	public:
		static void writeOutputFile(string outFile, vector< pair<int,int> > shots);
		static bool checkFile(string name);
		static bool checkOutputFile(string name);
		static vector<Mat> extractVideoHistograms(string videoPath);
		static bool pairCompare(const pair<int, Mat> &fElem, const pair<int, Mat> &sElem);
};<|endoftext|>


<commit_before>#ifndef UTILS_HPP
#define UTILS_HPP

#include <type_traits>
#include <utility>

////////////////////////// Tuple unpack utils
////////////////////////// http://stackoverflow.com/questions/7858817/unpacking-a-tuple-to-call-a-matching-function-pointer

namespace util
{
	// helper class
	template <
		typename F,
		typename... Args,
		std::size_t... I
	>
		auto call_helper(const F& func, const std::tuple<Args...> & params) -> std::result_of_t<F(Args...)>
	{
		return func(std::get<I>(params)...);
	}

	template<
		typename F,
		typename... Args
	>
		auto call(const F& func, const std::tuple<Args...> & params) -> std::result_of_t<F(Args...)>
	{
		return call_helper<F, Args..., std::index_sequence_for<Args...> >(func, params);
	}
}

////////////////////////// Bitflags enumerations
template <typename T> struct is_enum_flags : std::false_type
{
	//static_assert(std::is_enum<T>::value, "T must be an enum type");
};

////////////////////////// operator|
template <
	typename T,
	typename = std::enable_if_t<is_enum_flags<T>::value>
>
inline T operator|(T a, T b)
{
	return static_cast<T>(static_cast<std::underlying_type_t<T>>(a) | static_cast<std::underlying_type_t<T>>(b));
}

////////////////////////// operator|=
template <
	typename T,
	typename = std::enable_if_t<is_enum_flags<T>::value>
>
inline T& operator|=(T& a, T b)
{
	a = a | b;
	return a;
}

////////////////////////// operator&=
template <
	typename T,
	typename = std::enable_if_t<is_enum_flags<T>::value>
>
inline T operator&(T a, T b)
{
	return static_cast<T>(static_cast<std::underlying_type_t<T>>(a) & static_cast<std::underlying_type_t<T>>(b));
}

#endif // !UTILS_HPP
<commit_msg>things<commit_after>#ifndef UTILS_HPP
#define UTILS_HPP

#include <type_traits>
#include <utility>

////////////////////////// Tuple unpack utils
////////////////////////// http://stackoverflow.com/questions/7858817/unpacking-a-tuple-to-call-a-matching-function-pointer

namespace util
{
	// helper class
	template <
		typename F,
		typename... Args,
		std::size_t... I
	>
	auto call_helper(const F& func, const std::tuple<Args...> & params) -> std::result_of_t<F(Args...)>
	{
		return func(std::get<I>(params)...);
	}

	template<
		typename F,
		typename... Args
	>
	auto call(const F& func, const std::tuple<Args...> & params) -> std::result_of_t<F(Args...)>
	{
		return call_helper<F, Args..., std::index_sequence_for<Args...> >(func, params);
	}
}

////////////////////////// Bitflags enumerations
template <typename T> struct is_enum_flags : std::false_type
{
	//static_assert(std::is_enum<T>::value, "T must be an enum type");
};

////////////////////////// operator|
template <
	typename T,
	typename = std::enable_if_t<is_enum_flags<T>::value>
>
inline T operator|(T a, T b)
{
	return static_cast<T>(static_cast<std::underlying_type_t<T>>(a) | static_cast<std::underlying_type_t<T>>(b));
}

////////////////////////// operator|=
template <
	typename T,
	typename = std::enable_if_t<is_enum_flags<T>::value>
>
inline T& operator|=(T& a, T b)
{
	a = a | b;
	return a;
}

////////////////////////// operator&=
template <
	typename T,
	typename = std::enable_if_t<is_enum_flags<T>::value>
>
inline T operator&(T a, T b)
{
	return static_cast<T>(static_cast<std::underlying_type_t<T>>(a) & static_cast<std::underlying_type_t<T>>(b));
}

#endif // !UTILS_HPP
<|endoftext|>


<commit_before>// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: main.C,v 1.5 2004/07/16 11:18:18 amoll Exp $
//

// order of includes is important: first qapplication, than BALL includes
#include <qapplication.h>

#include <BALL/SYSTEM/path.h>
#include <BALL/SYSTEM/directory.h>
#include "mainframe.h"

#include <BALL/FORMAT/DCDFile.h>
#include <BALL/MOLMEC/COMMON/snapShot.h>
#include <BALL/VIEW/RENDERING/POVRenderer.h>

#include <iostream>
#include <sys/wait.h>

using namespace BALL;
using namespace BALL::VIEW;

void showUsage()
{
 	Log.insert(std::cerr);
	Log.error() << "DCD2PNG <PDBFILE.pdb | HINFILE.hin> <DCDFILE.dcd> [<BALLViewProject.bvp>] [<DIRECTORY>]" << std::endl;
	Log.error() << "Read a molecular file, and create PNG images from them in the given directory." << std::endl;
 	Log.remove(std::cerr);
}

int main(int argc, char **argv)
{
	if (argc < 3)
	{
		showUsage();
		return 1;
	}

	// =============== testing if we can write in current directoy =====================
	bool dir_error = false;
	char* home_dir = 0;
	try
	{
		BALL::String temp_file_name;
		BALL::File::createTemporaryFilename(temp_file_name);
		BALL::File out(temp_file_name, std::ios::out);
		out << "test" << std::endl;
		out.remove();
	}
	catch(...)
	{
		// oh, we have a problem, look for the users home dir
		dir_error = true;

		// default for UNIX/LINUX
		home_dir = getenv("HOME");
		if (home_dir == 0)
		{
			// windows
			home_dir = getenv("HOMEPATH");
		}

		// changedir to the homedir
		if (home_dir != 0)
		{
			BALL::Directory dir(home_dir);
			dir_error = !dir.setCurrent();
		}

		if (dir_error)
		{
			Log.error() <<
					"You dont have write access to the current working directory\n"<<
					"and I can not find your home directory. This can cause\n" <<
					"unexpected behaviour. Please start DCD2PNG from your homedir with\n" <<
					"absolute path.\n";
		}
	}

	// =============== initialize Mainframe ============================================
	QApplication application(argc, argv);
	BALL::Mainframe mainframe;
//  	application.setMainWidget(&mainframe);
	mainframe.setIdentifier("MAIN");
	mainframe.registerThis();

 	mainframe.hide();

	// if we need to use the users homedir as working dir, do so
	if (home_dir != 0 && !dir_error)
	{
		mainframe.setWorkingDir(home_dir);
	}

	// =============== parsing command line arguments ==================================
	String dcd_file_name;
	String molecular_file_name;
	String working_dir = ".";
	bool error = false;
	System* system;
	Position nr = 100000000;

	DisplayProperties::getInstance(0)->enableCreationForNewMolecules(false);

	for (BALL::Index i = 1; i < argc && !error; ++i)
	{
		BALL::String argument(argv[i]);
		if (argument.hasSuffix(".bvp"))
		{
			DisplayProperties::getInstance(0)->enableCreationForNewMolecules(true);
			mainframe.loadBALLViewProjectFile(argument);
			continue;
		}

		if (argument.hasSuffix(".pdb") ||
		    argument.hasSuffix(".hin"))
		{
			MolecularFileDialog* mfd = MolecularFileDialog::getInstance(0);
			if (mfd == 0) return 0;
			system = mfd->openFile(argument);
			molecular_file_name = argument;
			continue;
		}

		if (argument.hasSuffix(".dcd"))
		{
			dcd_file_name = argument;
			continue;
		}

		if (argument.hasPrefix("-s"))
		{
			argument.after("-s");
			try
			{
				nr = argument.toUnsignedInt();
			}
			catch(...)
			{
			}
			continue;
		}

		Directory d(argument);
		if (d.isValid())
		{
			mainframe.setWorkingDir(argument);
			working_dir = argument;
			continue;
		}

		error = true;
	}

	if (molecular_file_name == "" ||
			dcd_file_name == "" ||
			error)
	{
		showUsage();
		application.quit();
		return 0;
	}

	if (dcd_file_name == "")
	{
		DisplayProperties::getInstance(0)->enableCreationForNewMolecules(true);
 		DisplayProperties::getInstance(0)->applyButtonClicked();
	}

	Size width = 1000;
	Size height = 750;

	DCDFile dcdfile(dcd_file_name);
	Scene::getInstance(0)->resize(width, height);

	int writepipe[2];
	pid_t childpid;
#define PARENT_WRITE 	writepipe[0]
#define CHILD_READ 		writepipe[1]
	int status;
// 	if (pipe(writepipe) < 0 ) return -1;

	String povray_options;
	povray_options = "-V -D +Imytemp +W" + String(width) + " +H" + String(height) + " +O" + working_dir + "/";

	std::cout << povray_options << std::endl;

	SnapShotManager sm(system, 0, &dcdfile, false);
	POVRenderer pov;
	pov.setFileName("mytemp");
	Position nr2 = 0;
	sm.applyFirstSnapShot();
	while(sm.applyNextSnapShot())
	{
		String pov_arg = povray_options + String(nr) + ".png" ;
 		Scene::getInstance(0)->exportScene(pov);
		nr++;
		nr2++;

		// abort, if we can not fork!
		if ( (childpid = fork()) < 0)
		{
			std::cout << "Could not fork!" << std::endl;
			return -1;
		}
		// we are in the parent process and wait for the child to finish
		else if (childpid != 0)
		{
			waitpid( -1, &status, 0 );
		}
		// we are in the child process and start povray
		else
		{
//		  	close(1);
			execl ("/home/student/amoll/bin/povray", "povray", pov_arg.c_str(), 0);
		}

//		 	close(1);
//  			dup (writepipe[1]);
// 			execl ("/home/student/amoll/bin/mytest", "mytest", "+I- +Oa.png", 0);

	}

	std::cout << "Written " + String(nr2) + " images." << std::endl;

// 	mainframe.show();
//   return application.exec();
	return 0;
}

<commit_msg>no message<commit_after>// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: main.C,v 1.6 2004/07/16 13:55:32 amoll Exp $
//

// order of includes is important: first qapplication, than BALL includes
#include <qapplication.h>

#include <BALL/SYSTEM/path.h>
#include <BALL/SYSTEM/directory.h>
#include "mainframe.h"

#include <BALL/FORMAT/DCDFile.h>
#include <BALL/MOLMEC/COMMON/snapShot.h>
#include <BALL/VIEW/RENDERING/POVRenderer.h>

#include <iostream>
#include <sys/wait.h>

using namespace BALL;
using namespace BALL::VIEW;

void showUsage()
{
 	Log.insert(std::cerr);
	Log.error() << "DCD2PNG <PDBFILE.pdb | HINFILE.hin> <DCDFILE.dcd> [<BALLViewProject.bvp>] [<DIRECTORY>]" << std::endl;
	Log.error() << "Read a molecular file, and create PNG images from them in the given directory." << std::endl;
 	Log.remove(std::cerr);
}

int main(int argc, char **argv)
{
	if (argc < 3)
	{
		showUsage();
		return 1;
	}

	// =============== testing if we can write in current directoy =====================
	bool dir_error = false;
	char* home_dir = 0;
	try
	{
		BALL::String temp_file_name;
		BALL::File::createTemporaryFilename(temp_file_name);
		BALL::File out(temp_file_name, std::ios::out);
		out << "test" << std::endl;
		out.remove();
	}
	catch(...)
	{
		// oh, we have a problem, look for the users home dir
		dir_error = true;

		// default for UNIX/LINUX
		home_dir = getenv("HOME");
		if (home_dir == 0)
		{
			// windows
			home_dir = getenv("HOMEPATH");
		}

		// changedir to the homedir
		if (home_dir != 0)
		{
			BALL::Directory dir(home_dir);
			dir_error = !dir.setCurrent();
		}

		if (dir_error)
		{
			Log.error() <<
					"You dont have write access to the current working directory\n"<<
					"and I can not find your home directory. This can cause\n" <<
					"unexpected behaviour. Please start DCD2PNG from your homedir with\n" <<
					"absolute path.\n";
		}
	}

	// =============== initialize Mainframe ============================================
	QApplication application(argc, argv);
	BALL::Mainframe mainframe;
//  	application.setMainWidget(&mainframe);
	mainframe.setIdentifier("MAIN");
	mainframe.registerThis();

 	mainframe.hide();

	// if we need to use the users homedir as working dir, do so
	if (home_dir != 0 && !dir_error)
	{
		mainframe.setWorkingDir(home_dir);
	}

	// =============== parsing command line arguments ==================================
	String dcd_file_name;
	String molecular_file_name;
	String working_dir = ".";
	bool error = false;
	System* system;
	Position nr = 100000000;

	DisplayProperties::getInstance(0)->enableCreationForNewMolecules(false);

	for (BALL::Index i = 1; i < argc && !error; ++i)
	{
		BALL::String argument(argv[i]);
		if (argument.hasSuffix(".bvp"))
		{
			DisplayProperties::getInstance(0)->enableCreationForNewMolecules(true);
			mainframe.loadBALLViewProjectFile(argument);
			continue;
		}

		if (argument.hasSuffix(".pdb") ||
		    argument.hasSuffix(".hin"))
		{
			MolecularFileDialog* mfd = MolecularFileDialog::getInstance(0);
			if (mfd == 0) return 0;
			system = mfd->openFile(argument);
			molecular_file_name = argument;
			continue;
		}

		if (argument.hasSuffix(".dcd"))
		{
			dcd_file_name = argument;
			continue;
		}

		if (argument.hasPrefix("-s"))
		{
			argument.after("-s");
			try
			{
				nr = argument.toUnsignedInt();
			}
			catch(...)
			{
			}
			continue;
		}

		Directory d(argument);
		if (d.isValid())
		{
			mainframe.setWorkingDir(argument);
			working_dir = argument;
			continue;
		}

		error = true;
	}

	if (molecular_file_name == "" ||
			dcd_file_name == "" ||
			error)
	{
		showUsage();
		application.quit();
		return 0;
	}

	if (dcd_file_name == "")
	{
		DisplayProperties::getInstance(0)->enableCreationForNewMolecules(true);
 		DisplayProperties::getInstance(0)->applyButtonClicked();
	}

	Size width = 1000;
	Size height = 750;

	DCDFile dcdfile(dcd_file_name);
	Scene::getInstance(0)->resize(width, height);

	int writepipe[2];
	pid_t childpid;
 	if (pipe(writepipe) < 0 )
	{
		std::cerr << "Could not pipe!" << std::endl;
		return -1;
	}

	String povray_options;
	povray_options = "-V -D +I- +W" + String(width) + " +H" + String(height) + " +O" + working_dir + "/";

	SnapShotManager sm(system, 0, &dcdfile, false);
	POVRenderer pov;
	std::stringstream pov_renderer_output;
	pov.setOstream(pov_renderer_output);
	pov.setHumanReadable(false);
	Position nr2 = 0;
	sm.applyFirstSnapShot();

	while(sm.applyNextSnapShot())
	{
		String pov_arg = povray_options + String(nr) + ".png" ;
 		Scene::getInstance(0)->exportScene(pov);

		// abort, if we can not fork!
		if ( (childpid = fork()) < 0)
		{
			std::cerr << "Could not fork!" << std::endl;
			return -1;
		}
		else if (childpid != 0)
		{
			// we are in the parent process and wait for the child to finish

			// Parent closes it's read end of the pipe
 			close (writepipe[0]);

			sleep(1);

			while (pov_renderer_output.good())
			{
				char buffer[1000];
				pov_renderer_output.getline(buffer, 1000);

				Size n = strlen(buffer);
				if(write(writepipe[1], buffer, n) != (int) n)
				{
					std::cerr << "Could not write to pipe!" << std::endl;
					return -1;
				}

				write(writepipe[1], "\n", 1);
			}

			// Parent closes it's write end of the pipe, this sends EOF to reader
			close (writepipe[1]);

			int status;
			waitpid( -1, &status, 0 );
		}
		else
		{
			// we are in the child process and start povray

			// Child closes it's write end of the pipe
		 	close (writepipe[1]);

			// handle stdin already closed
			if(writepipe[0] != STDIN_FILENO)
			{
				if(dup2(writepipe[0], STDIN_FILENO) < 0)
				{
					std::cout << "Could not dup2!" << std::endl;
					return -1;
				}

				close(writepipe[0]);
			}

	  	execl ("/home/student/amoll/bin/povray", "povray", pov_arg.c_str(), 0);
		}

		nr++;
		nr2++;
	}

	std::cout << "Written " + String(nr2) + " images." << std::endl;

// 	mainframe.show();
//   return application.exec();
	return 0;
}

<|endoftext|>