text:
#include "IcpNode.h"
#include <boost/smart_ptr/make_shared_array.hpp>
#include <mrs_lib/param_loader.h>
#include <filesystem>
#include "tools.h"
#include "static_cloud.h"

// Matous code https://mrs.felk.cvut.cz/gitlab/vrbamato/uav_detect/blob/ouster/src/pcl_selfloc_nodelet.cpp#L420

namespace icp_node {
    void IcpNode::callback_cloud(const pc_XYZ_t::ConstPtr &msg) {
        if (!m_apriori_map_initialized) {
            NODELET_WARN_THROTTLE(1.0, "[PCLSelfLocalizator]: Waiting for intialization of the apriori static map.");
            return;
        }
        pcl::ScopeTime t1("callback");
        pc_XYZ_t::Ptr input_pt_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud(*msg.get(), *input_pt_cloud);

        tools_box_filter_cloud(input_pt_cloud, 0.4, true);
        tools_box_filter_cloud(input_pt_cloud, 20, false);

        tools_voxel_filter_cloud(input_pt_cloud, 0.5);

        process_cloud(input_pt_cloud);
        pcl_conversions::toPCL(ros::Time::now(), m_static_cloud->header.stamp);
        m_pub_static_pc.publish(m_static_cloud);
    }

    // in robotics, "position" typically only means a point in space, whereas "pose" includes orientation
    std::pair<float, float> compare_two_poses(const Eigen::Affine3f &source, const Eigen::Affine3f &target) {
        const float translation_diff = (source.translation() - target.translation()).norm();
        const float angle_diff = Eigen::AngleAxisf().fromRotationMatrix(
                source.rotation().inverse() * target.rotation()).angle();
        return {translation_diff, angle_diff};
    }

    void IcpNode::process_cloud(const pc_XYZ_t::ConstPtr &msg_input_cloud) {
        std::lock_guard<std::mutex> lock(m_processing_mutex);
        if (m_origin_cloud == nullptr) {
            m_origin_cloud = msg_input_cloud;
            m_previous_cloud = msg_input_cloud;
            m_origin_gt_pose = m_latest_gt_pose;
        } else {
            if (m_algorithm_type == "iterative") {
                iterative_approach(msg_input_cloud);
            } else if (m_algorithm_type == "simple") {
                straight_forward_approach(msg_input_cloud);
            } else {
                std::cout << "unknown algorithm type. starting simple one\n";
                straight_forward_approach(msg_input_cloud);
            }
        }
    }

    void IcpNode::onInit() {
        ros::NodeHandle nh = nodelet::Nodelet::getMTPrivateNodeHandle();

        mrs_lib::ParamLoader pl(nh);
        std::string static_cloud_filename;
        NODELET_INFO("Loading static parameters:");
        pl.loadParam("uav_name", m_uav_name);
        pl.loadParam("map_frame_id", m_map_frame_id);
        pl.loadParam("algorithm_t", m_algorithm_type);
        pl.loadParam("icp/max_correspondence_distance", m_icp_max_corr_dist);
        pl.loadParam("icp/euclidean_fitness_epsilon", m_icp_fitness_eps);
        pl.loadParam("icp/transformation_epsilon", m_icp_tf_eps);
        pl.loadParam("icp/maximum_iterations", m_icp_max_its);
        pl.loadParam("static_cloud_filename", static_cloud_filename);
//        auto static_cloud_filename = pl.loadParam2<std::string>("icp/static_cloud_filename");

        // handle missing parameters
        if (!pl.loadedSuccessfully()) {
            NODELET_ERROR("Some compulsory parameters were not loaded successfully, ending the node.");
            ros::requestShutdown();
            return;
        }

        m_pub_orig = nh.advertise<pc_XYZ_t>("aligned_orig", 1);
        m_pub_last = nh.advertise<pc_XYZ_t>("aligned_prev", 1);
        m_pub_static_pc = nh.advertise<sensor_msgs::PointCloud2>("static_pc", 1, true);
        m_sub_pc = nh.subscribe("points_in", 1, &IcpNode::callback_cloud, this);
        m_sub_position = nh.subscribe("ground_truth", 1, &IcpNode::callback_pose, this);
        m_apriori_map_initialized = false;

        initialize_static_map(static_cloud_filename);
    }

    [[maybe_unused]] void IcpNode::straight_forward_approach(const pc_XYZ_t::ConstPtr &msg_input_cloud) {
        auto aligned_pc = boost::make_shared<pc_XYZ_t>();
        pcl::copyPointCloud(*msg_input_cloud.get(), *aligned_pc);
        m_global_transformation = pair_align(msg_input_cloud, m_origin_cloud, *aligned_pc);

        {
            geometry_msgs::TransformStamped msg;
            pcl_conversions::fromPCL(msg_input_cloud->header.stamp, msg.header.stamp);
            msg.header.frame_id = m_uav_name + "/local_origin";
            msg.child_frame_id = m_uav_name + "/icp_origin";
            msg.transform = tf2::eigenToTransform(
                    static_cast<const Eigen::Affine3d>(m_global_transformation.inverse())).transform;
            m_tf_broadcaster.sendTransform(msg);
        }

        if (m_pub_orig.getNumSubscribers() > 0) {
            pc_XYZ_t::Ptr tfd_orig_pc = boost::make_shared<pc_XYZ_t>();
            pcl::transformPointCloud(*m_origin_cloud, *tfd_orig_pc, m_global_transformation.inverse());
            tfd_orig_pc->header.stamp = msg_input_cloud->header.stamp;
            m_pub_orig.publish(tfd_orig_pc);
        }

        if (m_pub_last.getNumSubscribers() > 0) {
            aligned_pc->header.stamp = msg_input_cloud->header.stamp;
            aligned_pc->header.frame_id = m_uav_name + "/local_origin";
            m_pub_last.publish(aligned_pc);
        }
//        m_previous_cloud = msg_input_cloud;
        const Eigen::Affine3d orig2cur_tf_gt = m_origin_gt_pose.inverse() * m_latest_gt_pose;
        const auto epsilon = compare_two_poses(static_cast<const Eigen::Affine3f>(m_global_transformation),
                                               static_cast<const Eigen::Affine3f>(orig2cur_tf_gt));

        std::cout << "GT translation:  " << orig2cur_tf_gt.translation().transpose() << "\n";
        std::cout << "ICP translation: " << m_global_transformation.translation().transpose() << "\n";
        std::cout << "\ttransformation error is: " << epsilon.first << "m.\n"
                  << "\trotation error is: " << epsilon.second / M_PI * 180.0 << "deg.\n";
    }

