text:
/**TODO:  Add copyright*/

#define BOOST_TEST_MODULE Preprocessing test suite
#include <boost/test/included/unit_test.hpp>
#include <EvoNet/core/Preprocessing.h>

using namespace EvoNet;
using namespace std;

BOOST_AUTO_TEST_SUITE(preprocessing)

BOOST_AUTO_TEST_CASE(P_selectRandomElement)
{
	// [TODO: make test; currently, combined with selectRandomNode1]
}

BOOST_AUTO_TEST_CASE(P_UnitScaleFunctor)
{
	Eigen::Tensor<float, 2> data(2, 2);
	data.setValues({{ 0, 2 }, { 3, 4 }});
	UnitScaleFunctor<float> unit_scale(data);
	BOOST_CHECK_CLOSE(unit_scale.getUnitScale(), 0.25, 1e-6);

	Eigen::Tensor<float, 2> data_test = data.unaryExpr(UnitScaleFunctor<float>(data));

	BOOST_CHECK_CLOSE(data_test(0, 0), 0.0, 1e-6);
	BOOST_CHECK_CLOSE(data_test(1, 1), 1.0, 1e-6);
}

BOOST_AUTO_TEST_CASE(P_LinearScaleFunctor)
{
	Eigen::Tensor<float, 2> data(2, 2);
	data.setValues({ { 0, 2 }, { 4, 8 } });

	Eigen::Tensor<float, 2> data_test = data.unaryExpr(LinearScaleFunctor<float>(0, 8, -1, 1));

	BOOST_CHECK_CLOSE(data_test(0, 0), -1.0, 1e-6);
	BOOST_CHECK_CLOSE(data_test(0, 1), -0.5, 1e-6);
	BOOST_CHECK_CLOSE(data_test(1, 0), 0.0, 1e-6);
	BOOST_CHECK_CLOSE(data_test(1, 1), 1.0, 1e-6);
}

BOOST_AUTO_TEST_CASE(P_LinearScale)
{
  Eigen::Tensor<float, 3> data(2, 2, 2);
  data.setValues({
    {{ 0, 2 }, { 4, 8 }},
    {{ 1, 1 }, { 3, 5 }}
    });

  // Test default initialization for the domain and setters
  LinearScale<float, 3> linearScale1(-1, 1);
  linearScale1.setDomain(0, 8);
  Eigen::Tensor<float, 3> data_test = linearScale1(data);

  BOOST_CHECK_CLOSE(data_test(0, 0, 0), -1.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 0, 1), -0.5, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 1, 0), 0.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 1, 1), 1.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 0, 0), -0.75, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 0, 1), -0.75, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 1, 0), -0.25, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 1, 1), 0.25, 1e-6);

  // Test with manual domain and range initialization
  LinearScale<float, 3> linearScale(0, 8, -1, 1);
  data_test = linearScale(data);

  BOOST_CHECK_CLOSE(data_test(0, 0, 0), -1.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 0, 1), -0.5, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 1, 0), 0.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 1, 1), 1.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 0, 0), -0.75, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 0, 1), -0.75, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 1, 0), -0.25, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 1, 1), 0.25, 1e-6);

  // Test with domain calculation and range initialization
  LinearScale<float, 3> linearScale2(data, -1, 1);
  data_test = linearScale2(data);

  BOOST_CHECK_CLOSE(data_test(0, 0, 0), -1.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 0, 1), -0.5, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 1, 0), 0.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 1, 1), 1.0, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 0, 0), -0.75, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 0, 1), -0.75, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 1, 0), -0.25, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 1, 1), 0.25, 1e-6);
}

BOOST_AUTO_TEST_CASE(P_Standardize)
{
  Eigen::Tensor<float, 3> data(2, 2, 2);
  data.setValues({
    {{ 0, 2 }, { 4, 8 }},
    {{ 1, 3 }, { 3, 5 }}
    });

  // Test default initialization with setters and getters
  Standardize<float, 3> standardize1;
  standardize1.setMeanAndVar(1, 2);
  BOOST_CHECK_CLOSE(standardize1.getMean(), 1, 1e-6);
  BOOST_CHECK_CLOSE(standardize1.getVar(), 2, 1e-6);
  standardize1.setMeanAndVar(data);
  BOOST_CHECK_CLOSE(standardize1.getMean(), 3.25, 1e-6);
  BOOST_CHECK_CLOSE(standardize1.getVar(), 6.21428585, 1e-6);

  // Test with data initialization and getters
  Standardize<float, 3> standardize(data);
  BOOST_CHECK_CLOSE(standardize.getMean(), 3.25, 1e-6);
  BOOST_CHECK_CLOSE(standardize.getVar(), 6.21428585, 1e-6);

  // Test operator
  Eigen::Tensor<float, 3> data_test = standardize(data);
  BOOST_CHECK_CLOSE(data_test(0, 0, 0), -1.30373025, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 0, 1), -0.501434684, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 1, 0), 0.300860822, 1e-6);
  BOOST_CHECK_CLOSE(data_test(0, 1, 1), 1.90545189, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 0, 0), -0.902582467, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 0, 1), -0.100286946, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 1, 0), -0.100286946, 1e-6);
  BOOST_CHECK_CLOSE(data_test(1, 1, 1), 0.702008605, 1e-6);
}

BOOST_AUTO_TEST_CASE(P_MakeShuffleMatrix)
{
  const int shuffle_dim_size = 8;
  std::vector<int> indices = { 0, 1, 2, 3, 4, 5, 6, 7 };

  // Test default initialization with setters and getters
  MakeShuffleMatrix<float> shuffle1;
  shuffle1.setIndices(8);
  BOOST_CHECK(shuffle1.getIndices() != indices);
  for (int i = 0; i < shuffle_dim_size; ++i) {
    BOOST_CHECK_GE(shuffle1.getIndices().at(i), 0);
    BOOST_CHECK_LE(shuffle1.getIndices().at(i), 7);
  }
  shuffle1.setShuffleMatrix(true);
  //std::cout << "Shuffle_matrix\n" << shuffle1.getShuffleMatrix() << std::endl;
  for (int i = 0; i < shuffle_dim_size; ++i) {
    Eigen::Tensor<float, 0> row_sum = shuffle1.getShuffleMatrix().chip(i, 0).sum();
    BOOST_CHECK_EQUAL(row_sum(0), 1);
  }

  // Test initialization with dim size
  MakeShuffleMatrix<float> shuffle2(shuffle_dim_size, true);
  BOOST_CHECK(shuffle2.getIndices() != indices);
  for (int i = 0; i < shuffle_dim_size; ++i) {
    BOOST_CHECK_GE(shuffle2.getIndices().at(i), 0);
    BOOST_CHECK_LE(shuffle2.getIndices().at(i), 7);
  }

  // Test initialization with indices to use
  MakeShuffleMatrix<float> shuffle3(indices, true);
  BOOST_CHECK(shuffle3.getIndices() == indices);
  //std::cout << "Shuffle_matrix\n" << shuffle3.getShuffleMatrix() << std::endl;
  for (int i = 0; i < shuffle_dim_size; ++i) {
    BOOST_CHECK_EQUAL(shuffle3.getShuffleMatrix()(i, i), 1);
    Eigen::Tensor<float, 0> row_sum = shuffle3.getShuffleMatrix().chip(i, 0).sum();
    BOOST_CHECK_EQUAL(row_sum(0), 1);
  }

  // Test row/column shuffling on toy data
  Eigen::Tensor<float, 2> data(2, 3);
  data.setValues({ {1,2,3},{4,5,6} });
  MakeShuffleMatrix<float> shuffle_col(std::vector<int>({1,2,0}), true);
  Eigen::Tensor<float, 2> col_shuffle = data;
  shuffle_col(col_shuffle, true);
  BOOST_CHECK_EQUAL(col_shuffle(0, 0), 2);
  BOOST_CHECK_EQUAL(col_shuffle(0, 1), 3);
  BOOST_CHECK_EQUAL(col_shuffle(0, 2), 1);
  BOOST_CHECK_EQUAL(col_shuffle(1, 0), 5);
  BOOST_CHECK_EQUAL(col_shuffle(1, 1), 6);
  BOOST_CHECK_EQUAL(col_shuffle(1, 2), 4);
  MakeShuffleMatrix<float> shuffle_row(std::vector<int>({ 1,0 }), false);
  Eigen::Tensor<float, 2> row_shuffle = data;
  shuffle_row(row_shuffle, false);
  BOOST_CHECK_EQUAL(row_shuffle(0, 0), 4);
  BOOST_CHECK_EQUAL(row_shuffle(0, 1), 5);
  BOOST_CHECK_EQUAL(row_shuffle(0, 2), 6);
  BOOST_CHECK_EQUAL(row_shuffle(1, 0), 1);
  BOOST_CHECK_EQUAL(row_shuffle(1, 1), 2);
  BOOST_CHECK_EQUAL(row_shuffle(1, 2), 3);

  // Test row/column shuffling on toy data
  Eigen::Tensor<double, 2> data_db(2, 3);
  data_db.setValues({ {1,2,3},{4,5,6} });
  MakeShuffleMatrix<double> shuffle_col_db(std::vector<int>({ 1,2,0 }), true);
  Eigen::Tensor<double, 2> col_shuffle_db = data_db;
  shuffle_col_db(col_shuffle_db, true);
  BOOST_CHECK_EQUAL(col_shuffle_db(0, 0), 2);
  BOOST_CHECK_EQUAL(col_shuffle_db(0, 1), 3);
  BOOST_CHECK_EQUAL(col_shuffle_db(0, 2), 1);
  BOOST_CHECK_EQUAL(col_shuffle_db(1, 0), 5);
  BOOST_CHECK_EQUAL(col_shuffle_db(1, 1), 6);
  BOOST_CHECK_EQUAL(col_shuffle_db(1, 2), 4);
  MakeShuffleMatrix<double> shuffle_row_db(std::vector<int>({ 1,0 }), false);
  Eigen::Tensor<double, 2> row_shuffle_db = data_db;
  shuffle_row_db(row_shuffle_db, false);
  BOOST_CHECK_EQUAL(row_shuffle_db(0, 0), 4);
  BOOST_CHECK_EQUAL(row_shuffle_db(0, 1), 5);
  BOOST_CHECK_EQUAL(row_shuffle_db(0, 2), 6);
  BOOST_CHECK_EQUAL(row_shuffle_db(1, 0), 1);
  BOOST_CHECK_EQUAL(row_shuffle_db(1, 1), 2);
  BOOST_CHECK_EQUAL(row_shuffle_db(1, 2), 3);
}

BOOST_AUTO_TEST_CASE(P_LabelSmoother)
{
	Eigen::Tensor<float, 1> data(2);
	data.setValues({ 0, 1 });

	Eigen::Tensor<float, 1> data_test = data.unaryExpr(LabelSmoother<float>(0.1, 0.2));

	BOOST_CHECK_CLOSE(data_test(0), 0.1, 1e-4);
	BOOST_CHECK_CLOSE(data_test(1), 0.8, 1e-4);
}

