text:
#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include <memory>
#include <fstream>

#include "tasks/internal/archivate_task.h"
#include "tasks/internal/cp_task.h"
#include "tasks/internal/extract_task.h"
#include "tasks/internal/mkdir_task.h"
#include "tasks/internal/rename_task.h"
#include "tasks/internal/rm_task.h"
#include "tasks/internal/fetch_task.h"
#include "tasks/internal/exists_task.h"
#include "tasks/external_task.h"
#include "tasks/root_task.h"
#include "tasks/task_factory.h"
#include "tasks/create_params.h"
#include "config/sandbox_config.h"
#include "mocks.h"

#define BOOST_FILESYSTEM_NO_DEPRECATED
#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;

std::shared_ptr<task_metadata> get_task_meta()
{
	auto res = std::make_shared<task_metadata>();
	res->task_id = "id2";
	res->priority = 3;
	res->fatal_failure = false;
	res->dependencies = {"dep1", "dep2", "dep3"};
	res->binary = "command";
	res->cmd_args = {"arg1", "arg2"};
	res->sandbox = std::make_shared<sandbox_config>();
	res->sandbox->loaded_limits.insert(
		std::pair<std::string, std::shared_ptr<sandbox_limits>>("group1", std::make_shared<sandbox_limits>()));
	return res;
}

std::shared_ptr<task_metadata> get_three_args()
{
	auto res = get_task_meta();
	res->cmd_args = {"one", "two", "three"};
	return res;
}

std::shared_ptr<task_metadata> get_two_args()
{
	auto res = get_task_meta();
	res->cmd_args = {"one", "two"};
	return res;
}

std::shared_ptr<task_metadata> get_one_args()
{
	auto res = get_task_meta();
	res->cmd_args = {"one"};
	return res;
}

std::shared_ptr<task_metadata> get_zero_args()
{
	auto res = get_task_meta();
	res->cmd_args = {};
	return res;
}

TEST(Tasks, InternalArchivateTask)
{
	EXPECT_THROW(archivate_task(1, get_three_args()), task_exception);
	EXPECT_THROW(archivate_task(1, get_one_args()), task_exception);
	EXPECT_THROW(archivate_task(1, get_zero_args()), task_exception);
	EXPECT_NO_THROW(archivate_task(1, get_two_args()));
}

TEST(Tasks, InternalCpTask)
{
	EXPECT_THROW(cp_task(1, get_three_args()), task_exception);
	EXPECT_THROW(cp_task(1, get_one_args()), task_exception);
	EXPECT_THROW(cp_task(1, get_zero_args()), task_exception);
	EXPECT_NO_THROW(cp_task(1, get_two_args()));
}

TEST(Tasks, InternalCpTaskExceptionBug)
{
	auto meta = get_task_meta();
	meta->cmd_args = {"/proc/self/cmdline", "/nonexistingdir/a"};
	std::shared_ptr<task_results> res;
	EXPECT_NO_THROW(res = cp_task(1, meta).run());
	EXPECT_EQ(task_status::FAILED, res->status);
}

TEST(Tasks, InternalCpTaskWildcards)
{
	// prepare directories and files
	auto temp = fs::temp_directory_path();
	auto src_dir = temp / fs::unique_path();
	auto dst_dir = temp / fs::unique_path();
	fs::create_directories(src_dir);
	fs::create_directories(dst_dir);
	for (const auto &i : {"a1", "a2", "b1"}) {
		std::ofstream ofs((src_dir / i).string());
		ofs << i;
	}

	// run the test and check asserts
	auto meta = get_task_meta();
	meta->cmd_args = {src_dir.string() + "/a*", dst_dir.string()};
	std::shared_ptr<task_results> res;
	EXPECT_NO_THROW(res = cp_task(1, meta).run());
	EXPECT_EQ(task_status::OK, res->status);
	EXPECT_TRUE(fs::is_regular_file(dst_dir / "a1"));
	EXPECT_TRUE(fs::is_regular_file(dst_dir / "a2"));
	EXPECT_FALSE(fs::is_regular_file(dst_dir / "b1"));

	// cleanup
	fs::remove_all(src_dir);
	fs::remove_all(dst_dir);
}

TEST(Tasks, InternalExtractTask)
{
	EXPECT_THROW(extract_task(1, get_three_args()), task_exception);
	EXPECT_THROW(extract_task(1, get_one_args()), task_exception);
	EXPECT_THROW(extract_task(1, get_zero_args()), task_exception);
	EXPECT_NO_THROW(extract_task(1, get_two_args()));
}

TEST(Tasks, InternalMkdirTask)
{
	EXPECT_NO_THROW(mkdir_task(1, get_three_args()));
	EXPECT_NO_THROW(mkdir_task(1, get_one_args()));
	EXPECT_THROW(mkdir_task(1, get_zero_args()), task_exception);
	EXPECT_NO_THROW(mkdir_task(1, get_two_args()));
}

TEST(Tasks, InternalRenameTask)
{
	EXPECT_THROW(rename_task(1, get_three_args()), task_exception);
	EXPECT_THROW(rename_task(1, get_one_args()), task_exception);
	EXPECT_THROW(rename_task(1, get_zero_args()), task_exception);
	EXPECT_NO_THROW(rename_task(1, get_two_args()));
}

TEST(Tasks, InternalRmTask)
{
	EXPECT_NO_THROW(rm_task(1, get_three_args()));
	EXPECT_NO_THROW(rm_task(1, get_one_args()));
	EXPECT_THROW(rm_task(1, get_zero_args()), task_exception);
	EXPECT_NO_THROW(rm_task(1, get_two_args()));
}

TEST(Tasks, InternalFetchTask)
{
	EXPECT_THROW(fetch_task(1, get_three_args(), nullptr), task_exception);
	EXPECT_THROW(fetch_task(1, get_one_args(), nullptr), task_exception);
	EXPECT_THROW(fetch_task(1, get_zero_args(), nullptr), task_exception);
	EXPECT_NO_THROW(fetch_task(1, get_two_args(), nullptr));
}

TEST(Tasks, InternalExistsTask)
{
	EXPECT_THROW(exists_task(1, get_zero_args()), task_exception);
	EXPECT_THROW(exists_task(1, get_one_args()), task_exception);
	EXPECT_NO_THROW(exists_task(1, get_two_args()));
	EXPECT_NO_THROW(exists_task(1, get_three_args()));
}

class test_task_base : public task_base
{
public:
	test_task_base(std::size_t id, std::shared_ptr<task_metadata> task_meta) : task_base(id, task_meta)
	{
	}
	virtual ~test_task_base()
	{
	}
	std::shared_ptr<task_results> run()
	{
		return nullptr;
	}
};

TEST(Tasks, TaskBase)
{
	test_task_base base(1, get_task_meta());
	EXPECT_EQ(base.get_cmd(), "command");
	std::vector<std::string> dep{"dep1", "dep2", "dep3"};
	EXPECT_EQ(base.get_dependencies(), dep);
	EXPECT_EQ(base.get_fatal_failure(), false);
	EXPECT_EQ(base.get_id(), static_cast<std::size_t>(1));
	EXPECT_EQ(base.get_priority(), static_cast<std::size_t>(3));
	EXPECT_EQ(base.get_task_id(), "id2");
	EXPECT_TRUE(base.get_children().empty());
	auto children = std::shared_ptr<task_base>(new test_task_base(2, get_task_meta()));
	base.add_children(children);
	EXPECT_EQ(base.get_children()[0]->get_task_id(), children->get_task_id());
}

TEST(Tasks, TaskFactory)
{
	task_factory factory(nullptr);
	auto meta = get_task_meta();
	std::shared_ptr<task_base> task;

	// archivate task
	meta->binary = "archivate";
	task = factory.create_internal_task(0, meta);
	EXPECT_NE(std::dynamic_pointer_cast<archivate_task>(task), nullptr);

	// cp task
	meta->binary = "cp";
	task = factory.create_internal_task(0, meta);
	EXPECT_NE(std::dynamic_pointer_cast<cp_task>(task), nullptr);

	// extract task
	meta->binary = "extract";
	task = factory.create_internal_task(0, meta);
	EXPECT_NE(std::dynamic_pointer_cast<extract_task>(task), nullptr);

	// fetch task
	meta->binary = "fetch";
	task = factory.create_internal_task(0, meta);
	EXPECT_NE(std::dynamic_pointer_cast<fetch_task>(task), nullptr);

	// mkdir task
	meta->binary = "mkdir";
	task = factory.create_internal_task(0, meta);
	EXPECT_NE(std::dynamic_pointer_cast<mkdir_task>(task), nullptr);

	// rename task
	meta->binary = "rename";
	task = factory.create_internal_task(0, meta);
	EXPECT_NE(std::dynamic_pointer_cast<rename_task>(task), nullptr);

	// rm task
	meta->binary = "rm";
	task = factory.create_internal_task(0, meta);
	EXPECT_NE(std::dynamic_pointer_cast<rm_task>(task), nullptr);

	// root task
	// - with explicit nullptr argument
	meta->binary = "archivate";
	task = factory.create_internal_task(0, nullptr);
	EXPECT_NE(std::dynamic_pointer_cast<root_task>(task), nullptr);
	// - without explicit meta argument
	task = factory.create_internal_task(0);
	EXPECT_NE(std::dynamic_pointer_cast<root_task>(task), nullptr);

	// unknown internal task
	meta->binary = "unknown_internal_bianry";
	task = factory.create_internal_task(0, meta);
	EXPECT_EQ(task, nullptr);

	// worker config
	auto worker_conf = std::make_shared<mock_worker_config>();
	EXPECT_CALL((*worker_conf), get_worker_id()).WillRepeatedly(Return(8));

	// external task
	meta->binary = "external_command";
	create_params params = {worker_conf, 1, meta, meta->sandbox->loaded_limits["group1"], nullptr, "", fs::path()};
	EXPECT_THROW(factory.create_sandboxed_task(params), task_exception); // Sandbox config is nullptr
	meta->sandbox = std::make_shared<sandbox_config>();
	meta->sandbox->name = "whatever_sandbox";
	EXPECT_THROW(factory.create_sandboxed_task(params), task_exception); // Unknown sandbox type
// Isolate only supported on linux platform
#ifndef _WIN32
	meta->sandbox->name = "isolate";
	task = factory.create_sandboxed_task(params);
	EXPECT_NE(std::dynamic_pointer_cast<external_task>(task), nullptr);
#endif
}

max_stars_repo_name: rqu/worker

max_issues_repo_name: rqu/worker

max_forks_repo_name: rqu/worker


text: #include <boost/property_map/function_property_map.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018. Toyota Research Institute
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of CNRS-LAAS and AIST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** @author Sean Curtis */

#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h"

#include <gtest/gtest.h>
#include <Eigen/Dense>

#include "fcl/common/types.h"

