// -------------- test the visual odometry -------------
#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "acrbslam/config.h"
#include "acrbslam/visual_odometry.h"
#include "acrbslam/cloudmap.h"

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: run_vo parameter_file"<<endl;
        return 1;
    }

    acrbslam::Config::setParameterFile ( argv[1] );
    acrbslam::VisualOdometry::Ptr vo ( new acrbslam::VisualOdometry );

    acrbslam::Camera::Ptr camera ( new acrbslam::Camera );

    VideoCapture capture(CV_CAP_OPENNI);    //设置视频的来源为OPENNI设备，即Kinect

    // visualization
    cv::viz::Viz3d vis ( "Visual Odometry" );
    cv::viz::WCoordinateSystem world_coor ( 1.0 ), camera_coor ( 0.5 );
    cv::Point3d cam_pos ( 0, -1.0, -1.0 ), cam_focal_point ( 0,0,0 ), cam_y_dir ( 0,1,0 );
    cv::Affine3d cam_pose = cv::viz::makeCameraPose ( cam_pos, cam_focal_point, cam_y_dir );
    vis.setViewerPose ( cam_pose );

    world_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 2.0 );
    camera_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 1.0 );
    vis.showWidget ( "World", world_coor );
    vis.showWidget ( "Camera", camera_coor );

     pointCloud::Ptr pointCloud_all( new pointCloud ); //存放所有点云
     //pcl::visualization::CloudViewer viewer("cloudmap viewer");

    for ( int i=0; i<200; i++ )
    {
        capture.grab();

        cout<<"****** loop "<<i<<" ******"<<endl;
        Mat color;
        Mat depth ;
        capture.retrieve( color, CV_CAP_OPENNI_BGR_IMAGE );
        capture.retrieve( depth, CV_CAP_OPENNI_DEPTH_MAP );

        acrbslam::Frame::Ptr pFrame = acrbslam::Frame::createFrame();
        pFrame->camera_ = camera;
        pFrame->color_ = color;

        //test
        //    cout<<"imgsize:"<<sizeof(color)<<endl;           //96
        //     cout<<"depthsize:"<<sizeof(depth)<<endl;      //96
        //

        pFrame->depth_ = depth;

        boost::timer timer;
        vo->addFrame ( pFrame );
        cout<<"VO costs time: "<<timer.elapsed() <<endl;

        if ( vo->state_ == acrbslam::VisualOdometry::LOST )
            break;

        //draw cloudmap
        pointCloud_all=createPointCloud(pFrame, pointCloud_all);

        ////////

        SE3 Twc = pFrame->T_c_w_.inverse();

        // show the map and the camera pose
        cv::Affine3d M (
            cv::Affine3d::Mat3 (
                Twc.rotation_matrix() ( 0,0 ), Twc.rotation_matrix() ( 0,1 ), Twc.rotation_matrix() ( 0,2 ),
                Twc.rotation_matrix() ( 1,0 ), Twc.rotation_matrix() ( 1,1 ), Twc.rotation_matrix() ( 1,2 ),
                Twc.rotation_matrix() ( 2,0 ), Twc.rotation_matrix() ( 2,1 ), Twc.rotation_matrix() ( 2,2 )
            ),
            cv::Affine3d::Vec3 (
                Twc.translation() ( 0,0 ), Twc.translation() ( 1,0 ), Twc.translation() ( 2,0 )
            )
        );

        Mat img_show = color.clone();
        for ( auto& pt:vo->map_->map_points_ )
        {
            acrbslam::MapPoint::Ptr p = pt.second;
            Vector2d pixel = pFrame->camera_->world2pixel ( p->pos_, pFrame->T_c_w_ );
            cv::circle ( img_show, cv::Point2f ( pixel ( 0,0 ),pixel ( 1,0 ) ), 5, cv::Scalar ( 0,255,0 ), 2 );
        }

        cv::imshow ( "image", img_show );
        cv::waitKey ( 0 );
        // viewer.showCloud( pointCloud_all );
        vis.setWidgetPose ( "Camera", M );
        vis.spinOnce ( 1, false );
        cout<<endl;
    }

    cout<<"滤波之后，点云共有"<<pointCloud_all->size()<<"个点."<<endl;
         pcl::io::savePCDFileBinary( "data/result.pcd", *pointCloud_all );
         // while( !viewer.wasStopped() )
         //{}

    return 0;
}


// Copyright (c) 2015-2017 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chain.h>
#include <chainparams.h>
#include <pow.h>
#include <random.h>
#include <util.h>
#include <test/test_bitcoin.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(pow_tests, BasicTestingSetup)

/* Test calculation of next difficulty target with no constraints applying */
BOOST_AUTO_TEST_CASE(get_next_work)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    const int algo = ALGO_SHA256D;

    int nActualTimespan = 600;
    CBlockIndex pindexFirst;
    pindexFirst.nHeight = 2000000;
    pindexFirst.nTime = 1420070400;  // Block #(not real)
    pindexFirst.nBits = 0x1d00ffff;
    CBlockIndex pindexLast;
    pindexLast.nHeight = 2000010;
    pindexLast.nTime = 1420071000;  // Block #(not real)
    pindexLast.nBits = 0x1d00ffff;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d00f5c1);
    // Check MIP3-longblocks intervals
    nActualTimespan = 3010;  // Target is 3000s
    pindexFirst.nHeight = 2903020;  // nPowTargetSpacing
    pindexLast.nHeight = 2903030;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d0100d9);
    nActualTimespan = 6050;  // Target is 6000s
    pindexFirst.nHeight = 2903040;  // nPowTargetSpacingV3a
    pindexLast.nHeight = 2903050;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d010221);
    nActualTimespan = 12020;  // Target is 12000s
    pindexFirst.nHeight = 3386880;  // nPowTargetSpacingV3b
    pindexLast.nHeight = 3386890;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d01006c);
    nActualTimespan = 24070;  // Target is 24000s
    pindexFirst.nHeight = 3628800;  // nPowTargetSpacingV3c
    pindexLast.nHeight = 3628810;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d0100be);
    nActualTimespan = 24100;  // Target is 24000s
    pindexFirst.nHeight = 3749760;  // nPowTargetSpacingV3c
    pindexLast.nHeight = 3749770;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d010110);
}

/* Test the constraint on the upper bound for next work */
BOOST_AUTO_TEST_CASE(get_next_work_pow_limit)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    const int algo = ALGO_SHA256D;

    int nActualTimespan = 6000;
    CBlockIndex pindexFirst;
    pindexFirst.nHeight = 2000000;
    pindexFirst.nTime = 1420070400;  // Block #(not real)
    pindexFirst.nBits = 0x1d00ffff;
    CBlockIndex pindexLast;
    pindexLast.nHeight = 2000010;
    pindexLast.nTime = 1420076400;  // Block #(not real)
    pindexLast.nBits = 0x1d00ffff;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d010a3c);
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d010a3c);
}

/* Test the constraint on the lower bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_lower_limit_actual)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    const int algo = ALGO_SHA256D;

    int nActualTimespan = 10;
    CBlockIndex pindexFirst;
    pindexFirst.nHeight = 2000000;
    pindexFirst.nTime = 1420070400;  // Block #(not real)
    pindexFirst.nBits = 0x1d00ffff;
    CBlockIndex pindexLast;
    pindexLast.nHeight = 2000010;
    pindexLast.nTime = 1420070410;  // Block #(not real)
    pindexLast.nBits = 0x1d00ffff;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d00f5c1);
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d00f5c1);
}

/* Test the constraint on the upper bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_upper_limit_actual)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    const int algo = ALGO_SHA256D;

    int nActualTimespan = 1123200;
    CBlockIndex pindexFirst;
    pindexFirst.nHeight = 2000000;
    pindexFirst.nTime = 1420070400;  // Block #(not real)
    pindexFirst.nBits = 0x1d00ffff;
    CBlockIndex pindexLast;
    pindexLast.nHeight = 2000010;
    pindexLast.nTime = 1421193600;  // Block #(not real)
    pindexLast.nBits = 0x1d00ffff;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d010a3c);
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, &pindexFirst, chainParams->GetConsensus(), algo, nActualTimespan, pindexLast.nHeight+1), 0x1d010a3c);
}

BOOST_AUTO_TEST_CASE(GetBlockProofEquivalentTime_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    std::vector<CBlockIndex> blocks(10000);
    for (int i = 0; i < 10000; i++) {
        blocks[i].pprev = i ? &blocks[i - 1] : nullptr;
        blocks[i].nHeight = i;
        blocks[i].nTime = 1269211443 + i * chainParams->GetConsensus().nPowTargetSpacing;
        blocks[i].nBits = 0x207fffff; /* target 0x7fffff000... */
        blocks[i].nChainWork = i ? blocks[i - 1].nChainWork + GetBlockProof(blocks[i - 1]) : arith_uint256(0);
    }

    for (int j = 0; j < 1000; j++) {
        CBlockIndex *p1 = &blocks[InsecureRandRange(10000)];
        CBlockIndex *p2 = &blocks[InsecureRandRange(10000)];
        CBlockIndex *p3 = &blocks[InsecureRandRange(10000)];

        int64_t tdiff = GetBlockProofEquivalentTime(*p1, *p2, *p3, chainParams->GetConsensus());
        BOOST_CHECK_EQUAL(tdiff, p1->GetBlockTime() - p2->GetBlockTime());
    }
}

BOOST_AUTO_TEST_SUITE_END()


/** $Id: TrackBinner.cxx 128962 2015-02-12 06:57:51Z olivas $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 128962 $
 * $Date: 2015-02-11 23:57:51 -0700 (Wed, 11 Feb 2015) $
 */

#include <MuonGun/TrackBinner.h>
#include <boost/assign.hpp>
#include <boost/foreach.hpp>
#include <icetray/I3Units.h>

