text:
// Copyright @2012 JDSU Xtreme

#include "xtreme/common/curlwrapper/filecopy_client.h"

#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdio.h>

#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>

#include "xtreme/common/utils.h"
#include "xtreme/common/xtreme_exception.h"
#include "xtreme/common/curlwrapper/utils.h"

namespace xtreme {
  namespace curlwrapper {

    // Both SFTP and SCP use SSH port 22 [by default]
    const int SSH_PORT_DEFAULT = 22;

    struct CpFile {
      CpFile (const std::string& _fname) : st_fileName(_fname), st_fp(nullptr), st_fileSize(-1) {
        open();
      }

      ~CpFile() {
        close();
      }

      std::string st_fileName;
      FILE        * st_fp;
      //struct      st_fileStat;
      off_t       st_fileSize;   // off_t is s signed integer value that can hold -1

    private:
      inline void open() {
        // Open in binary mode ... default transfer mode of libcurl-easy-interface, is 'binary'
        st_fp = fopen(st_fileName.c_str(), "rb");

        // int ret = stat(st_fileName, &st_fileStat);
        // st_fileSize = (0 == ret) ? st_fileStat.st_size : -1;

        // Determine size using fseeko/ftello ... since the file is already open
        // POSIX fseeko/ftello :- provide large file (> 4GB) support
        if (nullptr != st_fp) {
          int retSeek = fseeko(st_fp, 0, SEEK_END);
          if(0 == retSeek) {
            st_fileSize = ftello(st_fp);
            retSeek = fseeko(st_fp, 0, SEEK_SET);  // rewind (may be a large) file.
          }

          // If either seek or ftell failed, close file
          if ((0 != retSeek) || (st_fileSize < 0))
            close();
        }
      }

      inline void close() {
        if (nullptr != st_fp)
          fclose(st_fp);

          st_fp = nullptr;
          st_fileSize = -1;
          CURLWRAPPER_INFO("Closed Copy-File : " << st_fileName);
      }
    };

    // Used as libcurl 'Callback' ... CURLOPT_READFUNCTION
    static size_t ReadData( void *ptr, size_t size, size_t nmemb, void *userdata) {
      CURLWRAPPER_TRACE("Read Copy-File : CURLOPT_READFUNCTION called back...");
      struct CpFile * pInput = static_cast<struct CpFile *> (userdata);

      if (nullptr == pInput)
        return CURL_READFUNC_ABORT;

      if (nullptr == pInput->st_fp)
          return CURL_READFUNC_ABORT;  // Failed to open read-file earlier

      size_t ret = fread (ptr, size, nmemb, pInput->st_fp);
      if ((0 == ret) && ferror(pInput->st_fp)) {
				CURLWRAPPER_DEBUG("Read Copy-File : " << pInput->st_fileName << ": returning CURL_READFUNC_ABORT. [size=" << size << "], [nmemb=" << nmemb << "], [ret=" << ret << "]");
        return CURL_READFUNC_ABORT;
      }

      curl_off_t nread = size * ret;
      CURLWRAPPER_DEBUG("Read Copy-File : " << pInput->st_fileName << ": " << ret << " [size=" << size <<
      		"], [nmemb=" << nmemb << "], [nread=" << nread << "]");
      return (size_t)(nread);
    }

    CFileCopyClient::CFileCopyClient(EnFileCopyType _transferType) : m_transferType(_transferType), m_sshPort(SSH_PORT_DEFAULT) {
       init();
    }

    CFileCopyClient::~CFileCopyClient() {
      clenup();
    }

    void CFileCopyClient::init() {
      m_pCurl = curl_easy_init();
      if (NULL == m_pCurl) {
        CURLWRAPPER_FATAL("Copy-File : Failed to create CURL easy-handle !!!");
        throw xtreme::XtremeException("Copy-File : Failed to create CURL easy-handle");
      }
      // Set the callback-function to read data [to be sent to peer]
      curl_easy_setopt(m_pCurl, CURLOPT_READFUNCTION, ReadData);

      // Set the callback-function to write data [received from peer]
      // curl_easy_setopt(m_pCurl, CURLOPT_WRITEFUNCTION, WriteData);

	// create remote missing directories & retry
	CURLWRAPPER_DEBUG("transfertype="<<m_transferType);
	if (kSFTP == m_transferType) {
        curl_easy_setopt(m_pCurl, CURLOPT_FTP_CREATE_MISSING_DIRS, 1L);
    }
	}

    void CFileCopyClient::clenup() {
      curl_easy_cleanup(m_pCurl);
      CURLWRAPPER_TRACE("Copy-File : Cleanup CURL easy-handle !!!");
    }

    void CFileCopyClient::set_USERPWD(const std::string& _user, const std::string& _passwd) {
      curl_easy_setopt(m_pCurl, CURLOPT_USERPWD, (_user+":"+_passwd).c_str());

      // TODO(later): set CURLOPT_SSH_AUTH_TYPES to CURLSSH_AUTH_PASSWORD ??
      return;
    }

    // Max-time (in seconds) allowed for libcurl to 'connect' to server
    void CFileCopyClient::SetConnTimeout(uint32_t seconds) {
      uint32_t effSeconds = ((seconds > 0) ? seconds : 0);
      CURLcode retCode = curl_easy_setopt(m_pCurl, CURLOPT_CONNECTTIMEOUT, effSeconds);
      CURLWRAPPER_DEBUG("Copy-File : Set CURLOPT_CONNECTTIMEOUT [" << effSeconds << "] ... " << ((CURLE_OK == retCode) ? "OK" : "NOT OK"));
      return;
    }

    // Max-time (in seconds) allowed for libcurl to 'perform' an operation
    void CFileCopyClient::SetOperTimeout(uint32_t seconds) {
      CURLcode retCode;
      uint32_t effSeconds = 0;

      effSeconds = ((seconds > 0) ? seconds : 0);
      retCode = curl_easy_setopt(m_pCurl, CURLOPT_TIMEOUT, effSeconds);
      CURLWRAPPER_DEBUG("Copy-File : Set CURLOPT_TIMEOUT [" << effSeconds << "] ... " << ((CURLE_OK == retCode) ? "OK" : "NOT OK"));

      effSeconds = ((seconds > 5) ? (seconds-5)  : 0);
      retCode = curl_easy_setopt(m_pCurl, CURLOPT_FTP_RESPONSE_TIMEOUT, effSeconds);
      CURLWRAPPER_DEBUG("Copy-File : Set CURLOPT_FTP_RESPONSE_TIMEOUT [" << effSeconds << "] ... " << ((CURLE_OK == retCode) ? "OK" : "NOT OK"));

      return;
    }

    // Create missing sub-directory if does not exist
    bool CFileCopyClient::createMissingDirs(const std::string& destpath, std::string& errstr){
    	std::string dirpath;
    	std::string fname;
    	xtreme::extractDirectoryNFileName(destpath, dirpath, fname);
    	boost::filesystem::path boostpath = dirpath;
    	bool result = false;
        boost::system::error_code ec;
    	try {
    	 result = boost::filesystem::create_directories(boostpath, ec);
     	 if (!result && ec.value() > 0){
     		CURLWRAPPER_ERROR("create dirs failed:error:"<<ec.value()<<", mesg:"<<ec.message());
     		return result;
     	 }
    	}
    	catch(boost::filesystem3::filesystem_error& fserr)
    	{
            ec = fserr.code();
            CURLWRAPPER_ERROR("create dirs failed:exception:"<<ec.message()<<", error:"<<ec.value());
            CURLWRAPPER_ERROR("exception:what:"<<fserr.what());
			errstr.assign(ec.message());
            return false;
        }
    	return true;
    }

    // TODO(IfRequired): Implement setter for CURLOPT_SSH_AUTH_TYPES='CURLSSH_AUTH_PUBLICKEY' if
    //                   authentication is to be done by SSH public/private KEY
    //   curl_setopt($curl, CURLOPT_SSH_AUTH_TYPES, CURLSSH_AUTH_PUBLICKEY);
    //   curl_setopt($curl, CURLOPT_SSH_PUBLIC_KEYFILE, $pubkey_filename); // you already did it
    //   curl_setopt($curl, CURLOPT_SSH_PRIVATE_KEYFILE, $privkey_filename);
    //   curl_setopt($curl, CURLOPT_KEYPASSWD, '');

    // _host ... is expected as IP address string
    // _remoteffname and _localffname ... are expected to be 'absolute' (full) filenames
    CPerformResult CFileCopyClient::copyTO(const std::string& _host, const std::string& _remoteffname,
        const std::string& _localffname) {

      CPerformResult result(CURLE_OK, "");

      // If copying to same appliance (machine) ... use file local File-Copy
      if (isApplianceIP(_host)) {
        boost::filesystem::path src_path = _localffname;
        boost::filesystem::path dst_path = _remoteffname;
        // create missing dirs
        std::string errstr;
        bool ret = createMissingDirs(_remoteffname, errstr);
        if (!ret){
        	std::string errinfo("Local File-Copy failed while creating sub directory with ");
        	errinfo.append(errstr);
            return CPerformResult(CURLE_NOT_BUILT_IN, errinfo);
        }
        CURLWRAPPER_DEBUG("Copy-File : Starting Local File-Copy from [" << _localffname << "] ...to... ["
            << _remoteffname << "]");
        boost::system::error_code ec;
        boost::filesystem::copy_file(src_path, dst_path, boost::filesystem::copy_option::fail_if_exists, ec);

        if (0 != ec) {
          CURLWRAPPER_ERROR("Copy-File : FAILED   Local File-Copy from [" << _localffname << "] ...to... ["
              << _remoteffname << "]");
          return CPerformResult (CURLE_NOT_BUILT_IN, "Local File-Copy failed.");
        }

        CURLWRAPPER_INFO("Copy-File : DONE     Local File-Copy from [" << _localffname << "] ...to... ["
            << _remoteffname << "]");
        return result;
      }

      // Otherwise, proceed to copy to a remote appliance (machine) ... use SFTP

      // Prepare for upload (send) to peer
      curl_easy_setopt(m_pCurl, CURLOPT_UPLOAD, 1);

      // Set destination URL in peer
      std::string destUrl = getDestURL(_host, _remoteffname);
      curl_easy_setopt(m_pCurl, CURLOPT_URL, destUrl.c_str());

      // Scoped 'CpFile'
      {
        CpFile _cpSendFile(_localffname);

        curl_easy_setopt(m_pCurl, CURLOPT_READDATA, (void*)(&_cpSendFile));

        curl_easy_setopt(m_pCurl, CURLOPT_INFILESIZE_LARGE, (curl_off_t)_cpSendFile.st_fileSize);

        // TODO(Later): CURLOPT_DEBUGFUNCTION that uses log4cxx ... required ??

        // TODO(Later): CURLOPT_ERRORBUFFER ??

        // TODO(later): remove debug setting of CURLOPT_VERBOSE.
        curl_easy_setopt(m_pCurl, CURLOPT_VERBOSE, 1L);

				CURLWRAPPER_DEBUG("Copy-File : Starting upload of ... " << _cpSendFile.st_fileName);
        result.result_code_ = curl_easy_perform(m_pCurl);
        CURLWRAPPER_DEBUG("Copy-File : " << _cpSendFile.st_fileName << " " << _cpSendFile.st_fileSize << " bytes upload to, host="
            <<_host << " [destination= " << destUrl << "] ...result_code=" << result.result_code_);
      }

      if(result.result_code_ != CURLE_OK) {
        result.info_ = std::string(curl_easy_strerror(result.result_code_));
        CURLWRAPPER_ERROR("FAILED: curl file copyTo, host=" <<_host << ", port=" << m_sshPort << "[destination= " << destUrl << "].");
        CURLWRAPPER_ERROR("curl error code [" << result.result_code_ << "]  info = " << result.info_);
      } else {
        CURLWRAPPER_DEBUG("DONE:   curl file copyTo, host=" <<_host << ", port=" << m_sshPort << "[destination= " << destUrl << "].");
      }
      return result;
    }