namespace fcl {
namespace detail {

using Vector3d = Vector3<double>;

// Helper functions to work with libccd types -- the ccd_vec3_t is a nuisance.
// This allows working with Eigen Vector3d's instead.

ccd_vec3_t eigen_to_ccd(const Vector3d& vector) {
  ccd_vec3_t out;
  out.v[0] = vector(0);
  out.v[1] = vector(1);
  out.v[2] = vector(2);
  return out;
}

Vector3d ccd_to_eigen(const ccd_vec3_t& vector) {
  // TODO(SeanCurtis-TRI): When libccd is *always* double precision, this can
  // become: `return Vector3d{vector.v};`
  return Vector3d{vector.v[0], vector.v[1], vector.v[2]};
}

// Tests that the given vector and libccd vector are the same with in a given
// tolerance.
::testing::AssertionResult are_same(const Vector3d& expected,
                                    const ccd_vec3_t& tested,
                                    double tolerance) {
  if (tolerance < 0) {
    return ::testing::AssertionFailure() << "Invalid tolerance: "
                                         << tolerance;
  }

  Vector3d ccd = ccd_to_eigen(tested);
  Vector3d error = (expected - ccd).cwiseAbs();

  for (int i = 0; i < 3; ++i) {
    if (error(i) > tolerance) {
      return ::testing::AssertionFailure()
          << "Values at index " << i
          << " exceed tolerance; " << expected(i) << " vs "
          << ccd(i) << ", diff = " << error(i)
          << ", tolerance = " << tolerance << "\nexpected = "
          << expected.transpose() << "\ntested = "
          << ccd.transpose() << "\n|delta| = "
          << error.transpose();
    }
  }
  return ::testing::AssertionSuccess();
}

// These are tests on the extractClosestPoints() function (and its underlying
// support functions).
//
// These functions map a simplex and a point of interest on the "surface" of the
// simplex into a pair of points. The simplex is defined by support vertices
// (ccd_support_t). Each support vertex contains three vectors in R3. They
// represent:
//   1. A vertex `v` on the boundary of Minkowski difference of two objects:
//      O₁ ⊕ -O₂. It is, by definition the difference of two vertex positions
//      on the two objects: `v = v₁ - v₂`.
//   2. The vertex `v₁` on O₁ that contributes to the definition of `v`.
//   3. The vertex `v₂` on O₂ that contributes to the definition of `v`.
//
// extractClosestPoints() supports 1-, 2- and 3-simplices.
//
// The corresponding points are written to output parameters. If the output
// parameters are null, the value is not computed. These tests exercise all
// permutations of the two output parameters.

// Simple wrapper to facilitate working with Eigen primitives in place of libccd
// primitives.
bool are_coincident(const Vector3d& p, const Vector3d& q) {
  return libccd_extension::are_coincident(eigen_to_ccd(p), eigen_to_ccd(q));
}

// Tests the `are_coincident` function.
GTEST_TEST(DegenerateGeometry, CoincidentPoints) {
  // The coincidence test uses this as the threshold.
  const ccd_real_t eps = constants<ccd_real_t>::eps();
  const ccd_real_t almost_eps = eps * 0.75;
  const ccd_real_t extra_eps = eps * 1.5;

  Vector3d p{1, 1, 1};
  Vector3d q{p};
  // Exact coincidence at unit scale (down to the last bit)
  EXPECT_TRUE(are_coincident(p, q));

  // Coincidence within a unit-scaled epsilon.
  EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps, 0, 0}, q));
  EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps, 0}, q));
  EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps}, q));

  // Distinct points just outside a unit-scaled epsilon.
  EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps, 0, 0}, q));
  EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps, 0}, q));
  EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps}, q));

  // Coincidence within a larger-than-unit-scale scale factor.
  const double scale = 100;
  p << scale, scale, scale;
  q = p;
  // Exact coincidence at a larger-than-unit-scale (down to the last bit).
  EXPECT_TRUE(are_coincident(p, q));

  // Coincidence within a larger-than-unit-scale epsilon.
  EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps * scale, 0, 0}, q));
  EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps * scale, 0}, q));
  EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps * scale}, q));

  // Distinct points just outside a larger-than-unit-scaled epsilon.
  EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps * scale, 0, 0}, q));
  EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps * scale, 0}, q));
  EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps * scale}, q));

  // Coincidence within a smaller-than-unit-scale scale factor. NOTE: eps
  // stays at an *absolute* tolerance as the scale gets smaller.
  p << 0.01, 0.01, 0.01;
  q = p;
  // Exact coincidence at a smaller-than-unit-scale (down to the last bit).
  EXPECT_TRUE(are_coincident(p, q));

  // Coincidence within a smaller-than-unit-scale epsilon.
  EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps, 0, 0}, q));
  EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps, 0}, q));
  EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps}, q));

  // Distinct points just outside a smaller-than-unit-scaled epsilon.
  EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps, 0, 0}, q));
  EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps, 0}, q));
  EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps}, q));
}

// Wrapper to allow invocation of triangle_area_is_zero with eigen primitives.
bool triangle_area_is_zero(const Vector3d& a, const Vector3d& b,
                           const Vector3d& c) {
  return fcl::detail::libccd_extension::triangle_area_is_zero(eigen_to_ccd(a),
                                                              eigen_to_ccd(b),
                                                              eigen_to_ccd(c));
}

// Tests the `triangle_area_is_zero()` function. NOTE: This computation
// makes use of `are_coincident()`. Only a single test of coincident
// vertices is provided (to exercise the code path). The permutations for how
// two points can be considered coincident are done in their own tests.
GTEST_TEST(DegenerateGeometry, ZeroAreaTriangle) {
  using std::asin;
  Vector3d a{0, 0, 0};
  Vector3d b{1, 0, 0};
  Vector3d c{0, 1, 0};

  // Viable triangle
  EXPECT_FALSE(triangle_area_is_zero(a, b, c));

  // Triangle with token coincident vertices to make sure it keys on it.
  EXPECT_TRUE(triangle_area_is_zero(a, a, c));

  // Co-linearity tests.

  // Obvious co-linearity
  EXPECT_TRUE(triangle_area_is_zero(a, b, 3 * b));

  // Test co-linearity
  // The co-linear threshold is based on the angles of the triangle. If the
  // triangle has an angle θ, such that |sin(θ)| < ε, then the triangle is
  // considered to be degenerate. This condition implicitly defines an envelope
  // around θ. We define θₑ such that sin(θₑ) = ε. The condition |sin(θ)| < ε
  // implies |θ| < θₑ. So, if the smallest angle is less than θₑ, the triangle
  // will be considered co-linear. If the smallest angle is larger than θₑ, the
  // triangle is considered valid.
  //
  // The test wants to provide proof as to the boundary of the tolerance
  // envelope. Therefore, it will construct two triangles. One whose smallest
  // angle is θₑ - δ and the other triangle's smallest angle is θₑ + δ, for
  // some suitably small δ.
  //
  // The `triangle_area_is_zero()` function has a second failure condition:
  // coincident points. We'll make sure that the points are all far enough away
  // from each other that this failure condition won't be triggered.
  //
  // The triangle consists of three vertices: A, B, C.
  //   A: [0, 0, 0] (for simplicity).
  //   B: [1, 1, 1] (again, for simplicity).
  //   C: We must define C such that the angle ∠CAB is θ = θₑ ± δ.
  //
  // We'll do it by construction:
  //   1. Pick a vector v perpendicular to [1, 1, 1] (e.g., v = [1, 1, -2]).
  //   2. C' = B + v̂ * |B|·tan(θ) (with v̂ = v / |v|). This produces a point C'
  //      that:
  //     a) forms a triangle where ∠C`AB < θₑ, if θ < θₑ, but
  //     b) the points B and C' *may* be coincident.
  //   3. Move the point C' farther away (in the AC' direction). This preserves
  //      the angle, but distances B from C'. So, C ≙ s·C' (for some scalar
  //      s ≠ 1). This trick works because A is the origin and we generally
  //      assume that |C'| >> ε.
  //
  // This triangle illustrates the construction (but rotated for ascii art).
  //
  //          A
  //         /|  θ = ∠BAC = ∠BAC'
  //        / |
  //     C'/__|
  //      /  / B
  //     / /
  //    //
  //    C

  const ccd_real_t eps = constants<ccd_real_t>::eps();
  const ccd_real_t theta_e = asin(eps);

  a << 0, 0, 0;
  b << 1, 1, 1;
  const Vector3d v_hat = Vector3d(1, 1, -2).normalized();

  // Triangle where θ < θₑ.
  const ccd_real_t colinear_theta = tan(theta_e * 0.75);
  Vector3d c_prime = b + (b.norm() * colinear_theta) * v_hat;
  c = c_prime * 2;
  EXPECT_FALSE(are_coincident(b, c));
  EXPECT_TRUE(triangle_area_is_zero(a, b, c));

  // Triangle where θ > θₑ.
  const ccd_real_t good_tan = tan(theta_e * 1.5);
  c_prime = b + (b.norm() * good_tan) * v_hat;
  c = c_prime * 2;
  // Confirm the test doesn't report false because the points are coincident.
  EXPECT_FALSE(are_coincident(b, c));
  ASSERT_FALSE(triangle_area_is_zero(a, b, c));
}

// This class creates a single simplex. It is the Minkowski difference of
// one triangles and a vertex.
class ExtractClosestPoint : public ::testing::Test {
 protected:
  void SetUp() override {
    // Configure a 3-simplex; "last" is the index of the last valid simplex.
    // For an n-simplex, last is always n - 1.
    simplex_.last = 2;
    for (int i = 0; i < 3; ++i) {
      const Vector3d minkowski_diff{v0_ - t1_[i]};
      write_support(minkowski_diff, v0_, t1_[i], &simplex_.ps[i]);
    }
  }

  // Write the three Eigen vector values into a ccd support vector.
  static void write_support(const Vector3d& minkowski_diff, const Vector3d& v0,
                            const Vector3d& v1, ccd_support_t* support) {
    support->v = eigen_to_ccd(minkowski_diff);
    support->v1 = eigen_to_ccd(v0);
    support->v2 = eigen_to_ccd(v1);
  }

  // Performs the common work of evaluating extractClosetPoint() on a
  // permutation of parameters.
  void EvaluateExtractClosestPoint(ccd_simplex_t* simplex,
                                   const Vector3d& p0_expected,
                                   const Vector3d& p1_expected,
                                   ccd_vec3_t* closest,
                                   const char* message) {
    using fcl::detail::libccd_extension::extractClosestPoints;

    const Vector3d& dummy1{-1, -2, -3};
    const Vector3d& dummy2{-2, -3, -4};
    ccd_vec3_t p0 = eigen_to_ccd(dummy1);
    ccd_vec3_t p1 = eigen_to_ccd(dummy2);

    // Confirm expected solution are not the dummy values.
    EXPECT_FALSE(are_same(p0_expected, p0, kTolerance));
    EXPECT_FALSE(are_same(p1_expected, p1, kTolerance));

    // Test extraction of neither.
    EXPECT_NO_THROW(extractClosestPoints(simplex, nullptr, nullptr, closest))
              << message;

    // Test extraction of p0.
    EXPECT_NO_THROW(extractClosestPoints(simplex, &p0, nullptr, closest))
              << message;
    EXPECT_TRUE(are_same(p0_expected, p0, kTolerance)) << message;

    // Test extraction of p1.
    EXPECT_NO_THROW(extractClosestPoints(simplex, nullptr, &p1, closest))
              << message;
    EXPECT_TRUE(are_same(p1_expected, p1, kTolerance)) << message;

    // Test extraction of both.
    p0 = eigen_to_ccd(dummy1);
    p1 = eigen_to_ccd(dummy2);
    EXPECT_NO_THROW(extractClosestPoints(simplex, &p0, &p1, closest))
              << message;
    EXPECT_TRUE(are_same(p0_expected, p0, kTolerance)) << message;
    EXPECT_TRUE(are_same(p1_expected, p1, kTolerance)) << message;
  }

  // Perform linear interpolation between points a & b.
  // @pre 0 <= s <= 1.
  static Vector3d lerp(const Vector3d& a, const Vector3d& b, double s) {
    return a * s + b * (1 - s);
  };

  ccd_simplex_t simplex_;

  // Vertex on object 0.
  const Vector3d v0_{0.5, 1, 0};

  // Vertices 0, 1, & 2 for triangle 1.
  const Vector3d t1_[3] = {{0, 0.25, 0.25}, {1, 0.25, 0.25}, {0.5, 0.8, 1.0}};

  // TODO(SeanCurtis-TRI): Change this to 1e-15 when the mac libccd
  // single/double precision has been worked out.
  const double kTolerance{constants<ccd_real_t>::eps_78()};
};