namespace I3MuonGun {

TrackBinner::TrackBinner(double mindepth, double maxdepth, unsigned steps)
{
	std::vector<double> multbins;
	{
		using namespace boost::assign;
		multbins += 0.5, 1.5, 2.5, 3.5, 9.5, 19.5, 39.5, 99.5;
	}

	using namespace histogram::binning;
	histogram::histogram<5>::bin_specification mspecs;
	mspecs[0] = uniform<cosine>::create(0, M_PI/2, 11);
	double dh = (maxdepth-mindepth)/(2*(steps-1));
	mspecs[1] = uniform<>::create(mindepth-dh, maxdepth-dh, steps);
	mspecs[2] = multbins;
	mspecs[3] = uniform<power<2> >::create(0, 250, 31);
	mspecs[4] = uniform<histogram::binning::log10>::create(1, 1e7, 101);

	{
		histogram::histogram<2>::bin_specification specs = {{mspecs[0], uniform<histogram::binning::log10>::create(1e2, 1e11, 101)}};
		primary_ = boost::make_shared<histogram::histogram<2> >(specs);
	}

	{
		histogram::histogram<3>::bin_specification specs = {{
		    uniform<cosine>::create(0, M_PI/2, 101), mspecs[1], uniform<>::create(0.5, 99.5, 100)}};
		multiplicity_ = boost::make_shared<histogram::histogram<3> >(specs);
	}

	{
		histogram::histogram<4>::bin_specification specs = {{mspecs[0], mspecs[1], mspecs[2],
			uniform<power<2> >::create(0, 250, 101)}};
		radius_ = boost::make_shared<histogram::histogram<4> >(specs);
	}

	energy_ = boost::make_shared<histogram::histogram<5> >(mspecs);

}

void TrackBinner::Consume(boost::shared_ptr<const TrackBundle> tracks,
    double energy, double zenith, double weight)
{
	{
		boost::array<double, 2> values = {{zenith, energy}};
		primary_->fill(values, weight);

	}
	for (std::map<double, std::vector<CompactTrack> >::const_iterator
	    i = tracks->begin(); i != tracks->end(); i++) {

		double mult = i->second.size();
		boost::array<double, 3> mult_values = {{zenith, i->first/I3Units::km, mult}};
		multiplicity_->fill(mult_values, weight);
		boost::array<double, 4> radius_values = {{zenith, i->first/I3Units::km, mult, 0}};
		boost::array<double, 5> energy_values = {{zenith, i->first/I3Units::km, mult, 0, 0}};
		BOOST_FOREACH(const CompactTrack &track, i->second) {
			radius_values[3] = energy_values[3] = track.GetRadius();
			energy_values[4] = track.GetEnergy();
			radius_->fill(radius_values, weight/mult);
			energy_->fill(energy_values, weight/mult);
		}
	}
}

NeutrinoBinner::NeutrinoBinner()
{
	using namespace histogram::binning;
	// Zenith angle, bundle energy, neutrino energy, parent type
	histogram::histogram<3>::bin_specification mspecs;
	mspecs[0] = uniform<cosine>::create(0, M_PI/2, 101);
	mspecs[1] = uniform<histogram::binning::log10>::create(1, 1e8, 101);
	mspecs[2] = uniform<histogram::binning::log10>::create(1e2, 1e8, 101);

	boost::array<I3Particle::ParticleType, 3> nutypes = {{I3Particle::NuE, I3Particle::NuMu, I3Particle::NuTau}};
	BOOST_FOREACH(I3Particle::ParticleType ptype, nutypes) {
		histmap::mapped_type &vec = histograms_[ptype];
		for (int i=0; i < 4; i++)
			vec.push_back(boost::make_shared<hist>(mspecs));
	}
}

void NeutrinoBinner::Consume(boost::shared_ptr<const TrackBundle> tracks,
    I3MCTreeConstPtr tree, I3MapI3ParticleIDDoubleConstPtr particle_weights, double flux_weight)

{
	if (tree->size() == 0)
		return;
	double zenith = tree->begin()->GetZenith();
	double total_energy = 0;
	if (tracks->size() > 0)
		BOOST_FOREACH(const CompactTrack &track, tracks->begin()->second)
			total_energy += track.GetEnergy();
	else
		total_energy = 0;

	boost::array<double, 3> values = {{zenith, total_energy, 0.}};
	for (I3MCTree::const_iterator it = tree->begin(); it != tree->end(); it++) {
		const I3Particle &p = *it;
		if (p.IsNeutrino()) {
			double weight = flux_weight;
			if (particle_weights != NULL) {
				I3MapI3ParticleIDDouble::const_iterator w = particle_weights->find(p.GetID());
				if (w != particle_weights->end())
					weight *= w->second;
			}
			values[2] = p.GetEnergy();

			// Find neutrino type
			histmap::iterator ptype = histograms_.find(std::abs(p.GetPdgEncoding()));
			if (ptype == histograms_.end())
				log_fatal("Unknown neutrino type %s!", p.GetTypeString().c_str());

			histmap::mapped_type::iterator target;

			// Find parent type
			I3MCTree::const_iterator parent = tree->parent(it);
			int pcode = parent != tree->end() ? std::abs(parent->GetPdgEncoding()) : 0;
			// Classify mesons by heaviest consituent quark
			if (pcode/1000 == 0 && pcode/100 > 0 /* mesons */) {
				// charm
				if (pcode/100 == 4)
					target = ptype->second.begin()+2;
				// strange (including K_long)
				else if (pcode/100 == 3 || pcode == 130)
					target = ptype->second.begin()+1;
				// all other mesons (almost entirely pions)
				else
					target = ptype->second.begin();
			// Put everything else in a big jug
			} else {
				target = ptype->second.begin()+3;
			}

			(*target)->fill(values, weight);
		}
	}
}

}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

/**
 * @file rk.hpp
 * @brief Integration of ODEs by Runge-Kutta
 * @author Ben Allanach, Alexander Voigt
 *
 * The implementation of the Runge-Kutta routines have been derived
 * from SOFTSUSY [hep-ph/0104145, Comp. Phys. Comm. 143 (2002) 305].
 */

#ifndef RK_H
#define RK_H

#include <algorithm>
#include <cmath>
#include <functional>

#include <Eigen/Core>

#include "logger.hpp"
#include "error.hpp"

namespace flexiblesusy {

namespace runge_kutta {

namespace {
/// Returns |a| with sign of b in front
inline double sign(double a, double b) noexcept {
   return b >= 0 ? std::fabs(a) : -std::fabs(a);
}
} // anonymous namespace

/// A single step of Runge Kutta (5th order), input:
/// y and dydx (derivative of y), x is independent variable. yout is value
/// after step. derivs is a user-supplied function
template <typename ArrayType, typename Derivs>
void rungeKuttaStep(const ArrayType& y, const ArrayType& dydx, double x,
		    double h, ArrayType& yout, ArrayType& yerr, Derivs derivs)
{
   const double a2 = 0.2;
   const double a3 = 0.3;
   const double a4 = 0.6;
   const double a5 = 1.0;
   const double a6 = 0.875;
   const double b21 = 0.2;
   const double b31 = 3.0 / 40.0;
   const double b32 = 9.0 / 40.0;
   const double b41 = 0.3;
   const double b42 = -0.9;
   const double b43 = 1.2;
   const double b51 = -11.0 / 54.0;
   const double b52 = 2.5;
   const double b53 = -70.0 / 27.0;
   const double b54 = 35.0 / 27.0;
   const double b61 = 1631.0 / 55296.0;
   const double b62 = 175.0 / 512.0;
   const double b63 = 575.0 / 13824.0;
   const double b64 = 44275.0 / 110592.0;
   const double b65 = 253.0 / 4096.0;
   const double c1 = 37.0 / 378.0;
   const double c3 = 250.0 / 621.0;
   const double c4 = 125.0 / 594.0;
   const double c6 = 512.0 / 1771.0;
   const double dc5 = -277.00 / 14336.0;
   const double dc1 = c1 - 2825.0 / 27648.0;
   const double dc3 = c3 - 18575.0 / 48384.0;
   const double dc4 = c4 - 13525.0 / 55296.0;
   const double dc6 = c6 - 0.25;

   ArrayType ytemp = b21 * h * dydx + y;
   const ArrayType ak2 = derivs(x + a2 * h, ytemp);

   // Allowing piece-wise calculating of ytemp for speed reasons
   ytemp = y + h * (b31 * dydx + b32 * ak2);
   const ArrayType ak3 = derivs(x + a3 * h, ytemp);

   ytemp = y + h * (b41 * dydx + b42 * ak2 + b43 * ak3);
   const ArrayType ak4 = derivs(x+a4*h,ytemp);

   ytemp = y + h * (b51 * dydx + b52 * ak2 + b53 * ak3 + b54 * ak4);
   const ArrayType ak5 = derivs(x + a5 * h, ytemp);

   ytemp = y + h * (b61 * dydx + b62 * ak2 + b63 * ak3 + b64 * ak4 + b65 * ak5);
   const ArrayType ak6 = derivs(x + a6 * h, ytemp);

   yout = y + h * (c1 * dydx + c3 * ak3 + c4 * ak4 + c6 * ak6);
   yerr = h * (dc1 * dydx + dc3 * ak3 + dc4 * ak4 + dc5 * ak5 + dc6 * ak6);
}

/// organises the variable step-size for Runge-Kutta evolution
template <typename ArrayType, typename Derivs>
double odeStepper(ArrayType& y, const ArrayType& dydx, double& x, double htry,
                  double eps, const ArrayType& yscal, Derivs derivs,
                  int& max_step_dir)
{
   const double SAFETY = 0.9;
   const double PGROW = -0.2;
   const double PSHRNK = -0.25;
   const double ERRCON = 1.89e-4;
   const int n = y.size();
   double errmax;
   double h = htry;
   ArrayType yerr(n);
   ArrayType ytemp(n);

   for (;;) {
      rungeKuttaStep(y, dydx, x, h, ytemp, yerr, derivs);
      errmax = (yerr / yscal).abs().maxCoeff(&max_step_dir);
      errmax  /= eps;
      if (!std::isfinite(errmax)) {
#ifdef ENABLE_VERBOSE
         ERROR("odeStepper: non-perturbative running at Q = "
               << std::exp(x) << " GeV of parameter y(" << max_step_dir
               << ") = " << y(max_step_dir) << ", dy(" << max_step_dir
               << ")/dx = " << dydx(max_step_dir));
#endif
         throw NonPerturbativeRunningError(std::exp(x), max_step_dir, y(max_step_dir));
      }
      if (errmax <= 1.0) {
         break;
      }
      const double htemp = SAFETY * h * std::pow(errmax, PSHRNK);
      h = (h >= 0.0 ? std::max(htemp, 0.1 * h) : std::min(htemp, 0.1 * h));
      if (x + h == x) {
#ifdef ENABLE_VERBOSE
         ERROR("At Q = " << std::exp(x) << " GeV "
               "stepsize underflow in odeStepper in parameter y("
               << max_step_dir << ") = " << y(max_step_dir) << ", dy("
               << max_step_dir << ")/dx = " << dydx(max_step_dir));
#endif
         throw NonPerturbativeRunningError(std::exp(x), max_step_dir, y(max_step_dir));
      }
   }
   x += h;
   y = ytemp;

   return errmax > ERRCON ? SAFETY * h * std::pow(errmax,PGROW) : 5.0 * h;
}

/// Organises integration of 1st order system of ODEs
template <typename ArrayType, typename Derivs,
          typename Stepper = decltype(runge_kutta::odeStepper<ArrayType,Derivs>)>
void integrateOdes(ArrayType& ystart, double from, double to, double eps,
                   double h1, double hmin, Derivs derivs,
                   Stepper rkqs = runge_kutta::odeStepper<ArrayType,Derivs>, int max_steps = 400)
{
   const int nvar = ystart.size();
   const double TINY = 1.0e-16;
   double x = from;
   double h = sign(h1, to - from);
   ArrayType yscal(nvar);
   ArrayType y(ystart);
   ArrayType dydx;
   int max_step_dir;

   for (int nstp = 0; nstp < max_steps; ++nstp) {
      dydx = derivs(x, y);
      yscal = y.abs() + (dydx * h).abs() + TINY;
      if ((x + h - to) * (x + h - from) > 0.0) {
         h = to - x;
      }

      const double hnext = rkqs(y, dydx, x, h, eps, yscal, derivs, max_step_dir);

      if ((x - to) * (to - from) >= 0.0) {
         ystart = y;
         return;
      }

      h = hnext;

      if (std::fabs(hnext) <= hmin) {
         break;
      }
   }

#ifdef ENABLE_VERBOSE
   ERROR("Bailed out of rk.cpp:too many steps in integrateOdes\n"
         "********** Q = " << std::exp(x) << " *********");
   ERROR("max step in direction of " << max_step_dir);
   for (int i = 0; i < nvar; i++)
      ERROR("y(" << i << ") = " << y(i) << " dydx(" << i <<
            ") = " << dydx(i));
#endif

   throw NonPerturbativeRunningError(std::exp(x), max_step_dir, y(max_step_dir));
}

} // namespace runge_kutta

} // namespace flexiblesusy