BOOST_AUTO_TEST_CASE(P_OneHotEncoder)
{
	// TODO
}

BOOST_AUTO_TEST_CASE(SFcheckNan)
{
	Eigen::Tensor<float, 1> values(2);
	values.setConstant(5.0f);
	Eigen::Tensor<float, 1> test(2);

	// control
  test = values.unaryExpr([](float c) { return checkNan<float>(c); });
	BOOST_CHECK_CLOSE(test(0), 5.0, 1e-3);
	BOOST_CHECK_CLOSE(test(1), 5.0, 1e-3);

	// test
	values(0) = NAN; //NaN
	values(1) = INFINITY; //infinity
  test = values.unaryExpr([](float c) { return checkNan<float>(c); });
	BOOST_CHECK_CLOSE(test(0), NAN, 1e-3);
	BOOST_CHECK_CLOSE(test(1), INFINITY, 1e-3);
}

BOOST_AUTO_TEST_CASE(SFsubstituteNanInf)
{
	Eigen::Tensor<float, 1> values(3);
	values.setConstant(5.0f);
	Eigen::Tensor<float, 1> test(3);

	// control
  test = values.unaryExpr([](float c) { return substituteNanInf<float>(c); });
	BOOST_CHECK_CLOSE(test(0), 5.0, 1e-3);
	BOOST_CHECK_CLOSE(test(1), 5.0, 1e-3);

	// test
	values(0) = NAN; //NaN
	values(1) = INFINITY; //infinity
	values(2) = -INFINITY; //infinity
  test = values.unaryExpr([](float c) { return substituteNanInf<float>(c); });
	BOOST_CHECK_CLOSE(test(0), 0.0, 1e-3);
	BOOST_CHECK_CLOSE(test(1), 1e9, 1e-3);
	BOOST_CHECK_CLOSE(test(2), -1e9, 1e-3);
}

BOOST_AUTO_TEST_CASE(SFClipOp)
{
	Eigen::Tensor<float, 1> net_input(3);
	net_input.setValues({ 0.0f, 1.0f, 0.5f });

	// test input
	Eigen::Tensor<float, 1> result = net_input.unaryExpr(ClipOp<float>(0.1f, 0.0f, 1.0f));
	BOOST_CHECK_CLOSE(result(0), 0.1, 1e-3);
	BOOST_CHECK_CLOSE(result(1), 0.9, 1e-3);
	BOOST_CHECK_CLOSE(result(2), 0.5, 1e-3);
}

BOOST_AUTO_TEST_CASE(SFRandBinaryOp)
{
	Eigen::Tensor<float, 1> net_input(2);
	net_input.setValues({ 2.0f, 2.0f });
	Eigen::Tensor<float, 1> result;

	// test input
	result = net_input.unaryExpr(RandBinaryOp<float>(0.0f));
	BOOST_CHECK_CLOSE(result(0), 2.0, 1e-3);
	BOOST_CHECK_CLOSE(result(1), 2.0, 1e-3);
	result = net_input.unaryExpr(RandBinaryOp<float>(1.0f));
	BOOST_CHECK_CLOSE(result(0), 0.0, 1e-3);
	BOOST_CHECK_CLOSE(result(1), 0.0, 1e-3);
}

BOOST_AUTO_TEST_CASE(assertClose)
{
	BOOST_CHECK(!assert_close<float>(1.1, 1.2, 1e-4, 1e-4));
	BOOST_CHECK(assert_close<float>(1.1, 1.2, 1, 1));
}

BOOST_AUTO_TEST_CASE(P_GaussianMixture)
{
	// TODO
}

BOOST_AUTO_TEST_CASE(P_SwissRoll)
{
	// TODO
}

BOOST_AUTO_TEST_CASE(P_GumbelSampler)
{
	Eigen::Tensor<float, 2> gumbel_samples = GumbelSampler<float>(2, 3);
	BOOST_CHECK_LE(gumbel_samples(0, 0), 10);
	BOOST_CHECK_GE(gumbel_samples(0, 0), -10);
	BOOST_CHECK_LE(gumbel_samples(1, 2), 10);
	BOOST_CHECK_GE(gumbel_samples(1, 2), -10);
	std::cout << gumbel_samples << std::endl;
}