    [[maybe_unused]] void IcpNode::iterative_approach(const pc_XYZ_t::ConstPtr &msg_input_cloud) {
        // Find the transformation to align the current message to the previous cloud
        auto aligned_input_cloud = boost::make_shared<pc_XYZ_t>();
        Eigen::Affine3d align_transformation = pair_align(msg_input_cloud, m_previous_cloud, *aligned_input_cloud);
        // update the estimated global TF
        m_global_transformation = align_transformation * m_global_transformation;

        // Publish the latest tf2 transformation
        {
            geometry_msgs::TransformStamped msg;
            pcl_conversions::fromPCL(msg_input_cloud->header.stamp, msg.header.stamp);
            msg.header.frame_id = m_uav_name + "/local_origin";
            msg.child_frame_id = m_uav_name + "/icp_origin";
            msg.transform = tf2::eigenToTransform(
                    static_cast<const Eigen::Affine3d>(m_global_transformation.inverse())).transform;
            m_tf_broadcaster.sendTransform(msg);
        }

        // publish some debug aligned pointclouds if requested
        if (m_pub_last.getNumSubscribers() > 0) {
            aligned_input_cloud->header.stamp = msg_input_cloud->header.stamp;
            m_pub_last.publish(aligned_input_cloud);
        }

        if (m_pub_orig.getNumSubscribers() > 0) {
            pc_XYZ_t::Ptr tfd_orig_pc = boost::make_shared<pc_XYZ_t>();
            pcl::transformPointCloud(*m_origin_cloud, *tfd_orig_pc, m_global_transformation.inverse());
            tfd_orig_pc->header.stamp = msg_input_cloud->header.stamp;
            m_pub_orig.publish(tfd_orig_pc);
        }

        // ground-truth transformation from origin (first received pose) to the current UAV pose
        const Eigen::Affine3d orig2cur_tf_gt = m_origin_gt_pose.inverse() * m_latest_gt_pose;
        const auto epsilon = compare_two_poses(static_cast<const Eigen::Affine3f>(m_global_transformation),
                                               static_cast<const Eigen::Affine3f>(orig2cur_tf_gt));

        std::cout << "GT translation:  " << orig2cur_tf_gt.translation().transpose() << "\n";
        std::cout << "ICP translation: " << m_global_transformation.translation().transpose() << "\n";
        std::cout << "\ttransformation error is: " << epsilon.first << "m.\n"
                  << "\trotation error is: " << epsilon.second / M_PI * 180.0 << "deg.\n";
        // update the previous cloud message
        m_previous_cloud = msg_input_cloud;
    }

    Eigen::Affine3d IcpNode::pair_align(const pc_XYZ_t::ConstPtr &src,
                                        const pc_XYZ_t::ConstPtr &tgt,
                                        pc_XYZ_t &res) {
        // the clouds are already filtered - this is redundant
        pcl::IterativeClosestPointNonLinear<p_XYZ_t, p_XYZ_t> icp;

        // parametrize stuff like this
        icp.setMaxCorrespondenceDistance(m_icp_max_corr_dist);
        icp.setEuclideanFitnessEpsilon(m_icp_fitness_eps);
        icp.setTransformationEpsilon(m_icp_tf_eps);
        icp.setMaximumIterations(m_icp_max_its);

        // Align
        icp.setInputSource(src);
        icp.setInputTarget(tgt);

        icp.align(res);

        return Eigen::Affine3d(icp.getFinalTransformation().cast<double>());
    }

    void IcpNode::initialize_static_map(const std::string &filename) {
        const pc_XYZ_t::Ptr loaded_cloud = load_cloud(filename);
        if (loaded_cloud == nullptr) {
            NODELET_ERROR("Failed to load the static pointcloud! Ending the node.");
            ros::shutdown();
            return;
        } else {
            m_static_cloud = loaded_cloud;
            NODELET_INFO("Loaded a static cloud with %lu points.", m_static_cloud->size());

            pc_XYZ_t::Ptr tmp_cloud = boost::make_shared<pc_XYZ_t>();

            tools_voxel_filter_cloud(m_static_cloud, m_apriori_map_voxelgrid_leaf_size);

            //pcl::VoxelGrid<p_XYZ_t> vg;

            NODELET_INFO("Downsampled the static cloud to %lu points.", m_static_cloud->size());

            const Eigen::Vector3f translation =
                    Eigen::Vector3f(m_apriori_map_tf_x, m_apriori_map_tf_y, m_apriori_map_tf_z) +
                    Eigen::Vector3f(m_apriori_map_correction_x, m_apriori_map_correction_y, m_apriori_map_correction_z);

            const Eigen::Matrix3f rotation = Eigen::AngleAxisf(static_cast<float>(m_apriori_map_tf_yaw / 180.0 * M_PI),
                                                               Eigen::Vector3f::UnitZ()).toRotationMatrix();
            Eigen::Affine3f corr_tf = Eigen::Affine3f::Identity();
            corr_tf.rotate(rotation);
            corr_tf.translate(translation);

            pc_XYZ_t::Ptr tfd_static_cloud = boost::make_shared<pc_XYZ_t>();      // contains points, which are apriori known to be a part of the static background
            pcl::transformPointCloud(*m_static_cloud, *tfd_static_cloud, corr_tf);
            tfd_static_cloud->header.frame_id = m_map_frame_id;

            const ros::Time stamp = ros::Time::now();
            pcl_conversions::toPCL(stamp, tfd_static_cloud->header.stamp);
            //tfd_static_cloud->header.frame_id = m_uav_name + "/local_origin";
            m_static_cloud = tfd_static_cloud;
        }
        m_apriori_map_initialized = true;
    }

    // in robotics, "position" typically only means a point in space, whereas "pose" includes orientation
    void IcpNode::callback_pose(const nav_msgs::Odometry::ConstPtr &msg) {
        std::lock_guard<std::mutex> lock(m_processing_mutex);
        tf2::fromMsg(msg->pose.pose, m_latest_gt_pose);
    }

}  // namespace icp_node

PLUGINLIB_EXPORT_CLASS(icp_node::IcpNode, nodelet::Nodelet);


text:
/*
 DepSpawn: Data Dependent Spawn library
 Copyright (C) 2012-2021 Carlos H. Gonzalez, Basilio B. Fraguela. Universidade da Coruna

 Distributed under the MIT License. (See accompanying file LICENSE)
*/