    // _remoteffname is expected to be 'absolute' (full) filename
    std::string CFileCopyClient::getDestURL(const std::string& _host, const std::string& _remoteffname) {
      std::string url("?");

      // Instead of specifying port in URL ...
      // CURLOPT_PORT can be set to control remote port to connect-to.
      switch (m_transferType) {
      case kSCP:
        url = "scp://" + _host + ":" + boost::lexical_cast<std::string>(m_sshPort) + _remoteffname;
        break;

      case kSFTP:
        url = "sftp://"+ _host + ":" + boost::lexical_cast<std::string>(m_sshPort) + _remoteffname;
        break;

      default:
        break;
      }

      return url;
    }

  }
}


text:
// Copyright (C) 2015, Pawel Tomulik <ptomulik@meil.pw.edu.pl>
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#define BOOST_TEST_MODULE test_txpl_lexer_get_oct_digit

#include <txpl/test_config.hpp>
#include <boost/test/unit_test.hpp>

#ifndef TXPL_TEST_SKIP_LEXER_GET_OCT_DIGIT

#include <txpl/lexer/get_oct_digit.hpp>

BOOST_AUTO_TEST_CASE(char__char)
{
  using txpl::lexer::get_oct_digit;
  char x = 123;
  BOOST_CHECK(get_oct_digit('0',x));
  BOOST_CHECK_EQUAL(x, '\0');
  BOOST_CHECK(get_oct_digit('1',x));
  BOOST_CHECK_EQUAL(x, '\1');
  BOOST_CHECK(get_oct_digit('2',x));
  BOOST_CHECK_EQUAL(x, '\2');
  BOOST_CHECK(get_oct_digit('3',x));
  BOOST_CHECK_EQUAL(x, '\3');
  BOOST_CHECK(get_oct_digit('4',x));
  BOOST_CHECK_EQUAL(x, '\4');
  BOOST_CHECK(get_oct_digit('5',x));
  BOOST_CHECK_EQUAL(x, '\5');
  BOOST_CHECK(get_oct_digit('6',x));
  BOOST_CHECK_EQUAL(x, '\6');
  BOOST_CHECK(get_oct_digit('7',x));
  BOOST_CHECK_EQUAL(x, '\7');
  // invalid digits
  BOOST_CHECK(!get_oct_digit('8',x));
  BOOST_CHECK(!get_oct_digit('A',x));
}
BOOST_AUTO_TEST_CASE(char__int)
{

  using txpl::lexer::get_oct_digit;
  int x = 123;
  BOOST_CHECK(get_oct_digit('0',x));
  BOOST_CHECK_EQUAL(x, 0);
  BOOST_CHECK(get_oct_digit('1',x));
  BOOST_CHECK_EQUAL(x, 1);
  BOOST_CHECK(get_oct_digit('2',x));
  BOOST_CHECK_EQUAL(x, 2);
  BOOST_CHECK(get_oct_digit('3',x));
  BOOST_CHECK_EQUAL(x, 3);
  BOOST_CHECK(get_oct_digit('4',x));
  BOOST_CHECK_EQUAL(x, 4);
  BOOST_CHECK(get_oct_digit('5',x));
  BOOST_CHECK_EQUAL(x, 5);
  BOOST_CHECK(get_oct_digit('6',x));
  BOOST_CHECK_EQUAL(x, 6);
  BOOST_CHECK(get_oct_digit('7',x));
  BOOST_CHECK_EQUAL(x, 7);
  // invalid digits
  BOOST_CHECK(!get_oct_digit('8',x));
  BOOST_CHECK(!get_oct_digit('A',x));
}

BOOST_AUTO_TEST_CASE(wchar_t__char)
{
  using txpl::lexer::get_oct_digit;
  char x = 123;
  BOOST_CHECK(get_oct_digit(L'0',x));
  BOOST_CHECK_EQUAL(x, '\0');
  BOOST_CHECK(get_oct_digit(L'1',x));
  BOOST_CHECK_EQUAL(x, '\1');
  BOOST_CHECK(get_oct_digit(L'2',x));
  BOOST_CHECK_EQUAL(x, '\2');
  BOOST_CHECK(get_oct_digit(L'3',x));
  BOOST_CHECK_EQUAL(x, '\3');
  BOOST_CHECK(get_oct_digit(L'4',x));
  BOOST_CHECK_EQUAL(x, '\4');
  BOOST_CHECK(get_oct_digit(L'5',x));
  BOOST_CHECK_EQUAL(x, '\5');
  BOOST_CHECK(get_oct_digit(L'6',x));
  BOOST_CHECK_EQUAL(x, '\6');
  BOOST_CHECK(get_oct_digit(L'7',x));
  BOOST_CHECK_EQUAL(x, '\7');
  // invalid digits
  BOOST_CHECK(!get_oct_digit('8',x));
  BOOST_CHECK(!get_oct_digit('A',x));
}
BOOST_AUTO_TEST_CASE(wchar_t__int)
{
  using txpl::lexer::get_oct_digit;
  int x = 123;
  BOOST_CHECK(get_oct_digit('0',x));
  BOOST_CHECK_EQUAL(x, 0);
  BOOST_CHECK(get_oct_digit('1',x));
  BOOST_CHECK_EQUAL(x, 1);
  BOOST_CHECK(get_oct_digit('2',x));
  BOOST_CHECK_EQUAL(x, 2);
  BOOST_CHECK(get_oct_digit('3',x));
  BOOST_CHECK_EQUAL(x, 3);
  BOOST_CHECK(get_oct_digit('4',x));
  BOOST_CHECK_EQUAL(x, 4);
  BOOST_CHECK(get_oct_digit('5',x));
  BOOST_CHECK_EQUAL(x, 5);
  BOOST_CHECK(get_oct_digit('6',x));
  BOOST_CHECK_EQUAL(x, 6);
  BOOST_CHECK(get_oct_digit('7',x));
  BOOST_CHECK_EQUAL(x, 7);
  // invalid digits
  BOOST_CHECK(!get_oct_digit('8',x));
  BOOST_CHECK(!get_oct_digit('A',x));
}
#else
BOOST_AUTO_TEST_CASE(dummy)
{
  BOOST_CHECK(true);
}
#endif

ext: cpp

max_stars_repo_name: ptomulik/txpl

max_issues_repo_name: ptomulik/txpl

max_forks_repo_name: ptomulik/txpl


text:
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <identify_duplos/Duplos.h>
//#include <std_msgs/String.h>
#include <sstream>
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <LinearMath/btVector3.h>
// PCL specific includes
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>

#define stddev(x, u, s) (x > (u-s) && x < (u+s))

class DuploColor
{
  public:
	std::string name;
	uint8_t r_u, r_s;
	uint8_t g_u, g_s;
	uint8_t b_u, b_s;

	DuploColor () :
	  r_u (0),
	  r_s (0),
	  g_u (0),
	  g_s (0),
	  b_u (0),
	  b_s (0)
	{}

    ~DuploColor () {}

    /*void
    ROSInfo()
    {
		std::stringstream ss;
		ss << "(r_u, r_s, g_u, g_s, b_u, b_s): " << name << " ";
		ss << (int)r_u << " " << (int)r_s << " " << (int)g_u;
		ss << " " << (int)g_s << " " << (int)b_u << " " << (int)b_s;
		ROS_INFO("%s", ss.str().c_str());
    }*/
};

class FeatureCloud
{
  public:
	std::string name;
    // A bit of shorthand
    typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
    typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
    typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
    typedef pcl::KdTree<pcl::PointXYZRGB> SearchMethod;

    FeatureCloud () :
      search_method_xyz_ (new pcl::KdTreeFLANN<pcl::PointXYZRGB>),
      normal_radius_ (0.02f),
      feature_radius_ (0.02f)
    {}

    ~FeatureCloud () {}

    // Process the given cloud
    void
    setInputCloud (PointCloud::Ptr xyz)
    {
      xyz_ = xyz;
      processInput ();
    }

    // Load and process the cloud in the given PCD file
    void
    loadInputCloud (const std::string &pcd_file)
    {
      xyz_ = PointCloud::Ptr (new PointCloud);
      pcl::io::loadPCDFile (pcd_file, *xyz_);
      processInput ();
    }

    // Get a pointer to the cloud 3D points
    PointCloud::Ptr
    getPointCloud () const
    {
      return (xyz_);
    }

