text:
// ./visual_odometry -i intrinsics.yml -e extrinsics.yml -s ../data/cropped/ -o ../data/out/ ../data/img_list.xml
#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iomanip>

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

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

#include "opencv2/core/core.hpp"
#include "opencv2/core/operations.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/video/tracking.hpp>

#include "../../../tracker/stereo/stereo_tracker_base.h"
#include "../../../tracker/stereo/stereo_tracker_sim.h"
#include "../../../tracker/stereo/stereo_tracker_libviso.h"
#include "../../../tracker/stereo/stereo_tracker_bfm.h"
#include "../../../tracker/detector/feature_detector_harris_cv.h"
#include "../../../tracker/detector/feature_detector_gftt_cv.h"
#include "../../../tracker/detector/feature_detector_base.h"
#include "../../extern/libviso2/src/viso_stereo.h"
#include "../../extern/libviso2/src/matrix.h"
#include "../../feature_filter.h"
#include "../../math_helper.h"
#include "../../eval_helper.h"
#include "../../format_helper.h"
#include "../../cv_plotter.h"
#include "../../matches.h"
using namespace vo;

// bumblebee dataset
#define CALIB_WIDTH		 640
#define CALIB_HEIGHT		 480
#define MAX_FEATURES		 500

// Lucas Kanade tracking params
// bumblebee dataset
//#define STEREO_BOX_W		 13		// 13
//#define STEREO_BOX_H		 5		// 5 or 3
//#define TRACK_BOX_W		 21 // 21
//#define TRACK_BOX_H		 21 // 21
//#define LK_PYRAMID_LEVEL	 3
//#define BORDER_FILTER		 5
//#define BORDER_MASK_SIZE	 10

#define STEREO_BOX_W		 21		// 13
#define STEREO_BOX_H		 21		// 5 or 3
#define TRACK_BOX_W		 21 // 21
#define TRACK_BOX_H		 21 // 21
#define LK_PYRAMID_LEVEL	 3
#define BORDER_FILTER		 5
#define BORDER_MASK_SIZE	 5

// detector params
#define QUALITY_LVL		 1	  // 0.15
#define MIN_DISTANCE		 4	  // 10
#define BLOCK_SIZE		 3	  // 3
#define USE_HARRIS		 true	  // false
#define HARRIS_K         0.04	  // 0.04

// libviso dataset
//#define STEREO_BOX_W		 11		 // 13
//#define STEREO_BOX_H		 5		// 5 or 3
//#define TRACK_BOX_W		 11 // 21
//#define TRACK_BOX_H		 11 // 21
//#define LK_PYRAMID_LEVEL	 3  // 5
//#define BORDER_FILTER		 5
//#define BORDER_MASK_SIZE	 60

//TODO hardcoded - in future add movement checking through optical flow
// feature tracking params
#define SBA_FRAME_STEP		      2  // 10

// plotting settings
#define SCALE_FACTOR             0.5	       // 0.5, zoom with 2.0
#define CENTER_X		            300
#define CENTER_Y		            200
#define WINDOW_WIDTH		         900
#define WINDOW_HEIGHT		      700
//#define WINDOW_CAMERA_WIDTH	   1000
//#define WINDOW_CAMERA_HEIGHT	   800

using namespace std;
//using namespace cv;

// functions
void getCalibParams(string& intrinsic_filename, string& extrinsic_filename, Mat& P_left, Mat& P_right, Mat& Q,
                    Mat& C_left, Mat &D_left, Mat& C_right, Mat& D_right);

static int printHelp()
{
   cout << "Usage:\n ./visual_odometry -i intrinsics.yml -e extrinsics.yml -s source_folder -o output_folder <image "
           "list XML/YML file>\n" << endl;
   return 0;
}

void printPoint() {}
void printMatch() {}

void writeExtrinsicParams(deque<Mat>& Rt_params, fstream& file)
{
   // Rt for extr params (no cam pose)
   // TODO left or right coord system
   double quat[4];
   //cout << extr_params[0] << "\n -> 0-Rt\n";
   //MathHelper::matToQuat(Rt_params[0], quat);
   //cout << quat << endl;

   std::setprecision(6);

   for(size_t i = 0; i < Rt_params.size(); i++) {
      MathHelper::matToQuat(Rt_params[i], quat);
      for(int j = 0; j < 4; j++) {
         file << quat[j] << " ";
      }
      Mat& Rt = Rt_params[i];
      file << Rt.at<double>(0,3) << " " << Rt.at<double>(1,3) << " " << Rt.at<double>(2,3)<< endl;
   }
   // TODO - y
}

void writeMatRt(Mat& Rt, ofstream& fp)
{
   std::setprecision(6);
   for(int i = 0; i < (Rt.rows-1); i++) {
      for(int j = 0; j < Rt.cols; j++)
         fp << Rt.at<double>(i,j) << " ";
   }
   fp << endl;
   //file << Rt.at<double>(0,3) << " " << Rt.at<double>(1,3) << " " << Rt.at<double>(2,3)<< endl;
}

void writePointProjections(fstream& file, deque<Matches>& features, deque<vector<uchar>>& track_status,
      deque<Mat>& ctw_params, VisualOdometryStereo::parameters& param)
{
   assert(features.size() == SBA_FRAME_STEP && track_status.size() == (features.size()-1));
   //unsigned start = features_left.size() - SBA_FRAME_STEP;
   Mat pt(4, 1, CV_64F);
   double d;
   //   double rerr_left = 0.0;
   //   double rerr_right = 0.0;
   Mat C = Mat::eye(3, 3, CV_64F);
   for(size_t k = 0; k < features.size(); k++) {
      for(size_t i = 0; i < features[k].left_.size(); i++) {
         if(k > 0 && track_status[k-1][i] == 1)
            continue;

         d = max(features[k].left_[i].pt.x - features[k].right_[i].pt.x, 1.f);
         pt.at<double>(0,0) = (features[k].left_[i].pt.x - param.calib.cu) * param.base / d;
         pt.at<double>(1,0) = (features[k].left_[i].pt.y - param.calib.cv) * param.base / d;
         pt.at<double>(2,0) = param.calib.f * param.base / d;
         pt.at<double>(3,0) = 1.0;

         for(int f = k; f >= 0; f--) {
            pt = ctw_params[f] * pt;
         }
         //	 pt = ctw_params[k] * pt;
         //TODO p.y = -p.y
         //assert(pt.at<double>(2,0) < 100.0);

         uint32_t cnt = 1;
         for(unsigned j = k;; j++) {
            if(j == track_status.size())
               break;
            if(track_status[j][i] == 1)
               cnt++;
            else
               break;
         }
         if(cnt == 1)
            continue;
         file << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << " ";
         //cout << p.x << " " << p.y << " " << p.z << " ";

         file << cnt;
         //cout << c;
         for(unsigned j = k; j < (k+cnt); j++) {
            file << " " << j << " " << features[j].left_[i].pt.x << " " << features[j].left_[i].pt.y;
            //cout << " " << j - start << " " << features_left[j][i].pt.x << " " << features_left[j][i].pt.y;
         }
         file << endl;
         //cout << endl;

         // test triangulation
         //	 cout << "start triang. test\n";
         //	 cout << "1. frame in file: " << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "\n";
         //	 for(uint32_t j = k; j < (k+cnt); j++) {
         //	    d = max(features[j].left_[i].pt.x - features[j].right_[i].pt.x, 1.f);
         //	    pt.at<double>(0,0) = (features[j].left_[i].pt.x - param.calib.cu) * param.base / d;
         //	    pt.at<double>(1,0) = (features[j].left_[i].pt.y - param.calib.cv) * param.base / d;
         //	    pt.at<double>(2,0) = param.calib.f * param.base / d;
         //	    pt.at<double>(3,0) = 1.0;
         //	    cout << (j-k)+1 << ". frame: " << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "  -->  ";
         //	    for(int32_t f = j; f >= 0; f--) {
         //	       pt = ctw_params[f] * pt;
         //	    }
         //	    cout << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "\n";
         //	 }
         //	 cout << "end triang. test\n";
      }
   }
   //cout << "reproj error left: " << rerr_left << "\nreproj error right: " << rerr_right << "\n";
}

void writePointProjectionsFromFristFrame(fstream& file, deque<Matches>& features, deque<vector<int32_t>>& track_idx,
                                         deque<Mat>& ctw_params, VisualOdometryStereo::parameters& param)
{
   //   cout << features_left.size() << endl;
   //   assert(features_left.size() == SBA_FRAME_STEP && features_left.size() == features_right.size() && track_status.size() == (features_left.size()-1));
   //unsigned start = features_left.size() - SBA_FRAME_STEP;
   Mat pt(4, 1, CV_64F);
   double d;

   //   for(int32_t f = ctw_params.size()-1; f >= 0; f--) {
   //      cout << f << " frame: " << ctw_params[f] << endl;
   //   }

   Mat C = Mat::eye(3, 3, CV_64F);
   for(size_t k = 0; k <= 0; k++) {
      for(size_t i = 0; i < features[k].left_.size(); i++) {
         if(k > 0 && track_idx[k-1][i] >= 0)
            continue;
         // TODO racunat sa nenormaliziranom znacajkama zbog ciste rektifikacije...?
         d = max(features[k].left_[i].pt.x - features[k].right_[i].pt.x, 1.f);
         pt.at<double>(0,0) = (features[k].left_[i].pt.x - param.calib.cu) * param.base / d;
         pt.at<double>(1,0) = (features[k].left_[i].pt.y - param.calib.cv) * param.base / d;
         pt.at<double>(2,0) = param.calib.f * param.base / d;
         pt.at<double>(3,0) = 1.0;
         for(int32_t f = k; f >= 0; f--) {
            pt = ctw_params[f] * pt;
         }
         //	 pt = ctw_params[k] * pt;
         //TODO p.y = -p.y
         //assert(pt.at<double>(2,0) < 100.0);

         //	 Point3f pt_right(p.x-0.12, p.y, p.z);
         //	 rerr_left += MathHelper::getReprojError(features_left[k][i].pt, p, C);
         //	 rerr_right += MathHelper::getReprojError(features_right[k][i].pt, pt_right, C);

         // TODO
         //	 p.x += extr_params[k][4];
         //	 p.y += extr_params[k][5];
         //	 p.z += extr_params[k][6];
         //cout << "camera system: " << pt << endl;
         //pt = extr_params[k] * pt;
         //cout << "world system: " << pt << endl;
         uint32_t cnt = 1;
         int32_t index = i;
         for(uint32_t j = k;; j++) {
            if(j == track_idx.size())
               break;
            if(track_idx[j][index] >= 0) {
               cnt++;
               index = track_idx[j][index];
            }
            else
               break;
         }
         if(cnt == 1)
            continue;
         file << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << " ";
         //cout << p.x << " " << p.y << " " << p.z << " ";

         file << cnt;
         //cout << c;
         index = i;
         for(uint32_t j = k; j < (k+cnt); j++) {
            if(j > k)
               index = track_idx[j-1][index];
            file << " " << j << " " << features[j].left_[index].pt.x << " " << features[j].left_[index].pt.y;
            //cout << " " << j - start << " " << features_left[j][i].pt.x << " " << features_left[j][i].pt.y;
         }
         file << endl;
         //cout << endl;

         // test triangulation
         //	 index = i;
         //	 cout << "start triang. test\n";
         //	 cout << "1. frame in file: " << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "\n";
         //	 for(uint32_t j = k; j < (k+cnt); j++) {
         //	    if(j > k)
         //	       index = track_idx[j-1][index];
         //
         //	    d = max(features[j].left_[index].pt.x - features[j].right_[index].pt.x, 1.f);
         //	    pt.at<double>(0,0) = (features[j].left_[index].pt.x - param.calib.cu) * param.base / d;
         //	    pt.at<double>(1,0) = (features[j].left_[index].pt.y - param.calib.cv) * param.base / d;
         //	    pt.at<double>(2,0) = param.calib.f * param.base / d;
         //	    pt.at<double>(3,0) = 1.0;
         //	    cout << (j-k)+1 << ". frame: " << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "  -->  ";
         //	    for(int32_t f = j; f >= 0; f--) {
         //	       pt = ctw_params[f] * pt;
         //	    }
         //	    cout << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << "\n";
         //	 }
         //	 cout << "end triang. test\n";
      }
   }
   //cout << "reproj error left: " << rerr_left << "\nreproj error right: " << rerr_right << "\n";
}