///
/// \author   Carlos H. Gonzalez  <cgonzalezv@udc.es>
/// \author   Basilio B. Fraguela <basilio.fraguela@udc.es>
///

#include <iostream>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <chrono>
#include "depspawn/depspawn.h"
#include "common_io.cpp"  // This is only for serializing parallel prints

using namespace depspawn;

//#define spawn(f,a...) f(a)

//TODO: Cuando F funcione, desmarcar el spawn de g

/////////////////////////////////////////////////////////

#include "common_A.cpp"

/////////////////////////////////////////////////////////

std::chrono::time_point<std::chrono::high_resolution_clock> t0;

//This function runs for long
void f(int &i) {
  LOG("f begin: " << std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t0).count());

  { // This is just used to make a delay
    A a(400);

    a.do_comp();
  }

  i = 10;

  LOG("f finish: " << std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t0).count() << " with i=" << i);
}

void h(int &i, int s) {
  LOG("h begin: " << std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t0).count() << " with i=" << i << " +=" << s);

  i += s;

  LOG("h finish: " << std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t0).count() << " with i=" << i);
}

void g(int *ptr) {
  LOG("g begin: " << std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t0).count());

  boost::function<void(int&)> binded_h = boost::bind(h, _1, 10);

  //binded_h(*ptr); //Test to see that the function works

  spawn(binded_h, (*ptr));

  LOG("g finish: " << std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t0).count());
}

int main()
{ int i = 0; //i is in the stack

  t0 = std::chrono::high_resolution_clock::now();

  //spawn(f, Ignore<int>(i)); //With this version h does not wait for f

  spawn(f, i);      //f runs for long. Sets i=10

  spawn(g, (&i));   //g gets the argument as a pointer, so it should run in parallel as no dependency is detected

  wait_for_all();

  std::cout << "Final i=" << i << std::endl;

  const bool test_ok = (i == 20);

  std::cout << "TEST " << (test_ok ? "SUCCESSFUL" : "UNSUCCESSFUL") << std::endl;

  return !test_ok;
}

ext: cpp

max_stars_repo_name: fraguela/depspawn

max_issues_repo_name: fraguela/depspawn

max_forks_repo_name: fraguela/depspawn


text:
/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Marc Glisse
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>

#include <boost/range/iterator_range.hpp>

#include <wasserstein.h> // Hera

#include <array>

namespace py = pybind11;
typedef py::array_t<double, py::array::c_style | py::array::forcecast> Dgm;

double wasserstein_distance(
    Dgm d1, Dgm d2,
    double wasserstein_power, double internal_p,
    double delta)
{
  py::buffer_info buf1 = d1.request();
  py::buffer_info buf2 = d2.request();
  // shape (n,2) or (0) for empty
  if((buf1.ndim!=2 || buf1.shape[1]!=2) && (buf1.ndim!=1 || buf1.shape[0]!=0))
    throw std::runtime_error("Diagram 1 must be an array of size n x 2");
  if((buf2.ndim!=2 || buf2.shape[1]!=2) && (buf2.ndim!=1 || buf2.shape[0]!=0))
    throw std::runtime_error("Diagram 2 must be an array of size n x 2");
  typedef std::array<double, 2> Point;
  auto p1 = (Point*)buf1.ptr;
  auto p2 = (Point*)buf2.ptr;
  auto diag1 = boost::make_iterator_range(p1, p1+buf1.shape[0]);
  auto diag2 = boost::make_iterator_range(p2, p2+buf2.shape[0]);

  hera::AuctionParams<double> params;
  params.wasserstein_power = wasserstein_power;
  // hera encodes infinity as -1...
  if(std::isinf(internal_p)) internal_p = hera::get_infinity<double>();
  params.internal_p = internal_p;
  params.delta = delta;
  // The extra parameters are purposedly not exposed for now.
  return hera::wasserstein_dist(diag1, diag2, params);
}

PYBIND11_MODULE(hera, m) {
      m.def("wasserstein_distance", &wasserstein_distance,
          py::arg("X"), py::arg("Y"),
          py::arg("order") = 1,
          py::arg("internal_p") = std::numeric_limits<double>::infinity(),
          py::arg("delta") = .01,
          R"pbdoc(
        Compute the Wasserstein distance between two diagrams.
        Points at infinity are supported.

        Parameters:
            X (n x 2 numpy array): First diagram
            Y (n x 2 numpy array): Second diagram
            order (float): Wasserstein exponent W_q
            internal_p (float): Internal Minkowski norm L^p in R^2
            delta (float): Relative error 1+delta

        Returns:
            float: Approximate Wasserstein distance W_q(X,Y)
    )pbdoc");
}

ext: cc

max_stars_repo_name: gtauzin/gudhi-devel

max_issues_repo_name: gtauzin/gudhi-devel

max_forks_repo_name: gtauzin/gudhi-devel


text:
// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
// Check that we can handle large tensors on two tiles
//
#define BOOST_TEST_MODULE NonLinearityTest
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <limits>
#include <poplar/Engine.hpp>
#include <poplar/IPUModel.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/NonLinearity.hpp>
#include <poplibs_test/Util.hpp>
#include <popnn/NonLinearity.hpp>
#include <popnn/codelets.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace popnn;
using namespace poplibs_test::util;
using namespace poplibs_support;

namespace utf = boost::unit_test;
namespace fpc = boost::test_tools::fpc;

#include <iostream>
#include <poplar/Engine.hpp>
#include <poplar/IPUModel.hpp>
#include <poplar/Program.hpp>
#include <popnn/NonLinearity.hpp>
#include <popnn/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <vector>