#endif // RK_H


#ifndef INCLUDE_SINGLE_NEXT_ACTIONS_HPP
#define INCLUDE_SINGLE_NEXT_ACTIONS_HPP

#include <array>
#include <cstdint>

#include <boost/hana/power.hpp>

#include <types.hpp>

namespace procon30 {
namespace util {
namespace hana = boost::hana;
template <std::size_t N>
struct single_next_actions {
  std::array<action_pattern_type, hana::power(9, hana::int_c<N>)> _array;
  constexpr single_next_actions() : _array() {}
  template <std::size_t M>
  constexpr auto get() const {
    return _array[M];
  }

  constexpr auto at(std::size_t idx) const { return _array[idx]; }
  constexpr auto size() const { return hana::power(9, hana::int_c<N>); }

  auto begin() { return _array.begin(); };
  auto end() { return _array.end(); };
  auto cbegin() { return _array.cbegin(); };
  auto cend() { return _array.cend(); };
  auto rbegin() { return _array.rbegin(); }
  auto crbegin() { return _array.crbegin(); }
  auto rend() { return _array.rend(); }
  auto crend() { return _array.crend(); }
};

template <>
constexpr single_next_actions<2>::single_next_actions() : _array() {
  std::size_t idx = 0;
  for (std::size_t i = 0; i < 9; ++i)
    for (std::size_t j = 0; j < 9; ++j) {
      _array[idx++] = std::bitset<64>(i | (j << 4));
    }
}

template <>
constexpr single_next_actions<3>::single_next_actions() : _array() {
  std::size_t idx = 0;
  for (std::size_t i = 0; i < 9; ++i)
    for (std::size_t j = 0; j < 9; ++j)
      for (std::size_t k = 0; k < 9; ++k) {
        _array[idx++] = std::bitset<64>(i | (j << 4) | (k << 8));
      }
}

template <>
constexpr single_next_actions<4>::single_next_actions() : _array() {
  std::size_t idx = 0;
  for (std::size_t i = 0; i < 9; ++i)
    for (std::size_t j = 0; j < 9; ++j)
      for (std::size_t k = 0; k < 9; ++k)
        for (std::size_t l = 0; l < 9; ++l) {
          _array[idx++] = std::bitset<64>(i | (j << 4) | (k << 8) | (l << 12));
        }
}

template <>
constexpr single_next_actions<5>::single_next_actions() : _array() {
  std::size_t idx = 0;
  for (std::size_t i = 0; i < 9; ++i)
    for (std::size_t j = 0; j < 9; ++j)
      for (std::size_t k = 0; k < 9; ++k)
        for (std::size_t l = 0; l < 9; ++l)
          for (std::size_t m = 0; m < 9; ++m) {
            _array[idx++] = std::bitset<64>(i | (j << 4) | (k << 8) | (l << 12) | (m << 16));
          }
}

template <>
constexpr single_next_actions<6>::single_next_actions() : _array() {
  std::size_t idx = 0;
  for (std::size_t i = 0; i < 9; ++i)
    for (std::size_t j = 0; j < 9; ++j)
      for (std::size_t k = 0; k < 9; ++k)
        for (std::size_t l = 0; l < 9; ++l)
          for (std::size_t m = 0; m < 9; ++m)
            for (std::size_t n = 0; n < 9; ++n) {
            _array[idx++] = std::bitset<64>(i | (j << 4) | (k << 8) | (l << 12) | (m << 16) | (n << 20));
          }
}

}  // namespace util
}  // namespace procon30

#endif


#include "localization_graph.h"
#include "debug_functions.h"
#include "features.h"
#include <Eigen/src/Core/Matrix.h>

#ifdef USE_OPEN3D
    #include <Open3D/Geometry/Geometry.h>
    #include <Open3D/Geometry/PointCloud.h>
    #include <Open3D/Geometry/TriangleMesh.h>
    #include <Open3D/Visualization/Utility/DrawGeometry.h>
    #include <Open3D/IO/ClassIO/TriangleMeshIO.h>
    #include <Open3D/IO/ClassIO/PointCloudIO.h>
#endif

#include <algorithm>
#include <cstdint>
// #include <fmt/format.h>
#include <iostream>
#include <memory>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/base.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/matx.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <stdexcept>
#include <utility>

using namespace k3d;

LGraph::LGraph()
#ifdef TRANSMIT_POSES
    : stream_handle(networking::acquire_stream_handle(STREAM_IP, STREAM_PORT))
#endif
{
    // double identity_vec[3*3] = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
    // identity_mat = utilities::restrucure_mat<double>(identity_vec, 3, 3);
}

bool LGraph::localize_frame(std::shared_ptr<Frame> frame)
{
    current_frame_points.clear();
    current_frame_point_colors.clear();

	frames.push_back(frame);

    bool status = false;

    // cannot localize if only 1 frame
    if (frames.size() == 1)
        status = true;

    // localize using essential matrix decomposition
    else if (frames.size() == 2)
        status = localize_frame_essential(frames[0], frame);

    // localize using PnP
    else
        status = localize_frame_pnp(frames[frames.size() - 2], frame);

    // stream points and frame to the remote device
    if (status)
    {
        #ifdef TRANSMIT_POSES
        networking::stream_points_camera(current_frame_points, current_frame_point_colors, frames.back(), stream_handle);
        #endif
    }

    return status;
}

bool LGraph::localize_frame_essential(const std::shared_ptr<Frame> ref_frame, std::shared_ptr<Frame> frame)
{
    std::vector<std::pair<uint32_t, uint32_t>> matches =
#if defined USE_FLANN_ESSENTIAL
        features::match_features_flann(ref_frame->descriptors, frame->descriptors);
#else
        features::match_features_bf_crosscheck(ref_frame->descriptors, frame->descriptors);
#endif

    matches = features::radius_distance_filter_matches(matches, ref_frame->keypoints, frame->keypoints, FEATURE_DIST_MAX_RADIUS);

    if (matches.size() < MIN_MATCH_FEATURE_COUNT)
        throw std::runtime_error("not enough feature matches: " + std::to_string(matches.size()));

    // DEBUG_visualize_matches(*ref_frame->rgb, *frame->rgb, matches, ref_frame->keypoints, frame->keypoints);

    std::vector<cv::Point2f> x1, x2;
    x1.reserve(matches.size());
    x2.reserve(matches.size());

    // populate the 2d feature position vectors
    for (const auto& m : matches)
    {
        x1.push_back(ref_frame->keypoints[m.first].pt);
        x2.push_back(frame->keypoints[m.second].pt);
    }
    cv::undistortPoints(x1, x1, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);
    cv::undistortPoints(x2, x2, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);

    cv::Mat mask;
    const cv::Mat essential = cv::findEssentialMat(x1, x2, frame->params.intr, cv::RANSAC, 0.999, 0.11, mask);

    std::cout << essential << "\n";

    cv::Mat local_R, local_t;
    cv::recoverPose(essential, x1, x2, frame->params.intr, local_R, local_t, mask);

    std::cout << local_R << "\n\n" << local_t << "\n";

    Eigen::Matrix3d dR;
    Eigen::Vector3d dt;
    cv::cv2eigen(local_R, dR);
    cv::cv2eigen(local_t, dt);

    frame->position = dt;
    frame->rotation = dR;

    const auto T_P = utilities::TP_from_Rt(dR.transpose(), -dR.transpose() * dt, frame->params.intr);
    frame->transformation = T_P.first;
    frame->projection = T_P.second;
    cv::eigen2cv(T_P.second, frame->projection_cv);

    #ifdef LOG
    const  Eigen::AngleAxisd ax(dR);
    std::cout << "essential angle: " << ax.angle() * (180. / 3.1415) << "\n";
    #endif

    create_landmarks_from_matches(ref_frame, frame, matches);

    // visualize_camera_tracks(true);

    return true;
}

void LGraph::create_landmarks_from_matches(const std::shared_ptr<Frame> ref_frame,
        const std::shared_ptr<Frame> frame, const std::vector<std::pair<uint32_t, uint32_t>>& matches)
{
    for (auto& m : matches)
    {
        Landmark lm;

        lm.descriptors.push_back(features::get_individual_descriptor(ref_frame->descriptors, m.first));
        lm.descriptors.push_back(features::get_individual_descriptor(frame->descriptors, m.second));

        std::vector<cv::Point2f> x1 = { ref_frame->keypoints[m.first].pt };
        std::vector<cv::Point2f> x2 = { frame->keypoints[m.second].pt };

        cv::undistortPoints(x1, x1, frame->params.intr, ref_frame->params.distortion, cv::noArray(), ref_frame->params.intr);
        cv::undistortPoints(x2, x2, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);

        lm.feature_2d_points.push_back(x1[0]);
        lm.feature_2d_points.push_back(x2[0]);

        lm.triangulate_2d_points.push_back(x1[0]);
        lm.triangulate_2d_points.push_back(x2[0]);

        lm.view_frames.push_back(ref_frame);
        lm.view_frames.push_back(frame);

        lm.triangulate_frames.push_back(ref_frame);
        lm.triangulate_frames.push_back(frame);

// use multiview triangulation. Considerably slower.
#if false
        const cv::Point3d new_p3d = features::triangulate_multiview_eigen(lm.triangulate_2d_points, [&lm] {
            std::vector<Mat34> pmats;
            for (const auto& f : lm.triangulate_frames)
                pmats.push_back(f->projection);
            return pmats;
        }());
// use 2view DLT. Fast.
#else
        cv::Mat p4d;
        cv::triangulatePoints(ref_frame->projection_cv, frame->projection_cv, x1, x2, p4d);
        const cv::Point3f new_p3d = cv::Point3f(
                    p4d.at<float>(0, 0) / p4d.at<float>(3, 0),
                    p4d.at<float>(1, 0) / p4d.at<float>(3, 0),
                    p4d.at<float>(2, 0) / p4d.at<float>(3, 0));
#endif

        // if the triangulated point is behind the camera or too far away -> ignore
        if (!utilities::point_in_front(frame->projection, Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z)) ||
            Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z).norm() > TRIANGULATE_DISTANCE_OUTLIER)
            continue;

        lm.location = new_p3d;
        const cv::Vec3b col = frame->rgb->at<cv::Vec3b>(x2[0].y, x2[0].x);
        lm.color = Eigen::Vector3d((float)(col.val[2]) / 255.0, (float)(col.val[1]) / 255.0, (float)(col.val[0]) / 255.0);

        lm.normal = frame->position - Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z);

        landmarks.push_back(lm);

        // <feature index, landmark index>
        frame->feature_landmark_lookup.push_back(std::make_pair(m.second, landmarks.size() - 1));

        // these will be sent over the network to the remote device
        current_frame_points.push_back(Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z));
        current_frame_point_colors.push_back(lm.color);
    }
}

