text:
#ifdef STAN_OPENCL
#include <stan/math/prim/mat.hpp>
#include <stan/math/opencl/copy.hpp>
#include <stan/math/opencl/multiply.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <gtest/gtest.h>
#include <algorithm>
boost::random::mt19937 rng;

#define EXPECT_MATRIX_NEAR(A, B, DELTA) \
  for (int i = 0; i < A.size(); i++)    \
    EXPECT_NEAR(A(i), B(i), DELTA);

TEST(MathMatrix, vector_row_vector) {
  stan::math::vector_d v(3);
  stan::math::row_vector_d rv(3);
  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m_cl(1, 1);
  EXPECT_NO_THROW(m_cl = rv_cl * v_cl);
}

TEST(MathMatrix, one_dim_zero_matrix) {
  stan::math::matrix_d m0(5, 0);
  stan::math::matrix_d m1(0, 3);

  stan::math::matrix_cl<double> m0_cl(m0);
  stan::math::matrix_cl<double> m1_cl(m1);
  EXPECT_NO_THROW(m0_cl * m1_cl);

  EXPECT_NO_THROW(m0_cl * 2.0);
  EXPECT_NO_THROW(2.0 * m0_cl);

  EXPECT_NO_THROW(m1_cl * 2.0);
  EXPECT_NO_THROW(2.0 * m1_cl);
}

TEST(MathMatrix, zero_result_matrix) {
  stan::math::matrix_d m0(0, 5);
  stan::math::matrix_d m1(5, 0);

  stan::math::matrix_cl<double> m0_cl(m0);
  stan::math::matrix_cl<double> m1_cl(m1);
  EXPECT_NO_THROW(m0_cl * m1_cl);
}

TEST(MathMatrix, zero_size_input_matrix) {
  stan::math::matrix_d m0(0, 0);
  stan::math::matrix_d m1(0, 0);

  stan::math::matrix_cl<double> m0_cl(m0);
  stan::math::matrix_cl<double> m1_cl(m1);
  EXPECT_NO_THROW(m0_cl * m1_cl);

  EXPECT_NO_THROW(m0_cl * 2.0);
  EXPECT_NO_THROW(2.0 * m0_cl);
}

TEST(MathMatrix, non_matching_dim_excpetion) {
  stan::math::matrix_d m0(5, 3);
  stan::math::matrix_d m1(2, 6);

  stan::math::matrix_cl<double> m0_cl(m0);
  stan::math::matrix_cl<double> m1_cl(m1);
  EXPECT_THROW(m0_cl * m1_cl, std::invalid_argument);
}