void testReluWithTensorOfSize(size_t nElms, bool alwaysFits) {
  auto device = createTestDevice(TEST_TARGET, 1, 2);
  const auto &target = device.getTarget();
  const auto tileMemory = target.getBytesPerTile();

  // crude estimate of whether the tensors will fit. This will need updating
  // for devices with different memory sizes
  const bool expectException = !alwaysFits && nElms * 4 > tileMemory;

  std::cout << "Size " << nElms
            << (expectException ? " NOT expected to fit\n"
                                : " expected to fit\n");

  poplar::Graph graph(device.getTarget());
  popnn::addCodelets(graph);
  popops::addCodelets(graph);

  std::vector<float> hPre(nElms);
  std::vector<float> hPost(nElms);
  for (unsigned i = 0; i < nElms; ++i) {
    // set all initial values to -1.0
    hPre[i] = -1.0;
    hPost[i] = 10.0;
  }
  poplar::Tensor dPre = graph.addVariable(poplar::FLOAT, {nElms});

  poputil::mapTensorLinearly(graph, dPre);

  poplar::Tensor dPost = graph.clone(dPre, "dPost");
  poplar::program::Sequence prog;
  poplar::program::Copy copyProg(dPre, dPost);
  prog.add(copyProg);
  popnn::nonLinearityInPlace(graph, popnn::NonLinearityType::RELU, dPost, prog,
                             "Relu");

  graph.createHostWrite("hPre", dPre);
  graph.createHostRead("hPost", dPost);

  try {
    poplar::Engine eng(graph, prog);
    device.bind([&](const Device &d) {
      eng.load(d);
      eng.writeTensor("hPre", hPre.data(), hPre.data() + hPre.size());
      eng.run();
      eng.readTensor("hPost", hPost.data(), hPost.data() + hPost.size());
    });
    BOOST_CHECK(!expectException);
  } catch (poplar::graph_memory_allocation_error &) {
    BOOST_CHECK(expectException);
    return;
  }

  float minVal = hPost[0];
  for (auto &x : hPost) {
    if (x < minVal) {
      minVal = x;
    }
  }
  std::cout << "with " << nElms << " elements, min val : " << minVal
            << std::endl;
  // Test a float is exactly zero without getting a compiler warning:
  BOOST_CHECK(minVal >= -0.f && minVal <= 0.f);
}

BOOST_AUTO_TEST_CASE(BigVectorList) {

  // These may need to be updated for different arch versions or
  // if the code size radically changes:
  std::vector<size_t> sizes({100, 1000, 10000, 100000, 1000000});

  const bool everythingFits =
      isIpuModel(TEST_TARGET) || TEST_TARGET == DeviceType::Cpu;

  for (const auto s : sizes) {
    testReluWithTensorOfSize(s, everythingFits);
  }
}

max_stars_repo_name: graphcore/poplibs


text:
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <Eigen/Eigen>
#include <queue>

using namespace Eigen;
using namespace std;
ros::Publisher twist_pub, debug_pub;
const double interval = 1000.0;
const double dt = 0.01;

int main(int argc, char **argv) {
    ros::init(argc, argv, "visual_ekf_test");
    ros::NodeHandle n("~");

    twist_pub = n.advertise<geometry_msgs::Vector3Stamped>("/visual_ekf/test_twist", 100);
    debug_pub = n.advertise<geometry_msgs::PoseStamped>("/visual_ekf/test_pose_debug", 100);

    ros::Rate r(1 / dt);

    Quaterniond start(1, 0, 0, 0);
    Quaterniond end(1, 2, 3, 100);
    end = end.normalized();

    Quaterniond q = end * start.conjugate();
    Quaterniond q_state = start;
    AngleAxisd R(q);
    double theta  = R.angle();
    Vector3d axis = R.axis();

    auto t_start = ros::Time::now();
    double t_total = interval * dt;
    // theta = theta / t_total;

    while (ros::ok()) {
        auto cur_t = ros::Time::now();
        if ((cur_t.toSec() - t_start.toSec()) < t_total) {
            geometry_msgs::Vector3Stamped twist;
            twist.header.stamp = cur_t;
            twist.vector.x = 0;
            twist.vector.y = 0;
            twist.vector.z = 0;
            twist_pub.publish(twist);

            geometry_msgs::PoseStamped pose;
            pose.header.stamp = cur_t;
            pose.pose.orientation.w = start.w();
            pose.pose.orientation.x = start.x();
            pose.pose.orientation.y = start.y();
            pose.pose.orientation.z = start.z();
            debug_pub.publish(pose);
        }
        else if ((cur_t.toSec() - t_start.toSec()) < t_total * 2) {
            geometry_msgs::Vector3Stamped twist;
            twist.header.stamp = cur_t;
            twist.vector.x = theta * axis(0);
            twist.vector.y = theta * axis(1);
            twist.vector.z = theta * axis(2);
            twist_pub.publish(twist);

            // AngleAxisd dR(d_theta, axis);
            Quaterniond dq;
            dq.w() = cos(0.5 * theta * dt);
            dq.x() = sin(0.5 * theta * dt) * axis(0);
            dq.y() = sin(0.5 * theta * dt) * axis(1);
            dq.z() = sin(0.5 * theta * dt) * axis(2);
            q_state = (q_state * dq).normalized();

            geometry_msgs::PoseStamped pose;
            pose.header.stamp = cur_t;
            pose.pose.orientation.w = q_state.w();
            pose.pose.orientation.x = q_state.x();
            pose.pose.orientation.y = q_state.y();
            pose.pose.orientation.z = q_state.z();
            debug_pub.publish(pose);
        }

        r.sleep();
        ros::spinOnce();
    }
}


text:
/**
 * @file plugin_loader.hpp
 * @brief Plugin Loader to be used throughout Tesseract for loading plugins
 *
 * @author Levi Armstrong
 * @date March 25, 2021
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2021, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * 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
 * @par
 * 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.
 */
#ifndef TESSERACT_COMMON_PLUGIN_LOADER_HPP
#define TESSERACT_COMMON_PLUGIN_LOADER_HPP

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/algorithm/string.hpp>
#include <console_bridge/console.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_common/plugin_loader.h>
#include <tesseract_common/class_loader.h>