void LGraph::update_landmarks(const std::shared_ptr<Frame> frame,
        std::vector<uint32_t>& feature_ids, std::vector<uint32_t>& landmark_ids)
{
    for (int ii = 0; ii < feature_ids.size(); ii++)
    {
        Landmark& lm = landmarks[landmark_ids[ii]];

        // const std::vector<cv::Point2f> x1 = { lm.first_feature_point };
        // const std::vector<cv::Point2f> x2 = { frame->keypoints[feature_ids[ii]].pt };

        // cv::Mat p4d;
        // cv::triangulatePoints(lm.first_frame->projection_cv, frame->projection_cv, x1, x2, p4d);
        // const cv::Point3f p3d = cv::Point3f(
        //             p4d.at<float>(0, 0) / p4d.at<float>(3, 0),
        //             p4d.at<float>(1, 0) / p4d.at<float>(3, 0),
        //             p4d.at<float>(2, 0) / p4d.at<float>(3, 0));

        // // if the triangulated point is behind the camera -> ignore
        // if (utilities::point_infront_of_camera(frame->projection, Eigen::Vector3d(p3d.x, p3d.y, p3d.z)) &&
        //     Eigen::Vector3d(p3d.x, p3d.y, p3d.z).norm() < TRIANGULATE_DISTANCE_OUTLIER)
        // {
        //     lm.location += p3d;
        //     lm.location /= 2.0;
        // }

        frame->feature_landmark_lookup.push_back(std::make_pair(feature_ids[ii], landmark_ids[ii]));
        lm.descriptors.push_back(features::get_individual_descriptor(frame->descriptors, feature_ids[ii]));
        lm.feature_2d_points.push_back(frame->keypoints[feature_ids[ii]].pt);
        lm.view_frames.push_back(frame);

        // for performance's sake skip the incremential 3d improvement
        continue;

        const Eigen::Vector3d test_p3d (lm.location.x, lm.location.y, lm.location.z);

        // find a triangulatable frame
        for (int jj = lm.triangulate_frames.size() - 1; jj >= 0; jj--)
        {
            const double frame_angle = utilities::calculate_triangulation_angle(lm.triangulate_frames[jj]->position, frame->position, test_p3d);

            // std::cout << frame_angle << ", " << RAD2DEG(frame_angle) << "\n";

            if (RAD2DEG(frame_angle) < MIN_TRIANGULATION_ANGLE)
                continue;

            lm.triangulate_2d_points.push_back(frame->keypoints[feature_ids[ii]].pt);

            const cv::Point3d new_p3d = features::triangulate_multiview_eigen(lm.feature_2d_points, [&lm, &frame] {
                std::vector<Mat34> pmats;
                for (const auto& f : lm.view_frames)
                    pmats.push_back(f->projection);
                pmats.push_back(frame->projection);
                return pmats;
            }());

            if (std::isnan(new_p3d.x) || std::isnan(new_p3d.y) || std::isnan(new_p3d.z) ||
                std::isinf(new_p3d.x) || std::isinf(new_p3d.y) || std::isinf(new_p3d.z))
            {
                lm.triangulate_2d_points.pop_back();
                continue;
            }

            // if the triangulated point is behind the camera or too far away -> ignore
            if (!utilities::point_in_front(frame->projection, Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z)) ||
                Eigen::Vector3d(new_p3d.x, new_p3d.y, new_p3d.z).norm() > TRIANGULATE_DISTANCE_OUTLIER)
            {
                lm.triangulate_2d_points.pop_back();
                continue;
            }

            lm.location = new_p3d;
            lm.triangulate_frames.push_back(frame);

            std::cout << ii << " updated, " << frame_angle << ", " << RAD2DEG(frame_angle) << "\n";

            break;
        }
    }
}

bool LGraph::localize_frame_pnp(const std::shared_ptr<Frame> prev_frame, std::shared_ptr<Frame> frame)
{
    std::vector<std::pair<uint32_t, uint32_t>> matches =
#if defined USE_FLANN
        features::match_features_flann(prev_frame->descriptors, frame->descriptors);
#else
        features::match_features_bf_crosscheck(prev_frame->descriptors, frame->descriptors);
#endif

    // matches = features::radius_distance_filter_matches(matches, prev_frame->keypoints, frame->keypoints, FEATURE_DIST_MAX_RADIUS);
    matches = homography_filter_matches(prev_frame, frame, matches);

    // DEBUG_visualize_matches(*prev_frame->rgb, *frame->rgb, matches, prev_frame->keypoints, frame->keypoints);

    // find landmark points for PnP
    std::vector<cv::Point3f> lm_points;
    std::vector<uint32_t> lm_ids;
    std::vector<cv::Point2f> feature_points;
    std::vector<uint32_t> feature_ids;

    for (const auto& lfc : find_landmark_feature_matches(prev_frame, matches))
    {
        feature_points.push_back(frame->keypoints[lfc.first].pt);
        feature_ids.push_back(lfc.first);

        lm_points.push_back(landmarks[lfc.second].location);
        lm_ids.push_back(lfc.second);
    }

    if (feature_points.size() < MIN_MATCH_FEATURE_COUNT)
    {
        std::cout << "could not localize frame, not enough matches: " << feature_points.size() << "\n";
        this->frames.pop_back();
        return false;
    }

    cv::undistortPoints(feature_points, feature_points, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);

    cv::Mat rcv, tcv, rcv_mat;
    cv::eigen2cv(prev_frame->position, tcv);
    cv::eigen2cv(prev_frame->rotation, rcv);

    std::vector<int> inliers;
    cv::solvePnPRansac(lm_points, feature_points, frame->params.intr, frame->params.distortion, rcv, tcv, true, 1000, 8.0, 0.988, inliers);
    cv::Rodrigues(rcv, rcv_mat);

    Eigen::Matrix3d dR;
    Eigen::Vector3d dt;
    cv::cv2eigen(rcv_mat, dR);
    cv::cv2eigen(tcv, dt);

    frame->rotation = dR;
    frame->position = dt;

    const auto T_P = utilities::TP_from_Rt(dR.transpose(), -dR.transpose() * dt, frame->params.intr);
    frame->transformation = T_P.first;
    frame->projection = T_P.second;
    cv::eigen2cv(T_P.second, frame->projection_cv);

    // reject the frame if it has too high location magnitude
    if (frame->position.norm() - prev_frame->position.norm() > FRAME_POSITION_DISTANCE_DEVIATION)
    {
        std::cout << "frame rejected for having an outlier position of " << frame->position.transpose() <<
            " compared to the previous frame pose of " << prev_frame->position.transpose() << "\n";

        frames.pop_back();
        return false;
    }

    // update matched landmarks using lookup
    update_landmarks(frame, feature_ids, lm_ids);

    if (feature_points.size() < MIN_MATCH_TRIANGULATE_NEW_COUNT)
    {
        // std::cout << "low number of active landmarks reached, creating new ones\n";
        new_landmarks_standalone(frame, lm_points);
    }

    if (frames.size() % DENSE_POINTS_EVERY_NTH_FRAME == 0)
    {
        std::cout << "project more points\n";
        project_more_points(find_triangulatable_point_frame(frame, lm_points, MIN_TRIANGULATION_ANGLE_LIBERAL), frame);
    }

    return true;
}

void LGraph::project_more_points(const std::shared_ptr<Frame> ref_frame, const std::shared_ptr<Frame> frame)
{
    // compute dense features
    const auto [ref_kps, ref_desc] = features::detect_features_orb(ref_frame->rgb, true);
    const auto [frame_kps, frame_desc] = features::detect_features_orb(frame->rgb, true);

    std::vector<std::pair<uint32_t, uint32_t>> matches =
        features::match_features_flann(ref_desc, frame_desc, KNN_DISTANCE_RATIO_LIBERAL);

    matches = features::radius_distance_filter_matches(matches, ref_kps, frame_kps, FEATURE_DIST_MAX_RADIUS);
    // matches = homography_filter_matches(ref_frame, frame, matches);

    // DEBUG_visualize_matches(*ref_frame->rgb, *frame->rgb, matches, ref_frame->keypoints, frame->keypoints);

    std::vector<cv::Point2f> x1, x2;

    for (const auto& m : matches)
    {
        x1.push_back(ref_kps[m.first].pt);
        x2.push_back(frame_kps[m.second].pt);
    }

    cv::undistortPoints(x1, x1, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);
    cv::undistortPoints(x2, x2, frame->params.intr, frame->params.distortion, cv::noArray(), frame->params.intr);

    cv::Mat p4ds;
    cv::triangulatePoints(ref_frame->projection_cv, frame->projection_cv, x1, x2, p4ds);

    // std::vector<Eigen::Vector3d> debug_points, debug_colors;

    // loop over all triangulated points and convert from homography to cartecian
    for (int ii = 0; ii < p4ds.cols; ii++)
    {
        const Eigen::Vector3d new_p3d (
                    p4ds.at<float>(0, ii) / p4ds.at<float>(3, ii),
                    p4ds.at<float>(1, ii) / p4ds.at<float>(3, ii),
                    p4ds.at<float>(2, ii) / p4ds.at<float>(3, ii));

        // if the triangulated point is behind the camera or too far away -> ignore
        if (!utilities::point_in_front(frame->projection, new_p3d) || new_p3d.norm() > TRIANGULATE_DISTANCE_OUTLIER)
            continue;

        extra_3d_points.push_back(new_p3d);

        const cv::Vec3b col = frame->rgb->at<cv::Vec3b>(x2[ii].y, x2[ii].x);
        extra_3d_points_colors.push_back(Eigen::Vector3d((float)(col.val[2]) / 255.0, (float)(col.val[1]) / 255.0, (float)(col.val[0]) / 255.0));

        extra_3d_points_normals.push_back(frame->position - new_p3d);

        // add to streamable points
        current_frame_points.push_back(new_p3d);
        current_frame_point_colors.push_back(extra_3d_points_colors.back());

        // debug_points.push_back(new_p3d);
        // debug_colors.push_back(Eigen::Vector3d((float)(col.val[2]) / 255.0, (float)(col.val[1]) / 255.0, (float)(col.val[0]) / 255.0));
    }

    // auto extra_cloud = std::make_shared<open3d::geometry::PointCloud>(open3d::geometry::PointCloud(debug_points));
    // extra_cloud->colors_ = debug_colors;

    // std::shared_ptr<open3d::geometry::TriangleMesh> camera_mesh = std::make_shared<open3d::geometry::TriangleMesh>(open3d::geometry::TriangleMesh());
    // open3d::io::ReadTriangleMeshFromOBJ("../assets/debug_camera_mesh.obj", *camera_mesh, false);
    // camera_mesh->Transform(frame->transformation);

    // open3d::visualization::DrawGeometries({ extra_cloud, camera_mesh });
}

