<commit_before>#include "calibrator.hpp"


#include <NaturalNeighbourInterpolator.hpp>
#include <ChessboardSampling.hpp>
#include <PlaneFit.hpp>
#include <resampler.hpp>

#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>


#include <chrono>
#include <cmath>

#include <map>
#include <algorithm>
#include <unistd.h>
#include <limits>
#include <fstream>

#include <string.h> // memset

namespace{

  template <class T>
  inline std::string
  toString(T value)
  {
    std::ostringstream stream;
    stream << value;
    return stream.str();
  }


  float
  gauss(float x, float sigma, float mean){
    return (1.0f/(sigma*sqrt(2.0f * M_PI))) * exp( -0.5f * ((x-mean)/sigma) * ((x-mean)/sigma) );
  }


}

/*static*/ bool Calibrator::using_nni = false;

Calibrator::Calibrator()
  : m_nni_possible(0)
{}


Calibrator::~Calibrator(){
  if(m_nni_possible){
    delete [] m_nni_possible;
  }
}



void
Calibrator::applySamples(CalibVolume* cv, const std::vector<samplePoint>& sps, const RGBDConfig& cfg, unsigned idwneighbours, const char* basefilename, RGBDSensor* sensor, const glm::mat4* eye_d_to_world){

  auto start_time = std::chrono::system_clock::now();
  //CGAL : build Tree, search 100 neighbors, try NNI of neighbourhood, fallback to IDW small neighborhood
  std::cout << "INFO: Calibrator::applySamples applying " << sps.size()
	    << " for volume using on-the-fly initial calibration: " << (sensor != 0 ? "yes" : "no") << std::endl;

  std::vector<nniSample> nnisamples;

  for(unsigned s = 0; s < sps.size(); ++s){
    
    nniSample nnis;
    nnis.quality = sps[s/*sample point*/].quality;

    nnis.s_pos_cs  = sps[s/*sample point*/].pos_real;

    nnis.s_pos.x = cv->width *  ( sps[s].tex_depth.u)/ cfg.size_d.x;
    nnis.s_pos.y = cv->height *  ( sps[s].tex_depth.v)/ cfg.size_d.y;
    nnis.s_pos.z = cv->depth  *  (sps[s].depth - cv->min_d) / (cv->max_d - cv->min_d);
    if( !((sps[s].depth > cv->min_d) && (sps[s].depth < cv->max_d)) ){
      std::cerr << "ERROR: calibrator.cpp: skipping sample because invalid depth: " << sps[s] << std::endl;
      continue;
    }


    // here we will introduce a small error because we have a tri-linear interpolation already in the first stage
    nnis.s_pos_off = sps[s/*sample point*/].pos_offset; // in world coordinates (metric) 
    nnis.s_tex_off = sps[s/*sample point*/].tex_offset; // normalized between 0...1

    if(0 && sensor && eye_d_to_world /*use on the fly offset computation from initial calibration to avoid above error*/){

      const float depth = sps[s].depth;
      const float xd = sps[s].tex_depth.u;
      const float yd = sps[s].tex_depth.v;

      glm::vec3 pos3D_local = sensor->calc_pos_d(xd, yd, depth);
      glm::vec2 pos2D_rgb   = sensor->calc_pos_rgb(pos3D_local);
      pos2D_rgb.x /= sensor->config.size_rgb.x;
      pos2D_rgb.y /= sensor->config.size_rgb.y;

      glm::vec4 pos3D_world = (*eye_d_to_world) * glm::vec4(pos3D_local.x, pos3D_local.y, pos3D_local.z, 1.0);

      xyz pos3D;
      pos3D.x = pos3D_world.x;
      pos3D.y = pos3D_world.y;
      pos3D.z = pos3D_world.z;
      
      
      uv posUV;
      posUV.u = pos2D_rgb.x;
      posUV.v = pos2D_rgb.y;
      std::cout << "INFO: s_pos_off before: " << nnis.s_pos_off << std::endl;
#if 0
      xyz pos_tmp;
      xyz pos_tmp_tri = getTrilinear(cv->cv_xyz, cv->width, cv->height, cv->depth,
				     nnis.s_pos.x ,nnis.s_pos.y , nnis.s_pos.z );
      pos_tmp.x = sps[s/*sample point*/].pos_real[0] - pos_tmp_tri.x;
      pos_tmp.y = sps[s/*sample point*/].pos_real[1] - pos_tmp_tri.y;
      pos_tmp.z = sps[s/*sample point*/].pos_real[2] - pos_tmp_tri.z;
#endif
      //std::cout << "INFO: s_pos_off with Trilinear: " << pos_tmp << std::endl;
      //std::cout << "INFO: pos_tmp_tri: " << pos_tmp_tri << std::endl;
      //std::cout << "INFO: pos3D_onthefly: " << pos3D << std::endl;
      
      nnis.s_pos_off.x = sps[s/*sample point*/].pos_real[0] - pos3D.x;
      nnis.s_pos_off.y = sps[s/*sample point*/].pos_real[1] - pos3D.y;
      nnis.s_pos_off.z = sps[s/*sample point*/].pos_real[2] - pos3D.z;
      std::cout << "INFO: s_pos_off after: " << nnis.s_pos_off << std::endl;
      uv sps_tex_color_norm = sps[s/*sample point*/].tex_color;
      sps_tex_color_norm.u /= sensor->config.size_rgb.x;
      sps_tex_color_norm.v /= sensor->config.size_rgb.y;
      std::cout << "INFO: s_tex_off before: " << nnis.s_tex_off << std::endl;
      nnis.s_tex_off = sps_tex_color_norm - posUV;
      std::cout << "INFO: s_tex_off after: " << nnis.s_tex_off << std::endl;
    }

    
    


    //std::cerr << s << " " << nnis << std::endl;
    nnisamples.push_back(nnis);
  }

  
  Resampler rsa;
  rsa.resampleGridBased(nnisamples, cv,	basefilename);

#if 0
  std::cout << "INFO: initializing nearest neighbor search for border fill " << nnisamples.size() << " samples." << std::endl;
  NearestNeighbourSearch nns_before_border_fill(nnisamples);
  rsa.fillBorder(nnisamples, cv, &nns_before_border_fill, idwneighbours, basefilename);
  //exit(0);
#endif

  std::cout << "INFO: initializing nearest neighbor search for interpolation of calibvolume using " << nnisamples.size() << " samples." << std::endl;
  NearestNeighbourSearch nns(nnisamples);

  // init calib volume for natural neighbor interpolation
  NaturalNeighbourInterpolator* nnip = 0;
  CalibVolume* cv_nni = 0;
  if(using_nni){
    if(m_nni_possible == 0){
      m_nni_possible = new unsigned char [cv->width * cv->height * cv->depth];
    }
    memset(m_nni_possible, 0, cv->width * cv->height * cv->depth);
    cv_nni = new CalibVolume(cv->width, cv->height, cv->depth, cv->min_d, cv->max_d);
    std::cout << "INFO: initializing natural neighbor interpolation for interpolation of calibvolume using " << nnisamples.size() << " samples." << std::endl;
    std::shuffle(std::begin(nnisamples), std::end(nnisamples), std::default_random_engine());
    nnip =   new NaturalNeighbourInterpolator(nnisamples);
  }

  const unsigned numthreads = 32;
  std::cout << "INFO: start interpolation per thread for " << numthreads << " threads." << std::endl;
  boost::thread_group threadGroup;
  for (unsigned tid = 0; tid < numthreads; ++tid){
    threadGroup.create_thread(boost::bind(&Calibrator::applySamplesPerThread, this, cv, &nns, tid, numthreads, idwneighbours, cv_nni, nnip));
  }
  threadGroup.join_all();

  
  if(using_nni){
    blendIDW2NNI(cv, cv_nni, basefilename);
  }

  auto end_time = std::chrono::system_clock::now();
  std::cout << "INFO: finished interpolation in "
	    <<  std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time).count()
	    << " seconds" << std::endl;

}