// Test extraction from a 1-simplex support method.
TEST_F(ExtractClosestPoint, ExtractFrom1SimplexSupport) {
  using namespace fcl::detail::libccd_extension;
  const Vector3d& dummy1{-1, -2, -3};
  const Vector3d& dummy2{-2, -3, -4};
  // Set up the single support *point*.
  ccd_vec3_t p0 = eigen_to_ccd(dummy1);
  ccd_vec3_t p1 = eigen_to_ccd(dummy2);

  // Test extraction of neither.
  EXPECT_NO_THROW(
      extractObjectPointsFromPoint(&simplex_.ps[0], nullptr, nullptr));

  // Test extraction of p1.
  EXPECT_NO_THROW(extractObjectPointsFromPoint(&simplex_.ps[0], &p0, nullptr));
  EXPECT_TRUE(are_same(v0_, p0, kTolerance));

  // Test extraction of p2.
  EXPECT_NO_THROW(extractObjectPointsFromPoint(&simplex_.ps[0], nullptr, &p1));
  EXPECT_TRUE(are_same(t1_[0], p1, kTolerance));

  // Test extraction of both.
  p0 = eigen_to_ccd(dummy1);
  p1 = eigen_to_ccd(dummy2);
  extractObjectPointsFromPoint(&simplex_.ps[0], &p0, &p1);
  EXPECT_TRUE(are_same(v0_, p0, kTolerance));
  EXPECT_TRUE(are_same(t1_[0], p1, kTolerance));
}

// Test extraction from a 1-simplex through the extractClosestPoints() method.
TEST_F(ExtractClosestPoint, ExtractFrom1Simplex) {
  simplex_.last = 0;

  // NOTE: For a 1-simplex, the closest point isn't used.
  ccd_vec3_t closest = eigen_to_ccd({0, 0, 0});

  EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest,
                              "ExtractFrom1Simplex");
}

// Test extraction from a 2-simplex support method.
TEST_F(ExtractClosestPoint, ExtractFrom2SimplexSupport) {
  using namespace fcl::detail::libccd_extension;
  const Vector3d& dummy1{-1, -2, -3};
  const Vector3d& dummy2{-2, -3, -4};

  ccd_vec3_t p0 = eigen_to_ccd(dummy1);
  ccd_vec3_t p1 = eigen_to_ccd(dummy2);

  // The query point we're going to use is a simple linear combination of the
  // two end points.
  const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v);
  const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v);

  const double s = 1 / 3.0;
  ASSERT_TRUE(s >= 0 && s <= 1);

  ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s));
  const Vector3d p0_expected = v0_;
  const Vector3d p1_expected = lerp(t1_[0], t1_[1], s);

  // Test extraction of neither.
  EXPECT_NO_THROW(extractObjectPointsFromSegment(
      &simplex_.ps[0], &simplex_.ps[1], nullptr, nullptr, &closest));

  // Test extraction of p1.
  EXPECT_NO_THROW(extractObjectPointsFromSegment(
      &simplex_.ps[0], &simplex_.ps[1], &p0, nullptr, &closest));
  EXPECT_TRUE(are_same(p0_expected, p0, kTolerance));

  // Test extraction of p2.
  EXPECT_NO_THROW(extractObjectPointsFromSegment(
      &simplex_.ps[0], &simplex_.ps[1], nullptr, &p1, &closest));
  EXPECT_TRUE(are_same(p1_expected, p1, kTolerance));

  // Test extraction of both.
  p0 = eigen_to_ccd(dummy1);
  p1 = eigen_to_ccd(dummy2);
  EXPECT_NO_THROW(extractObjectPointsFromSegment(
      &simplex_.ps[0], &simplex_.ps[1], &p0, &p1, &closest));
  EXPECT_TRUE(are_same(p0_expected, p0, kTolerance));
  EXPECT_TRUE(are_same(p1_expected, p1, kTolerance));
}

// Test extraction from a 2-simplex through the extractClosestPoints() method.
TEST_F(ExtractClosestPoint, ExtractFrom2Simplex) {
  simplex_.last = 1;

  // The query point we're going to use is a simple linear combination of the
  // two end points.
  const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v);
  const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v);

  const double s = 1 / 3.0;
  ASSERT_TRUE(s >= 0 && s <= 1);

  ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s));
  const Vector3d p0_expected = v0_;
  const Vector3d p1_expected = lerp(t1_[0], t1_[1], s);

  EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest,
                              "ExtractFrom2Simplex");
}

// Tests the case where the simplex is a degenerate simplex -- i.e., it is
// actually a line segment.
TEST_F(ExtractClosestPoint, ExtractFrom2SimplexDegenerate) {
  simplex_.last = 1;
  // NOTE: This exercises the knowledge that the coincidence tolerance is eps.
  const ccd_real_t eps = 0.5 * constants<ccd_real_t>::eps();

  // Copy the first support vertex into the second support vertex and then
  // perturb the second a small amount. We add a small amount to the
  // x-components of the minkowski sum *and* the object1 vertex.
  ccdSupportCopy(&simplex_.ps[1], &simplex_.ps[0]);
  simplex_.ps[1].v.v[0] += eps;
  simplex_.ps[1].v1.v[0] += eps;
  // Confirm that the input and expected answer (v0_) match.
  ASSERT_TRUE(are_same(v0_, simplex_.ps[0].v1, kTolerance));

  // The line segment is now of length 1; the answer should be essentially the
  // same as evaluating for a single point.
  ccd_vec3_t closest = eigen_to_ccd({0, 0, 0});

  EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest,
                              "ExtractFrom2SimplexDegenerate");
}

// Test extraction from a 3-simplex through the extractClosestPoints() method.
// Note: there is no support method for the 3-simplex like there is for the 1-
// and 2-simplices.
TEST_F(ExtractClosestPoint, ExtractFrom3Simplex) {
  // Compute a "closest point" based on arbitrary barycentric coordinates.
  const double alpha = 0.25;
  const double beta = 0.33;
  ASSERT_TRUE(alpha >= 0 && alpha <= 1 && beta >= 0 && beta <= 1 &&
              alpha + beta <= 1);

  const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v);
  const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v);
  const Vector3d m2 = ccd_to_eigen(simplex_.ps[2].v);

  // Interpolate three vertices via barycentric coordinates s1 and s2.
  auto interpolate = [](const Vector3d& a, const Vector3d& b, const Vector3d& c,
                        double s1, double s2) -> Vector3d {
    return a * s1 + b * s2 + c * (1 - s1 - s2);
  };

  ccd_vec3_t closest = eigen_to_ccd(interpolate(m0, m1, m2, alpha, beta));
  const Vector3d p0_expected = v0_;
  const Vector3d p1_expected = interpolate(t1_[0], t1_[1], t1_[2], alpha, beta);

  EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest,
                              "ExtractFrom3Simplex");
}

// Tests the case where the 3-simplex is degenerate -- the points are considered
// coincident.
TEST_F(ExtractClosestPoint, ExtractFrom3SimplesDegenerateCoincident) {
  // This test essentially reproduces the *valid* result from
  // ExtractFrom2Simplex(). The difference is that it *claims* to be a triangle.

  // NOTE: This exercises the knowledge that the coincidence tolerance is eps.
  const ccd_real_t eps = 0.5 * constants<ccd_real_t>::eps();

  // Copy the first support vertex into the second and third support vertices
  // and then perturb the copies a small amount. We add a small amount to the
  // x-components of the minkowski sum *and* the object1 vertex.
  for (auto i : {1, 2}) {
    ccdSupportCopy(&simplex_.ps[i], &simplex_.ps[0]);
    simplex_.ps[i].v.v[0] += eps;
    simplex_.ps[i].v1.v[0] += eps;
  }
  // Confirm that the input and expected answer (v0_) match.
  ASSERT_TRUE(are_same(v0_, simplex_.ps[0].v1, kTolerance));

  // The triangle has zero area because the vertices are all coincident;
  // the answer should be essentially the same as evaluating for a single point.
  ccd_vec3_t closest = eigen_to_ccd({0, 0, 0});

  EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest,
                              "ExtractFrom3SimplexDegenerateCoincident");
}

// Tests the case where the 3-simplex is degenerate -- the points are considered
// co-linear.
TEST_F(ExtractClosestPoint, ExtractFrom3SimplesDegenerateColinear) {
  // The query point we're going to use is a simple linear combination of the
  // v0 and v1 (from the minkowski sum). That means the points on the triangle
  // should likewise be a combination of v0 and v1 from each shape.
  const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v);
  const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v);

  const double s = 1 / 3.0;
  ASSERT_TRUE(s >= 0 && s <= 1);

  ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s));
  const Vector3d p0_expected = v0_;
  const Vector3d p1_expected = lerp(t1_[0], t1_[1], s);

  // Now set up co-linear configuration. Vertex 2 will simply be a linear
  // combination of vertex 0 and vertex 1.
  //   v2 = 2 * (v1 - v0) + v0 = 2 * v1 - v0.
  // Note: this puts v2 on the same line, but not inside the line segment
  // spanned by v0 and v1. Because the closest point lies on the segment, this
  // confirms that arbitrarily extending the degeneracy doesn't change the
  // answer.
  auto linearly_combine = [](const ccd_vec3_t& a, const ccd_vec3_t& b,
                             ccd_vec3_t* dst) {
    auto A = ccd_to_eigen(a);
    auto B = ccd_to_eigen(b);
    auto C = 2 * B - A;
    *dst = eigen_to_ccd(C);
  };
  linearly_combine(simplex_.ps[0].v, simplex_.ps[1].v, &simplex_.ps[2].v);
  linearly_combine(simplex_.ps[0].v1, simplex_.ps[1].v1, &simplex_.ps[2].v1);
  linearly_combine(simplex_.ps[0].v2, simplex_.ps[1].v2, &simplex_.ps[2].v2);

  EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest,
                              "ExtractFrom3SimplexDegenerateColinear");
}

}  // namespace detail
}  // namespace fcl

//==============================================================================
int main(int argc, char* argv[]) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}

ext: cpp

max_stars_repo_name: haraisao/fcl

max_issues_repo_name: sthagen/fcl

max_forks_repo_name: sthagen/fcl


text:
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <tuple>

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

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/sample_consensus/sac_model_normal_plane.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/mls.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/passthrough.h>
#include <vtkPlaneSource.h>
#include <vtkImplicitPlaneWidget.h>

#include "../../../core/format_helper.h"
#include "../../../core/math_helper.h"

void createPolyFromPlane(const pcl::ModelCoefficients::Ptr coefficients,
                         pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud)
{
  double a = coefficients->values[0];
  double b = coefficients->values[1];
  double c = coefficients->values[2];
  double d = coefficients->values[3];

  double x[4] = {-20, 20, 20, -20};
  double z[4] = {  0,  0, 100, 100};
  for(int i = 0; i < 4; i++) {
    double y = (a*x[i] + c*z[i] + d) / (-b);
    pcl::PointXYZ pt(x[i], y, z[i]);
    poly_cloud->points.push_back(pt);
  }
}

void triangulate(const double (&cam_params)[5], double x, double y, double disp, cv::Mat& pt3d) {
  double f = cam_params[0];
  double cx = cam_params[2];
  double cy = cam_params[3];
  double b = cam_params[4];

  // add - in front for x and y just to set the positive y to skies
  pt3d.at<double>(0) = -(x - cx) * b / disp;
  pt3d.at<double>(1) = -(y - cy) * b / disp;
  pt3d.at<double>(2) = f * b / disp;
  pt3d.at<double>(3) = 1.0;
}