BOOST_AUTO_TEST_CASE(P_GaussianSampler)
{
	Eigen::Tensor<float, 2> gaussian_samples = GaussianSampler<float>(2, 3);
	BOOST_CHECK_LE(gaussian_samples(0, 0), 2);
	BOOST_CHECK_GE(gaussian_samples(0, 0), -2);
	BOOST_CHECK_LE(gaussian_samples(1, 2), 2);
	BOOST_CHECK_GE(gaussian_samples(1, 2), -2);
	std::cout << gaussian_samples << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


text:
#include <blitz/array.h>
#include <blitz/tinyvec-et.h>

BZ_USING_NAMESPACE(blitz)

/*
 * The current implementation of stencil objects forces these variables
 * to be placed in global scope.  Ugh.  This restriction will be removed
 * eventually.
 */
double rho;                        // Density of fluid
double recip_rho;                  // 1/rho
double eta;                        // Kinematic viscosity
double time_now;                   // Elapsed seconds
double delta_t;                    // Time step
double volume;                     // Volume of a cell
double airPressure;                // Air pressure (Pa)
double spatialStep;                // Grid element size
double gravity;                    // Acceleration due to gravity
double gravityPressureGradient;    // Pressure gradient due to gravity
/*
 * The "geometry" object specifies how an array is mapped into real-world
 * space.  In this case, "UniformCubicGeometry" is used, which means that
 * the real-world grid is orthogonal, regularly spaced, with the same spatial
 * step in each dimension.
 */

UniformCubicGeometry<3> geom;      // Geometry
/*
 * Some typedefs to make life easier.
 */

typedef TinyVector<double,3> vector3d;
typedef Array<vector3d,3> vectorField;
typedef Array<double,3>   scalarField;

/***********          Timestep the velocity field           ************
 * This is a 63-point stencil.  For example, Laplacian3DVec4 turns into
 * a 45-point stencil: each 2nd derivative is a 5-point stencil, and
 * there are 9 of these derivatives to take the Laplacian of a 3D vector
 * field.
 */

BZ_DECLARE_STENCIL5(timestep, V, nextV, P, advect, force)

    nextV = *V + delta_t * ( recip_rho * (
      eta * Laplacian3DVec4(V,geom) - grad3D4(P, geom) + *force) - *advect);

BZ_END_STENCIL
/*
 * Allocate arrays and set their initial state
 */
void setup(const int N, vectorField& V, vectorField& nextV, scalarField& P,
    scalarField& P_rhs, vectorField& advect, vectorField& force)
{
    // A 1m x 1m x 1m domain
    spatialStep = 1.0 / (N - 1);
    geom = UniformCubicGeometry<3>(spatialStep);

    // Allocate arrays
    allocateArrays(shape(N,N,N), advect, V, nextV, force);  // vector fields
    allocateArrays(shape(N,N,N), P, P_rhs);                 // scalar fields

    // Since incompressibility is assumed, pressure only shows up as
    // derivative terms in the equations.  We choose airPressure = 0
    // as an arbitrary datum.

    airPressure = 0;             // Pa
    rho = 1000;                  // density of fluid, kg/m^3
    recip_rho = 1.0 / rho;       // inverse of density
    eta = 1.0e-6;                // kinematic viscosity of fluid, m^2/s
    gravity = 9.81;              // m/s^2
    delta_t = 0.001;             // initial time step, in seconds
    volume = pow3(spatialStep);  // cubic volume associated with grid point

    // Kludge: Set eta high, so that the flow will spread faster.
    // This means the cube is filled with molasses, rather than water.
    eta *= 1000;

    // Initial conditions: quiescent
    V = 0.0;
    P_rhs = 0.0;
    advect = 0.0;
    nextV = 0.0;
    P = 0.0;
    force = 0.0;
}

// Calculate a simple check on a vector field
void record(vectorField& V)
{
    // Calculate the magnitude of a field
    const int x=0, y=1, z=2;
    double magx = sum(pow2(V[x])) / V.numElements();
    double magy = sum(pow2(V[y])) / V.numElements();
    double magz = sum(pow2(V[z])) / V.numElements();

    cout << "norm = [" << magx
        << " " << magy << " " << magz << " ]" << endl;
}

void iterate(vectorField& V, vectorField& nextV, scalarField& P,
    scalarField& P_rhs, vectorField& advect, vectorField& force)
{
    // Time step
    applyStencil(timestep(), V, nextV, P, advect, force);
}

int main()
{
    vectorField V, nextV;        // Velocity fields
    scalarField P, P_rhs;        // Pressure fields
    vectorField advect;          // Advection field
    vectorField force;           // Forcing function

    const int N = 50;            // Arrays are NxNxN

    setup(N, V, nextV, P, P_rhs, advect, force);

    const int nIters = 10;

    for (int i=0; i < nIters; ++i)
    {
        iterate(V, nextV, P, P_rhs, advect, force);
    }

    return 0;
}

ext: cpp

max_stars_repo_name: fraguela/depspawn

max_issues_repo_name: MSV-Project/IBAMR

max_forks_repo_name: MSV-Project/IBAMR


text:
#ifndef BOOST_CONNECTOR_TYPES_TIMESTAMP_HPP
#define BOOST_CONNECTOR_TYPES_TIMESTAMP_HPP

#include <boost/functional/hash.hpp>

#include <chrono>

namespace boost::connector
{
struct timestamp_type
{
    using native_type = std::chrono::system_clock::duration;

    explicit timestamp_type(std::chrono::system_clock::time_point tp =
                                std::chrono::system_clock::now())
    : impl_(tp.time_since_epoch())
    {
    }

    std::uint64_t
    nanos() const
    {
        return std::chrono::duration_cast< std::chrono::nanoseconds >(impl_)
            .count();
    }

    std::uint64_t
    millis() const
    {
        return std::chrono::duration_cast< std::chrono::milliseconds >(impl_)
            .count();
    }

    static timestamp_type
    now()
    {
        return timestamp_type(std::chrono::system_clock::now());
    }

    inline friend std::size_t
    hash_value(timestamp_type ts);

    inline friend bool
    operator==(timestamp_type l, timestamp_type r);

    inline friend bool
    operator<(timestamp_type l, timestamp_type r);

  private:
    native_type impl_;
};

inline std::size_t
hash_value(timestamp_type ts)
{
    using rep = timestamp_type::native_type::rep;
    return hash< rep >()(ts.impl_.count());
}

bool
operator==(timestamp_type l, timestamp_type r)
{
    return l.impl_ == r.impl_;
}

bool
operator<(timestamp_type l, timestamp_type r)
{
    return l.impl_ < r.impl_;
}

inline std::string
to_string(timestamp_type ts)
{
    return std::to_string(ts.nanos());
}

inline std::ostream &
operator<<(std::ostream &os, timestamp_type ts)
{
    return os << ts.nanos();
}

}   // namespace boost::connector

#endif

ext: hpp


text:
#pragma once

#ifndef APP_SRC_FORMATTERS_HPP_
#define APP_SRC_FORMATTERS_HPP_

#include "fmt/format.h"

#include <boost/asio/serial_port.hpp>

#include <string>

// https://fmt.dev/latest/api.html#formatting-user-defined-types

/*
 * Custom boost::asio::serial_port::baud_rate formatter for fmt::format
 */
template <>
struct fmt::formatter<boost::asio::serial_port::baud_rate> : public fmt::formatter<std::string>
{
    template <typename FormatContext>
    auto format(boost::asio::serial_port::baud_rate const & v, FormatContext & ctx)
    {
        return fmt::formatter<std::string>::format(fmt::format("baud_rate::{}", v.value()), ctx);
    }
};

/*
 * Custom boost::asio::serial_port::flow_control formatter for fmt::format
 */
template <>
struct fmt::formatter<boost::asio::serial_port::flow_control> : public fmt::formatter<std::string>
{
    template <typename FormatContext>
    auto format(boost::asio::serial_port::flow_control const & v, FormatContext & ctx)
    {
        std::string name = "unknown";

        switch (v.value())
        {
            case boost::asio::serial_port::flow_control::none:      name = "none";  break;
            case boost::asio::serial_port::flow_control::software:  name = "sw";    break;
            case boost::asio::serial_port::flow_control::hardware:  name = "hw";    break;
        }

        return fmt::formatter<std::string>::format(fmt::format("flow_control::{}", name), ctx);
    }
};

/*
 * Custom boost::asio::serial_port::parity formatter for fmt::format
 */
template <>
struct fmt::formatter<boost::asio::serial_port::parity> : public fmt::formatter<std::string>
{
    template <typename FormatContext>
    auto format(boost::asio::serial_port::parity const & v, FormatContext & ctx)
    {
        std::string name = "unknown";

        switch (v.value())
        {
            case boost::asio::serial_port::parity::none:    name = "none";  break;
            case boost::asio::serial_port::parity::odd:     name = "odd";   break;
            case boost::asio::serial_port::parity::even:    name = "even";  break;
        }

        return fmt::formatter<std::string>::format(fmt::format("parity::{}", name), ctx);
    }
};

/*
 * Custom boost::asio::serial_port::stop_bits formatter for fmt::format
 */
template <>
struct fmt::formatter<boost::asio::serial_port::stop_bits> : public fmt::formatter<std::string>
{
    template <typename FormatContext>
    auto format(boost::asio::serial_port::stop_bits const & v, FormatContext & ctx)
    {
        std::string name = "unknown";

        switch (v.value())
        {
            case boost::asio::serial_port::stop_bits::one:          name = "one";           break;
            case boost::asio::serial_port::stop_bits::onepointfive: name = "onepointfive";  break;
            case boost::asio::serial_port::stop_bits::two:          name = "two";           break;
        }

        return fmt::formatter<std::string>::format(fmt::format("stop_bits::{}", name), ctx);
    }
};

/*
 * Custom boost::asio::serial_port::character_size formatter for fmt::format
 */
template <>
struct fmt::formatter<boost::asio::serial_port::character_size> : public fmt::formatter<std::string>
{
    template <typename FormatContext>
    auto format(boost::asio::serial_port::character_size const & v, FormatContext & ctx)
    {
        return fmt::formatter<std::string>::format(fmt::format("character_size::{}", v.value()), ctx);
    }
};

#endif /* APP_SRC_FORMATTERS_HPP_ */

max_stars_repo_name: WojciechMigda/TCO-Ringbeller

max_issues_repo_name: WojciechMigda/TCO-Ringbeller

max_forks_repo_name: WojciechMigda/TCO-Ringbeller


text:
// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/core/to.hpp>
#include <boost/hana/string.hpp>
namespace hana = boost::hana;

constexpr auto str = hana::string_c<'h', 'i'>;
constexpr char const* s = hana::to<char const*>(str);
static_assert(s[0] == 'h' && s[1] == 'i' && s[2] == '\0', "");

int main() { }

max_stars_repo_name: nxplatform/nx-mobile

max_issues_repo_name: nxplatform/nx-mobile

max_forks_repo_name: nxplatform/nx-mobile


text:
//
// Created by X-ray on 6/3/2021.
//
#pragma once
#include <utility>
#include <iostream>
#include <boost/beast.hpp>
#include "websocket.hpp"
#include "handle_request/handle_request.hpp"
#include "../utility/fail.hpp"

namespace web_server {
    namespace session {
        namespace beast = boost::beast;
        namespace http = beast::http;

        template<class Derived>
        class http_session
        {
            // Access the derived class, this is part of
            // the Curiously Recurring Template Pattern idiom.
            Derived& derived()
            {
                return static_cast<Derived&>(*this);
            }

            // This queue is used for HTTP pipelining.
            class queue
            {
                enum
                {
                    // Maximum number of responses we will queue
                    limit = 8
                };

                // The type-erased, saved work item
                struct work
                {
                    virtual ~work() = default;
                    virtual void operator()() = 0;
                };

                http_session& self_;
                std::vector<std::unique_ptr<work>> items_;

            public:
                explicit queue(http_session& self) : self_(self)
                {
                    static_assert(limit > 0, "queue limit must be positive");
                    items_.reserve(limit);
                }

                // Returns `true` if we have reached the queue limit
                [[nodiscard]] bool is_full() const {
                    return items_.size() >= limit;
                }

                // Called when a message finishes sending
                // Returns `true` if the caller should initiate a read
                bool on_write() {
                    BOOST_ASSERT(! items_.empty());
                    auto const was_full = is_full();
                    items_.erase(items_.begin());
                    if(! items_.empty())
                        (*items_.front())();
                    return was_full;
                }

                // Called by the HTTP handler to send a response.
                template<bool isRequest, class Body, class Fields>
                void operator()(http::message<isRequest, Body, Fields>&& msg) {
                    // This holds a work item
                    struct work_impl : work
                    {
                        http_session& self_;
                        http::message<isRequest, Body, Fields> msg_;

                        work_impl(http_session& self, http::message<isRequest, Body, Fields>&& msg) : self_(self), msg_(std::move(msg))
                        {
                        }

                        void operator()()
                        {
                            http::async_write(self_.derived().stream(), msg_, beast::bind_front_handler(&http_session::on_write, self_.derived().shared_from_this(), msg_.need_eof()));
                        }
                    };

                    // Allocate and store the work
                    items_.push_back(boost::make_unique<work_impl>(self_, std::move(msg)));

                    // If there was no previous work, start this one
                    if(items_.size() == 1)
                        (*items_.front())();
                }
            };

            std::shared_ptr<std::string const> doc_root_;
            queue queue_;

            // The parser is stored in an optional container so we can
            // construct it from scratch it at the beginning of each new message.
            boost::optional<http::request_parser<http::string_body>> parser_;

        protected:
            beast::flat_buffer buffer_;

        public:
            // Construct the session
            http_session(beast::flat_buffer buffer, std::shared_ptr<std::string const>  doc_root) : doc_root_(std::move(doc_root)), queue_(*this), buffer_(std::move(buffer))
            {
            }

            void do_read()
            {
                // Construct a new parser for each message
                parser_.emplace();

                // Apply a reasonable limit to the allowed size
                // of the body in bytes to prevent abuse.
                parser_->body_limit(10000);

                // Set the timeout.
                beast::get_lowest_layer(derived().stream()).expires_after(std::chrono::seconds(30));

                // Read a request using the parser-oriented interface
                http::async_read(derived().stream(), buffer_, *parser_, beast::bind_front_handler(&http_session::on_read, derived().shared_from_this()));
            }

            void on_read(beast::error_code ec, std::size_t bytes_transferred) {
                boost::ignore_unused(bytes_transferred);

                // This means they closed the connection
                if(ec == http::error::end_of_stream)
                    return derived().do_eof();

                if(ec)
                    return utility::fail(ec, "read");

                // See if it is a WebSocket Upgrade
                if(beast::websocket::is_upgrade(parser_->get()))
                {
                    // websocket connections are disabled
                    std::cout << "Websocket connection refused\n";
                    return;

                    //beast::get_lowest_layer(derived().stream()).expires_never();
                    //return make_websocket_session(derived().release_stream(), parser_->release());
                }

                // Send the response
                handle_request::handle request_handler;
                request_handler.run(*doc_root_, parser_->release(), queue_);
                //handle_request::handle_request(*doc_root_, parser_->release(), queue_);

                // If we aren't at the queue limit, try to pipeline another request
                if(!queue_.is_full())
                    do_read();
            }

            void on_write(bool close, beast::error_code ec, std::size_t bytes_transferred)
            {
                boost::ignore_unused(bytes_transferred);

                if(ec)
                    return utility::fail(ec, "write");

                if(close)
                {
                    // This means we should close the connection, usually because
                    // the response indicated the "Connection: close" semantic.
                    return derived().do_eof();
                }

                // Inform the queue that a write completed
                if(queue_.on_write())
                {
                    // Read another request
                    do_read();
                }
            }
        };
    } // session
} // web_server


text:
#include "MeshCuboidRelation.h"

#include "Utilities.h"

#include <cstdint>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

// Note:
// Assume that z = 0 plane is the ground plane, and +z-axis is the normal direction.
const MyMesh::Normal MeshCuboidAttributes::k_up_direction = MyMesh::Normal(0.0, 0.0, 1.0);

MeshCuboidAttributes::MeshCuboidAttributes()
	: object_name_("")
{
	initialize();
}

MeshCuboidAttributes::MeshCuboidAttributes(const std::string _object_name)
	: object_name_(_object_name)
{
	initialize();
}

MeshCuboidAttributes::MeshCuboidAttributes(const MeshCuboidAttributes& _other)
	: object_name_(_other.object_name_)
	, attributes_(_other.attributes_)
{
}

MeshCuboidAttributes::~MeshCuboidAttributes()
{
}

void MeshCuboidAttributes::initialize()
{
	// Note:
	// Initialized with NaN.
	if (!std::numeric_limits<Real>::has_quiet_NaN)
	{
		std::cerr << "Error: " << typeid(Real).name() << " does not support quiet NaN." << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	const int num_attributes = static_cast<int>(k_num_attributes);

	attributes_ = Eigen::VectorXd::Zero(num_attributes);
	for (int attribute_index = 0; attribute_index < num_attributes; ++attribute_index)
	{
		attributes_[attribute_index] = std::numeric_limits<Real>::quiet_NaN();
	}
}

void MeshCuboidAttributes::compute_attributes(const MeshCuboid *_cuboid)
{
	assert(attributes_.size() == static_cast<int>(k_num_attributes));

	for (unsigned int i = 0; i < 3; ++i)
	{
		//attributes_[k_center_index + i] = _cuboid->get_bbox_center()[i];

		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			attributes_[k_corner_index + 3 * corner_index + i] = _cuboid->get_bbox_corner(corner_index)[i];
	}
}

void MeshCuboidAttributes::get_attribute_collection_matrix(const std::list<MeshCuboidAttributes *>& _stats,
	Eigen::MatrixXd& _values)
{
	unsigned int num_objects = _stats.size();

	const int num_attributes = static_cast<int>(k_num_attributes);
	_values = Eigen::MatrixXd(num_objects, num_attributes);

	unsigned int object_index = 0;
	for (std::list<MeshCuboidAttributes *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (int attribute_index = 0; attribute_index < num_attributes; ++attribute_index)
			_values(object_index, attribute_index) = (*stat_it)->attributes_[attribute_index];
	}
}

bool MeshCuboidAttributes::save_attribute_collection(const std::list<MeshCuboidAttributes *>& _stats,
	const char* _filename)
{
	const int num_attributes = static_cast<int>(k_num_attributes);

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	//unsigned int num_objects = _stats.size();

	// Write attribute values.
	unsigned int object_index = 0;
	for (std::list<MeshCuboidAttributes *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (int attribute_index = 0; attribute_index < num_attributes; ++attribute_index)
		{
			Real value = (*stat_it)->attributes_[attribute_index];
			if (std::isnan(value))
			{
				// Note:
				// Undefined attributes are recorded as NaN.
				file << "NaN,";
			}
			else
			{
				file << value << ",";
			}
		}
		file << std::endl;
	}

	file.close();
	return true;
}

MeshCuboidFeatures::MeshCuboidFeatures()
	: object_name_("")
{
	initialize();
}

MeshCuboidFeatures::MeshCuboidFeatures(const std::string _object_name)
	: object_name_(_object_name)
{
	initialize();
}

MeshCuboidFeatures::MeshCuboidFeatures(const MeshCuboidFeatures& _other)
	: object_name_(_other.object_name_)
	, features_(_other.features_)
{
}

MeshCuboidFeatures::~MeshCuboidFeatures()
{
}

void MeshCuboidFeatures::initialize()
{
	// Note:
	// Initialized with NaN.
	if (!std::numeric_limits<Real>::has_quiet_NaN)
	{
		std::cerr << "Error: " << typeid(Real).name() << " does not support quiet NaN." << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	features_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	for (int attribute_index = 0; attribute_index < MeshCuboidFeatures::k_num_features; ++attribute_index)
	{
		features_[attribute_index] = std::numeric_limits<Real>::quiet_NaN();
	}
}

void MeshCuboidFeatures::compute_features(const MeshCuboid *_cuboid,
	Eigen::MatrixXd *_attributes_to_features_map)
{
	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;
	assert(features_.size() == static_cast<int>(MeshCuboidFeatures::k_num_features));

	// Linear map.
	Eigen::MatrixXd attributes_to_features_map =
		Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, num_attributes);

	Eigen::RowVector3d up_direction_vec;
	for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		up_direction_vec(axis_index) = MeshCuboidAttributes::k_up_direction[axis_index];

	unsigned int next_feature_index = 0;

	// Center point.
	for (unsigned int i = 0; i < 3; ++i)
	{
		features_[next_feature_index] = _cuboid->get_bbox_center()[i];

		//attributes_to_features_map.row(next_feature_index)(
		//	MeshCuboidAttributes::k_center_index + i) = 1.0;
		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
		{
			attributes_to_features_map.row(next_feature_index)(
				MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
				1.0 / MeshCuboid::k_num_corners;
		}

		++next_feature_index;
	}

	// Corner points.
	for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
	{
		MyMesh::Point corner = _cuboid->get_bbox_corner(corner_index);

		for (unsigned int i = 0; i < 3; ++i)
		{
			features_[next_feature_index] = corner[i];
			attributes_to_features_map.row(next_feature_index)(
				MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) = 1.0;
			++next_feature_index;
		}
	}

	// Center height.
	features_[next_feature_index] = dot(MeshCuboidAttributes::k_up_direction, _cuboid->get_bbox_center());
	for (unsigned int i = 0; i < 3; ++i)
	{
		//attributes_to_features_map.row(next_feature_index)(
		//	MeshCuboidAttributes::k_center_index + i) = MeshCuboidAttributes::k_up_direction[i];
		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
		{
			attributes_to_features_map.row(next_feature_index)(
				MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
				(1.0 / MeshCuboid::k_num_corners) * MeshCuboidAttributes::k_up_direction[i];
		}
	}
	++next_feature_index;

	// Corner height.
	for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
	{
		MyMesh::Point corner = _cuboid->get_bbox_corner(corner_index);
		features_[next_feature_index] = dot(MeshCuboidAttributes::k_up_direction, corner);

		for (unsigned int i = 0; i < 3; ++i)
		{
			attributes_to_features_map.row(next_feature_index)(
				MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
				MeshCuboidAttributes::k_up_direction[i];
		}
		++next_feature_index;
	}

	assert(next_feature_index == static_cast<int>(MeshCuboidFeatures::k_num_features));

#ifdef DEBUG_TEST
	MeshCuboidAttributes attributes;
	attributes.compute_attributes(_cuboid);
	Eigen::VectorXd same_features = attributes_to_features_map * attributes.get_attributes();

	assert(same_features.rows() == MeshCuboidFeatures::k_num_features);
	Real error = (same_features - features_).array().abs().sum();

	CHECK_NUMERICAL_ERROR(__FUNCTION__, error);
#endif

	// Optional.
	if (_attributes_to_features_map)
		(*_attributes_to_features_map) = attributes_to_features_map;
}

void MeshCuboidFeatures::get_feature_collection_matrix(const std::list<MeshCuboidFeatures *>& _stats,
	Eigen::MatrixXd& _values)
{
	unsigned int num_objects = _stats.size();

	const int num_features = static_cast<int>(k_num_features);
	_values = Eigen::MatrixXd(num_objects, num_features);

	unsigned int object_index = 0;
	for (std::list<MeshCuboidFeatures *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (int feature_index = 0; feature_index < num_features; ++feature_index)
			_values(object_index, feature_index) = (*stat_it)->features_[feature_index];
	}
}

bool MeshCuboidFeatures::load_feature_collection(const char* _filename,
	std::list<MeshCuboidFeatures *>& _stats)
{
	for (std::list<MeshCuboidFeatures *>::iterator it = _stats.begin(); it != _stats.end(); ++it)
		delete (*it);
	_stats.clear();

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't load file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;
	bool succeded = true;

	while (!file.eof())
	{
		std::getline(file, buffer);
		if (buffer == "") break;

		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		MeshCuboidFeatures *new_features = new MeshCuboidFeatures();
		assert(new_features);

		for (int feature_index = 0; feature_index < MeshCuboidFeatures::k_num_features; ++feature_index)
		{
			std::getline(strstr, token, ',');
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				succeded = false;
				break;
			}

			if (token == "NaN")
			{
				// Note:
				// Undefined attributes are recorded as NaN.
				new_features->features_[feature_index] = std::numeric_limits<Real>::quiet_NaN();
			}
			else
			{
				new_features->features_[feature_index] = std::stof(token);
			}
		}

		if (!succeded) break;
		_stats.push_back(new_features);
	}

	if (!succeded)
	{
		for (std::list<MeshCuboidFeatures *>::iterator it = _stats.begin(); it != _stats.end(); ++it)
			delete (*it);
		_stats.clear();
		return false;
	}

	return true;
}

bool MeshCuboidFeatures::save_feature_collection(const char* _filename,
	const std::list<MeshCuboidFeatures *>& _stats)
{
	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	//unsigned int num_objects = _stats.size();

	// Write attribute values.
	unsigned int object_index = 0;
	for (std::list<MeshCuboidFeatures *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (int feature_index = 0; feature_index < MeshCuboidFeatures::k_num_features; ++feature_index)
		{
			Real value = (*stat_it)->features_[feature_index];
			if (std::isnan(value))
			{
				// Note:
				// Undefined attributes are recorded as NaN.
				file << "NaN,";
			}
			else
			{
				file << value << ",";
			}
		}
		file << std::endl;
	}

	file.close();
	return true;
}

MeshCuboidTransformation::MeshCuboidTransformation()
: object_name_("")
{
	initialize();
}

MeshCuboidTransformation::MeshCuboidTransformation(const std::string _object_name)
: object_name_(_object_name)
{
	initialize();
}

MeshCuboidTransformation::~MeshCuboidTransformation()
{
}

void MeshCuboidTransformation::initialize()
{
	first_translation_ = Eigen::Vector3d::Zero();
	second_rotation_ = Eigen::Matrix3d::Identity();
}

void MeshCuboidTransformation::compute_transformation(const MeshCuboid *_cuboid)
{
	assert(_cuboid);

	for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
	{
		for (unsigned int i = 0; i < 3; ++i)
		{
			second_rotation_.row(axis_index)(i) = _cuboid->get_bbox_axis(axis_index)[i];
		}
		first_translation_(axis_index) = -_cuboid->get_bbox_center()[axis_index];
	};
}

Eigen::VectorXd
MeshCuboidTransformation::get_transformed_features(
const MeshCuboidFeatures& _other_features)const
{
	Eigen::VectorXd transformed_features = _other_features.get_features();

	for (unsigned int i = 0; i < MeshCuboidFeatures::k_num_local_points; ++i)
	{
		Eigen::VectorXd sub_values = transformed_features.block(3 * i, 0, 3, 1);
		sub_values = first_translation_ + sub_values;
		sub_values = second_rotation_ * sub_values;
		transformed_features.block(3 * i, 0, 3, 1) = sub_values;
	}

//#ifdef DEBUG_TEST
//	Eigen::MatrixXd rotation;
//	Eigen::MatrixXd translation;
//	get_linear_map_transformation(rotation, translation);
//	Eigen::VectorXd same_transformed_features = rotation * (translation * _other_features.get_features());
//
//	assert(same_transformed_features.rows() == MeshCuboidFeatures::k_num_features);
//	Real error = (same_transformed_features - transformed_features).array().abs().sum();
//
//	CHECK_NUMERICAL_ERROR(__FUNCTION__, error);
//#endif

	return transformed_features;
}

Eigen::VectorXd
MeshCuboidTransformation::get_transformed_features(const MeshCuboid *_other_cuboid)const
{
	assert(_other_cuboid);

	MeshCuboidFeatures other_features;
	other_features.compute_features(_other_cuboid);

	return get_transformed_features(other_features);
}

Eigen::VectorXd
MeshCuboidTransformation::get_inverse_transformed_features(
	const MeshCuboidFeatures& _other_features)const
{
	Eigen::VectorXd inverse_transformed_features = _other_features.get_features();

	for (unsigned int i = 0; i < MeshCuboidFeatures::k_num_local_points; ++i)
	{
		Eigen::VectorXd sub_values = inverse_transformed_features.block(3 * i, 0, 3, 1);
		sub_values = second_rotation_.transpose() * sub_values;
		sub_values = -first_translation_ + sub_values;
		inverse_transformed_features.block(3 * i, 0, 3, 1) = sub_values;
	}

//#ifdef DEBUG_TEST
//	Eigen::MatrixXd rotation;
//	Eigen::MatrixXd translation;
//	get_linear_map_inverse_transformation(rotation, translation);
//	Eigen::VectorXd same_transformed_features = rotation * (translation * _other_features.get_features());
//
//	assert(same_transformed_features.rows() == MeshCuboidFeatures::k_num_features);
//	Real error = (same_transformed_features - inverse_transformed_features).array().abs().sum();
//
//	CHECK_NUMERICAL_ERROR(__FUNCTION__, error);
//#endif

	return inverse_transformed_features;
}

Eigen::VectorXd
MeshCuboidTransformation::get_inverse_transformed_features(const MeshCuboid *_other_cuboid)const
{
	assert(_other_cuboid);

	MeshCuboidFeatures other_features;
	other_features.compute_features(_other_cuboid);

	return get_inverse_transformed_features(other_features);
}

void MeshCuboidTransformation::get_transformation(
	Eigen::Matrix3d &_rotation, Eigen::Vector3d &_translation) const
{
	_rotation = second_rotation_;
	_translation = first_translation_;
	_translation = _rotation * _translation;
}

void MeshCuboidTransformation::get_inverse_transformation(
	Eigen::Matrix3d &_rotation, Eigen::Vector3d &_translation) const
{
	_rotation = second_rotation_.transpose();
	_translation = -first_translation_;
}

void MeshCuboidTransformation::get_linear_map_transformation(
	Eigen::MatrixXd &_rotation, Eigen::MatrixXd &_translation) const
{
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	_rotation = Eigen::MatrixXd::Identity(num_features, num_features);
	_translation = Eigen::MatrixXd::Zero(num_features, MeshCuboidAttributes::k_num_attributes);

	for (unsigned int k = 0; k < MeshCuboidFeatures::k_num_local_points; ++k)
	{
		_rotation.block<3, 3>(3 * k, 3 * k) = second_rotation_;

		for (unsigned int i = 0; i < 3; ++i)
		{
			//_translation(3 * k + i, MeshCuboidAttributes::k_center_index + i) = -1;
			for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			{
				_translation(3 * k + i, MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
					-1.0 / MeshCuboid::k_num_corners;
			}
		}
	}

	_translation = _rotation * _translation;
}

void MeshCuboidTransformation::get_linear_map_inverse_transformation(
	Eigen::MatrixXd &_rotation, Eigen::MatrixXd &_translation) const
{
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	_rotation = Eigen::MatrixXd::Identity(num_features, num_features);
	_translation = Eigen::MatrixXd::Zero(num_features, MeshCuboidAttributes::k_num_attributes);

	for (unsigned int k = 0; k < MeshCuboidFeatures::k_num_local_points; ++k)
	{
		_rotation.block<3, 3>(3 * k, 3 * k) = second_rotation_.transpose();

		for (unsigned int i = 0; i < 3; ++i)
		{
			//_translation(3 * k + i, MeshCuboidAttributes::k_center_index + i) = 1;
			for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			{
				_translation(3 * k + i, MeshCuboidAttributes::k_corner_index + 3 * corner_index + i) =
					1.0 / MeshCuboid::k_num_corners;
			}
		}
	}
}

bool MeshCuboidTransformation::load_transformation_collection(const char* _filename,
	std::list<MeshCuboidTransformation *>& _stats)
{
	for (std::list<MeshCuboidTransformation *>::iterator it = _stats.begin(); it != _stats.end(); ++it)
		delete (*it);
	_stats.clear();

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't load file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;
	bool succeded = true;

	while (!file.eof())
	{
		std::getline(file, buffer);
		if (buffer == "") break;

		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		MeshCuboidTransformation *new_transformation = new MeshCuboidTransformation();
		assert(new_transformation);

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		{
			for (unsigned int i = 0; i < 3; ++i)
			{
				std::getline(strstr, token, ',');
				if (strstr.eof())
				{
					std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
					succeded = false;
					break;
				}

				if (!succeded) break;
				new_transformation->second_rotation_.col(axis_index)(i) = std::stof(token);
			}
		}

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
		{
			std::getline(strstr, token, ',');
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				succeded = false;
				break;
			}

			new_transformation->first_translation_(axis_index) = std::stof(token);
		}

		if (!succeded) break;
		_stats.push_back(new_transformation);
	}

	if (!succeded)
	{
		for (std::list<MeshCuboidTransformation *>::iterator it = _stats.begin(); it != _stats.end(); ++it)
			delete (*it);
		_stats.clear();
		return false;
	}

	return true;
}

bool MeshCuboidTransformation::save_transformation_collection(const char* _filename,
	const std::list<MeshCuboidTransformation *>& _stats)
{
	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	//unsigned int num_objects = _stats.size();

	// Write attribute values.
	unsigned int object_index = 0;
	for (std::list<MeshCuboidTransformation *>::const_iterator stat_it = _stats.begin(); stat_it != _stats.end();
		++stat_it, ++object_index)
	{
		assert(*stat_it);

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
			for (unsigned int i = 0; i < 3; ++i)
				file << (*stat_it)->second_rotation_.col(axis_index)(i) << ",";

		for (unsigned int axis_index = 0; axis_index < 3; ++axis_index)
			file << (*stat_it)->first_translation_(axis_index) << ",";

		file << std::endl;
	}

	file.close();
	return true;
}

MeshCuboidJointNormalRelations::MeshCuboidJointNormalRelations()
{
	mean_ = Eigen::VectorXd::Zero(k_mat_size);
	inv_cov_ = Eigen::MatrixXd::Zero(k_mat_size, k_mat_size);
}

MeshCuboidJointNormalRelations::~MeshCuboidJointNormalRelations()
{

}

bool MeshCuboidJointNormalRelations::load_joint_normal_csv(const char* _filename)
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ", ", "", "", "", "", "");

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < k_mat_size; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');

		// Transposed.
		mean_(j) = std::stof(token);
	}

	for (int i = 0; i < k_mat_size; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < k_mat_size; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			inv_cov_(i, j) = std::stof(token);
		}
	}

	file.close();

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(inv_cov_);
	Real min_eigenvalue = es.eigenvalues().minCoeff();
	if (min_eigenvalue < -1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not positive-semidefinite: ("
			<< _filename << " = " << min_eigenvalue << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	if (symmetry_diff > 1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not symmetric: ("
			<< _filename << " = " << symmetry_diff << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	return true;
}

bool MeshCuboidJointNormalRelations::save_joint_normal_csv(const char* _filename)const
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	file << mean_.transpose().format(csv_format) << std::endl;
	file << inv_cov_.format(csv_format) << std::endl;

	file.close();
	return true;
}

bool MeshCuboidJointNormalRelations::load_joint_normal_dat(const char* _filename)
{
	std::ifstream file(_filename, std::ios::in | std::ios::binary);
	if (!file.good())
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	int16_t rows, cols;

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == k_mat_size);
	assert(static_cast<int>(cols) == 1);
	mean_.resize(rows);
	file.read((char *)mean_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == k_mat_size);
	assert(static_cast<int>(cols) == k_mat_size);
	inv_cov_.resize(rows, cols);
	file.read((char *)inv_cov_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	file.close();

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(inv_cov_);
	Real min_eigenvalue = es.eigenvalues().minCoeff();
	if (min_eigenvalue < -1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not positive-semidefinite: ("
			<< _filename << " = " << min_eigenvalue << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	if (symmetry_diff > 1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not symmetric: ("
			<< _filename << " = " << symmetry_diff << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	return true;
}

void MeshCuboidJointNormalRelations::get_pairwise_cuboid_features(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	Eigen::VectorXd &_pairwise_features_vec)
{
	assert(_cuboid_1);
	assert(_cuboid_2);

	MeshCuboidFeatures features_1;
	features_1.compute_features(_cuboid_1);

	MeshCuboidFeatures features_2;
	features_2.compute_features(_cuboid_2);

	get_pairwise_cuboid_features(features_1, features_2,
		_transformation_1, _transformation_2, _pairwise_features_vec);
}

void MeshCuboidJointNormalRelations::get_pairwise_cuboid_features(
	const MeshCuboidFeatures &_features_1, const MeshCuboidFeatures &_features_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	Eigen::VectorXd &_pairwise_features_vec)
{
	assert(_transformation_1);
	assert(_transformation_2);

	Eigen::VectorXd transformed_features_vec_11 = _transformation_1->get_transformed_features(_features_1);
	assert(std::abs(transformed_features_vec_11[0]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_11[1]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_11[2]) < NUMERIAL_ERROR_THRESHOLD);
	assert(transformed_features_vec_11.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd transformed_features_vec_12 = _transformation_1->get_transformed_features(_features_2);
	assert(transformed_features_vec_12.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd transformed_features_vec_22 = _transformation_2->get_transformed_features(_features_2);
	assert(std::abs(transformed_features_vec_22[0]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_22[1]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_22[2]) < NUMERIAL_ERROR_THRESHOLD);
	assert(transformed_features_vec_22.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd transformed_features_vec_21 = _transformation_2->get_transformed_features(_features_1);
	assert(transformed_features_vec_21.rows() == MeshCuboidFeatures::k_num_features);

	// NOTE:
	// Since the center point is always the origin in the local coordinates,
	// it is not used as the feature values.
	assert(k_mat_size == 2 * (2 * MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index));
	_pairwise_features_vec.resize(k_mat_size);
	_pairwise_features_vec <<
		transformed_features_vec_11.bottomRows(MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index),
		transformed_features_vec_12,
		transformed_features_vec_22.bottomRows(MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index),
		transformed_features_vec_21;
}

double MeshCuboidJointNormalRelations::compute_error(const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2) const
{
	Eigen::VectorXd pairwise_cuboid_feature;
	get_pairwise_cuboid_features(_cuboid_1, _cuboid_2, _transformation_1, _transformation_2,
		pairwise_cuboid_feature);

	assert(mean_.rows() == pairwise_cuboid_feature.rows());
	assert(inv_cov_.rows() == pairwise_cuboid_feature.rows());
	assert(inv_cov_.cols() == pairwise_cuboid_feature.rows());

	Eigen::VectorXd diff = pairwise_cuboid_feature - mean_;

	// Mahalanobis norm.
	double error = diff.transpose() * inv_cov_ * diff;
	assert(error >= 0);

	//std::cerr << "Negative error value (error = " << error << ")" << std::endl;
	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	//Real inv_cov_det = inv_cov_.determinant();
	//std::cout << "inv_cov_det = " << inv_cov_det << std::endl;

	//Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	//std::cerr << "symmetry_diff = " << symmetry_diff << std::endl;

	//std::cout << "Norm(diff) = " << diff.transpose() * diff << std::endl;

	//std::cout << "diff = " << std::endl;
	//std::cout << diff.format(csv_format) << std::endl;

	//do {
	//	std::cout << '\n' << "Press the Enter key to continue.";
	//} while (std::cin.get() != '\n');

	return error;
}

double MeshCuboidJointNormalRelations::compute_conditional_error(const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1) const
{
	Eigen::VectorXd conditional_pairwise_cuboid_feature;
	assert(_transformation_1);

	MeshCuboidFeatures features_1;
	features_1.compute_features(_cuboid_1);

	MeshCuboidFeatures features_2;
	features_2.compute_features(_cuboid_2);

	Eigen::VectorXd transformed_features_vec_11 = _transformation_1->get_transformed_features(features_1);
	assert(std::abs(transformed_features_vec_11[0]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_11[1]) < NUMERIAL_ERROR_THRESHOLD);
	assert(std::abs(transformed_features_vec_11[2]) < NUMERIAL_ERROR_THRESHOLD);
	assert(transformed_features_vec_11.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd transformed_features_vec_12 = _transformation_1->get_transformed_features(features_2);
	assert(transformed_features_vec_12.rows() == MeshCuboidFeatures::k_num_features);

	// NOTE:
	// Since the center point is always the origin in the local coordinates,
	// it is not used as the feature values.
	const int num_rows = 2 * MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index;
	conditional_pairwise_cuboid_feature.resize(num_rows);
	conditional_pairwise_cuboid_feature <<
		transformed_features_vec_11.bottomRows(MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index),
		transformed_features_vec_12;

	Eigen::VectorXd conditional_mean = mean_.segment(0, num_rows);
	Eigen::MatrixXd conditional_inv_cov = inv_cov_.block(0, 0, num_rows, num_rows);
	Eigen::VectorXd diff = conditional_pairwise_cuboid_feature - conditional_mean;

	// Mahalanobis norm.
	double error = diff.transpose() * conditional_inv_cov * diff;
	assert(error >= 0);

	return error;
}

MeshCuboidCondNormalRelations::MeshCuboidCondNormalRelations()
{
	assert(MeshCuboidFeatures::k_num_global_feature_values > 0);

	mean_A_ = Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, MeshCuboidFeatures::k_num_global_feature_values);
	mean_b_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	inv_cov_ = Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, MeshCuboidFeatures::k_num_features);
}

MeshCuboidCondNormalRelations::~MeshCuboidCondNormalRelations()
{

}

bool MeshCuboidCondNormalRelations::load_cond_normal_csv(const char* _filename)
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ", ", "", "", "", "", "");

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;

	for (int i = 0; i < MeshCuboidFeatures::k_num_global_feature_values; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');

			// Transposed.
			mean_A_(j, i) = std::stof(token);
		}
	}

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');

		// Transposed.
		mean_b_(j) = std::stof(token);
	}

	for (int i = 0; i < MeshCuboidFeatures::k_num_features; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			inv_cov_(i, j) = std::stof(token);
		}
	}

	file.close();

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(inv_cov_);
	Real min_eigenvalue = es.eigenvalues().minCoeff();
	if (min_eigenvalue < -1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not positive-semidefinite: ("
			<< _filename << " = " << min_eigenvalue << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	if (symmetry_diff > 1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not symmetric: ("
			<< _filename << " = " << symmetry_diff << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	return true;
}

bool MeshCuboidCondNormalRelations::save_cond_normal_csv(const char* _filename)const
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	// NOTE:
	// mean_A_. mean_b_: Transposed.
	file << mean_A_.transpose().format(csv_format) << std::endl;
	file << mean_b_.transpose().format(csv_format) << std::endl;
	file << inv_cov_.format(csv_format) << std::endl;

	file.close();
	return true;
}

bool MeshCuboidCondNormalRelations::load_cond_normal_dat(const char* _filename)
{
	std::ifstream file(_filename, std::ios::in | std::ios::binary);
	if (!file.good())
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	int16_t rows, cols;

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == MeshCuboidFeatures::k_num_features);
	assert(static_cast<int>(cols) == MeshCuboidFeatures::k_num_global_feature_values);
	mean_A_.resize(rows, cols);
	file.read((char *)mean_A_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == MeshCuboidFeatures::k_num_features);
	assert(static_cast<int>(cols) == 1);
	mean_b_.resize(rows);
	file.read((char *)mean_b_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	rows = 0, cols = 0;
	file.read((char*)(&rows), sizeof(int16_t));
	file.read((char*)(&cols), sizeof(int16_t));
	assert(static_cast<int>(rows) == MeshCuboidFeatures::k_num_features);
	assert(static_cast<int>(cols) == MeshCuboidFeatures::k_num_features);
	inv_cov_.resize(rows, cols);
	file.read((char *)inv_cov_.data(), rows*cols*sizeof(Eigen::MatrixXd::Scalar));

	file.close();

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(inv_cov_);
	Real min_eigenvalue = es.eigenvalues().minCoeff();
	if (min_eigenvalue < -1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not positive-semidefinite: ("
			<< _filename << " = " << min_eigenvalue << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	Real symmetry_diff = (inv_cov_ - inv_cov_.transpose()).array().abs().sum();
	if (symmetry_diff > 1.0E-6)
	{
		std::cerr << "Error: The inverse covariance matrix is not symmetric: ("
			<< _filename << " = " << symmetry_diff << ")" << std::endl;
		do {
			std::cout << '\n' << "Press the Enter key to continue.";
		} while (std::cin.get() != '\n');
	}

	return true;
}

void MeshCuboidCondNormalRelations::get_pairwise_cuboid_features(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	Eigen::VectorXd &_global_features_vec_1, Eigen::VectorXd &_transformed_features_vec_12)
{
	assert(_cuboid_1);
	assert(_cuboid_2);

	MeshCuboidFeatures features_1;
	features_1.compute_features(_cuboid_1);

	MeshCuboidFeatures features_2;
	features_2.compute_features(_cuboid_2);

	get_pairwise_cuboid_features(features_1, features_2,
		_transformation_1, _transformation_2,
		_global_features_vec_1, _transformed_features_vec_12);
}

void MeshCuboidCondNormalRelations::get_pairwise_cuboid_features(
	const MeshCuboidFeatures &_features_1, const MeshCuboidFeatures &_features_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	Eigen::VectorXd &_global_features_vec_1, Eigen::VectorXd &_transformed_features_vec_12)
{
	assert(_transformation_1);
	assert(_transformation_2);

	Eigen::VectorXd features_vec_1 = _features_1.get_features();
	assert(features_vec_1.rows() == MeshCuboidFeatures::k_num_features);

	_transformed_features_vec_12 = _transformation_1->get_transformed_features(_features_2);
	assert(_transformed_features_vec_12.rows() == MeshCuboidFeatures::k_num_features);

	_global_features_vec_1 = features_vec_1.bottomRows(MeshCuboidFeatures::k_num_global_feature_values);
}

double MeshCuboidCondNormalRelations::compute_error(const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2)const
{
	Eigen::VectorXd global_features_vec_1, transformed_features_vec_12;
	get_pairwise_cuboid_features(_cuboid_1, _cuboid_2, _transformation_1, _transformation_2,
		global_features_vec_1, transformed_features_vec_12);

	assert(mean_A_.rows() == transformed_features_vec_12.rows());
	assert(mean_A_.cols() == global_features_vec_1.rows());
	assert(mean_b_.rows() == transformed_features_vec_12.rows());
	assert(inv_cov_.rows() == transformed_features_vec_12.rows());
	assert(inv_cov_.cols() == transformed_features_vec_12.rows());

	const Eigen::VectorXd mean_2 = mean_A_ * global_features_vec_1 + mean_b_;
	Eigen::VectorXd diff = transformed_features_vec_12 - mean_2;

	// Mahalanobis norm.
	double error = diff.transpose() * inv_cov_ * diff;
	assert(error >= 0);

	//std::cerr << "Negative error value (error = " << error << ")" << std::endl;
	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	//Real inv_cov_det = inv_cov_.determinant();
	//std::cout << "inv_cov_det = " << inv_cov_det << std::endl;

	//std::cout << "Norm(diff) = " << diff.transpose() * diff << std::endl;

	//std::cout << "diff = " << std::endl;
	//std::cout << diff.format(csv_format) << std::endl;

	//do {
	//	std::cout << '\n' << "Press the Enter key to continue.";
	//} while (std::cin.get() != '\n');

	return error;
}

/*
MeshCuboidPCARelations::MeshCuboidPCARelations()
{
	mean_ = Eigen::VectorXd::Zero(k_mat_size);
	pca_bases_ = Eigen::MatrixXd::Zero(k_mat_size, k_mat_size);
}

MeshCuboidPCARelations::~MeshCuboidPCARelations()
{

}

bool MeshCuboidPCARelations::load_pca_csv(const char* _filename)
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ", ", "", "", "", "", "");

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < k_mat_size; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');
		mean_(j) = std::stof(token);
	}

	for (int i = 0; i < k_mat_size; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < k_mat_size; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			pca_bases_(i, j) = std::stof(token);
		}
	}

	file.close();

	return true;
}

bool MeshCuboidPCARelations::save_pca_csv(const char* _filename)const
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	file << mean_.transpose().format(csv_format) << std::endl;
	file << pca_bases_.format(csv_format) << std::endl;

	file.close();
	return true;
}

double MeshCuboidPCARelations::compute_error(
	const Eigen::VectorXd &_features_vec_1, const Eigen::VectorXd &_features_vec_2) const
{
	assert(_features_vec_1.rows() == MeshCuboidFeatures::k_num_features);
	assert(_features_vec_2.rows() == MeshCuboidFeatures::k_num_features);

	Eigen::VectorXd concat_features(2 * MeshCuboidFeatures::k_num_features);
	concat_features << _features_vec_1, _features_vec_2;
	assert(mean_.rows() == concat_features.rows());
	assert(pca_bases_.rows() == concat_features.rows());
	assert(pca_bases_.cols() == concat_features.rows());

	Eigen::VectorXd diff = concat_features - mean_;
	diff = diff - (pca_bases_ * diff);

	double error = diff.transpose() * diff;
	return error;
}

MeshCuboidCCARelations::MeshCuboidCCARelations()
{
	mean_1_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	mean_2_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	correlations_ = Eigen::VectorXd::Zero(MeshCuboidFeatures::k_num_features);
	bases_12_ = Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, MeshCuboidFeatures::k_num_features);
	bases_21_ = Eigen::MatrixXd::Zero(MeshCuboidFeatures::k_num_features, MeshCuboidFeatures::k_num_features);
}

MeshCuboidCCARelations::~MeshCuboidCCARelations()
{

}

bool MeshCuboidCCARelations::load_cca_bases(const char* _filename)
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't open file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	std::string buffer;
	std::stringstream strstr;
	std::string token;

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');
		mean_1_(j) = std::stof(token);
	}

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');
		mean_2_(j) = std::stof(token);
	}

	std::getline(file, buffer);
	strstr.str(std::string());
	strstr.clear();
	strstr.str(buffer);

	for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
	{
		if (strstr.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(strstr, token, ',');
		correlations_(j) = std::stof(token);
	}

	for (int i = 0; i < MeshCuboidFeatures::k_num_features; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			bases_12_(i, j) = std::stof(token);
		}
	}

	for (int i = 0; i < MeshCuboidFeatures::k_num_features; ++i)
	{
		if (file.eof())
		{
			std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
			return false;
		}

		std::getline(file, buffer);
		strstr.str(std::string());
		strstr.clear();
		strstr.str(buffer);

		for (int j = 0; j < MeshCuboidFeatures::k_num_features; ++j)
		{
			if (strstr.eof())
			{
				std::cerr << "Wrong file format: \"" << _filename << "\"" << std::endl;
				return false;
			}

			std::getline(strstr, token, ',');
			bases_21_(i, j) = std::stof(token);
		}
	}

	file.close();
	return true;
}

bool MeshCuboidCCARelations::save_cca_bases(const char* _filename)const
{
	Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");

	std::ofstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't save file: \"" << _filename << "\"" << std::endl;
		return false;
	}
	std::setprecision(std::numeric_limits<long double>::digits10 + 1);
	std::cout << std::scientific;

	file << mean_1_.transpose().format(csv_format) << std::endl;
	file << mean_2_.transpose().format(csv_format) << std::endl;
	file << correlations_.transpose().format(csv_format) << std::endl;
	file << bases_12_.format(csv_format) << std::endl;
	file << bases_21_.format(csv_format) << std::endl;

	file.close();
	return true;
}

double MeshCuboidCCARelations::compute_error(
	const Eigen::VectorXd &_features_vec_1, const Eigen::VectorXd &_features_vec_2) const
{
	assert(bases_12_.cols() == _features_vec_1.rows());
	assert(bases_21_.cols() == _features_vec_2.rows());

	Eigen::VectorXd transformed_attributes_1 = bases_12_ * (_features_vec_1 - mean_1_);
	Eigen::VectorXd transformed_attributes_2 = bases_21_ * (_features_vec_2 - mean_2_);
	Eigen::VectorXd diff = transformed_attributes_1 - transformed_attributes_2;

	// Mahalanobis norm.
	double error = diff.transpose() * correlations_.asDiagonal() * diff;
	return error;
}
*/

max_stars_repo_name: mhsung/cuboid-prediction

max_issues_repo_name: mhsung/cuboid-prediction

max_forks_repo_name: mhsung/cuboid-prediction


text:
#include <boost/timer/timer.hpp>

#ifndef GRAMTOOLS_TIMER_REPORT_HPP
#define GRAMTOOLS_TIMER_REPORT_HPP

namespace gram {
class TimerReport {
 public:
  void start(std::string note);

  void stop();

  void report() const;

  template <typename TypeCol1, typename TypeCol2>
  void cout_row(TypeCol1 col1, TypeCol2 col2) const;

 private:
  using Note = std::string;
  using Entry = std::pair<Note, double>;

  Note note;
  std::vector<Entry> logger;
  boost::timer::cpu_timer timer;
};
}  // namespace gram

#endif  // GRAMTOOLS_TIMER_REPORT_HPP

max_stars_repo_name: bricoletc/gramtools

max_issues_repo_name: bricoletc/gramtools

max_forks_repo_name: bricoletc/gramtools


text:
/**
3DSmoothNet
core.cpp

Purpose: containes all the functions used to generate the  Smoothed Density Value voxel representation
of the interest points neighborhood

@Author : Zan Gojcic, Caifa Zhou
@Version : 1.0

*/

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <pcl/common/centroid.h>
#include <pcl/filters/extract_indices.h>
#include <omp.h>
#include "core.h"

namespace po = boost::program_options;

/*
	Utility functions
*/

// Processes command line arguments and returns the default values if the parameters are not specified by the user
bool processCommandLine(int argc, char** argv,
	std::string &file_cloud,
	float &support_radius,
	int &num_voxels,
	float &smoothing_kernel_width,
	std::string &file_keypoints,
	std::string &output_folder)
{
	try
	{
		po::options_description desc("Allowed options");
		desc.add_options()
			("help", "Given a point cloud and a support radius, this code generates a SDV voxel representation for the selected interest points.")
			("fileCloud,f", po::value<std::string>(&file_cloud)->required(), "Input point cloud file in .ply format")
			("supportRadius,r", po::value<float>(&support_radius)->default_value(0.150),
				"Half size of the voxel grid.")
			("numVoxels,n", po::value<int>(&num_voxels)->default_value(16),
				"Number of voxels in a side of the grid. Whole grid is nxnxn.")
			("smoothingKernelWidth,h", po::value<float>(&smoothing_kernel_width)->default_value(1.75),
				"Width of the Gaussia kernel used for smoothing.")
			("fileKeypoints,k", po::value<std::string>(&file_keypoints)->default_value("0"),
				"Path to the file with the indices of the interest points. If 0, SDV voxel grid representation if computed for all the points")
			("outputFolder,o", po::value<std::string>(&output_folder)->default_value("./data/sdv_voxel_grid/"),
				"Output folder path.")
			;

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

		if (vm.count("help"))
		{
			std::cout << desc << "\n";
			return false;
		}

		po::notify(vm);
	}
	catch (std::exception& e)
	{
		std::cerr << "ERROR: " << e.what() << "\n";
		return false;
	}
	catch (...)
	{
		std::cerr << "Unknown error!" << "\n";
		return false;
	}

	return true;
}

// Reads a file with the interest point indices file
std::vector<int> readKeypoints(std::string filename)
{
    char separator = ' ';
    std::vector<int> result;
    std::string row, item;

    std::ifstream in(filename);

    while (getline(in, row))
    {
        std::stringstream ss(row);
        std::getline(ss, item, separator);
        result.push_back(std::stoi(item.c_str()));
    }

    in.close();
    return result;
}

// Checks if the file exists
bool fileExist(const std::string& name)
{
    std::ifstream f(name.c_str());  // New enough C++ library will accept just name
    return f.is_open();
}

// Saves the descriptor to a binary csv file
void saveVector(std::string filename, const std::vector<std::vector<float>> descriptor)
{
    std::cout << "Saving Features to a CSV file:" << std::endl;
    std::cout << filename << std::endl;

    std::ofstream outFile;
    outFile.open(filename, std::ios::binary);

    float writerTemp;
    for (int i = 0; i < descriptor.size(); i++)
    {
        for (int j = 0; j < descriptor[i].size(); j++)
        {
            writerTemp = descriptor[i][j];
            outFile.write(reinterpret_cast<const char*>(&writerTemp), sizeof(float));
        }
    }
    outFile.close();
}

// Initizales a grid using the step size and the number of voxels per side
flann::Matrix<float> initializeGridMatrix(const int n, float x_step, float y_step, float z_step)
{
    int grid_size = n*n*n;
    flann::Matrix<float> input(new float[grid_size * 3], grid_size, 3);

    float xs = -(n / 2)*x_step + 0.5*x_step;
    float ys = -(n / 2)*y_step + 0.5*y_step;
    float zs = -(n / 2)*z_step + 0.5*z_step;

    for (int i = 0; i < n; i++)
    {
        //move on x axis
        for (int j = 0; j < n; j++)
        {
            //move on y axis
            for (int k = 0; k < n; k++)
            {
                //move on z axis
                input[i + n*j + n*n*k][0] = xs + x_step * i;
                input[i + n*j + n*n*k][1] = ys + y_step * j;
                input[i + n*j + n*n*k][2] = zs + z_step * k;
            }
        }
    }
    return input;
}

/*
	Functions for estimation of the local reference frame
*/

// Estimates the Z axis of the local reference frame
void toldiComputeZaxis(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Vertex &z_axis, std::vector<float> point_dst)
{
    int i;
    pcl::PointXYZ query_point = cloud->points[0];
    // calculate covariance matrix
    Eigen::Matrix3f Cov = Eigen::Matrix3f::Zero();
    Eigen::Matrix<float, 4, 1> centroid;
    centroid[0] = query_point.x;
    centroid[1] = query_point.y;
    centroid[2] = query_point.z;

    Eigen::Vector4f queryPointVector = query_point.getVector4fMap();
    Eigen::Matrix<float, Eigen::Dynamic, 4> vij(point_dst.size(), 4);
    int valid_nn_points = 0;
    double distance = 0.0;
    double sum = 0.0;

    pcl::computeCovarianceMatrix(*cloud, centroid, Cov);

    EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_min;
    EIGEN_ALIGN16 Eigen::Vector3f normal;
    pcl::eigen33(Cov, eigen_min, normal);
    z_axis.x = normal(0);
    z_axis.y = normal(1);
    z_axis.z = normal(2);

    // z-axis sign disambiguity
    float z_sign = 0;
    for (i = 0; i < cloud->points.size(); i++)
    {
        float vec_x = query_point.x - cloud->points[i].x;
        float vec_y = query_point.y - cloud->points[i].y;
        float vec_z = query_point.z - cloud->points[i].z;
        z_sign += (vec_x*z_axis.x + vec_y*z_axis.y + vec_z*z_axis.z);
    }
    if (z_sign < 0)
    {
        z_axis.x = -z_axis.x;
        z_axis.y = -z_axis.y;
        z_axis.z = -z_axis.z;
    }
}

// Estimates the X axis of the local reference frame
void toldiComputeXaxis(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Vertex z_axis, float sup_radius, std::vector<float> point_dst, Vertex &x_axis)
{
    int i, j;
    pcl::PointXYZ query_point = cloud->points[0];
    //
    std::vector<Vertex> vec_proj;
    std::vector<float> dist_weight, sign_weight;//store weights w1,w2
    for (i = 0; i < cloud->points.size(); i++)
    {
        Vertex temp;
        Vertex pq = { cloud->points[i].x - query_point.x,cloud->points[i].y - query_point.y,cloud->points[i].z - query_point.z };
        float proj = z_axis.x*pq.x + z_axis.y*pq.y + z_axis.z*pq.z;
        if (proj >= 0)
            sign_weight.push_back(pow(proj, 2));
        else
            sign_weight.push_back(-pow(proj, 2));
        temp.x = pq.x - proj*z_axis.x;
        temp.y = pq.y - proj*z_axis.y;
        temp.z = pq.z - proj*z_axis.z;
        vec_proj.push_back(temp);
    }

    for (i = 0; i < point_dst.size(); i++)
    {
        float wei_temp = sup_radius - point_dst[i];
        wei_temp = pow(wei_temp, 2);
        dist_weight.push_back(wei_temp);
    }
    Vertex x_axis_temp = { 0.0f,0.0f,0.0f };
    for (i = 0; i < cloud->points.size(); i++)
    {
        float weight_sum = dist_weight[i] * sign_weight[i];
        x_axis_temp.x += weight_sum*vec_proj[i].x;
        x_axis_temp.y += weight_sum*vec_proj[i].y;
        x_axis_temp.z += weight_sum*vec_proj[i].z;
    }
    //Normalization
    float size = sqrt(pow(x_axis_temp.x, 2) + pow(x_axis_temp.y, 2) + pow(x_axis_temp.z, 2));
    x_axis_temp.x /= size;
    x_axis_temp.y /= size;
    x_axis_temp.z /= size;
    x_axis = x_axis_temp;
}

// Estimates the Y axis of the local reference frame
void toldiComputeYaxis(Vertex x_axis, Vertex z_axis, Vertex &y_axis)
{
    Eigen::Vector3f x(x_axis.x, x_axis.y, x_axis.z);
    Eigen::Vector3f z(z_axis.x, z_axis.y, z_axis.z);
    Eigen::Vector3f y;

    y = x.cross(z);//cross product

    y_axis.x = y(0);
    y_axis.y = y(1);
    y_axis.z = y(2);
}

// Compute the lrf accordin to the method from toldi paper, for the points selected with the indices
void toldiComputeLRF(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                     std::vector<int> indices,
                     float sup_radius,
                     float smoothingFactor,
                     std::vector<LRF> &cloud_LRF,
                     std::vector<std::vector <int>>& neighbors,
                     std::vector<std::vector <int>>& neighbors_smoothing_idx,
                     std::vector<std::vector <float>>& neighbors_smoothing_distance)
{
    int i, j, m;
    // Initialize all the variables
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    std::vector<int> point_idx;
    std::vector<float> point_dst;
    kdtree.setInputCloud(cloud);
    pcl::PointXYZ query_point;
    pcl::PointXYZ test;
    //LRF calculation
    for (i = 0; i < indices.size(); i++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_neighbor(new pcl::PointCloud<pcl::PointXYZ>);//local surface
        pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_neighbor_z(new pcl::PointCloud<pcl::PointXYZ>);//local surface for computing the z-axis of LRF
        query_point = cloud->points[indices[i]];

        if (kdtree.radiusSearch(query_point, sup_radius, point_idx, point_dst) > 10)//only if there are more than 10 points in the local surface
        {

            for (j = 0; j < point_idx.size(); j++)
            {

                test.x = cloud->points[point_idx[j]].x - query_point.x;
                test.y = cloud->points[point_idx[j]].y - query_point.y;
                test.z = cloud->points[point_idx[j]].z - query_point.z;

                sphere_neighbor_z->points.push_back(test);
            }

            // Save points for feature computation
			neighbors.at(indices[i]) = point_idx;

            std::transform(point_dst.begin(), point_dst.end(), point_dst.begin(), std::ptr_fun<float, float>(std::sqrt));
            // Find first element that has a distance bigger than the smoothing threshold
            auto lower = std::lower_bound(point_dst.begin(), point_dst.end(), smoothingFactor);
            // Index of the last element smaller then the threshold
            int  index_last_element = lower - point_dst.begin();

            // Copy neighbours to vector
            std::vector<int> point_idx_smoothing(&point_idx[0], &point_idx[index_last_element]);
            std::vector<float> point_distance_smoothing(&point_dst[0], &point_dst[index_last_element]);

			neighbors_smoothing_idx.at(indices[i]) = point_idx_smoothing;
			neighbors_smoothing_distance.at(indices[i]) = point_distance_smoothing;

            for (j = 0; j < point_idx.size(); j++)
            {
                sphere_neighbor->points.push_back(cloud->points[point_idx[j]]);
            }

            Vertex x_axis, y_axis, z_axis;
            toldiComputeZaxis(sphere_neighbor_z, z_axis, point_dst);
            toldiComputeXaxis(sphere_neighbor, z_axis, sup_radius, point_dst, x_axis);
            toldiComputeYaxis(x_axis, z_axis, y_axis);
            LRF temp = { indices[i],x_axis,y_axis,z_axis };
			cloud_LRF.at(indices[i]) = temp;
        }
        else
        {
            std::cout << "Less then ten points in the neighborhood!!!" << std::endl;
            LRF temp = { NULL_POINTID,{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f} };
			cloud_LRF.at(indices[i]) = temp;
        }
    }
}

/*
	Functions used for computation of the SDV voxel grid representation
*/

// transform the local neighbhorhood of the selecred interest point to its canonical representation defined by the estimated local reference frame
void transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, LRF pointLRF, pcl::PointCloud<pcl::PointXYZ>::Ptr &transformed_cloud)
{
    pcl::PointXYZ point = cloud->points[0]; //the centroid of the local surface
    int number_of_points = cloud->points.size() - 1; // remove the first point which is centroid
    transformed_cloud->points.resize(number_of_points);
    Eigen::Matrix3f matrix;
    matrix(0, 0) = pointLRF.x_axis.x;
    matrix(0, 1) = pointLRF.x_axis.y;
    matrix(0, 2) = pointLRF.x_axis.z;
    matrix(1, 0) = pointLRF.y_axis.x;
    matrix(1, 1) = pointLRF.y_axis.y;
    matrix(1, 2) = pointLRF.y_axis.z;
    matrix(2, 0) = pointLRF.z_axis.x;
    matrix(2, 1) = pointLRF.z_axis.y;
    matrix(2, 2) = pointLRF.z_axis.z;

    // Iterrate over all the points and save the transfomed version (+1 because one point is ommited at the start )
    for (int i = 0; i < number_of_points; i++)
    {
        Eigen::Vector3f transformed_point(
            cloud->points[i + 1].x - point.x,
            cloud->points[i + 1].y - point.y,
            cloud->points[i + 1].z - point.z);

        transformed_point = matrix * transformed_point;

        pcl::PointXYZ new_point;
        new_point.x = transformed_point(0);
        new_point.y = transformed_point(1);
        new_point.z = transformed_point(2);
        transformed_cloud->points[i] = new_point;
    }
}

// estimate the SDV voxel grid for all interes points
void computeLocalDepthFeature(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                              std::vector<int> evaluation_points,
                              std::vector<std::vector<int>> indices_neighbors,
                              std::vector<LRF> cloud_LRF,
                              float sup_radius,
                              flann::Matrix<float> voxel_coordinates,
                              int num_voxels,
                              float smoothing_factor,
                              std::string saveFileName)
{
    // Iterrate over all the points for which the descriptor is to be computed
    pcl::PointXYZ queryPoint;
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    int counter_voxel = num_voxels * num_voxels * num_voxels;

    // Create the filtering object
    std::string saving_path_file;
    int threads_ = omp_get_max_threads();
    std::cout << "Starting SDV computation!" << std::endl;
    std::cout << threads_ << " threads will be used!!" << std::endl;

    // Initialize the space for the SDV values
    std::vector <std::vector <float>> DIMATCH_Descriptor(evaluation_points.size(), std::vector<float>(counter_voxel, 0));

    // Initialize the point to the descriptor for each thread used
    Eigen::VectorXf *descriptor = new Eigen::VectorXf[threads_];

    for (int i = 0; i < threads_; i++)
    {
        descriptor[i].setZero(counter_voxel);
    }

    int tid;

    // Create path for saving
    const char* path = "data";
    boost::filesystem::path dir(path);
    boost::filesystem::create_directory(dir);
    int progress_counter = 0;

    #pragma omp parallel for shared(cloud,evaluation_points,indices_neighbors,counter_voxel,DIMATCH_Descriptor,progress_counter) private(tid,extract)  num_threads(threads_)
    for (int i = 0; i < evaluation_points.size(); i++)
    {

        pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_neighbors(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_neighbors_transformed(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        std::vector<std::vector<size_t>> indices;
        std::vector<std::vector<float>> dists;

        tid = omp_get_thread_num();
        descriptor[tid].setZero(counter_voxel);
        // Save query points coordinates
        queryPoint = cloud->points[evaluation_points[i]];

        // Extract neighbors from point cloud
        extract.setInputCloud(cloud);
        inliers->indices = indices_neighbors[evaluation_points[i]];
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(*sphere_neighbors);

        // Transform the neighbors to the local reference frame
        if (sphere_neighbors->points.size() != 0)
        {
            transformCloud(sphere_neighbors, cloud_LRF[evaluation_points[i]], sphere_neighbors_transformed);

            flann::Matrix<float> data(new float[sphere_neighbors_transformed->size() * 3], sphere_neighbors_transformed->size(), 3);

            for (int j = 0; j < sphere_neighbors_transformed->size(); j++)
            {
                data[j][0] = sphere_neighbors_transformed->points[j].x;
                data[j][1] = sphere_neighbors_transformed->points[j].y;
                data[j][2] = sphere_neighbors_transformed->points[j].z;
            }

            flann::SearchParams search_parameters = flann::SearchParams();
            search_parameters.checks = -1;
            search_parameters.sorted = false;
            search_parameters.use_heap = flann::FLANN_True;

            flann::KDTreeSingleIndexParams index_parameters = flann::KDTreeSingleIndexParams();
            flann::KDTreeSingleIndex<flann::L2_3D<float> > index(data, index_parameters);
            index.buildIndex();

            // Square the smoothing factor as flann uses sqaured distances
            float smoothing_factor_sqrd = 9 * smoothing_factor*smoothing_factor;
            index.radiusSearch(voxel_coordinates, indices, dists, smoothing_factor_sqrd, search_parameters);

            // Computes the normalization term and the variance
            double normalization_term = 1 / (sqrt(2 * M_PI) * smoothing_factor);
            double variance_term = -1 / (2 * (smoothing_factor * smoothing_factor));

            for (int voxel_idx = 0; voxel_idx < counter_voxel - 1; voxel_idx++)
            {
                //cout << " thread: " << omp_get_thread_num() << endl;
                // Extract all the distances
                std::vector<float> point_distances = dists[voxel_idx];
                if (!point_distances.empty())
                {
                    // Multiply with variance term
                    std::transform(point_distances.begin(), point_distances.end(), point_distances.begin(),
                                   std::bind1st(std::multiplies<float>(), variance_term));

                    // Exponent
                    std::transform(point_distances.begin(), point_distances.end(), point_distances.begin(), std::ptr_fun<float, float>(std::exp));

                    // Normalization term
                    std::transform(point_distances.begin(), point_distances.end(), point_distances.begin(),
                                   std::bind1st(std::multiplies<float>(), normalization_term));

                    // Sum the elements
                    float sum_of_elems = std::accumulate(point_distances.begin(), point_distances.end(), 0.0f) / point_distances.size();

                    // Sum up all the depths value to the voxel depth value
                    descriptor[tid][voxel_idx] = sum_of_elems;

                }
                else
                {
                    // if no points in the neighborhood set the voxel depth to 0
                    descriptor[tid][voxel_idx] = 0;
                }

            }

            // Normalize the descriptor
            float descriptor_sum = descriptor[tid].sum();
            if (descriptor_sum != 0)
                descriptor[tid] = descriptor[tid] / descriptor_sum;

            for (int d = 0; d < descriptor[tid].size(); ++d)
                DIMATCH_Descriptor[i][d] = descriptor[tid][d];

        }
        else
        {
            for (int voxel_idx = 0; voxel_idx < counter_voxel - 1; voxel_idx++)
            {
                descriptor[tid][voxel_idx] = 0;
            }

            for (int d = 0; d < descriptor[tid].size(); ++d)
                DIMATCH_Descriptor[i][d] = descriptor[tid][d];

        }
    }

    // Create the name with the radius, number of voxels and the smoothing factor
    saving_path_file = saveFileName + "_" + std::to_string(sup_radius) + "_" + std::to_string(num_voxels) + "_" + std::to_string(smoothing_factor * num_voxels / sup_radius) + ".csv";

    // Write descriptor to CSV file
    saveVector(saving_path_file, DIMATCH_Descriptor);
}