    // Get a pointer to the cloud of 3D surface normals
    SurfaceNormals::Ptr
    getSurfaceNormals () const
    {
      return (normals_);
    }

    // Get a pointer to the cloud of feature descriptors
    LocalFeatures::Ptr
    getLocalFeatures () const
    {
      return (features_);
    }

  protected:
    // Compute the surface normals and local features
    void
    processInput ()
    {
    	search_method_xyz_->setInputCloud(xyz_);
		computeSurfaceNormals ();
		computeLocalFeatures ();
    }

    // Compute the surface normals
    void
    computeSurfaceNormals ()
    {
		normals_ = SurfaceNormals::Ptr (new SurfaceNormals);

		pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> norm_est;
		norm_est.setInputCloud (xyz_);
		norm_est.setSearchMethod (search_method_xyz_);
		norm_est.setRadiusSearch (normal_radius_);
		norm_est.compute (*normals_);
	}

    // Compute the local feature descriptors
    void
    computeLocalFeatures ()
    {
		features_ = LocalFeatures::Ptr (new LocalFeatures);

		pcl::FPFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
		fpfh_est.setInputCloud (xyz_);
		fpfh_est.setInputNormals (normals_);
		fpfh_est.setSearchMethod (search_method_xyz_);
		fpfh_est.setRadiusSearch (feature_radius_);
		fpfh_est.compute (*features_);
    }

  private:
    // Point cloud data
    PointCloud::Ptr xyz_;
    SurfaceNormals::Ptr normals_;
    LocalFeatures::Ptr features_;
    SearchMethod::Ptr search_method_xyz_;

    // Parameters
    float normal_radius_;
    float feature_radius_;
};

class TemplateAlignment
{
  public:
    std::vector<FeatureCloud> templates_;

    // A struct for storing alignment results
    struct Result
    {
      float fitness_score;
      Eigen::Matrix4f final_transformation;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    TemplateAlignment () :
      min_sample_distance_ (0.05f), // started as 0.05f
      max_correspondence_distance_ (0.01f*0.01f), // started as 0.01f*0.01f
      nr_iterations_ (500)
    {
      // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
      sac_ia_.setMinSampleDistance (min_sample_distance_);
      sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
      sac_ia_.setMaximumIterations (nr_iterations_);
    }

    ~TemplateAlignment () {}

    // Set the given cloud as the target to which the templates will be aligned
    void
    setTargetCloud (FeatureCloud &target_cloud)
    {
      target_ = target_cloud;
      sac_ia_.setInputTarget (target_cloud.getPointCloud ());
      sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
    }

    // Add the given cloud to the list of template clouds
    void
    addTemplateCloud (FeatureCloud &template_cloud)
    {
      templates_.push_back (template_cloud);
    }

    // Align the given template cloud to the target specified by setTargetCloud ()
    void
    align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)
    {
      sac_ia_.setInputCloud (template_cloud.getPointCloud ());
      sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());

      pcl::PointCloud<pcl::PointXYZRGB> registration_output;
      sac_ia_.align (registration_output);

      result.fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);
      result.final_transformation = sac_ia_.getFinalTransformation ();
    }

    // Align all of template clouds set by addTemplateCloud to the target specified by setTargetCloud ()
    void
    alignAll (std::vector<TemplateAlignment::Result, Eigen::aligned_allocator<Result> > &results)
    {
      results.resize (templates_.size ());
      for (size_t i = 0; i < templates_.size (); ++i)
      {
        align (templates_[i], results[i]);
      }
    }

    // Align all of template clouds to the target cloud to find the one with best alignment score
    int
    findBestAlignment (TemplateAlignment::Result &result)
    {
      // Align all of the templates to the target cloud
      std::vector<Result, Eigen::aligned_allocator<Result> > results;
      alignAll (results);

      // Find the template with the best (lowest) fitness score
      float lowest_score = std::numeric_limits<float>::infinity ();
      int best_template = 0;
      for (size_t i = 0; i < results.size (); ++i)
      {
        const Result &r = results[i];
        if (r.fitness_score < lowest_score)
        {
          lowest_score = r.fitness_score;
          best_template = (int) i;
        }
      }

      // Output the best alignment
      result = results[best_template];
      return (best_template);
    }

  private:
    // A list of template clouds and the target to which they will be aligned
    FeatureCloud target_;

    // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZRGB, pcl::PointXYZRGB, pcl::FPFHSignature33> sac_ia_;
    float min_sample_distance_;
    float max_correspondence_distance_;
    int nr_iterations_;
};

class DuploModel
{
  public:
	TemplateAlignment template_align;
	std::vector<DuploColor> duplo_colors;

	DuploModel ()
	{
	}

	~DuploModel () {}

	int
	identifyShape(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
	{
		// set target cloud
		FeatureCloud target_cloud;
		target_cloud.setInputCloud(cloud);
		template_align.setTargetCloud(target_cloud);

		// find the best template alignment
		TemplateAlignment::Result best_alignment;
		int best_index = template_align.findBestAlignment (best_alignment);
		//const FeatureCloud &best_template = object_templates[best_index];
		//ROS_INFO("Fitness score: %f", best_alignment.fitness_score);

		return best_index;
	}

	int
	identifyColor(uint8_t r, uint8_t g, uint8_t b)
	{
		int bestIndex = -1, lowestError = 1000, error;
		float lowLight = 1;//0.60f; // for night time in my bed room
		float devAmt = 2; // number of std devs to accept

		for(size_t i = 0; i < duplo_colors.size(); i++)
		{
			// check to see if we are in range
			if(stddev(r, duplo_colors[i].r_u*lowLight, duplo_colors[i].r_s*lowLight*devAmt) &&
				stddev(g, duplo_colors[i].g_u*lowLight, duplo_colors[i].g_s*lowLight*devAmt) &&
				stddev(b, duplo_colors[i].b_u*lowLight, duplo_colors[i].b_s*lowLight*devAmt))
			{
				error = abs(duplo_colors[i].r_u*lowLight - r) + abs(duplo_colors[i].g_u*lowLight - g) + abs(duplo_colors[i].b_u*lowLight - b);
				if(error < lowestError)
				{
					lowestError = error;
					bestIndex = i;
				}
			}
		}

		return bestIndex;
	}

	pcl::ModelCoefficients::Ptr
	identifyOrientation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
	{
		// Separate largest contiguous plane (floor) via RANSAC
		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
		pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
		pcl::SACSegmentation<pcl::PointXYZRGB> seg;
		seg.setModelType (pcl::SACMODEL_PLANE);
		seg.setMethodType (pcl::SAC_RANSAC);
		seg.setMaxIterations (1000);
		seg.setDistanceThreshold (0.001);
		seg.setOptimizeCoefficients (true); // optional
		seg.setInputCloud (cloud);
		seg.segment (*inliers, *coefficients);

		// Debug surface
		pcl::ExtractIndices<pcl::PointXYZRGB> extract;
		extract.setInputCloud (cloud);
		extract.setIndices (inliers);
		extract.setNegative (false);
		extract.filter (*cloud);

		return coefficients;
	}

	void
	loadConfig(char *config_filename)
	{
		std::ifstream input_stream (config_filename);
		std::string line;
		enum readStates {SHAPES, COLORS, GENERAL};
		readStates read_state = GENERAL;
		while (input_stream.good ())
		{
			std::getline (input_stream, line);
			if (line.empty () || line.at (0) == '#') // Skip blank lines or comments
				continue;
			else if (strcmp(line.c_str(), "SHAPES") == 0) {
				read_state = SHAPES;
				continue;
			}
			else if (strcmp(line.c_str(), "COLORS") == 0) {
				read_state = COLORS;
				continue;
			}

			// find first space in line
			size_t pos = line.find(" ");

			if(read_state == SHAPES)
			{
				// create and add FeatureCloud
				FeatureCloud template_cloud;
				template_cloud.name = line.substr(0, pos);
				template_cloud.loadInputCloud (line.substr(pos+1));
				template_align.addTemplateCloud (template_cloud);

				// let the world know
				std::stringstream ss;
				ss << "Shape loaded: " << template_cloud.name << " with ";
				ss << template_cloud.getPointCloud()->size() << " points";
				ROS_INFO("%s", ss.str().c_str());
			}
			else if(read_state == COLORS)
			{
				// create and add DuploColor
				DuploColor color;
				color.name = line.substr(0, pos);

				int r_u, r_s, g_u, g_s, b_u, b_s;
				sscanf (line.substr(pos+1).c_str(),"%d %d %d %d %d %d", &r_u, &r_s, &g_u, &g_s, &b_u, &b_s);

				color.r_u = (uint8_t)r_u;
				color.r_s = (uint8_t)r_s;
				color.g_u = (uint8_t)g_u;
				color.g_s = (uint8_t)g_s;
				color.b_u = (uint8_t)b_u;
				color.b_s = (uint8_t)b_s;

				duplo_colors.push_back(color);

				// let the world know
				std::stringstream ss;
				ss << "Color loaded: " << color.name << " (R";
				ss << (int)r_u << " " << (int)r_s << " G" << (int)g_u;
				ss << " " << (int)g_s << " B" << (int)b_u << " " << (int)b_s << ")";
				ROS_INFO("%s", ss.str().c_str());
			}
		}
		input_stream.close ();
	}

	void
	vectorAppend(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> in, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &tothis)
	{
		for(size_t i = 0; i < in.size(); i++)
		{
			tothis.push_back(in[i]);
		}
	}

	void
	extractRGB(pcl::PointXYZRGB point, uint8_t &r, uint8_t &g, uint8_t &b)
	{
		uint32_t rgb_val_;
		memcpy(&rgb_val_, &(point.rgb), sizeof(float));
		r = (uint8_t)((rgb_val_ >> 16) & 0x000000ff);
		g = (uint8_t)((rgb_val_ >> 8) & 0x000000ff);
		b = (uint8_t)((rgb_val_) & 0x000000ff);
	}

	pcl::PointXYZRGB
	averageCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, uint8_t &r_ret, uint8_t &g_ret, uint8_t &b_ret)
	{
		pcl::PointXYZRGB avg;
		avg.x = 0; avg.y = 0; avg.z = 0;
		long r_avg = 0, g_avg = 0, b_avg = 0;

		for(size_t i = 0; i < cloud->points.size(); i++)
		{
			avg.x += cloud->points[i].x;
			avg.y += cloud->points[i].y;
			avg.z += cloud->points[i].z;

			uint8_t r, g, b;
			extractRGB(cloud->points[i], r, g, b);
			r_avg += r;
			g_avg += g;
			b_avg += b;
		}

		avg.x /= cloud->points.size();
		avg.y /= cloud->points.size();
		avg.z /= cloud->points.size();

		r_ret = r_avg / cloud->points.size();
		g_ret = g_avg / cloud->points.size();
		b_ret = b_avg / cloud->points.size();

		return avg;
	}