void writePointProjections2Frames(fstream& file, deque<vector<KeyPoint>>& features_left, deque<vector<KeyPoint>>& features_right,
      deque<Mat>& extr_inv, VisualOdometryStereo::parameters& param)
{
   assert(features_left.size() >= SBA_FRAME_STEP && features_left.size() == features_right.size());
   //assert(features_left.size() == extr_params.size());
   unsigned start = features_left.size() - SBA_FRAME_STEP;
   Mat pt(4, 1, CV_64F);
   double d;
   Mat C = Mat::eye(3, 3, CV_64F);
   for(unsigned k = start; k < (start+1); k++) {
      for(unsigned i = 0; i < features_left[k].size(); i++) {
         // TODO ovdje dodat sakupljeni pomak od prvog frejma
         // todo racunat sa nenormaliziranom znacajkama zbog ciste rektifikacije...?
         d = max(features_left[k][i].pt.x - features_right[k][i].pt.x, 1.f);
         pt.at<double>(0,0) = (features_left[k][i].pt.x - param.calib.cu) * param.base / d;
         pt.at<double>(1,0) = (features_left[k][i].pt.y - param.calib.cv) * param.base / d;
         pt.at<double>(2,0) = param.calib.f * param.base / d;
         pt.at<double>(3,0) = 1.0;
         //TODO p.y = -p.y
         //assert(pt.at<double>(2,0) < 100.0);

         //	 Point3f pt_right(p.x-0.12, p.y, p.z);
         //	 rerr_left += MathHelper::getReprojError(features_left[k][i].pt, p, C);
         //	 rerr_right += MathHelper::getReprojError(features_right[k][i].pt, pt_right, C);

         // TODO
         //cout << "camera system: " << pt << endl;
         //pt = extr_inv[k-start] * pt;
         //cout << "world system: " << pt << endl;

         file << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << " ";
         //cout << pt.at<double>(0,0) << " " << pt.at<double>(1,0) << " " << pt.at<double>(2,0) << " ";
         unsigned c = 2;
         file << c;
         //cout << c;
         for(unsigned j = k; j < (k+c); j++) {
            file << " " << j - start << " " << features_left[j][i].pt.x << " " << features_left[j][i].pt.y;
            //cout << " " << j - start << " " << features_left[j][i].pt.x << " " << features_left[j][i].pt.y;
         }
         file << endl;
         //cout << endl;
      }
   }
}

// TODO - not direct motion but inverse camera motion matrix?
void readMotion(string filename, double motion_params[7])
{
   ifstream file(filename);

   string line, tmp_line;

   while(std::getline(file, tmp_line)) {
      line = tmp_line;
   }
   stringstream stream(line);
   string val;
   //cout << "LINE: " << line << endl;
   for(int i = 0; i < 7; i++) {
      stream >> val;
      motion_params[i] = std::stod(val);
      //cout << "READ: " << motion_params[i] << endl << endl;
   }

   file.close();
}

void matToKeypoints(Mat& mat, vector<KeyPoint>& keys)
{
   assert(mat.cols == keys.size() && mat.rows == 1 && mat.channels() == 2);
   for(unsigned i = 0; i < keys.size(); i++) {
      keys[i].pt.x = mat.at<Vec2d>(0,i)[0];
      keys[i].pt.y = mat.at<Vec2d>(0,i)[1];
   }
}
void keypointsToMat(vector<KeyPoint>& keys, Mat& mat)
{
   assert(mat.cols == keys.size() && mat.rows == 1 && mat.channels() == 2);
   for(unsigned i = 0; i < keys.size(); i++) {
      mat.at<Vec2d>(0,i)[0] = keys[i].pt.x;
      mat.at<Vec2d>(0,i)[1] = keys[i].pt.y;
   }
}

void cvKeysToVisoMatches(deque<vector<KeyPoint>>& keys_left, deque<vector<KeyPoint>>& keys_right,
      vector<uchar>& status, vector<Matcher::p_match>& matches)
{
   //assert(keys_left.back().size() == keys_right.back.size() && keys_left.back().size() == status.size());
   Matcher::p_match m;

   size_t last = keys_left.size() - 1;
   for(size_t i = 0; i < status.size(); i++) {
      if(status[i] == 1) {
         m.u1p = keys_left[last-1][i].pt.x;
         m.v1p = keys_left[last-1][i].pt.y;
         m.u1c = keys_left[last][i].pt.x;
         m.v1c = keys_left[last][i].pt.y;
         m.u2p = keys_right[last-1][i].pt.x;
         m.v2p = keys_right[last-1][i].pt.y;
         m.u2c = keys_right[last][i].pt.x;
         m.v2c = keys_right[last][i].pt.y;
         matches.push_back(m);
      }
   }
}

void cvKeysToVisoMatches(deque<Matches>& all_features, vector<uchar>& status, vector<Matcher::p_match>& matches)
{
   Matcher::p_match m;

   size_t last = all_features.size() - 1;
   assert(all_features[last-1].left_.size() == status.size());
   for(size_t i = 0; i < status.size(); i++) {
      if(status[i] == 1) {
         m.u1p = all_features[last-1].left_[i].pt.x;
         m.v1p = all_features[last-1].left_[i].pt.y;
         m.u1c = all_features[last].left_[i].pt.x;
         m.v1c = all_features[last].left_[i].pt.y;
         m.u2p = all_features[last-1].right_[i].pt.x;
         m.v2p = all_features[last-1].right_[i].pt.y;
         m.u2c = all_features[last].right_[i].pt.x;
         m.v2c = all_features[last].right_[i].pt.y;
         //cout << m.u1p << " " << m.v1p << endl << m.u1c << " " << m.v1c << endl;
         matches.push_back(m);
      }
   }
}

void convertAllMatchesToKeys(vector<Matcher::p_match>& matches, vector<vector<KeyPoint>>& keypoints)
{
   keypoints.resize(4);
   KeyPoint kp;
   for(size_t i = 0; i < matches.size(); i++) {
      //cout << "match " << i << endl;
      kp.pt.x = matches[i].u1p;
      kp.pt.y = matches[i].v1p;
      keypoints[0].push_back(kp);
      kp.pt.x = matches[i].u2p;
      kp.pt.y = matches[i].v2p;
      keypoints[1].push_back(kp);
      kp.pt.x = matches[i].u1c;
      kp.pt.y = matches[i].v1c;
      keypoints[2].push_back(kp);
      kp.pt.x = matches[i].u2c;
      kp.pt.y = matches[i].v2c;
      keypoints[3].push_back(kp);
   }
}
void convertInlierMatchesToKeys(vector<Matcher::p_match>& matches, vector<int32_t>& inliers, vector<vector<KeyPoint>>& keypoints)
{
   keypoints.resize(4);
   KeyPoint kp;
   for(size_t i = 0; i < inliers.size(); i++) {
      //cout << "match " << i << endl;
      int32_t idx = inliers[i];
      kp.pt.x = matches[idx].u1p;
      kp.pt.y = matches[idx].v1p;
      keypoints[0].push_back(kp);
      kp.pt.x = matches[idx].u2p;
      kp.pt.y = matches[idx].v2p;
      keypoints[1].push_back(kp);
      kp.pt.x = matches[idx].u1c;
      kp.pt.y = matches[idx].v1c;
      keypoints[2].push_back(kp);
      kp.pt.x = matches[idx].u2c;
      kp.pt.y = matches[idx].v2c;
      keypoints[3].push_back(kp);
   }
}
Mat getCameraMatrix(VisualOdometryStereo::parameters& param)
{
   Mat C = Mat::zeros(3, 4, CV_64F);
   C.at<double>(0,0) = param.calib.f;
   C.at<double>(1,1) = param.calib.f;
   C.at<double>(2,2) = 1.0;
   C.at<double>(0,2) = param.calib.cu;
   C.at<double>(1,2) = param.calib.cv;
   return C;
}

void readGpsPoints(string filename, vector<Mat> points)
{
   ifstream file(filename);
   string val;
   points.clear();
   Mat pt(2, 1, CV_64F);
   while(!file.eof()) {
      file >> val;
      pt.at<double>(0,0) = std::stod(val);
      file >> val;
      pt.at<double>(1,0) = std::stod(val);
      cout << pt << endl;
      points.push_back(pt.clone());
      for(int i = 0; i < 9; i++) file >> val;
   }
}