void renderPointCloud(const std::vector<std::string>& imagelist, const std::vector<std::string>& disp_imglist,
                      const std::string& gt_file, const std::string& cparams_file,
                      const std::string& source_folder, const std::string& disp_folder, const std::string& output_folder)
{
  double cam_params[5];
  core::FormatHelper::readCameraParams(cparams_file, cam_params);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
  std::vector<std::tuple<int,int>> points_img;

  ifstream gt_fp(gt_file);
  cv::Mat Rt = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat pt3d = cv::Mat::zeros(4, 1, CV_64F);
  cv::Mat img_left;
  cv::Mat mat_disp;
  double min_disp = 30.0;
  int img_step = 1;
  //size_t start_frame = 100;
  //size_t start_frame = 315;
  //size_t start_frame = 27;
  // meadow on right
  size_t start_frame = 349;
  // car in front
  //size_t start_frame = 995;
  // traffic sign
  //size_t start_frame = 442;
  //size_t start_frame = 620;
  //for(size_t i = 0; i < end_frame; i++) {
  //for(size_t i = start_frame; i < end_frame; i++) {
  for(size_t i = start_frame; i < start_frame+1; i++) {
    //core::FormatHelper::readNextMatrixKitti(gt_fp, Rt);
    if(!(i % img_step == 0) || i < start_frame) continue;
    std::cout << "----------------\n" << source_folder + imagelist[2*i] << "\n";
    img_left = cv::imread(source_folder + imagelist[2*i], CV_LOAD_IMAGE_GRAYSCALE);
    //string fname = output_folder + imagelist[i].substr(9, imagelist[i].size()-13) + "_disp.png";
    //std::string fname = output_folder + disp_imglist[i];
    //img_disp = imread(disp_folder + disp_imglist[i], CV_LOAD_IMAGE_GRAYSCALE);
    std::string mat_fname = disp_folder + "mat/" + disp_imglist[i].substr(0,11) + ".yml";
    std::cout << "reading: " << mat_fname << "\n";
    cv::FileStorage mat_file(mat_fname, cv::FileStorage::READ);
    mat_file["disparity_matrix"] >> mat_disp;
    for(int y = 0; y < mat_disp.rows; y++) {
      for(int x = 0; x < mat_disp.cols; x++) {
        float disp = mat_disp.at<float>(y,x);
        if(disp > min_disp) {
          triangulate(cam_params, x, y, disp, pt3d);
          //pt3d = Rt * pt3d;
          //std::cout << pt3d << "\n\n";
          pcl::PointXYZ pt;
          pt.x = pt3d.at<double>(0);
          pt.y = pt3d.at<double>(1);
          pt.z = pt3d.at<double>(2);
          point_cloud->points.push_back(pt);
          pcl::PointXYZRGB point;
          point.x = pt3d.at<double>(0);
          point.y = pt3d.at<double>(1);
          point.z = pt3d.at<double>(2);
          point.r = img_left.at<uint8_t>(y,x);
          point.g = img_left.at<uint8_t>(y,x);
          point.b = img_left.at<uint8_t>(y,x);
          point_cloud_rgb->points.push_back(point);
          points_img.push_back(std::make_tuple(y,x));
        }
      }
    }
    //imshow("disparity", img_disp);
    //waitKey(0);
  }
  std::cout << "Point cloud size: " << point_cloud->points.size() << "\n";
  point_cloud->width = point_cloud->points.size();
  point_cloud->height = 1;

  // PCL MLS
  // Create a KD-Tree
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  // Output has the PointNormal type in order to store the normals calculated by MLS
  pcl::PointCloud<pcl::PointNormal> mls_points;

  // Init object (second point type is for the normals, even if unused)
  pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;

  mls.setComputeNormals (true);
  // Set parameters
  mls.setInputCloud (point_cloud);
  mls.setPolynomialFit (true);
  mls.setSearchMethod (tree);
  //mls.setSearchRadius (0.03);
  mls.setSearchRadius (0.1);

  // Reconstruct
  mls.process (mls_points);
  pcl::io::savePCDFile ("pc-mls.pcd", mls_points);
}

int main(int argc, char** argv)
{
  std::string config_file;
  std::string imagelistfn, disp_imagelistfn;
  std::string cam_params_file;
  std::string source_folder;
  std::string disp_folder, gt_file;
  std::string output_folder;

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

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

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

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

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

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

  std::vector<std::string> imagelist, disp_imagelist;
  bool ok = core::FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
  {
    std::cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
    return -1;
  }
  ok = core::FormatHelper::readStringList(disp_imagelistfn, disp_imagelist);

  renderPointCloud(imagelist, disp_imagelist, gt_file, cam_params_file, source_folder, disp_folder, output_folder);

  return 0;
}

ext: cc


text:
// The Open Trading Project - open-trading.org
//
// Copyright (c) 2011 Martin Tapia - martin.tapia@open-trading.org
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//    * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef __BACKTESTER_BACKTESTER__
#define __BACKTESTER_BACKTESTER__

#include <boost/noncopyable.hpp>
#include <thread>
#include "tools/Timer.hpp"

namespace Core
{
    class History;
}

namespace Backtester
{
    class Logger;
    class Conf;
    class StratParamsMap;
    class Report;
    class ReportManager;
    class ParamsGenerator;

    class Backtester :
        private boost::noncopyable
    {
        public:
            explicit Backtester(Logger const& logger, Conf& conf, Core::History& history);
            ~Backtester();
            void Run();
            bool GetNewParamsFromThread(StratParamsMap& params);
            void SubmitReportFromThread(Report const& report);
        private:
            ParamsGenerator* _ParamsGeneratorFactory(std::string const& name) const;
            Logger const& _logger;
            Conf& _conf;
            Core::History& _history;
            std::mutex _mutex;
            ReportManager* _reportManager;
            ParamsGenerator* _paramsGenerator;
            unsigned int _nbFinishedTasks;
            Tools::Timer _timer;
    };
}

#endif

max_stars_repo_name: paps/Open-Trading

max_issues_repo_name: paps/Open-Trading

max_forks_repo_name: paps/Open-Trading


text:
/*****************************************************************************
 * Licensed to Qualys, Inc. (QUALYS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * QUALYS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ****************************************************************************/

/**
 * @file
 * @brief IronBee --- CLIPP Raw Consumer Implementation
 *
 * @author Christopher Alfeld <calfeld@qualys.com>
 */

#include "ironbee_config_auto.h"

#include "raw_consumer.hpp"

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

#include <fstream>
#include <map>
#include <stdexcept>

using namespace std;

namespace IronBee {
namespace CLIPP {

namespace  {

class RawConsumerDelegate :
    public Input::Delegate
{
public:
    explicit
    RawConsumerDelegate(const string& output_path) :
        m_next_event_id(1),
        m_output_path(output_path),
        m_info(NULL)
    {
        // nop
    }

    void request_started(const Input::RequestEvent& event)
    {
        ofstream& out = output(REQUEST);

        if (event.raw.length > 0) {
            out << event.raw << endl;
        }
        else {
            out << event.method << " "
                << event.uri << " "
                << event.protocol << endl;
        }
    }

    void request_header(const Input::HeaderEvent& event)
    {
        ofstream& out = output(REQUEST);

        BOOST_FOREACH(const Input::header_t& header, event.headers)
        {
            out << header.first << ": " << header.second << endl;
        }
    }

    void request_header_finished(const Input::NullEvent& event)
    {
        output(REQUEST) << endl;
    }

    void request_body(const Input::DataEvent& event)
    {
        output(REQUEST) << event.data;
    }

    void request_finished(const Input::NullEvent&)
    {
        output(REQUEST).close();
    }

    void response_started(const Input::ResponseEvent& event)
    {
        ofstream& out = output(RESPONSE);

        if (event.raw.length > 0) {
            out << event.raw << endl;
        }
        else {
            out
                << event.protocol << " "
                << event.status << " "
                << event.message << endl;
        }
    }

    void response_header(const Input::HeaderEvent& event)
    {
        ofstream& out = output(RESPONSE);

        BOOST_FOREACH(const Input::header_t& header, event.headers)
        {
            out << header.first << ": " << header.second << endl;
        }
    }

    void response_header_finished(const Input::NullEvent& event)
    {
        output(RESPONSE) << endl;
    }

    void response_body(const Input::DataEvent& event)
    {
        output(RESPONSE) << event.data;
    }

    void response_finished(const Input::NullEvent&)
    {
        output(RESPONSE).close();
    }

    void begin_input(const string& id)
    {
        event_map_t::iterator i = m_event_infos.find(id);
        if (i == m_event_infos.end()) {
            i = m_event_infos.insert(
                i,
                make_pair(id, event_info_t(m_next_event_id))
            );
            ++m_next_event_id;
        }
        m_info = &i->second;
    }

private:
    enum event_info_e {
        NONE,
        REQUEST,
        RESPONSE
    };
    string output_path(size_t event_id, size_t id, event_info_e which)
    {
        static const string event_info_e_label[] = {
            "none",
            "request",
            "response"
        };

        return m_output_path + "/" +
            boost::lexical_cast<string>(event_id) + "." +
			event_info_e_label[which] + "." +
            boost::lexical_cast<string>(id) + ".raw";
    }

    struct event_info_t
    {
        event_info_t(size_t event_id_) :
            which(NONE), event_id(event_id_), next_id(1)
        {
            // nop
        }

        event_info_e which;
        size_t event_id;
        size_t next_id;
        boost::shared_ptr<ofstream> file;
    };

    ofstream& output(event_info_e which)
    {
        if (m_info->which != which) {
			string path =
                output_path(m_info->event_id, m_info->next_id, which);
            m_info->file.reset(new ofstream(path.c_str()));
            ++m_info->next_id;
            m_info->which = which;

			if (! *(m_info->file)) {
				throw runtime_error("Error opening file: " + path);
			}
        }

        return *(m_info->file);
    }

    size_t m_next_event_id;
    string m_output_path;
    event_info_t* m_info;
    typedef map<string, event_info_t> event_map_t;
    event_map_t m_event_infos;
};

}

struct RawConsumer::State
{
    State(const string& path) :
        delegate(path)
    {
        boost::filesystem::create_directories(path);
    }

    RawConsumerDelegate delegate;
};

RawConsumer::RawConsumer()
{
    // nop
}

RawConsumer::RawConsumer(const string& output_path) :
    m_state(boost::make_shared<State>(output_path))
{
  // nop
}

bool RawConsumer::operator()(const Input::input_p& input)
{
    assert(m_state);

    m_state->delegate.begin_input(input->id);
    input->connection.dispatch(m_state->delegate);

    return true;
}

} // CLIPP
} // IronBee

max_stars_repo_name: crustymonkey/ironbee

max_issues_repo_name: crustymonkey/ironbee

max_forks_repo_name: crustymonkey/ironbee


text:
#include <pcl/point_types.h>
#include <pcl/features/boundary.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/extract_indices.h>

#include <Eigen/Dense>

#include "minipcl.h"

// set ksearch and radius to < 0 to disable
void mpcl_compute_normals(pcl::PointCloud<pcl::PointXYZRGB> &cloud,
                          int ksearch,
                          double searchRadius,
                          pcl::PointCloud<pcl::Normal> &out)
{
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;

    ne.setSearchMethod (tree);
    ne.setInputCloud (cloud.makeShared());
    if (ksearch >= 0)
        ne.setKSearch (ksearch);
    if (searchRadius >= 0.0)
        ne.setRadiusSearch (searchRadius);
    ne.compute (out);
}

// Boundary estimation (uses normal estimation, hence the parameters).
void mpcl_estimate_boundaries(pcl::PointCloud<pcl::PointXYZRGB> &cloud,
                              float angle_threshold,
                              double search_radius,
                              int normal_ksearch,
                              double normal_search_radius,
                              char *out, size_t n)
{
    pcl::PointCloud<pcl::Normal> normals;
    mpcl_compute_normals(cloud, normal_ksearch, normal_search_radius, normals);
    pcl::PointCloud<pcl::Boundary> boundaries;
    pcl::BoundaryEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::Boundary> est;
    est.setInputCloud(cloud.makeShared());
    est.setInputNormals(normals.makeShared());

    if (search_radius >= 0) {
        est.setRadiusSearch(search_radius);
    }

    est.setSearchMethod(typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr(
                            new pcl::search::KdTree<pcl::PointXYZRGB>));
    est.compute(boundaries);

    for (size_t i = 0; i < n; i++) {
        out[i] = boundaries[i].boundary_point;
    }
}

void mpcl_sacnormal_set_axis(pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> &sac,
                             double ax, double ay, double az)
{
    Eigen::Vector3f vect(ax,ay,az);
    sac.setAxis(vect);
}

void mpcl_extract(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &incloud,
                  pcl::PointCloud<pcl::PointXYZRGB> *outcloud,
                  pcl::PointIndices *indices,
                  bool negative)
{
    pcl::PointIndices::Ptr indicesptr (indices);
    pcl::ExtractIndices<pcl::PointXYZRGB> ext;
    ext.setInputCloud(incloud);
    ext.setIndices(indicesptr);
    ext.setNegative(negative);
    ext.filter(*outcloud);
}