	void
	colorStats(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
	{
		uint8_t r_avg = 0, g_avg = 0, b_avg = 0;
		averageCloud(cloud, r_avg, g_avg, b_avg);
		long r_sigma = 0, g_sigma = 0, b_sigma = 0; // one standard deviation

		for(size_t i = 0; i < cloud->points.size(); i++)
		{
			uint8_t r, g, b;
			extractRGB(cloud->points[i], r, g, b);
			r_sigma += sqrt((r-r_avg)*(r-r_avg));
			g_sigma += sqrt((g-g_avg)*(g-g_avg));
			b_sigma += sqrt((b-b_avg)*(b-b_avg));
		}

		r_sigma /= cloud->points.size();
		g_sigma /= cloud->points.size();
		b_sigma /= cloud->points.size();

		std::stringstream ss;
		ss << "(r_u, r_s, g_u, g_s, b_u, b_s): ";
		ss << (int)r_avg << " " << r_sigma << " " << (int)g_avg;
		ss << " " << g_sigma << " " << (int)b_avg << " " << b_sigma;
		ROS_INFO("%s", ss.str().c_str());
	}

	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>
	segmentByColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
	{
		// Create index groups for each color
		std::vector<pcl::PointIndices::Ptr> indicies;
		for(size_t i = 0; i < duplo_colors.size(); i++)
		{
			pcl::PointIndices::Ptr indexGroup (new pcl::PointIndices ());
			indicies.push_back(indexGroup);
		}

		// Segment by color
		for(size_t i = 0; i < cloud->points.size(); i++)
		{
			uint8_t r, g, b;
			extractRGB(cloud->points[i], r, g, b);
			int bestIndex = identifyColor(r, g, b);

			if(bestIndex != -1) indicies[bestIndex]->indices.push_back(i);
		}

		// Create clouds from indices
		std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clusters;
		for(size_t i = 0; i < duplo_colors.size(); i++)
		{
			// Create cloud if big enough
			size_t minSize = 50;
			//std::cout << "indices: " << indicies[i]->indices.size() << std::endl;
			if(indicies[i]->indices.size() > minSize)
			{
				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
				pcl::ExtractIndices<pcl::PointXYZRGB> extract;
				extract.setInputCloud (cloud);
				extract.setIndices (indicies[i]);
				extract.setNegative (false);
				extract.filter (*cluster);

				cluster->width = cluster->points.size ();
				cluster->height = 1;
				cluster->is_dense = true;

				clusters.push_back(cluster);
			}
		}

		return clusters;
	}

	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>
	segmentByDistance(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
	{
		// Creating the KdTree object and perform Euclidean distance search
		pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
		tree->setInputCloud (cloud);
		std::vector<pcl::PointIndices> cluster_indices;
		pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
		ec.setClusterTolerance (0.005); // 5 mm
		ec.setMinClusterSize (100);
		ec.setMaxClusterSize (5000);
		ec.setSearchMethod (tree);
		ec.setInputCloud(cloud);
		ec.extract (cluster_indices);

		// Separate clusters
		std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clusters;
		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
		{
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
			for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
				cloud_cluster->points.push_back (cloud->points[*pit]); //
			cloud_cluster->width = cloud_cluster->points.size ();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;
			cloud_cluster->header = cloud->header;

			clusters.push_back(cloud_cluster);
		}

		return clusters;
	}

	// find points behind a plane (opposite of the normal vector given in ax+by+cz+d=0)
	pcl::PointIndices::Ptr
	filterBehindPlane(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::ModelCoefficients::Ptr coefficients)
	{
		pcl::PointIndices::Ptr indicies (new pcl::PointIndices ());
		float d = 0.0f; // part of the distance formula from eq 10 at http://mathworld.wolfram.com/Point-PlaneDistance.html
		for(size_t i = 0; i < cloud->points.size(); i++)
		{
			d = coefficients->values[0] * cloud->points[i].x + // a * x0
					coefficients->values[1] * cloud->points[i].y + // b * y0
					coefficients->values[2] * cloud->points[i].z + // c * z0
					coefficients->values[3]; // d
			if(d < 0)
			{
				indicies->indices.push_back(i);
			}
		}

		return indicies;
	}

	pcl::PointIndices::Ptr
	filterColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, uint8_t r_u, uint8_t r_s, uint8_t g_u, uint8_t g_s, uint8_t b_u, uint8_t b_s)
	{
		pcl::PointIndices::Ptr indicies (new pcl::PointIndices ());
		for(size_t i = 0; i < cloud->points.size(); i++)
		{
			uint8_t r, g, b;
			extractRGB(cloud->points[i], r, g, b);

			// check to see if we are in range
			if(stddev(r, r_u, r_s) && stddev(g, g_u, g_s) && stddev(b, b_u, b_s))
			{
				indicies->indices.push_back(i);
			}
		}

		return indicies;
	}
};

max_stars_repo_name: joshvillbrandt/identify-duplos-ros-pkg

max_issues_repo_name: joshvillbrandt/identify-duplos-ros-pkg

max_forks_repo_name: joshvillbrandt/identify-duplos-ros-pkg


text:
// example appsrc for gstreamer 1.0 with own mainloop & external buffers. based on example from gstreamer docs.
// public domain, 2015 by Florian Echtler <floe@butterbrot.org>. compile with:
// gcc --std=c99 -Wall $(pkg-config --cflags gstreamer-1.0) -o gst gst.c $(pkg-config --libs gstreamer-1.0) -lgstapp-1.0

#include <gst/gst.h>
#include <gst/app/gstappsrc.h>

#include <stdio.h>
//#include <gst/video/video.h>
#include "FlyCapture2.h"
#include <iostream>
#include <sstream>

#include <stdint.h>
#include <cstdlib>
//#include <stdlib.h>

#include <time.h>
#include <signal.h>

#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

double ms_time(void)
{
    struct timespec now_timespec;
    clock_gettime(CLOCK_MONOTONIC, &now_timespec);
    return ((double)now_timespec.tv_sec)*1000.0 + ((double)now_timespec.tv_nsec)*1.0e-6;
}

FlyCapture2::Property frameRateProp;
FILE* timeStampFile = NULL;

struct cameraTime {
    unsigned int frameCounter;
    unsigned int embedTime;
    FlyCapture2::TimeStamp fc2Timestamp;
    double tegraTime;
} camTime;

struct ConfigOptions // cmdline and config file options go here
{
    std::string output; // where to write the video
    std::string encoder;
    std::string encoder_name;
    int iHeight;
    int iWidth;
    int frameRate;
    int bitRate;
    int nFramesToCapture; // number of frames to capture
    int saveMetadata;
} co;

int want = 1;

static GMainLoop *loop;
GstElement *pipeline;

GstElement *source, *filter1, *accel, *encoder, *queue, *muxer, *sink;
GstElement *convert;
GstElement *filter2;
GstElement *parser;
GstElement *payloader;

GstCaps *filter1_caps, *filter2_caps;

GstBus *bus;
guint bus_watch_id;

#define WIDTH 1280
#define HEIGHT 720

#define NUM_FRAMES_TO_CAPTURE 3600

//#define TARGET_BITRATE 10000000

static void sigint_restore (void)
{
  struct sigaction action;

  memset (&action, 0, sizeof (action));
  action.sa_handler = SIG_DFL;

  sigaction (SIGINT, &action, NULL);
}

/* Signal handler for ctrl+c */
void intHandler(int dummy) {

	/* Out of the main loop, clean up nicely */
//	g_print ("Returned, stopping playback\n");
//	gst_element_set_state (pipeline, GST_STATE_NULL);
//	g_print ("Deleting pipeline\n");
//	gst_object_unref (GST_OBJECT (pipeline));
//	g_main_loop_unref (loop);

	//! Try sending an EOS signal:

	g_print("Sending EOS signal to pipeline\n");
	gst_element_send_event(pipeline, gst_event_new_eos());
	sigint_restore();
	//exit(0);
}

static gboolean bus_call	(GstBus     *bus,
							 GstMessage *msg,
							 gpointer    data)
{
//	GMainLoop *loop = (GMainLoop *) data;

	switch (GST_MESSAGE_TYPE (msg)) {

		case GST_MESSAGE_EOS:
			g_print ("End of stream\n");
			g_main_loop_quit (loop);
		break;

		case GST_MESSAGE_ERROR: {
			gchar  *debug;
			GError *error;

			gst_message_parse_error (msg, &error, &debug);
			g_free (debug);

			g_printerr ("Error: %s\n", error->message);
			g_error_free (error);

			g_main_loop_quit (loop);
		break;
		}

		default:
		break;
	}

	return TRUE;
}

//uint16_t b_white[WIDTH*HEIGHT];
//uint16_t b_black[WIDTH*HEIGHT];

//! Point Grey Camera:
FlyCapture2::Camera pg_camera;
FlyCapture2::Error pg_error;
FlyCapture2::EmbeddedImageInfo pg_embeddedInfo;
FlyCapture2::ImageMetadata pg_metadata;
FlyCapture2::CameraInfo pg_camInfo;
FlyCapture2::Format7ImageSettings fmt7ImageSettings;
FlyCapture2::Format7PacketInfo fmt7PacketInfo;
FlyCapture2::Format7Info fmt7Info;
FlyCapture2::PixelFormat k_fmt7PixFmt;
FlyCapture2::Mode k_fmt7Mode;
FlyCapture2::Image pg_rawImage;
int imageSize = 0;

//uint8_t buffer[WIDTH*HEIGHT*3]; // This buffer is not used...

static void cb_need_data (GstElement *appsrc, guint unused_size, gpointer user_data) {

  	static gboolean white = FALSE;
	static double last_cap_time = ms_time();
	static double frame_rate_avg = 30.0;
	static double frame_dt_ms = 33.0;
	static double last_frameRate_count_time = ms_time() - 1001.0;
	static int last_frameCount = 0;
	static int frameCount = 0;
	static bool once = false;
	GstBuffer *buffer;
	guint size;
	GstFlowReturn ret;

	double cap_time = ms_time();

    pg_error = pg_camera.RetrieveBuffer( &pg_rawImage );
    if (pg_error != FlyCapture2::PGRERROR_OK)
    {
        printf("Failed to capture image from PG Camera!\n");
        assert(false);
        return;
    }

    //! It is important to get out at this stage so as to not corrupt the data in the file:
    //if (ret != GST_FLOW_OK) {
		/* something wrong, stop pushing */

	//	printf("Something went wrong.  Stop pushing!\n");
		 //g_main_loop_quit (loop);
	//	 return;
	//}

    buffer = gst_buffer_new_wrapped_full(
				(GstMemoryFlags)0,
				(gpointer)pg_rawImage.GetData(),
				imageSize,
				0,
				imageSize,
				NULL,
				NULL);

	g_signal_emit_by_name (appsrc, "push-buffer", buffer, &ret);

	pg_metadata = pg_rawImage.GetMetadata();

    //! Store the time parameters immediately:
    camTime.frameCounter = pg_metadata.embeddedFrameCounter;
    camTime.tegraTime = ms_time();
    camTime.embedTime = pg_metadata.embeddedTimeStamp;
    camTime.fc2Timestamp = pg_rawImage.GetTimeStamp();

    //! Log if enabled:
    if (co.saveMetadata)
    {
        //printf("Embedded time: %u\n", pg_rawImage.GetMetadata().embeddedTimeStamp);
        fprintf(timeStampFile, "[%05d, %u, %u, %lld, %u, %u, %u, %d, %lf];...\n",
            frameCount,
            camTime.frameCounter,
            camTime.embedTime,
            camTime.fc2Timestamp.seconds,
            camTime.fc2Timestamp.microSeconds,
            camTime.fc2Timestamp.cycleSeconds,
            camTime.fc2Timestamp.cycleCount,
            camTime.fc2Timestamp.cycleOffset,
            camTime.tegraTime
            );
        fflush(timeStampFile);
    }

	if (!once) {
		//printf("Hello once\n");
		once = true;
	} else {
		frame_dt_ms = cap_time - last_cap_time;
		if (cap_time - last_frameRate_count_time > 1000.0) {
			int framesInOneSecond = frameCount - last_frameCount;
			printf("Frame Rate: %d\n", framesInOneSecond);
			last_frameRate_count_time = cap_time;
			last_frameCount = frameCount;
		}
	}

	last_cap_time = cap_time;
    frameCount++;

}

bool OpenCamera(int frameRate) {
	//! Create a camera and connect to it (this is FlyCapture code):

    //! Connect Point Grey Camera:
    pg_error = pg_camera.Connect ( 0 );
    if (pg_error != FlyCapture2::PGRERROR_OK)
    {
        printf("Failed to connect to a PG Camera.  Check connection!\n");
        return false;
    }

    printf("  $$ Opened Point Grey camera!\n");

    pg_error = pg_camera.GetEmbeddedImageInfo( &pg_embeddedInfo );
    if (pg_error != FlyCapture2::PGRERROR_OK)
    {
        printf("Unable to get flycapture camera info:\n");
        pg_error.PrintErrorTrace();
        return false;
    }

    //! Turn on time stamp on PG camera if it's off:
    if ( pg_embeddedInfo.timestamp.onOff == false )
    {
        printf("PG Camera: Timestamps were off.  Turning them on!\n");
        pg_embeddedInfo.timestamp.onOff = true;
    }

    if ( pg_embeddedInfo.frameCounter.onOff == false )
    {
        printf("PG Camera: Enabling frame counter\n");
        pg_embeddedInfo.frameCounter.onOff = true;
    }

    pg_error = pg_camera.SetEmbeddedImageInfo( &pg_embeddedInfo );
    if (pg_error != FlyCapture2::PGRERROR_OK )
    {
        printf("Unable to turn on timestamp feature on PG Camera\n");
        pg_error.PrintErrorTrace();
    }

    //! Get the camera info and print it out:
    pg_error = pg_camera.GetCameraInfo( &pg_camInfo );
    if (pg_error != FlyCapture2::PGRERROR_OK )
    {
        printf("Unable to get PG Camera info\n");
        pg_error.PrintErrorTrace();
    }

    printf("Fly Capture Camera Information:\n");
    printf("  Vendor: %s\n", pg_camInfo.vendorName);
    printf("  Model: %s\n", pg_camInfo.modelName);
    printf("  Serial Num: %d\n", pg_camInfo.serialNumber);

    /// Need to find a place for these variables
    k_fmt7Mode = FlyCapture2::MODE_0;

    k_fmt7PixFmt = FlyCapture2::PIXEL_FORMAT_RGB;

    //! Check if mode is supported
    bool supported;
    fmt7Info.mode = k_fmt7Mode;
    pg_error = pg_camera.GetFormat7Info( &fmt7Info, &supported );
    if (pg_error != FlyCapture2::PGRERROR_OK )
    {
        printf("Format Mode not supported\n");
        pg_error.PrintErrorTrace();
    }
    //! Check if pixel format is supported
     if ( (k_fmt7PixFmt & fmt7Info.pixelFormatBitField) == 0 )
    {
		printf("Pixel Format not supported\n");
        pg_error.PrintErrorTrace();
    }
    //! Create struct with new settings
    fmt7ImageSettings.mode = k_fmt7Mode;
//    fmt7ImageSettings.offsetX = (fmt7Info.maxWidth-WIDTH)/2;
//    fmt7ImageSettings.offsetY = (fmt7Info.maxHeight-HEIGHT)/2;
//    fmt7ImageSettings.width = WIDTH;
//    fmt7ImageSettings.height = HEIGHT;
    fmt7ImageSettings.offsetX = (fmt7Info.maxWidth-co.iWidth)/2;
    fmt7ImageSettings.offsetY = (fmt7Info.maxHeight-co.iHeight)/2;
    fmt7ImageSettings.width = co.iWidth;
    fmt7ImageSettings.height = co.iHeight;
    fmt7ImageSettings.pixelFormat = k_fmt7PixFmt;

    //! Validate new settings
    bool valid;
    // Validate the settings to make sure that they are valid
    pg_error = pg_camera.ValidateFormat7Settings(&fmt7ImageSettings,&valid, &fmt7PacketInfo );
    if (pg_error != FlyCapture2::PGRERROR_OK )
    {
        printf("Cannot validate format settings\n");
        pg_error.PrintErrorTrace();
    }

    if ( !valid )
    {
		printf("Format settings required are not supported\n");
        pg_error.PrintErrorTrace();
        return false;
    }

    //! Set the new settings to the camera
    pg_error = pg_camera.SetFormat7Configuration(&fmt7ImageSettings,fmt7PacketInfo.recommendedBytesPerPacket );
    if (pg_error != FlyCapture2::PGRERROR_OK )
    {
        printf("Unable to set 7 camera settings\n");
        pg_error.PrintErrorTrace();
    }

    //! Start capturing images
    pg_error = pg_camera.StartCapture();
    if (pg_error == FlyCapture2::PGRERROR_ISOCH_BANDWIDTH_EXCEEDED)
    {
        printf("Bandwidth exceeded!\n");
        return false;
    }
    else if (pg_error != FlyCapture2::PGRERROR_OK)
    {
        printf("Error starting capture!\n");
        pg_error.PrintErrorTrace();
    }

    //! Capture first image to make sure things are working ok:
    pg_error = pg_camera.RetrieveBuffer( &pg_rawImage );
    if (pg_error != FlyCapture2::PGRERROR_OK)
    {
        printf("Failed to capture first image from PG Camera!\n");
        return false;
    }

    //! The user can set a different "Max Frame Rate", which will allow the autoexposure to
    //! increase shutter speed.
    frameRateProp.type = FlyCapture2::FRAME_RATE;
    if (frameRate == 0)
    {
        frameRateProp.onOff = true;
        frameRateProp.autoManualMode = true;
        frameRateProp.absControl = false;
    }
    else
    {
        frameRateProp.onOff = true;
        frameRateProp.autoManualMode = false;
        frameRateProp.absControl = true;
        frameRateProp.absValue = (float)frameRate;
    }
    pg_error = pg_camera.SetProperty(&frameRateProp);
    if ( pg_error != FlyCapture2::PGRERROR_OK )
    {
        printf("Error setting frame rate on camera!\n");
        pg_error.PrintErrorTrace();
        return false;
    }

    //! Create dump file if we are saving images:
    imageSize = pg_rawImage.GetDataSize();

    printf("  IMAGE SIZE: %d\n", imageSize);

    //! Make file for timestamps:
    if (co.saveMetadata) {
		std::stringstream ss("");
		ss << co.output << ".m";
		timeStampFile = fopen(ss.str().c_str(), "w+");
		if (timeStampFile == NULL)
		{
			printf("Error creating timestamp file\n");
			return false;
		}

		fprintf(timeStampFile, "captureHeaders = {'Index', 'PG_Index', 'PG_embedTime', 'PG_Time_Sec', 'PG_microsec', "
			"'Cycle_seconds', 'Cycle_count', 'Cycle_offset', 'tegra_time'"
			"};\n");
		fprintf(timeStampFile, "captureData = [...\n");
	}
	return true;
}

int factory_make(char const* location)
{
	//! Encode to file:
	//! This pipeline works well.  Now, to hook it up to our appsrc:
	//gst-launch-1.0 videotestsrc \
	! 'video/x-raw, format=(string)RGB, width=(int)800, height=(int)600' \
	! videoconvert \
	! omxh264enc \
	! 'video/x-h264, stream-format=(string)byte-stream' \
	! h264parse \
	! qtmux \
	! filesink location=testconvert2.mp4 -e

	//! Streaming pipeline:
	//gst-launch-1.0 -v videotestsrc \
	! 'video/x-raw, format=(string)RGB, width=(int)800, height=(int)600' \
	! videoconvert \
	! omxh264enc \
	! 'video/x-h264, stream-format=(string)byte-stream' \
	! queue \
	! rtph264pay pt=96 \
	! udpsink host=192.168.0.108 port=5000

	/* Create gstreamer elements */
	pipeline 	= gst_pipeline_new ("pipeline");
	source		= gst_element_factory_make ("appsrc", "source");
	//source   	= gst_element_factory_make ("videotestsrc",	"videotestsrc");
	filter1  	= gst_element_factory_make ("capsfilter",	"filter1");
	convert	 	= gst_element_factory_make ("videoconvert", "conv");
//	encoder  	= gst_element_factory_make ("omxh264enc",	"h264");
    printf("Using encoder %s\n", co.encoder.c_str());
	encoder  	= gst_element_factory_make (co.encoder.c_str(), co.encoder_name.c_str());
	filter2	 	= gst_element_factory_make ("capsfilter", "filter2");
	queue	 	= gst_element_factory_make ("queue", "myqueue");
	payloader 	= gst_element_factory_make ("rtph264pay", "payloader");
	sink		= gst_element_factory_make ("udpsink", "sink");

	//parser	 = gst_element_factory_make ("h264parse", "parser");

	//! Note: qtmux is good if you can cleanly exit the app and send EOS upstream.  Otherwise, your files are corrupted.
	//muxer    = gst_element_factory_make ("qtmux",			"mux");
	//GstElement *muxer	 = gst_element_factory_make ("mpegtsmux", "mpegmux");
	//sink     = gst_element_factory_make ("filesink",		"filesink");
	//GstElement *fakesink = gst_element_factory_make ("fakesink", "fakey");

	//! setup
	g_object_set (G_OBJECT (source), "caps",
		gst_caps_new_simple ("video/x-raw",
					 "format", G_TYPE_STRING, "RGB",
					 "width", G_TYPE_INT, co.iWidth,
					 "height", G_TYPE_INT, co.iHeight,
					 "framerate", GST_TYPE_FRACTION, 0, 1,
					 NULL), NULL);
//	g_object_set (G_OBJECT (source), "caps",
//		gst_caps_new_simple ("video/x-raw",
//					 "format", G_TYPE_STRING, "RGB",
//					 "width", G_TYPE_INT, WIDTH,
//					 "height", G_TYPE_INT, HEIGHT,
//					 "framerate", GST_TYPE_FRACTION, 0, 1,
//					 NULL), NULL);

	/* Video caps */
	filter1_caps = gst_caps_new_simple ("video/x-raw",
		"format", G_TYPE_STRING, "RGB",
		"width", G_TYPE_INT, co.iWidth,
		"height", G_TYPE_INT, co.iHeight,
		NULL);
//	filter1_caps = gst_caps_new_simple ("video/x-raw",
//		"format", G_TYPE_STRING, "RGB",
//		"width", G_TYPE_INT, WIDTH,
//		"height", G_TYPE_INT, HEIGHT,
//		NULL);

	filter2_caps = gst_caps_new_simple ("video/x-h264",
		"stream-format", G_TYPE_STRING, "byte-stream",
		NULL);

	if (!pipeline) {
		g_printerr ("Pipeline could not be created. Exiting.\n");
		return -1;
	}
	if (!source) {
		g_printerr ("Source could not be created. Exiting.\n");
		return -1;
	}
	if (!filter1) {
		g_printerr ("Filter 1 could not be created. Exiting.\n");
		return -1;
	}
	if (!convert) {
		g_printerr ("Convert element could not be created. Exiting.\n");
		return -1;
	}
	if (!encoder) {
		g_printerr ("Encoder element could not be created. Exiting.\n");
		return -1;
	}
	if (!filter2) {
		g_printerr ("Filter 2 element could not be created. Exiting.\n");
		return -1;
	}
	/*if (!parser) {
		g_printerr ("Parser element could not be created. Exiting.\n");
		return -1;
	}
	if (!muxer) {
		g_printerr ("Muxer element could not be created. Exiting.\n");
		return -1;
	}*/
	if (!sink) {
		g_printerr ("Sink element could not be created. Exiting.\n");
		return -1;
	}

	if (!payloader) {
		g_printerr ("Error making payloader element\n");
		return -1;
	}

	if (!filter1_caps) {
		g_printerr ("Filter1 caps could not be created. Exiting.\n");
		return -1;
	}

	if (!filter2_caps) {
		g_printerr ("Filter 2 caps could not be created. Exiting.\n");
		return -1;
	}

	/* Set up elements */
	g_object_set (G_OBJECT (filter1), "caps", filter1_caps, NULL);
	g_object_set (G_OBJECT (filter2), "caps", filter2_caps, NULL);
	gst_caps_unref (filter1_caps);
	gst_caps_unref (filter2_caps);
//	g_object_set (G_OBJECT (source), "num-buffers", NUM_FRAMES_TO_CAPTURE, NULL);
	g_object_set (G_OBJECT (source), "num-buffers", co.nFramesToCapture, NULL);

	/*int target_bitrate = atoi(bitrate_arg);

	if (target_bitrate < 100000) {
		printf("Bitrate too low! setting to 100000\n");
		target_bitrate = 100000;
	} else if (target_bitrate > 100000000) {
		printf("Bitrate is ridiculous!  Setting to 100 Mbps\n");
		target_bitrate = 100000000;
	}*/

	//! Settings for the h264 encoder:
	g_object_set (G_OBJECT (encoder), "bitrate", co.bitRate, NULL);

	/* Settings for payloader: */
	g_object_set (G_OBJECT (payloader), "pt", 96, NULL);

	/* we set the input filename to the source element */
	//g_object_set (G_OBJECT (sink), "location", location, NULL);
	//! UDP SINK:
	g_object_set (G_OBJECT (sink), "host", "192.168.0.108", "port", 5000, NULL);

	// Fixing intermittentcorrupted file issue
	//g_object_set(G_OBJECT (muxer), "faststart", TRUE, NULL);
	//g_object_set(G_OBJECT (muxer), "fragment-duration", 500, NULL);

	/* setup appsrc */
	g_object_set (G_OBJECT (source),
		"stream-type", GST_APP_STREAM_TYPE_STREAM,
		"format", GST_FORMAT_TIME,
		"is-live", TRUE,
		"do-timestamp", TRUE,
		NULL);
	g_signal_connect (source, "need-data", G_CALLBACK (cb_need_data), NULL);

	return 0;
}

int pipeline_make()
{
	//! Streaming pipeline:
	//gst-launch-1.0 -v videotestsrc \
	! 'video/x-raw, format=(string)RGB, width=(int)800, height=(int)600' \
	! videoconvert \
	! omxh264enc \
	! 'video/x-h264, stream-format=(string)byte-stream' \
	! queue \
	! rtph264pay pt=96 \
	! udpsink host=192.168.0.108 port=5000

	/* we add all elements into the pipeline */
	gst_bin_add_many (GST_BIN (pipeline),
		source, filter1, convert, encoder, filter2, queue, payloader, sink, NULL);
	/* we link the elements together */
	gst_element_link_many (source, filter1, convert, encoder, filter2, queue, payloader, sink, NULL);

	//! To get verbose output (needed for setting up video streaming)
	g_signal_connect( pipeline, "deep-notify", G_CALLBACK( gst_object_default_deep_notify ), NULL );

	return 0;
}

int watcher_make()
{
	/* we add a message handler */
	bus = gst_pipeline_get_bus (GST_PIPELINE (pipeline));
	bus_watch_id = gst_bus_add_watch (bus, bus_call, loop);
	gst_object_unref (bus);
	return 0;
}

gint main (gint argc, gchar *argv[]) {

	//GstElement *pipeline, , *queue, *videosink;
	signal(SIGINT, intHandler);

//    struct sigaction sigact;
//    sigact.sa_handler = intHandler;
//    sigemptyset(&sigact.sa_mask);
//    sigact.sa_flags   = 0;
//    sigaction(SIGINT, &sigact, 0);

//	/* Check input arguments */
//	if (argc != 2) {
//		g_printerr ("Usage: %s <Recorded file name eg: test.h264>\n", argv[0]);
//		return -1;
//	}

    try
    {
        /**
            Options that can only be specified on the command line:
         */
        po::options_description cmdline_only_opts("Command Line Options");
        cmdline_only_opts.add_options()
            ("help",        "produce help message and exit")
        ;

        po::options_description config_opts("Configuration Options");
        config_opts.add_options()
            ("output-file,o",
                po::value<std::string>(&(co.output))->default_value("videoFile"),
                "output file name, ex: vga_output1.vga")
//            ("encoder,e",
//                po::value<std::string>(&(co.encoder))->default_value("h264"),
//                "encoder type: h264, mpeg4, vp8")
            ("width,w",
                po::value<int>(&(co.iWidth))->default_value(1280),
                "Image capture width (pixels)")
            ("height,h",
                po::value<int>(&(co.iHeight))->default_value(720),
                "Image capture height (pixels)")
            ("frameRate,r",
                po::value<int>(&(co.frameRate))->default_value(0),
                "Frame Rate (Integer) (0=no limit, >0 may help auto exposure work better)")
            ("bitRate,b",
                po::value<int>(&(co.bitRate))->default_value(10000000),
                "Bit Rate (Integer), ex: 10000000")
            ("nFrames,n",
                po::value<int>(&(co.nFramesToCapture))->default_value(1000),
                "Number of frames to capture (Integer), ex: 1000")
            ("saveMetadata,s",
				po::value<int>(&(co.saveMetadata))->default_value(1),
				"Save Metadata (timestamps, etc, in an m-file)")
            ;

        //! If an option is not prepended by a flag in the arguments list, treat it as an output file:
        po::positional_options_description p;
        p.add("output-file", -1);
        po::options_description cmdline_opts;
        cmdline_opts.add(cmdline_only_opts).add(config_opts);
         //! This is the map of variables that can be used to access the arguments:
        po::variables_map vm;
        //! Parse the commandline:
        po::store(po::command_line_parser(argc, argv).options(cmdline_opts).positional(p).run(), vm);
        po::notify(vm);

        if (vm.count("help")) {
            std::cout << "Usage: " << argv[0] << " [options] -o output-file\n";
            std::cout << cmdline_opts << "\n";
            exit(0);
        }

        // All params have dafaults
        co.bitRate = vm["bitRate"].as<int>();
//        if(boost::iequals(co.encoder, "mpeg4"))
//        {
//            co.encoder = "avenc_mpeg4";
//            co.encoder_name = "mp4";
//        }
//        else if(boost::iequals(co.encoder, "vp8"))
//        {
//            co.encoder = "omxvp8enc";
//            co.encoder_name = "vp8";
//        }
//        else
//        {
//            printf("Defaulting to omxh264enc encoder (h.264)\n");
//            co.encoder = "omxh264enc";
//            co.encoder_name = "h264";
//        }
        co.encoder = "omxh264enc";
        co.encoder_name = "h264";
        co.frameRate = vm["frameRate"].as<int>();
        co.iHeight = vm["height"].as<int>();
        co.iWidth = vm["width"].as<int>();
        co.nFramesToCapture = vm["nFrames"].as<int>();
        co.output = vm["output-file"].as<std::string>();
    }
    catch(std::exception &e)
    {
        std::cout << "Commandline parsing exception: " << e.what() << std::endl;
        return -1;
    }

	if (!OpenCamera(co.frameRate))
	{
	  return -1;
	}

	/* init GStreamer */
	gst_init (&argc, &argv);

	loop = g_main_loop_new (NULL, FALSE);

	/* Check input arguments */
//	if (argc < 3) {
//		g_printerr ("Usage: %s <Recorded file name eg: test.h264> target_bitrate\n", argv[0]);
//		return -1;
//	}

	char const* output_location = co.output.c_str();
	if(factory_make(output_location) != 0)
 		return -1;
//	if(factory_make(argv[1]) != 0)
//		return -1;

	/* Add function to watch bus */
	if(watcher_make() != 0)
		return -1;

	/* Add elements to pipeline, and link them */
	if(pipeline_make() != 0)
		return -1;

	/* play */
	gst_element_set_state (pipeline, GST_STATE_PLAYING);

	g_main_loop_run(loop);

	/* clean up */
	gst_element_set_state (pipeline, GST_STATE_NULL);
	gst_object_unref (GST_OBJECT (pipeline));
	g_main_loop_unref (loop);

	//! Close log file:
	if (co.saveMetadata) {
		fprintf(timeStampFile, "];\n");
        fclose(timeStampFile);
        timeStampFile = NULL;
	}

	return 0;
}


text:
// ---------------------------------------------------------------------
//
// Copyright (c) 2019 - 2021 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files
#include <SAMRAI_config.h>

// Headers for basic PETSc functions
#include <petscsys.h>

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <StandardTagAndInitialize.h>

// other samrai stuff
#include <HierarchyDataOpsManager.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/equation_systems.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_refinement.h>
#include <libmesh/mesh_triangle_interface.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBFEMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/FEProjector.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/IBTK_MPI.h>
#include <ibtk/StableCentroidPartitioner.h>
#include <ibtk/libmesh_utilities.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

// This file is the main driver for velocity interpolation tests (i.e.,
// IBFEmethod::interpolateVelocity).
//
// Due to limitations in how we set up and initialize the Eulerian and
// Lagrangian discretizations, the tests that utilize this executable only
// solve a single interpolation problem. We check the convergence rate by
// running several tests with different grid resolutions (e.g.,
// interpolate_velocity_01.a, interpolate_velocity_01.b, etc.).

// A scalar function parser that behaves similarly to our own
// muParserCartGridFunction.
class ParsedFunction
{
public:
    ParsedFunction(std::vector<std::string> expressions, const unsigned int n_vars = 0)
        : string_functions(std::move(expressions)),
          vars(n_vars == 0 ? string_functions.size() : n_vars, 0.0),
          parsers(string_functions.size())
    {
        for (unsigned int var_n = 0; var_n < vars.size(); ++var_n)
            for (mu::Parser& parser : parsers) parser.DefineVar("X_" + std::to_string(var_n), &vars[var_n]);

        for (unsigned int p_n = 0; p_n < string_functions.size(); ++p_n) parsers[p_n].SetExpr(string_functions[p_n]);
    }