void
Calibrator::blendIDW2NNI(CalibVolume* cv, CalibVolume* cv_nni, const char* basefilename){

  std::cerr << "USING NNI therefore blending IDW into NNI" << std::endl;

  // print stats for nni_possible and write to volume file
  size_t possible = 0;
  for(size_t idx = 0; idx < (cv->width * cv->height * cv->depth); ++idx){
    possible += m_nni_possible[idx] > 0 ? 1 : 0;
  }
  std::cerr << "natural_neighbor_interpolation_was_possible for: "
	    << possible << " out of: " << (cv->width * cv->height * cv->depth)
	    << std::endl;

  // 1st pass detect border
  unsigned char* nni_border = new unsigned char[cv->width * cv->height * cv->depth];
  memset(nni_border, 0, cv->width * cv->height * cv->depth);
  const int border_size = 1;
  for(unsigned z = 0; z < cv->depth; ++z){
    for(unsigned y = 0; y < cv->height; ++y){
      for(unsigned x = 0; x < cv->width; ++x){
	const unsigned cv_index = (z * cv->width * cv->height) + (y * cv->width) + x;
	if(m_nni_possible[cv_index]){
	  bool border = false;
	  for(int z_l = std::max(0 , (int(z) - border_size));
	      z_l < std::min(int(cv->depth), (int(z) + border_size + 1));
	      ++z_l){
	    for(int y_l = std::max(0 , (int(y) - border_size));
		y_l < std::min(int(cv->height), (int(y) + border_size + 1));
		++y_l){
	      for(int x_l = std::max(0 , (int(x) - border_size));
		  x_l < std::min(int(cv->width), (int(x) + border_size + 1));
		  ++x_l){
		const unsigned cv_index_l = (z_l * cv->width * cv->height) + (y_l * cv->width) + x_l;
		if(0 == m_nni_possible[cv_index_l]){
		  border = true;
		}
	      }
	    }
	  }
	  nni_border[cv_index] = border ? 255 : 0;
	}
      }
    }
  }

  // 2nd pass blend between NNI and IDW
  unsigned char* nni_percentage = new unsigned char [cv->width * cv->height * cv->depth];
  memset(nni_percentage, 0, cv->width * cv->height * cv->depth);
  const int kernel_size = 8;
  const float min_dist = 1.0f;
  const float max_dist = 6.0f;

  for(unsigned z = 0; z < cv->depth; ++z){
    for(unsigned y = 0; y < cv->height; ++y){
      for(unsigned x = 0; x < cv->width; ++x){
	const unsigned cv_index = (z * cv->width * cv->height) + (y * cv->width) + x;
	if(m_nni_possible[cv_index]){
	  float distance_to_border = std::numeric_limits<float>::max();
	  glm::vec3 voxel_curr(1.0f * x, 1.0f * y, 1.0f * z);
	  for(int z_l = std::max(0 , (int(z) - kernel_size));
	      z_l < std::min(int(cv->depth), (int(z) + kernel_size + 1));
	      ++z_l){
	    for(int y_l = std::max(0 , (int(y) - kernel_size));
		y_l < std::min(int(cv->height), (int(y) + kernel_size + 1));
		++y_l){
	      for(int x_l = std::max(0 , (int(x) - kernel_size));
		  x_l < std::min(int(cv->width), (int(x) + kernel_size + 1));
		  ++x_l){
		const unsigned cv_index_l = (z_l * cv->width * cv->height) + (y_l * cv->width) + x_l;
		if(nni_border[cv_index_l] > 0){
		  distance_to_border = std::min(distance_to_border,
						glm::length(voxel_curr - glm::vec3(1.0f * x_l, 1.0f * y_l, 1.0f * z_l)));
		}
	      }
	    }
	  }
  
	  const float t_blend = std::min( max_dist, std::max(min_dist, distance_to_border)) / max_dist;
	  if(t_blend > 1.0f || t_blend < 0.0f){
	    std::cerr << "ERROR: invalid t_blend: " << t_blend << std::endl;
	    exit(0);
	  }

	  nni_percentage[cv_index] = (unsigned char) std::max(0.0f, std::min(255.0f, 255.0f * t_blend));
	  const xyz xyz_idw = cv->cv_xyz[cv_index];
	  const xyz xyz_nni = cv_nni->cv_xyz[cv_index];
	  
	  const uv uv_idw = cv->cv_uv[cv_index];
	  const uv uv_nni = cv_nni->cv_uv[cv_index];
	  
	  cv->cv_xyz[cv_index] = interpolate(xyz_idw, xyz_nni, t_blend);
	  cv->cv_uv[cv_index]  = interpolate(uv_idw , uv_nni , t_blend);

	}
      }
    }
  }


  std::cerr << "writing " << (std::string(basefilename) + "_nnistats").c_str() << std::endl;
  FILE* f_nni_stats = fopen((std::string(basefilename) + "_nnistats").c_str(), "wb");
  fwrite(m_nni_possible, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_stats);
  fclose(f_nni_stats);


  std::cerr << "writing " << (std::string(basefilename) + "_nniborder").c_str() << std::endl;
  FILE* f_nni_border = fopen((std::string(basefilename) + "_nniborder").c_str(), "wb");
  fwrite(nni_border, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_border);
  fclose(f_nni_border);
  
  std::cerr << "writing " << (std::string(basefilename) + "_percentage").c_str() << std::endl;
  FILE* f_nni_percentage = fopen((std::string(basefilename) + "_percentage").c_str(), "wb");
  fwrite(nni_percentage, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_percentage);
  fclose(f_nni_percentage);


  delete [] nni_border;
  delete [] nni_percentage;

}