TEST(MathMatrix, multiply_scalar) {
  auto v = stan::math::vector_d::Random(25).eval();
  stan::math::vector_d v_cl_res(25);
  auto rv = stan::math::row_vector_d::Random(25).eval();
  stan::math::row_vector_d rv_cl_res(25);
  auto m = stan::math::matrix_d::Random(5, 5).eval();
  stan::math::matrix_d m_cl_res(5, 5);

  stan::math::matrix_cl<double> v_cl(v);
  v_cl = v_cl * 2.0;
  v_cl_res = stan::math::from_matrix_cl(v_cl);

  stan::math::matrix_cl<double> rv_cl(rv);
  rv_cl = rv_cl * 2.0;
  rv_cl_res = stan::math::from_matrix_cl(rv_cl);

  stan::math::matrix_cl<double> m_cl(m);
  m_cl = m_cl * 2.0;
  m_cl_res = stan::math::from_matrix_cl(m_cl);

  v = v * 2.0;
  rv = rv * 2.0;
  m = m * 2.0;

  EXPECT_MATRIX_NEAR(v, v_cl_res, 1e-10);
  EXPECT_MATRIX_NEAR(rv, rv_cl_res, 1e-10);
  EXPECT_MATRIX_NEAR(m, m_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_vector) {
  auto v = stan::math::vector_d::Random(5).eval();
  auto rv = stan::math::row_vector_d::Random(5).eval();
  stan::math::matrix_d m0(1, 1);
  stan::math::matrix_d m0_cl_res(1, 1);
  stan::math::matrix_d m1(5, 5);
  stan::math::matrix_d m1_cl_res(5, 5);

  m0 = rv * v;
  m1 = v * rv;

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 1);
  stan::math::matrix_cl<double> m1_cl(5, 5);

  m0_cl = rv_cl * v_cl;
  m1_cl = v_cl * rv_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  m1_cl_res = stan::math::from_matrix_cl(m1_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
  EXPECT_MATRIX_NEAR(m1, m1_cl_res, 1e-10);
}

TEST(MathMatrix, matrix_vector_small) {
  auto m = stan::math::matrix_d::Random(4, 5).eval();
  auto v = stan::math::vector_d::Random(5).eval();
  stan::math::matrix_d m0(4, 1);
  stan::math::matrix_d m0_cl_res(4, 1);

  m0 = m * v;

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> m0_cl(4, 1);

  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, matrix_vector_big) {
  auto m = stan::math::matrix_d::Random(400, 600).eval();
  auto v = stan::math::vector_d::Random(600).eval();
  stan::math::matrix_d m0(400, 1);
  stan::math::matrix_d m0_cl_res(400, 1);

  m0 = m * v;

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> m0_cl(400, 1);

  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_matrix_small) {
  auto m = stan::math::matrix_d::Random(5, 4).eval();
  auto rv = stan::math::row_vector_d::Random(5).eval();
  stan::math::matrix_d m0(1, 4);
  stan::math::matrix_d m0_cl_res(1, 4);

  m0 = rv * m;

  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 4);

  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_matrix_big) {
  auto m = stan::math::matrix_d::Random(600, 400).eval();
  auto rv = stan::math::row_vector_d::Random(600).eval();
  stan::math::matrix_d m0(1, 400);
  stan::math::matrix_d m0_cl_res(1, 400);

  m0 = rv * m;

  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 400);

  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, matrix_vector_tri_small) {
  auto m = stan::math::matrix_d::Random(4, 5).eval();
  auto v = stan::math::vector_d::Random(5).eval();
  stan::math::matrix_d m0(4, 1);
  stan::math::matrix_d m0_cl_res(4, 1);

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> m0_cl(4, 1);

  m0 = m * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Entire);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m.triangularView<Eigen::Lower>() * v;
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Entire);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m.triangularView<Eigen::Upper>() * v;
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  // the cast is needed because operator* for for two triangular views does not
  // exist
  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Lower>())
       * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Upper>())
       * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Lower>())
       * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Upper>())
       * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, matrix_vector_tri_big) {
  auto m = stan::math::matrix_d::Random(400, 600).eval();
  auto v = stan::math::vector_d::Random(600).eval();
  stan::math::matrix_d m0(400, 1);
  stan::math::matrix_d m0_cl_res(400, 1);

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> m0_cl(400, 1);

  m0 = m * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Entire);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m.triangularView<Eigen::Lower>() * v;
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Entire);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m.triangularView<Eigen::Upper>() * v;
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  // the cast is needed because operator* for for two triangular views does not
  // exist
  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Lower>())
       * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Upper>())
       * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Lower>())
       * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Upper>())
       * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_matrix_tri_small) {
  auto m = stan::math::matrix_d::Random(5, 4).eval();
  auto rv = stan::math::row_vector_d::Random(5).eval();
  stan::math::matrix_d m0(1, 4);
  stan::math::matrix_d m0_cl_res(1, 4);

  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 4);

  m0 = rv * m;
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Entire);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv.triangularView<Eigen::Lower>() * m;
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Entire);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv.triangularView<Eigen::Upper>() * m;
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  // the cast is needed because operator* for for two triangular views does not
  // exist
  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Lower>())
       * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Upper>())
       * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Lower>())
       * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Upper>())
       * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_matrix_tri_big) {
  auto m = stan::math::matrix_d::Random(600, 400).eval();
  auto rv = stan::math::row_vector_d::Random(600).eval();
  stan::math::matrix_d m0(1, 400);
  stan::math::matrix_d m0_cl_res(1, 400);

  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 400);

  m0 = rv * m;
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Entire);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv.triangularView<Eigen::Lower>() * m;
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Entire);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv.triangularView<Eigen::Upper>() * m;
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  // the cast is needed because operator* for for two triangular views does not
  // exist
  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Lower>())
       * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Upper>())
       * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Lower>())
       * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Upper>())
       * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_small) {
  using stan::math::multiply;
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_big) {
  using stan::math::multiply;
  int size = 512;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, lower_tri_rect_multiply_small) {
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Lower);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Lower>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, lower_tri_rect_multiply_big) {
  int size = 512;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Lower);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Lower>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, lower_tri_rect_multiply_big_rect) {
  int size = 321;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size * 3).eval();
  stan::math::matrix_d m3_cl_res(size, size * 3);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Lower);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Lower>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, upper_tri_rect_multiply_small) {
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Upper);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Upper>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, upper_tri_rect_multiply_big) {
  int size = 472;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Upper);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Upper>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, upper_tri_rect_multiply_big_rect) {
  int size = 463;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size * 3).eval();
  stan::math::matrix_d m3_cl_res(size, size * 3);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Upper);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Upper>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_lower_tri_multiply_small) {
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Lower);

  auto m3 = (m1 * m2.triangularView<Eigen::Lower>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_lower_tri_multiply_big) {
  int size = 451;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Lower);

  auto m3 = (m1 * m2.triangularView<Eigen::Lower>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_lower_tri_multiply_big_rect) {
  int size = 444;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size * 3).eval();
  stan::math::matrix_d m3_cl_res(size, size * 3);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Lower);

  auto m3 = (m1 * m2.triangularView<Eigen::Lower>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_upper_tri_multiply_small) {
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Upper);

  auto m3 = (m1 * m2.triangularView<Eigen::Upper>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_upper_tri_multiply_big) {
  int size = 468;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Upper);

  auto m3 = (m1 * m2.triangularView<Eigen::Upper>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_upper_tri_multiply_big_rect) {
  int size = 345;
  auto m1 = stan::math::matrix_d::Random(size * 3, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size * 3, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Upper);

  auto m3 = (m1 * m2.triangularView<Eigen::Upper>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_big_split_4) {
  using stan::math::multiply;
  int size = 512;
  auto m1 = stan::math::matrix_d::Random(size, size * 2).eval();
  auto m2 = stan::math::matrix_d::Random(size * 2, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = stan::math::opencl::multiply(m11, m22);

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_big_split_11) {
  using stan::math::multiply;
  int size = 433;
  auto m1 = stan::math::matrix_d::Random(size, size * 11).eval();
  auto m2 = stan::math::matrix_d::Random(size * 11, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = stan::math::opencl::multiply(m11, m22);

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_small_split_big) {
  using stan::math::multiply;
  int size = 32;
  auto m1 = stan::math::matrix_d::Random(size, size * 10).eval();
  auto m2 = stan::math::matrix_d::Random(size * 10, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = stan::math::opencl::multiply(m11, m22);

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}
#endif

max_stars_repo_name: alashworth/stan-monorepo

max_issues_repo_name: alashworth/stan-monorepo

max_forks_repo_name: alashworth/stan-monorepo


text:
/*
Copyright 2013, 2015 Rogier van Dalen.

Licensed 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
Implement the polymorphic interface for actual ranges underlying any_range.

This file follows the same structure as interface.hpp.
Some of the class names are even the same, but then in a different namespace.
Before reading this file, it is probably easiest to read and understand
interface.hpp.

The same idea of linear inheritance is used in this file, but here each
combination of direction and operation gets its own class, and the
implementation linearly inherits from all of them.
*/

#ifndef RANGE_ANY_RANGE_IMPLEMENTATION_HPP_INCLUDED
#define RANGE_ANY_RANGE_IMPLEMENTATION_HPP_INCLUDED

#include <utility>
#include <stdexcept>

#include <boost/mpl/placeholders.hpp>

#include "utility/unique_ptr.hpp"

#include "meta/range.hpp"
#include "meta/fold.hpp"

#include "range/core.hpp"
#include "capability.hpp"
#include "interface.hpp"

namespace range { namespace any_range_implementation {

    /** \brief
    Implement the interface in ./interface.hpp for \a Capabilities, for all
    operations that \a Underlying supports.

    Operations that are not supported by \a Underlying lead to undefined
    behaviour when called.
    */
    template <class Element, class CapabilityKeys, class Underlying>
        class implementation;

    template <class Element, class CapabilityKeys, class Underlying>
        class base
    : public any_range_interface::interface <Element, CapabilityKeys,
        typename capability::detect_default_direction <Underlying>::type>
    {
        static_assert (capability::is_capability_keys <CapabilityKeys>::value,
            "");

        Underlying underlying_;

    public:
        typedef Underlying underlying_type;
        typedef implementation <Element, CapabilityKeys, Underlying>
            implementation_type;

        template <class OtherUnderlying> struct implementation_for {
            typedef implementation <Element, CapabilityKeys, OtherUnderlying>
                type;
        };

        template <class Argument> base (Argument && argument)
        : underlying_ (std::forward <Argument> (argument)) {}

        base (base const & other) : underlying_ (other.underlying_) {}
        base (base && other) : underlying_ (std::move (other.underlying_)) {}

        Underlying & underlying() { return underlying_; }
        Underlying const & underlying() const { return underlying_; }
    };

    /** Implement the losing of a capability. */
    template <class Element, class CapabilityKeys, class KeyToRemove,
            class Base, class Enable = void>
        struct implement_lose_capability;

    // Without copy construction: can not be implemented.
    template <class Element, class CapabilityKeys, class KeyToRemove,
            class Base>
        struct implement_lose_capability <Element, CapabilityKeys, KeyToRemove,
            Base, typename boost::disable_if <
                meta::has_key <capability::copy_construct, CapabilityKeys>
            >::type>
    : Base
    {
        static_assert (capability::is_capability_keys <CapabilityKeys>::value,
            "");

        template <class Argument> implement_lose_capability (
            Argument && argument)
        : Base (std::forward <Argument> (argument)) {}
    };

    // With copy construction: can be implemented.
    template <class Element, class CapabilityKeys, class KeyToRemove,
            class Base>
        struct implement_lose_capability <Element, CapabilityKeys, KeyToRemove,
            Base, typename boost::enable_if <
                meta::has_key <capability::copy_construct, CapabilityKeys>
            >::type>
    : Base
    {
        static_assert (capability::is_capability_keys <CapabilityKeys>::value,
            "");

        template <class Argument> implement_lose_capability (
            Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        using Base::lose_capability;

        virtual typename Base::template
            interface_ptr_without <KeyToRemove>::type
                lose_capability (capability::type <KeyToRemove>) const
        {
            typedef typename meta::remove <KeyToRemove, CapabilityKeys>::type
                new_capability_keys;
            typedef implementation <Element, new_capability_keys,
                typename Base::underlying_type> new_implementation_type;

            return utility::make_unique <new_implementation_type> (
                this->underlying());
        }
    };

    /* Implement capabilities. */
    template <class Element, class Capability, class Base, class Enable = void>
        struct implement;

    template <class Element, class Base>
        struct implement <Element,
            meta::map_element <capability::copy_construct, void>,
            Base>
    : Base
    {
        template <class Argument> implement (Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        virtual typename Base::interface_ptr copy() const {
            return utility::make_unique <typename Base::implementation_type> (
                this->underlying());
        }
    };

    // default_direction
    template <class Element, class Direction, class Base>
        struct implement <Element,
            meta::map_element <capability::default_direction, Direction>,
            Base>
    : Base
    {
        template <class Argument> implement (Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        // Sometimes this overrides something, but if the default direction
        // is default-constructible, the method is not virtual in the interface.
        virtual Direction default_direction() const
        { throw std::logic_error ("Bug in any_range."); }
    };

    template <class Element, class Capability, class Direction, class Base>
        struct implement_capability;

    // Farm out to implement_capability for each capability.
    template <class Element, class Direction, class Capabilities, class Base>
        struct implement <Element,
            meta::map_element <Direction, Capabilities>, Base>
    : meta::fold <implement_capability <
            Element, boost::mpl::_2, Direction, boost::mpl::_1>,
        Base, Capabilities>::type
    {
        typedef typename meta::fold <implement_capability <
                Element, boost::mpl::_2, Direction, boost::mpl::_1>,
            Base, Capabilities>::type base_type;

        template <class Argument> implement (Argument && argument)
        : base_type (std::forward <Argument> (argument)) {}
    };

    // empty.
    template <class Element, class Direction, class Base>
        struct implement_capability <Element,
            capability::empty, Direction, Base>
    : Base
    {
        template <class Argument> implement_capability (Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        using Base::empty;

        virtual bool empty (Direction const & direction) const
        { return range::empty (this->underlying(), direction); }
    };

    // size.
    template <class Element, class Direction, class Base>
        struct implement_capability <Element,
            capability::size, Direction, Base>
    : Base
    {
        template <class Argument> implement_capability (Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        using Base::size;

        virtual std::size_t size (Direction const & direction) const
        { return range::size (this->underlying(), direction); }
    };

    // first.
    template <class Element, class Direction, class Base>
        struct implement_capability <Element,
            capability::first, Direction, Base>
    : Base
    {
        template <class Argument> implement_capability (Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        using Base::first;

    private:
        Element implementation (
            Direction const & direction, rime::false_type) const
        { return range::first (this->underlying(), direction); }

        Element implementation (Direction const &, rime::true_type) const
        { throw std::logic_error ("first() not implemented for empty range."); }

    public:
        virtual Element first (Direction const & direction) const {
            return implementation (direction,
                always_empty <typename Base::underlying_type, Direction>());
        }
    };

    // drop_one.
    template <class Element, class Direction, class Base>
        struct implement_capability <Element,
            capability::drop_one, Direction, Base>
    : Base
    {
        template <class Argument> implement_capability (Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        using Base::drop_one;

        typedef typename Base::interface_ptr interface_ptr;

    private:
        interface_ptr implementation (
            Direction const & direction, rime::false_type) const
        {
            typedef typename Base::underlying_type underlying_type;
            // If the range is heterogeneous, the new underlying type will be
            // different.
            // It is assumed it has the same capabilities.
            typedef typename result_of <callable::drop (
                underlying_type, Direction)>::type new_underlying_type;

            typedef typename Base::template implementation_for <
                new_underlying_type>::type new_implementation_type;
            return utility::make_unique <new_implementation_type> (
                range::drop (this->underlying(), direction));
        }

        interface_ptr implementation (Direction const &, rime::true_type) const
        { throw std::logic_error ("drop() not implemented for empty range."); }

    public:
        virtual interface_ptr drop_one (
            Direction const & direction) const
        {
            return implementation (direction,
                always_empty <typename Base::underlying_type, Direction>());
        }
    };

    // drop_n.
    template <class Element, class Direction, class Base>
        struct implement_capability <Element,
            capability::drop_n, Direction, Base>
    : Base
    {
        template <class Argument> implement_capability (Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        using Base::drop_n;

        typedef typename Base::interface_ptr interface_ptr;

    private:
        // Not always empty.
        interface_ptr implementation (std::size_t increment,
            Direction const & direction, rime::false_type) const
        {
            typedef typename Base::underlying_type underlying_type;
            typedef typename result_of <callable::drop (
                    underlying_type, std::size_t, Direction)>::type
                new_underlying_type;
            typedef typename Base::template implementation_for <
                new_underlying_type>::type new_implementation_type;
            return utility::make_unique <new_implementation_type> (
                range::drop (this->underlying(), increment, direction));
        }

        // Always empty.
        interface_ptr implementation (
            std::size_t, Direction const &, rime::true_type) const
        { throw std::logic_error ("drop() not implemented for empty range."); }

    public:
        virtual interface_ptr drop_n (
            std::size_t increment, Direction const & direction) const
        {
            return implementation (increment, direction,
                always_empty <typename Base::underlying_type, Direction>());
        }
    };

    template <class Element, class Direction, class Base>
        struct implement_capability <Element,
            capability::chop_destructive, Direction, Base>
    : Base
    {
        template <class Argument> implement_capability (Argument && argument)
        : Base (std::forward <Argument> (argument)) {}

        using Base::chop_destructive;

        typedef typename Base::interface_ptr interface_ptr;

    public:
        // Don't hide this type from subclasses.
        typedef typename Base::underlying_type underlying_type;

    private:
        typedef any_range_interface::chopped <Element, interface_ptr>
            chop_destructive_result;

        // Dispatch based on: has <chop_in_place>, always_empty.
        chop_destructive_result implementation (
            Direction const & direction, interface_ptr & this_,
            rime::true_type, rime::false_type)
        {
            return chop_destructive_result (
                range::chop_in_place (this->underlying(), direction),
                interface_ptr());
        }

        chop_destructive_result implementation (
            Direction const & direction, interface_ptr & this_,
            rime::false_type, rime::false_type)
        {
            auto c = range::chop (std::move (this->underlying()), direction);
            typedef typename std::decay <decltype (c.forward_rest())>::type
                new_underlying_type;
            typedef typename Base::template implementation_for <
                new_underlying_type>::type new_implementation_type;
            interface_ptr discardable = std::move (this_);
            this_ =
                utility::make_unique <new_implementation_type> (c.move_rest());
            return chop_destructive_result (
                c.move_first(), std::move (discardable));
        }

        template <class Bool>
            chop_destructive_result implementation (
                Direction const &, interface_ptr &, Bool, rime::true_type)
        { throw std::logic_error ("chop() not implemented for empty range."); }

    public:
        virtual chop_destructive_result chop_destructive (
            Direction const & direction, interface_ptr & this_)
        {
            return implementation (direction, this_,
                has <callable::chop_in_place (underlying_type &, Direction)>(),
                always_empty <typename Base::underlying_type, Direction>());
        }
    };

    // Types for implementation class.

    template <class Element, class CapabilityKeys, class Underlying>
        struct implementation_types
    {
        typedef typename capability::detect_capabilities <
            Underlying, CapabilityKeys>::type capabilities;

        typedef typename meta::fold <
                implement_lose_capability <Element, CapabilityKeys,
                    boost::mpl::_2, boost::mpl::_1>,
                base <Element, CapabilityKeys, Underlying>,
                CapabilityKeys>::type lose_capabilities_type;

        typedef typename meta::fold <
            implement <Element, boost::mpl::_2, boost::mpl::_1>,
            lose_capabilities_type,
            capabilities>::type base_type;
    };

    /* Main implementation class. */

    template <class Element, class CapabilityKeys, class Underlying>
        class implementation
    : public implementation_types <
        Element, CapabilityKeys, Underlying>::base_type
    {
        typedef implementation_types <Element, CapabilityKeys, Underlying>
            types;
    public:
        typedef typename types::capabilities capabilities;
    private:
        typedef typename types::base_type base_type;

    public:
        template <class Argument> implementation (Argument && argument)
        : base_type (std::forward <Argument> (argument)) {}
    };

}} // namespace range::any_range_implementation

#endif // RANGE_ANY_RANGE_IMPLEMENTATION_HPP_INCLUDED

ext: hpp

max_stars_repo_name: rogiervd/range

max_issues_repo_name: rogiervd/range

max_forks_repo_name: rogiervd/range


text:
/*

Copyright (c) 2005-2019, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 the University of Oxford 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 HOLDER 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 TESTCELLLABEL_HPP_
#define TESTCELLLABEL_HPP_

#include "CheckpointArchiveTypes.hpp"

#include "AbstractCellBasedTestSuite.hpp"

#include <boost/shared_ptr.hpp>

#include "CellLabel.hpp"
#include "ApcOneHitCellMutationState.hpp"
#include "ApcTwoHitCellMutationState.hpp"
#include "BetaCateninOneHitCellMutationState.hpp"
#include "WildTypeCellMutationState.hpp"
#include "ApoptoticCellProperty.hpp"
#include "OutputFileHandler.hpp"
#include "CellPropertyRegistry.hpp"
#include "SmartPointers.hpp"

#include "FakePetscSetup.hpp"

class TestCellLabel : public AbstractCellBasedTestSuite
{
public:

    void TestCellLabelMethods()
    {
        MAKE_PTR(CellLabel, p_label);
        TS_ASSERT_EQUALS(p_label->GetCellCount(), 0u);
        p_label->IncrementCellCount();
        TS_ASSERT_EQUALS(p_label->GetCellCount(), 1u);
        p_label->DecrementCellCount();
        TS_ASSERT_EQUALS(p_label->GetCellCount(), 0u);
        TS_ASSERT_THROWS_THIS(p_label->DecrementCellCount(),
                "Cannot decrement cell count: no cells have this cell property");
        TS_ASSERT_EQUALS(p_label->GetColour(), 5u);

        TS_ASSERT_EQUALS(p_label->IsType<CellLabel>(), true);
        TS_ASSERT_EQUALS(p_label->IsType<ApcOneHitCellMutationState>(), false);

        MAKE_PTR(CellLabel, p_other_label);
        TS_ASSERT_EQUALS(p_other_label->IsSame(p_label.get()), true);
        TS_ASSERT_EQUALS(p_label->IsSame(p_other_label), true);

        // Check that const-ness doesn't matter
        TS_ASSERT_EQUALS(p_other_label->IsType<const CellLabel>(), true);
        const CellLabel const_label;
        TS_ASSERT_EQUALS(p_other_label->IsSame(&const_label), true);
        TS_ASSERT_EQUALS(const_label.IsSame(p_other_label), true);
        TS_ASSERT(const_label.IsSame(p_other_label.get()));
    }

    void TestCellPropertyRegistry()
    {
        boost::shared_ptr<AbstractCellProperty> p_property1(
                CellPropertyRegistry::Instance()->Get<WildTypeCellMutationState>());
        boost::shared_ptr<AbstractCellProperty> p_property2(
                CellPropertyRegistry::Instance()->Get<WildTypeCellMutationState>());

        TS_ASSERT(p_property1 == p_property2);
        TS_ASSERT_EQUALS(p_property1->IsType<WildTypeCellMutationState>(), true);
        TS_ASSERT_EQUALS(p_property1->IsSubType<AbstractCellMutationState>(), true);
        TS_ASSERT_EQUALS(p_property1->IsType<CellLabel>(), false);

        std::vector<boost::shared_ptr<AbstractCellProperty> > properties =
            CellPropertyRegistry::Instance()->rGetAllCellProperties();
        TS_ASSERT_EQUALS(properties.size(), 1u);
        TS_ASSERT(properties[0] == p_property1);

        CellPropertyRegistry::Instance()->Clear();
        properties = CellPropertyRegistry::Instance()->rGetAllCellProperties();
        TS_ASSERT_EQUALS(properties.size(), 0u);

        // The taking-ownership functionality
        p_property1 = CellPropertyRegistry::Instance()->Get<ApcTwoHitCellMutationState>();
        CellPropertyRegistry::Instance()->Get<BetaCateninOneHitCellMutationState>();
        TS_ASSERT_EQUALS(CellPropertyRegistry::Instance()->rGetAllCellProperties().size(), 2u);
        CellPropertyRegistry* p_instance = CellPropertyRegistry::Instance();
        CellPropertyRegistry* p_registry = CellPropertyRegistry::Instance()->TakeOwnership();
        TS_ASSERT_EQUALS(p_instance, p_registry);
        TS_ASSERT_EQUALS(p_registry->rGetAllCellProperties().size(), 2u);
        TS_ASSERT_DIFFERS(CellPropertyRegistry::Instance(), p_registry);
        TS_ASSERT_EQUALS(CellPropertyRegistry::Instance()->rGetAllCellProperties().size(), 0u);
        TS_ASSERT_EQUALS(p_registry->rGetAllCellProperties().size(), 2u);

        // Tidy up
        delete p_registry;
    }

    void TestCellPropertyOrdering()
    {
        CellPropertyRegistry* p_instance = CellPropertyRegistry::Instance();
        p_instance->Clear();
        p_instance->Get<ApcOneHitCellMutationState>();

        std::vector<boost::shared_ptr<AbstractCellProperty> > property_ordering;
        property_ordering.push_back(p_instance->Get<WildTypeCellMutationState>());
        property_ordering.push_back(p_instance->Get<ApcOneHitCellMutationState>());
        property_ordering.push_back(p_instance->Get<ApoptoticCellProperty>());
        property_ordering.push_back(p_instance->Get<CellLabel>());

        TS_ASSERT_EQUALS(p_instance->HasOrderingBeenSpecified(), false);
        p_instance->SpecifyOrdering(property_ordering);
        TS_ASSERT_EQUALS(p_instance->HasOrderingBeenSpecified(), true);

        TS_ASSERT_THROWS_THIS(p_instance->SpecifyOrdering(property_ordering), "An ordering has already been specified.");

        std::vector<boost::shared_ptr<AbstractCellProperty> > properties = p_instance->rGetAllCellProperties();
        TS_ASSERT_EQUALS(properties.size(), 4u);
        TS_ASSERT_EQUALS(properties[0]->IsType<WildTypeCellMutationState>(), true);
        TS_ASSERT_EQUALS(properties[1]->IsType<ApcOneHitCellMutationState>(), true);
        TS_ASSERT_EQUALS(properties[2]->IsType<ApoptoticCellProperty>(), true);
        TS_ASSERT_EQUALS((boost::static_pointer_cast<ApoptoticCellProperty>(properties[2]))->GetColour(), 6u);
        TS_ASSERT_EQUALS(properties[3]->IsType<CellLabel>(), true);
        p_instance->Clear();
    }

    void TestArchiveCellLabel()
    {
        OutputFileHandler handler("archive", false);
        std::string archive_filename = handler.GetOutputDirectoryFullPath() + "label.arch";

        // Archive a cell label
        {
            CellLabel* p_label = new CellLabel();
            p_label->IncrementCellCount();

            TS_ASSERT_EQUALS(p_label->GetCellCount(), 1u);
            TS_ASSERT_EQUALS(p_label->GetColour(), 5u);

            // Create an output archive
            std::ofstream ofs(archive_filename.c_str());
            boost::archive::text_oarchive output_arch(ofs);

            // Write the cell to the archive
            const AbstractCellProperty* const p_const_label = p_label;
            output_arch << p_const_label;

            delete p_label;
        }

        // Restore cell label
        {
            AbstractCellProperty* p_label;

            // Restore the cell label
            std::ifstream ifs(archive_filename.c_str());
            boost::archive::text_iarchive input_arch(ifs);

            input_arch >> p_label;

            CellLabel* p_real_label = dynamic_cast<CellLabel*>(p_label);
            TS_ASSERT(p_real_label != NULL);

            TS_ASSERT_EQUALS(p_real_label->GetCellCount(), 1u);
            TS_ASSERT_EQUALS(p_real_label->GetColour(), 5u);

            // Tidy up
            delete p_label;
        }
    }

    void TestArchiveCellPropertyRegistry()
    {
        OutputFileHandler handler("archive", false);
        std::string archive_filename = handler.GetOutputDirectoryFullPath() + "property.arch";

        // Save
        {
            boost::shared_ptr<AbstractCellProperty> p_label(CellPropertyRegistry::Instance()->Get<CellLabel>());

            TS_ASSERT_EQUALS(p_label->IsType<CellLabel>(), true);
            TS_ASSERT_EQUALS(p_label->IsSubType<AbstractCellProperty>(), true);
            TS_ASSERT_EQUALS(p_label->IsSubType<AbstractCellMutationState>(), false);

            const CellPropertyRegistry* const p_registry = CellPropertyRegistry::Instance()->TakeOwnership();

            // Create an output archive
            std::ofstream ofs(archive_filename.c_str());
            boost::archive::text_oarchive output_arch(ofs);

            // Write the registry to the archive
            output_arch << p_registry;

            // Tidy up
            delete p_registry;
        }

        // Restore boost::shared_ptr to cell property
        {
            CellPropertyRegistry::Instance()->Clear();

            // Initialize a registry
            CellPropertyRegistry* p_registry;

            // Restore the registry state
            std::ifstream ifs(archive_filename.c_str(), std::ios::binary);
            boost::archive::text_iarchive input_arch(ifs);

            input_arch >> p_registry;

            boost::shared_ptr<AbstractCellProperty> p_label = p_registry->Get<CellLabel>();

            TS_ASSERT_EQUALS(p_label->IsType<CellLabel>(), true);
            TS_ASSERT_EQUALS(p_label->IsSubType<AbstractCellProperty>(), true);
            TS_ASSERT_EQUALS(p_label->IsSubType<AbstractCellMutationState>(), false);

            // Tidy up
            delete p_registry;
        }
    }
};

#endif /* TESTCELLLABEL_HPP_ */


text:
#include <boost/python/module.hpp>
#include <boost/preprocessor/expand.hpp>
#include <boost/preprocessor/stringize.hpp>
#include <iostream>

#ifdef NDEBUG
#define RELEASE "RELEASE"
#else
#define RELEASE "DEBUG"
#endif

#define MODNAME_THUNK() (MODNAME)

BOOST_PP_EXPAND(BOOST_PYTHON_MODULE MODNAME_THUNK())
{
  std::cout << RELEASE << " " << BOOST_PP_STRINGIZE(FLAGS) << "\n";
}

ext: cpp

max_stars_repo_name: randolphwong/mcsema

max_issues_repo_name: randolphwong/mcsema

max_forks_repo_name: randolphwong/mcsema


text:
// Copyright 2017 Nest Labs, Inc.
//
// Licensed 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.

#include "graph.hpp"
#include "detector.hpp"
#include "graphanalyzer.hpp"
#include "processorcontainer.hpp"
#include "lag.hpp"

#include <iostream>
#include <cmath>
#include <algorithm>
#include <sstream>
#include <string>
#include <Eigen/Dense>

using namespace std;
using namespace DetectorGraph;
using namespace Eigen;

/**
 * @file robotlocalization.cpp
 * @brief A localization system for a mobile robot with Lag-based feedback loop.
 *
 * @section ex-rl-intro Introduction
 * Back in the 2000s all [_citation needed_] robots were _differential wheeled
 * robots_ [1] and localization with sensor fusion was a popular problem.
 * This example shows a simple solution to that problem using DetectorGraph.
 * It uses an Extended Kalman Filter [2] to continuously predict and correct
 * the robot's Pose (and associated uncertainty) from two information sources:
 * the input to the wheels and a GPS-like system.
 *
 * @section ex-rl-fpvslag FuturePublisher vs. Lag\<T\>
 * This localization algorithm depends on a feedback-loop in that the output of
 * one graph evaluation (i.e. `LocalizationBelief`) is used as an input for the
 * next one (i.e. `Lagged<LocalizationBelief>`):
 @snippetlineno robotlocalization.cpp KalmanPoseCorrector Feedback Loop
 * Note that in this graph the TopicState where the feedback loop closes is a
 * legitimate output in itself and it's very likely that new Detectors in the
 * graph would subscribe to it. In cases like this the use of
 * DetectorGraph::Lag is preferred as it is more extensible and it preserves
 * the normal TopicState guarantee for the TopicState in question as well as it
 * makes the Lagged version of the TopicState clearly documented and also
 * available. Lag allows even for detectors that subscribes to both the immediate
 * and the Lagged version of a TopicState - this could be useful for things like
 * differentiation etc.
 *
 * @section ex-rl-conf-topicstates Configuration TopicStates
 * It also shows how TopicStates can be used for static/configuration data
 * (e.g. `RobotConfig`). This allows for easy dependency tracking, visualization
 * and testing at pretty much no runtime cost.
 *
 * @section ex-rl-localization Localization Algorithm
 * Regarding the Localization algorithm itself, it's mostly taken from [3]
 * except for the correction model where this example uses a different input
 * (i.e. `GPSPosition`) with much simpler transfer function to the Pose vector.
 *
 * @section ex-rl-arch Architecture
 * The graph below shows the relationships between the topics (rectangles) and
 * detectors (ellipses). Note that this graph can be automatically generated
 * for any instance of DetectorGraph::Graph using DetectorGraph::GraphAnalyzer.
 *  @dot "RobotBrain"
digraph GraphAnalyzer {
    rankdir = "LR";
    node[fontname=Helvetica];
    size="12,5";

    "RobotConfig" [label="0:RobotConfig",style=filled, shape=box, color=lightblue];
        "RobotConfig" -> "ExtendedKalmanPosePredictor";
    "InitialPose" [label="1:InitialPose",style=filled, shape=box, color=lightblue];
        "InitialPose" -> "ExtendedKalmanPosePredictor";
    "WheelSpeeds" [label="2:WheelSpeeds",style=filled, shape=box, color=lightblue];
        "WheelSpeeds" -> "ExtendedKalmanPosePredictor";
    "GPSPosition" [label="3:GPSPosition",style=filled, shape=box, color=lightblue];
        "GPSPosition" -> "KalmanPoseCorrector";
    "LaggedLocalizationBelief" [label="4:Lagged<LocalizationBelief>",style=filled, shape=box, color=lightblue];
        "LaggedLocalizationBelief" -> "ExtendedKalmanPosePredictor";
        "LaggedLocalizationBelief" -> "KalmanPoseCorrector";
    "KalmanPoseCorrector" [label="6:KalmanPoseCorrector", color=blue];
        "KalmanPoseCorrector" -> "LocalizationBelief";
    "ExtendedKalmanPosePredictor" [label="5:ExtendedKalmanPosePredictor", color=blue];
        "ExtendedKalmanPosePredictor" -> "LocalizationBelief";
    "LocalizationBelief" [label="7:LocalizationBelief",style=filled, shape=box, color=red];
        "LocalizationBelief" -> "LagLocalizationBelief";
    "LagLocalizationBelief" [label="8:Lag<LocalizationBelief>", color=blue];
        "LagLocalizationBelief" -> "LaggedLocalizationBelief" [style=dotted, color=red, constraint=false];
}
 *  @enddot
 *
 *
 * @section ex-rl-other-notes Other Notes
 * This example also uses the Eigen library [4] for matrix/vector arithmetic
 * and linear-algebraic operations.
 *
 * Note that this entire algorithm is contained in a single file for the sake
 * of unity as an example. In real-world scenarios the suggested pattern is to
 * split the code into:
 *
 @verbatim
   detectorgraph/
        include/
            robotbrain.hpp (RobotBrain header)
        src/
            robotbrain.hpp (RobotBrain implementation)
        detectors/
            include/
                ExtendedKalmanPosePredictor.hpp
                KalmanPoseCorrector.hpp
            src/
                ExtendedKalmanPosePredictor.cpp
                KalmanPoseCorrector.cpp
        topicstates/
            include/
                RobotConfig.hpp
                InitialPose.hpp
                WheelSpeeds.hpp
                LocalizationBelief.hpp
                GPSPosition.hpp
@endverbatim
 *
 * @section ex-rl-refs References
 *  - [1] Differential Wheeled Robots - https://en.wikipedia.org/wiki/Differential_wheeled_robot
 *  - [2] Extended Kalman Filter - https://en.wikipedia.org/wiki/Extended_Kalman_filter
 *  - [3] EKF applied to Mobile Robot’s Localization (Section 2.4.4, page 17) - http://cpscotti.com/pdf/pfc_scotti.pdf
 *  - [4] Eigen - http://eigen.tuxfamily.org/dox/index.html
 */

/// @cond DO_NOT_DOCUMENT

// Contains the configuration parameters of this differential mobile Robot
// or vehicle.
struct RobotConfig : public TopicState
{
    RobotConfig() : r(), b(), e() {}
    RobotConfig(double aR, double aB, double aE)
        : r(aR), b(aB), e(aE) {}
    double r; // wheels r
    double b; // displacement between wheels 2*l (l=d(wheel,center))

    double e; // relative wheel slippage/error
};

struct InitialPose : public TopicState
{
    InitialPose() : timestampMs(0), pose(Vector3d::Zero()) {}
    InitialPose(Vector3d aPose) : timestampMs(0), pose(aPose) {}
    InitialPose(uint64_t aTimestampMs, Vector3d aPose) : timestampMs(aTimestampMs), pose(aPose) {}
    uint64_t timestampMs;
    Vector3d pose;
};

// The measured (via encoders or other odometry method) wheel speeds
struct WheelSpeeds : public TopicState
{
    WheelSpeeds() : timestampMs(), phi(Vector2d::Zero()) {}
    WheelSpeeds(uint64_t ts, Vector2d aPhi) : timestampMs(ts), phi(aPhi) {}
    WheelSpeeds(uint64_t ts, double r, double l) : timestampMs(ts), phi(r, l) {}
    uint64_t timestampMs;

    // phi_r, phi_l in radians
    Vector2d phi;
};

//! [TopicStates Composition Example]
struct KalmanState
{
    KalmanState() : pose(Vector3d::Zero()), error(Matrix3d::Zero()) {}
    KalmanState(Vector3d aPose, Matrix3d aError) : pose(aPose), error(aError) {}
    Vector3d pose;
    Matrix3d error;
};

//! [Mutually Atomic Variables]
struct LocalizationBelief : public TopicState
{
    LocalizationBelief() : timestampMs(), state() {}
    LocalizationBelief(uint64_t ts, const KalmanState& aState) : timestampMs(ts), state(aState) {}
    LocalizationBelief(uint64_t ts, const Vector3d& aPose, const Matrix3d& aError) : timestampMs(ts), state(aPose, aError) {}
    uint64_t timestampMs;
    KalmanState state;
};
//! [Mutually Atomic Variables]

struct GPSPosition : public TopicState
{
    GPSPosition() : timestampMs(), state() {}
    GPSPosition(uint64_t ts, const KalmanState& aState) : timestampMs(ts), state(aState) {}
    uint64_t timestampMs;
    KalmanState state;
};
//! [TopicStates Composition Example]

double WrapAngle(double th)
{
    // remainder requires C++11
    return std::remainder(th, 2. * M_PI);
}

class ExtendedKalmanPosePredictor : public Detector,
    public SubscriberInterface<RobotConfig>,
    public SubscriberInterface<InitialPose>,
    public SubscriberInterface< Lagged<LocalizationBelief> >,
    public SubscriberInterface<WheelSpeeds>,
    public Publisher<LocalizationBelief>
{
public:
    ExtendedKalmanPosePredictor(Graph* graph) : Detector(graph), mCurrentBelief(), mConfig()
    {
        Subscribe<RobotConfig>(this);
        Subscribe<InitialPose>(this);
        Subscribe< Lagged<LocalizationBelief> >(this);
        Subscribe<WheelSpeeds>(this);
        SetupPublishing<LocalizationBelief>(this);
    }

    virtual void Evaluate(const RobotConfig& aConfig)
    {
        mConfig = aConfig;
    }

    virtual void Evaluate(const InitialPose& aInitialPose)
    {
        mCurrentBelief.state = KalmanState(aInitialPose.pose, Matrix3d::Zero());
        mCurrentBelief.timestampMs = aInitialPose.timestampMs;
    }

    virtual void Evaluate(const Lagged<LocalizationBelief>& aLaggedBelief)
    {
        mCurrentBelief.state = aLaggedBelief.data.state;
        mCurrentBelief.timestampMs = aLaggedBelief.data.timestampMs;
    }

    virtual void Evaluate(const WheelSpeeds& aWheelSpeeds)
    {
        // [\Delta S_{r}, \Delta S_{l}] from phi
        Vector2d wheelLinearSpeed = aWheelSpeeds.phi * mConfig.r;

        // Eq. 2.36
        double d_th = (wheelLinearSpeed.x() - wheelLinearSpeed.y()) / mConfig.b;
        // Eq. 2.37
        double d_travel = (wheelLinearSpeed.x() + wheelLinearSpeed.y()) / 2.;

        double theta = mCurrentBelief.state.pose.z();

        // Used on most equations below
        double th_dth2 = theta + d_th / 2.;

        // Eq. 2.33 on [3]
        Vector3d cartesianSpeed;
        cartesianSpeed << d_travel * cos(th_dth2),
                          d_travel * sin(th_dth2),
                                             d_th;

        // Compute Time Delta
        double tDelta = (aWheelSpeeds.timestampMs - mCurrentBelief.timestampMs) * 1e-3;

        // Eq. 2.33 (i.e. 2.23) on [3]
        mCurrentBelief.state.pose = mCurrentBelief.state.pose + (cartesianSpeed * tDelta);
        mCurrentBelief.state.pose.z() = WrapAngle(mCurrentBelief.state.pose.z());

        // Eq. 2.34 on [3]
        Matrix3d G;
        G << 1, 0, -d_travel * sin(th_dth2),
             0, 1,  d_travel * cos(th_dth2),
             0, 0,                        1;

        // Eq. 2.35 on [3]
        Matrix<double, 3, 2> V;
        V << 0.5 * cos(th_dth2) - (d_travel / (2. * mConfig.b)) * sin(th_dth2), 0.5 * cos(th_dth2) + (d_travel / (2. * mConfig.b)) * sin(th_dth2),
             0.5 * sin(th_dth2) + (d_travel / (2. * mConfig.b)) * cos(th_dth2), 0.5 * sin(th_dth2) - (d_travel / (2. * mConfig.b)) * cos(th_dth2),
                                                                  1./mConfig.b,                                                     -1./mConfig.b;

        // Eq. 2.38 on [3]
        Matrix2d M;
        M << mConfig.e * abs(wheelLinearSpeed.x()),                                     0,
                                                 0, mConfig.e * abs(wheelLinearSpeed.y());

        // Eq. 2.24 on [3]
        mCurrentBelief.state.error = G * mCurrentBelief.state.error * G.transpose() + V * M * V.transpose();

        mCurrentBelief.timestampMs = aWheelSpeeds.timestampMs;
        Publish(mCurrentBelief);
    }

    LocalizationBelief mCurrentBelief;
    RobotConfig mConfig;
};

///! [KalmanPoseCorrector Feedback Loop]
class KalmanPoseCorrector : public Detector,
    public SubscriberInterface< Lagged<LocalizationBelief> >,
    public SubscriberInterface<GPSPosition>,
    public Publisher<LocalizationBelief>
{
public:
    KalmanPoseCorrector(Graph* graph) : Detector(graph), mCurrentBelief()
    {
        Subscribe< Lagged<LocalizationBelief> >(this);
        Subscribe<GPSPosition>(this);
        SetupPublishing<LocalizationBelief>(this);
    }
///! [KalmanPoseCorrector Feedback Loop]

    virtual void Evaluate(const Lagged<LocalizationBelief>& aLaggedBelief)
    {
        mCurrentBelief.state = aLaggedBelief.data.state;
        mCurrentBelief.timestampMs = aLaggedBelief.data.timestampMs;
    }

    virtual void Evaluate(const GPSPosition& aCorrectionInput)
    {
        // C = Identity(3)

        // Eq. 2.15 on [3]
        Matrix3d K = mCurrentBelief.state.error * (mCurrentBelief.state.error + aCorrectionInput.state.error).inverse();

        // Eq. 2.13 on [3]
        mCurrentBelief.state.pose = mCurrentBelief.state.pose + K * (aCorrectionInput.state.pose - mCurrentBelief.state.pose);

        // Eq. 2.14 on [3]
        mCurrentBelief.state.error = (Matrix3d::Identity() - K) * mCurrentBelief.state.error;

        Publish(mCurrentBelief);
    }

    LocalizationBelief mCurrentBelief;
};

//![RobotBrain Static]
class RobotBrain : public ProcessorContainer
{
public:
    RobotBrain()
    : mPosePredictor(&mGraph)
    , mKalmanPoseCorrector(&mGraph)
    , mBeliefFeedback(&mGraph)
    {
    }

    ExtendedKalmanPosePredictor mPosePredictor;
    KalmanPoseCorrector mKalmanPoseCorrector;
    Lag<LocalizationBelief> mBeliefFeedback;
    //![RobotBrain Static]

    virtual void ProcessOutput()
    {
        Topic<LocalizationBelief>* correctedStateTopic = mGraph.ResolveTopic<LocalizationBelief>();
        if (correctedStateTopic->HasNewValue())
        {
            const LocalizationBelief& belief = correctedStateTopic->GetNewValue();
            const auto& pose = belief.state.pose;
            cout << "Pose = [" << pose.x() << ", " << pose.y() << ", " << pose.z() << "], ";
            const auto& error = belief.state.error.diagonal();
            cout << "Error = [" << error.x() << ", " << error.y() << ", " << error.z() << "]";
            cout << endl;
        }
    }
};

int main()
{
    RobotBrain robot;

    uint64_t kSimStep = 1000;
    uint64_t dataIndex = 0;

    robot.ProcessData(RobotConfig(1, 1, 0.1));
    robot.ProcessData(InitialPose(Vector3d(10.0, 10.0, 0.0)));

    // Circular Movement
    // for (int i=0;i<100;i++) robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1., 0.5));

    // Fwd, Rotate ~180, Fwd, GPS Update
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, -1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, -1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(GPSPosition((dataIndex++)*kSimStep,
        KalmanState(Vector3d(10., 10., 4),  // Position
                    Vector3d(0.1,0.1,300000).asDiagonal()  // Error
            )));

    cout << "---- Done ----" << endl;

    GraphAnalyzer analyzer(robot.mGraph);
    analyzer.GenerateDotFile("robot_localization.dot");

    return 0;
}

/// @endcond DO_NOT_DOCUMENT

max_stars_repo_name: google/detectorgraph

max_issues_repo_name: google/detectorgraph

max_forks_repo_name: google/detectorgraph


text:
//  Copyright John Maddock 2016.
//  Use, modification and distribution are subject to 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)

#ifndef BOOST_MATH_TOOLS_CONVERT_FROM_STRING_INCLUDED
#define BOOST_MATH_TOOLS_CONVERT_FROM_STRING_INCLUDED

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/type_traits/is_constructible.hpp>
#include <boost/type_traits/conditional.hpp>
#include <boost/lexical_cast.hpp>

namespace boost{ namespace math{ namespace tools{

   template <class T>
   struct convert_from_string_result
   {
      typedef typename boost::conditional<boost::is_constructible<T, const char*>::value, const char*, T>::type type;
   };

   template <class Real>
   Real convert_from_string(const char* p, const boost::false_type&)
   {
#ifdef BOOST_MATH_NO_LEXICAL_CAST
      // This function should not compile, we don't have the necessary functionality to support it:
      BOOST_STATIC_ASSERT(sizeof(Real) == 0);
#else
      return boost::lexical_cast<Real>(p);
#endif
   }
   template <class Real>
   BOOST_CONSTEXPR const char* convert_from_string(const char* p, const boost::true_type&) BOOST_NOEXCEPT
   {
      return p;
   }
   template <class Real>
   BOOST_CONSTEXPR typename convert_from_string_result<Real>::type convert_from_string(const char* p) BOOST_NOEXCEPT_IF((boost::is_constructible<Real, const char*>::value))
   {
      return convert_from_string<Real>(p, boost::is_constructible<Real, const char*>());
   }

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_CONVERT_FROM_STRING_INCLUDED

max_stars_repo_name: kindlychung/mediasoup-sfu-cpp

max_issues_repo_name: kindlychung/mediasoup-sfu-cpp

max_forks_repo_name: kindlychung/mediasoup-sfu-cpp


text:
#include "NormalPrior.h"

#include <iomanip>

#include <SmurffCpp/Utils/linop.h>
#include <SmurffCpp/IO/MatrixIO.h>
#include <SmurffCpp/Utils/counters.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <SmurffCpp/Utils/Distribution.h>
#include <SmurffCpp/Utils/Error.h>

using namespace smurff;

//  base class NormalPrior

NormalPrior::NormalPrior(std::shared_ptr<Session> session, uint32_t mode, std::string name)
   : ILatentPrior(session, mode, name)
{

}

void NormalPrior::init()
{
   //does not look that there was such init previously
   ILatentPrior::init();

   const int K = num_latent();
   m_mu = std::make_shared<Eigen::VectorXd>(K);
   hyperMu().setZero();

   Lambda.resize(K, K);
   Lambda.setIdentity();
   Lambda *= 10;

   // parameters of Inv-Whishart distribution
   WI.resize(K, K);
   WI.setIdentity();
   mu0.resize(K);
   mu0.setZero();
   b0 = 2;
   df = K;

   const auto &config = getSession().getConfig();
   if (config.hasPropagatedPosterior(getMode()))
   {
      mu_pp = std::make_shared<Eigen::MatrixXd>(matrix_utils::dense_to_eigen(*config.getMuPropagatedPosterior(getMode())));
      Lambda_pp = std::make_shared<Eigen::MatrixXd>(matrix_utils::dense_to_eigen(*config.getLambdaPropagatedPosterior(getMode())));
      m_name += " with posterior propagation";
   }
}

const Eigen::VectorXd NormalPrior::fullMu(int n) const
{
   if (getSession().getConfig().hasPropagatedPosterior(getMode()))
   {
      return mu_pp->col(n);
   }
   //else
   return hyperMu();
}

const Eigen::MatrixXd NormalPrior::getLambda(int n) const
{
   if (getSession().getConfig().hasPropagatedPosterior(getMode()))
   {
      return Eigen::Map<Eigen::MatrixXd>(Lambda_pp->col(n).data(), num_latent(), num_latent());
   }
   //else
   return Lambda;
}
void NormalPrior::update_prior()
{
   std::tie(hyperMu(), Lambda) = CondNormalWishart(num_item(), getUUsum(), getUsum(), mu0, b0, WI, df);
}

//n is an index of column in U matrix
void  NormalPrior::sample_latent(int n)
{
   const auto &mu_u = fullMu(n);
   const auto &Lambda_u = getLambda(n);

   Eigen::VectorXd &rr = rrs.local();
   Eigen::MatrixXd &MM = MMs.local();

   rr.setZero();
   MM.setZero();

   // add pnm
   data().getMuLambda(model(), m_mode, n, rr, MM);

   // add hyperparams
   rr.noalias() += Lambda_u * mu_u;
   MM.noalias() += Lambda_u;

   //Solve system of linear equations for x: MM * x = rr - not exactly correct  because we have random part
   //Sample from multivariate normal distribution with mean rr and precision matrix MM

   Eigen::LLT<Eigen::MatrixXd> chol;
   {
      chol = MM.llt(); // compute the Cholesky decomposition X = L * U
      if(chol.info() != Eigen::Success)
      {
         THROWERROR("Cholesky Decomposition failed!");
      }
   }

   chol.matrixL().solveInPlace(rr); // solve for y: y = L^-1 * b
   rr.noalias() += nrandn(num_latent());
   chol.matrixU().solveInPlace(rr); // solve for x: x = U^-1 * y

   U().col(n).noalias() = rr; // rr is equal to x
}

std::ostream &NormalPrior::status(std::ostream &os, std::string indent) const
{
   os << indent << m_name << std::endl;
   os << indent << "  mu: " <<  hyperMu().transpose() << std::endl;
   return os;
}

max_stars_repo_name: msteijaert/smurff

max_issues_repo_name: msteijaert/smurff

max_forks_repo_name: msteijaert/smurff


text:
#include <boost/scoped_ptr.hpp>
#include <iostream>

struct Shoe { ~Shoe() { std::cout << "Buckle my shoe\n"; } };

class MyClass {
    boost::scoped_ptr<int> ptr;
  public:
    MyClass() : ptr(new int) { *ptr = 0; }
    int add_one() { return ++*ptr; }
};

int main()
{
    boost::scoped_ptr<Shoe> x(new Shoe);
    MyClass my_instance;
    std::cout << my_instance.add_one() << '\n';
    std::cout << my_instance.add_one() << '\n';
}

max_stars_repo_name: MatrixWood/some-modern-cpp-training

max_issues_repo_name: MatrixWood/some-modern-cpp-training

max_forks_repo_name: MatrixWood/some-modern-cpp-training


text:
// This file is part of CBOR-lite which is copyright Isode Limited
// and others and released under a MIT license. For details, see the
// COPYRIGHT.md file in the top-level folder of the CBOR-lite software
// distribution.
#include "../include/cbor-lite/codec.h"
#include <boost/test/unit_test.hpp>
#include <tuple>

BOOST_AUTO_TEST_SUITE(cbor_lite)
BOOST_AUTO_TEST_SUITE(canon)

BOOST_AUTO_TEST_CASE(toolong) {
    std::int_fast64_t small = 23u;
    const std::vector<std::string> cases{
        "\x18\x17",
        std::string("\x19\x00\x17", 3),
        std::string("\x1a\x00\x00\00\x17", 5),
        std::string("\x1b\x00\x00\x00\x00\x00\x00\x00\x17", 9),
    };
    for (const auto& encoding : cases) {
        BOOST_CHECK_NO_THROW({
            std::uint_fast64_t value = 0u;
            auto pos = std::begin(encoding);
            auto len = CborLite::decodeUnsigned(pos, std::end(encoding), value);
            BOOST_CHECK(pos == std::end(encoding));
            BOOST_CHECK_EQUAL(len, encoding.size());
            BOOST_CHECK_EQUAL(value, small);
            pos = std::begin(encoding);
            BOOST_CHECK_EXCEPTION(CborLite::decodeUnsigned(pos, std::end(encoding), value, CborLite::Flag::requireMinimalEncoding),
                CborLite::Exception, [](CborLite::Exception const& e) {
                    const std::string expected = "CBOR Exception: encoding not minimal";
                    BOOST_CHECK_EQUAL(e.what(), expected);
                    return e.what() == expected;
                });
        });
    }
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


text:
/*
 * Copyright (c) 2018 Eliane Briand
 *
 * This file is part of SmolDock.
 *
 * SmolDock is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SmolDock is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SmolDock.  If not, see <https://www.gnu.org/licenses/>.
 *
 */

#include <boost/python.hpp>

#include "PyUtilities.h"

#include <Utilities/ReScorer.h>
#include <Utilities/PDBWriter.h>

namespace p = boost::python;

namespace sd = SmolDock;

/*        ReScorer(Protein &prot, Molecule &mol, std::function<double(const iConformer &, const iTransform &, const iProtein &)>& scorFunc);

        bool prepare();
        double getScore();*/

void export_Utilities() {
    p::class_<sd::ReScorer>("ReScorer",
                            p::init<sd::Protein &, sd::Molecule &, sd::Score::ScoringFunctionType>())
            .def("prepare", &sd::ReScorer::prepare)
            .def("getScore", &sd::ReScorer::getScore)
        //.def("set", &World::set)
        //.add_property("rovalue", &Num::get)
        //.add_property("value", &Num::get, &Num::set);
            ;

    p::class_<sd::PDBWriter>("PDBWriter")
            .def("addLigand", &sd::PDBWriter::addLigand)
            .def("writePDB", &sd::PDBWriter::writePDB);

}

max_stars_repo_name: ElianeBriand/SMolDock

max_issues_repo_name: ElianeBriand/SMolDock

max_forks_repo_name: ElianeBriand/SMolDock


text:
//#ifndef AMPAR_GILLESPIE_CLASS_HPP_INCLUDED
//#define AMPAR_GILLESPIE_CLASS_HPP_INCLUDED

#include <iostream>
#include <stdlib.h>
#include <cmath>
#include <vector>
#include <ctime>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/linear_congruential.hpp>

#include "astron_utility_functions.hpp"
#include "stl_vector_operation_functions.hpp"
#include "boost_vector_matrix_operation_functions.hpp"

namespace bno=boost::numeric::odeint;
namespace bnu=boost::numeric::ublas;

typedef double value_type;
typedef bnu::matrix< value_type > matrix_type;

class stochastic_ampar
{
   private:
      unsigned int seed;
      boost::minstd_rand rng;   //Boost random number generator
      boost::uniform_real<double> uni_dist;   //Uniform random number distribution which produces values b/w 0 and 1 (0 inclusive, 1 exlusive).
      boost::variate_generator<boost::minstd_rand&, boost::uniform_real< double> > uni_rand;// variate generator
      //--------------
      double kf1 = 4.59E06;    //M^-1 S^-1
      double kb1 = 4.26E03;    //S^-1
      double kf2 = 28.4E06;    //M^-1 * S^-1
      double kb2 = 3.26E03;    //S^-1
      double kf3 = 1.27E06;    //M^-1 S^-1
      double kb3 = 45.7;       //S^-1
      double a0 = 4.24E03;     //S^-1
      double b0 = 900.0;       //S^-1
      double a1 = 2.89E03;     //S^-1
      double b1 = 39.2;        //S^-1
      double a2 = 172.0;       //S^-1
      double b2 = 0.727;       //S^-1
      double a3 = 17.7;        //S^-1
      double b3 = 4.0;         //S^-1
      double a4 = 16.8;        //S^-1
      double b4 = 190.4;       //S^-1
      double Volume = 1.0;
   public:
      //Construct vectors
      matrix_type X;       // A matrix to hold the states
      matrix_type P;       // A matrix that holds the probability at time t
      matrix_type T;       // The transition probability matrix
      matrix_type Q;       // The infinitly small matrix, Q matrix
      /* stochastic_ampar class constructor */
      stochastic_ampar(matrix_type X_, unsigned int seed_=std::time(0)):   seed(seed_),
                                                                        uni_dist(boost::uniform_real<double>(0,1)),
                                                                        uni_rand(boost::variate_generator<boost::minstd_rand&, boost::uniform_real<double> >(rng, uni_dist)),
                                                                        X(X_)
      {
         rng.seed(seed);
         std::vector<double> Tv;
         Tv = {
      };
      //------------------ Function declarations
      void operator() (  matrix_type &p, matrix_type &dpdt , const double  t )
      {
         dpdt = bnu::prod(p,Q);
      };
};
int main(int argc, char **argv)
{
   matrix_type mat(3,2);
   mat(2,1) = 32;
   stochastic_ampar test(mat);
   std::cout << test.X << std::endl;
}

max_stars_repo_name: anupgp/astron

max_issues_repo_name: anupgp/astron

max_forks_repo_name: anupgp/astron


text:
// vim:set ts=4 sw=4 et:
//
#include <cstdio>
#include <cassert>
#include <cstring>
#include <cstdlib>
#include <iostream>
#include <list>
#include <boost/chrono.hpp>
#include <boost/format.hpp>
#include <unistd.h>

class passe_temps
{
    typedef boost::chrono::high_resolution_clock hr_clock;

    struct element
    {
        void    *ctx;
        const char *nom;
        size_t  calls;
        hr_clock::duration elapsed;
        hr_clock::time_point start;
    };

    // exercice: remplacer la list par une map
    typedef std::list<element>  liste;

    void        *ctx_;
    const char  *nom_;

    liste::iterator    last_active_;       // la mesure active précédente
    liste::iterator    current_active_;    // ce qu'on mesure

    static liste stack_;                    // la mesure active
    static liste::iterator active_;         // toutes les mesures

public:
    passe_temps(const char *nom, void *ctx)
    {
        hr_clock::time_point now = hr_clock::now();

        nom_ = nom;
        ctx_ = ctx;
        last_active_ = active_;

        if (active_ != stack_.end())
        {
            //std::cout << "suspend " << active_->nom << std::endl;

            active_->elapsed += now - active_->start;
            active_->start = hr_clock::time_point::min();
        }

        for (active_ = stack_.begin(); active_ != stack_.end(); ++active_)
        {
            if (active_->ctx == ctx_ && active_->nom == nom_)
            {
                assert(active_->start == hr_clock::time_point::min());
                //std::cout << "restart " << active_->nom << std::endl;

                ++active_->calls;
                active_->start = now;

                current_active_ = active_;
                return;
            }
        }

        element el;
        el.nom = nom;
        el.ctx = ctx;
        el.start = now;
        el.calls = 1;
        el.elapsed = hr_clock::duration::zero();

        stack_.push_back(el);
        active_ = stack_.end();
        --active_;

        current_active_ = active_;

        //std::cout << "start " << current_active_->nom << std::endl;
    }

    ~passe_temps()
    {
        hr_clock::time_point now = hr_clock::now();

        current_active_->elapsed += now - current_active_->start;
        current_active_->start = hr_clock::time_point::min();

        nom_ = 0;
        ctx_ = 0;

        //std::cout << "finish " << current_active_->nom << std::endl;

        active_ = last_active_;

        if (active_ != stack_.end())
        {
            //std::cout << "resume " << active_->nom << std::endl;
            active_->start = now;
        }
    }

    static void print()
    {
        std::cout << std::endl;
        std::cout << "mesures:" << std::endl;
        for (auto& i : stack_)
        {
            double d = i.elapsed.count() * ((double)hr_clock::period::num / hr_clock::period::den);
            std::cout << boost::format("  %-6s: %4s call(s)  %8.6f s   %7.3f ms/call") % i.nom % i.calls % d % (d / i.calls * 1000.) << std::endl;
        }
    }
};

passe_temps::liste passe_temps::stack_;
passe_temps::liste::iterator passe_temps::active_ = passe_temps::stack_.end();

// simulation imbriquée
//
class mikado
{
    int n = 0;
    int c1 = 0, c2 = 0, c3 = 0;

public:
    mikado()
    {
        run("A");

        std::cout << std::endl;
        std::cout << "stats d'appel:" << std::endl;
        std::cout << "  run = " << c1 << std::endl;
        std::cout << "  f1  = " << c2 << std::endl;
        std::cout << "  f2  = " << c3 << std::endl;
    }

private:
    void attente(int& c, int duree) const
    {
        ++c;

        // 10 ms
        usleep(1000 * duree);
    }

    void run(const char *nom)
    {
        passe_temps mm("run", this);

        std::cout << std::string(n * 2, ' ') << " run :";

        ++n;

        attente(c1, 11);

        if (strpbrk(nom, "ABC"))
        {
            passe_temps mm("f1", this);
            f1(nom);
        }
        else
        {
            passe_temps mm("f2", this);
            f2(nom);
        }

        --n;
    }

    void f1(const char *nom)
    {
        std::cout << " f1 " << nom << std::endl;
        attente(c2, 13);

        if (n >= 9) return;

        if (!strcmp(nom, "A")) run("B");
        run("y");
        if (!strcmp(nom, "B")) run("C");
        run("z");
        if (!strcmp(nom, "C")) run("D");
    }

    void f2(const char *nom)
    {
        std::cout << " f2 " << nom << std::endl;
        attente(c3, 17);

        if (strpbrk(nom, "xyz")) return;

        run("A");
    }
};

void imbrique()
{
    mikado   m;
}

void simple2()
{
    // crée un autre point de mesure, le précédent est suspendu
    passe_temps m("100ms", NULL);

    usleep(100000);
}

void simple()
{
    // crée un point de mesure: on mesure le temps passé dans la fonction
    // jusqu'à en sortir, en excluant les autres fonctions chronométrées
    passe_temps m("500ms", NULL);

    usleep(500000);
    simple2();
}

int main()
{
    simple();
    imbrique();

    // résultats
    passe_temps::print();

    return 0;
}


text:
#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here

  // number of edges
  int N = mesh.NumEntities(1);
  // number of vertices
  int M = mesh.NumEntities(2);

  Eigen::SparseMatrix<int, Eigen::RowMajor> G(N,M);

  G.reserve(Eigen::VectorXi::Constant(N, 2));

  for(int i = 0; i < N; i++) {
    auto vertices = mesh.Entities(1)[i]->SubEntities(1);
    G.coeffRef(i, mesh.Index(*vertices[0])) = 1;
    G.coeffRef(i, mesh.Index(*vertices[1])) = -1;
  }

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {

  // number of cells
  int N = mesh.NumEntities(0);
  // number of edges
  int M = mesh.NumEntities(1);

  Eigen::SparseMatrix<int, Eigen::RowMajor> D(N,M);

  D.reserve(Eigen::VectorXi::Constant(N, 4));

  for(int i = 0; i < N; i++) {

    auto edges = mesh.Entities(0)[i]->SubEntities(1);

    for(int k = 0; k < edges.size(); k++) {
      D.coeffRef(i, mesh.Index(*edges[k])) = lf::mesh::to_sign(mesh.Entities(0)[i]->RelativeOrientations()[k]);
    }

  }

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  return (computeCellEdgeIncidenceMatrix(mesh) * computeEdgeVertexIncidenceMatrix(mesh)).norm() == 0;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices

ext: cc


text:
#include <iostream>
#include <opencv2/opencv.hpp>
#include <cmath>
#include <random>
#include <limits>

#include "GRANSAC.hpp"
#include "least_squares_model.hpp"

#include <vector>
#include <Eigen/QR>

using namespace std;

int main(int argc, char *argv[])
{
    if (argc != 1 && argc != 3)
    {
        std::cout << "[ USAGE ]: " << argv[0] << " [<Image Size> = 1000] [<nPoints> = 500]" << std::endl;
        return -1;
    }

    int side = 400;
    int n_points = 50;
    if (argc == 3)
    {
        side = std::atoi(argv[1]);
        n_points = std::atoi(argv[2]);
    }

    cv::Mat img_canvas(side, side, CV_8UC3);
    img_canvas.setTo(255);

    // Randomly generate points in a 2D plane roughly aligned in a line for testing
    std::random_device seed_device;
    std::mt19937 RNG = std::mt19937(seed_device());

    std::uniform_int_distribution<int> uni_dist(0, side - 1); // [Incl, Incl]
    int perturb = 40;
    std::normal_distribution<float> perturb_dist(0, perturb);

    std::vector<std::shared_ptr<GRANSAC::AbstractParameter>> cand_points;
    while (n_points > 0){
        int diag = uni_dist(RNG);
        float x = diag + perturb_dist(RNG);
        // y = 2100-20x+0.05*x*x
        float y;
        if (n_points > 5 )
            y = 2100 - 20 * x + 0.05 * x * x + perturb_dist(RNG);
        else
            y = uni_dist(RNG);

        if (x<=side && x > 0 && y<=side && y > 0){
            cv::Point pt(floor(x), floor(y));
            cv::circle(img_canvas, pt, floor(side / 100) + 2, cv::Scalar(0, 0, 0), 2, cv::LINE_AA);

            std::shared_ptr<GRANSAC::AbstractParameter> cand_pt = std::make_shared<GRANSAC::Point2D>(pt.x, pt.y);
            cand_points.push_back(cand_pt);

            n_points -= 1;
        }
    }

    //set grid
    std::map<string, float> additional_params = {{"img_width", float(side)}, {"img_height", float(side)}, {"grid_num_x", float(side/10)}, {"grid_num_y", float(side/10)}};

    //draw grid
    cv::Mat img_overlay;
    float alpha = 0.3;
    img_canvas.copyTo(img_overlay);
    for (int i=0; i < side/10; i++){
        cv::Point pt_1(i*10, 0);
        cv::Point pt_2(i*10, side);
        cv::line(img_overlay, pt_1, pt_2, cv::Scalar(0, 100, 0), 1, cv::LINE_AA, 0);
        cv::Point pt_3(0, i*10);
        cv::Point pt_4(side, i*10);
        cv::line(img_overlay, pt_3, pt_4, cv::Scalar(0, 100, 0), 1, cv::LINE_AA, 0);
    }
    cv::addWeighted(img_overlay, alpha, img_canvas, 1 - alpha, 0, img_canvas);

    // estimate parameters with RANSAC
    int param_num = 4; // 3rd order
    additional_params["sample_number"]=10; // sample number for over-deterministic least squares
    GRANSAC::RANSAC<GRANSAC::LinearLeastSquaresModel<4>, 4> estimator;
    estimator.initialize(1, 100, additional_params); // Threshold, iterations, the threshold is p-1 distance on grid, 1 means 1 grid cell distance

    float time_average = 0;
    for (int i=0; i<100; i++){
        int start = cv::getTickCount();
        estimator.estimate(cand_points);
        int end = cv::getTickCount();
        float time_of_trial = float(end - start) / float(cv::getTickFrequency()) * 1000.0; // [ms]
        time_average += time_of_trial;
    }
    std::cout << "RANSAC took, average time in 100 trials: " << time_average/100 << " ms." << std::endl;

    // draw definition points from best model
    auto best_inliers = estimator.getBestInliers();
    if (best_inliers.size() > 0)
    {
        for (auto &inlier : best_inliers)
        {
            auto RPt = std::dynamic_pointer_cast<GRANSAC::Point2D>(inlier);
            cv::Point pt(floor(RPt->m_point2D[0]), floor(RPt->m_point2D[1]));
            cv::circle(img_canvas, pt, floor(side / 100), cv::Scalar(0, 255, 0), -1, cv::LINE_AA);
        }
    }

    // draw curve from best model
    auto best_line = estimator.getBestModel();
    if (best_line)
    {
        cout << "coefficients are : " << endl;
        for (auto each : best_line->getModelCoefficients()){
            cout << each << ", ";
        }
        cout << endl;

        std::vector<float> x_values, y_values, coeff;
        for (int i=0; i<best_line->getModelDefParams().size(); i++){
            auto best_line_pt = std::dynamic_pointer_cast<GRANSAC::Point2D>(best_line->getModelDefParams()[i]);
            x_values.push_back(best_line_pt->m_point2D[0]); //x
            y_values.push_back(best_line_pt->m_point2D[1]); //x
            cv::Point pt(floor(best_line_pt->m_point2D[0]), floor(best_line_pt->m_point2D[1]));
            cv::circle(img_canvas, pt, floor(side / 100), cv::Scalar(0, 0, 255), -1, cv::LINE_AA);
        }

        // draw polynomial line
        float start_point_x = 0;
        float end_point_x = side;
        vector<cv::Point2f> curve_points;
        //Define the curve through equation. In this example, a simple parabola
        for (float x = start_point_x; x <= end_point_x; x+=1){
            float y = 0;
            for (int j = 0; j < best_line->getModelCoefficients().size(); j++){
                y += best_line->getModelCoefficients()[j]*std::pow(x, float(j));
            }

            cv::Point2f new_point = cv::Point2f(x, y);                  //resized to better visualize
            curve_points.push_back(new_point);                       //add point to vector/list
        }
        for (int i = 0; i < curve_points.size() - 1; i++){
            cv::line(img_canvas, curve_points[i], curve_points[i + 1], cv::Scalar(0,255,0), 2, CV_AA);
        }
    }

    while (true)
    {
        cv::imshow("RANSAC Example", img_canvas);
        char Key = cv::waitKey(1);
        if (Key == 27)
            return 0;
    }

    return 0;
}

max_stars_repo_name: masszhou/GRANSAC

max_issues_repo_name: masszhou/GRANSAC

max_forks_repo_name: masszhou/GRANSAC