void visualOdometry(vector<string>& imagelist, string& cparams_file, string& source_folder, string& output_folder)
{
   vector<KeyPoint> features_full;
   vector<KeyPoint> features_left;
   vector<KeyPoint> features_right;

   deque<vector<uchar>> track_status;
   deque<Matches> all_features;
   deque<vector<KeyPoint>> features_libviso_left;
   deque<vector<KeyPoint>> features_libviso_right;

   deque<Mat> extr_params; // pose mat
   deque<Mat> Rt_params;

   double cam_params[5];
   FormatHelper::readCameraParams(cparams_file, cam_params);
   VisualOdometryStereo::parameters param;
   param.calib.f = cam_params[0];	 // focal length in pixels
   param.calib.cu = cam_params[2];	 // principal point (u-coordinate) in pixels
   param.calib.cv = cam_params[3];	 // principal point (v-coordinate) in pixels
   param.base = cam_params[4];

   // bumblebee dataset - rectified - lcd calib
   //param.calib.f  = 491.22;	    // focal length
   //param.calib.cu = 328.8657;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 249.43165;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.119744;
   ////param.base	  = 0.5;

   // old lcd
   //param.calib.f  = 491.598;	 // focal length
   //param.calib.cu = 336.437;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 249.277;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.12;

   // bumblebee - alan calib rectified - with opencv
   //param.calib.f  = 492.2473;	 // focal length in pixels
   //param.calib.cu = 333.5488;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 240.6432;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.12043;      // 386.1448 / 718.856

   // bumblebee - alan calib rectified - original from pdf - i dont have calib data, only rectif data
// param.calib.f = 425.3849;
// param.calib.cu = 325.9;
// param.calib.cv = 244.54275;
// param.base = 0.12;

   // libviso datasets: 00, 14
   //param.calib.f  = 718.856;	 // focal length in pixels
   //param.calib.cu = 607.1928;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 185.2157;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.53716;      // 386.1448 / 718.856

   // libviso 11 dataset
   //param.calib.f  = 707.0912;	 // focal length in pixels
   //param.calib.cu = 601.8873;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 183.1104;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.53715;      // 386.1448 / 718.856
   // optional
   //   param.inlier_threshold = 1.5;       // 1.5
   //   param.ransac_iters = 200;	       // 200
   //   param.bucket.bucket_height = 50;    // 50
   //   param.bucket.bucket_width = 50;     // 50
   //param.bucket.max_features = 6;      // 2

   //Image nullimg;
   string simdata_folder = "/home/kivan/projects/datasets/stereo_model/points_kitti_cam_nonoise_base_0.50/";
   string simdata_xml = "/home/kivan/projects/datasets/stereo_model/stereosim_viso00path.xml";
   Mat mask = imread("/home/kivan/Projects/project-vista/config_files/mask_kitti_rect.png", CV_LOAD_IMAGE_GRAYSCALE);
   FeatureDetectorBase* detector = new FeatureDetectorGFTTCV(5000, 0.0001, 1, 3, true, 0.04, mask);
   //FeatureDetectorBase* detector = new FeatureDetectorHarrisCV(10, 10, 10, 3, 1, 0.04);
   StereoTrackerBase* tracker = new StereoTrackerBFM(detector, 2000, 0.9);
   //StereoTrackerBase* tracker = new StereoTrackerSim(simdata_folder, simdata_xml);
   //StereoTrackerBase* tracker = new StereoTrackerLibviso(param);
   // init visual odometry
   VisualOdometryStereo viso(param, tracker);
   // LIBVISO end

   //Mat P_left, P_right, Q, C_left, D_left, C_right, D_right;
   //getCalibParams(intrinsic_filename, extrinsic_filename, P_left, P_right, Q, C_left, D_left, C_right, D_right);

   //	Mat descriptors_1, descriptors_2;
   Mat img_left_prev = imread(source_folder + imagelist[0], CV_LOAD_IMAGE_GRAYSCALE);
   Mat img_right_prev = imread(source_folder + imagelist[1], CV_LOAD_IMAGE_GRAYSCALE);

   //TODO try histo eq;

   Matrix pose = Matrix::eye(4);
   Matrix viso_cvtrack = Matrix::eye(4);
   //Matrix point_rt = Matrix::eye(4);
   Mat Rt_inv;
   Mat mat_I = Mat::eye(4, 4, CV_64F);
   mat_I.copyTo(Rt_inv);
   Vec<double,7> trans_vec;
   extr_params.push_back(mat_I.clone());
   Mat Rt(4, 4, CV_64F);
   Mat Rt_gt(4, 4, CV_64F);
   Mat Rt_gt_prev = Mat::eye(4, 4, CV_64F);
   Mat Rt_gt_curr = Mat::eye(4, 4, CV_64F);
   MathHelper::invTrans(Rt_inv, Rt);
   Rt_params.push_back(Rt.clone());
   cout << Rt << endl;
   cout << Rt_inv << endl;

   Mat prev_location_viso = Mat::zeros(4, 1, CV_64F);
   Mat prev_location_viso_cvtrack = Mat::zeros(4, 1, CV_64F);
   Mat prev_location_sba = Mat::zeros(4, 1, CV_64F);

   int32_t dims[] = {img_left_prev.cols, img_left_prev.rows, img_left_prev.cols};
   // init the tracker
   if(!viso.process(img_left_prev.data, img_right_prev.data, dims))
      cout << "init frame - no estimation" << endl;

   Matches matches(features_left, features_right);

   Mat disp_libviso = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_libviso_cvtracker = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_libviso_birch = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_sba = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_camera = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_camera_right = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);

   imshow("libviso_orig", disp_libviso);
   //imshow("libviso_cvtracker", disp_libviso_cvtracker);
   //imshow("SBA", disp_sba);
   //moveWindow("libviso_orig", 0, 0);
   //moveWindow("libviso_cvtracker", 682, 0);

   // for SBA
   int frame = 2;
   string extr_filename = "cam_extr.txt";
   string points_filename = "cam_points.txt";
   fstream extr_file;
   fstream points_file;
   //   vector<Mat> gps_pts;
   //   readGpsPoints("gps_route.txt", gps_pts);

   Mat Rt_sba_path = Mat::eye(4, 4, CV_64F);	   // sba extrinsic params in whole scene path
   Mat Rt_sba = Mat::eye(4, 4, CV_64F);		   // sba output extrinsic params
   Mat pose_sba = Mat::eye(4, 4, CV_64F);
   Mat pose_libviso = Mat::eye(4, 4, CV_64F);   // matrix transforms points in current camera coordinate system to world coord system
   CvPlotter plotter(WINDOW_WIDTH, WINDOW_HEIGHT, SCALE_FACTOR , CENTER_X, CENTER_Y);

   ifstream groundtruth_file("/home/kivan/Projects/datasets/KITTI/poses/00.txt");
   ofstream reprojerr_file("stereo_reproj_error.txt");
   ofstream libviso_ptsfile("viso_points.txt");
   writeMatRt(pose_libviso, libviso_ptsfile);

   // -- demo start
   //string left_out_folder = "/home/kivan/projects/master_thesis/demo/libviso_demo/left_cam/";
   //string right_out_folder = "/home/kivan/projects/master_thesis/demo/libviso_demo/right_cam/";
   //std::ostringstream frame_num;
   //frame_num << std::setw(6) << std::setfill('0') << 0;
   //cout << frame_num.str() << "\n";
   //imwrite(left_out_folder + "img_left_" + frame_num.str() + ".jpg", img_left_prev);
   //imwrite(right_out_folder + "img_right_" + frame_num.str() + ".jpg", img_right_prev);
   // -- demo end

   for(unsigned i = 2; i < imagelist.size(); i+=(2)) {
      cout << source_folder + imagelist[i] << endl;
      Mat img_left = imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
      Mat img_right = imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);
      cout << i/2 << " frame:" << endl;

      if(viso.process(img_left.data, img_right.data, dims)) {
         vector<Matcher::p_match>& libviso_features = viso.getFeatures();
         vector<int32_t>& inliers = viso.getInliers();
//         double reproj_err = MathHelper::getReprojError(features_libviso_left, features_libviso_right, param.base, C, Rt);
//         cout << "reproj error: " << reproj_err << endl;

         ////	 cout << "libviso features size: " << viso.p_matched.size() << endl;
         vector<vector<KeyPoint>> matches, matches_all;
         convertInlierMatchesToKeys(libviso_features, inliers, matches);
         //convertAllMatchesToKeys(libviso_features, matches_all);

         // SBA begin
         //features_libviso_left.clear();
         //features_libviso_right.clear();
         //features_libviso_left.push_back(matches[0]);
         //features_libviso_right.push_back(matches[1]);
         //features_libviso_left.push_back(matches[2]);
         //features_libviso_right.push_back(matches[3]);
         // SBA end

         Mat disp_allfeats;
         vector<uchar> viso_status(matches[0].size(), 1);
         cvtColor(img_left_prev, disp_camera, COLOR_GRAY2RGB);
         cvtColor(img_left_prev, disp_allfeats, COLOR_GRAY2RGB);
         FeatureFilter::drawOpticalFlow(disp_camera, matches[0], matches[2], viso_status, Scalar(0,0,255));
         //FeatureFilter::drawOpticalFlow(disp_allfeats, matches_all[0], matches_all[2], viso_status, Scalar(0,0,255));
         imshow("camera_left", disp_camera);
         //imshow("camera_left_all", disp_allfeats);
         //moveWindow("camera left", 400, 200);

         // save imgs for demo
         // -- demo start
         //cvtColor(img_right, disp_camera_right, COLOR_GRAY2RGB);
         //FeatureFilter::drawOpticalFlow(disp_camera_right, matches[1], matches[3], viso_status, Scalar(0,0,255));
         //frame_num.str("");
         //frame_num << std::setw(6) << std::setfill('0') << (i/2);
         //imwrite(left_out_folder + "img_left_" + frame_num.str() + ".jpg", disp_camera);
         //imwrite(right_out_folder + "img_right_" + frame_num.str() + ".jpg", disp_camera_right);
         // -- demo end

         // on success, update current pose
         // TODO try without inverse
         pose = pose * Matrix::inv(viso.getMotion());
         //cout << pose << endl << endl;
         MathHelper::matrixToMat(pose, pose_libviso);
         writeMatRt(pose_libviso, libviso_ptsfile);
         //viso_ptsfile << ~pose.extractCols(std::vector<int>(1,3)) << endl;

         //	 cout << "point_rt - rt * viso.getMotion:\n" << point_rt_mat << endl;
         //TODO - why Rt and point_rt not excatly the same? because of Matrix::inv?
         Matrix Rt_inv_libviso = Matrix::inv(viso.getMotion());
         Matrix Rt_libviso = viso.getMotion();
         MathHelper::matrixToMat(Rt_inv_libviso, Rt_inv);
         MathHelper::matrixToMat(Rt_libviso, Rt);
         //MathHelper::invTrans(Rt, Rt_inv);    // better
         extr_params.push_back(Rt_inv.clone());

         // output some statistics
         vector<core::Point> points_lp, points_rp, points_lc, points_rc;
         FeatureHelper::LibvisoInliersToPoints(libviso_features, inliers, points_lp, points_rp, points_lc, points_rc);
         //cout << points_lp.size() << ", " << points_rp.size() << ", " << points_lc.size() << ", " << points_rc.size() << endl;
         Mat C = getCameraMatrix(param);
         //cout << Rt_inv << "\n";

         FormatHelper::readNextMatrixKitti(groundtruth_file, Rt_gt_curr);
         Mat Rt_gt_inv = Rt_gt_prev * Rt_gt_curr;
         //cout << Rt_gt_inv << endl;
         MathHelper::invTrans(Rt_gt_inv, Rt_gt);
         //Rt_gt = Rt_gt_inv;
         cout << "Groundtruth:\n" << Rt_gt << endl;
         cout << "Odometry:\n" << Rt << endl;
         cout << "-------------------------------\n";
         MathHelper::invTrans(Rt_gt_curr, Rt_gt_prev);
         double reproj_error = EvalHelper::getStereoReprojError(points_lp, points_rp, points_lc, points_rc, C,
                                                                Rt, param.base);
         double reproj_error_gt = EvalHelper::getStereoReprojError(points_lp, points_rp, points_lc, points_rc, C,
                                                                   Rt_gt, param.base);
         cout << "reprojection error (libviso): " << reproj_error << "\n";
         cout << "reprojection error (groundtruth): " << reproj_error_gt << "\n";
         cout << "-----------------------------------------------------------------------\n";
         reprojerr_file << reproj_error << "\n";
         if(reproj_error > reproj_error_gt) {
            cout << "Found better for GT!!!\n";
            //return;
         }

         double num_matches = viso.getNumberOfMatches();
         double num_inliers = viso.getNumberOfInliers();
         cout << "Matches: " << num_matches;
         cout << ", Inliers: " << num_inliers << " -> " << 100.0*num_inliers/num_matches << " %" << endl;
         Mat location_viso(pose_libviso, Range(0,4), Range(3,4)); // extract 4-th column
         //cout << location_viso << endl;
         //Matrix location = ~point_rt.extractCols(std::vector<int>(1,3));
         //for(int k = 0; k < 3; k++) location.val[0][k] = - location.val[0][k];
         //cout << point_rt * ~location << endl;
         //cout << location << endl << endl;

         // drawing
         //drawLine(disp_libviso, Point(100.5,100.5), Point(500,500));
         //Point pt1 = coordToPoint(prev_location, SCALE_FACTOR);
         //Point pt2 = coordToPoint(location, SCALE_FACTOR);
         plotter.drawLine(prev_location_viso, location_viso, disp_libviso);
         imshow("libviso_orig", disp_libviso);
         waitKey(10);
         //cout << pose << endl << endl;
         //cout << endl << location.val[0][3] << endl;

         location_viso.copyTo(prev_location_viso);
      } else {
         cout << "libviso ... failed!" << endl;
         waitKey(0);
         //writeMatRt(pose_libviso, libviso_ptsfile);
         //extr_params.push_back(Rt_inv.clone());
         exit(1);
      }

      // BEGIN SBA
      //Rt_params.push_back(Rt.clone());

      //if(frame >= SBA_FRAME_STEP) {
      //   extr_file.open(extr_filename, std::ios_base::out | std::fstream::trunc);
      //   writeExtrinsicParams(Rt_params, extr_file);
      //   extr_file.close();

      //   points_file.open(points_filename, std::ios_base::out | std::fstream::trunc);
      //   deque<vector<uchar>> status_viso;
      //   vector<uchar> s_viso(features_libviso_left[0].size(), 1);
      //   status_viso.push_back(s_viso);

      //   //writePointProjectionsFromFristFrame(points_file, all_features, track_idx, extr_params, param);
      //   //writePointProjections(points_file, all_features, track_status, extr_params, param);
      //   //writePointProjections(points_file, features_libviso_left, features_libviso_right, status_viso, extr_params, param);
      //   writePointProjections2Frames(points_file, features_libviso_left, features_libviso_right, extr_params, param);
      //   //writePointProjectionsDebug(points_file, all_features_left, all_features_right, track_status, extr_params, imagelist, source_folder);
      //   points_file.close();

      //   pid_t pid = fork();
      //   if(pid >= 0) {
      //      // child
      //      if(pid == 0) {
      //         //execl("eucsbademo", "eucsbademo", "cam_extr.txt", "cam_points.txt", "C_left_cam.txt", "output.txt", (char*)NULL);
      //         //const char* camera_mat = normalized_points ? "C_normal.txt" : "C_left_cam.txt";
      //         //const char* camera_mat = "C_libviso_00.txt";
      //         const char* camera_mat = "C_bb.txt"; // TODO - why not better
      //         cout << "Using camera matrix: " << camera_mat << endl;
      //         execl("eucsbademo_mot", "eucsbademo_mot", "cam_extr.txt", "cam_points.txt", camera_mat, "output.txt", (char*)NULL);
      //      }

      //      //execl("eucsbademo", "eucsbademo", "cam_extr.txt", "cam_points.txt", "cam_intr.txt", "output.txt", (char*)NULL);
      //      // parent
      //      else
      //         wait(NULL);
      //   }
      //   else {
      //      cout << "fork failed... exiting." << endl;
      //      exit(-1);
      //   }

      //   if(frame == SBA_FRAME_STEP)
      //      plotter.drawFirstFrames("output.txt", prev_location_sba, disp_sba);

      //   double motion_params[7];
      //   readMotion("output.txt", motion_params);
      //   // TODO ovo i za libviso
      //   MathHelper::quatToMat(motion_params, Rt_sba);
      //   Rt_sba_path = Rt_sba_path * Rt_sba;
      //   //	 cout << "viso Rt:\n" << Rt << endl;
      //   //	 cout << "sba Rt:\n" << Rt_sba << endl;

      //   //Mat pose_sba = Rt_sba_path.inv();
      //   Mat pose_sba_tmp;
      //   MathHelper::invTrans(Rt_sba, pose_sba_tmp);
      //   pose_sba = pose_sba * pose_sba_tmp;
      //   Mat location_sba(4, 1, CV_64F);
      //   location_sba.at<double>(0,0) = pose_sba.at<double>(0,3);
      //   location_sba.at<double>(1,0) = pose_sba.at<double>(1,3);
      //   location_sba.at<double>(2,0) = pose_sba.at<double>(2,3);

      //   // isto kao iznad sa pose_sba?
      //   //	 location_sba.at<double>(0,0) = Rt_sba_path.at<double>(0,3);
      //   //	 location_sba.at<double>(1,0) = -Rt_sba_path.at<double>(1,3);
      //   //	 location_sba.at<double>(2,0) = -Rt_sba_path.at<double>(2,3);

      //   //	 Point pt1 = coordToPoint(prev_location_sba, SCALE_FACTOR);
      //   //	 Point pt2 = coordToPoint(location_sba, SCALE_FACTOR);
      //   plotter.drawLine(prev_location_sba, location_sba, disp_sba);
      //   //imshow("Libviso", disp_libviso);
      //   imshow("SBA", disp_sba);
      //   waitKey(10);
      //   location_sba.copyTo(prev_location_sba);
      //   // calculate and print reproj errors
      //   Mat C = getCameraMatrix(param);
      //   double err_viso_l, err_viso_r, err_sba_l, err_sba_r;
      //   MathHelper::getReprojError(features_libviso_left, features_libviso_right, param.base, C, Rt, err_viso_l, err_viso_r);
      //   MathHelper::getReprojError(features_libviso_left, features_libviso_right, param.base, C, Rt_sba, err_sba_l, err_sba_r);
      //   cout << "Reprojection error (libviso):\n" << "left: " << err_viso_l << "\nright: " << err_viso_r << endl;
      //   cout << "Reprojection error (sba):\n" << "left: " << err_sba_l << "\nright: " << err_sba_r << endl;

      //   //all_features.pop_front();
      //   //track_status.pop_front();
      //   Rt_params.pop_front();
      //   Rt_params[0] = mat_I.clone();
      //   extr_params.pop_front();
      //   //extr_params.pop_front();
      //   //extr_params.push_front(mat_I.clone());
      //   extr_params[0] = mat_I.clone();
      //}
      // END SBA
      //

      frame++;

      cv::swap(img_left_prev, img_left);
      cv::swap(img_right_prev, img_right);
      //		img_left_prev.deallocate();
      //		img_left_prev = std::move(img_left);
      //		img_right_prev.deallocate();
      //		img_right_prev = std::move(img_right);
   }
   groundtruth_file.close();
   reprojerr_file.close();
   libviso_ptsfile.close();
   waitKey(0);
}