    // Evaluate a single component.
    double value(const libMesh::Point& p, const unsigned int component_n) const
    {
        TBOX_ASSERT(component_n < parsers.size());
        for (unsigned int var_n = 0; var_n < vars.size(); ++var_n) vars[var_n] = p(var_n);

        try
        {
            return parsers[component_n].Eval();
        }
        catch (mu::ParserError& e)
        {
            std::cerr << "Message:  <" << e.GetMsg() << ">\n";
            std::cerr << "Formula:  <" << e.GetExpr() << ">\n";
            std::cerr << "Token:    <" << e.GetToken() << ">\n";
            std::cerr << "Position: <" << e.GetPos() << ">\n";
            std::cerr << "Errc:     <" << e.GetCode() << ">" << std::endl;
            throw e;
        }
        return 1.0;
    }

    // Evaluate all components. Only makes sense if the number of variables is
    // equal to the number of functions.
    libMesh::Point value(const libMesh::Point& p) const
    {
        TBOX_ASSERT(vars.size() == parsers.size());
        for (unsigned int var_n = 0; var_n < vars.size(); ++var_n) vars[var_n] = p(var_n);

        libMesh::Point out;
        for (unsigned int component_n = 0; component_n < parsers.size(); ++component_n)
            out(component_n) = parsers[component_n].Eval();
        return out;
    }

private:
    const std::vector<std::string> string_functions;
    mutable std::vector<double> vars;