std::vector<std::pair<uint32_t, uint32_t>> LGraph::find_landmark_feature_matches(const std::shared_ptr<Frame> ref_frame,
        const std::vector<std::pair<uint32_t, uint32_t>>& matches)
{
    // <2d feature, 3d landmark>
    std::vector<std::pair<uint32_t, uint32_t>> feature_lm_correspondences;
    feature_lm_correspondences.reserve(std::min(matches.size(), ref_frame->feature_landmark_lookup.size()));

    for (const auto& flm : ref_frame->feature_landmark_lookup)
    {
        for (const auto& match : matches)
        {
            // feature match was found in landmark lookup
            if (match.first == flm.first)
                feature_lm_correspondences.push_back(std::make_pair(match.second, flm.second));
        }
    }

    feature_lm_correspondences.shrink_to_fit();
    return feature_lm_correspondences;
}

std::vector<std::pair<uint32_t, uint32_t>> LGraph::backpropagate_future_matches(const std::shared_ptr<Frame> ref_frame,
        const std::shared_ptr<Frame> frame,
        const std::vector<std::pair<uint32_t, uint32_t>>& matches)
{
    throw std::runtime_error("this does not work, to the surprise of absolutely no one ");

    // <2d feature, 3d landmark>
    std::vector<std::pair<uint32_t, uint32_t>> feature_lm_correspondences;
    std::vector<uint32_t> ref_feature_ids;
    feature_lm_correspondences.reserve(std::min(matches.size(), ref_frame->feature_landmark_lookup.size()));

    // collect landmark point ids and current frame feature point ids
    // find prev-current matched form prev-landmarks
    for (const auto& flm : ref_frame->feature_landmark_lookup)
    {
        for (const auto& match : matches)
        {
            // feature match was found in landmark lookup
            if (match.first == flm.first)
            {
                feature_lm_correspondences.push_back(std::make_pair(match.second, flm.second));
                ref_feature_ids.push_back(match.first);
            }
        }
    }

    // not enough matches, backpropagate new ones
    // if (feature_lm_correspondences.size() < MIN_MATCH_FEATURE_COUNT)
    if (frames.size() > 5)
    {
        std::vector<std::pair<uint32_t, uint32_t>> new_matches;
        std::shared_ptr<Frame> latest_frame;

        // find a frame until STATISTICAL_FEATURE_COUNT condition not met
        for (int ii = frames.size() - 4; ii >= 0; ii--)
        {
            latest_frame = frames[ii];

            new_matches = features::match_features_flann(latest_frame->descriptors, ref_frame->descriptors);
            new_matches = features::take_only_indexed_matches(new_matches, ref_feature_ids);

            // new_matches = features::match_features_bf_crosscheck(latest_frame->descriptors, ref_desc);

            // new_matches = homography_filter_matches(latest_frame, ref_frame, new_matches);
            // new_matches = features::radius_distance_filter_matches(new_matches, ref_kps, latest_frame->keypoints, frame_kps;

            // DEBUG_visualize_matches(*latest_frame->rgb, *ref_frame->rgb, new_matches, latest_frame->keypoints, ref_frame->keypoints);

            // std::cout << ref_feature_ids.size() << ", " << new_matches.size() << ", " << matches.size() << "\n";

            // frame found, stop searching
            if (new_matches.size() < 250)
                break;
        }

        feature_lm_correspondences.shrink_to_fit();
        create_landmarks_from_matches(latest_frame, frame, new_matches);

        feature_lm_correspondences.clear();

        for (const auto& flm : ref_frame->feature_landmark_lookup)
        {
            for (const auto& match : matches)
            {
                // feature match was found in landmark lookup
                if (match.first == flm.first)
                {
                    feature_lm_correspondences.push_back(std::make_pair(match.second, flm.second));
                    ref_feature_ids.push_back(match.first);
                }
            }
        }
    }

    return feature_lm_correspondences;
}

void LGraph::new_landmarks_standalone(const std::shared_ptr<Frame> frame, const std::vector<cv::Point3f>& tr_angle_points)
{
    /**
     *  - find a triangulatable frame
     *  - match and filter features
     *  - triangulate
     *  - add to frame->feature_landmark_lookup
     */

    std::shared_ptr<Frame> ref_frame = find_triangulatable_point_frame(frame, tr_angle_points);

    std::vector<std::pair<uint32_t, uint32_t>> matches =
#if defined USE_FLANN
        features::match_features_flann(ref_frame->descriptors, frame->descriptors);
#else
        features::match_features_bf_crosscheck(ref_frame->descriptors, frame->descriptors);
#endif

    // if (matches.size() > HOMOGRAPHY_MIN_FEATURE_COUNT)
    //     matches = homography_filter_matches(ref_frame, frame, matches);

    matches = features::radius_distance_filter_matches(matches, ref_frame->keypoints, frame->keypoints, FEATURE_DIST_MAX_RADIUS);

    // DEBUG_visualize_matches(*ref_frame->rgb, *frame->rgb, matches, ref_frame->keypoints, frame->keypoints);
    // visualize_camera_tracks(true);

    // std::cout << "created new landmarks: " << matches.size() << "\n";

    // create landmarks and add to feature_landmark_lookup
    create_landmarks_from_matches(ref_frame, frame, matches);
}

void LGraph::backpropagate_new_landmarks_homography(const std::shared_ptr<Frame> frame,
        const cv::Point3f tr_angle_point)
{
    // TODO: find the largest difference frame to match against

    std::shared_ptr<Frame> ref_frame = nullptr;
    const Eigen::Vector3d tr_point (tr_angle_point.x, tr_angle_point.y, tr_angle_point.z);

    if (frames.size() < 4)
        return;

    for (int ii = frames.size() - 3; ii >= 0; ii--)
    {
        const double frame_angle = utilities::calculate_triangulation_angle(frames[ii]->position, frame->position, tr_point);

        if (RAD2DEG(frame_angle) < MIN_TRIANGULATION_ANGLE)
            continue;

        ref_frame = frames[ii];
        break;
    }

    if (!ref_frame)
        return;

    std::vector<std::pair<uint32_t, uint32_t>> matches =
        features::match_features_bf_crosscheck(ref_frame->descriptors, frame->descriptors);
    matches = homography_filter_matches(ref_frame, frame, matches);

    // filter matches for already existing landmarks

    std::vector<std::pair<uint32_t, uint32_t>> new_matches;
    for (const auto& m : matches)
    {
        // find if the feature id already exists in teh landmark lookup
        const auto it = std::find_if(ref_frame->feature_landmark_lookup.begin(), ref_frame->feature_landmark_lookup.end(),
            [&m](const std::pair<uint32_t, uint32_t>& feature_landmark) {
                return feature_landmark.first == m.first;
            });

        // the feature was not found --> create new landmarks
        if (it == ref_frame->feature_landmark_lookup.end())
            new_matches.push_back(m);
    }

    create_landmarks_from_matches(ref_frame, frame, new_matches);

    std::cout << "matches " << matches.size() << " out of " << new_matches.size() << "\n";

    // DEBUG_visualize_matches(*ref_frame->rgb, *frame->rgb, matches, ref_frame->keypoints, frame->keypoints);
    // visualize_camera_tracks(true);

}

std::vector<std::pair<uint32_t, uint32_t>> LGraph::homography_filter_matches(
        const std::shared_ptr<Frame> ref_frame, const std::shared_ptr<Frame> frame,
        const std::vector<std::pair<uint32_t, uint32_t>>& matches)
{
    if (matches.size() < HOMOGRAPHY_MIN_FEATURE_COUNT)
        throw std::runtime_error("not enough matches for computing homography");

    std::vector<cv::Point2f> fpoints1, fpoints2;
    fpoints1.reserve(matches.size());
    fpoints2.reserve(matches.size());

    for (const auto& m : matches)
    {
        fpoints1.push_back(ref_frame->keypoints[m.first].pt);
        fpoints2.push_back(frame->keypoints[m.second].pt);
    }

    cv::Mat homography;

    homography = findHomography(fpoints1, fpoints2, cv::RANSAC, HOMOGRAPHY_RANSAC_THRESHOLD, cv::noArray());

    std::vector<std::pair<uint32_t, uint32_t>> good_matches;
    good_matches.reserve(matches.size());

    for (size_t ii = 0; ii < matches.size(); ii++)
    {
        cv::Mat col = cv::Mat::ones(3, 1, CV_64F);
        col.at<double>(0) = fpoints1[ii].x;
        col.at<double>(1) = fpoints1[ii].y;
        col = homography * col;
        col /= col.at<double>(2);

        const double dist = sqrt(pow(col.at<double>(0) - fpoints2[ii].x, 2) + pow(col.at<double>(1) - fpoints2[ii].y, 2));
        if (dist < HOMOGRAPHY_FILTER_MAX_DIST)
            good_matches.push_back(matches[ii]);
    }

    good_matches.shrink_to_fit();
    return good_matches;
}

void LGraph::backpropagate_new_landmarks(const std::shared_ptr<Frame> frame,
        const std::shared_ptr<Frame> ref_frame,
        const std::vector<uint32_t>& ref_frame_fids_inv,
        const std::vector<uint32_t>& current_frame_fids_inv,
        const std::vector<std::pair<uint32_t, uint32_t>>& matches,
        const cv::Point3f tr_angle_point)
{
    // traverse the frames backwards, matching the features, until
    // a frame is found which has enough triangulatable movement

    // Create a data structure for holding the to-be-triangulated (tbt) feature chains.
    // Is also used as a lookup for pruning the triangulation list
    std::unordered_map<uint32_t, std::vector<uint32_t>> feature_chain;
    feature_chain.reserve(current_frame_fids_inv.size());

    // used to map feature ids from frame -> last_frame
    std::map<uint32_t, uint32_t> first_current_feature_lookup;

    for (int ii = 0; ii < current_frame_fids_inv.size(); ii++)
    {
        feature_chain.insert(std::make_pair(current_frame_fids_inv[ii], std::vector<uint32_t> { ref_frame_fids_inv[ii] }));
        first_current_feature_lookup.insert(std::make_pair(current_frame_fids_inv[ii], ref_frame_fids_inv[ii]));
    }

    std::vector<uint32_t> last_frame_fids = ref_frame_fids_inv;
    cv::Mat last_frame_descriptor = features::descriptor_from_feature_ids(ref_frame->descriptors, last_frame_fids);
    std::shared_ptr<Frame> last_frame = ref_frame;

    // -3: -1 for size, -2 for frame, -3 for ref_frame
    for (int ii = frames.size() - 3; ii >= 0; ii--)
    {
        const std::shared_ptr<Frame> current_frame = frames[ii];;

        // match the current latest descriptors against a new frame
        std::vector<std::pair<uint32_t, uint32_t>> matches =
            features::match_features_bf_crosscheck(last_frame_descriptor, current_frame->descriptors);
        matches = features::radius_distance_filter_matches(matches, last_frame->keypoints, current_frame->keypoints, FEATURE_DIST_MAX_RADIUS);

        std::cout << "matches size: " << matches.size() << "\n";

        last_frame_fids.clear();

        // populate tbt feature chain lookup
        for (const auto& match : matches)
        {
            // find match.first in first_current_feature_lookup, acquire the "key", use the key to
            // index into feature_chain, append match.second
            const auto key_iter = std::find_if(
                first_current_feature_lookup.begin(),
                first_current_feature_lookup.end(),
                [match](const auto& fcfl) { return fcfl.second == match.first; });

            if (key_iter == first_current_feature_lookup.end())
                continue;

            const uint32_t key = key_iter->first;
            feature_chain.at(key).push_back(match.second);

            // append the current frame feature id, next iteration last frame id
            last_frame_fids.push_back(match.second);
        }

        // check if enough triangulatable distance

        // if enough movement, stop iterating and triangulate
        throw std::runtime_error("todo");

        last_frame = current_frame;
        last_frame_descriptor = features::descriptor_from_feature_ids(current_frame->descriptors, last_frame_fids);
    }
}