max_stars_repo_name: NLeSC/python-pcl

max_issues_repo_name: NLeSC/python-pcl

max_forks_repo_name: NLeSC/python-pcl


text:
/**
 * @file lsh_test.cpp
 *
 * Unit tests for the 'LSHSearch' class.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

#include <mlpack/methods/lsh/lsh_search.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>

using namespace std;
using namespace mlpack;
using namespace mlpack::neighbor;

/**
 * Generates a point set of four clusters:
 * -C1 around (0.5, 3.5),
 * -C2 around (3.5, 3.5),
 * -C3 around (0.5, 0.5),
 * -C4 around (3.5, 3.5).
 *
 * It then merges these clusters into one set, rdata.
 */
void GetPointset(const size_t N, arma::mat& rdata)
{
  const size_t d = 2;
  // Create four clusters of points.
  arma::mat c1(d, N / 4, arma::fill::randu);
  arma::mat c2(d, N / 4, arma::fill::randu);
  arma::mat c3(d, N / 4, arma::fill::randu);
  arma::mat c4(d, N / 4, arma::fill::randu);

  arma::colvec offset1;
  offset1 << 0 << arma::endr
          << 3 << arma::endr;

  arma::colvec offset2;
  offset2 << 3 << arma::endr
          << 3 << arma::endr;

  arma::colvec offset4;
  offset4 << 3 << arma::endr
          << 0 << arma::endr;

  // Spread points in plane.
  for (size_t p = 0; p < N / 4; ++p)
  {
    c1.col(p) += offset1;
    c2.col(p) += offset2;
    c4.col(p) += offset4;
  }

  rdata.set_size(d, N);
  rdata.cols(0, (N / 4) - 1) = c1;
  rdata.cols(N / 4, (N / 2) - 1) = c2;
  rdata.cols(N / 2, (3 * N / 4) - 1) = c3;
  rdata.cols(3 * N / 4, N - 1) = c4;
}

/**
 * Generates two queries, one around (0.5, 0.5) and one around (3.5, 3.5).
 */
void GetQueries(arma::mat& qdata)
{
  const size_t d = 2;
  // Generate two queries inside two of the clusters.

  // Put query 1 into cluster 3.
  arma::colvec q1, q2;
  q1.randu(d, 1);

  // Offset second query to go into cluster 2.
  q2.randu(d, 1);
  q2.row(0) += 3;
  q2.row(1) += 3;

  qdata.set_size(d, 2);
  qdata.col(0) = q1;
  qdata.col(1) = q2;
}

BOOST_AUTO_TEST_SUITE(LSHTest);

/**
 * Test: Run LSH with varying number of tables, keeping all other parameters
 * constant. Compute the recall, i.e. the number of reported neighbors that
 * are real neighbors of the query.
 * LSH's property is that (with high probability), increasing the number of
 * tables will increase recall. Epsilon ensures that if noise lightly affects
 * the projections, the test will not fail.
 * This produces false negatives, so we attempt the test numTries times and
 * only declare failure if all of them fail.
 */
BOOST_AUTO_TEST_CASE(NumTablesTest)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int numProj = 10;
  const double hashWidth = 0;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // Test parameters.
  const double epsilon = 0.1; // Allowed deviation from expected monotonicity.
  const int numTries = 5; // Tries for each test before declaring failure.

  // Read iris training and testing data as reference and query sets.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference data.
  KNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);

  bool fail;
  for (int t = 0; t < numTries; ++t)
  {
    fail = false;

    const int lSize = 6; // Number of runs.
    const int lValue[] = {1, 8, 16, 32, 64, 128}; // Number of tables.
    double lValueRecall[lSize] = {0.0}; // Recall of each LSH run.

    for (size_t l = 0; l < lSize; ++l)
    {
      // Run LSH with only numTables varying (other values are defaults).
      LSHSearch<> lshTest(rdata, numProj, lValue[l], hashWidth, secondHashSize,
          bucketSize);
      arma::Mat<size_t> lshNeighbors;
      arma::mat lshDistances;
      lshTest.Search(qdata, k, lshNeighbors, lshDistances);

      // Compute recall for each query.
      lValueRecall[l] = LSHSearch<>::ComputeRecall(lshNeighbors, groundTruth);

      if (l > 0)
      {
        if (lValueRecall[l] < lValueRecall[l - 1] - epsilon)
        {
          fail = true; // If test fails at one point, stop and retry.
          break;
        }
      }
    }

    if (!fail)
      break; // If test passes one time, it is sufficient.
  }

  BOOST_REQUIRE(fail == false);
}

/**
 * Test: Run LSH with varying hash width, keeping all other parameters
 * constant. Compute the recall, i.e. the number of reported neighbors that
 * are real neighbors of the query.
 * LSH's property is that (with high probability), increasing the hash width
 * will increase recall. Epsilon ensures that if noise lightly affects the
 * projections, the test will not fail.
 */
BOOST_AUTO_TEST_CASE(HashWidthTest)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int numTables = 30;
  const int numProj = 10;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // Test parameters.
  const double epsilon = 0.1; // Allowed deviation from expected monotonicity.

  // Read iris training and testing data as reference and query.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference data.
  KNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);
  const int hSize = 7; // Number of runs.
  const double hValue[] = {0.1, 0.5, 1, 5, 10, 50, 500}; // Hash width.
  double hValueRecall[hSize] = {0.0}; // Recall of each run.

  for (size_t h = 0; h < hSize; ++h)
  {
    // Run LSH with only hashWidth varying (other values are defaults).
    LSHSearch<> lshTest(
        rdata,
        numProj,
        numTables,
        hValue[h],
        secondHashSize,
        bucketSize);

    arma::Mat<size_t> lshNeighbors;
    arma::mat lshDistances;
    lshTest.Search(qdata, k, lshNeighbors, lshDistances);

    // Compute recall for each query.
    hValueRecall[h] = LSHSearch<>::ComputeRecall(lshNeighbors, groundTruth);

    if (h > 0)
      BOOST_REQUIRE_GE(hValueRecall[h], hValueRecall[h - 1] - epsilon);
  }
}

/**
 * Test: Run LSH with varying number of projections, keeping other parameters
 * constant. Compute the recall, i.e. the number of reported neighbors that
 * are real neighbors of the query.
 * LSH's property is that (with high probability), increasing the number of
 * projections per table will decrease recall. Epsilon ensures that if noise
 * lightly affects the projections, the test will not fail.
 */
BOOST_AUTO_TEST_CASE(NumProjTest)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int numTables = 30;
  const double hashWidth = 0;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // Test parameters.
  const double epsilon = 0.1; // Allowed deviation from expected monotonicity.

  // Read iris training and testing data as reference and query sets.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference data.
  KNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);

  // LSH test parameters for numProj.
  const int pSize = 5; // Number of runs.
  const int pValue[] = {1, 10, 20, 50, 100}; // Number of projections.
  double pValueRecall[pSize] = {0.0}; // Recall of each run.

  for (size_t p = 0; p < pSize; ++p)
  {
    // Run LSH with only numProj varying (other values are defaults).
    LSHSearch<> lshTest(
        rdata,
        pValue[p],
        numTables,
        hashWidth,
        secondHashSize,
        bucketSize);

    arma::Mat<size_t> lshNeighbors;
    arma::mat lshDistances;
    lshTest.Search(qdata, k, lshNeighbors, lshDistances);

    // Compute recall for each query.
    pValueRecall[p] = LSHSearch<>::ComputeRecall(lshNeighbors, groundTruth);

    // Don't check the first run; only check that increasing P decreases recall.
    if (p > 0)
      BOOST_REQUIRE_LE(pValueRecall[p] - epsilon, pValueRecall[p - 1]);
  }
}

/**
 * Test: Run two LSH searches:
 * First, a very expensive LSH search, with a large number of hash tables
 * and a large hash width. This run should return an acceptable recall. We set
 * the bar very low (recall >= 50%) to make sure that a test fail means bad
 * implementation.
 * Second, a very cheap LSH search, with parameters that should cause recall
 * to be very low. Set the threshhold very high (recall <= 25%) to make sure
 * that a test fail means bad implementation.
 */
BOOST_AUTO_TEST_CASE(RecallTest)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // Read iris training and testing data as reference and query sets.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference data.
  KNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);

  // Expensive LSH run.
  const int hExp = 10000; // First-level hash width.
  const int kExp = 1; // Projections per table.
  const int tExp = 128; // Number of tables.
  const double recallThreshExp = 0.5;

  LSHSearch<> lshTestExp(
      rdata,
      kExp,
      tExp,
      hExp,
      secondHashSize,
      bucketSize);
  arma::Mat<size_t> lshNeighborsExp;
  arma::mat lshDistancesExp;
  lshTestExp.Search(qdata, k, lshNeighborsExp, lshDistancesExp);

  const double recallExp = LSHSearch<>::ComputeRecall(lshNeighborsExp, groundTruth);

  // This run should have recall higher than the threshold.
  BOOST_REQUIRE_GE(recallExp, recallThreshExp);

  // Cheap LSH run.
  const int hChp = 1; // Small first-level hash width.
  const int kChp = 100; // Large number of projections per table.
  const int tChp = 1; // Only one table.
  const double recallThreshChp = 0.25; // Recall threshold.

  LSHSearch<> lshTestChp(
      rdata,
      kChp,
      tChp,
      hChp,
      secondHashSize,
      bucketSize);
  arma::Mat<size_t> lshNeighborsChp;
  arma::mat lshDistancesChp;
  lshTestChp.Search(qdata, k, lshNeighborsChp, lshDistancesChp);

  const double recallChp = LSHSearch<>::ComputeRecall(lshNeighborsChp,
      groundTruth);

  // This run should have recall lower than the threshold.
  BOOST_REQUIRE_LE(recallChp, recallThreshChp);
}

/**
 * Test: This is a deterministic test that projects 2-dpoints to a known line
 * (axis 2). The reference set contains 4 well-separated clusters that will
 * merge into 2 clusters when projected on that axis.
 *
 * We create two queries, each one belonging in one cluster (q1 in cluster 3
 * located around (0, 0) and q2 in cluster 2 located around (3, 3). After the
 * projection, q1 should have neighbors in C3 and C4 and q2 in C1 and C2.
 */
BOOST_AUTO_TEST_CASE(DeterministicMerge)
{
  const size_t N = 40; // Must be divisible by 4 to create 4 clusters properly.
  arma::mat rdata;
  arma::mat qdata;
  GetPointset(N, rdata);
  GetQueries(qdata);

  const int k = N / 2;
  const double hashWidth = 1;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // 1 table, with one projection to axis 1.
  arma::cube projections(2, 1, 1);
  projections(0, 0, 0) = 0;
  projections(1, 0, 0) = 1;

  LSHSearch<> lshTest(rdata, projections, hashWidth, secondHashSize,
      bucketSize);

  arma::Mat<size_t> neighbors;
  arma::mat distances;
  lshTest.Search(qdata, k, neighbors, distances);

  // Test query 1.
  size_t q;
  for (size_t j = 0; j < k; ++j) // For each neighbor.
  {
    // If the neighbor is not found, ignore the point.
    if (neighbors(j, 0) == N || neighbors(j, 1) == N)
      continue;

    // Query 1 is in cluster 3, which under this projection was merged with
    // cluster 4. Clusters 3 and 4 have points 20:39, so only neighbors among
    //those should be found.
    q = 0;
    BOOST_REQUIRE_GE(neighbors(j, q), N / 2);

    // Query 2 is in cluster 2, which under this projection was merged with
    // cluster 1. Clusters 1 and 2 have points 0:19, so only neighbors among
    // those should be found.
    q = 1;
    BOOST_REQUIRE_LT(neighbors(j, q), N / 2);
  }
}

/**
 * Test: This is a deterministic test that projects 2-d points to the plane.
 * The reference set contains 4 well-separated clusters that should not merge.
 *
 * We create two queries, each one belonging in one cluster (q1 in cluster 3
 * located around (0, 0) and q2 in cluster 2 located around (3, 3). The test is
 * a success if, after the projection, q1 should have neighbors in c3 and q2
 * in c2.
 */