    std::vector<mu::Parser> parsers;
};

// Coordinate mapping function.
void
coordinate_mapping_function(libMesh::Point& X, const libMesh::Point& s, void* /*ctx*/)
{
    X(0) = s(0) + 0.6;
    X(1) = s(1) + 0.5;
#if (NDIM == 3)
    X(2) = s(2) + 0.5;
#endif
    return;
} // coordinate_mapping_function

int
main(int argc, char** argv)
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);
    const LibMeshInit& init = ibtk_init.getLibMeshInit();

    // set up options for the linear solver to not depend on the parallel partitioning
    PetscOptionsSetValue(nullptr, "-ksp_rtol", "1e-14");
    PetscOptionsSetValue(nullptr, "-ksp_atol", "1e-12");
    PetscOptionsSetValue(nullptr, "-ksp_type", "cg");
    PetscOptionsSetValue(nullptr, "-pc_type", "jacobi");
    PetscOptionsSetValue(nullptr, "-pc_jacobi_type", "diagonal");

    // prevent a warning about timer initializations
    TimerManager::createManager(nullptr);
    {
        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");

        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Create a simple FE mesh.
        ReplicatedMesh mesh(init.comm(), NDIM);
        const double dx = input_db->getDouble("DX");
        string elem_type = input_db->getString("ELEM_TYPE");
        const double R = 0.2;

        // we want R/2^n_refinements = dx so that we have roughly equal spacing for both elements and cells
        const int n_refinements = int(std::log2(R / dx));
        if (elem_type == "TET4" || elem_type == "TET10")
        {
            const int n_cells = std::ceil(R / dx);
            MeshTools::Generation::build_cube(
                mesh, n_cells, n_cells, n_cells, -R, R, -R, R, -R, R, Utility::string_to_enum<ElemType>(elem_type));
        }
        else
        {
            MeshTools::Generation::build_sphere(
                mesh, R, n_refinements, Utility::string_to_enum<ElemType>(elem_type), 10);
        }
        mesh.prepare_for_use();

        // Set up subdomain ids (which are only used in the multilevel test versions):
        {
            for (auto elem_iter = mesh.active_elements_begin(); elem_iter != mesh.active_elements_end(); ++elem_iter)
            {
                Elem* elem = *elem_iter;
#if LIBMESH_VERSION_LESS_THAN(1, 7, 0)
                const libMesh::Point centroid = elem->centroid();
#else
                const libMesh::Point centroid = elem->vertex_average();
#endif
#if NDIM == 2
                if (centroid(1) > 0.0)
                    elem->subdomain_id() = 1;
                else
                    elem->subdomain_id() = 2;
#else
                if (centroid(1) > 0.0 && centroid(2) > 0.0)
                    elem->subdomain_id() = 1;
                else
                    elem->subdomain_id() = 2;
#endif
            }
        }

        MeshRefinement mesh_refinement(mesh);
        if (input_db->getBoolWithDefault("use_amr", false))
        {
            std::size_t i = 0;
            for (auto elem_iter = mesh.active_elements_begin(); elem_iter != mesh.active_elements_end();
                 ++elem_iter, ++i)
            {
                if (i % 8 == 0) (*elem_iter)->set_refinement_flag(Elem::REFINE);
            }
            mesh_refinement.refine_and_coarsen_elements();
        }

        // metis does a good job partitioning, but the partitioning relies on
        // random numbers: the seed changed in libMesh commit
        // 98cede90ca8837688ee13aac5e299a3765f083da (between 1.3.1 and
        // 1.4.0). Hence, to achieve consistent partitioning, use a simpler partitioning scheme:
        IBTK::StableCentroidPartitioner partitioner;
        partitioner.partition(mesh);

        plog << "Number of elements: " << mesh.n_active_elem() << std::endl;

        // Create major algorithm and data objects that comprise the
        // application.  These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"), false);
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy =
            new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry, false);
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();

        Pointer<INSHierarchyIntegrator> navier_stokes_integrator;
        const string solver_type = app_initializer->getComponentDatabase("Main")->getString("solver_type");
        if (solver_type == "STAGGERED")
        {
            navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
                "INSStaggeredHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"),
                false);
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"),
                false);
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }
        Pointer<IBFEMethod> ib_method_ops =
            new IBFEMethod("IBFEMethod",
                           app_initializer->getComponentDatabase("IBFEMethod"),
                           &mesh,
                           app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                           false);
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_method_ops,
                                              navier_stokes_integrator,
                                              false);

        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer,
                                        false);

        // Configure the IBFE solver.
        ib_method_ops->registerInitialCoordinateMappingFunction(coordinate_mapping_function);
        ib_method_ops->initializeFEEquationSystems();
        ib_method_ops->initializeFEData();
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
            "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);

        // Now for the actual test. The stored velocity field does not contain
        // ghost data, so we set up a new context and velocity field that does:
        VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
        const Pointer<SAMRAI::hier::Variable<NDIM> > u_var = time_integrator->getVelocityVariable();
        const Pointer<VariableContext> u_ghost_ctx = var_db->getContext("u_ghost");

        int n_ghosts = 3;
        if (app_initializer->getComponentDatabase("IBFEMethod")->keyExists("min_ghost_cell_width"))
        {
            n_ghosts = app_initializer->getComponentDatabase("IBFEMethod")->getInteger("min_ghost_cell_width");
        }
        const int u_ghost_idx = var_db->registerVariableAndContext(u_var, u_ghost_ctx, n_ghosts);

        for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
        {
            Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
            level->allocatePatchData(u_ghost_idx);
        }
        u_init->setDataOnPatchHierarchy(u_ghost_idx, u_var, patch_hierarchy, 0.0);

        // synch ghost data
        using InterpolationTransactionComponent = HierarchyGhostCellInterpolation::InterpolationTransactionComponent;
        std::vector<InterpolationTransactionComponent> ghost_cell_components(1);
        ghost_cell_components[0] = InterpolationTransactionComponent(u_ghost_idx,
                                                                     "CONSERVATIVE_LINEAR_REFINE",
                                                                     true,
                                                                     "CONSERVATIVE_COARSEN",
                                                                     "LINEAR",
                                                                     false,
                                                                     {}, // u_bc_coefs
                                                                     nullptr);
        HierarchyGhostCellInterpolation ghost_fill_op;
        ghost_fill_op.initializeOperatorState(ghost_cell_components, patch_hierarchy);
        ghost_fill_op.fillData(/*time*/ 0.0);

        // TODO what is the best way to set the linear solver tolerance in the
        // L2 projection, besides, e.g., setting -ksp_rtol 1e-12 at the
        // command line?
        const double dt = time_integrator->getMaximumTimeStepSize();
        time_integrator->preprocessIntegrateHierarchy(
            time_integrator->getIntegratorTime(), time_integrator->getIntegratorTime() + dt, 1 /*???*/);
        ib_method_ops->interpolateVelocity(u_ghost_idx, {}, {}, 0.0);

        {
            const std::string& velocity_name = IBFEMethod::VELOCITY_SYSTEM_NAME;
            const std::string& displacement_name = IBFEMethod::COORDS_SYSTEM_NAME;
            EquationSystems* equation_systems = ib_method_ops->getFEDataManager()->getEquationSystems();
            System& velocity_system = equation_systems->get_system(velocity_name);
            System& displacement_system = equation_systems->get_system(displacement_name);

            const unsigned int n_vars = velocity_system.n_vars();
            const DofMap& dof_map = velocity_system.get_dof_map();
            FEDataManager::SystemDofMapCache& X_dof_map_cache =
                *(ib_method_ops->getFEDataManager()->getDofMapCache(displacement_name));
            FEDataManager::SystemDofMapCache& velocity_dof_map_cache =
                *(ib_method_ops->getFEDataManager()->getDofMapCache(velocity_name));
            FEType fe_type = dof_map.variable_type(0);

            PetscVector<double>& current_velocity =
                *dynamic_cast<PetscVector<double>*>(velocity_system.current_local_solution.get());
            PetscVector<double>& current_X =
                *dynamic_cast<PetscVector<double>*>(displacement_system.current_local_solution.get());

            std::unique_ptr<FEBase> v_fe(FEBase::build(NDIM, fe_type));
            std::unique_ptr<FEBase> X_fe(FEBase::build(NDIM, fe_type));
            QGauss v_qrule(NDIM, FIFTH);
            QGauss X_qrule(NDIM, FIFTH);
            v_fe->attach_quadrature_rule(&v_qrule);
            X_fe->attach_quadrature_rule(&X_qrule);
            const std::vector<std::vector<Real> >& v_phi = v_fe->get_phi();
            const std::vector<std::vector<Real> >& X_phi = X_fe->get_phi();
            std::vector<dof_id_type> v_dof_indices;
            std::array<double, NDIM> max_norm_errors;
            std::fill(max_norm_errors.begin(), max_norm_errors.end(), 0.0);
            TBOX_ASSERT(n_vars == NDIM);

            std::vector<std::string> fs;
            {
                Pointer<Database> v_db = input_db->getDatabase("VelocityInitialConditions");
                for (unsigned int var_n = 0; var_n < n_vars; ++var_n)
                    fs.push_back(v_db->getString("function_" + std::to_string(var_n)));
            }
            ParsedFunction exact_solution(fs);

            std::vector<libMesh::Point> mapped_q_points;
            double max_vertex_distance = 0.0;
            for (auto elem_iter = mesh.active_local_elements_begin(); elem_iter != mesh.active_local_elements_end();
                 ++elem_iter)
            {
                auto elem = *elem_iter;
                max_vertex_distance = std::max(max_vertex_distance, elem->hmax());
                v_fe->reinit(elem);
                X_fe->reinit(elem);
                const auto& v_dof_indices = velocity_dof_map_cache.dof_indices(elem);
                const auto& X_dof_indices = X_dof_map_cache.dof_indices(elem);
                const std::size_t v_n_qp = v_qrule.n_points();
                const std::size_t X_n_qp = X_qrule.n_points();
                TBOX_ASSERT(v_n_qp == X_n_qp);

                // 1. Figure out where the quadrature points really are:
                mapped_q_points.resize(v_n_qp);
                std::fill(mapped_q_points.begin(), mapped_q_points.end(), libMesh::Point());
                for (unsigned int var_n = 0; var_n < n_vars; ++var_n)
                {
                    const std::size_t n_basis = X_dof_indices[var_n].size();
                    for (unsigned int q_point_n = 0; q_point_n < X_n_qp; ++q_point_n)
                    {
                        for (unsigned int i = 0; i < n_basis; ++i)
                        {
                            mapped_q_points[q_point_n](var_n) +=
                                current_X(X_dof_indices[var_n][i]) * X_phi[i][q_point_n];
                        }
                    }
                }

                // 2. Compute the difference:
                for (unsigned int var_n = 0; var_n < n_vars; ++var_n)
                {
                    const std::size_t n_basis = v_dof_indices[var_n].size();
                    for (unsigned int q_point_n = 0; q_point_n < v_n_qp; ++q_point_n)
                    {
                        const double exact_point_value = exact_solution.value(mapped_q_points[q_point_n], var_n);
                        double fe_point_value = 0.0;
                        for (unsigned int i = 0; i < n_basis; ++i)
                        {
                            fe_point_value += current_velocity(v_dof_indices[var_n][i]) * v_phi[i][q_point_n];
                        }
                        max_norm_errors[var_n] =
                            std::max(max_norm_errors[var_n], std::abs(fe_point_value - exact_point_value));
                    }
                }
            }

            plog << "max vertex distance: " << IBTK_MPI::maxReduction(max_vertex_distance) << std::endl;
            plog << "max norm errors: ";
            for (double& max_error : max_norm_errors) max_error = IBTK_MPI::maxReduction(max_error);
            plog << std::setprecision(20);
            for (unsigned int i = 0; i < n_vars - 1; ++i)
            {
                plog << max_norm_errors[i] << "   ";
            }
            plog << max_norm_errors[n_vars - 1] << std::endl;

            // For testing purposes its sometimes useful to look at the diagonal
            // mass matrix
            if (input_db->getBoolWithDefault("print_diagonal_mass_matrix", false))
            {
                Pointer<Database> db(new InputDatabase("database"));
                db->putBool("enable_logging", true);

                FEProjector fe_projector(equation_systems, db);
                PetscVector<double>& diagonal_mass = *fe_projector.buildDiagonalL2MassMatrix(velocity_system.name());
                diagonal_mass.print_global(plog);
            }
        }
    } // cleanup dynamically allocated objects prior to shutdown
} // main