std::shared_ptr<Frame> LGraph::find_triangulatable_movement_frame(const std::shared_ptr<Frame> frame, const double angle_threshold)
{
    // use angle to find good frame

    const Eigen::Vector3d current_pos = frame->position;

    for (int ii = frames.size() - 2; ii >= 0; ii--)
    {
        const std::shared_ptr<Frame> ref_frame = frames[ii];

        if ((ref_frame->position - current_pos).norm() > angle_threshold)
            return ref_frame;
    }

    return nullptr;
}

std::shared_ptr<Frame> LGraph::find_triangulatable_point_frame(const std::shared_ptr<Frame> frame,
        const std::vector<cv::Point3f>& tr_angle_points, const double angle_threshold)
{
    const Eigen::Vector3d current_pos = frame->position;

    // take the average of all of the triangulated points
    const Eigen::Vector3d tr_point_eigen = [&tr_angle_points] {
        cv::Point3f trp (0.0, 0.0, 0.0);
        for (const auto& p : tr_angle_points)
            trp += p;
        return Eigen::Vector3d(trp.x, trp.y, trp.z) / (double)tr_angle_points.size();

    }();

    for (int ii = frames.size() - 2; ii >= 0; ii--)
    {
        const std::shared_ptr<Frame> ref_frame = frames[ii];

        const double frame_angle = utilities::calculate_triangulation_angle(frames[ii]->position, frame->position, tr_point_eigen);

        if (RAD2DEG(frame_angle) < angle_threshold)
            continue;

        std::cout << "use frame id " << ii << " for triangulation, current frame is " << frames.size() - 1 << "\n";
        return ref_frame;
    }

    std::cout << "use frame id 0 for triangulation\n";
    return frames[0];
}

void LGraph::new_landmarks_from_matched(const std::shared_ptr<Frame> ref_frame,
        const std::shared_ptr<Frame> frame)
{
    std::vector<std::pair<uint32_t, uint32_t>> matches =
        features::match_features_bf_crosscheck(ref_frame->descriptors, frame->descriptors);

    matches = features::radius_distance_filter_matches(matches, ref_frame->keypoints, frame->keypoints, FEATURE_DIST_MAX_RADIUS);

    std::vector<std::pair<uint32_t, uint32_t>> new_matches;

    // filter matches for already existing landmarks
    for (const auto& m : matches)
    {
        // find if the feature id already exists in teh landmark lookup
        const auto it = std::find_if(ref_frame->feature_landmark_lookup.begin(), ref_frame->feature_landmark_lookup.end(),
            [&m](const std::pair<uint32_t, uint32_t>& feature_landmark) {
                return feature_landmark.first == m.first;
            });

        // the feature was not found --> create new landmarks
        if (it == ref_frame->feature_landmark_lookup.end())
            new_matches.push_back(m);
    }

    // std::cout << "new landmarks: " << new_matches.size() << "\n";

    // std::cout << ref_frame->position.transpose() << ", " << frame->position.transpose() << "\n";

    create_landmarks_from_matches(ref_frame, frame, new_matches);
}

#ifdef USE_OPEN3D
void LGraph::visualize_camera_tracks(const bool visualize_landmarks, bool generate_mesh) const
{
    std::vector<std::shared_ptr<const open3d::geometry::Geometry>> debug_cameras;

    for (int ii = 0; ii < frames.size(); ii++)
    {
        std::cout << "frame " << ii << ": " << frames[ii]->position.transpose() << "\n";

        std::shared_ptr<open3d::geometry::TriangleMesh> camera_mesh = std::make_shared<open3d::geometry::TriangleMesh>(open3d::geometry::TriangleMesh());
        open3d::io::ReadTriangleMeshFromOBJ("../assets/debug_camera_mesh.obj", *camera_mesh, false);

        camera_mesh->Transform(frames[ii]->transformation);
        debug_cameras.push_back(camera_mesh);
    }

    if (visualize_landmarks)
    {
        std::vector<Eigen::Vector3d> landmark_points;
        std::vector<Eigen::Vector3d> landmark_colors;
        std::vector<Eigen::Vector3d> landmark_normals;

        for (const auto& lm : landmarks)
        {
            landmark_points.push_back(Eigen::Vector3d(lm.location.x, lm.location.y, lm.location.z));
            landmark_colors.push_back(lm.color);
            landmark_normals.push_back(lm.normal);
        }

        auto lms_cloud = std::make_shared<open3d::geometry::PointCloud>(open3d::geometry::PointCloud(landmark_points));
        lms_cloud->colors_ = landmark_colors;
        lms_cloud->normals_ = landmark_normals;

        if (generate_mesh)
        {
            auto mesh_cloud = std::make_shared<open3d::geometry::PointCloud>(open3d::geometry::PointCloud(extra_3d_points));
            mesh_cloud->colors_ = extra_3d_points_colors;
            mesh_cloud->normals_ = extra_3d_points_normals;

            *mesh_cloud += *lms_cloud;

            // mesh_cloud->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(0.5, 16));

            auto [new_mesh, trash] = open3d::geometry::TriangleMesh::CreateFromPointCloudPoisson(*mesh_cloud, MESH_POISSON_DEPTH);
            new_mesh = new_mesh->FilterSmoothLaplacian(LAPLACIAN_ITERATIONS, LAPLACIAN_LAMBDA);

            // new_mesh->Translate(Eigen::Vector3d(0.0, 20.0, 0.0));

            debug_cameras.push_back(new_mesh);
            // debug_cameras.push_back(mesh_cloud);
        }
        else
        {
            debug_cameras.push_back(lms_cloud);

            auto extra_cloud = std::make_shared<open3d::geometry::PointCloud>(open3d::geometry::PointCloud(extra_3d_points));
            extra_cloud->colors_ = extra_3d_points_colors;
            extra_cloud->normals_ = extra_3d_points_normals;
            *extra_cloud += *lms_cloud;

            // open3d::io::WritePointCloudToPLY("export_cloud.ply", *extra_cloud);

            // debug_cameras.push_back(extra_cloud);
        }
    }

    open3d::visualization::DrawGeometries(debug_cameras, "track visualization", 1920, 1080);
}
#endif

void LGraph::print_camera_tracks() const
{
    for (int ii = 0; ii < frames.size(); ii++)
    {
        std::cout << "frame " << ii << ": " << frames[ii]->position.transpose() << "\n";
    }
}


// renderer of lidar data in collections renderer
// uses pcl and can be used to dump cloud to file as PCD
// fully working as of august 2013

#include <stdio.h>
#include <inttypes.h>
#include <iostream>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/vector.hpp>

#include <lcm/lcm-cpp.hpp>

#include <bot_lcmgl_client/lcmgl.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pointcloud_tools/pointcloud_vis.hpp> // visualize pt clds

#include "lcmtypes/bot_core.hpp"
#include "lcmtypes/multisense.hpp"
#include "lcmtypes/visualization.hpp"

using namespace std;
using namespace Eigen;
using namespace boost::assign; // bring 'operator+()' into scope

class Pass{
  public:
    Pass(boost::shared_ptr<lcm::LCM> &lcm_);

    ~Pass(){
    }
  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    void lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg);
    void cloudHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  multisense::pointcloud_t* msg);
    void camToScanHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::rigid_transform_t* msg);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_incoming_; // 3D points that were precomputed

    Eigen::Isometry3d local_to_camera_ ;
    Eigen::Isometry3d camera_to_scan_; // transform used to do conversion

    pointcloud_vis* pc_vis_ ;
    bot_lcmgl_t* lcmgl_;
    int printf_counter_; // used for terminal feedback
};

Pass::Pass(boost::shared_ptr<lcm::LCM> &lcm_):
    lcm_(lcm_){
  lcmgl_= bot_lcmgl_init(lcm_->getUnderlyingLCM(), "lidar-to-3dpoints");

  lcm_->subscribe( "SCAN" ,&Pass::lidarHandler,this);
  lcm_->subscribe( "SCAN_CLOUD" ,&Pass::cloudHandler,this);
  lcm_->subscribe( "CAMERA_TO_SCAN" ,&Pass::camToScanHandler,this);
  printf_counter_ =0;

  bool reset =0;
  pc_vis_ = new pointcloud_vis( lcm_->getUnderlyingLCM() );
  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  pc_vis_->obj_cfg_list.push_back( obj_cfg(60000,"Pose - Laser",5,reset) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(60001,"Cloud - Laser"         ,1,reset, 60000,1, {0.0, 0.0, 1.0} ));
  pc_vis_->obj_cfg_list.push_back( obj_cfg(2000,"Pose - Null",5,reset) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(2001,"Cloud - Null"           ,1,reset, 2000,1, { 1.0, 1.0, 0.0} ));
  pc_vis_->obj_cfg_list.push_back( obj_cfg(3000,"Pose - Camera",5,reset) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(3001,"Cloud - Camera [Interpolated]"           ,1,reset, 3000,1, { 0.0, 1.0, 1.0} ));

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_incoming_ptr (new pcl::PointCloud<pcl::PointXYZRGB> ());
  cloud_incoming_ = cloud_incoming_ptr;

  // Tranform from Local to Camera:
  Matrix3d m;
  m = AngleAxisd (-M_PI/2, Vector3d::UnitZ ())
                  * AngleAxisd (0, Vector3d::UnitY ())
                  * AngleAxisd ( -M_PI/2, Vector3d::UnitX ());
  local_to_camera_ =  Eigen::Isometry3d::Identity();
  local_to_camera_ *= m;
  local_to_camera_.translation()  << 0,0,0;
}

void Pass::cloudHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  multisense::pointcloud_t* msg){

  cloud_incoming_->width   = msg->num_points;
  cloud_incoming_->height   = 1;
  cloud_incoming_->points.resize (msg->num_points);
  for (size_t i=0; i < msg->num_points; ++i) {
    cloud_incoming_->points[i].x = msg->x[i];
    cloud_incoming_->points[i].y = msg->y[i];
    cloud_incoming_->points[i].z = msg->z[i];
  }

  // Plot interpolated scan from local:
  int64_t pose_id=msg->utime;
  Isometry3dTime local_to_camera_T = Isometry3dTime(pose_id, local_to_camera_ );
  pc_vis_->pose_to_lcm_from_list(3000, local_to_camera_T);
  pc_vis_->ptcld_to_lcm_from_list(3001, *cloud_incoming_, pose_id, pose_id);
}

void Pass::camToScanHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::rigid_transform_t* msg){
  Eigen::Isometry3d camera_to_scan_in;
  camera_to_scan_in.setIdentity();
  camera_to_scan_in.translation()  << msg->trans[0], msg->trans[1], msg->trans[2];
  Eigen::Quaterniond quat = Eigen::Quaterniond(msg->quat[0], msg->quat[1],
                                               msg->quat[2], msg->quat[3]);
  camera_to_scan_in.rotate(quat);

  camera_to_scan_ = local_to_camera_ * camera_to_scan_in;
}