BOOST_AUTO_TEST_CASE(DeterministicNoMerge)
{
  const size_t N = 40;
  arma::mat rdata;
  arma::mat qdata;
  GetPointset(N, rdata);
  GetQueries(qdata);

  const int k = N / 2;
  const double hashWidth = 1;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // 1 table, with one projection to axis 1.
  arma::cube projections(2, 2, 1);
  projections(0, 0, 0) = 0;
  projections(1, 0, 0) = 1;
  projections(0, 1, 0) = 1;
  projections(1, 1, 0) = 0;

  LSHSearch<> lshTest(rdata, projections, hashWidth, secondHashSize,
      bucketSize);

  arma::Mat<size_t> neighbors;
  arma::mat distances;
  lshTest.Search(qdata, k, neighbors, distances);

  // Test query 1.
  size_t q;
  for (size_t j = 0; j < k; ++j) // For each neighbor.
  {
    // If the neighbor is not found, ignore the point.
    if (neighbors(j, 0) == N || neighbors(j, 1) == N)
      continue;

    // Query 1 is in cluster 3, which is points 20:29.
    q = 0;
    BOOST_REQUIRE_LT(neighbors(j, q), 3 * N / 4);
    BOOST_REQUIRE_GE(neighbors(j, q), N / 2);

    // Query 2 is in cluster 2, which is points 10:19.
    q = 1;
    BOOST_REQUIRE_LT(neighbors(j, q), N / 2);
    BOOST_REQUIRE_GE(neighbors(j, q), N / 4);
  }
}

/**
 * Test: Create an LSHSearch object and use an increasing number of probes to
 * search for points. Require that recall for the same object doesn't decrease
 * with increasing number of probes. Also require that at least a few times
 * there's some increase in recall.
 */
BOOST_AUTO_TEST_CASE(MultiprobeTest)
{
  // Test parameters.
  const double epsilonIncrease = 0.05;
  const size_t repetitions = 5; // Train five objects.

  const size_t probeTrials = 5;
  const size_t numProbes[probeTrials] = {0, 1, 2, 3, 4};

  // Algorithm parameters.
  const int k = 4;
  const int numTables = 16;
  const int numProj = 3;
  const double hashWidth = 0;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference set.
  KNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);

  bool foundIncrease = 0;

  for (size_t rep = 0; rep < repetitions; ++rep)
  {
    // Train a model.
    LSHSearch<> multiprobeTest(rdata, numProj, numTables, hashWidth,
        secondHashSize, bucketSize);

    double prevRecall = 0;
    // Search with varying number of probes.
    for (size_t p = 0; p < probeTrials; ++p)
    {
      arma::Mat<size_t> lshNeighbors;
      arma::mat lshDistances;

      multiprobeTest.Search(qdata, k, lshNeighbors, lshDistances, 0,
          numProbes[p]);

      // Compute recall of this run.
      double recall = LSHSearch<>::ComputeRecall(lshNeighbors, groundTruth);
      if (p > 0)
      {
        // More probes should at the very least not lower recall...
        BOOST_REQUIRE_GE(recall, prevRecall);

        // ... and should ideally increase it a bit.
        if (recall > prevRecall + epsilonIncrease)
          foundIncrease = true;
        prevRecall = recall;
      }
    }
  }
  BOOST_REQUIRE(foundIncrease);
}

/**
 * Test: This is a deterministic test that verifies multiprobe LSH works
 * correctly. To do this, we generate two queries, q1 and q2. q1 is hashed
 * directly under cluster C2, q2 is hashed in C2's center.
 * We verify that:
 * 1) q1 should have no neighbors without multiprobe.
 * 2) q1 should have neighbors only from C2 with 1 additional probe.
 * 3) q2 should have all neighbors found with 3 additional probes.
 */
BOOST_AUTO_TEST_CASE(MultiprobeDeterministicTest)
{
  // Generate known deterministic clusters of points.
  const size_t N = 40;
  arma::mat rdata;
  GetPointset(N, rdata);

  const int k = N / 4;
  const double hashWidth = 1;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // 1 table, projections on orthonormal plane.
  arma::cube projections(2, 2, 1);
  projections(0, 0, 0) = 1;
  projections(1, 0, 0) = 0;
  projections(0, 1, 0) = 0;
  projections(1, 1, 0) = 1;

  // Construct LSH object with given tables.
  LSHSearch<> lshTest(rdata, projections,
                      hashWidth, secondHashSize, bucketSize);

  const arma::mat offsets = lshTest.Offsets();

  // Construct q1 so it is hashed directly under C2.
  arma::mat q1;
  q1 << 3.9 << arma::endr << 2.99;
  q1 -= offsets;

  // Construct q2 so it is hashed near the center of C2.
  arma::mat q2;
  q2 << 3.6 << arma::endr << 3.6;
  q2 -= offsets;

  arma::Mat<size_t> neighbors;
  arma::mat distances;

  // Test that q1 simple search comes up empty.
  lshTest.Search(q1, k, neighbors, distances);
  BOOST_REQUIRE(arma::all(neighbors.col(0) == N));

  // Test that q1 search with 1 additional probe returns some C2 points.
  lshTest.Search(q1, k, neighbors, distances, 0, 1);
  BOOST_REQUIRE(arma::all(
        neighbors.col(0) == N ||
        (neighbors.col(0) >= N / 4 && neighbors.col(0) < N / 2)));

  // Test that q2 simple search returns some C2 points.
  lshTest.Search(q2, k, neighbors, distances);
  BOOST_REQUIRE(arma::all(
      neighbors.col(0) == N ||
      (neighbors.col(0) >= N / 4 && neighbors.col(0) < N / 2)));

  // Test that q2 with 3 additional probes returns all C2 points.
  lshTest.Search(q2, k, neighbors, distances, 0, 3);
  BOOST_REQUIRE(arma::all(
      neighbors.col(0) >= N / 4 && neighbors.col(0) < N / 2));
}

BOOST_AUTO_TEST_CASE(LSHTrainTest)
{
  // This is a not very good test that simply checks that the re-trained LSH
  // model operates on the correct dimensionality and returns the correct number
  // of results.
  arma::mat referenceData = arma::randu<arma::mat>(3, 100);
  arma::mat newReferenceData = arma::randu<arma::mat>(10, 400);
  arma::mat queryData = arma::randu<arma::mat>(10, 200);

  LSHSearch<> lsh(referenceData, 3, 2, 2.0, 11, 3);

  lsh.Train(newReferenceData, 4, 3, 3.0, 12, 4);

  arma::Mat<size_t> neighbors;
  arma::mat distances;

  lsh.Search(queryData, 3, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 200);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 3);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 200);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 3);
}

/**
 * Test: this verifies ComputeRecall works correctly by providing two identical
 * vectors and requiring that Recall is equal to 1.
 */
BOOST_AUTO_TEST_CASE(RecallTestIdentical)
{
  const size_t k = 5; // 5 nearest neighbors
  const size_t numQueries = 1;

  // base = [1; 2; 3; 4; 5]
  arma::Mat<size_t> base;
  base.set_size(k, numQueries);
  base.col(0) = arma::linspace< arma::Col<size_t> >(1, k, k);

  // q1 = [1; 2; 3; 4; 5]. Expect recall = 1
  arma::Mat<size_t> q1;
  q1.set_size(k, numQueries);
  q1.col(0) = arma::linspace< arma::Col<size_t> >(1, k, k);

  BOOST_REQUIRE_EQUAL(LSHSearch<>::ComputeRecall(base, q1), 1);
}

/**
 * Test: this verifies ComputeRecall returns correct values for partially
 * correct found neighbors. This is important because this is a good example of
 * how the recall and accuracy metrics differ - accuracy in this case would be
 * 0, recall should not be
 */
BOOST_AUTO_TEST_CASE(RecallTestPartiallyCorrect)
{
  const size_t k = 5; // 5 nearest neighbors
  const size_t numQueries = 1;

  // base = [1; 2; 3; 4; 5]
  arma::Mat<size_t> base;
  base.set_size(k, numQueries);
  base.col(0) = arma::linspace< arma::Col<size_t> >(1, k, k);

  // q2 = [2; 3; 4; 6; 7]. Expect recall = 0.6. This is important because this
  // is a good example of how recall and accuracy differ. Accuracy here would
  // be 0 but recall should not be.
  arma::Mat<size_t> q2;
  q2.set_size(k, numQueries);
  q2 <<
    2 << arma::endr <<
    3 << arma::endr <<
    4 << arma::endr <<
    6 << arma::endr <<
    7 << arma::endr;

  BOOST_REQUIRE_CLOSE(LSHSearch<>::ComputeRecall(base, q2), 0.6, 0.0001);
}

/**
 * Test: If given a completely wrong vector, ComputeRecall should return 0
 */
BOOST_AUTO_TEST_CASE(RecallTestIncorrect)
{
  const size_t k = 5; // 5 nearest neighbors
  const size_t numQueries = 1;

  // base = [1; 2; 3; 4; 5]
  arma::Mat<size_t> base;
  base.set_size(k, numQueries);
  base.col(0) = arma::linspace< arma::Col<size_t> >(1, k, k);
  // q3 = [6; 7; 8; 9; 10]. Expected recall = 0
  arma::Mat<size_t> q3;
  q3.set_size(k, numQueries);
  q3.col(0) = arma::linspace< arma::Col<size_t> >(k + 1, 2 * k, k);

  BOOST_REQUIRE_EQUAL(LSHSearch<>::ComputeRecall(base, q3), 0);
}

/**
 * Test: If given a vector of wrong shape, ComputeRecall should throw an
 * exception.
 */
BOOST_AUTO_TEST_CASE(RecallTestException)
{
  const size_t k = 5; // 5 nearest neighbors
  const size_t numQueries = 1;

  // base = [1; 2; 3; 4; 5]
  arma::Mat<size_t> base;
  base.set_size(k, numQueries);
  base.col(0) = arma::linspace< arma::Col<size_t> >(1, k, k);
  // verify that nonsense arguments throw exception
  arma::Mat<size_t> q4;
  q4.set_size(2 * k, numQueries);

  BOOST_REQUIRE_THROW(LSHSearch<>::ComputeRecall(base, q4),
      std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(EmptyConstructorTest)
{
  // If we create an empty LSH model and then call Search(), it should throw an
  // exception.
  LSHSearch<> lsh;

  arma::mat dataset = arma::randu<arma::mat>(5, 50);
  arma::mat distances;
  arma::Mat<size_t> neighbors;
  BOOST_REQUIRE_THROW(lsh.Search(dataset, 2, neighbors, distances),
      std::invalid_argument);

  // Now, train.
  lsh.Train(dataset, 4, 3, 3.0, 12, 4);

  lsh.Search(dataset, 3, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 50);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 3);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 50);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 3);
}

// These two tests are only compiled if the user has specified OpenMP to be
// used.
#ifdef HAS_OPENMP
/**
 * Test: This test verifies that parallel query processing returns correct
 * results for the bichromatic search.
 */
BOOST_AUTO_TEST_CASE(ParallelBichromatic)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int numTables = 16;
  const int numProj = 3;

  // Read iris training and testing data as reference and query sets.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Where to store neighbors and distances
  arma::Mat<size_t> sequentialNeighbors;
  arma::Mat<size_t> parallelNeighbors;
  arma::mat distances;

  // Construct an LSH object. By default, it uses the maximum number of threads
  LSHSearch<> lshTest(rdata, numProj, numTables); //default parameters
  lshTest.Search(qdata, k, parallelNeighbors, distances);

  // Now perform same search but with 1 thread
  size_t prevNumThreads = omp_get_max_threads(); // Store number of threads used.
  omp_set_num_threads(1);
  lshTest.Search(qdata, k, sequentialNeighbors, distances);
  omp_set_num_threads(prevNumThreads);

  // Require both have same results
  double recall = LSHSearch<>::ComputeRecall(sequentialNeighbors, parallelNeighbors);
  BOOST_REQUIRE_EQUAL(recall, 1);
}