void getCalibParams(String& intrinsic_filename, String& extrinsic_filename, Mat& P_left,
                    Mat& P_right, Mat& Q, Mat& C_left, Mat& D_left, Mat& C_right, Mat& D_right)
{
   // reading intrinsic parameters
   FileStorage fs(intrinsic_filename, CV_STORAGE_READ);
   if(!fs.isOpened())
   {
      cout << "Failed to open file " << intrinsic_filename << endl;
      return;
   }

   //Mat M1, D1, M2, D2;
   fs["M1"] >> C_left;
   fs["D1"] >> D_left;
   fs["M2"] >> C_right;
   fs["D2"] >> D_right;
   Mat R, T, R1, R2;
   fs["R"] >> R;
   fs["T"] >> T;

   cv::Size imageSize(CALIB_WIDTH, CALIB_HEIGHT);
   stereoRectify(C_left, D_left, C_right, D_right, imageSize, R, T, R1, R2, P_left, P_right, Q, CALIB_ZERO_DISPARITY, -1,
         imageSize);
   return;
}

inline static bool readStringList(const string& filename, vector<string>& l)
{
   l.resize(0);
   FileStorage fs(filename, FileStorage::READ);
   if( !fs.isOpened() )
      return false;
   FileNode n = fs.getFirstTopLevelNode();
   if( n.type() != FileNode::SEQ )
      return false;
   FileNodeIterator it = n.begin(), it_end = n.end();
   for( ; it != it_end; ++it )
      l.push_back((string)*it);
   return true;
}

int main(int argc, char	** argv)
{
   string config_file;
   string imagelistfn;
   string cam_params_file;
   string source_folder;
   string output_folder;

   try {
      po::options_description generic("Generic options");
      generic.add_options()
         ("help", "produce help message")
         ("config,c", po::value<string>(&config_file)->default_value("config.txt"), "config filename")
         ;
      po::options_description config("Config file options");
      config.add_options()
         ("camera_params,p", po::value<string>(&cam_params_file)->default_value("camera_params.txt"), "camera params file")
         ("source_folder,s", po::value<string>(&source_folder), "folder with source")
         ("output_folder,o", po::value<string>(&output_folder), "folder for output")
         ("imglist,l", po::value<string>(&imagelistfn), "file with image list")
         ;

      po::options_description cmdline_options;
      cmdline_options.add(generic).add(config);

      po::options_description config_file_options;
      config_file_options.add(config);
      po::variables_map vm;
      //po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
      po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
      notify(vm);
      if(vm.count("help")) {
         cout << generic;
         cout << config;
         return 0;
      }

      ifstream ifs(config_file.c_str());
      if (!ifs) {
         cout << "can not open config file: " << config_file << "\n";
         cout << generic;
         cout << config;
         return 0;
      }
      else {
         po::store(parse_config_file(ifs, config_file_options), vm);
         notify(vm);
      }
      cout << "Configuring done, using:" << endl;

      if(vm.count("camera_params")) {
         cout << "Camera params: ";
         cout << cam_params_file << endl;
      }
      if(vm.count("source_folder")) {
         cout << "Source folder: ";
         cout << source_folder << endl;
      }
      if(vm.count("output_folder")) {
         cout << "Output folder: ";
         cout << output_folder << endl;
      }
      if(vm.count("imglist")) {
         cout << "Image list file: ";
         cout << imagelistfn << endl;
      }
   }
   catch(std::exception& e) {
      cout << e.what() << "\n";
      return 1;
   }

   if(imagelistfn == "")
   {
      cout << "error: no xml image list given." << endl;
      return printHelp();
   }
   if(output_folder == "" || source_folder == "")
   {
      cout << "error: no output or source folder given." << endl;
      return printHelp();
   }

   vector<string> imagelist;
   bool ok = readStringList(imagelistfn, imagelist);
   if(!ok || imagelist.empty())
   {
      cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
      return printHelp();
   }

   visualOdometry(imagelist, cam_params_file, source_folder, output_folder);

   return 0;
}

ext: cc


text:
// -*- C++ -*-

//---------------------------------------------------------------------
//  Created:            Thursday, September 11, 2013
//  Original author:    Alexander Samoilov
//---------------------------------------------------------------------

#include "elliptic_integral.hpp"

#if defined PRINT_ME

#include <iostream>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>

int main()
{
    unsigned nsteps = 42;
    double dx = 1.0 / nsteps;
    for (unsigned i = 1; i < nsteps; ++i) {
        double rk = i * dx;
        double f, e;
        elliptic_integral(rk, f, e);
        double bf = boost::math::ellint_1(sqrt(rk));
        double be = boost::math::ellint_2(sqrt(rk));
        std::cout << "rk: " << rk << " f: " << f << " bf: " << bf << " f/bf: " << f/bf << " e: " << e << " be: " << be << std::endl;
    }
}

#endif

ext: cpp

max_stars_repo_name: alsam/cpp-samples

max_issues_repo_name: alsam/cpp-samples

max_forks_repo_name: alsam/cpp-samples


text:
/**
 * Detect collisions between different geometry.
 * Includes continuous collision detection to compute the time of impact.
 * Supported geometry: point vs edge
 */

#pragma once

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/broad_phase/hash_grid.hpp>

#include <ccd/ccd.hpp>
#include <ccd/impact.hpp>
#include <utils/eigen_ext.hpp>

namespace ipc::rigid {

///////////////////////////////////////////////////////////////////////////////
// Broad-Phase CCD
///////////////////////////////////////////////////////////////////////////////

/// @brief Use broad-phase method to create a set of candidate collisions.
void detect_collision_candidates_linear(
    const RigidBodyAssembler& bodies,
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const int collision_types,
    Candidates& candidates,
    DetectionMethod method,
    const double inflation_radius = 0.0);

/// @brief Use broad-phase method to create a set of candidate collisions.
void detect_collision_candidates(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::MatrixXi& edges,
    const Eigen::MatrixXi& faces,
    const Eigen::VectorXi& group_ids,
    const int collision_types,
    Candidates& candidates,
    DetectionMethod method,
    const double inflation_radius = 0.0);

/**
 * @brief Use a brute force method to create a set of all candidate collisions.
 *
 * @param[in] vertices     The vertices of the bodies.
 * @param[in] edges        The edges of the bodies defined as pairs of indices
 *                         into the rows of the vertices matrix. Each row is an
 *                         edge.
 * @param[in] group_ids    If two vertices share a group they are not considered
 *                         possible collisions.
 * @param[out] candidates  Vectors of candidates to build.
 */
void detect_collision_candidates_brute_force(
    const Eigen::MatrixXd& vertices,
    const Eigen::MatrixXi& edges,
    const Eigen::MatrixXi& faces,
    const Eigen::VectorXi& group_ids,
    const int collision_types,
    Candidates& candidates);

/**
 * @brief Use a hash grid method to create a set of all candidate collisions.
 *
 * @param[in] vertices_t0     The vertices of the bodies at the start of the
 *                            time-step.
 * @param[in] vertices_t1     The vertices of the bodies at the end of the
 *                            time-step.
 * @param[in] edges          The edges of the bodies defined as pairs of indices
 *                           into the rows of the vertices matrix. Each row is
 *                           an edge.
 * @param[in] group_ids      If two vertices share a group they are not
 *                           considered possible collisions.
 * @param[out] candidates    Vector of candidates to build.
 */
void detect_collision_candidates_hash_grid(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::MatrixXi& edges,
    const Eigen::MatrixXi& faces,
    const Eigen::VectorXi& group_ids,
    const int collision_types,
    Candidates& candidates,
    const double inflation_radius = 0.0);

/// @brief Use a BVH to create a set of all candidate collisions.
void detect_collision_candidates_linear_bvh(
    const RigidBodyAssembler& bodies,
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const int collision_types,
    Candidates& candidates,
    const double inflation_radius = 0.0);

///////////////////////////////////////////////////////////////////////////////
// Helper functions
///////////////////////////////////////////////////////////////////////////////

inline AABB vertex_aabb(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const size_t vi,
    double inflation_radius)
{
    return AABB(
        V0.row(vi).cwiseMin(V1.row(vi)).array() - inflation_radius,
        V0.row(vi).cwiseMax(V1.row(vi)).array() + inflation_radius);
}

inline AABB edge_aabb(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::Vector2i& E,
    double inflation_radius)
{
    return AABB(
        vertex_aabb(V0, V1, E(0), inflation_radius),
        vertex_aabb(V0, V1, E(1), inflation_radius));
}

inline AABB face_aabb(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::Vector3i& F,
    double inflation_radius)
{
    return AABB(
        vertex_aabb(V0, V1, F(0), inflation_radius),
        AABB(
            vertex_aabb(V0, V1, F(1), inflation_radius),
            vertex_aabb(V0, V1, F(2), inflation_radius)));
}

void detect_collision_candidates_linear_bvh(
    const RigidBodyAssembler& bodies,
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const int collision_types,
    const size_t bodyA_id,
    const size_t bodyB_id,
    Candidates& candidates,
    const double inflation_radius);

} // namespace ipc::rigid

ext: hpp

max_stars_repo_name: ipc-sim/rigid-ipc

max_issues_repo_name: ipc-sim/rigid-ipc

max_forks_repo_name: ipc-sim/rigid-ipc


text:
#ifndef RAY_TRACING_RENDERER_HPP
#define RAY_TRACING_RENDERER_HPP

#include <algorithm>
#include <memory>
#include <string>
#include <iomanip>
#include <iostream>
#include <utility>
#include <vector>

#include <Eigen/Core>

#include <tbb/tbb.h>
#include <tbb/parallel_for.h>

#include "camera.hpp"
#include "image.hpp"
#include "log.hpp"
#include "projection.hpp"
#include "ray.hpp"
#include "renderer.hpp"
#include "scene.hpp"
#include "viewport.hpp"

class RayTracingRenderer : public Renderer {
  public:
    RayTracingRenderer(std::shared_ptr<Viewport> viewport, std::shared_ptr<Projection> projection, std::shared_ptr<Camera> camera);
    void animate(Scene scene, double duration, double step, std::string prefix, ProjectionType projectionType);
    Image render(Scene scene, ProjectionType projectionType);

  private:
    void updateCamera(double step);
    double cameraRadius {2};
    double cameraXZAngle {M_PI_2};
    double cameraAngularVelocity {M_PI/5};

    std::string formatFrameName(std::string prefix, int frameNumber);

    Ray generateRay(int x, int y, ProjectionType projectionType);
    Ray updateRay(Scene scene, Ray ray);

    std::shared_ptr<Color> calculatePixelColor(Scene scene, Ray ray);
    bool calculateSpherePixelColor(std::shared_ptr<Sphere> sphere, Ray ray, double t);
    std::pair<std::shared_ptr<Sphere>, double> findIntersectedBlackHole(Scene scene, Ray ray);
    std::pair<std::shared_ptr<Sphere>, double> findIntersectedStar(Scene scene, Ray ray);
    std::pair<std::shared_ptr<Disk>, double> findIntersectedAccretionDisk(Scene scene, Ray ray);