// From pointcloud_lcm
void convertLidar(std::vector< float > ranges, int numPoints, double thetaStart,
        double thetaStep,
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
        double minRange = 0., double maxRange = 1e10,
        double validRangeStart = -1000, double validRangeEnd = 1000)
{
  int count = 0;
  double theta = thetaStart;
  cloud->width   = numPoints;
  cloud->height   = 1;
  cloud->points.resize (numPoints);
  for (int i = 0; i < numPoints; i++) {
    if (ranges[i] > minRange && ranges[i] < maxRange && theta > validRangeStart
            && theta < validRangeEnd) {
        //hokuyo driver seems to report maxRanges as .001 :-/
        //project to body centered coordinates
        cloud->points[count].x = ranges[i] * cos(theta);
        cloud->points[count].y = ranges[i] * sin(theta);
        count++;
    }
    theta += thetaStep;
  }
  // Resize outgoing cloud
  cloud->width   = count;
  cloud->points.resize (count);
}

void Pass::lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg){
  int64_t pose_id=msg->utime;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan_laser (new pcl::PointCloud<pcl::PointXYZRGB> ());
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan_camera (new pcl::PointCloud<pcl::PointXYZRGB> ());

  // 1. Convert scan into simple XY point cloud:
  double minRange =0.0; // consider everything - don't remove any points
  double maxRange = 100.0;
  double validBeamAngles[] ={-10,10};
  convertLidar(msg->ranges, msg->nranges, msg->rad0,
      msg->radstep, scan_laser, minRange, maxRange,
      validBeamAngles[0], validBeamAngles[1]);

  // 2. Project the scan into local frame:
  Eigen::Isometry3f camera_to_scan_f= camera_to_scan_.cast<float>();
  pcl::transformPointCloud (*scan_laser, *scan_camera,
      camera_to_scan_f.translation(), Eigen::Quaternionf(camera_to_scan_f.rotation())  );

  ///
  /// scan_camera now contains the lidar returns as 3d points in the camera frame
  ///

  // Plot original scan from lidar frame:
  Isometry3dTime camera_to_scan_T = Isometry3dTime(pose_id, camera_to_scan_);
  pc_vis_->pose_to_lcm_from_list(60000, camera_to_scan_T);
  pc_vis_->ptcld_to_lcm_from_list(60001, *scan_laser, pose_id, pose_id);

  // Plot scan from camera frame:
  Isometry3dTime null_T = Isometry3dTime(pose_id, Eigen::Isometry3d::Identity() );
  pc_vis_->pose_to_lcm_from_list(2000, null_T);
  pc_vis_->ptcld_to_lcm_from_list(2001, *scan_camera, pose_id, pose_id);

  if (printf_counter_%200 ==0){
    cout << "Filtering: " <<  " "  << msg->utime << "\n";

    vs::reset_collections_t reset;
    lcm_->publish("RESET_COLLECTIONS", &reset);
  }
  printf_counter_++;
}

int main( int argc, char** argv ){
  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  Pass app(lcm);
  cout << "Ready to filter lidar points" << endl << "============================" << endl;
  while(0 == lcm->handle());
  return 0;
}


#include <cnn/cnn.h>
#include <cnn/expr.h>
#include <cnn/grad-check.h>
#include <boost/test/unit_test.hpp>
#include <stdexcept>

using namespace cnn;
using namespace cnn::expr;
using namespace std;

struct NodeTest {
  NodeTest() {
    // set up some dummy arguments to cnn
    for (auto x : {"NodeTest", "--cnn-mem", "10"}) {
      av.push_back(strdup(x));
    }
    char **argv = &av[0];
    int argc = av.size();
    cnn::Initialize(argc, argv);
    ones3_vals = {1.f,1.f,1.f};
    first_one_vals = {1.f,0.f,0.f};
    ones2_vals = {1.f,1.f};
    batch_vals = {1.f,2.f,3.f,4.f,5.f,6.f};
    // Create parameters
    std::vector<float> param1_vals = {1.1f,-2.2f,3.3f};
    std::vector<float> param2_vals = {2.2f,3.4f,-1.2f};
    std::vector<float> param3_vals = {1.1f,2.2f,3.3f};
    std::vector<float> param_scalar1_vals = {2.2f};
    std::vector<float> param_scalar2_vals = {1.1f};
    param1 = mod.add_parameters({3});
    TensorTools::SetElements(param1->values,param1_vals);
    param2 = mod.add_parameters({3});
    TensorTools::SetElements(param2->values,param2_vals);
    param3 = mod.add_parameters({3});
    TensorTools::SetElements(param3->values,param3_vals);
    param_scalar1 = mod.add_parameters({1});
    TensorTools::SetElements(param_scalar1->values,param_scalar1_vals);
    param_scalar2 = mod.add_parameters({1});
    TensorTools::SetElements(param_scalar2->values,param_scalar2_vals);
  }
  ~NodeTest() {
    for (auto x : av) free(x);
  }

  template <class T>
  std::string print_vec(const std::vector<T> vec) {
    ostringstream oss;
    if(vec.size()) oss << vec[0];
    for(size_t i = 1; i < vec.size(); i++)
      oss << ' ' << vec[i];
    return oss.str();
  }

  std::vector<float> ones3_vals, ones2_vals, first_one_vals, batch_vals;
  std::vector<char*> av;
  cnn::Model mod;
  cnn::Parameters *param1, *param2, *param3, *param_scalar1, *param_scalar2;
};

// define the test suite
BOOST_FIXTURE_TEST_SUITE(node_test, NodeTest);