namespace tesseract_common
{
inline std::list<std::string> parseEnvironmentVariableList(const std::string& env_variable)
{
  std::list<std::string> list;
  std::string evn_str = std::string(std::getenv(env_variable.c_str()));
  boost::split(list, evn_str, boost::is_any_of(":"), boost::token_compress_on);
  return list;
}

inline std::list<std::string> getAllSearchPaths(const std::string& search_paths_env,
                                                const std::list<std::string>& existing_search_paths)
{
  // Check for environment variable to override default library
  if (!search_paths_env.empty())
  {
    std::list<std::string> search_paths = parseEnvironmentVariableList(search_paths_env);
    search_paths.insert(search_paths.end(), existing_search_paths.begin(), existing_search_paths.end());
    return search_paths;
  }

  return existing_search_paths;
}

inline std::unordered_map<std::string, std::string> parseEnvironmentVariableMap(const std::string& env_variable)
{
  std::unordered_map<std::string, std::string> plugins;

  // Parse list first
  std::list<std::string> list;
  std::string evn_str = std::string(std::getenv(env_variable.c_str()));
  boost::split(list, evn_str, boost::is_any_of(":"), boost::token_compress_on);

  // Parse pairs
  for (const auto& pair : list)
  {
    std::vector<std::string> plugin_pair;
    boost::split(plugin_pair, pair, boost::is_any_of(","), boost::token_compress_on);
    if (plugin_pair.size() == 2)
    {
      plugins[plugin_pair[0]] = plugin_pair[1];
    }
    else
    {
      CONSOLE_BRIDGE_logWarn("Failed to parse information for plugin from environment variable: %s", pair.c_str());
    }
  }
  return plugins;
}

inline std::unordered_map<std::string, std::string>
getAllPlugins(const std::string& plugins_env, const std::unordered_map<std::string, std::string>& existing_plugins)
{
  // Check for environment variable to override default library
  if (!plugins_env.empty())
  {
    std::unordered_map<std::string, std::string> plugins = parseEnvironmentVariableMap(plugins_env);
    plugins.insert(existing_plugins.begin(), existing_plugins.end());
    return plugins;
  }

  return existing_plugins;
}

template <class PluginBase>
std::shared_ptr<PluginBase> PluginLoader::instantiate(const std::string& plugin_name) const
{
  // Check for environment variable for plugin definitions
  std::unordered_map<std::string, std::string> plugins_local = getAllPlugins(plugins_env, plugins);
  auto it = plugins_local.find(plugin_name);
  if (it == plugins_local.end())
  {
    CONSOLE_BRIDGE_logError("Failed to find information for plugin: %s", plugin_name.c_str());
    return nullptr;
  }

  // Check for environment variable for search paths
  std::list<std::string> search_paths_local = getAllSearchPaths(search_paths_env, search_paths);
  for (const auto& path : search_paths_local)
  {
    if (ClassLoader::isClassAvailable(plugin_name, it->second, path))
      return ClassLoader::createSharedInstance<PluginBase>(plugin_name, it->second, path);
  }

  // If not found in any of the provided search paths then search system folders if allowed
  if (search_system_folders)
  {
    if (ClassLoader::isClassAvailable(plugin_name, it->second))
      return ClassLoader::createSharedInstance<PluginBase>(plugin_name, it->second);
  }

  CONSOLE_BRIDGE_logError("Failed to instantiate plugin '%s' from library: %s",
                          plugin_name.c_str(),
                          ClassLoader::decorate(it->second).c_str());
  return nullptr;
}

bool PluginLoader::isPluginAvailable(const std::string& plugin_name) const
{
  // Check for environment variable for plugin definitions
  std::unordered_map<std::string, std::string> plugins_local = getAllPlugins(plugins_env, plugins);
  auto it = plugins_local.find(plugin_name);
  if (it == plugins_local.end())
    return false;

  // Check for environment variable to override default library
  std::list<std::string> search_paths_local = getAllSearchPaths(search_paths_env, search_paths);
  for (const auto& path : search_paths_local)
  {
    if (ClassLoader::isClassAvailable(plugin_name, it->second, path))
      return true;
  }

  // If not found in any of the provided search paths then search system folders if allowed
  if (search_system_folders)
    return ClassLoader::isClassAvailable(plugin_name, it->second);

  return false;
}

int PluginLoader::count() const { return static_cast<int>(getAllPlugins(plugins_env, plugins).size()); }

}  // namespace tesseract_common

#endif  // TESSERACT_COMMON_PLUGIN_LOADER_HPP

ext: hpp

max_stars_repo_name: jf---/tesseract

max_issues_repo_name: jf---/tesseract

max_forks_repo_name: jf---/tesseract


text:
//  Copyright (c) 2005-2009 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#if !defined(SAGA_IMPL_ENGINE_SYNC_ASYNC_HPP)
#define SAGA_IMPL_ENGINE_SYNC_ASYNC_HPP

#include <boost/assert.hpp>

#include <saga/impl/engine/cpi_info.hpp>

#include <saga/saga/error.hpp>
#include <saga/saga/util.hpp>

#include <saga/impl/engine/adaptor_selector_state.hpp>

///////////////////////////////////////////////////////////////////////////////
namespace saga { namespace detail
{
    // store the selector state to be used with this task instance
    SAGA_EXPORT void set_selector_state(saga::task t,
        TR1::shared_ptr<impl::adaptor_selector_state> state);
}}

///////////////////////////////////////////////////////////////////////////////
namespace saga { namespace impl
{
    // in general:
    // Adaptors can provide their API functions synchronously or asynchronously
    // or both of them.
    // Towards the application, the engine has to provide all api functions
    // synchronously, asynchronously and as a Task (asynchronously but not yet
    // started).
    // The following code dispatches between those possibilities.

    ///////////////////////////////////////////////////////////////////////////
    //  need sync, have sync
    //  The application wants a synchronous call, the adaptor provides a
    //  synchronous one. So there is nothing to do, we have only to call the
    //  function.
    //  If the sync call fails, the exception is simply passed upwards
    template <typename Cpi, typename Base, typename RetVal>
    inline saga::task
    sync_sync (TR1::shared_ptr<Cpi> cpi, void (Base::*sync)(RetVal&))
    {
        saga::task t(saga::task::Done);
        (cpi.get()->*sync)(t.get_result<RetVal>());
        return t;
    }

    ///////////////////////////////////////////////////////////////////////////
    //  Just as above, but for void methods
    template <typename Cpi, typename Base>
    inline saga::task
    sync_sync (TR1::shared_ptr<Cpi> cpi, void (Base::*sync)(saga::impl::void_t&))
    {
        saga::task t(saga::task::Done);
        saga::impl::void_t void_result;

        (cpi.get()->*sync)(void_result);

        return t;
    }

    ///////////////////////////////////////////////////////////////////////////
    //  need async, have sync
    //  The application wants a asynchronous call, but the adaptor provides
    //  only a synchronous one. So the call is executed in a thread, and by
    //  that way made asynchronous.
    template <typename Cpi, typename Base, typename RetVal>
    inline saga::task
    async_sync (proxy *prxy,
                TR1::shared_ptr<Cpi> cpi,
                TR1::shared_ptr<adaptor_selector_state> state,
                void (Base::*sync)(RetVal&),
                bool (Base::*prep)(RetVal&, saga::uuid))
    {
        saga::adaptors::task t(state->get_op_name(), cpi, prxy, sync, prep);
        detail::set_selector_state(t, state);
        return t;
    }