    std::shared_ptr<Viewport> viewport;
    std::shared_ptr<Projection> projection;
    std::shared_ptr<Camera> camera;

    double timeStep {1};
    int numIterations {100};
};

#endif // RAY_TRACING_RENDERER_HPP

max_stars_repo_name: matheusportela/black-hole-ray-tracer

max_issues_repo_name: matheusportela/black-hole-ray-tracer

max_forks_repo_name: matheusportela/black-hole-ray-tracer


text:
#include "HecubaSession.h"
#include "ArrayDataStore.h"
#include "SpaceFillingCurve.h"
#include "IStorage.h"
#include "yaml-cpp/yaml.h"
#include "ObjSpec.h"
#include "DataModel.h"

#include <cstdlib>
#include <vector>
#include <string>
#include <sstream>
#include <bits/stdc++.h>

#include <iostream>

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>

//#include "numpy/arrayobject.h" // FIXME to use the numpy constants NPY_*
#define NPY_ARRAY_C_CONTIGUOUS    0x0001
#define NPY_ARRAY_F_CONTIGUOUS    0x0002
#define NPY_ARRAY_OWNDATA         0x0004
#define NPY_ARRAY_FORCECAST       0x0010
#define NPY_ARRAY_ENSURECOPY      0x0020
#define NPY_ARRAY_ENSUREARRAY     0x0040
#define NPY_ARRAY_ELEMENTSTRIDES  0x0080
#define NPY_ARRAY_ALIGNED         0x0100
#define NPY_ARRAY_NOTSWAPPED      0x0200
#define NPY_ARRAY_WRITEABLE       0x0400
#define NPY_ARRAY_UPDATEIFCOPY    0x1000

// This code adds the decoding of YAML files to ObjSpec (NOT the other way around)
namespace YAML {
    template<>
        struct convert<DataModel::datamodel_spec> {
            static Node encode(const DataModel::datamodel_spec& obj) {
                Node node;

                return node;
            }
            static std::string mapUserType(std::string name) {
                /* Translates 'user type' to 'data model type'. numpy.ndarray --> hecuba.hnumpy.StorageNumpy */
                if ((name == "numpy.ndarray") || (name == "StorageNumpy")) {
                    return "hecuba.hnumpy.StorageNumpy";
                }
                name = ObjSpec::yaml_to_cass(name);
                return name;
            }
            static bool decode(const Node& node, DataModel::datamodel_spec& dmodel) {
                ObjSpec::valid_types obj_type;
                std::vector<std::pair<std::string, std::string>> partitionKeys;
                std::vector<std::pair<std::string, std::string>> clusteringKeys;
                std::vector<std::pair<std::string, std::string>> cols;
                std::string pythonString="";

                std::string attrName, attrType;
                if(!node.IsMap()) { return false; }

                const Node typespec = node["TypeSpec"];
                if (!typespec.IsSequence() || (typespec.size() != 2)) { return false; }
                dmodel.id=typespec[0].as<std::string>();
                pythonString="class " + dmodel.id;

                if (typespec[1].as<std::string>() == "StorageDict") {
                    pythonString="from hecuba import StorageDict\n\n" + pythonString;
                    obj_type=ObjSpec::valid_types::STORAGEDICT_TYPE;
                    const Node keyspec =  node["KeySpec"];
                    if (!keyspec.IsSequence() || (keyspec.size() == 0)) { return false; }

                    pythonString+=" (StorageDict):\n   '''\n   @TypeSpec dict <<";

                    if (!keyspec[0].IsSequence() || (keyspec[0].size() != 2)) { return false; }

                    attrName=keyspec[0][0].as<std::string>();
                    attrType=keyspec[0][1].as<std::string>();
                    partitionKeys.push_back(std::pair<std::string,std::string>(attrName,mapUserType(attrType)));
                    pythonString+=attrName+":"+attrType;

                    for (uint32_t i=1; i<keyspec.size();i++) {
                        if (!keyspec[i].IsSequence() || (keyspec[i].size() != 2)) { return false; }

                        attrName=keyspec[i][0].as<std::string>();
                        attrType=keyspec[i][1].as<std::string>();
                        clusteringKeys.push_back(std::pair<std::string,std::string>(attrName,mapUserType(attrType)));
                        pythonString+=","+attrName+":"+attrType;
                    }
                    pythonString+=">";
                    const Node valuespec =  node["ValueSpec"];
                    if (!valuespec.IsSequence() || (valuespec.size() == 0)) { return false; }
                    for (uint32_t i=0; i<valuespec.size();i++) {
                        if (!valuespec[i].IsSequence() || (valuespec[i].size() != 2)) { return false; }
                        attrName=valuespec[i][0].as<std::string>();
                        attrType=valuespec[i][1].as<std::string>();
                        cols.push_back(std::pair<std::string,std::string>(attrName,mapUserType(attrType)));
                        pythonString+=","+attrName+":"+attrType;
                    }
                    pythonString+=">\n   '''";
                } else if (typespec[1].as<std::string>() == "StorageObject") {
                    pythonString="from hecuba import StorageObj\n\n" + pythonString;
                    pythonString=pythonString+" (StorageObj):\n   '''\n";
                    obj_type=ObjSpec::valid_types::STORAGEOBJ_TYPE;
                    const Node classfields =  node["ClassField"];
                    if (!classfields.IsSequence() || (classfields.size() == 0)) { return false; }
                    partitionKeys.push_back(std::pair<std::string,std::string>("storage_id","uuid"));

                    for (uint32_t i=0; i<classfields.size();i++) {
                        if (!classfields[i].IsSequence() || (classfields[i].size() != 2)) { return false; }
                        attrName=classfields[i][0].as<std::string>();
                        attrType=mapUserType(classfields[i][1].as<std::string>());
                        cols.push_back(std::pair<std::string,std::string>(attrName,attrType));
                        pythonString+="   @ClassField "+attrName+" "+attrType+"\n";
                        std::cout<<"7-"<<i<<": "<<pythonString<<std::endl;
                    }
                    pythonString+="   '''\n";
                    std::cout<<"8: "<<pythonString<<std::endl;

                } else return false;

                dmodel.o=ObjSpec(obj_type,partitionKeys,clusteringKeys,cols,pythonString);
                return true;
            }
        };
};

void HecubaSession::parse_environment(config_map &config) {
    const char * nodePort = std::getenv("NODE_PORT");
    if (nodePort == nullptr) {
        nodePort = "9042";
    }
    config["NODE_PORT"] = std::string(nodePort);

    const char * contactNames = std::getenv("CONTACT_NAMES");
    if (contactNames == nullptr) {
        contactNames = "127.0.0.1";
    }
    config["CONTACT_NAMES"] = std::string(contactNames);

    const char * createSchema = std::getenv("CREATE_SCHEMA");
    std::string createSchema2 ;
    if (createSchema == nullptr) {
        createSchema2 = std::string("true");
    } else {
        createSchema2 = std::string(createSchema);
        std::transform(createSchema2.begin(), createSchema2.end(), createSchema2.begin(),
                [](unsigned char c){ return std::tolower(c); });
    }
    config["CREATE_SCHEMA"] = createSchema2;

    const char * executionName = std::getenv("EXECUTION_NAME");
    if (executionName == nullptr) {
        executionName = "my_app";
    }
    config["EXECUTION_NAME"] = std::string(executionName);

    const char * timestampedWrites = std::getenv("TIMESTAMPED_WRITES");
    std::string timestampedWrites2;
    if (timestampedWrites == nullptr) {
        timestampedWrites2 = std::string("false");
    } else {
        timestampedWrites2 = std::string(timestampedWrites);
        std::transform(timestampedWrites2.begin(), timestampedWrites2.end(), timestampedWrites2.begin(),
                [](unsigned char c){ return std::tolower(c); });
    }
    config["TIMESTAMPED_WRITES"] = timestampedWrites2;

        //{"writer_buffer",      std::to_string(writer_queue)},??? == WRITE_BUFFER_SIZE?
    const char * writeBufferSize = std::getenv("WRITE_BUFFER_SIZE");
    if (writeBufferSize == nullptr) {
        writeBufferSize = "1000";
    }
    config["WRITE_BUFFER_SIZE"] = std::string(writeBufferSize);

        ///writer_par ==> 'WRITE_CALLBACKS_NUMBER'
    const char *writeCallbacksNum = std::getenv("WRITE_CALLBACKS_NUMBER");
    if (writeCallbacksNum == nullptr) {
        writeCallbacksNum = "16";
    }
    config["WRITE_CALLBACKS_NUMBER"] = std::string(writeCallbacksNum);

    const char * cacheSize = std::getenv("MAX_CACHE_SIZE");
    if (cacheSize == nullptr) {
        cacheSize = "1000";
    }
    config["MAX_CACHE_SIZE"] = std::string(cacheSize);

    const char *replicationFactor = std::getenv("REPLICA_FACTOR");
    if (replicationFactor == nullptr) {
        replicationFactor = "1";
    }
    config["REPLICA_FACTOR"] = std::string(replicationFactor);

    const char *replicationStrategy = std::getenv("REPLICATION_STRATEGY");
    if (replicationStrategy == nullptr) {
        replicationStrategy = "SimpleStrategy";
    }
    config["REPLICATION_STRATEGY"] = std::string(replicationStrategy);

    const char *replicationStrategyOptions = std::getenv("REPLICATION_STRATEGY_OPTIONS");
    if (replicationStrategyOptions == nullptr) {
        replicationStrategyOptions = "";
    }
    config["REPLICATION_STRATEGY_OPTIONS"] = replicationStrategyOptions;

    if (config["REPLICATION_STRATEGY"] == "SimpleStrategy") {
        config["REPLICATION"] = std::string("{'class' : 'SimpleStrategy', 'replication_factor': ") + config["REPLICA_FACTOR"] + "}";
    } else {
        config["REPLICATION"] = std::string("{'class' : '") + config["REPLICATION_STRATEGY"] + "', " + config["REPLICATION_STRATEGY_OPTIONS"] + "}";
    }
}

CassError HecubaSession::run_query(std::string query) const{
	CassStatement *statement = cass_statement_new(query.c_str(), 0);

    //std::cout << "DEBUG: HecubaSession.run_query : "<<query<<std::endl;
    CassFuture *result_future = cass_session_execute(const_cast<CassSession *>(storageInterface->get_session()), statement);
    cass_statement_free(statement);

    CassError rc = cass_future_error_code(result_future);
    if (rc != CASS_OK) {
        printf("Query execution error: %s - %s\n", cass_error_desc(rc), query.c_str());
    }
    cass_future_free(result_future);
    return rc;
}

void HecubaSession::decodeNumpyMetadata(HecubaSession::NumpyShape *s, void* metadata) {
    // Numpy Metadata(all unsigned): Ndims + Dim1 + Dim2 + ... + DimN  (Layout in C by default)
    unsigned* value = (unsigned*)metadata;
    s->ndims = *(value);
    value ++;
    s->dim = (unsigned *)malloc(s->ndims);
    for (unsigned i = 0; i < s->ndims; i++) {
        s->dim[i] = *(value + i);
    }
}
void HecubaSession::getMetaData(void * raw_numpy_meta, ArrayMetadata &arr_metas) {
    std::vector <uint32_t> dims;
    std::vector <uint32_t> strides;

    // decode void *metadatas
    HecubaSession:: NumpyShape * s = new HecubaSession::NumpyShape();
    decodeNumpyMetadata(s, raw_numpy_meta);
    uint32_t acum=1;
    for (uint32_t i=0; i < s->ndims; i++) {
        dims.push_back( s->dim[i]);
        acum *= s->dim[i];
    }
    for (uint32_t i=0; i < s->ndims; i++) {
        strides.push_back(acum * sizeof(double));
        acum /= s->dim[s->ndims-1-i];
    }
    uint32_t flags=NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_WRITEABLE | NPY_ARRAY_ALIGNED;

    arr_metas.dims = dims;
    arr_metas.strides = strides;
    arr_metas.elem_size = sizeof(double);
    arr_metas.flags = flags;
    arr_metas.partition_type = ZORDER_ALGORITHM;
    arr_metas.typekind = 'f';
    arr_metas.byteorder = '=';
}