/**
 * Test: This test verifies that parallel query processing returns correct
 * results for the monochromatic search.
 */
BOOST_AUTO_TEST_CASE(ParallelMonochromatic)
{
  // kNN and LSH parameters.
  const int k = 4;
  const int numTables = 16;
  const int numProj = 3;

  // Read iris training data as reference and query set.
  const string trainSet = "iris_train.csv";
  arma::mat rdata;
  data::Load(trainSet, rdata, true);

  // Where to store neighbors and distances
  arma::Mat<size_t> sequentialNeighbors;
  arma::Mat<size_t> parallelNeighbors;
  arma::mat distances;

  // Construct an LSH object, using maximum number of available threads.
  LSHSearch<> lshTest(rdata, numProj, numTables);
  lshTest.Search(k, parallelNeighbors, distances);

  // Now perform same search but with 1 thread.
  size_t prevNumThreads = omp_get_max_threads(); // Store number of threads used.
  omp_set_num_threads(1);
  lshTest.Search(k, sequentialNeighbors, distances);
  omp_set_num_threads(prevNumThreads);

  // Require both have same results.
  double recall = LSHSearch<>::ComputeRecall(sequentialNeighbors, parallelNeighbors);
  BOOST_REQUIRE_EQUAL(recall, 1);
}
#endif

BOOST_AUTO_TEST_SUITE_END();


text:
//
// Copyright (c) 2016-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/CPPAlliance/url
//

#ifndef BOOST_URL_GRAMMAR_IMPL_RANGE_HPP
#define BOOST_URL_GRAMMAR_IMPL_RANGE_HPP

#include <boost/url/detail/except.hpp>
#include <boost/url/grammar/error.hpp>
#include <boost/assert.hpp>
#include <iterator>

namespace boost {
namespace urls {
namespace grammar {

template<class T>
void
range_::
parse_impl(
    char const*& it,
    char const* end,
    error_code& ec,
    range_& t)
{
    typename T::value_type t0;
    auto start = it;
    std::size_t n = 0;
    T::begin(it, end, ec, t0);
    if(ec.failed())
    {
        if(ec == error::end)
        {
            ec = {};
            goto finish;
        }
        return;
    }
    for(;;)
    {
        ++n;
        T::increment(
            it, end, ec, t0);
        if(ec.failed())
        {
            if(ec == error::end)
            {
                ec = {};
                break;
            }
            return;
        }
    }
finish:
    t.str = string_view(
        start, it - start);
    t.count = n;
}

template<class T>
range_::
range_(T const*) noexcept
    : fp_(&range_::parse_impl<T>)
{
    // Type requirements not met!
    BOOST_STATIC_ASSERT(
        is_range<T>::value);
}

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

template<class R>
class range_base<R>::iterator
{
    friend class range_base<R>;

    char const* next_ = nullptr;
    char const* end_ = nullptr;
    typename R::reference v_;

    explicit
    iterator(string_view s)
        : next_(s.data())
        , end_(s.data() + s.size())
    {
        error_code ec;
        R::begin(next_, end_, ec, v_);
        BOOST_ASSERT(! ec.failed());
    }

    iterator(
        string_view s,
        int) noexcept
        : next_(nullptr)
        , end_(s.data() + s.size())
    {
    }

public:
    using value_type = typename R::value_type;
    using reference = typename R::reference;
    using pointer = void const*;
    using difference_type = std::ptrdiff_t;
    using iterator_category =
        std::forward_iterator_tag;

    iterator() = default;
    iterator(
        iterator const&) noexcept = default;
    iterator& operator=(
        iterator const&) noexcept = default;

    reference
    operator*() const noexcept
    {
        return v_;
    }

    bool
    operator==(
        iterator other) const noexcept
    {
        return
            next_ == other.next_ &&
            end_ == other.end_;
    }

    bool
    operator!=(
        iterator other) const noexcept
    {
        return
            next_ != other.next_ ||
            end_ != other.end_;
    }

    iterator&
    operator++() noexcept
    {
        BOOST_ASSERT(next_ != nullptr);
        error_code ec;
        R::increment(next_, end_, ec, v_);
        if(ec == error::end)
            next_ = nullptr;
        else
            BOOST_ASSERT(! ec.failed());
        return *this;
    }

    iterator
    operator++(int) noexcept
    {
        auto tmp = *this;
        ++*this;
        return tmp;
    }
};

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

template<class R>
auto
range_base<R>::
begin() const ->
    iterator
{
    return iterator(s_);
}

template<class R>
auto
range_base<R>::
end() const ->
    iterator
{
    return iterator(s_, 1);
}

template<class R>
void
range_base<R>::
parse(
    char const*& it,
    char const* const end,
    error_code& ec,
    std::size_t N,
    std::size_t M)
{
    std::size_t n = 0;
    auto const start = it;
    typename range<R>::reference v;

    R::begin(it, end, ec, v);
    if(ec.failed())
    {
        if(ec != error::end)
            return;
        if(n < N)
        {
            // too few
            ec = error::syntax;
            return;
        }

        // good
        ec = {};
        n_ = n;
        s_ = string_view(
            start, it - start);
        return;
    }

    for(;;)
    {
        ++n;
        R::increment(it, end, ec, v);
        if(ec.failed())
        {
            if(ec != error::end)
                return;
            ec = {};
            break;
        }
        if(n > M)
        {
            // too many
            ec = error::syntax;
            return;
        }
    }

    // good
    n_ = n;
    s_ = string_view(start, it - start);
}

} // grammar
} // urls
} // boost

#endif

max_stars_repo_name: abitmore/url

max_issues_repo_name: abitmore/url

max_forks_repo_name: alandefreitas/url


text:
#include "sev/acp_ros_conversions/conversions.h"

#include <array>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <limits>
#include <sstream>
#include <utility>

#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include "minkindr_conversions/kindr_msg.h"
#include "std_msgs/Header.h"

#include "sev/acp/types.h"

#include <Eigen/Core>

namespace sev_conversions {

using Transformation = kindr::minimal::QuatTransformation;
using Position3D = kindr::minimal::Position;

// Conversion from rotation matrix to roll, pitch, yaw angles.
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
RotationMatrixToRollPitchYaw(const Eigen::MatrixBase<Derived>& rot) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 3);
  Eigen::Matrix<typename Derived::Scalar, 3, 1> rpy;
  rpy(1, 0) =
      atan2(-rot(2, 0), sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)));
  if (std::abs(cos(rpy(1, 0))) >
      static_cast<typename Derived::Scalar>(1.0e-12)) {
    rpy(2, 0) = atan2(rot(1, 0) / cos(rpy(1, 0)), rot(0, 0) / cos(rpy(1, 0)));
    rpy(0, 0) = atan2(rot(2, 1) / cos(rpy(1, 0)), rot(2, 2) / cos(rpy(1, 0)));
  } else if (sin(rpy(1, 0)) > static_cast<typename Derived::Scalar>(0)) {
    rpy(2, 0) = static_cast<typename Derived::Scalar>(0);
    rpy(0, 0) = atan2(rot(0, 1), rot(1, 1));
  } else {
    rpy(2, 0) = static_cast<typename Derived::Scalar>(0);
    rpy(0, 0) = -atan2(rot(0, 1), rot(1, 1));
  }
  return rpy;
}

// Conversion from roll, pitch, yaw to rotation matrix.
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
RollPitchYawToRotationMatrix(const Eigen::MatrixBase<Derived>& roll_pitch_yaw) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 1);
  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_x;
  typename Derived::Scalar zero = static_cast<typename Derived::Scalar>(0.);
  typename Derived::Scalar one = static_cast<typename Derived::Scalar>(1.);
  rotation_matrix_x << one, zero, zero, zero, cos(roll_pitch_yaw(0)),
      -sin(roll_pitch_yaw(0)), zero, sin(roll_pitch_yaw(0)),
      cos(roll_pitch_yaw(0));

  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_y;
  rotation_matrix_y << cos(roll_pitch_yaw(1)), zero, sin(roll_pitch_yaw(1)),
      zero, one, zero, -sin(roll_pitch_yaw(1)), 0, cos(roll_pitch_yaw(1));

  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_z;
  rotation_matrix_z << cos(roll_pitch_yaw(2)), -sin(roll_pitch_yaw(2)), zero,
      sin(roll_pitch_yaw(2)), cos(roll_pitch_yaw(2)), zero, zero, zero, one;

  return rotation_matrix_z * rotation_matrix_y * rotation_matrix_x;
}

using TimePoint = std::chrono::time_point<std::chrono::system_clock>;
using Duration = std::chrono::nanoseconds;
constexpr double kNanosecondsToSeconds = 1e-9;
constexpr double kSecondsToNanoSeconds = 1e9;

inline TimePoint toTimePoint(const int64_t timestamp_ns) {
  // Making sure that conversion between TimePoint and int64_t comes at no
  // runtime cost.
  static_assert(std::is_same<TimePoint::rep, int64_t>::value, "");
  static_assert(
      std::is_same<
          std::chrono::system_clock::period, std::ratio<1, 1000000000>>::value,
      "");
  return TimePoint{std::chrono::nanoseconds{timestamp_ns}};
}

inline TimePoint toTimePoint(const ros::Time& ros_time) {
  const int64_t timestamp_ns = ros_time.toNSec();
  return toTimePoint(timestamp_ns);
}

inline int64_t toNs(const TimePoint timestamp) {
  return timestamp.time_since_epoch().count();
}

inline ros::Time toRosTime(const TimePoint& time_point) {
  const int64_t timestamp_ns = toNs(time_point);
  const int32_t ros_timestamp_sec = timestamp_ns * kNanosecondsToSeconds;
  const int32_t ros_timestamp_nsec =
      timestamp_ns % static_cast<int64_t>(kSecondsToNanoSeconds);
  return ros::Time(ros_timestamp_sec, ros_timestamp_nsec);
}

inline ros::Time convertNanosecondsInt64ToRosTime(
    const int64_t timestamp_nanoseconds) {
  return toRosTime(toTimePoint(timestamp_nanoseconds));
}

inline int64_t convertRosTimeToNanosecondsInt64(
    const ros::Time& ros_timestamp) {
  return toNs(toTimePoint(ros_timestamp));
}
}  // namespace sev_conversions