    ///////////////////////////////////////////////////////////////////////////
    //  need sync, have async
    //  The application wants a synchronous call, the adaptor provides an
    //  asynchronous one. So, we wait for the call to finish.
    template <typename Cpi, typename Base>
    inline saga::task
    sync_async (TR1::shared_ptr<Cpi> cpi, saga::task (Base::*async)())
    {
        return saga::detail::run_wait((cpi.get()->*async)());
    }

    ///////////////////////////////////////////////////////////////////////////
    //  need async, have async
    //  The application wants an asynchronous call, the adaptor provides an
    //  asynchronous one. So there is nothing to do, we have only to call the
    //  function.
    template <typename Cpi, typename Base>
    inline saga::task
    async_async (TR1::shared_ptr<Cpi> cpi,
                 TR1::shared_ptr<adaptor_selector_state> state,
                 saga::task (Base::*async)())
    {
        saga::task t ((cpi.get()->*async)());
        detail::set_selector_state(t, state);
        return t;
    }

    ///////////////////////////////////////////////////////////////////////////
    //  dispatching to the appropriate functions defined above.
    template <typename Base, typename RetVal>
    inline saga::task
    dispatch_sync (run_mode     mode,
                   char const * name,
                   TR1::shared_ptr<v1_0::cpi> cpi_instance,
                   void       (Base::*sync)(RetVal&),
                   saga::task (Base::*async)())
    {
        TR1::shared_ptr<Base> c (TR1::static_pointer_cast<Base>(cpi_instance));

        switch (mode) {
        case Sync_Sync:
            return sync_sync(c, sync);

        case Sync_Async:
            return sync_async(c, async);

        case Async_Sync:
        case Async_Async:
            BOOST_ASSERT(false);
            break;

        default:
            break;
        }

        // no adaptor found (Invalid mode)!
        SAGA_THROW_VERBATIM(cpi_instance.get(),
                            std::string ("No adaptor implements method: ") + name,
                            adaptors::NoAdaptor);

        return saga::task(saga::task_base::Done);
    }

    ///////////////////////////////////////////////////////////////////////////
    //  dispatching to the appropriate functions defined above.
    template <typename Base, typename RetVal>
    inline saga::task
    dispatch_async (proxy *prxy,
                    TR1::shared_ptr<adaptor_selector_state> state,
                    void (Base::*sync_)(RetVal &), saga::task (Base::*async_)(),
                    bool (Base::*prep_)(RetVal &, saga::uuid) = NULL)
    {
        typedef void       (Base::*sync_func )(RetVal&);
        typedef saga::task (Base::*async_func)();
        typedef bool       (Base::*prep_func )(RetVal&, saga::uuid);

        void       (Base::*sync )() = NULL;
        saga::task (Base::*async)() = NULL;
        bool       (Base::*prep )() = NULL;

        run_mode mode = Unknown;

        TR1::shared_ptr<Base> c (
            state->get_next_cpi(mode, &sync, &async, &prep));

//      BOOST_ASSERT(NULL == sync  || (sync_func)  sync  == sync_);
//      BOOST_ASSERT(NULL == async || (async_func) async == async_);
//      BOOST_ASSERT(NULL == prep  || (prep_func)  prep  == prep_);

        switch (mode) {
        case Async_Sync:
            BOOST_ASSERT(sync);
            return async_sync(prxy, c, state, (sync_func)sync, (prep_func)prep);

        case Async_Async:
            BOOST_ASSERT(async);
            return async_async(c, state, (async_func)async);

        case Sync_Sync:
        case Sync_Async:
            BOOST_ASSERT(false);
            break;

        default:
            break;
        }

        // no adaptor found (Invalid mode)!
        SAGA_THROW_VERBATIM(c.get(),
                            std::string ("No adaptor implements method: ") + state->get_op_name(),
                            adaptors::NoAdaptor);

        // this makes some compilers happy, but will never be called in fact
        // (didn't you see the throw above?)
        return saga::task(saga::task_base::Done);
    }

    ///////////////////////////////////////////////////////////////////////////
    template <typename Base, typename RetVal>
    inline saga::task
    execute_sync_async (proxy      * prxy,
                        char const * cpi_name,
                        char const * name,
                        char const * op_name,
                        bool         is_sync,
                        void       (Base::*sync) (RetVal &),
                        saga::task (Base::*async)());

    ///////////////////////////////////////////////////////////////////////////
    // bring in higher order functions, i.e. functions with multiple parameters
    #include <saga/impl/engine/sync_async_impl.hpp>

///////////////////////////////////////////////////////////////////////////////
}}  // namespace saga::impl

#endif // SAGA_IMPL_ENGINE_SYNC_ASYNC_HPP

max_stars_repo_name: saga-project/saga-cpp

max_issues_repo_name: saga-project/saga-cpp

max_forks_repo_name: saga-project/saga-cpp


text:
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <iostream>

#include "common/parameters.hpp"

#include "build/parameters.hpp"

using namespace gram;
using namespace gram::commands::build;

BuildParams commands::build::parse_parameters(
    po::variables_map &vm, const po::parsed_options &parsed) {
  std::string gram_dirpath, fasta_ref;
  uint32_t kmer_size;
  uint32_t max_read_size;

  po::options_description build_description("build options");
  build_description.add_options()(
      "gram_dir", po::value<std::string>(&gram_dirpath)->required(),
      "gramtools directory")("ref",
                             po::value<std::string>(&fasta_ref)->required(),
                             "fasta reference file")(
      "kmer_size", po::value<uint32_t>(&kmer_size)->required(),
      "kmer size used in constructing the kmer index")(
      "max_threads", po::value<uint32_t>()->default_value(1),
      "maximum number of threads used")(
      "all_kmers", po::bool_switch()->default_value(false),
      "generate all kmers of given size (as opposed to inspecting PRG for min "
      "set)")("max_read_size",
              po::value<uint32_t>(&max_read_size)->default_value(0),
              "read maximum size for the set of reads used when quasimaping");

  std::vector<std::string> opts =
      po::collect_unrecognized(parsed.options, po::include_positional);
  if (opts.size() > 0)
    opts.erase(opts.begin());  // Takes out the command itself
  try {
    po::store(po::command_line_parser(opts).options(build_description).run(),
              vm);
    po::notify(vm);
  } catch (const std::exception &e) {
    std::cerr << e.what() << std::endl;
    std::cerr << build_description << std::endl;
    exit(1);
  }

  BuildParams parameters = {};
  fill_common_parameters(parameters, gram_dirpath);

  parameters.sdsl_memory_log_fpath = full_path(gram_dirpath, "sdsl_memory_log");
  parameters.kmers_size = kmer_size;
  parameters.fasta_ref = fasta_ref;

  parameters.all_kmers_flag = vm["all_kmers"].as<bool>();
  parameters.max_read_size = vm["max_read_size"].as<uint32_t>();
  parameters.maximum_threads = vm["max_threads"].as<uint32_t>();

  if (!parameters.all_kmers_flag and parameters.max_read_size == 0)
    throw std::invalid_argument(
        "--max_read_size must be > 0 when --all_kmers flag is not used");

  return parameters;
}