void HecubaSession::registerNumpy(ArrayMetadata &numpy_meta, std::string name, uint64_t* uuid) {

    //std::cout<< "DEBUG: HecubaSession::registerNumpy BEGIN "<< name << UUID2str(uuid)<<std::endl;
    void *keys = std::malloc(sizeof(uint64_t *));
    uint64_t *c_uuid = (uint64_t *) malloc(sizeof(uint64_t) * 2);//new uint64_t[2];
    c_uuid[0] = *uuid;
    c_uuid[1] = *(uuid + 1);

    std::memcpy(keys, &c_uuid, sizeof(uint64_t *));

    char *c_name = (char *) std::malloc(name.length() + 1);
    std::memcpy(c_name, name.c_str(), name.length() + 1);

    //COPY VALUES
    int offset = 0;
    uint64_t size_name = strlen(c_name)+1;
    uint64_t size = 0;

    //size of the vector of dims
    size += sizeof(uint32_t) * numpy_meta.dims.size();

    //plus the other metas
    size += sizeof(numpy_meta.elem_size)
		+ sizeof(numpy_meta.partition_type)
		+ sizeof(numpy_meta.flags)
		+ sizeof(uint32_t)*numpy_meta.strides.size() // dims & strides
		+ sizeof(numpy_meta.typekind)
		+ sizeof(numpy_meta.byteorder);

    //allocate plus the bytes counter
    unsigned char *byte_array = (unsigned char *) malloc(size+ sizeof(uint64_t));
    unsigned char *name_array = (unsigned char *) malloc(size_name);

    // copy table name
    memcpy(name_array, c_name, size_name); //lgarrobe

    // Copy num bytes
    memcpy(byte_array+offset, &size, sizeof(uint64_t));
    offset += sizeof(uint64_t);

    //copy everything from the metas
	//	flags int, elem_size int, partition_type tinyint,
    //       dims list<int>, strides list<int>, typekind text, byteorder text

    memcpy(byte_array + offset, &numpy_meta.flags, sizeof(numpy_meta.flags));
    offset += sizeof(numpy_meta.flags);

    memcpy(byte_array + offset, &numpy_meta.elem_size, sizeof(numpy_meta.elem_size));
    offset += sizeof(numpy_meta.elem_size);

    memcpy(byte_array + offset, &numpy_meta.partition_type, sizeof(numpy_meta.partition_type));
    offset += sizeof(numpy_meta.partition_type);

    memcpy(byte_array + offset, &numpy_meta.typekind, sizeof(numpy_meta.typekind));
    offset +=sizeof(numpy_meta.typekind);

    memcpy(byte_array + offset, &numpy_meta.byteorder, sizeof(numpy_meta.byteorder));
    offset +=sizeof(numpy_meta.byteorder);

    memcpy(byte_array + offset, numpy_meta.dims.data(), sizeof(uint32_t) * numpy_meta.dims.size());
    offset +=sizeof(uint32_t)*numpy_meta.dims.size();

    memcpy(byte_array + offset, numpy_meta.strides.data(), sizeof(uint32_t) * numpy_meta.strides.size());
    offset +=sizeof(uint32_t)*numpy_meta.strides.size();

    //memcpy(byte_array + offset, &numpy_meta.inner_type, sizeof(numpy_meta.inner_type));
    //offset += sizeof(numpy_meta.inner_type);

    int offset_values = 0;
    char *values = (char *) malloc(sizeof(char *)*4);

    uint64_t *base_numpy = (uint64_t *) malloc(sizeof(uint64_t) * 2);//new uint64_t[2];
    memcpy(base_numpy, c_uuid, sizeof(uint64_t)*2);
    //std::cout<< "DEBUG: HecubaSession::registerNumpy &base_numpy = "<<base_numpy<<std::endl;
    std::memcpy(values, &base_numpy, sizeof(uint64_t *));  // base_numpy
    offset_values += sizeof(unsigned char *);

    char *class_name=(char*)malloc(strlen("hecuba.hnumpy.StorageNumpy")+1);
    strcpy(class_name, "hecuba.hnumpy.StorageNumpy");
    //std::cout<< "DEBUG: HecubaSession::registerNumpy &class_name = "<<class_name<<std::endl;
    memcpy(values+offset_values, &class_name, sizeof(unsigned char *)); //class_name
    offset_values += sizeof(unsigned char *);

    //std::cout<< "DEBUG: HecubaSession::registerNumpy &name = "<<name_array<<std::endl;
    memcpy(values+offset_values, &name_array, sizeof(unsigned char *)); //name
    offset_values += sizeof(unsigned char *);

    //std::cout<< "DEBUG: HecubaSession::registerNumpy &np_meta = "<<byte_array<<std::endl;
    memcpy(values+offset_values, &byte_array,  sizeof(unsigned char *)); // numpy_meta
    offset_values += sizeof(unsigned char *);

    try {
        numpyMetaWriter->write_to_cassandra(keys, values);
    }
    catch (std::exception &e) {
        std::cerr << "HecubaSession::registerNumpy: Error writing" <<std::endl;
        std::cerr << e.what();
        throw e;
    };

}

void HecubaSession::createSchema(void) {
    // Create hecuba
    std::vector<std::string> queries;
    std::string create_hecuba_keyspace = std::string(
            "CREATE KEYSPACE IF NOT EXISTS hecuba  WITH replication = ") +  config["REPLICATION"];
    queries.push_back(create_hecuba_keyspace);
    std::string create_hecuba_qmeta = std::string(
            "CREATE TYPE IF NOT EXISTS hecuba.q_meta("
            "mem_filter text,"
            "from_point frozen<list<double>>,"
            "to_point frozen<list<double>>,"
            "precision float);");
    queries.push_back(create_hecuba_qmeta);
    std::string create_hecuba_npmeta = std::string(
            "CREATE TYPE IF NOT EXISTS hecuba.np_meta ("
            "flags int, elem_size int, partition_type tinyint,"
            "dims list<int>, strides list<int>, typekind text, byteorder text)");
    queries.push_back(create_hecuba_npmeta);
    std::string create_hecuba_istorage = std::string(
            "CREATE TABLE IF NOT EXISTS hecuba.istorage"
            "(storage_id uuid,"
            "class_name text,name text,"
            "istorage_props map<text,text>,"
            "tokens list<frozen<tuple<bigint,bigint>>>,"
            "indexed_on list<text>,"
            "qbeast_random text,"
            "qbeast_meta frozen<q_meta>,"
            "numpy_meta frozen<np_meta>,"
            "block_id int,"
            "base_numpy uuid,"
            "view_serialization blob,"
            "primary_keys list<frozen<tuple<text,text>>>,"
            "columns list<frozen<tuple<text,text>>>,"
            "PRIMARY KEY(storage_id));");
    queries.push_back(create_hecuba_istorage);
    // Create keyspace EXECUTION_NAME
    std::string create_keyspace = std::string(
            "CREATE KEYSPACE IF NOT EXISTS ") + config["EXECUTION_NAME"] +
        std::string(" WITH replication = ") +  config["REPLICATION"];
    queries.push_back(create_keyspace);

    for(auto q: queries) {
        CassError rc = run_query(q);
        if (rc != CASS_OK) {
            std::string msg = std::string("HecubaSession:: Error Creating Schema executing: ") + q;
            throw ModuleException(msg);
        }
    }
}

/***************************
 * PUBLIC
 ***************************/

/* Constructor: Establish connection with underlying storage system */
HecubaSession::HecubaSession() : currentDataModel(NULL) {

    parse_environment(this->config);

    /* Establish connection */
    this->storageInterface = std::make_shared<StorageInterface>(stoi(config["NODE_PORT"]), config["CONTACT_NAMES"]);
    //this->storageInterface = new StorageInterface(stoi(config["NODE_PORT"]), config["CONTACT_NAMES"]);

    if (this->config["CREATE_SCHEMA"] == "true") {
        createSchema();
    }

// TODO: extend writer to support lists	std::vector<config_map> pkeystypes = { {{"name", "storage_id"}} };
// TODO: extend writer to support lists	std::vector<config_map> ckeystypes = {};
// TODO: extend writer to support lists	std::vector<config_map> colstypes = {{{"name", "class_name"}},
// TODO: extend writer to support lists							{{"name", "name"}},
// TODO: extend writer to support lists							{{"name", "tokens"}},   //list
// TODO: extend writer to support lists							{{"name", "primary_keys"}}, //list
// TODO: extend writer to support lists							{{"name", "columns"}},  //list
// TODO: extend writer to support lists							{{"name", "indexed_on"}} }; //list
// TODO: extend writer to support lists	dictMetaWriter = storageInterface->make_writer("istorage", "hecuba",
// TODO: extend writer to support lists													pkeystypes, colstypes,
// TODO: extend writer to support lists													config);
// TODO: extend writer to support lists

std::vector<config_map> pkeystypes_n = { {{"name", "storage_id"}} };
std::vector<config_map> ckeystypes_n = {};
std::vector<config_map> colstypes_n = {
						{{"name", "base_numpy"}},
                        {{"name", "class_name"}},
						{{"name", "name"}},
						{{"name", "numpy_meta"}},
						//{{"name", "block_id"}}, //NOT USED
						//{{"name", "view_serialization"}},  //Used by Python, uses pickle format. Let it be NULL and initialized at python code
};
						// TODO: extend writer to support lists {{"name", "tokens"}} }; //list

// The attributes stored in istorage for all numpys are the same, we use a single writer for the session
numpyMetaWriter = storageInterface->make_writer("istorage", "hecuba",
												pkeystypes_n, colstypes_n,
												config);

}

HecubaSession::~HecubaSession() {
    delete(currentDataModel);
    delete(numpyMetaWriter);
}

/* loadDataModel: loads a DataModel from 'model_filename' path which should be
 * a YAML file. It also generates its corresponding python generated class in
 * the same directory where the model resides or in the 'pythonSpecPath'
 * directory. */
void HecubaSession::loadDataModel(const char * model_filename, const char * pythonSpecPath=NULL) {

    if (currentDataModel != NULL) {
        std::cerr << "WARNING: HecubaSession::loadDataModel: DataModel already defined. Discarded and load again"<<std::endl;
        delete(currentDataModel);
    }

    // TODO: parse file to get model information NOW HARDCODED

    // class dataModel(StorageDict):
    //    '''
    //         @TypeSpec dict <<lat:int,ts:int>,metrics:numpy.ndarray>
    //    '''

    DataModel* d = new DataModel();

    if (model_filename == NULL) {
		throw std::runtime_error("Trying to load a NULL model file name ");
    }

    // Detect dirname and basename for 'model_filename'
    std::string pythonPath("");
    std::string baseName(model_filename);

    size_t pos = baseName.find_last_of('/');
    if (pos != std::string::npos) { // model_filename has a path
        if (pythonSpecPath != NULL) {
            pythonPath = pythonSpecPath;
        } else {
            pythonPath = baseName.substr(0,pos);
        }
        baseName = baseName.substr(pos+1, baseName.size());
    }
    // Remove .yaml extension
    pos = baseName.find_last_of('.');
    if (pos != std::string::npos) {
        baseName = baseName.substr(0, pos);
    }
    // Create moduleName
    std::string moduleName;
    if (pythonPath == "") {
        moduleName = baseName;
    } else {
        moduleName = pythonPath + "/" + baseName;
    }

    d->setModuleName(moduleName);

    // Add .py extension to moduleName
    moduleName =  moduleName + ".py";

    std::ofstream fd(moduleName);

    YAML::Node node = YAML::LoadFile(model_filename);

    assert(node.Type() == YAML::NodeType::Sequence);

    for(std::size_t i=0;i<node.size();i++) {
		DataModel::datamodel_spec x = node[i].as<DataModel::datamodel_spec>();
		d->addObjSpec(x.id, x.o);
        fd<<x.o.getPythonString();
    }
    fd.close();

    // ALWAYS Add numpy (just in case) ##############################
    std::vector<std::pair<std::string, std::string>> pkeystypes_numpy = {
                                  {"storage_id", "uuid"},
                                  {"cluster_id", "int"}
    };
    std::vector<std::pair<std::string, std::string>> ckeystypes_numpy = {{"block_id","int"}};
    std::vector<std::pair<std::string, std::string>> colstypes_numpy = {
                                  {"payload", "blob"},
    };
    d->addObjSpec(ObjSpec::valid_types::STORAGENUMPY_TYPE, "hecuba.hnumpy.StorageNumpy", pkeystypes_numpy, ckeystypes_numpy, colstypes_numpy);
    // ##############################

    currentDataModel = d;
}