void
Calibrator::evaluateSamples(CalibVolume* cv, std::vector<samplePoint>& sps, const RGBDConfig& cfg, const char* basefilename, bool isnni){

  unsigned char* cv_nnistats = 0;
  unsigned nni_valids = 0;
  if(isnni){
    cv_nnistats = new unsigned char [cv->width * cv->height * cv->depth];
    // load .cv_nnistats if nni has to be evaluated
    FILE* f_nni_stats = fopen((std::string(basefilename) + "_nnistats").c_str(), "rb");
    fread(cv_nnistats, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_stats);
    fclose(f_nni_stats);
  }

  std::vector<nniSample> nnisamples_error_vol;

  std::vector<float> errors_3D;
  std::vector<float> errors_2D;
  float max_3D = std::numeric_limits<float>::lowest();
  float max_2D = std::numeric_limits<float>::lowest();


  // ranges in depth 0.5-1.75 1.75-3.0 3-4.5
  const float range_A_start = 0.5;
  const float range_A_end   = 1.25;
  float range_A_max_3D = std::numeric_limits<float>::lowest();
  float range_A_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_A_errors_3D;
  std::vector<float> range_A_errors_2D;

  const float range_B_start = 1.25;
  const float range_B_end   = 2.0;
  float range_B_max_3D = std::numeric_limits<float>::lowest();
  float range_B_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_B_errors_3D;
  std::vector<float> range_B_errors_2D;

  const float range_C_start = 2.0;
  const float range_C_end   = 3.0;
  float range_C_max_3D = std::numeric_limits<float>::lowest();
  float range_C_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_C_errors_3D;
  std::vector<float> range_C_errors_2D;

  
  for(unsigned i = 0; i < sps.size(); ++i){
    const unsigned cv_width = cv->width;
    const unsigned cv_height = cv->height;
    const unsigned cv_depth = cv->depth;

    const float x = cv_width  *  ( sps[i].tex_depth.u) / cfg.size_d.x;
    const float y = cv_height *  ( sps[i].tex_depth.v)/ cfg.size_d.y;
    const float z = cv_depth  *  ( sps[i].depth - cv->min_d)/(cv->max_d - cv->min_d);


    if(isnni){
      const unsigned cv_index000 = (std::floor(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::floor(x);
      const unsigned cv_index001 = (std::floor(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::ceil(x);
      const unsigned cv_index010 = (std::floor(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::floor(x);
      const unsigned cv_index011 = (std::floor(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::ceil(x);
      const unsigned cv_index100 = (std::ceil(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::floor(x);
      const unsigned cv_index101 = (std::ceil(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::ceil(x);
      const unsigned cv_index110 = (std::ceil(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::floor(x);
      const unsigned cv_index111 = (std::ceil(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::ceil(x);
      bool nni_valid  = ( cv_nnistats[cv_index000]
			  && cv_nnistats[cv_index001]
			  && cv_nnistats[cv_index010]
			  && cv_nnistats[cv_index011]
			  && cv_nnistats[cv_index100]
			  && cv_nnistats[cv_index101]
			  && cv_nnistats[cv_index110]
			  && cv_nnistats[cv_index111]);
      if(!nni_valid){
	continue;
      }
      else{
	++nni_valids;
      }
    }


    xyz pos = getTrilinear(cv->cv_xyz, cv_width, cv_height, cv_depth, x , y , z );
    uv  tex = getTrilinear(cv->cv_uv,  cv_width, cv_height, cv_depth, x , y , z );

    sps[i].pos_offset.x = sps[i].pos_real[0] - pos.x;
    sps[i].pos_offset.y = sps[i].pos_real[1] - pos.y;
    sps[i].pos_offset.z = sps[i].pos_real[2] - pos.z;
      
    sps[i].tex_offset.u = sps[i].tex_color.u/cfg.size_rgb.x - tex.u;
    sps[i].tex_offset.v = sps[i].tex_color.v/cfg.size_rgb.y - tex.v;

    sps[i].quality = 1.0f;

    const float err_3D(glm::length(glm::vec3(sps[i].pos_offset.x,sps[i].pos_offset.y,sps[i].pos_offset.z)));
    const float err_2D(glm::length(glm::vec3(sps[i].tex_offset.u * cfg.size_rgb.x,
					      sps[i].tex_offset.v * cfg.size_rgb.y,0.0)));

    errors_3D.push_back(err_3D);
    errors_2D.push_back(err_2D);
   
    max_3D = std::max(max_3D, err_3D);
    max_2D = std::max(max_2D, err_2D);

    // track local error for volume vis
    nniSample nnis;
    nnis.s_tex_off.u = err_3D;
    nnis.s_tex_off.v = err_2D;
    
    nnis.s_pos.x = x;
    nnis.s_pos.y = y;
    nnis.s_pos.z = z;
    
    nnisamples_error_vol.push_back(nnis);


    if(range_A_start < sps[i].depth &&
       range_A_end >=  sps[i].depth){
      range_A_errors_3D.push_back(err_3D);
      range_A_errors_2D.push_back(err_2D);
      range_A_max_3D = std::max(range_A_max_3D, err_3D);
      range_A_max_2D = std::max(range_A_max_2D, err_2D);
    }
    else if(range_B_start < sps[i].depth &&
       range_B_end >=  sps[i].depth){
      range_B_errors_3D.push_back(err_3D);
      range_B_errors_2D.push_back(err_2D);
      range_B_max_3D = std::max(range_B_max_3D, err_3D);
      range_B_max_2D = std::max(range_B_max_2D, err_2D);
    }
    else if(range_C_start < sps[i].depth &&
       range_C_end >=  sps[i].depth){
      range_C_errors_3D.push_back(err_3D);
      range_C_errors_2D.push_back(err_2D);
      range_C_max_3D = std::max(range_C_max_3D, err_3D);
      range_C_max_2D = std::max(range_C_max_2D, err_2D);
    }
    else{
      ;//std::cout << "ERROR: not in range: " << sps[i].depth << std::endl;
    }

  }

  double mean3D, mean2D, sd3D, sd2D;
  calcMeanSD(errors_3D, mean3D, sd3D);
  calcMeanSD(errors_2D, mean2D, sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "Evalation of ground truth samples: " << errors_3D.size() << std::endl;
  std::cout << "mean_error_3D: " << mean3D * 1000 << " (" << sd3D * 1000 << ") [" << max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "mean_error_2D: " << mean2D << " (" << sd2D << ") [" << max_2D << "] (in pixels)" << std::endl;
  
  std::cout << "could evaluate " << nni_valids << " samples from " << sps.size() << std::endl;
  

  createErrorVis(nnisamples_error_vol, cv->width, cv->height, cv->depth, basefilename, isnni);


  double range_A_mean3D, range_A_mean2D, range_A_sd3D, range_A_sd2D;
  calcMeanSD(range_A_errors_3D, range_A_mean3D, range_A_sd3D);
  calcMeanSD(range_A_errors_2D, range_A_mean2D, range_A_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_A_start << "m, " << range_A_end << "m]" << std::endl;
  std::cout << "Evalation of ground truth samples range_A: " << range_A_errors_3D.size() << std::endl;
  std::cout << "range_A_mean_error_3D: " << range_A_mean3D * 1000 << " (" << range_A_sd3D * 1000 << ") [" << range_A_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_A_mean_error_2D: " << range_A_mean2D << " (" << range_A_sd2D << ") [" << range_A_max_2D << "] (in pixels)" << std::endl;

  double range_B_mean3D, range_B_mean2D, range_B_sd3D, range_B_sd2D;
  calcMeanSD(range_B_errors_3D, range_B_mean3D, range_B_sd3D);
  calcMeanSD(range_B_errors_2D, range_B_mean2D, range_B_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_B_start << "m, " << range_B_end << "m]" << std::endl;
  std::cout << "Evalation of ground truth samples range_B: " << range_B_errors_3D.size() << std::endl;
  std::cout << "range_B_mean_error_3D: " << range_B_mean3D * 1000 << " (" << range_B_sd3D * 1000 << ") [" << range_B_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_B_mean_error_2D: " << range_B_mean2D << " (" << range_B_sd2D << ") [" << range_B_max_2D << "] (in pixels)" << std::endl;

  double range_C_mean3D, range_C_mean2D, range_C_sd3D, range_C_sd2D;
  calcMeanSD(range_C_errors_3D, range_C_mean3D, range_C_sd3D);
  calcMeanSD(range_C_errors_2D, range_C_mean2D, range_C_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_C_start << "m, " << range_C_end << "m]" << std::endl;
  std::cout << "Evalation of ground truth samples range_C: " << range_C_errors_3D.size() << std::endl;
  std::cout << "range_C_mean_error_3D: " << range_C_mean3D * 1000 << " (" << range_C_sd3D * 1000 << ") [" << range_C_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_C_mean_error_2D: " << range_C_mean2D << " (" << range_C_sd2D << ") [" << range_C_max_2D << "] (in pixels)" << std::endl;

  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << range_A_mean3D << std::endl;
  std::cout << range_A_sd3D << std::endl;
  std::cout << range_A_mean2D << std::endl;
  std::cout << range_A_sd2D << std::endl;

  std::cout << range_B_mean3D << std::endl;
  std::cout << range_B_sd3D << std::endl;
  std::cout << range_B_mean2D << std::endl;
  std::cout << range_B_sd2D << std::endl;

  std::cout << range_C_mean3D << std::endl;
  std::cout << range_C_sd3D << std::endl;
  std::cout << range_C_mean2D << std::endl;
  std::cout << range_C_sd2D << std::endl;


}

void
Calibrator::createErrorVis(const std::vector<nniSample>& sps, const unsigned width, const unsigned height, const unsigned depth, const char* basefilename, bool isnni){

  // create volumes for world and texture coordinates
  unsigned char* error_vol_3D = new unsigned char [width * height * depth];
  memset(error_vol_3D, 0, width * height * depth);
  
  unsigned char* error_vol_2D = new unsigned char [width * height * depth];
  memset(error_vol_2D, 0, width * height * depth);

  unsigned char* error_vol_3D_nni = new unsigned char [width * height * depth];
  memset(error_vol_3D_nni, 0, width * height * depth);
  
  unsigned char* error_vol_2D_nni = new unsigned char [width * height * depth];
  memset(error_vol_2D_nni, 0, width * height * depth);<commit_msg>write stats in millimeter<commit_after>#include "calibrator.hpp"


#include <NaturalNeighbourInterpolator.hpp>
#include <ChessboardSampling.hpp>
#include <PlaneFit.hpp>
#include <resampler.hpp>

#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>


#include <chrono>
#include <cmath>

#include <map>
#include <algorithm>
#include <unistd.h>
#include <limits>
#include <fstream>

#include <string.h> // memset

namespace{

  template <class T>
  inline std::string
  toString(T value)
  {
    std::ostringstream stream;
    stream << value;
    return stream.str();
  }


  float
  gauss(float x, float sigma, float mean){
    return (1.0f/(sigma*sqrt(2.0f * M_PI))) * exp( -0.5f * ((x-mean)/sigma) * ((x-mean)/sigma) );
  }


}

/*static*/ bool Calibrator::using_nni = false;

Calibrator::Calibrator()
  : m_nni_possible(0)
{}


Calibrator::~Calibrator(){
  if(m_nni_possible){
    delete [] m_nni_possible;
  }
}



void
Calibrator::applySamples(CalibVolume* cv, const std::vector<samplePoint>& sps, const RGBDConfig& cfg, unsigned idwneighbours, const char* basefilename, RGBDSensor* sensor, const glm::mat4* eye_d_to_world){

  auto start_time = std::chrono::system_clock::now();
  //CGAL : build Tree, search 100 neighbors, try NNI of neighbourhood, fallback to IDW small neighborhood
  std::cout << "INFO: Calibrator::applySamples applying " << sps.size()
	    << " for volume using on-the-fly initial calibration: " << (sensor != 0 ? "yes" : "no") << std::endl;

  std::vector<nniSample> nnisamples;

  for(unsigned s = 0; s < sps.size(); ++s){
    
    nniSample nnis;
    nnis.quality = sps[s/*sample point*/].quality;

    nnis.s_pos_cs  = sps[s/*sample point*/].pos_real;

    nnis.s_pos.x = cv->width *  ( sps[s].tex_depth.u)/ cfg.size_d.x;
    nnis.s_pos.y = cv->height *  ( sps[s].tex_depth.v)/ cfg.size_d.y;
    nnis.s_pos.z = cv->depth  *  (sps[s].depth - cv->min_d) / (cv->max_d - cv->min_d);
    if( !((sps[s].depth > cv->min_d) && (sps[s].depth < cv->max_d)) ){
      std::cerr << "ERROR: calibrator.cpp: skipping sample because invalid depth: " << sps[s] << std::endl;
      continue;
    }


    // here we will introduce a small error because we have a tri-linear interpolation already in the first stage
    nnis.s_pos_off = sps[s/*sample point*/].pos_offset; // in world coordinates (metric) 
    nnis.s_tex_off = sps[s/*sample point*/].tex_offset; // normalized between 0...1

    if(0 && sensor && eye_d_to_world /*use on the fly offset computation from initial calibration to avoid above error*/){

      const float depth = sps[s].depth;
      const float xd = sps[s].tex_depth.u;
      const float yd = sps[s].tex_depth.v;

      glm::vec3 pos3D_local = sensor->calc_pos_d(xd, yd, depth);
      glm::vec2 pos2D_rgb   = sensor->calc_pos_rgb(pos3D_local);
      pos2D_rgb.x /= sensor->config.size_rgb.x;
      pos2D_rgb.y /= sensor->config.size_rgb.y;

      glm::vec4 pos3D_world = (*eye_d_to_world) * glm::vec4(pos3D_local.x, pos3D_local.y, pos3D_local.z, 1.0);

      xyz pos3D;
      pos3D.x = pos3D_world.x;
      pos3D.y = pos3D_world.y;
      pos3D.z = pos3D_world.z;
      
      
      uv posUV;
      posUV.u = pos2D_rgb.x;
      posUV.v = pos2D_rgb.y;
      std::cout << "INFO: s_pos_off before: " << nnis.s_pos_off << std::endl;
#if 0
      xyz pos_tmp;
      xyz pos_tmp_tri = getTrilinear(cv->cv_xyz, cv->width, cv->height, cv->depth,
				     nnis.s_pos.x ,nnis.s_pos.y , nnis.s_pos.z );
      pos_tmp.x = sps[s/*sample point*/].pos_real[0] - pos_tmp_tri.x;
      pos_tmp.y = sps[s/*sample point*/].pos_real[1] - pos_tmp_tri.y;
      pos_tmp.z = sps[s/*sample point*/].pos_real[2] - pos_tmp_tri.z;
#endif
      //std::cout << "INFO: s_pos_off with Trilinear: " << pos_tmp << std::endl;
      //std::cout << "INFO: pos_tmp_tri: " << pos_tmp_tri << std::endl;
      //std::cout << "INFO: pos3D_onthefly: " << pos3D << std::endl;
      
      nnis.s_pos_off.x = sps[s/*sample point*/].pos_real[0] - pos3D.x;
      nnis.s_pos_off.y = sps[s/*sample point*/].pos_real[1] - pos3D.y;
      nnis.s_pos_off.z = sps[s/*sample point*/].pos_real[2] - pos3D.z;
      std::cout << "INFO: s_pos_off after: " << nnis.s_pos_off << std::endl;
      uv sps_tex_color_norm = sps[s/*sample point*/].tex_color;
      sps_tex_color_norm.u /= sensor->config.size_rgb.x;
      sps_tex_color_norm.v /= sensor->config.size_rgb.y;
      std::cout << "INFO: s_tex_off before: " << nnis.s_tex_off << std::endl;
      nnis.s_tex_off = sps_tex_color_norm - posUV;
      std::cout << "INFO: s_tex_off after: " << nnis.s_tex_off << std::endl;
    }

    
    


    //std::cerr << s << " " << nnis << std::endl;
    nnisamples.push_back(nnis);
  }

  
  Resampler rsa;
  rsa.resampleGridBased(nnisamples, cv,	basefilename);

#if 0
  std::cout << "INFO: initializing nearest neighbor search for border fill " << nnisamples.size() << " samples." << std::endl;
  NearestNeighbourSearch nns_before_border_fill(nnisamples);
  rsa.fillBorder(nnisamples, cv, &nns_before_border_fill, idwneighbours, basefilename);
  //exit(0);
#endif

  std::cout << "INFO: initializing nearest neighbor search for interpolation of calibvolume using " << nnisamples.size() << " samples." << std::endl;
  NearestNeighbourSearch nns(nnisamples);

  // init calib volume for natural neighbor interpolation
  NaturalNeighbourInterpolator* nnip = 0;
  CalibVolume* cv_nni = 0;
  if(using_nni){
    if(m_nni_possible == 0){
      m_nni_possible = new unsigned char [cv->width * cv->height * cv->depth];
    }
    memset(m_nni_possible, 0, cv->width * cv->height * cv->depth);
    cv_nni = new CalibVolume(cv->width, cv->height, cv->depth, cv->min_d, cv->max_d);
    std::cout << "INFO: initializing natural neighbor interpolation for interpolation of calibvolume using " << nnisamples.size() << " samples." << std::endl;
    std::shuffle(std::begin(nnisamples), std::end(nnisamples), std::default_random_engine());
    nnip =   new NaturalNeighbourInterpolator(nnisamples);
  }

  const unsigned numthreads = 32;
  std::cout << "INFO: start interpolation per thread for " << numthreads << " threads." << std::endl;
  boost::thread_group threadGroup;
  for (unsigned tid = 0; tid < numthreads; ++tid){
    threadGroup.create_thread(boost::bind(&Calibrator::applySamplesPerThread, this, cv, &nns, tid, numthreads, idwneighbours, cv_nni, nnip));
  }
  threadGroup.join_all();

  
  if(using_nni){
    blendIDW2NNI(cv, cv_nni, basefilename);
  }

  auto end_time = std::chrono::system_clock::now();
  std::cout << "INFO: finished interpolation in "
	    <<  std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time).count()
	    << " seconds" << std::endl;

}


void
Calibrator::blendIDW2NNI(CalibVolume* cv, CalibVolume* cv_nni, const char* basefilename){

  std::cerr << "USING NNI therefore blending IDW into NNI" << std::endl;

  // print stats for nni_possible and write to volume file
  size_t possible = 0;
  for(size_t idx = 0; idx < (cv->width * cv->height * cv->depth); ++idx){
    possible += m_nni_possible[idx] > 0 ? 1 : 0;
  }
  std::cerr << "natural_neighbor_interpolation_was_possible for: "
	    << possible << " out of: " << (cv->width * cv->height * cv->depth)
	    << std::endl;

  // 1st pass detect border
  unsigned char* nni_border = new unsigned char[cv->width * cv->height * cv->depth];
  memset(nni_border, 0, cv->width * cv->height * cv->depth);
  const int border_size = 1;
  for(unsigned z = 0; z < cv->depth; ++z){
    for(unsigned y = 0; y < cv->height; ++y){
      for(unsigned x = 0; x < cv->width; ++x){
	const unsigned cv_index = (z * cv->width * cv->height) + (y * cv->width) + x;
	if(m_nni_possible[cv_index]){
	  bool border = false;
	  for(int z_l = std::max(0 , (int(z) - border_size));
	      z_l < std::min(int(cv->depth), (int(z) + border_size + 1));
	      ++z_l){
	    for(int y_l = std::max(0 , (int(y) - border_size));
		y_l < std::min(int(cv->height), (int(y) + border_size + 1));
		++y_l){
	      for(int x_l = std::max(0 , (int(x) - border_size));
		  x_l < std::min(int(cv->width), (int(x) + border_size + 1));
		  ++x_l){
		const unsigned cv_index_l = (z_l * cv->width * cv->height) + (y_l * cv->width) + x_l;
		if(0 == m_nni_possible[cv_index_l]){
		  border = true;
		}
	      }
	    }
	  }
	  nni_border[cv_index] = border ? 255 : 0;
	}
      }
    }
  }

  // 2nd pass blend between NNI and IDW
  unsigned char* nni_percentage = new unsigned char [cv->width * cv->height * cv->depth];
  memset(nni_percentage, 0, cv->width * cv->height * cv->depth);
  const int kernel_size = 8;
  const float min_dist = 1.0f;
  const float max_dist = 6.0f;

  for(unsigned z = 0; z < cv->depth; ++z){
    for(unsigned y = 0; y < cv->height; ++y){
      for(unsigned x = 0; x < cv->width; ++x){
	const unsigned cv_index = (z * cv->width * cv->height) + (y * cv->width) + x;
	if(m_nni_possible[cv_index]){
	  float distance_to_border = std::numeric_limits<float>::max();
	  glm::vec3 voxel_curr(1.0f * x, 1.0f * y, 1.0f * z);
	  for(int z_l = std::max(0 , (int(z) - kernel_size));
	      z_l < std::min(int(cv->depth), (int(z) + kernel_size + 1));
	      ++z_l){
	    for(int y_l = std::max(0 , (int(y) - kernel_size));
		y_l < std::min(int(cv->height), (int(y) + kernel_size + 1));
		++y_l){
	      for(int x_l = std::max(0 , (int(x) - kernel_size));
		  x_l < std::min(int(cv->width), (int(x) + kernel_size + 1));
		  ++x_l){
		const unsigned cv_index_l = (z_l * cv->width * cv->height) + (y_l * cv->width) + x_l;
		if(nni_border[cv_index_l] > 0){
		  distance_to_border = std::min(distance_to_border,
						glm::length(voxel_curr - glm::vec3(1.0f * x_l, 1.0f * y_l, 1.0f * z_l)));
		}
	      }
	    }
	  }
  
	  const float t_blend = std::min( max_dist, std::max(min_dist, distance_to_border)) / max_dist;
	  if(t_blend > 1.0f || t_blend < 0.0f){
	    std::cerr << "ERROR: invalid t_blend: " << t_blend << std::endl;
	    exit(0);
	  }

	  nni_percentage[cv_index] = (unsigned char) std::max(0.0f, std::min(255.0f, 255.0f * t_blend));
	  const xyz xyz_idw = cv->cv_xyz[cv_index];
	  const xyz xyz_nni = cv_nni->cv_xyz[cv_index];
	  
	  const uv uv_idw = cv->cv_uv[cv_index];
	  const uv uv_nni = cv_nni->cv_uv[cv_index];
	  
	  cv->cv_xyz[cv_index] = interpolate(xyz_idw, xyz_nni, t_blend);
	  cv->cv_uv[cv_index]  = interpolate(uv_idw , uv_nni , t_blend);

	}
      }
    }
  }


  std::cerr << "writing " << (std::string(basefilename) + "_nnistats").c_str() << std::endl;
  FILE* f_nni_stats = fopen((std::string(basefilename) + "_nnistats").c_str(), "wb");
  fwrite(m_nni_possible, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_stats);
  fclose(f_nni_stats);


  std::cerr << "writing " << (std::string(basefilename) + "_nniborder").c_str() << std::endl;
  FILE* f_nni_border = fopen((std::string(basefilename) + "_nniborder").c_str(), "wb");
  fwrite(nni_border, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_border);
  fclose(f_nni_border);
  
  std::cerr << "writing " << (std::string(basefilename) + "_percentage").c_str() << std::endl;
  FILE* f_nni_percentage = fopen((std::string(basefilename) + "_percentage").c_str(), "wb");
  fwrite(nni_percentage, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_percentage);
  fclose(f_nni_percentage);


  delete [] nni_border;
  delete [] nni_percentage;

}

void
Calibrator::evaluateSamples(CalibVolume* cv, std::vector<samplePoint>& sps, const RGBDConfig& cfg, const char* basefilename, bool isnni){

  unsigned char* cv_nnistats = 0;
  unsigned nni_valids = 0;
  if(isnni){
    cv_nnistats = new unsigned char [cv->width * cv->height * cv->depth];
    // load .cv_nnistats if nni has to be evaluated
    FILE* f_nni_stats = fopen((std::string(basefilename) + "_nnistats").c_str(), "rb");
    fread(cv_nnistats, sizeof(unsigned char), (cv->width * cv->height * cv->depth), f_nni_stats);
    fclose(f_nni_stats);
  }

  std::vector<nniSample> nnisamples_error_vol;

  std::vector<float> errors_3D;
  std::vector<float> errors_2D;
  float max_3D = std::numeric_limits<float>::lowest();
  float max_2D = std::numeric_limits<float>::lowest();


  // ranges in depth 0.5-1.75 1.75-3.0 3-4.5
  const float range_A_start = 0.5;
  const float range_A_end   = 1.25;
  float range_A_max_3D = std::numeric_limits<float>::lowest();
  float range_A_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_A_errors_3D;
  std::vector<float> range_A_errors_2D;

  const float range_B_start = 1.25;
  const float range_B_end   = 2.0;
  float range_B_max_3D = std::numeric_limits<float>::lowest();
  float range_B_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_B_errors_3D;
  std::vector<float> range_B_errors_2D;

  const float range_C_start = 2.0;
  const float range_C_end   = 3.0;
  float range_C_max_3D = std::numeric_limits<float>::lowest();
  float range_C_max_2D = std::numeric_limits<float>::lowest();
  std::vector<float> range_C_errors_3D;
  std::vector<float> range_C_errors_2D;

  
  for(unsigned i = 0; i < sps.size(); ++i){
    const unsigned cv_width = cv->width;
    const unsigned cv_height = cv->height;
    const unsigned cv_depth = cv->depth;

    const float x = cv_width  *  ( sps[i].tex_depth.u) / cfg.size_d.x;
    const float y = cv_height *  ( sps[i].tex_depth.v)/ cfg.size_d.y;
    const float z = cv_depth  *  ( sps[i].depth - cv->min_d)/(cv->max_d - cv->min_d);


    if(isnni){
      const unsigned cv_index000 = (std::floor(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::floor(x);
      const unsigned cv_index001 = (std::floor(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::ceil(x);
      const unsigned cv_index010 = (std::floor(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::floor(x);
      const unsigned cv_index011 = (std::floor(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::ceil(x);
      const unsigned cv_index100 = (std::ceil(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::floor(x);
      const unsigned cv_index101 = (std::ceil(z) * cv_width * cv_height) + (std::floor(y) * cv_width) + std::ceil(x);
      const unsigned cv_index110 = (std::ceil(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::floor(x);
      const unsigned cv_index111 = (std::ceil(z) * cv_width * cv_height) + (std::ceil(y) * cv_width) + std::ceil(x);
      bool nni_valid  = ( cv_nnistats[cv_index000]
			  && cv_nnistats[cv_index001]
			  && cv_nnistats[cv_index010]
			  && cv_nnistats[cv_index011]
			  && cv_nnistats[cv_index100]
			  && cv_nnistats[cv_index101]
			  && cv_nnistats[cv_index110]
			  && cv_nnistats[cv_index111]);
      if(!nni_valid){
	continue;
      }
      else{
	++nni_valids;
      }
    }


    xyz pos = getTrilinear(cv->cv_xyz, cv_width, cv_height, cv_depth, x , y , z );
    uv  tex = getTrilinear(cv->cv_uv,  cv_width, cv_height, cv_depth, x , y , z );

    sps[i].pos_offset.x = sps[i].pos_real[0] - pos.x;
    sps[i].pos_offset.y = sps[i].pos_real[1] - pos.y;
    sps[i].pos_offset.z = sps[i].pos_real[2] - pos.z;
      
    sps[i].tex_offset.u = sps[i].tex_color.u/cfg.size_rgb.x - tex.u;
    sps[i].tex_offset.v = sps[i].tex_color.v/cfg.size_rgb.y - tex.v;

    sps[i].quality = 1.0f;

    const float err_3D(glm::length(glm::vec3(sps[i].pos_offset.x,sps[i].pos_offset.y,sps[i].pos_offset.z)));
    const float err_2D(glm::length(glm::vec3(sps[i].tex_offset.u * cfg.size_rgb.x,
					      sps[i].tex_offset.v * cfg.size_rgb.y,0.0)));

    errors_3D.push_back(err_3D);
    errors_2D.push_back(err_2D);
   
    max_3D = std::max(max_3D, err_3D);
    max_2D = std::max(max_2D, err_2D);

    // track local error for volume vis
    nniSample nnis;
    nnis.s_tex_off.u = err_3D;
    nnis.s_tex_off.v = err_2D;
    
    nnis.s_pos.x = x;
    nnis.s_pos.y = y;
    nnis.s_pos.z = z;
    
    nnisamples_error_vol.push_back(nnis);


    if(range_A_start < sps[i].depth &&
       range_A_end >=  sps[i].depth){
      range_A_errors_3D.push_back(err_3D);
      range_A_errors_2D.push_back(err_2D);
      range_A_max_3D = std::max(range_A_max_3D, err_3D);
      range_A_max_2D = std::max(range_A_max_2D, err_2D);
    }
    else if(range_B_start < sps[i].depth &&
       range_B_end >=  sps[i].depth){
      range_B_errors_3D.push_back(err_3D);
      range_B_errors_2D.push_back(err_2D);
      range_B_max_3D = std::max(range_B_max_3D, err_3D);
      range_B_max_2D = std::max(range_B_max_2D, err_2D);
    }
    else if(range_C_start < sps[i].depth &&
       range_C_end >=  sps[i].depth){
      range_C_errors_3D.push_back(err_3D);
      range_C_errors_2D.push_back(err_2D);
      range_C_max_3D = std::max(range_C_max_3D, err_3D);
      range_C_max_2D = std::max(range_C_max_2D, err_2D);
    }
    else{
      ;//std::cout << "ERROR: not in range: " << sps[i].depth << std::endl;
    }

  }

  double mean3D, mean2D, sd3D, sd2D;
  calcMeanSD(errors_3D, mean3D, sd3D);
  calcMeanSD(errors_2D, mean2D, sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "Evalation of ground truth samples: " << errors_3D.size() << std::endl;
  std::cout << "mean_error_3D: " << mean3D * 1000 << " (" << sd3D * 1000 << ") [" << max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "mean_error_2D: " << mean2D << " (" << sd2D << ") [" << max_2D << "] (in pixels)" << std::endl;
  
  std::cout << "could evaluate " << nni_valids << " samples from " << sps.size() << std::endl;
  

  createErrorVis(nnisamples_error_vol, cv->width, cv->height, cv->depth, basefilename, isnni);


  double range_A_mean3D, range_A_mean2D, range_A_sd3D, range_A_sd2D;
  calcMeanSD(range_A_errors_3D, range_A_mean3D, range_A_sd3D);
  calcMeanSD(range_A_errors_2D, range_A_mean2D, range_A_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_A_start << "m, " << range_A_end << "m]" << std::endl;
  std::cout << "Evalation of ground truth samples range_A: " << range_A_errors_3D.size() << std::endl;
  std::cout << "range_A_mean_error_3D: " << range_A_mean3D * 1000 << " (" << range_A_sd3D * 1000 << ") [" << range_A_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_A_mean_error_2D: " << range_A_mean2D << " (" << range_A_sd2D << ") [" << range_A_max_2D << "] (in pixels)" << std::endl;

  double range_B_mean3D, range_B_mean2D, range_B_sd3D, range_B_sd2D;
  calcMeanSD(range_B_errors_3D, range_B_mean3D, range_B_sd3D);
  calcMeanSD(range_B_errors_2D, range_B_mean2D, range_B_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_B_start << "m, " << range_B_end << "m]" << std::endl;
  std::cout << "Evalation of ground truth samples range_B: " << range_B_errors_3D.size() << std::endl;
  std::cout << "range_B_mean_error_3D: " << range_B_mean3D * 1000 << " (" << range_B_sd3D * 1000 << ") [" << range_B_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_B_mean_error_2D: " << range_B_mean2D << " (" << range_B_sd2D << ") [" << range_B_max_2D << "] (in pixels)" << std::endl;

  double range_C_mean3D, range_C_mean2D, range_C_sd3D, range_C_sd2D;
  calcMeanSD(range_C_errors_3D, range_C_mean3D, range_C_sd3D);
  calcMeanSD(range_C_errors_2D, range_C_mean2D, range_C_sd2D);
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "(" << range_C_start << "m, " << range_C_end << "m]" << std::endl;
  std::cout << "Evalation of ground truth samples range_C: " << range_C_errors_3D.size() << std::endl;
  std::cout << "range_C_mean_error_3D: " << range_C_mean3D * 1000 << " (" << range_C_sd3D * 1000 << ") [" << range_C_max_3D * 1000 << "] (in millimeter)" << std::endl;
  std::cout << "range_C_mean_error_2D: " << range_C_mean2D << " (" << range_C_sd2D << ") [" << range_C_max_2D << "] (in pixels)" << std::endl;

  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << range_A_mean3D * 1000 << std::endl;
  std::cout << range_A_sd3D * 1000 << std::endl;
  std::cout << range_A_mean2D << std::endl;
  std::cout << range_A_sd2D << std::endl;

  std::cout << range_B_mean3D * 1000 << std::endl;
  std::cout << range_B_sd3D * 1000 << std::endl;
  std::cout << range_B_mean2D << std::endl;
  std::cout << range_B_sd2D << std::endl;

  std::cout << range_C_mean3D * 1000 << std::endl;
  std::cout << range_C_sd3D * 1000 << std::endl;
  std::cout << range_C_mean2D << std::endl;
  std::cout << range_C_sd2D << std::endl;


}

void
Calibrator::createErrorVis(const std::vector<nniSample>& sps, const unsigned width, const unsigned height, const unsigned depth, const char* basefilename, bool isnni){

  // create volumes for world and texture coordinates
  unsigned char* error_vol_3D = new unsigned char [width * height * depth];
  memset(error_vol_3D, 0, width * height * depth);
  
  unsigned char* error_vol_2D = new unsigned char [width * height * depth];
  memset(error_vol_2D, 0, width * height * depth);

  unsigned char* error_vol_3D_nni = new unsigned char [width * height * depth];
  memset(error_vol_3D_nni, 0, width * height * depth);
  
  unsigned char* error_vol_2D_nni = new unsigned char [width * height * depth];
  memset(error_vol_2D_nni, 0, width * height * depth);
{'fraction_non_alphanumeric': 0.18586473203838053, 'fraction_numerical': 0.020500819096653404, 'mean_word_length': 3.5613791631084544, 'pattern_counts': {'":': 0, '<': 967, '<?xml version=': 0, '>': 301, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2609421', 'n_tokens_mistral': 17637, 'n_tokens_neox': 16794, 'n_words': 3142}