namespace sev::acp::udp_ros_bridge {
constexpr double M_TO_MM = 1000;
#ifndef M_PI
#error "M_PI is not defined (issue with _USE_MATH_DEFINES?)"
#endif
constexpr double RAD_TO_CDEG = 100 * 180 / M_PI;

std_msgs::Header convertToRosStamped(sev::acp::MessageHeader const& mh) {
  std_msgs::Header retval;
  retval.seq = mh.seq;
  retval.stamp =
      sev_conversions::convertNanosecondsInt64ToRosTime(mh.timestamp);
  return retval;
}

sev::acp::MessageHeader convertToAcp(const std_msgs::Header& header) {
  sev::acp::MessageHeader mh;
  mh.seq = header.seq;
  mh.timestamp =
      sev_conversions::convertRosTimeToNanosecondsInt64(header.stamp);
  return mh;
}

auto getRotationMatrix(const geometry_msgs::Pose& ros_pose) {
  // Protection against invalid data.
  const auto compute_sq_norm = [](const auto& o) {
    return o.x * o.x + o.y * o.y + o.z * o.z + o.w * o.w;
  };
  const auto norm_sq = compute_sq_norm(ros_pose.orientation);

  // Copied from minkindr.
  // The decision boundary should be at the (binary represented) same number,
  // therefore using abs() would lead to slightly different decision than in
  // kindr's CHECK.
  if ((norm_sq > (1. - 1.e-4)) && (norm_sq < (1. + 1.e-4))) {
    sev_conversions::Transformation T_M_I;
    tf::poseMsgToKindr(ros_pose, &T_M_I);
    return T_M_I.getRotation().getRotationMatrix();
  } else {
    std::stringstream ss;
    ss << "Encountered non-normalized orientation during conversion:\n";
    ss << ros_pose.orientation;
    throw ConversionError(ss.str());
  }
}

geometry_msgs::Pose poseFromMatrix(
    const std::array<double, 3>& position_array,
    const sev_conversions::Transformation::RotationMatrix& rotation_matrix) {
  // TODO(pseyfert): Simplify until poseKindrToMsg.
  if (sev_conversions::Transformation::Rotation::isValidRotationMatrix(
          rotation_matrix)) {
    sev_conversions::Position3D position(&position_array[0]);
    const auto quat_trafo = sev_conversions::Transformation(
        sev_conversions::Transformation::Rotation{std::move(rotation_matrix)},
        std::move(position));
    geometry_msgs::Pose ros_pose;
    tf::poseKindrToMsg(std::move(quat_trafo), &ros_pose);
    return ros_pose;
  } else {
    std::stringstream ss;
    ss << "Encountered invalid rotation during conversion:\n";
    ss << rotation_matrix;
    throw ConversionError(ss.str());
  }
}

geometry_msgs::Pose poseFromMatrix(
    const std::array<double, 3>& position_array, const sev::acp::Rot3& R_m_r) {
  // TODO(pseyfert): Simplify until poseKindrToMsg.
  sev_conversions::Transformation::RotationMatrix rotation_matrix;
  for (std::size_t i = 0; i < 9; ++i) {
    rotation_matrix.data()[i] = R_m_r.mat[i];
  }
  return poseFromMatrix(position_array, rotation_matrix);
}

std::tuple<sev::acp::Pose> convertToAcp(
    const geometry_msgs::PoseStamped& ros_message) {
  sev::acp::Pose out_pose;
  const auto& ros_pose = ros_message.pose;
  out_pose.position[0] = ros_pose.position.x;
  out_pose.position[1] = ros_pose.position.y;
  out_pose.position[2] = ros_pose.position.z;

  for (std::size_t i = 0; i < 3; ++i) {
    out_pose.velocity[i] = std::numeric_limits<double>::quiet_NaN();
  }

  const auto rotation_matrix = getRotationMatrix(ros_pose);

  for (std::size_t i = 0; i < 9; ++i) {
    out_pose.R_m_r.mat[i] = rotation_matrix.data()[i];
  }
  out_pose.yaw =
      sev_conversions::RotationMatrixToRollPitchYaw(rotation_matrix)[2];

  out_pose.header = convertToAcp(ros_message.header);
  return std::make_tuple(std::move(out_pose));
}

std::tuple<geometry_msgs::PoseStamped> convertToRosStamped(
    const sev::acp::Pose& acp_pose) {
  geometry_msgs::PoseStamped ros_pose;

  std::array<double, 3> position;
  for (std::size_t i = 0; i < position.size(); i++) {
    position[i] = acp_pose.position[i];
  }
  ros_pose.pose = poseFromMatrix(position, acp_pose.R_m_r);
  // TODO(pseyfert): acp_pose.velocity is ignored.

  ros_pose.header = convertToRosStamped(acp_pose.header);
  return std::make_tuple(std::move(ros_pose));
}

std::tuple<sev::acp::WheelOdometryIntegrated, sev::acp::WheelOdometryInt>
convertToAcp(const geometry_msgs::TwistStamped& ros_message) {
  const auto& linear = ros_message.twist.linear;
  const auto& angular = ros_message.twist.angular;
  const auto header = convertToAcp(ros_message.header);

  const auto woi_int = [&]() {
    sev::acp::WheelOdometryInt woi;
    const auto buffer =
        std::array<int, 6>{linear.x * M_TO_MM,      linear.y * M_TO_MM,
                           linear.z * M_TO_MM,      angular.x * RAD_TO_CDEG,
                           angular.y * RAD_TO_CDEG, angular.z * RAD_TO_CDEG};
    // Can't use std::copy because woi.twist is packed and STL algorithms don't
    // handle packed structs.
    for (std::size_t i = 0; i < buffer.size(); ++i) {
      woi.twist[i] = buffer[i];
    }
    woi.header = header;
    return woi;
  }();
  const auto woi_integrated = [&]() {
    sev::acp::WheelOdometryIntegrated woi;
    const auto buffer = std::array{linear.x,  linear.y,  linear.z,
                                   angular.x, angular.y, angular.z};
    // Can't use std::copy because woi.twist is packed and STL algorithms don't
    // handle packed structs.
    for (std::size_t i = 0; i < buffer.size(); ++i) {
      woi.twist[i] = buffer[i];
    }
    woi.header = header;
    return woi;
  }();
  return {std::move(woi_integrated), std::move(woi_int)};
}

std::tuple<geometry_msgs::TwistStamped> convertToRosStamped(
    const sev::acp::WheelOdometryIntegrated& woi) {
  geometry_msgs::TwistStamped ros_message;

  std::size_t i = 0;
  ros_message.twist.linear.x = woi.twist[i++];
  ros_message.twist.linear.y = woi.twist[i++];
  ros_message.twist.linear.z = woi.twist[i++];
  ros_message.twist.angular.x = woi.twist[i++];
  ros_message.twist.angular.y = woi.twist[i++];
  ros_message.twist.angular.z = woi.twist[i++];

  ros_message.header = convertToRosStamped(woi.header);
  return std::make_tuple(std::move(ros_message));
}

std::tuple<sev::acp::OperationState> convertToAcp(
    const state_machine_msgs::State& status) {
  sev::acp::OperationState acp;
  acp.task = status.state_int;
  acp.stage = status.substate_int;
  // TODO(pseyfert): Fill with meaningful data.
  for (std::size_t i = 0; i < 16; ++i) {
    acp.task_uuid[i] = 0;
  }
  acp.optimization_progress = -1;
  acp.header = convertToAcp(status.header);
  return std::make_tuple(std::move(acp));
}

std::tuple<state_machine_msgs::State> convertToRosStamped(
    const sev::acp::OperationState& os) {
  state_machine_msgs::State state;
  state.state_int = os.task;
  state.substate_int = os.stage;
  // TODO(pseyfert): Fill with meaningful data.
  // state.state =
  // state.substate =
  state.header = convertToRosStamped(os.header);
  return std::make_tuple(std::move(state));
}

// sev::acp::Notifications
std::tuple<sev::acp::Notifications> convertToAcp(
    const state_machine_msgs::Status& status) {
  sev::acp::Notifications acp;
  acp.module_id = status.module;
  acp.status_code = status.status_code;
  acp.severity = status.severity;
  // TODO(pseyfert): Fill with meaningful data.
  acp.logId = -1;
  acp.logEntryId = -1;
  acp.header = convertToAcp(status.header);
  return std::make_tuple(std::move(acp));
}

std::tuple<state_machine_msgs::Status> convertToRosStamped(
    const sev::acp::Notifications& notifications) {
  state_machine_msgs::Status status;
  status.severity = notifications.severity;
  status.module = notifications.module_id;
  status.status_code = notifications.status_code;
  // TODO(pseyfert): Fill with meaningful data.
  // status.status_text =
  status.header = convertToRosStamped(notifications.header);
  return std::make_tuple(std::move(status));
}

std::tuple<atlas_msgs::PositioningUpdate> convertToRosStamped(
    const sev::acp::PoseInt& acp_pose) {
  atlas_msgs::PositioningUpdate retval;
  retval.header = convertToRosStamped(acp_pose.header);
  std::array<double, 3> position;
  for (std::size_t i = 0; i < 3; ++i) {
    position[i] = acp_pose.position_mm[i] / M_TO_MM;
  }
  Eigen::Matrix<double, 3, 1> angles{
      acp_pose.roll_cdeg / RAD_TO_CDEG,
      acp_pose.pitch_cdeg / RAD_TO_CDEG,
      acp_pose.yaw_cdeg / RAD_TO_CDEG,
  };
  const auto rotation_matrix =
      sev_conversions::RollPitchYawToRotationMatrix(angles);
  retval.pose = poseFromMatrix(position, rotation_matrix);
  retval.quality.level = acp_pose.quality;
  retval.is_relocalization_event = acp_pose.relocalization;
  return std::make_tuple(std::move(retval));
}

std::tuple<sev::acp::PoseFloat, sev::acp::PoseInt> convertToAcp(
    const atlas_msgs::PositioningUpdate& ros_msg) {
  const auto rotation_matrix = getRotationMatrix(ros_msg.pose);
  const auto angles =
      sev_conversions::RotationMatrixToRollPitchYaw(rotation_matrix);
  const auto header = convertToAcp(ros_msg.header);

  const auto pose_int = [&]() {
    sev::acp::PoseInt retval;

    retval.position_mm[0] = ros_msg.pose.position.x * M_TO_MM;
    retval.position_mm[1] = ros_msg.pose.position.y * M_TO_MM;
    retval.position_mm[2] = ros_msg.pose.position.z * M_TO_MM;

    for (std::size_t i = 0; i < 3; ++i) {
      // TODO(pseyfert): Fill with meaningful data.
      retval.velocity_mmps[i] = 0;
    }
    retval.quality = ros_msg.quality.level;

    retval.roll_cdeg = angles[0] * RAD_TO_CDEG;
    retval.pitch_cdeg = angles[1] * RAD_TO_CDEG;
    retval.yaw_cdeg = angles[2] * RAD_TO_CDEG;

    retval.relocalization = ros_msg.is_relocalization_event ? 1 : 0;

    retval.header = header;
    return retval;
  }();

  const auto pose_float = [&]() {
    sev::acp::PoseFloat retval;

    retval.position[0] = ros_msg.pose.position.x;
    retval.position[1] = ros_msg.pose.position.y;
    retval.position[2] = ros_msg.pose.position.z;

    for (std::size_t i = 0; i < 3; ++i) {
      retval.velocity[i] = std::numeric_limits<double>::quiet_NaN();
    }
    retval.quality = ros_msg.quality.level;

    for (std::size_t i = 0; i < 9; ++i) {
      retval.rot.mat[i] = rotation_matrix.data()[i];
    }
    retval.roll = angles[0];
    retval.pitch = angles[1];
    retval.yaw = angles[2];

    retval.relocalization = ros_msg.is_relocalization_event ? 1 : 0;

    retval.header = header;
    return retval;
  }();

  return {std::move(pose_float), std::move(pose_int)};
}

std::tuple<geometry_msgs::PoseStamped, atlas_msgs::PositioningUpdate>
convertToRosStamped(const sev::acp::PoseFloat& acp_pose) {
  atlas_msgs::PositioningUpdate positioning_update;
  geometry_msgs::PoseStamped ros_pose;

  std::array<double, 3> position;
  for (std::size_t i = 0; i < position.size(); i++) {
    position[i] = acp_pose.position[i];
  }
  positioning_update.pose = poseFromMatrix(position, acp_pose.rot);
  ros_pose.pose = poseFromMatrix(position, acp_pose.rot);

  positioning_update.header = convertToRosStamped(acp_pose.header);
  ros_pose.header = convertToRosStamped(acp_pose.header);
  positioning_update.quality.level = acp_pose.quality;
  positioning_update.is_relocalization_event = acp_pose.relocalization;
  return std::make_tuple(std::move(ros_pose), std::move(positioning_update));
}

std::tuple<geometry_msgs::TwistStamped> convertToRosStamped(
    const sev::acp::WheelOdometryInt& woi) {
  geometry_msgs::TwistStamped ros_message;

  std::size_t i = 0;
  ros_message.twist.linear.x = woi.twist[i++] / M_TO_MM;
  ros_message.twist.linear.y = woi.twist[i++] / M_TO_MM;
  ros_message.twist.linear.z = woi.twist[i++] / M_TO_MM;
  ros_message.twist.angular.x = woi.twist[i++] / RAD_TO_CDEG;
  ros_message.twist.angular.y = woi.twist[i++] / RAD_TO_CDEG;
  ros_message.twist.angular.z = woi.twist[i++] / RAD_TO_CDEG;

  ros_message.header = convertToRosStamped(woi.header);
  return std::make_tuple(std::move(ros_message));
}
}  // namespace sev::acp::udp_ros_bridge