IStorage* HecubaSession::createObject(const char * id_model, const char * id_object, void * metadata, void* value) {
    // Create Cassandra tables 'ksp.id_object' for object 'id_object' according to its type 'id_model' in 'model'
    // TODO: create type depending on 'model', now its only dictionary

    DataModel* model = currentDataModel;
    if (model == NULL) {
        throw ModuleException("HecubaSession::createObject No data model loaded");
    }

    IStorage * o;

    ObjSpec oType = model->getObjSpec(id_model);
    //std::cout << "DEBUG: HecubaSession::createObject '"<<id_model<< "' ==> " <<oType.debug()<<std::endl;

    std::string object_name(config["EXECUTION_NAME"] + "." + std::string(id_object));
    uint64_t *c_uuid = generateUUID5(object_name.c_str()); // UUID for the new object

    switch(oType.getType()) {
        case ObjSpec::valid_types::STORAGEOBJ_TYPE:
            {
                // StorageObj case
                //  Create table 'class_name' "CREATE TABLE ksp.class_name (storage_id UUID, nom typ, ... PRIMARY KEY (storage_id))"
                std::string query = "CREATE TABLE IF NOT EXISTS " +
                    config["EXECUTION_NAME"] + "." + id_model +
                    oType.table_attr;

                CassError rc = run_query(query);
                if (rc != CASS_OK) {
                    if (rc == CASS_ERROR_SERVER_INVALID_QUERY) { // keyspace does not exist
                        std::cout<< "HecubaSession Creating keyspace "<< config["EXECUTION_NAME"]<< std::endl;
                        std::string create_keyspace = std::string(
                                "CREATE KEYSPACE IF NOT EXISTS ") + config["EXECUTION_NAME"] +
                            std::string(" WITH replication = ") +  config["REPLICATION"];
                        rc = run_query(create_keyspace);
                        if (rc != CASS_OK) {
                            std::string msg = std::string("HecubaSession:: Error executing query ") + create_keyspace;
                            throw ModuleException(msg);
                        } else {
                            rc = run_query(query); // Repeat table creation after creating keyspace
                            if (rc != CASS_OK) {
                                std::string msg = std::string("HecubaSession:: Error executing query ") + query;
                                throw ModuleException(msg);
                            }
                        }
                    } else {
                        std::string msg = std::string("HecubaSession:: Error executing query ") + query;
                        throw ModuleException(msg);
                    }
                }
                // Table for storageobj class created
                // Add entry to ISTORAGE: TODO add the tokens attribute
                std::string name = config["EXECUTION_NAME"] + "." + id_object;
                std::string insquery = std::string("INSERT INTO ") +
                    std::string("hecuba.istorage") +
                    std::string("(storage_id, name, class_name, columns)") +
                    std::string("VALUES ") +
                    std::string("(") +
                    UUID2str(c_uuid) + std::string(", ") +
                    "'" + name + "'" + std::string(", ") +
                    "'" + model->getModuleName() + "." + id_model + "'" + std::string(", ") +
                    oType.getColsStr() +
                    std::string(")");
                run_query(insquery);

                //  Create Writer for storageobj
                std::vector<config_map>* keyNamesDict = oType.getKeysNamesDict();
                std::vector<config_map>* colNamesDict = oType.getColsNamesDict();

                Writer *writer = storageInterface->make_writer(id_model, config["EXECUTION_NAME"].c_str(),
                          *keyNamesDict, *colNamesDict,
                          config);
                delete keyNamesDict;
                delete colNamesDict;
                o = new IStorage(this, id_model, config["EXECUTION_NAME"] + "." + id_object, c_uuid, writer);

            }
            break;
        case ObjSpec::valid_types::STORAGEDICT_TYPE:
            {
                // Dictionary case
                //  Create table 'name' "CREATE TABLE ksp.name (nom typ, nom typ, ... PRIMARY KEY (nom, nom))"
                bool new_element = true;
                std::string query = "CREATE TABLE " +
                    config["EXECUTION_NAME"] + "." + id_object +
                    oType.table_attr;

                CassError rc = run_query(query);
                if (rc != CASS_OK) {
                    if (rc == CASS_ERROR_SERVER_ALREADY_EXISTS ) {
                        new_element = false; //OOpps, creation failed. It is an already existent object.
                    } else if (rc == CASS_ERROR_SERVER_INVALID_QUERY) {
                        std::cout<< "HecubaSession Creating keyspace "<< config["EXECUTION_NAME"]<< std::endl;
                        std::string create_keyspace = std::string(
                                "CREATE KEYSPACE IF NOT EXISTS ") + config["EXECUTION_NAME"] +
                            std::string(" WITH replication = ") +  config["REPLICATION"];
                        rc = run_query(create_keyspace);
                        if (rc != CASS_OK) {
                            std::string msg = std::string("HecubaSession:: Error executing query ") + create_keyspace;
                            throw ModuleException(msg);
                        } else {
                            rc = run_query(query);
                            if (rc != CASS_OK) {
                                if (rc == CASS_ERROR_SERVER_ALREADY_EXISTS) {
                                    new_element = false; //OOpps, creation failed. It is an already existent object.
                                }  else {
                                    std::string msg = std::string("HecubaSession:: Error executing query ") + query;
                                    throw ModuleException(msg);
                                }
                            }
                        }
                    } else {
                        std::string msg = std::string("HecubaSession:: Error executing query ") + query;
                        throw ModuleException(msg);
                    }
                }

                if (new_element) {
                    //  Add entry to hecuba.istorage: TODO add the tokens attribute
                    std::string name = config["EXECUTION_NAME"] + "." + id_object;
                    // TODO EXPECTED:vvv NOW HARDCODED
                    //classname = id_model
                    // keys = {c_uuid}, values={name, class_name, primary_keys, columns } # no tokens, no numpy_meta, ...
                    //try {
                    //	dictMetaWriter->write_to_cassandra(keys, values);
                    //}
                    //catch (std::exception &e) {
                    //	std::cerr << "Error writing in registering" <<std::endl;
                    //	std::cerr << e.what();
                    //	throw e;
                    //}
                    // EXPECTED^^^^
                    // Example: insert into hecuba.istorage (storage_id, primary_keys) values (3dd30d5d-b0d4-45b5-a21a-c6ad313007fd, [('lat','int'),('ts','int')]);
                    std::string insquery = std::string("INSERT INTO ") +
                        std::string("hecuba.istorage") +
                        std::string("(storage_id, name, class_name, primary_keys, columns)") +
                        std::string("VALUES ") +
                        std::string("(") +
                        UUID2str(c_uuid) + std::string(", ") +
                        "'" + name + "'" + std::string(", ") +
                        "'" + id_model + "'" + std::string(", ") +
                        oType.getKeysStr() + std::string(", ") +
                        oType.getColsStr() +
                        std::string(")");
                    run_query(insquery);
                } else {
                    std::cerr << "WARNING: Object "<<id_object<<" already exists. Trying to overwrite it. It may fail if the schema does not match."<<std::endl;
                    // TODO: THIS IS NOT WORKING. We need to get the storage_id (c_uuid) from istorage DISABLE
                    // TODO: Check the schema in Cassandra matches the model
                }

                //  Create Writer for dictionary
                std::vector<config_map>* keyNamesDict = oType.getKeysNamesDict();
                std::vector<config_map>* colNamesDict = oType.getColsNamesDict();

                Writer *writer = storageInterface->make_writer(id_object, config["EXECUTION_NAME"].c_str(),
                          *keyNamesDict, *colNamesDict,
                          config);
                delete keyNamesDict;
                delete colNamesDict;
                o = new IStorage(this, id_model, config["EXECUTION_NAME"] + "." + id_object, c_uuid, writer);
            }
            break;

        case ObjSpec::valid_types::STORAGENUMPY_TYPE:
            {
                // Create table
                std::string query = "CREATE TABLE IF NOT EXISTS " + config["EXECUTION_NAME"] + "." + id_object +
                    " (storage_id uuid, cluster_id int, block_id int, payload blob, "
                    "PRIMARY KEY((storage_id,cluster_id),block_id)) "
                    "WITH compaction = {'class': 'SizeTieredCompactionStrategy', 'enabled': false};";

                this->run_query(query);

                // StorageNumpy
                ArrayDataStore *array_store = new ArrayDataStore(id_object, config["EXECUTION_NAME"].c_str(),
                        this->storageInterface->get_session(), config);
                //std::cout << "DEBUG: HecubaSession::createObject After ArrayDataStore creation " <<std::endl;

                // Create entry in hecuba.istorage for the new numpy
                ArrayMetadata numpy_metas;
                getMetaData(metadata, numpy_metas); // numpy_metas = getMetaData(metadata);
                //std::cout<< "DEBUG: HecubaSession::createObject After metadata creation " <<std::endl;
                registerNumpy(numpy_metas, id_object, c_uuid);
                //std::cout<< "DEBUG: HecubaSession::createObject After REGISTER numpy into ISTORAGE" <<std::endl;

                //Create keys, values to store the numpy
                //double* tmp = (double*)value;
                //std::cout<< "DEBUG: HecubaSession::createObject BEFORE store FIRST element in NUMPY is "<< *tmp << " and second is "<<*(tmp+1)<< " 3rd " << *(tmp+2)<< std::endl;
                array_store->store_numpy_into_cas(c_uuid, numpy_metas, value);
                array_store->wait_stores();

                // TODO: el writer para pasarselo al istorage esta en la cache table del array datastor: anaydir getCache al arraydatastore
                o = new IStorage(this, id_model, config["EXECUTION_NAME"] + "." + id_object, c_uuid, array_store->getWriteCache()->get_writer());

            }
            break;
        default:
            throw ModuleException("HECUBA Session: createObject Unknown type ");// + std::string(oType.objtype));
            break;
    }
    //std::cout << "DEBUG: HecubaSession::createObject DONE" << std::endl;
    return o;
}

uint64_t* HecubaSession::generateUUID(void) const {
    uint64_t *c_uuid; // UUID for the new object
    c_uuid = (uint64_t *) malloc(sizeof(uint64_t) * 2);

    boost::uuids::random_generator gen;
    boost::uuids::uuid u = gen();

    memcpy(c_uuid, &u, 16);

    return c_uuid;
}

uint64_t* HecubaSession::generateUUID5(const char* name) const{
    /* uses sha1 */
    uint64_t *c_uuid; // UUID for the new object
    c_uuid = (uint64_t *) malloc(sizeof(uint64_t) * 2);

    boost::uuids::string_generator gen1;
    boost::uuids::uuid dns_namespace_uuid = gen1("{6ba7b810-9dad-11d1-80b4-00c04fd430c8}");
    boost::uuids::name_generator gen(dns_namespace_uuid);

    // boost::uuids::name_generator_md5 gen(boost::uuids::ns::dns());

    boost::uuids::uuid u = gen(name);

    memcpy(c_uuid, &u, 16);

    return c_uuid;
}

std::string HecubaSession::UUID2str(uint64_t* c_uuid) {
    /* This MUST match with the 'cass_statement_bind_uuid' result */
    char str[37] = {};
    unsigned char* uuid = reinterpret_cast<unsigned char*>(c_uuid);
    //std::cout<< "HecubaSession: uuid2str: BEGIN "<<std::hex<<c_uuid[0]<<c_uuid[1]<<std::endl;
    sprintf(str,
        "%02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-%02x%02x%02x%02x%02x%02x",
        uuid[0], uuid[1], uuid[2], uuid[3],
        uuid[4], uuid[5],
        uuid[6], uuid[7],
        uuid[8], uuid[9],
        uuid[10], uuid[11], uuid[12], uuid[13], uuid[14], uuid[15]
        );
    //std::cout<< "HecubaSession: uuid2str: "<<str<<std::endl;
    return std::string(str);
}

DataModel* HecubaSession::getDataModel() {
    return currentDataModel;
}

max_stars_repo_name: cugni/hecuba

max_issues_repo_name: cugni/hecuba

max_forks_repo_name: cugni/hecuba


text:
// ---------------------------------------------------------------------
// pion:  a Boost C++ framework for building lightweight HTTP interfaces
// ---------------------------------------------------------------------
// Copyright (C) 2007-2012 Cloudmeter, Inc.  (http://www.cloudmeter.com)
//
// Distributed under the Boost Software License, Version 1.0.
// See http://www.boost.org/LICENSE_1_0.txt
//

#include "EchoService.hpp"
#include <boost/bind.hpp>
#include <pion/algorithm.hpp>
#include <pion/http/response_writer.hpp>
#include <pion/user.hpp>

using namespace pion;