// Expression operator-(const Expression& x);
BOOST_AUTO_TEST_CASE( negate_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = -x1;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator+(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( add_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  Expression y = x1+x2;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator+(const Expression& x, real y);
BOOST_AUTO_TEST_CASE( addscalar_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = x1+2.0;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator+(real x, const Expression& y);
BOOST_AUTO_TEST_CASE( scalaradd_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = 2.0+x1;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator-(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( subtract_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  Expression y = x1+x2;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator-(real x, const Expression& y);
BOOST_AUTO_TEST_CASE( scalarsubtract_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = 2.0-x1;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator-(const Expression& x, real y);
BOOST_AUTO_TEST_CASE( subtractscalar_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = x1-2.0;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator*(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( multiply_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  Expression y = x1*transpose(x2);
  Expression ones3 = input(cg, {1,3}, ones3_vals);
  ones3 * y * transpose(ones3);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator*(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( multiply_batch_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = input(cg, Dim({3},2), batch_vals);
  Expression y = x1*transpose(x2);
  Expression ones3 = input(cg, {1,3}, ones3_vals);
  sum_batches(ones3 * y * transpose(ones3));
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator*(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( affine_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression scalar = parameter(cg, param_scalar1);
  Expression x2 = parameter(cg, param2);
  Expression y = affine_transform({x1, x2, scalar});
  Expression ones3 = input(cg, {1,3}, ones3_vals);
  ones3 * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator*(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( affine_batch_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression scalar = parameter(cg, param_scalar1);
  Expression x2 = input(cg, Dim({3},2), batch_vals);
  Expression y = affine_transform({x1, x2, scalar});
  Expression ones3 = input(cg, {1,3}, ones3_vals);
  sum_batches(ones3 * y);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator*(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( affine_batch2_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = input(cg, Dim({1,3},2), batch_vals);
  Expression scalar = parameter(cg, param_scalar1);
  Expression x2 = parameter(cg, param2);
  Expression y = affine_transform({x1, scalar, transpose(x2) });
  Expression ones3 = input(cg, {3,1}, ones3_vals);
  sum_batches(y * ones3);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression operator*(const Expression& x, float y);
BOOST_AUTO_TEST_CASE( multiplyscalar_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = x1*2.0;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// inline Expression operator*(float y, const Expression& x) { return x * y; }
BOOST_AUTO_TEST_CASE( scalarmultiply_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = 2.0*x1;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// inline Expression operator/(const Expression& x, float y) { return x * (1.f / y); }
BOOST_AUTO_TEST_CASE( dividescalar_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = x1/2.0;
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression cdiv(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( cdiv_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  Expression y = cdiv(x1, x2);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression colwise_add(const Expression& x, const Expression& bias);
BOOST_AUTO_TEST_CASE( colwise_add_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  Expression y = colwise_add(x1 * transpose(x2), x2);
  Expression ones3 = input(cg, {1,3}, ones3_vals);
  ones3 * y * transpose(ones3);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression contract3d_1d(const Expression& x, const Expression& y);
// TODO

// Expression contract3d_1d(const Expression& x, const Expression& y, const Expression& b);
// TODO

// Expression sqrt(const Expression& x);
BOOST_AUTO_TEST_CASE( sqrt_gradient ) {
  cnn::ComputationGraph cg;
  Expression x3 = parameter(cg, param3);
  Expression y = sqrt(x3);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression erf(const Expression& x);
BOOST_AUTO_TEST_CASE( erf_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = erf(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression tanh(const Expression& x);
BOOST_AUTO_TEST_CASE( tanh_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = tanh(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression exp(const Expression& x);
BOOST_AUTO_TEST_CASE( exp_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = exp(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression square(const Expression& x);
BOOST_AUTO_TEST_CASE( square_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = square(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression cube(const Expression& x);
BOOST_AUTO_TEST_CASE( cube_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = cube(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression lgamma(const Expression& x);
BOOST_AUTO_TEST_CASE( lgamma_gradient ) {
  cnn::ComputationGraph cg;
  Expression x2 = parameter(cg, param2);
  Expression y = lgamma(x2);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression log(const Expression& x);
BOOST_AUTO_TEST_CASE( log_gradient ) {
  cnn::ComputationGraph cg;
  Expression x3 = parameter(cg, param3);
  Expression y = log(x3);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression logistic(const Expression& x);
BOOST_AUTO_TEST_CASE( logistic_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = logistic(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression rectify(const Expression& x);
BOOST_AUTO_TEST_CASE( rectify_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = rectify(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression hinge(const Expression& x, unsigned index, float m = 1.0);
BOOST_AUTO_TEST_CASE( hinge_gradient ) {
  unsigned index = 0;
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  hinge(x1, index, 0.5);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression hinge(const Expression& x, const unsigned* pindex, float m = 1.0);
BOOST_AUTO_TEST_CASE( hingeptr_gradient ) {
  unsigned index = 0;
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  hinge(x1, &index, 0.5);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression log_softmax(const Expression& x);
BOOST_AUTO_TEST_CASE( log_softmax_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = log_softmax(x1);
  input(cg, {1,3}, first_one_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression log_softmax(const Expression& x, const std::vector<unsigned>& restriction);
BOOST_AUTO_TEST_CASE( restricted_log_softmax_gradient ) {
  vector<unsigned> restriction = {0,1};
  cnn::ComputationGraph cg;
  Expression x3 = parameter(cg, param3);
  Expression y = exp( log_softmax(x3, restriction) );
  input(cg, {1,3}, first_one_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression softmax(const Expression& x);
BOOST_AUTO_TEST_CASE( softmax_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = softmax(x1);
  input(cg, {1,3}, first_one_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression softsign(const Expression& x);
BOOST_AUTO_TEST_CASE( softsign_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = softsign(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression pow(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( pow_gradient ) {
  cnn::ComputationGraph cg;
  Expression x3 = parameter(cg, param3);
  Expression x_scalar1 = parameter(cg, param_scalar1);
  Expression y = pow(x3, x_scalar1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression min(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( min_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  Expression y = min(x1, x2);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression max(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( max_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  Expression y = max(x1, x2);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// TODO: Noise is random, so it cannot be tested simply?
// // Expression noise(const Expression& x, real stddev);
// BOOST_AUTO_TEST_CASE( noise_gradient ) {
//   cnn::ComputationGraph cg;
//   Expression x1 = parameter(cg, param1);
//   Expression y = noise(x1, 0.5);
//   input(cg, {1,3}, ones3_vals) * y;
//   BOOST_CHECK(CheckGrad(mod, cg, 0));
// }

// TODO: Dropout scales the gradients at training time, so they don't match.
// // Expression dropout(const Expression& x, real p);
// BOOST_AUTO_TEST_CASE( dropout_gradient ) {
//   cnn::ComputationGraph cg;
//   Expression x1 = parameter(cg, param1);
//   Expression y = dropout(x1, 0.5);
//   input(cg, {1,3}, ones3_vals) * y;
//   BOOST_CHECK(CheckGrad(mod, cg, 0));
// }

// Expression block_dropout(const Expression& x, real p);
// TODO

// Expression reshape(const Expression& x, const Dim& d);
BOOST_AUTO_TEST_CASE( reshape_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = reshape(x1, {1,3});
  y * input(cg, {3}, ones3_vals);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression transpose(const Expression& x);
BOOST_AUTO_TEST_CASE( transpose_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = softsign(x1);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression trace_of_product(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( trace_of_product_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  trace_of_product(x1, x2);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression cwise_multiply(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( cwise_multiply_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  Expression y = cwise_multiply(x1, x2);
  input(cg, {1,3}, ones3_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression dot_product(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( dot_product_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  dot_product(x1, x2);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression squared_distance(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( squared_distance_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  squared_distance(x1, x2);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression huber_distance(const Expression& x, const Expression& y, float c = 1.345f);
BOOST_AUTO_TEST_CASE( huber_distance_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  huber_distance(x1, x2);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression l1_distance(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( l1_distance_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = parameter(cg, param2);
  l1_distance(x1, x2);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression binary_log_loss(const Expression& x, const Expression& y);
BOOST_AUTO_TEST_CASE( binary_log_loss_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = logistic( parameter(cg, param1) );
  Expression x2 = input(cg, {3}, ones3_vals);
  binary_log_loss(x1, x2);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression pairwise_rank_loss(const Expression& x, const Expression& y, real m=1.0);
BOOST_AUTO_TEST_CASE( pairwise_rank_loss_gradient ) {
  cnn::ComputationGraph cg;
  Expression x_scalar1 = parameter(cg, param_scalar1);
  Expression x_scalar2 = parameter(cg, param_scalar2);
  pairwise_rank_loss(x_scalar1, x_scalar2);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// TODO: These are all unimplemented
// Expression poisson_loss(const Expression& x, unsigned y);
// Expression poisson_loss(const Expression& x, const unsigned* py);
//
// Expression conv1d_narrow(const Expression& x, const Expression& f);
// Expression conv1d_wide(const Expression& x, const Expression& f);
// Expression kmax_pooling(const Expression& x, unsigned k);
// Expression fold_rows(const Expression& x, unsigned nrows=2);
// Expression sum_cols(const Expression& x);
// Expression kmh_ngram(const Expression& x, unsigned n);
//
// Expression sum_batches(const Expression& x);

// Expression pick(const Expression& x, unsigned v);
BOOST_AUTO_TEST_CASE( pick_gradient ) {
  unsigned idx = 1;
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  pick(x1, idx);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression pick(const Expression& x, unsigned* pv);
BOOST_AUTO_TEST_CASE( pickptr_gradient ) {
  unsigned idx = 1;
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  pick(x1, &idx);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression pickneglogsoftmax(const Expression& x, unsigned v);
BOOST_AUTO_TEST_CASE( pick_batch_gradient ) {
  std::vector<unsigned> idx = {1,2};
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = input(cg, Dim({3},2), batch_vals);
  sum_batches(pick(x1+x2, idx));
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression pickrange(const Expression& x, unsigned v, unsigned u);
BOOST_AUTO_TEST_CASE( pickrange_gradient ) {
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression y = pickrange(x1, 0, 2);
  input(cg, {1,2}, ones2_vals) * y;
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression pickneglogsoftmax(const Expression& x, unsigned v);
BOOST_AUTO_TEST_CASE( pickneglogsoftmax_gradient ) {
  unsigned idx = 1;
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  pickneglogsoftmax(x1, idx);
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

// Expression pickneglogsoftmax(const Expression& x, unsigned v);
BOOST_AUTO_TEST_CASE( pickneglogsoftmax_batch_gradient ) {
  std::vector<unsigned> idx = {1,2};
  cnn::ComputationGraph cg;
  Expression x1 = parameter(cg, param1);
  Expression x2 = input(cg, Dim({3},2), batch_vals);
  sum_batches(pickneglogsoftmax(x1+x2, idx));
  BOOST_CHECK(CheckGrad(mod, cg, 0));
}

BOOST_AUTO_TEST_SUITE_END()


/// \file
/// Maintainer: Luzian Hug
/// Created: 25.03.2018
///
///

#include "mean_shift_cpu_optimized.h"
#include "generic/erase_indices.h"
#include <Eigen/Dense>
#include <boost/log/trivial.hpp>
#include <iostream>

namespace MouseTrack {

MeanShiftCpuOptimized::MeanShiftCpuOptimized(double window_size)
    : MeanShift(window_size) {
  // empty
}

std::vector<Eigen::VectorXd>
MeanShiftCpuOptimized::convergePoints(const Oracle::PointList &points) const {

  const int dimensions = points.rows();

  std::vector<Eigen::VectorXd> currCenters;

  for (int i = 0; i < points.cols(); i += 1) {
    auto v = points.col(i);
    currCenters.push_back(v);
  }

  std::lock_guard<std::mutex> lock(_convergeOracleMutex);
  if (_cachedConvergeOracle.get() == nullptr) {
    OFactory::Query q;
    q.maxR = 2 * getWindowSize();
    q.dimensions = dimensions;
    _cachedConvergeOracle = oracleFactory().forQuery(q);
  }
  Oracle &oracle = *_cachedConvergeOracle;

  oracle.compute(points);

  // For each point...
  for (size_t i = 0; i < currCenters.size(); i++) {
    int iterations = 0; // for logging and abort condition

    Eigen::VectorXd prevCenter;
    // ... iterate until convergence
    do {
      iterations++;
      // perform one iteration of mean shift
      prevCenter = currCenters[i];
      std::vector<PointIndex> locals =
          oracle.find_in_range(currCenters[i], 2 * getWindowSize())[0];
      if (locals.empty()) {
        BOOST_LOG_TRIVIAL(warning)
            << "No points in neighborhood, falling back to brute force.";
        currCenters[i] = iterate_mode(currCenters[i], points);
        break;
      } else {
        PointList localPoints(points.rows(), locals.size());
        for (size_t i = 0; i < locals.size(); ++i) {
          const auto li = locals[i];
          localPoints.col(i) = points.col(li);
        }

        currCenters[i] = iterate_mode(currCenters[i], localPoints);
      }

      if (iterations > getMaxIterations()) {
        BOOST_LOG_TRIVIAL(warning)
            << "Max number of iterations for point " << i
            << " reached - continuing without convergence for this point";
        break;
      }
    } while ((prevCenter - currCenters[i]).norm() > getConvergenceThreshold());
    if (i % 1024 == 0) {
      BOOST_LOG_TRIVIAL(trace)
          << "converged i " << i << " after " << iterations << " iterations";
    }
  }
  return currCenters;
}

std::vector<Cluster> MeanShiftCpuOptimized::mergePoints(
    std::vector<Eigen::VectorXd> &currCenters) const {
  std::vector<Cluster> clusters;
  std::vector<int> remainingPoints(currCenters.size());
  std::iota(remainingPoints.begin(), remainingPoints.end(), 0);
  const int dimensions = currCenters[0].size();
  const size_t nPoints = currCenters.size();

  std::lock_guard<std::mutex> lock(_mergeOracleMutex);
  if (_cachedMergeOracle.get() == nullptr) {
    OFactory::Query q;
    q.maxR = getMergeThreshold();
    q.dimensions = dimensions;
    _cachedMergeOracle = oracleFactory().forQuery(q);
  }
  Oracle &mergeOracle = *_cachedMergeOracle;

  // For every mode..
  int merged = 0;
  int mergedTotal = 0;
  // as long as there are un-classified points:
  // pick first point and ask spatial oracle for it's neighbors within the merge
  // threshold and create a new cluster from it.
  //
  // Remove the clustered points from `remainingPoints` and repeat
  while (!remainingPoints.empty()) {
    Oracle::PointList points(dimensions, remainingPoints.size());

    for (size_t j = 0; j < remainingPoints.size(); j++) {
      points.col(j) = currCenters[remainingPoints[j]];
    }

    mergeOracle.compute(points);
    auto neighbors = mergeOracle.find_in_range(currCenters[remainingPoints[0]],
                                               getMergeThreshold())[0];
    // erase_indices assumes a sorted index vector
    std::sort(neighbors.begin(), neighbors.end());

    std::vector<size_t> cluster(neighbors.size());
    for (const auto ni : neighbors) {
      cluster.push_back(remainingPoints[ni]);
    }
    // check cluster
    {
      auto max = *std::max_element(cluster.begin(), cluster.end());
      assert(max < nPoints);
    }
    merged += cluster.size();
    mergedTotal += cluster.size();
    clusters.push_back(std::move(cluster));

    if (merged > 1024) {
      BOOST_LOG_TRIVIAL(trace)
          << mergedTotal << " points merged into " << clusters.size()
          << " clusters, remaining: " << remainingPoints.size();
      merged = 0;
    }

    erase_indices(remainingPoints, neighbors);
  }

  return clusters;
}

Eigen::VectorXd
MeanShiftCpuOptimized::iterate_mode(const Eigen::VectorXd &mode,
                                    const PointList &fixedPoints) const {
  auto w = (fixedPoints.colwise() - mode).colwise().squaredNorm();
  Eigen::RowVectorXd weights = (-w.array() / (2 * getWindowSize())).exp();
  assert(weights.rows() == 1);
  assert(weights.cols() == fixedPoints.cols());
  // COG Normalization Factor
  double normfact = weights.sum();
  Eigen::VectorXd cog =
      (fixedPoints.array().rowwise() * weights.array()).rowwise().sum();
  assert(cog.size() == mode.size());
  cog = cog * (1.0 / normfact);
  return cog;
}

} // namespace MouseTrack