max_stars_repo_name: bricoletc/gramtools

max_issues_repo_name: bricoletc/gramtools

max_forks_repo_name: bricoletc/gramtools


text:
/*
 * indicator_main.cpp
 *
 *  Created on: 2012-10-18
 *      Author: fasiondog
 */

#include <boost/python.hpp>

using namespace boost::python;

void export_Indicator();
void export_IndicatorImp();
void export_Indicator_build_in();
void export_Operand();

BOOST_PYTHON_MODULE(_indicator) {
    export_Indicator();
    export_IndicatorImp();
    export_Indicator_build_in();
    export_Operand();
}

max_stars_repo_name: CodingNowNow/jiaoyi

max_issues_repo_name: nvsnvyu/hikyuu

max_forks_repo_name: nvsnvyu/hikyuu


text:
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "sparse_solver.h"
#include <Eigen/IterativeLinearSolvers>

template<typename T> void test_bicgstab_T()
{
  BiCGSTAB<SparseMatrix<T>, DiagonalPreconditioner<T> > bicgstab_colmajor_diag;
  BiCGSTAB<SparseMatrix<T>, IdentityPreconditioner    > bicgstab_colmajor_I;
  BiCGSTAB<SparseMatrix<T>, IncompleteLUT<T> >           bicgstab_colmajor_ilut;
  //BiCGSTAB<SparseMatrix<T>, SSORPreconditioner<T> >     bicgstab_colmajor_ssor;

  CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_diag)  );
//   CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_I)     );
  CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_ilut)     );
  //CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_ssor)     );
}

void test_bicgstab()
{
  CALL_SUBTEST_1(test_bicgstab_T<double>());
  CALL_SUBTEST_2(test_bicgstab_T<std::complex<double> >());
}


text:
//          Copyright Oliver Kowalke 2009.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_TASKS_WATER_MARK_H
#define BOOST_TASKS_WATER_MARK_H

#include <cstddef>

#include <boost/task/detail/config.hpp>

#include <boost/config/abi_prefix.hpp>

# if defined(BOOST_MSVC)
# pragma warning(push)
# pragma warning(disable:4251 4275)
# endif

namespace boost {
namespace tasks {

class BOOST_TASKS_DECL high_watermark
{
private:
	std::size_t	value_;

public:
	explicit high_watermark( std::size_t value);

	operator std::size_t () const;
};

class BOOST_TASKS_DECL low_watermark
{
private:
	std::size_t	value_;

public:
	explicit low_watermark( std::size_t value);

	operator std::size_t () const;
};

}}

# if defined(BOOST_MSVC)
# pragma warning(pop)
# endif

#include <boost/config/abi_suffix.hpp>

#endif // BOOST_TASKS_WATER_MARK_H

max_stars_repo_name: ghisguth/tasks

max_issues_repo_name: ghisguth/tasks

max_forks_repo_name: ghisguth/tasks


text:
#include <stdio.h>
#include "ds3.h"
#include "test.h"
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE( bulk_put ) {
    ds3_request* request;
    ds3_error* error;
    ds3_get_bucket_response* response;
    ds3_client* client = get_client();
    const char* bucket_name = "unit_test_bucket";
    uint64_t num_objs;

    populate_with_objects(client, bucket_name);

    request = ds3_init_get_bucket(bucket_name);
    error = ds3_get_bucket(client, request, &response);
    ds3_free_request(request);

    num_objs = response->num_objects;

    BOOST_CHECK(error == NULL);
    BOOST_CHECK_EQUAL(num_objs, 5);

    BOOST_CHECK(contains_object(response->objects, num_objs, "resources/beowulf.txt"));

    ds3_free_bucket_response(response);

    clear_bucket(client, bucket_name);
    free_client(client);
}

BOOST_AUTO_TEST_CASE( prefix ) {
    ds3_request* request;
    ds3_error* error;
    ds3_get_bucket_response* response;
    ds3_client* client = get_client();
    const char* bucket_name = "unit_test_bucket";
    uint64_t num_objs;

    populate_with_objects(client, bucket_name);

    request = ds3_init_get_bucket(bucket_name);
    ds3_request_set_prefix(request, "resources/beo");
    error = ds3_get_bucket(client, request, &response);
    ds3_free_request(request);

    BOOST_CHECK(error == NULL);
    num_objs = response->num_objects;
    BOOST_CHECK_EQUAL(num_objs, 1);

    BOOST_CHECK(contains_object(response->objects, num_objs, "resources/beowulf.txt"));

    ds3_free_bucket_response(response);

    clear_bucket(client, bucket_name);

    free_client(client);
}

BOOST_AUTO_TEST_CASE( delimiter ) {
    ds3_request* request;
    ds3_error* error;
    ds3_get_bucket_response* response;
    ds3_client* client = get_client();
    const char* bucket_name = "unit_test_bucket";
    uint64_t num_objs;

    populate_with_objects(client, bucket_name);

    request = ds3_init_get_bucket(bucket_name);
    ds3_request_set_delimiter(request, "/");
    error = ds3_get_bucket(client, request, &response);
    ds3_free_request(request);

    BOOST_CHECK(error == NULL);
    num_objs = response->num_objects;
    BOOST_CHECK_EQUAL(num_objs, 0);

    BOOST_CHECK_EQUAL(response->num_common_prefixes, 1);
    BOOST_CHECK_EQUAL(strcmp(response->common_prefixes[0]->value, "resources/"), 0);

    ds3_free_bucket_response(response);

    clear_bucket(client, bucket_name);
    free_client(client);
}