max_stars_repo_name: akashdhruv/IBAMR

max_issues_repo_name: akashdhruv/IBAMR

max_forks_repo_name: akashdhruv/IBAMR


text:
/*
 *             Copyright Andrey Semashev 2021.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/config/abi_prefix.hpp>

#if !defined(BOOST_FILESYSTEM_ENABLE_WARNINGS)

#if defined(_MSC_VER) && !defined(__clang__)

#pragma warning(push, 3)
// 'm_A' : class 'A' needs to have dll-interface to be used by clients of class 'B'
#pragma warning(disable: 4251)
// non dll-interface class 'A' used as base for dll-interface class 'B'
#pragma warning(disable: 4275)
// 'int' : forcing value to bool 'true' or 'false' (performance warning)
#pragma warning(disable: 4800)
// unreferenced formal parameter
#pragma warning(disable: 4100)
// conditional expression is constant
#pragma warning(disable: 4127)
// function marked as __forceinline not inlined
#pragma warning(disable: 4714)
// decorated name length exceeded, name was truncated
#pragma warning(disable: 4503)
// 'X': This function or variable may be unsafe. Consider using Y instead. To disable deprecation, use _CRT_SECURE_NO_WARNINGS. See online help for details.
#pragma warning(disable: 4996)

#elif (defined(__GNUC__) && !(defined(__INTEL_COMPILER) || defined(__ICL) || defined(__ICC) || defined(__ECC)) \
    && (__GNUC__ * 100 + __GNUC_MINOR__) >= 406) || defined(__clang__)

// Note: clang-cl goes here as well, as it seems to support gcc-style warning control pragmas.

#pragma GCC diagnostic push
// unused parameter 'arg'
//#pragma GCC diagnostic ignored "-Wunused-parameter"

#endif

#endif // !defined(BOOST_FILESYSTEM_ENABLE_WARNINGS)

ext: hpp

max_stars_repo_name: robalexclark/SilveR-Dev

max_issues_repo_name: robalexclark/SilveR-Dev

max_forks_repo_name: robalexclark/SilveR-Dev