namespace pion {        // begin namespace pion
namespace plugins {     // begin namespace plugins

/// used by handle_request to write dictionary terms
void writeDictionaryTerm(http::response_writer_ptr& writer,
                         const ihash_multimap::value_type& val)
{
    // text is copied into writer text cache
    writer << val.first << http::types::HEADER_NAME_VALUE_DELIMITER
    << val.second
    << http::types::STRING_CRLF;
}

// EchoService member functions

/// handles requests for EchoService
void EchoService::operator()(http::request_ptr& http_request_ptr, tcp::connection_ptr& tcp_conn)
{
    // this web service uses static text to test the mixture of "copied" with
    // "static" (no-copy) text
    static const std::string REQUEST_ECHO_TEXT("[Request Echo]");
    static const std::string REQUEST_HEADERS_TEXT("[Request Headers]");
    static const std::string QUERY_PARAMS_TEXT("[Query Parameters]");
    static const std::string COOKIE_PARAMS_TEXT("[Cookie Parameters]");
    static const std::string POST_CONTENT_TEXT("[POST Content]");
    static const std::string USER_INFO_TEXT("[USER Info]");

    // Set Content-type to "text/plain" (plain ascii text)
    http::response_writer_ptr writer(http::response_writer::create(tcp_conn, *http_request_ptr,
                                                            boost::bind(&tcp::connection::finish, tcp_conn)));
    writer->get_response().set_content_type(http::types::CONTENT_TYPE_TEXT);

    // write request information
    writer->write_no_copy(REQUEST_ECHO_TEXT);
    writer->write_no_copy(http::types::STRING_CRLF);
    writer->write_no_copy(http::types::STRING_CRLF);
    writer
        << "Request method: "
        << http_request_ptr->get_method()
        << http::types::STRING_CRLF
        << "Resource originally requested: "
        << http_request_ptr->get_original_resource()
        << http::types::STRING_CRLF
        << "Resource delivered: "
        << http_request_ptr->get_resource()
        << http::types::STRING_CRLF
        << "Query string: "
        << http_request_ptr->get_query_string()
        << http::types::STRING_CRLF
        << "HTTP version: "
        << http_request_ptr->get_version_major() << '.' << http_request_ptr->get_version_minor()
        << http::types::STRING_CRLF
        << "Content length: "
        << (unsigned long)http_request_ptr->get_content_length()
        << http::types::STRING_CRLF
        << http::types::STRING_CRLF;

    // write request headers
    writer->write_no_copy(REQUEST_HEADERS_TEXT);
    writer->write_no_copy(http::types::STRING_CRLF);
    writer->write_no_copy(http::types::STRING_CRLF);
    std::for_each(http_request_ptr->get_headers().begin(), http_request_ptr->get_headers().end(),
                  boost::bind(&writeDictionaryTerm, writer, _1));
    writer->write_no_copy(http::types::STRING_CRLF);

    // write query parameters
    writer->write_no_copy(QUERY_PARAMS_TEXT);
    writer->write_no_copy(http::types::STRING_CRLF);
    writer->write_no_copy(http::types::STRING_CRLF);
    std::for_each(http_request_ptr->get_queries().begin(), http_request_ptr->get_queries().end(),
                  boost::bind(&writeDictionaryTerm, writer, _1));
    writer->write_no_copy(http::types::STRING_CRLF);

    // write cookie parameters
    writer->write_no_copy(COOKIE_PARAMS_TEXT);
    writer->write_no_copy(http::types::STRING_CRLF);
    writer->write_no_copy(http::types::STRING_CRLF);
    std::for_each(http_request_ptr->get_cookies().begin(), http_request_ptr->get_cookies().end(),
                  boost::bind(&writeDictionaryTerm, writer, _1));
    writer->write_no_copy(http::types::STRING_CRLF);

    // write POST content
    writer->write_no_copy(POST_CONTENT_TEXT);
    writer->write_no_copy(http::types::STRING_CRLF);
    writer->write_no_copy(http::types::STRING_CRLF);
    if (http_request_ptr->get_content_length() != 0) {
        writer->write(http_request_ptr->get_content(), http_request_ptr->get_content_length());
        writer->write_no_copy(http::types::STRING_CRLF);
        writer->write_no_copy(http::types::STRING_CRLF);
    }

    // if authenticated, write user info
    user_ptr user = http_request_ptr->get_user();
    if (user) {
        writer->write_no_copy(USER_INFO_TEXT);
        writer->write_no_copy(http::types::STRING_CRLF);
        writer->write_no_copy(http::types::STRING_CRLF);
        writer << "User authenticated, username: " << user->get_username();
        writer->write_no_copy(http::types::STRING_CRLF);
    }

    // send the writer
    writer->send();
}

}   // end namespace plugins
}   // end namespace pion

/// creates new EchoService objects
extern "C" PION_PLUGIN pion::plugins::EchoService *pion_create_EchoService(void)
{
    return new pion::plugins::EchoService();
}

/// destroys EchoService objects
extern "C" PION_PLUGIN void pion_destroy_EchoService(pion::plugins::EchoService *service_ptr)
{
    delete service_ptr;
}

max_stars_repo_name: paolovolpi/pion

max_issues_repo_name: pmed/pion

max_forks_repo_name: pmed/pion


text:
#include "RendermanPointsSchemaHandler.h"
#include "RendermanPolyMeshSchemaHandler.h"
#include <String2ArgcArgv.h>
#include <boost/program_options.hpp>
#include <boost/shared_ptr.hpp>
#include <stdio.h> // required when building as procedural "RunProgram"
#include <simba_logging.h>

namespace po = boost::program_options;

/*!
 * \todo Remove the i_level param as this is a procedural
 */
void locate_geometry_in_hierarchy(const Alembic::Abc::IObject& top,
								  const StringContainer&       i_hierachy_path,
								  size_t 					   i_requested_index,
								  float          			   i_relative_shutter_open,
								  float          			   i_relative_shutter_close,
								  Alembic::Abc::uint8_t        num_motion_samples,
								  size_t                       i_level = 0)
{
	size_t numChildren = top.getNumChildren();
	std::string unique_object_path;

	for (size_t i=0;i<numChildren;i++)
	{
		std::string child_name =top.getChildHeader(i).getName();
		for (size_t indent=0;indent<i_level;indent++)
			DLOG(INFO) << "  ";
		Alembic::Abc::IObject child(top,child_name);
		const Alembic::Abc::MetaData &child_md = child.getMetaData();
		std::string metadata_string = child_md.serialize();
		DLOG(INFO) << boost::format("metadata_string='%1%' child_name='%2%'") % metadata_string % child_name;
		if (Alembic::AbcGeom::IPolyMeshSchema::matches(child_md))
		{
			RendermanPolyMeshSchemaHandler apmsh;

			apmsh.ProcessPolyMesh(top, child_name, i_hierachy_path, i_requested_index, i_relative_shutter_open, i_relative_shutter_close, num_motion_samples);
		}
		else if (Alembic::AbcGeom::IPointsSchema::matches(child_md))
		{
			RendermanPointsSchemaHandler apsh;
			apsh.ProcessPoints(top, child_name, i_hierachy_path, i_requested_index, i_relative_shutter_open, i_relative_shutter_close, num_motion_samples);
		}
		DLOG(INFO) << std::endl;
		StringContainer       concatenated_hierachy_path = i_hierachy_path;
		concatenated_hierachy_path.push_back(child_name);
		locate_geometry_in_hierarchy(child,
								concatenated_hierachy_path,
								i_requested_index,
								i_relative_shutter_open,
								i_relative_shutter_close,
								num_motion_samples,
								i_level+1);
	}
}

int EmitGeometry(int argc, const char** argv)
{
	try {

	    size_t                frame_to_export = 1;
	    std::string           alembic_fileName;
	    std::string           velocity_fileName;
	    std::string           geometry_filter;
		float                 relative_shutter_open = -0.25f;
		float                 relative_shutter_close = 0.25f;
		size_t                num_motion_samples = 3;

		po::options_description desc("Allowed options");
		desc.add_options()
    		("version", "print version string")
    		("help", "produce help message")
    		("samples,s", po::value<size_t>(&num_motion_samples),
    				"number of samples to generate [default 3]")
    		("open,o", po::value<float>(&relative_shutter_open),
    				"relative shutter open [default -0.25]")
    		("close,c", po::value<float>(&relative_shutter_close),
    				"relative shutter close [default 0.25]")
    		("frame,f", po::value<size_t>(&frame_to_export),
    				"frame to export [default 1]")
    		("abc,a", po::value<std::string>(&alembic_fileName),
    				"alembic file")
			("velocity,v", po::value<std::string>(&velocity_fileName),
					"velocity file")
    				;

		po::variables_map vm;
		po::store(po::parse_command_line(argc, argv, desc), vm);
		po::notify(vm);

		if (vm.count("help") || alembic_fileName.empty()) {
			std::cout << desc << "\n";
			return 1;
		}

		Alembic::AbcCoreFactory::IFactory factory;
		Alembic::AbcCoreFactory::IFactory::CoreType oType;

	    Alembic::AbcGeom::IArchive alembic_archive(factory.getArchive(alembic_fileName, oType));

		StringContainer       hierachy_path;
		locate_geometry_in_hierarchy(alembic_archive.getTop(),hierachy_path,frame_to_export,relative_shutter_open,relative_shutter_close,num_motion_samples);
	}
	catch(std::exception& e) {
		LOG(ERROR) << "error: " << e.what() << "\n";
		return 1;
	}
	catch(...) {
		LOG(ERROR) << "Exception of unknown type!\n";
	}
	return 0;
}

#if defined(__cplusplus) || defined(c_plusplus)
extern "C" {
#endif

RtPointer ConvertParameters(RtString paramstr)
{
	std::string param_std_string = (boost::format("dummy %1%") % paramstr).str();
	DLOG(INFO) << boost::format("param_std_string = '%1%'") % param_std_string << std::endl;
	PI::String2ArgcArgv* s2aa = new PI::String2ArgcArgv(param_std_string);

	/* return the blind data pointer */
	return (RtPointer)s2aa;
}

RtVoid Subdivide(RtPointer data, RtFloat detail)
{
	PI::String2ArgcArgv* s2aa = (PI::String2ArgcArgv *)data;

	EmitGeometry(s2aa->argc(), s2aa->argv());

}

RtVoid Free(RtPointer data)
{
	PI::String2ArgcArgv* s2aa = (PI::String2ArgcArgv *)data;
	delete s2aa;
}

#if defined(__cplusplus) || defined(c_plusplus)
}
#endif

/*!
 * \note This should be harmless when compiled as a procedural "DynamicLoad" but will
 *       be the entry point for procedural "RunProgram"
 */
int main(int argc, char** argv)
{
	google::InitGoogleLogging(argv[0]);
	RtFloat normalize_pixel_coverage;
	DLOG(INFO) << boost::format("normalize_pixel_coverage = %1%") % normalize_pixel_coverage;
	char  args[BUFSIZ];

	while(fgets(args,BUFSIZ,stdin) != NULL) {
		std::string args_string(args);
		args_string.erase(std::remove(args_string.begin(), args_string.end(), '\n'), args_string.end());
		DLOG(INFO) << boost::format("args_string = '%1%'") % args_string << std::endl;
		// std::cerr << boost::format("args = '%1%'") % args << std::endl;
		PI::String2ArgcArgv s2aa(args_string);
		sscanf(args, "%g", &normalize_pixel_coverage);
		RiBegin("stdout");
		RiAttributeBegin();
		EmitGeometry(s2aa.argc(), s2aa.argv());
		RiAttributeEnd();
		RiArchiveRecord(RI_VERBATIM, "\377");
		RiEnd();
	}
	return 0;
}

// == Emacs ================
// -------------------------
// Local variables:
// tab-width: 4
// indent-tabs-mode: t
// c-basic-offset: 4
// end:
//
// == vi ===================
// -------------------------
// Format block
// ex:ts=4:sw=4:expandtab
// -------------------------


text:
#pragma once

#include <iostream>
#include <boost/thread/thread.hpp>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <vector>

//-------------------------------------------------------------------------------------------------------------------//

/* reference: https://kezunlin.me/post/f241bd30/ */

class ThreadPool {
public:
    ThreadPool(unsigned int num_threads) : io_work(ios) {
        for (int i = 0; i < num_threads; i++) {
            threads.create_thread(boost::bind(&boost::asio::io_service::run, &ios));
        }
    }

    ~ThreadPool() {
        ios.stop();
        try { // suppress all exceptions
            threads.join_all(); // wait for all threads to terminate
        }
        catch ( const std::exception& ) {}
    }

    template<class Function>
    void execute(Function func) {
        ios.post(func); // add the function to the io_service queue
                        // to be run in the threads created in the constructor
        // non-blocking, return immediately

        /* if there aren't available threads in the pool, i.e. every
           thread in the pool is already busy executing some other
           function, the new coming thread has to wait until one of
           the running thread is finished. */

        /* No counter provided to track the number of free threads
           due to implementation overheads with wrapping func with
           a counter addition when function finishes, and at the same
           time supporting boost::bind externally for passing func.
           Refer to this post for more details:
                https://stackoverflow.com/questions/12215395/thread-pool-using-boost-asio/12267138#12267138
        */
       num_tasks++;
    }

    /* total CUMULATIVE number of tasks ever being posted by ThreadPool::execute,
     * which also include those tasks that are already finished
     * */
    unsigned int num_posted_funcs() {
        return num_tasks;
    }

private:
    boost::thread_group threads;
    boost::asio::io_service ios;
    boost::asio::io_service::work io_work;
    unsigned int num_tasks = 0; // this includes those tasks that finished early and got dequeued,
                            // this measurement can't deduce anything about the
                            // the current available free threads

};

//-------------------------------------------------------------------------------------------------------------------//