text:
/*
 * Copyright 2017-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// Define our Module name (prints at testing)
#define BOOST_TEST_MODULE MyTest

#include <boost/test/unit_test.hpp>
#include "gram_savitzky_golay/gram_savitzky_golay.h"
#include <chrono>
#include <cmath>
#include <iostream>

using namespace gram_sg;

BOOST_AUTO_TEST_CASE(TestGorryTables)
{
  // Compare with tables in the paper from Gorry.
  // Convolution weights for quadratic initial-point smoothing:
  // polynomial order = 2, derivative = 0
  std::vector<double> sg7_gram{32, 15, 3, -4, -6, -3, 5};

  SavitzkyGolayFilter filter(3, -3, 2, 0);

  const auto & filter_weights = filter.weights();
  for(unsigned int i = 0; i < sg7_gram.size(); i++)
  {
    std::cout << "ref: " << sg7_gram[i] << ", computed: " << filter_weights[i] * 42 << std::endl;
    BOOST_REQUIRE_CLOSE(sg7_gram[i], filter_weights[i] * 42, 10e-6);
  }

  // BOOST_CHECK( test_object.is_valid() );
}

BOOST_AUTO_TEST_CASE(TestGorryDerivative)
{
  // Compare with tables in the paper from Gorry.
  // Convolution weights for quadratic initial-point first derivative:
  // polynomial order = 2, derivative = 1
  std::vector<double> sg7_deriv_gram{-13, -2, 5, 8, 7, 2, -7};

  SavitzkyGolayFilter filter(3, -3, 2, 1);

  const auto & filter_weights = filter.weights();
  for(unsigned int i = 0; i < sg7_deriv_gram.size(); i++)
  {
    std::cout << "ref: " << sg7_deriv_gram[i] << ", computed: " << filter_weights[i] * 28 << std::endl;
    BOOST_REQUIRE_CLOSE(sg7_deriv_gram[i], filter_weights[i] * 28, 10e-6);
  }
}

BOOST_AUTO_TEST_CASE(TestIdentity)
{
  SavitzkyGolayFilter filter(3, 0, 2, 0);
  std::vector<double> data = {1, 1, 1, 1, 1, 1, 1};
  double res = filter.filter(data);
  BOOST_REQUIRE_CLOSE(res, 1, 10e-6);
}

BOOST_AUTO_TEST_CASE(TestRealTimeFilter)
{
  // Window size is 2*m+1
  const unsigned m = 3;
  // Polynomial Order
  const unsigned n = 2;
  // Initial Point Smoothing (ie evaluate polynomial at first point in the window)
  // Points are defined in range [-m;m]
  const int t = m;
  // Derivate? 0: no derivation, 1: first derivative...
  SavitzkyGolayFilter filter(m, t, n, 0);

  // Filter some data
  std::vector<double> data = {.1, .7, .9, .7, .8, .5, -.3};
  double result = filter.filter(data);
  double result_ref = -0.22619047619047616;
  BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);
}

BOOST_AUTO_TEST_CASE(TestRealTimeDerivative)
{
  // Window size is 2*m+1
  const unsigned m = 3;
  // Polynomial Order
  const unsigned n = 2;
  // Initial Point Smoothing (ie evaluate polynomial at first point in the window)
  // Points are defined in range [-m;m]
  const int t = m;

  // Test First Order Derivative
  SavitzkyGolayFilter filter(m, t, n, 1);
  SavitzkyGolayFilter filter_dt(m, t, n, 1, 0.005);
  BOOST_REQUIRE(filter_dt.config().time_step() == 0.005);

  // Filter some data
  std::vector<double> data = {.1, .2, .3, .4, .5, .6, .7};
  double result = filter.filter(data);
  double result_ref = 0.1;
  BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);

  // Test filtering with timestep=0.005
  data = {.1, .2, .3, .4, .5, .6, .7};
  result = filter_dt.filter(data);
  result_ref = 0.1 / filter_dt.config().time_step();
  BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);

  // Filter some data
  data = {-1, -2, -3, -4, -5, -6, -7};
  result = filter.filter(data);
  result_ref = -1;
  BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);
  // Test filtering with timestep=0.005
  result = filter_dt.filter(data);
  result_ref = -1. / filter_dt.config().time_step();
  BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);

  // Test Second Order Derivative
  SavitzkyGolayFilter second_order_filter(m, t, n, 2);

  // Filter some data
  data = {.1, .2, .3, .4, .5, .6, .7};
  result = second_order_filter.filter(data);
  BOOST_CHECK_SMALL(result, 10e-6);

  // Filter some data
  data = {-1, -2, -3, -4, -5, -6, -7};
  result = second_order_filter.filter(data);
  BOOST_CHECK_SMALL(result, 10e-6);
}

// Test derivation on a known polynomial function
BOOST_AUTO_TEST_CASE(TestPolynomialDerivative)
{
  // Polynomial is a*x^3 + bx^2 + c*x^1 + d
  double a = 10;
  double b = 2;
  double c = -3;
  double d = -4;
  double timeStep = 0.42;

  // Window size is 2*m+1
  const unsigned m = 50;
  // Polynomial Order
  const unsigned n = 3;
  // Points are defined in range [-m;m]
  // Eval at central point
  const int t = 0;

  SavitzkyGolayFilter filter_order1(m, t, n, 1, timeStep);
  SavitzkyGolayFilter filter_order2(m, t, n, 2, timeStep);
  std::vector<double> data;
  std::vector<double> derivative_order1, derivative_order2;
  data.resize(2 * m + 1);
  derivative_order1.resize(2 * m + 1);
  derivative_order2.resize(2 * m + 1);
  // Generate some data points
  for(unsigned x = 0; x < data.size(); ++x)
  {
    data[x] = a * std::pow(x, 3) + b * std::pow(x, 2) + c * std::pow(x, 1) + d;
    derivative_order1[x] = (3 * a * std::pow(x, 2) + 2 * b * std::pow(x, 1) + c) / timeStep;
    derivative_order2[x] = (6 * a * std::pow(x, 1) + 2 * b) / std::pow(timeStep, 2);
  }
  const auto result_order1 = filter_order1.filter(data);
  const auto expected_result_order1 = derivative_order1[m];
  const auto result_order2 = filter_order2.filter(data);
  const auto expected_result_order2 = derivative_order2[m];

  BOOST_REQUIRE_CLOSE(result_order1, expected_result_order1, 10e-8);
  BOOST_REQUIRE_CLOSE(result_order2, expected_result_order2, 10e-8);
}