#ifndef GEODB_BLOOM_FILTER_HPP
#define GEODB_BLOOM_FILTER_HPP

#include "geodb/common.hpp"
#include "geodb/type_traits.hpp"

#include <boost/math/constants/constants.hpp>
#include <boost/range/functions.hpp>
#include <boost/range/metafunctions.hpp>

#include <array>
#include <ostream>

/// \file
/// Contains a bloom filter implementation.

namespace geodb {

/// Returns the murmur3 hash for the given byte sequence.
std::array<u64, 2> murmur3(const u8* data, size_t size);

/// Returns the murmur3 hash for a given value.
/// The value is reinterpreted as a sequence of bytes, then hashed.
template<typename T>
std::array<u64, 2> murmur3(const T& value) {
    static_assert(std::is_trivially_copyable<T>::value, "");
    return murmur3(reinterpret_cast<const u8*>(std::addressof(value)), sizeof(value));
}

/// A basic bloom filter.
///
/// This class is trivially copyable and thus serializable by the TPIE IO classes.
/// It takes approximately `ceil(bits / 8)` bytes of storage.
template<typename T, u32 Bits>
class bloom_filter {
    static_assert(std::is_trivially_copyable<T>::value,
                  "The type must be trivially copyable, since its raw data "
                  "is being hashed.");

private:
    /// Returns the number of uint64_t fields required to store the
    /// requested number of bits (they are 64 bits each).
    static constexpr u32 fields() {
        return (Bits + 63) / 64;
    }

    /// The number of different hash values computed
    /// for every value.
    static constexpr u32 hashes() {
        return 5;
    }

    using data_t = std::array<u64, fields()>;

    using hashes_t = std::array<u64, hashes()>;

public:
    using value_type = T;

    static constexpr u32 bits() { return Bits; }

public:
    /// Returns the approximate error rate when inserting the given number of elements
    /// into a bloom filter with the current number of bits.
    static double error_rate(u64 elements) {
        double k = hashes();
        double n = elements;
        double m = Bits;
        return std::pow(1.0 - std::pow(1.0 - 1.0/m, k * n), k);
    }

    /// Computes the union of the given range of bloom filters.
    /// They must all be of this type.
    template<typename BloomFilters>
    static bloom_filter set_union(const BloomFilters& range) {
        bloom_filter result;
        for (const bloom_filter& filter : range) {
            for (u32 i = 0; i < fields(); ++i) {
                result.m_data[i] |= filter.m_data[i];
            }
        }
        return result;
    }

    /// Computes the intersection of the given range of bloom filters.
    /// They must all be of this type.
    template<typename BloomFilters>
    static bloom_filter set_intersection(const BloomFilters& range) {
        using const_iter = typename boost::range_const_iterator<const BloomFilters>::type;
        using value_type = typename boost::range_value<const BloomFilters>::type;

        static_assert(std::is_same<value_type, bloom_filter>::value,
                      "The range must contain bloom filter values.");

        const_iter first = boost::const_begin(range);
        const_iter last = boost::const_end(range);
        if (first == last) {
            return bloom_filter();
        }

        bloom_filter result = *first;
        ++first;
        for (; first != last; ++first) {
            const bloom_filter& filter = *first;

            for (u32 i = 0; i < fields(); ++i) {
                result.m_data[i] &= filter.m_data[i];
            }
        }
        return result;
    }

public:
    bloom_filter() = default;

    bloom_filter(std::initializer_list<T> items)
        : bloom_filter(items.begin(), items.end())
    {}

    template<typename Iter>
    bloom_filter(Iter first, Iter last) {
        for (; first != last; ++first) {
            add(*first);
        }
    }

    /// Inserts the value into the bloom filter.
    /// \post `contains(value) == true`.
    void add(const value_type& value) {
        for (u64 hash : compute_hashes(value)) {
            set(index(hash));
        }
    }

    /// Clears the instance and adds all values in the given iterator range.
    template<typename Iter>
    void assign(Iter first, Iter last) {
        clear();
        for (; first != last; ++first) {
            add(*first);
        }
    }

    /// Resets the instance.
    /// \post `empty() == true`.
    void clear() {
        m_data = data_t{};
    }

    /// Returns true if this bloom filter is empty,
    /// i.e. true iff all bits are zero.
    bool empty() const {
        for (u64 v : m_data) {
            if (v != 0) {
                return false;
            }
        }
        return true;
    }

    /// Returns true if the value is stored in this instance.
    ///
    /// This might be a false positive, but can never be
    /// a false negative.
    /// In other words, returning true means "probably".
    bool contains(const value_type& value) const {
        for (u64 hash : compute_hashes(value)) {
            if (!test(index(hash))) {
                return false;
            }
        }
        return true;
    }

    /// Returns the union of `*this` and `other`.
    bloom_filter union_with(const bloom_filter& other) const {
        bloom_filter result = *this;
        for (u32 i = 0; i < fields(); ++i) {
            result.m_data[i] |= other.m_data[i];
        }
        return result;
    }

    /// Returns the intersection of `*this` and `other`.
    bloom_filter intersection_with(const bloom_filter& other) const {
        bloom_filter result = *this;
        for (u32 i = 0; i < fields(); ++i) {
            result.m_data[i] &= other.m_data[i];
        }
        return result;
    }

private:
    /// Returns the bit index for the given hash.
    u32 index(u64 hash) const {
        return hash % Bits;
    }

    /// Computes the hashes for the given value by computing the 128 bit murmur hash.
    /// Then treats both 64 bit components of the murmur hash as independent hash functions
    /// (they're uniformly distributed, so this should be OK) and uses them to emulate N
    /// hash functions.
    ///
    /// For the last part, see "Building a better Bloom Filter" by Adam Kirsch and Michael Mitzenmacher.
    hashes_t compute_hashes(const value_type& value) const {
        std::array<u64, 2> murmur = murmur3(value);

        hashes_t result;
        for (u32 i = 0; i < hashes(); ++i) {
            result[i] = murmur[0] + i * murmur[1];
        }
        return result;
    }

    /// Sets the bit with the given index to "1".
    void set(u32 index) {
        geodb_assert(index < Bits, "bit index out of bounds");

        u32 element_index = index / 64;
        u32 bit_index = index % 64;
        m_data[element_index] |= (1 << bit_index);
    }

    /// Returns true if the bit with the given index is set.
    bool test(u32 index) const {
        geodb_assert(index < Bits, "bit index out of bounds");

        u32 element_index = index / 64;
        u32 bit_index = index % 64;
        return m_data[element_index] & (1 << bit_index);
    }

private:
    friend bool operator==(const bloom_filter& a, const bloom_filter& b) {
        for (u32 i = 0; i < fields(); ++i) {
            if (a.m_data[i] != b.m_data[i]) {
                return false;
            }
        }
        return true;
    }

    friend bool operator!=(const bloom_filter& a, const bloom_filter& b) {
        for (u32 i = 0; i < fields(); ++i) {
            if (a.m_data[i] == b.m_data[i]) {
                return false;
            }
        }
        return true;
    }

    friend std::ostream& operator<<(std::ostream& o, const bloom_filter& filter) {
        for (u32 i = 0; i < Bits; ++i) {
            o << (filter.test(i) ? '1' : '0');
        }
        return o;
    }

private:
    data_t m_data{};
};

} // namespace geodb

#endif // GEODB_BLOOM_FILTER_HPP


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt 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 Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include "bayesopt/bayesopt.hpp"

#include <boost/bind.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
//#include "randgen.hpp"
#include "lhs.hpp"
#include "gridsampling.hpp"
#include "log.hpp"

namespace bayesopt
{

  // DiscreteModel::DiscreteModel(const vecOfvec &validSet):
  //   BayesOptBase(), mInputSet(validSet)
  // {} // Constructor

  DiscreteModel::DiscreteModel( const vecOfvec &validSet,
				Parameters parameters):
    BayesOptBase(validSet[0].size(),parameters), mInputSet(validSet)
  {
    mDims = mInputSet[0].size();
  } // Constructor

  DiscreteModel::DiscreteModel(const vectori &categories,
			       Parameters parameters):
   BayesOptBase(categories.size(),parameters)
  {
    mDims = categories.size();
    utils::buildGrid(categories,mInputSet);
  }

  DiscreteModel::~DiscreteModel()
  {} // Default destructor

  // PROTECTED
  vectord DiscreteModel::samplePoint()
  {
    randInt sample(mEngine, intUniformDist(0,mInputSet.size()-1));
    return mInputSet[sample()];
  };

  void DiscreteModel::findOptimal(vectord &xOpt)
  {
    std::vector<double> critv(mInputSet.size());
    std::transform(mInputSet.begin(),mInputSet.end(),critv.begin(),
		   boost::bind(&DiscreteModel::evaluateCriteria,this,_1));

    xOpt = mInputSet[std::distance(critv.begin(),
			 std::max_element(critv.begin(),critv.end()))];

    // xOpt = *mInputSet.begin();
    // double min = evaluateCriteria(xOpt);

    // for(vecOfvec::iterator it = mInputSet.begin();
    // 	it != mInputSet.end(); ++it)
    //   {
    // 	double current = evaluateCriteria(*it);
    // 	if (current < min)
    // 	  {
    // 	    xOpt = *it;
    // 	    min = current;
    // 	  }
    //   }
  }

  //In this case, it is the trivial function
  vectord DiscreteModel::remapPoint(const vectord& x)
  { return x; }

  void DiscreteModel::generateInitialPoints(matrixd& xPoints)
  {

    vecOfvec perms = mInputSet;

    // By using random permutations, we guarantee that
    // the same point is not selected twice
    utils::randomPerms(perms,mEngine);

    // vectord xPoint(mInputSet[0].size());
    for(size_t i = 0; i < xPoints.size1(); i++)
    {
        const vectord xP = perms[i];
        row(xPoints,i) = xP;
    }
  }

}  // namespace bayesopt


//
// $Id: Chemistry.hpp 6865 2014-10-31 21:47:12Z chambm $
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2006 Louis Warschaw Prostate Cancer Center
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#ifndef _CHEMISTRY_HPP_
#define _CHEMISTRY_HPP_

#include "pwiz/utility/misc/Export.hpp"
#include <iosfwd>
#include <string>
#include <vector>
#include "pwiz/utility/misc/virtual_map.hpp"
#include <boost/shared_ptr.hpp>

namespace pwiz {
namespace chemistry {

/// the mass of a proton in unified atomic mass units
const double Proton   = 1.00727646688;

/// the mass of a neutron in unified atomic mass units
const double Neutron  = 1.00866491560;

/// the mass of an electron in unified atomic mass units
const double Electron = 0.00054857991;

/// struct for holding isotope information
struct PWIZ_API_DECL MassAbundance
{
    double mass;
    double abundance;

    MassAbundance(double m = 0, double a = 0)
    :   mass(m), abundance(a)
    {}

    bool operator==(const MassAbundance& that) const;
    bool operator!=(const MassAbundance& that) const;
};

/// struct for holding isotope distribution
typedef std::vector<MassAbundance> MassDistribution;

PWIZ_API_DECL std::ostream& operator<<(std::ostream& os, const MassAbundance& ma);
PWIZ_API_DECL std::ostream& operator<<(std::ostream& os, const MassDistribution& md);

/// scope for declarations related to elements
namespace Element {

/// enumeration of the elements
enum PWIZ_API_DECL Type
{
    C, H, O, N, S, P, _13C, _2H, _18O, _15N,
    He, Li, Be, B, F, Ne,
    Na, Mg, Al, Si, Cl, Ar, K, Ca,
    Sc, Ti, V, Cr, Mn, Fe, Co, Ni, Cu, Zn,
    Ga, Ge, As, Se, Br, Kr, Rb, Sr, Y, Zr,
    Nb, Mo, Tc, Ru, Rh, Pd, Ag, Cd, In, Sn,
    Sb, Te, I, Xe, Cs, Ba, La, Ce, Pr, Nd,
    Pm, Sm, Eu, Gd, Tb, Dy, Ho, Er, Tm, Yb,
    Lu, Hf, Ta, W, Re, Os, Ir, Pt, Au, Hg,
    Tl, Pb, Bi, Po, At, Rn, Fr, Ra, Ac, Th,
    Pa, U, Np, Pu, Am, Cm, Bk, Cf, Es, Fm,
    Md, No, Lr, Rf, Db, Sg, Bh, Hs, Mt, Uun,
    Uuu, Uub, Uuq, Uuh
};

PWIZ_API_DECL std::ostream& operator<<(std::ostream& os, Type type);

/// class for obtaining information about elements
namespace Info
{

struct PWIZ_API_DECL Record
{
    Type type;
    std::string symbol;
    int atomicNumber;
    double atomicWeight;
    MassAbundance monoisotope; /// the most abundant isotope
    MassDistribution isotopes;
};

/// retrieve the record for an element
PWIZ_API_DECL const Record& record(Type type);

/// retrieve the record for an element
PWIZ_API_DECL const Record& record(const std::string& symbol);

} // namespace Info

PWIZ_API_DECL std::ostream& operator<<(std::ostream& os, const Info::Record& record);

} // namespace Element

class CompositionMap;

/// class to represent a chemical formula
class PWIZ_API_DECL Formula
{
    public:

    /// formula string given by symbol/count pairs, e.g. water: "H2 O1" (whitespace optional)
    Formula(const std::string& formula = "");
    Formula(const char* formula);
    Formula(const Formula& formula);
    const Formula& operator=(const Formula& formula);
    ~Formula();

    double monoisotopicMass() const;
    double molecularWeight() const;
    std::string formula() const;

    /// access to the Element's count in the formula
    int operator[](Element::Type e) const;
    int& operator[](Element::Type e);

    // direct access to the map, for iteration
    typedef std::map<Element::Type, int> Map;
    Map data() const;

    // operations
    Formula& operator+=(const Formula& that);
    Formula& operator-=(const Formula& that);
    Formula& operator*=(int scalar);

    /// formulas are equal iff their elemental compositions are equal
    bool operator==(const Formula& that) const;
    bool operator!=(const Formula& that) const;

    private:
    class Impl;
    boost::shared_ptr<Impl> impl_;
};

PWIZ_API_DECL Formula operator+(const Formula& a, const Formula& b);
PWIZ_API_DECL Formula operator-(const Formula& a, const Formula& b);
PWIZ_API_DECL Formula operator*(const Formula& a, int scalar);
PWIZ_API_DECL Formula operator*(int scalar, const Formula& a);

/// output a Formula
PWIZ_API_DECL std::ostream& operator<<(std::ostream& os, const Formula& formula);

} // namespace chemistry
} // namespace pwiz

#endif // _CHEMISTRY_HPP_


// **************************************************************************/
//  作者: 陈雪飞<chenxuefei_pp@163.com>
//  日期: 2017年1月15日  14:53:16
//  工程: OpticalFlow
//  程序: OpticalFlow
//  文件: OpticalFlow.cpp
//  描述: 光流法裂纹检测
// **************************************************************************/

#include <opencv2/opencv.hpp>
#include <boost/thread.hpp>

//基本类型定义
//PI定义
const static double  PI = 3.14159265358979323846;
//特征点个数定义
const static uint    FEATURE_MAX_NUM = 600;
//图像二值化阈值
const static double  IMAGE_THRESHOLD = 127.00f;

/**
* \brief 创建窗口线程函数，将窗口独立到线程中
*/
void    output_windows_thread(const std::string &windowname)
{
    cv::namedWindow(windowname);
    cv::waitKeyEx();
}

/**
* \brief 定义平方函数
* \tparam _Ty 平方类型
* \param a 平方因子
* \return
*/
template<typename _Ty>
double square(_Ty a)
{
    return a * a;
}

/**
* \brief 两点之间线段长度
* \param p1 点向量1
* \param p2 点向量2
* \return 计算出的线段长度
*/
double  segment_length(const cv::Point & p1, const cv::Point &p2)
{
    return std::sqrt(square(p1.x - p2.x) + square(p1.y - p2.y));
}

/**
* \brief 定义图像处理函数
* \param name 处理方法名
*/
#define     DEF_IMG_HANDLER(name)   \
    void  name(cv::Mat &in,cv::Mat &out)

/**
* \brief 定义处理操作符
* \tparam Functor 处理方法
* \param img 处理图像
* \param f 方法
* \return 处理后的图像
*/
template <typename Functor>
cv::Mat  &operator << (cv::Mat &img, Functor f)
{
    f(img, img);
    return img;
}

/**
* \brief 灰度化处理方法
* \param in 原图像三通道
* \param out 输出图像，灰度图像
*/
DEF_IMG_HANDLER(cvtgray)
{
    assert(in.channels() == 3);
    cv::cvtColor(in, out, cv::COLOR_BGR2GRAY);
}

/**
* \brief 灰度图像转彩色图像
* \param in 灰度图像，单通道
* \param out 彩色图像，三通道
*/
DEF_IMG_HANDLER(cvtbgr)
{
    assert(in.channels() == 1);
    cv::cvtColor(in, out, cv::COLOR_GRAY2BGR);
}

/**
* \brief 图像转HSV格式
* \param in 源图像
* \param out hsv格式图像
*/
DEF_IMG_HANDLER(cvthsv)
{
    cv::cvtColor(in, out, cv::COLOR_BGR2HSV);
}

/**
* \brief 二值化图像
* \param in 传入图像
* \param out 二值化图像，单通道
*/
DEF_IMG_HANDLER(binary)
{
    //cv::threshold(in, out, IMAGE_THRESHOLD, 255.0f, CV_THRESH_BINARY);

    cv::adaptiveThreshold(in, out, 255.0f,
        cv::AdaptiveThresholdTypes::ADAPTIVE_THRESH_GAUSSIAN_C,
        cv::ThresholdTypes::THRESH_BINARY, 7, 0.0);
}

/**
* \brief 白平衡处理，均衡化三通道
* \param in 输入图像三通道
* \param out 输出图像三通道
*/
DEF_IMG_HANDLER(whiteblace)
{
    assert(in.channels() == 3);
    using namespace cv;
    Mat &g_srcImage = in, &dstImage = out;
    std::vector<Mat> g_vChannels;

    split(g_srcImage, g_vChannels);
    Mat imageBlueChannel = g_vChannels.at(0);
    Mat imageGreenChannel = g_vChannels.at(1);
    Mat imageRedChannel = g_vChannels.at(2);

    double imageBlueChannelAvg = 0;
    double imageGreenChannelAvg = 0;
    double imageRedChannelAvg = 0;

    //求各通道的平均值
    imageBlueChannelAvg = mean(imageBlueChannel)[0];
    imageGreenChannelAvg = mean(imageGreenChannel)[0];
    imageRedChannelAvg = mean(imageRedChannel)[0];

    //求出个通道所占增益
    double K = (imageRedChannelAvg + imageGreenChannelAvg + imageRedChannelAvg) / 3;
    double Kb = K / imageBlueChannelAvg;
    double Kg = K / imageGreenChannelAvg;
    double Kr = K / imageRedChannelAvg;

    //更新白平衡后的各通道BGR值
    addWeighted(imageBlueChannel, Kb, 0, 0, 0, imageBlueChannel);
    addWeighted(imageGreenChannel, Kg, 0, 0, 0, imageGreenChannel);
    addWeighted(imageRedChannel, Kr, 0, 0, 0, imageRedChannel);

    merge(g_vChannels, dstImage);//图像各通道合并
}

/**
* \brief 灰度直方图均衡化
* \param in 输入灰度图
* \param out 输出灰度图
*/
DEF_IMG_HANDLER(equalizeHist)
{
    assert(in.channels() == 1);
    cv::equalizeHist(in, out);
}

/**
* \brief 图像锐化，拉普拉斯算子
* \param in 输入图像
* \param out 输出图像
*/
DEF_IMG_HANDLER(sharpen)
{
    cv::Mat kernela(3, 3, CV_32F, cv::Scalar(0));
    kernela.at<float>(1, 1) = 5.0;
    kernela.at<float>(0, 1) = -1.0;
    kernela.at<float>(2, 1) = -1.0;
    kernela.at<float>(1, 0) = -1.0;
    kernela.at<float>(1, 2) = -1.0;
    filter2D(in, out, in.depth(), kernela);
}

/**
* \brief 对图像进行预处理
* \param base_image 待预处理的图像
*/
void handle_base_out_pre(cv::Mat& base_image)
{
    std::vector<cv::Mat> mv;

    //分离红色通道分量
    cv::split(base_image, mv);
    auto &red = mv.at(2);

    /// 膨胀操作
    int dilation_size = 1;
    auto element = getStructuringElement(cv::MORPH_RECT,
        cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
        cv::Point(dilation_size, dilation_size));
    dilate(red, red, element);
    dilate(red, red, element);
    dilate(red, red, element);
    //将分量组合
    cv::merge(mv, base_image);

    cv::imshow("superposition_window", base_image);

    cv::imwrite("叠加膨胀结果.png", base_image);
}
/**
* \brief 查找最大连通域
* \param base_image
*/
void handle_base_out_last(const cv::Mat& base_image, cv::Mat & backend_image)
{
    //最大面积区域需要显示几个
    const static  auto area_number = 3;

    cv::Mat		new_image = backend_image;

    assert(!new_image.empty());//检查是否为空

    std::vector<cv::Vec4i>          hierarchy;
    std::vector<cv::Mat>            contours;

    cv::Mat input;
    base_image.copyTo(input);

    cv::cvtColor(input, input, cv::COLOR_BGR2GRAY);

    input << binary;//二值化
    cv::imwrite("二值结果.png", input);

    cv::findContours(input, contours, hierarchy,
        cv::RetrievalModes::RETR_CCOMP,
        cv::ContourApproximationModes::CHAIN_APPROX_SIMPLE);

    //输出查找到的所有轮廓
    {
        cv::Mat all_outline;
        new_image.copyTo(all_outline);
        cv::drawContours(all_outline, contours, -1, CV_RGB(0, 255, 255), 1 , cv::LINE_AA);
        cv::imwrite("查找到的所有轮廓.png", all_outline);
    }

    /// Find the convex hull object for each contour
    std::vector<std::vector<cv::Point2i> >	hull(contours.size()), dest_hull;
    // Int type hull
    std::vector<std::vector<int>>	hullsI(contours.size());
    // Convexity defects
    std::vector<cv::Mat>			defects(contours.size());

    //凹点还原
    for (size_t i = 0; i < contours.size(); i++)
    {
        cv::convexHull(contours[i], hull[i], false);
        // find int type hull
        cv::convexHull(contours[i], hullsI[i], false);
        // get convexity defects
        convexityDefects(contours[i], hullsI[i], defects[i]);
    }

    //查找最大轮廓
    for (auto i = 0; i < area_number; ++i)
    {
        auto max_contours = std::max_element(hull.begin(), hull.end(), [](const std::vector<cv::Point> &l, const std::vector<cv::Point> &r)->bool
        {
            auto area1 = fabs(cv::contourArea(l));
            auto area2 = fabs(cv::contourArea(r));
            return area1 < area2;
        });
        dest_hull.push_back(*max_contours);
        hull.erase(max_contours);
    }

    cv::drawContours(new_image, dest_hull, -1, CV_RGB(0, 255, 255), 1 , cv::LINE_AA);

    //cv::drawContours(base_image, contours ,-1, CV_RGB(255, 0, 0),1,cv::LINE_AA);
    //根据凹点还原在原图连线，表示出最大连通点
    //for (size_t i = 0; i< contours.size(); i++)
    //{
    //	cv::Scalar color = CV_RGB(0,255,0);
    //	drawContours(new_image, hull, i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point2i());

    //	//auto d = defects[i].begin<cv::Vec4i>();
    //	//while (d != defects[i].end<cv::Vec4i>()) {
    //	//	cv::Vec4i& v = (*d);
    //	//	//if(IndexOfBiggestContour == i)
    //	//	{

    //	//		int startidx = v[0];
    //	//		cv::Point2i ptStart(contours[i].at<cv::Point2i>(startidx)); // point of the contour where the defect begins
    //	//		int endidx = v[1];
    //	//		cv::Point2i ptEnd(contours[i].at<cv::Point2i>(endidx)); // point of the contour where the defect ends
    //	//		int faridx = v[2];
    //	//		cv::Point2i ptFar(contours[i].at<cv::Point2i>(faridx));// the farthest from the convex hull point within the defect
    //	//		int depth = v[3] / 256; // distance between the farthest point and the convex hull

    //	//		if (depth > 20 && depth < 80)
    //	//		{
    //	//			//line(new_image, ptStart, ptFar, CV_RGB(0, 255, 0), 2);
    //	//			//line(new_image, ptEnd, ptFar, CV_RGB(0, 255, 0), 2);
    //	//			/*circle(base_image, ptStart, 4, cv::Scalar(255, 0, 100), 2);
    //	//			circle(base_image, ptEnd, 4, cv::Scalar(255, 0, 100), 2);
    //	//			circle(base_image, ptFar, 4, cv::Scalar(100, 0, 255), 2);*/
    //	//		}
    //	//	}
    //	//	++d;
    //	//}
    //}
    //cv::drawContours(base_image, defects, -1, CV_RGB(0, 255, 0), 1, cv::LINE_AA);
    cv::imshow("output_window", new_image);
    cv::imwrite("最终结果.png", new_image);
}
/**
* \brief 开始绘制运动轨迹 如需要
* \param m1 基本矩阵
* \param frame1_features 查找到的第一帧特征点
* \param frame2_features 查找到的第二帧特征点
* \param found_features 特征点错误矩阵
*/
void    draw_path_on_baseimage(cv::Mat &m1, const cv::Mat &frame1_features, const cv::Mat &frame2_features, const cv::Mat &found_features)
{
    const static cv::Scalar line_color = CV_RGB(255, 0, 0);

    for (auto pos = 0; pos < FEATURE_MAX_NUM; ++pos)
    {
        /* 如果没找到对应特征点 */
        if (found_features.at<uchar>(pos) == 0)
            continue;

        auto p = frame1_features.at<cv::Point2f>(pos);
        auto q = frame2_features.at<cv::Point2f>(pos);

        if (segment_length(p, q) > 20.0f)
            return;

        double angle;
        angle = atan2(static_cast<double>(p.y) - q.y, static_cast<double>(p.x) - q.x);
        double hypotenuse;
        hypotenuse = sqrt(square(p.y - q.y) + square(p.x - q.x));

        /*执行缩放*/
        q.x -= 5 * hypotenuse * cos(angle);
        q.y -= 5 * hypotenuse * sin(angle);

        /*画箭头主线*/
        cv::line(m1, p, q, line_color, 1, cv::LineTypes::LINE_AA, 0);

        /* 画箭的头部*/
        p.x = q.x + 9 * cos(angle + PI / 4);
        p.y = q.y + 9 * sin(angle + PI / 4);
        cv::line(m1, p, q, line_color, 1, cv::LineTypes::LINE_AA, 0);
        p.x = q.x + 9 * cos(angle - PI / 4);
        p.y = q.y + 9 * sin(angle - PI / 4);
        cv::line(m1, p, q, line_color, 1, cv::LineTypes::LINE_AA, 0);
    }
}
/**
* \brief 将特征点信息绘制到基础背景图像中去
* \param base_image 要绘制的图像
* \param frame1_features 第一帧特征点
* \param frame2_features 第二帧特征点
* \param found_features 特征点信息
*/
void draw_features_in_base_image(cv::Mat& base_image, const cv::Mat& frame1_features, const cv::Mat& frame2_features, const cv::Mat& found_features)
{
    const static cv::Scalar area_color = CV_RGB(255, 0, 0);

    for (auto pos = 0; pos < FEATURE_MAX_NUM; ++pos)
    {
        /* 如果没找到对应特征点 */
        if (found_features.at<uchar>(pos) == 0)
            continue;
        auto start_p = frame1_features.at<cv::Point2f>(pos);
        auto end_p = frame2_features.at<cv::Point2f>(pos);
        //如果线段长度大于某个值，说明不是细微运动则排除该线段
        if (segment_length(start_p, end_p) > 20.0f)
            return;

        cv::line(base_image, start_p, end_p, area_color, 3, cv::LINE_AA);
    }
}
/**
* \brief 打开视频文件并返回帧数
* \param capture 视频句柄
* \param filename 文件名
* \return 返回帧数
*/
int open_video(cv::VideoCapture & capture, const std::string &filename)
{
    //打开视频文件
    capture.open(filename);
    auto  frame_count = 0; //总帧长

    assert(capture.isOpened());//检查是否打开

    //获取总帧数
    frame_count = static_cast<int>(capture.get(cv::VideoCaptureProperties::CAP_PROP_FRAME_COUNT));

    //进度条，可以查看处理了多少帧
    return frame_count;
}

/**
* \brief 开始LK光流法算法
* \param base_out 叠加输出基本源图像
*/
void draw_features_by_lk(cv::Mat & base_out, const std::string & in_video_name, const std::string & out_video_name)
{
    //打开视频文件并且创建进度条
    cv::VideoCapture capture{};
    auto frame_count = open_video(capture, in_video_name);
    //boost::progress_display progress_bar(frame_count, std::cout, "当前处理进度\n\t\t", "总进度\t\t", "当前进度\t");
    //打开输出视频文件 如需要
    cv::VideoWriter writer(out_video_name, CV_FOURCC('X', 'V', 'I', 'D'), 29.0, cv::Size{ 656,488 });

    //如果还有新帧则继续
    while (capture.grab())
    {
        cv::Mat     m1, m2, frame1, frame2;
        //取出视频两帧
        capture >> m1;
        capture >> m2;
        //如果返回不足两帧则退出
        if (m1.empty() || m2.empty())
            break;
        //复制到新空间
        frame1 = m1;
        frame2 = m2;
        //两帧灰度化
        frame1 << cvtgray;
        frame2 << cvtgray;

        //创建两个特征值存储矩阵
        cv::Mat frame1_features, frame2_features;
        //开始进行特征值检测，先检测出第一帧特征值
        cv::goodFeaturesToTrack(frame1, frame1_features, FEATURE_MAX_NUM, .01, .01);
        //创建查找错误矩阵
        cv::Mat found_futures, found_err;
        //创建迭代算子
        cv::TermCriteria optical_flow_termination_criteria(cv::TermCriteria::Type::MAX_ITER | cv::TermCriteria::Type::EPS, 20, .3);
        //开始进行LK光流法计算，查找第二个特征值
        cv::calcOpticalFlowPyrLK(
            frame1, frame2,
            frame1_features, frame2_features,
            found_futures, found_err,
            cv::Size{ 5,5 }, 5, optical_flow_termination_criteria);
        //绘制移动轨迹 ， 如需要
        draw_path_on_baseimage(m1, frame1_features, frame2_features, found_futures);
        //绘制运动轨迹线到背景矩阵
        draw_features_in_base_image(base_out, frame1_features, frame2_features, found_futures);

        //进度条更新
        //progress_bar += 2;

        //将运动轨迹线展示到窗口，如需要
        cv::imshow("output_window", m1);
        cv::imshow("superposition_window", base_out);
        //将运动轨迹线输出到视频文件 如需要
        writer << m1;
        //writer << (out << cvtbgr);
    }

    writer.release();
    capture.release();
}

/**
* \brief 主函数
* \return
*/
int main(int argc, char ** args)
{
    if (argc < 3)
    {
        std::cout << "请输入输入视频文件名,输入背景图像文件名,(或者输出视频文件名),按顺序空格分离." << std::endl;
        std::cout << "Example:\n\tOpticalFlow 输入视频.avi 输入背景.jpg [输出视频.avi]" << std::endl;
        exit(-1);
    }
    std::string input_video_filename = args[1];
    std::string base_image_filename = args[2];
    std::string base_output_filename = "VideoOut.avi";
    if (argc == 4)
        base_output_filename = args[3];

    //创建窗口
    auto  output_window_fu = boost::thread(output_windows_thread, "output_window");
    auto  superposition_window_fu = boost::thread(output_windows_thread, "superposition_window");

    //boost::thread(output_windows_thread).detach();
    //打开背景图像
    auto            base_image = cv::imread(base_image_filename);
    //打开是否失败
    assert(!base_image.empty());

    //创建空白背景图像
    cv::Mat         base_out{ base_image.rows,base_image.cols,base_image.type() };
    //cv::Mat          base_out = cv::imread("post.png");
    //开始进行LK光流算法，将结果图像输出到base out
    draw_features_by_lk(base_out, input_video_filename, base_output_filename);

    /*
    * ！！！！此处开始对LK算法处理后的图像进行处理！！！！
    */
    //对叠加输出图片进行预处理
    handle_base_out_pre(base_out);
    //对预处理图片进行整合处理，边缘过滤，凹点过滤
    handle_base_out_last(base_out, base_image);

    //等待窗口程序结束
    superposition_window_fu.join();
    output_window_fu.join();
    return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE LibmeshVariableFieldTester
#include <boost/test/included/unit_test.hpp>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <vector>

#include <LibmeshManager.hpp>
#include <LibmeshVariableField.hpp>

#include <Teuchos_Array.hpp>
#include <Teuchos_ArrayRCP.hpp>
#include <Teuchos_Comm.hpp>
#include <Teuchos_DefaultComm.hpp>
#include <Teuchos_DefaultMpiComm.hpp>
#include <Teuchos_RCP.hpp>

#include <Tpetra_CrsMatrix.hpp>
#include <Tpetra_MultiVector.hpp>

#include <libmesh/cell_hex8.h>
#include <libmesh/dof_map.h>
#include <libmesh/enum_elem_type.h>
#include <libmesh/equation_systems.h>
#include <libmesh/explicit_system.h>
#include <libmesh/libmesh.h>
#include <libmesh/linear_partitioner.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/node.h>
#include <libmesh/parallel.h>
#include <libmesh/point.h>
#include <libmesh/quadrature_gauss.h>
#include <libmesh/system.h>

//---------------------------------------------------------------------------//
BOOST_AUTO_TEST_CASE( checkLibmeshVariableField ) {
	const std::string argv_string = "unit_test --keep-cout";
	const char *argv_char = argv_string.c_str();
	Teuchos::GlobalMPISession mpiSession(
			&boost::unit_test::framework::master_test_suite().argc,
			&boost::unit_test::framework::master_test_suite().argv);
	auto comm = Teuchos::DefaultComm<int>::getComm();
	auto mpi_comm = Teuchos::rcp_dynamic_cast<const Teuchos::MpiComm<int>>(
			comm);
	auto opaque_comm = mpi_comm->getRawMpiComm();
	auto raw_comm = (*opaque_comm)();

	// Create the mesh.
	BOOST_VERIFY(!libMesh::initialized());
	libMesh::LibMeshInit libmesh_init(1, &argv_char, raw_comm);
	BOOST_VERIFY(libMesh::initialized());
	BOOST_VERIFY((int )libmesh_init.comm().rank() == comm->getRank());
	Teuchos::RCP<libMesh::Mesh> mesh = Teuchos::rcp(
			new libMesh::Mesh(libmesh_init.comm()));
	libMesh::MeshTools::Generation::build_cube(*mesh, 4, 4, 4, 0.0, 1.0, 0.0,
			1.0, 0.0, 1.0, libMesh::HEX8);

	// Parition the mesh.
	libMesh::LinearPartitioner partitioner;
	partitioner.partition(*mesh);

	// Check libmesh validity.
	mesh->libmesh_assert_valid_parallel_ids();

	// Make a libmesh system. We will put a first order linear basis on the
	// elements for all subdomains.
	std::string var_name = "test_var";
	libMesh::EquationSystems equation_systems(*mesh);
	libMesh::ExplicitSystem &system = equation_systems.add_system<
			libMesh::ExplicitSystem>("Test System");
	int var_id = system.add_variable(var_name);
	equation_systems.init();

	// Put some data in the variable.
	int sys_id = system.number();
	libMesh::MeshBase::node_iterator nodes_begin = mesh->local_nodes_begin();
	libMesh::MeshBase::node_iterator nodes_end = mesh->local_nodes_end();
	for (auto node_it = nodes_begin; node_it != nodes_end; ++node_it) {
		auto dof_id = (*node_it)->dof_number(sys_id, var_id, 0);
		system.solution->set(dof_id, (*node_it)->id());
	}
	system.solution->close();
	system.update();

	// Create a vector from the variable over all subdomains.
	LibmeshAdapter::LibmeshManager manager(mesh, Teuchos::rcpFromRef(system));
	auto var_vec = manager.createFieldMultiVector(var_name);

	// Test the vector size.
	unsigned int num_nodes = mesh->n_local_nodes();
	BOOST_VERIFY(1 == Teuchos::as<int>(var_vec->getNumVectors()));
	BOOST_VERIFY(num_nodes == var_vec->getLocalLength());
	BOOST_VERIFY(125 == var_vec->getGlobalLength());

	// Test the variable data and add some.
	var_vec->pullDataFromApplication();
	auto var_vec_view = var_vec->get2dViewNonConst();
	int n = 0;
	for (auto node_it = nodes_begin; node_it != nodes_end; ++node_it) {
		BOOST_VERIFY(var_vec_view[0][n] == (*node_it)->id());
		var_vec_view[0][n] += 1.0;
		++n;
	}

	// Push the data back to Libmesh
	var_vec->pushDataToApplication();

	// Test the Libmesh variable.
	for (auto node = nodes_begin; node != nodes_end; ++node) {
		auto dof_id = (*node)->dof_number(sys_id, var_id, 0);
		BOOST_VERIFY(system.solution->el(dof_id) == (*node)->id() + 1);
	}
}

//---------------------------------------------------------------------------//
// end of tstLibmeshVariableField.cpp
//---------------------------------------------------------------------------//


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_DEFINITION_SIGNMASK_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_SIGNMASK_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct signmask_ : boost::dispatch::constant_value_<signmask_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,signmask_,boost::dispatch::constant_value_<signmask_>);

      struct value_map
      {
        template<typename X>
        static std::integral_constant<X,-127-1> value(boost::dispatch::int8_<X> const&);

        template<typename X>
        static std::integral_constant<X,-32767-1> value(boost::dispatch::int16_<X> const&);

        template<typename X>
        static std::integral_constant<X,-2147483647-1> value(boost::dispatch::int32_<X> const&);

        template<typename X>
        static std::integral_constant<X,-9223372036854775807LL-1> value(boost::dispatch::int64_<X> const&);

        template<typename X>
        static std::integral_constant<X,0x80U> value(boost::dispatch::uint8_<X> const&);

        template<typename X>
        static std::integral_constant<X,0x8000U> value(boost::dispatch::uint16_<X> const&);

        template<typename X>
        static std::integral_constant<X,0x80000000UL> value(boost::dispatch::uint32_<X> const&);

        template<typename X>
        static std::integral_constant<X,0x8000000000000000ULL> value(boost::dispatch::uint64_<X> const&);

        template<typename X>
        static brigand::single_<0x80000000UL> value(boost::dispatch::single_<X> const&);

        template<typename X>
        static brigand::double_<0x8000000000000000ULL> value(boost::dispatch::double_<X> const&);
      };

    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, signmask_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::signmask_,signmask);
  }

  template<typename T> BOOST_FORCEINLINE auto Signmask()
  BOOST_NOEXCEPT_DECLTYPE(detail::signmask( boost::dispatch::as_<T>{}))
  {
    return detail::signmask( boost::dispatch::as_<T>{} );
  }
} }

#endif


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <functional>
#include <unordered_map>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <sdf_tools/sdf.hpp>
#include <sdf_tools/CollisionMap.h>

#include <eigen3/Eigen/Dense>

#ifndef COLLISION_MAP_HPP
#define COLLISION_MAP_HPP

#define ENABLE_UNORDERED_MAP_SIZE_HINTS

namespace sdf_tools
{
    struct COLLISION_CELL
    {
        float occupancy;
        uint32_t component;

        COLLISION_CELL() : occupancy(0.0), component(0) {}

        COLLISION_CELL(const float in_occupancy) : occupancy(in_occupancy), component(0) {}

        COLLISION_CELL(const float in_occupancy, const uint32_t in_component) : occupancy(in_occupancy), component(in_component) {}
    };

    inline std::vector<uint8_t> CollisionCellToBinary(const COLLISION_CELL& value)
    {
        std::vector<uint8_t> binary(sizeof(COLLISION_CELL));
        memcpy(&binary.front(), &value, sizeof(COLLISION_CELL));
        return binary;
    }

    inline COLLISION_CELL CollisionCellFromBinary(const std::vector<uint8_t>& binary)
    {
        if (binary.size() != sizeof(COLLISION_CELL))
        {
            std::cerr << "Binary value is not " << sizeof(COLLISION_CELL) << " bytes" << std::endl;
            return COLLISION_CELL(NAN, 0u);
        }
        else
        {
            COLLISION_CELL loaded;
            memcpy(&loaded, &binary.front(), sizeof(COLLISION_CELL));
            return loaded;
        }
    }

    class CollisionMapGrid
    {
    protected:

        inline static std_msgs::ColorRGBA GenerateComponentColor(const uint32_t component, const float alpha=1.0f)
        {
            return arc_helpers::GenerateUniqueColor<std_msgs::ColorRGBA>(component, alpha);
        }

        inline bool IsSurfaceIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            // First, we make sure that indices are within bounds
            // Out of bounds indices are NOT surface cells
            if (x_index < 0 || y_index < 0 || z_index < 0 || x_index >= GetNumXCells() || y_index >= GetNumYCells() || z_index >= GetNumZCells())
            {
                return false;
            }
            // Edge indices are automatically surface cells
            if (x_index == 0 || y_index == 0 || z_index == 0 || x_index == (GetNumXCells() - 1) || y_index == (GetNumYCells() - 1) || z_index == (GetNumZCells()))
            {
                return true;
            }
            // If the cell is inside the grid, we check the neighbors
            // Note that we must check all 26 neighbors
            uint32_t our_component = collision_field_.GetImmutable(x_index, y_index, z_index).first.component;
            // Check neighbor 1
            if (our_component != collision_field_.GetImmutable(x_index, y_index, z_index - 1).first.component)
            {
                return true;
            }
            // Check neighbor 2
            else if (our_component != collision_field_.GetImmutable(x_index, y_index, z_index + 1).first.component)
            {
                return true;
            }
            // Check neighbor 3
            else if (our_component != collision_field_.GetImmutable(x_index, y_index - 1, z_index).first.component)
            {
                return true;
            }
            // Check neighbor 4
            else if (our_component != collision_field_.GetImmutable(x_index, y_index + 1, z_index).first.component)
            {
                return true;
            }
            // Check neighbor 5
            else if (our_component != collision_field_.GetImmutable(x_index - 1, y_index, z_index).first.component)
            {
                return true;
            }
            // Check neighbor 6
            else if (our_component != collision_field_.GetImmutable(x_index + 1, y_index, z_index).first.component)
            {
                return true;
            }
            // If none of the faces are exposed, it's not a surface voxel
            return false;
        }

        typedef struct
        {
            uint32_t location[3];
            uint32_t closest_point[3];
            double distance_square;
            int32_t update_direction;
        } bucket_cell;

        typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;

        inline DistanceField BuildDistanceField(const std::vector<VoxelGrid::GRID_INDEX>& points) const
        {
            // Make the DistanceField container
            bucket_cell default_cell;
            default_cell.distance_square = INFINITY;
            DistanceField distance_field(collision_field_.GetOriginTransform(), GetResolution(), collision_field_.GetXSize(), collision_field_.GetYSize(), collision_field_.GetZSize(), default_cell);
            // Compute maximum distance square
            long max_distance_square = (distance_field.GetNumXCells() * distance_field.GetNumXCells()) + (distance_field.GetNumYCells() * distance_field.GetNumYCells()) + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
            // Make bucket queue
            std::vector<std::vector<bucket_cell>> bucket_queue(max_distance_square + 1);
            bucket_queue[0].reserve(points.size());
            // Set initial update direction
            int initial_update_direction = GetDirectionNumber(0, 0, 0);
            // Mark all points with distance zero and add to the bucket queue
            for (size_t index = 0; index < points.size(); index++)
            {
                const VoxelGrid::GRID_INDEX& current_index = points[index];
                std::pair<bucket_cell&, bool> query = distance_field.GetMutable(current_index);
                if (query.second)
                {
                    query.first.location[0] = current_index.x;
                    query.first.location[1] = current_index.y;
                    query.first.location[2] = current_index.z;
                    query.first.closest_point[0] = current_index.x;
                    query.first.closest_point[1] = current_index.y;
                    query.first.closest_point[2] = current_index.z;
                    query.first.distance_square = 0.0;
                    query.first.update_direction = initial_update_direction;
                    bucket_queue[0].push_back(query.first);
                }
                // If the point is outside the bounds of the SDF, skip
                else
                {
                    continue;
                }
            }
            // Process the bucket queue
            std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods = MakeNeighborhoods();
            for (size_t bq_idx = 0; bq_idx < bucket_queue.size(); bq_idx++)
            {
                std::vector<bucket_cell>::iterator queue_itr = bucket_queue[bq_idx].begin();
                while (queue_itr != bucket_queue[bq_idx].end())
                {
                    // Get the current location
                    bucket_cell& cur_cell = *queue_itr;
                    double x = cur_cell.location[0];
                    double y = cur_cell.location[1];
                    double z = cur_cell.location[2];
                    // Pick the update direction
                    int D = bq_idx;
                    if (D > 1)
                    {
                        D = 1;
                    }
                    // Make sure the update direction is valid
                    if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
                    {
                        ++queue_itr;
                        continue;
                    }
                    // Get the current neighborhood list
                    std::vector<std::vector<int>>& neighborhood = neighborhoods[D][cur_cell.update_direction];
                    // Update the distance from the neighboring cells
                    for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
                    {
                        // Get the direction to check
                        int dx = neighborhood[nh_idx][0];
                        int dy = neighborhood[nh_idx][1];
                        int dz = neighborhood[nh_idx][2];
                        int nx = x + dx;
                        int ny = y + dy;
                        int nz = z + dz;
                        std::pair<bucket_cell&, bool> neighbor_query = distance_field.GetMutable((int64_t)nx, (int64_t)ny, (int64_t)nz);
                        if (!neighbor_query.second)
                        {
                            // "Neighbor" is outside the bounds of the SDF
                            continue;
                        }
                        // Update the neighbor's distance based on the current
                        int new_distance_square = ComputeDistanceSquared(nx, ny, nz, cur_cell.closest_point[0], cur_cell.closest_point[1], cur_cell.closest_point[2]);
                        if (new_distance_square > max_distance_square)
                        {
                            // Skip these cases
                            continue;
                        }
                        if (new_distance_square < neighbor_query.first.distance_square)
                        {
                            // If the distance is better, time to update the neighbor
                            neighbor_query.first.distance_square = new_distance_square;
                            neighbor_query.first.closest_point[0] = cur_cell.closest_point[0];
                            neighbor_query.first.closest_point[1] = cur_cell.closest_point[1];
                            neighbor_query.first.closest_point[2] = cur_cell.closest_point[2];
                            neighbor_query.first.location[0] = nx;
                            neighbor_query.first.location[1] = ny;
                            neighbor_query.first.location[2] = nz;
                            neighbor_query.first.update_direction = GetDirectionNumber(dx, dy, dz);
                            // Add the neighbor into the bucket queue
                            bucket_queue[new_distance_square].push_back(neighbor_query.first);
                        }
                    }
                    // Increment the queue iterator
                    ++queue_itr;
                }
                // Clear the current queue now that we're done with it
                bucket_queue[bq_idx].clear();
            }
            return distance_field;
        }

        inline std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods()  const
        {
            std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods;
            neighborhoods.resize(2);
            for (size_t n = 0; n < neighborhoods.size(); n++)
            {
                neighborhoods[n].resize(27);
                // Loop through the source directions
                for (int dx = -1; dx <= 1; dx++)
                {
                    for (int dy = -1; dy <= 1; dy++)
                    {
                        for (int dz = -1; dz <= 1; dz++)
                        {
                            int direction_number = GetDirectionNumber(dx, dy, dz);
                            // Loop through the target directions
                            for (int tdx = -1; tdx <= 1; tdx++)
                            {
                                for (int tdy = -1; tdy <= 1; tdy++)
                                {
                                    for (int tdz = -1; tdz <= 1; tdz++)
                                    {
                                        if (tdx == 0 && tdy == 0 && tdz == 0)
                                        {
                                            continue;
                                        }
                                        if (n >= 1)
                                        {
                                            if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
                                            {
                                                continue;
                                            }
                                            if ((dx * tdx) < 0 || (dy * tdy) < 0 || (dz * tdz) < 0)
                                            {
                                                continue;
                                            }
                                        }
                                        std::vector<int> new_point;
                                        new_point.resize(3);
                                        new_point[0] = tdx;
                                        new_point[1] = tdy;
                                        new_point[2] = tdz;
                                        neighborhoods[n][direction_number].push_back(new_point);
                                    }
                                }
                            }
                        }
                    }
                }
            }
            return neighborhoods;
        }

        inline int GetDirectionNumber(const int dx, const int dy, const int dz) const
        {
            return ((dx + 1) * 9) + ((dy + 1) * 3) + (dz + 1);
        }

        inline double ComputeDistanceSquared(const int32_t x1, const int32_t y1, const int32_t z1, const int32_t x2, const int32_t y2, const int32_t z2) const
        {
            int32_t dx = x1 - x2;
            int32_t dy = y1 - y2;
            int32_t dz = z1 - z2;
            return double((dx * dx) + (dy * dy) + (dz * dz));
        }

        VoxelGrid::VoxelGrid<COLLISION_CELL> collision_field_;
        uint32_t number_of_components_;
        std::string frame_;
        bool initialized_;
        bool components_valid_;

        std::vector<uint8_t> PackBinaryRepresentation(std::vector<COLLISION_CELL>& raw);

        std::vector<COLLISION_CELL> UnpackBinaryRepresentation(std::vector<uint8_t>& packed);

        int64_t MarkConnectedComponent(int64_t x_index, int64_t y_index, int64_t z_index, uint32_t connected_component);

    public:

        inline CollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const COLLISION_CELL& default_value, const COLLISION_CELL& OOB_value) : initialized_(true)
        {
            frame_ = frame;
            VoxelGrid::VoxelGrid<COLLISION_CELL> new_field(resolution, x_size, y_size, z_size, default_value, OOB_value);
            collision_field_ = new_field;
            number_of_components_ = 0;
            components_valid_ = false;
        }

        inline CollisionMapGrid(const Eigen::Affine3d& origin_transform, const std::string& frame, const double resolution, const double x_size, double y_size, const double z_size, const COLLISION_CELL& default_value, const COLLISION_CELL& OOB_value) : initialized_(true)
        {
            frame_ = frame;
            VoxelGrid::VoxelGrid<COLLISION_CELL> new_field(origin_transform, resolution, x_size, y_size, z_size, default_value, OOB_value);
            collision_field_ = new_field;
            number_of_components_ = 0;
            components_valid_ = false;
        }

        inline CollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const COLLISION_CELL& OOB_default_value) : initialized_(true)
        {
            frame_ = frame;
            VoxelGrid::VoxelGrid<COLLISION_CELL> new_field(resolution, x_size, y_size, z_size, OOB_default_value);
            collision_field_ = new_field;
            number_of_components_ = 0;
            components_valid_ = false;
        }

        inline CollisionMapGrid(const Eigen::Affine3d& origin_transform, const std::string& frame, const double resolution, const double x_size, double y_size, const double z_size, const COLLISION_CELL& OOB_default_value) : initialized_(true)
        {
            frame_ = frame;
            VoxelGrid::VoxelGrid<COLLISION_CELL> new_field(origin_transform, resolution, x_size, y_size, z_size, OOB_default_value);
            collision_field_ = new_field;
            number_of_components_ = 0;
            components_valid_ = false;
        }

        inline CollisionMapGrid() : number_of_components_(0), initialized_(false), components_valid_(false) {}

        inline bool IsInitialized() const
        {
            return initialized_;
        }

        inline bool AreComponentsValid() const
        {
            return components_valid_;
        }

        inline std::pair<COLLISION_CELL, bool> Get3d(const Eigen::Vector3d& location) const
        {
            return collision_field_.GetImmutable3d(location);
        }

        inline std::pair<COLLISION_CELL, bool> Get4d(const Eigen::Vector4d& location) const
        {
            return collision_field_.GetImmutable4d(location);
        }

        inline std::pair<COLLISION_CELL, bool> Get(const double x, const double y, const double z) const
        {
            return collision_field_.GetImmutable(x, y, z);
        }

        inline std::pair<COLLISION_CELL, bool> Get(const VoxelGrid::GRID_INDEX& index) const
        {
            return collision_field_.GetImmutable(index);
        }

        inline std::pair<COLLISION_CELL, bool> Get(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return collision_field_.GetImmutable(x_index, y_index, z_index);
        }

        inline bool Set(const double x, const double y, const double z, COLLISION_CELL value)
        {
            components_valid_ = false;
            return collision_field_.SetValue(x, y, z, value);
        }

        void Set3d(const Eigen::Vector3d& location, COLLISION_CELL value)
        {
            //components_valid_ = false;
            collision_field_.SetValue3d(location, value);
        }

        inline bool Set4d(const Eigen::Vector4d& location, COLLISION_CELL value)
        {
            components_valid_ = false;
            return collision_field_.SetValue4d(location, value);
        }

        inline bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, COLLISION_CELL value)
        {
            components_valid_ = false;
            return collision_field_.SetValue(x_index, y_index, z_index, value);
        }

        inline bool Set(const VoxelGrid::GRID_INDEX& index, COLLISION_CELL value)
        {
            components_valid_ = false;
            return collision_field_.SetValue(index, value);
        }

        inline double GetXSize() const
        {
            return collision_field_.GetXSize();
        }

        inline double GetYSize() const
        {
            return collision_field_.GetYSize();
        }

        inline double GetZSize() const
        {
            return collision_field_.GetZSize();
        }

        inline double GetResolution() const
        {
            return collision_field_.GetCellSizes()[0];
        }

        inline COLLISION_CELL GetDefaultValue() const
        {
            return collision_field_.GetDefaultValue();
        }

        inline COLLISION_CELL GetOOBValue() const
        {
            return collision_field_.GetOOBValue();
        }

        inline int64_t GetNumXCells() const
        {
            return collision_field_.GetNumXCells();
        }

        inline int64_t GetNumYCells() const
        {
            return collision_field_.GetNumYCells();
        }

        inline int64_t GetNumZCells() const
        {
            return collision_field_.GetNumZCells();
        }

        inline const Eigen::Affine3d& GetOriginTransform() const
        {
            return collision_field_.GetOriginTransform();
        }

        inline const Eigen::Affine3d& GetInverseOriginTransform() const
        {
            return collision_field_.GetInverseOriginTransform();
        }

        inline std::string GetFrame() const
        {
            return frame_;
        }

        inline std::pair<uint32_t, bool> GetNumConnectedComponents() const
        {
            return std::pair<uint32_t, bool>(number_of_components_, components_valid_);
        }

        inline std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const
        {
            return collision_field_.LocationToGridIndex3d(location);
        }

        inline std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const
        {
            return collision_field_.LocationToGridIndex4d(location);
        }

        inline std::vector<int64_t> LocationToGridIndex(double x, double y, double z) const
        {
            return collision_field_.LocationToGridIndex(x, y, z);
        }

        inline bool Inside(Eigen::Vector3i index) const
        {
            return collision_field_.IndexInBounds(index(0), index(1), index(2));
        }

        inline Eigen::Vector3i LocationToGridIndex(Eigen::Vector3d location) const
        {
            return collision_field_.LocationToGridIndex(location);
        }

        inline std::vector<double> GridIndexToLocation(int64_t x_index, int64_t y_index, int64_t z_index) const
        {
            return collision_field_.GridIndexToLocation(x_index, y_index, z_index);
        }

        inline Eigen::Vector3d GridIndexToLocation(Eigen::Vector3i index) const
        {
            return collision_field_.GridIndexToLocation(index);
        }

        bool SaveToFile(const std::string& filepath);

        bool LoadFromFile(const std::string &filepath);

        sdf_tools::CollisionMap GetMessageRepresentation();

        bool LoadFromMessageRepresentation(sdf_tools::CollisionMap& message);

        uint32_t UpdateConnectedComponents();

        std::map<uint32_t, std::pair<int32_t, int32_t>> ComputeComponentTopology(bool ignore_empty_components, bool recompute_connected_components, bool verbose);

        std::map<uint32_t, std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>> ExtractComponentSurfaces(const bool ignore_empty_components) const;

        std::pair<int32_t, int32_t> ComputeHolesInSurface(const uint32_t component, const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const bool verbose) const;

        int32_t ComputeConnectivityOfSurfaceVertices(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface_vertex_connectivity) const;

        inline std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const float oob_value) const
        {
            // Make the SDF
            SignedDistanceField new_sdf(collision_field_.GetOriginTransform(), frame_, GetResolution(), collision_field_.GetXSize(), collision_field_.GetYSize(), collision_field_.GetZSize(), oob_value);
            std::vector<VoxelGrid::GRID_INDEX> filled;
            std::vector<VoxelGrid::GRID_INDEX> free;
            for (int64_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++)
            {
                for (int64_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++)
                {
                    for (int64_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++)
                    {
                        VoxelGrid::GRID_INDEX current_index(x_index, y_index, z_index);
                        COLLISION_CELL stored = Get(x_index, y_index, z_index).first;
                        if (stored.occupancy > 0.5)
                        {
                            // Mark as filled
                            filled.push_back(current_index);
                        }
                        else
                        {
                            // Mark as free space
                            free.push_back(current_index);
                        }
                    }
                }
            }
            // Make two distance fields (one for distance to filled voxels, one for distance to free voxels
            DistanceField filled_distance_field = BuildDistanceField(filled);
            DistanceField free_distance_field = BuildDistanceField(free);
            // Generate the SDF
            double max_distance = -INFINITY;
            double min_distance = INFINITY;
            for (int64_t x_index = 0; x_index < filled_distance_field.GetNumXCells(); x_index++)
            {
                for (int64_t y_index = 0; y_index < filled_distance_field.GetNumYCells(); y_index++)
                {
                    for (int64_t z_index = 0; z_index < filled_distance_field.GetNumZCells(); z_index++)
                    {
                        double distance1 = sqrt(filled_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * new_sdf.GetResolution();
                        double distance2 = sqrt(free_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * new_sdf.GetResolution();
                        double distance = distance1 - distance2;
                        //double distance = distance1;
                        if (distance > max_distance)
                        {
                            max_distance = distance;
                        }
                        if (distance < min_distance)
                        {
                            min_distance = distance;
                        }
                        new_sdf.Set(x_index, y_index, z_index, distance);
                    }
                }
            }
            std::pair<double, double> extrema(max_distance, min_distance);
            return std::pair<SignedDistanceField, std::pair<double, double>>(new_sdf, extrema);
        }

        inline DistanceField ExtractDistanceField(const float oob_value) const
        {
            // Make the SDF
            SignedDistanceField new_sdf(collision_field_.GetOriginTransform(), frame_, GetResolution(), collision_field_.GetXSize(), collision_field_.GetYSize(), collision_field_.GetZSize(), oob_value);
            std::vector<VoxelGrid::GRID_INDEX> filled;
            for (int64_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++)
            {
                for (int64_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++)
                {
                    for (int64_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++)
                    {
                        VoxelGrid::GRID_INDEX current_index(x_index, y_index, z_index);
                        if (Get(x_index, y_index, z_index).first.occupancy > 0.5)
                        {
                            // Mark as filled
                            filled.push_back(current_index);
                        }
                    }
                }
            }
            // Make two distance fields (one for distance to filled voxels, one for distance to free voxels
            DistanceField filled_distance_field = BuildDistanceField(filled);

            return filled_distance_field;
        }

        // reset ervery voxel 0.0
        void RestMap()
        {
            // Reset components first
            for (int64_t x_index = 0; x_index < collision_field_.GetNumXCells(); x_index++)
            {
                for (int64_t y_index = 0; y_index < collision_field_.GetNumYCells(); y_index++)
                {
                    for (int64_t z_index = 0; z_index < collision_field_.GetNumZCells(); z_index++)
                    {
                        COLLISION_CELL free_cell(0.0);
                        collision_field_.SetValue(x_index, y_index, z_index, free_cell);
                    }
                }
            }
        }

        visualization_msgs::Marker ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;

        visualization_msgs::Marker ExportConnectedComponentsForDisplay(bool color_unknown_components) const;
    };
}

#endif // COLLISION_MAP_HPP


#ifndef options_hpp
#define options_hpp

#include <iostream>
#include <sstream>
#include <fstream>
#include <stdio.h>
#include <cstring>
#include <string>
#include <limits.h>
#include <boost/format.hpp>
#include "gadgets.hpp"
#include <Eigen/Eigen>
#include "common.h"

using namespace std;
using namespace boost;

const unsigned Megabase = 1e6;

class Options {
public:
    static MatrixXd parseVarianceComponents(const std::string &arg);

    unsigned chainLength;
    unsigned burnin;
    unsigned seed;
    unsigned numThread = 0; // Default to tbb::flow::unlimited
    int numThreadSpawned = 0; // Default to 0, let TBB do its thing
    size_t decompressionNodeConcurrency = 0;
    size_t decompressionTokens = 40;
    size_t analysisNodeConcurrency = 0;
    size_t analysisTokens = 20;
    unsigned preprocessChunks = 1;
    unsigned thin;  // save every this th sampled value in MCMC
    Eigen::MatrixXd S;    //variance components

    unsigned int numGroups;
    string groupFile;
    string failureFile;
    string bayesW_version;
    string quad_points;
    string fixedFile;
    unsigned int fixedEffectNumber;

    string title;
    AnalysisType analysisType = AnalysisType::Unknown;
    string phenotypeFile;
    string dataFile;
    InputType inputType = InputType::Unknown;
    string mcmcSampleFile;
    string optionFile;
    bool compress = false;
    PreprocessDataType preprocessDataType = PreprocessDataType::Dense;
    string iterLogFile;
    bool iterLog = false;
    string colLogFile;
    bool colLog =false;
    bool useMarkerCache = false;

    double v0E  = 0.0001;
    double s02E = 0.0001;
    double v0G  = 0.0001;
    double s02G = 0.0001;
    Options(){
        chainLength             = 10000;
        burnin                  = 5000;
        seed                    = static_cast<unsigned int>(std::time(0));
        numThread               = 0;
        numThreadSpawned        = 0;
        decompressionNodeConcurrency = 0;
        decompressionTokens     = 40;
        analysisNodeConcurrency = 0;
        analysisTokens          = 20;
        preprocessChunks        = 1;
        thin                    = 5;

        S.resize(1, 3);
        S(0,0)                  = 0.01;
        S(0,1)                  = 0.001;
        S(0,2)                  = 0.0001;

        title                   = "brr";
        analysisType            = AnalysisType::Unknown;
        dataFile                = "";
        inputType               = InputType::Unknown;
        fixedFile 				= "";
        phenotypeFile           = "";
        mcmcSampleFile          = "bayesOutput.csv";
        optionFile				= "";
        numGroups				=2;
        preprocessDataType      = PreprocessDataType::Dense;

        bayesW_version		= "marginal";
        fixedEffectNumber       = 0;
    }

    void inputOptions(const int argc, const char* argv[]);

private:
    void readFile(const string &file);
    void makeTitle(void);
    void seedEngine(void);
};

#endif /* options_hpp */


#ifndef _FRANKA_ROBOT_HPP_
#define _FRANKA_ROBOT_HPP_

#include <Eigen/Core>
#include <Eigen/Dense>

#include <franka/exception.h>
#include <franka/robot.h>

namespace cartesian_franka { // cf = cartesian_franka
    class Robot {
    public:

        /// take the IP of the robot as input
        Robot(const std::string& ip, bool auto_init=true, double speed_factor = 0.5)
            : _robot(ip.c_str())
        {
            _set_default_behavior();
            if (auto_init)
              init(speed_factor);
        }

        /// go to the starting position using joint positions
        void init(double speed_factor = 0.5);

        // all these function are relative to the current position/orientation
        void translate(const Eigen::Vector3d& delta, double duration = -1);
        void rotate(const Eigen::Vector3d& rpy, double duration = -1);
        void move(const Eigen::Vector3d& delta, const Eigen::Vector3d& rpy, double duration = -1);
        void move(const Eigen::Affine3d& transform_delta, double duration = -1);

        // move to a joint position
        void move_joints(const std::array<double, 7>& joint_positions, double duration = 1);

        /// end-effector transform (position & rotation)
        Eigen::Affine3d affine3d();
        Eigen::Vector3d position();
        Eigen::Vector3d orientation();

        /// the libfranka robot if needed for other functions
        const franka::Robot& franka() const { return _robot; }

        void automaticErrorRecovery();

    protected:
        void _set_default_behavior();
        franka::Robot _robot;
    };
} // namespace cartesian_franka

#endif


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

#include <cstdio>
#include <cstdlib>
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <geometry.h>
#include <config_loader.h>
#include <kitti_stereo_loader.h>
#include <stereo_cloud_generator.h>
#include <reconstruction_visualizer.h>
#include <stereo_optical_flow_visual_odometry.h>

using namespace std;
using namespace cv;

int main(int argc, char **argv)
{
    EventLogger& logger = EventLogger::getInstance();
    logger.setVerbosityLevel(EventLogger::L_DEBUG);
    logger.activateLoggingOnlyFor(EventLogger::M_MOTION_ESTIMATION);

	KITTIStereoLoader loader;
	ReconstructionVisualizer visualizer;

    if (argc != 2)
    {
        logger.print(EventLogger::L_ERROR,
                     "[stereo_optical_flow_visual_odometry.cpp] ERROR: "
                     "Usage: %s <path/to/config_file.yaml>\n", argv[0]);
        exit(0);
    }

    Mat left, right;
    float fx, fy, cx, cy, baseline, ransac_thr;
    string root_path;
    int kitti_seq;
    bool use_color_camera;

    ConfigLoader param_loader(argv[1]);
    param_loader.checkAndGetFloat("fx", fx);
    param_loader.checkAndGetFloat("fy", fy);
    param_loader.checkAndGetFloat("cx", cx);
    param_loader.checkAndGetFloat("cy", cy);
    param_loader.checkAndGetFloat("baseline", baseline);
    param_loader.checkAndGetString("kitti_root_dir", root_path);
    param_loader.checkAndGetInt("kitti_seq", kitti_seq);
    param_loader.checkAndGetBool("use_color_camera", use_color_camera);
    param_loader.checkAndGetFloat("ransac_thr", ransac_thr);

    Intrinsics intr(fx, fy, cx, cy, baseline);
    StereoOpticalFlowVisualOdometry vo(intr, ransac_thr);

    loader.loadStereoSequence(root_path, kitti_seq, use_color_camera);

    for(size_t i = 0; i < loader.num_pairs_; i++)
    {
    	//Load stereo pair
        left = loader.getNextLeftImage(false);
        right = loader.getNextRightImage(false);

        //Estimate current camera pose
        vo.computeCameraPose(left, right);

        //View tracked points
		for(size_t k = 0; k < vo.tracker_.curr_pts_.size(); k++)
		{
			Point2i pt1 = vo.tracker_.prev_pts_[k];
			Point2i pt2 = vo.tracker_.curr_pts_[k];
			circle(left, pt1, 1, CV_RGB(255,0,0), -1);
			circle(left, pt2, 3, CV_RGB(0,0,255), -1);
			line(left, pt1, pt2, CV_RGB(0,0,255));
		}

		if(i == 0) visualizer.addReferenceFrame(vo.pose_, "origin");
		//visualizer.addQuantizedPointCloud(vo.curr_dense_cloud_, 0.1, vo.pose_);
		//visualizer.addPointCloud(vo.curr_dense_cloud_, vo.pose_);
		visualizer.viewReferenceFrame(vo.pose_);
		//visualizer.viewPointCloud(vo.curr_dense_cloud_, vo.pose_);
		visualizer.viewQuantizedPointCloud(vo.curr_dense_cloud_, 0.5, vo.pose_);
		//visualizer.addCameraPath(vo.pose_);

		visualizer.spinOnce();

        //Show images
		imshow("Left Image", left);

		char key = waitKey(1);
		if(key == 27 || key == 'q' || key == 'Q')
		{
			printf("Exiting.\n");
			break;
		}
    }

    visualizer.close();

	return 0;
}


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at http://www.somewhere.org/somepath/license.html.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#include <assert.h>
#include <boost/exception/diagnostic_information.hpp>
#include <iostream>
#include <duds/hardware/devices/clocks/Clock.hpp>
#include <duds/data/GenericValueVisitor.hpp>
#include <duds/time/interstellar/Serialize.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <fstream>
#include <sstream>
#include <boost/uuid/uuid_io.hpp>
#include <boost/uuid/uuid_serialize.hpp>
#include <boost/serialization/variant.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/array.hpp>

// following needed for Boost 1.55
//#include <duds/general/ArraySerialize-depricate.hpp>

using duds::data::Unit;
using duds::data::Sample;
using duds::data::GenericSample;
using duds::data::CompactSample;
using duds::data::SampleNU;
using duds::data::GenericValue;
using duds::data::Measurement;
using duds::data::GenericMeasurement;
using duds::data::CompactMeasurement;

/*
template <class VT = double, class QT = VT, class TT = duds::time::interstellar::NanoTime>
struct Measurement {
	Sample<TT, QT> time;
	Sample<VT, QT> measured;
};
*/
template <class VT = double, class QT = VT, class TT = duds::time::interstellar::NanoTime>
struct MeasurementTNU {
	SampleNU<TT, QT> time;
	GenericSample<VT, QT> measured;
};
/*
template <class VT = double, class QT = VT, class TT = duds::time::interstellar::NanoTime>
struct CompactMeasurement {
	CompactSample<TT, QT> time;
	CompactSample<VT, QT> measured;
};
*/
struct Qual {
	double accuracy;
	double precision;
	double resolution;
};

template <typename VT>
struct SampoC : public Qual {
	VT value;
	Unit qualityUnit() const {
		// not sure if this is the best way to write a compile-time type check,
		// but optimization should make it a non-issue for the end result
		if (std::is_same<duds::data::Quantity,VT>::value) {
			// for VT == Quantity
			return value.unit;
		} else {
			// for VT == something else
			return duds::data::units::Second;
		}
	}
	duds::data::Quantity getAccuracyQuantity() const {
		return duds::data::Quantity(accuracy, qualityUnit());
	}
};

typedef SampoC<duds::data::Quantity>  Sampo;
typedef SampoC<duds::time::interstellar::NanoTime>  SampoT;
typedef SampoC<duds::time::interstellar::Nanoseconds>  SampoD;

/*
struct Sampo {
	double accuracy;
	double precision;
	double resolution;
	duds::data::Quantity value;
	//std::int32_t flags;
};

template <typename VT>
struct Sampo2 {
	VT accuracy;
	VT precision;
	VT resolution;
	VT value;
	//std::int32_t flags;
};
*/

typedef boost::variant<
	Sampo,
	SampoT,
	SampoD,
	//Sampo2<duds::time::interstellar::Nanoseconds>,
	//Sampo2<duds::time::interstellar::NanoTime>,
	//boost::recursive_wrapper< Sampo2<duds::time::interstellar::FemtoTime> >,
	GenericValue
	//std::array<char,32>
	//boost::recursive_wrapper<GenericValue>

	// maybe some kind of error for a failed attempt to read a sensor?

>  ValVar;

struct SampleCont {
	boost::uuids::uuid origin;
	ValVar values;
};
struct SampleCont2 {
	boost::uuids::uuid origin;
	GenericValue value;
};
struct SampleCont3 {
	SampleCont sc;
	boost::uuids::uuid origin;
	//Sampo2<duds::time::interstellar::Nanoseconds> value;
	SampoT value;
};
/*
struct SampleCont4 {
	SampleCont sc;
	boost::uuids::uuid origin;
	//Sampo timevalue;
	Sampo2<double> timevalue;
};
*/

int main()
try {
	Unit power = duds::data::units::Watt;
	Unit current = duds::data::units::Ampere;
	Unit voltage = duds::data::units::Volt;
	std::cout << "A,A: " << current.ampere() << "\nV,A: " << voltage.ampere()
		<< "\nV,s: " << voltage.second() << std::endl;
	Unit test = current * voltage;
	assert(test == power);
	std::cout << "Worked." << std::endl;

	std::cout << "Size of Sample: " << sizeof(Sample) <<
	"\nSize of GenericSample<GenericValue,Quantity>: " <<
	sizeof(GenericSample< GenericValue,duds::data::Quantity >) <<
	"\nSize of GenericSample<Femtoseconds,double>: " <<
	sizeof(GenericSample<duds::time::interstellar::Femtoseconds,double>) <<
	"\nSize of GenericSample<Nanoseconds,double>: " <<
	sizeof(GenericSample<duds::time::interstellar::Nanoseconds,double>) <<
	"\nSize of GenericSample<Femtoseconds,Femtoseconds>: " <<
	sizeof(GenericSample<duds::time::interstellar::Femtoseconds, duds::time::interstellar::Femtoseconds>)
	<< "\nSize of GenericSample<GenericValue,double>: " <<
	sizeof(GenericSample<GenericValue,double>) <<
	"\nSize of CompactSample<GenericValue,double>: " <<
	sizeof(CompactSample<GenericValue,double>) <<
	"\nSize of Measurement: " <<
	sizeof(Measurement) <<
	"\nSize of GenericMeasurement<Quantity,double,duds::time::interstellar::Nanoseconds,float>: " <<
	sizeof(GenericMeasurement<duds::data::Quantity,double,duds::time::interstellar::Nanoseconds,float>) <<
	//"\nSize of CompactMeasurement<GenericValue,double>: " <<
	//sizeof(CompactMeasurement<GenericValue,double>) <<
	"\nSize of GenericValue:     " << sizeof(GenericValue)
	//<< "\nSize of array 2: " << sizeof(std::array<double, 2>)
	//<< "\nSize of array 3: " << sizeof(std::array<double, 3>)
	<< "\nSize of Quantity:         " << sizeof(duds::data::Quantity)
	<< "\nSize of ExtendedQuantity: " << sizeof(duds::data::ExtendedQuantity<>)
	<< "\nSize of Sampo:            " << sizeof(Sampo)
	//<< "\nSize of Sampo2:           " << sizeof(Sampo2<duds::time::interstellar::NanoTime>)
	<< "\nSize of SampoT:           " << sizeof(SampoT)
	<< "\nSize of ValVar:           " << sizeof(ValVar)
	<< "\nSize of SampleCont:       " << sizeof(SampleCont)
	<< "\nSize of SampleCont2:      " << sizeof(SampleCont2)
	<< "\nSize of SampleCont3:      " << sizeof(SampleCont3)
	//<< "\nSize of SampleCont4:      " << sizeof(SampleCont4)
	<< "\nSize of Femtoseconds:     " << sizeof(duds::time::interstellar::Femtoseconds)
	/*
	<< "\nSize of time sample:      " << sizeof(duds::time::TimeSample)
	<< "\nSize of nano time sample: " << sizeof(duds::time::NanoTimeSample)
	<< "\nSize of femto time sample:" << sizeof(duds::time::FemtoTimeSample)
	*/
	<< std::endl;

	duds::data::int128_w fs;
	GenericValue sv;
	{
		std::ifstream ifs("femto.xml");
		if (ifs.good()) {
			try {
				boost::archive::xml_iarchive ia(ifs);
				ia >> boost::serialization::make_nvp("time", fs) >>
					boost::serialization::make_nvp("value", sv);
				std::cout << "Read in time: " << std::hex << fs << std::endl;
			} catch (...) {
				std::cout << "Failed to read femto.xml." << std::endl;
			}
		} else {
			std::cout << "Failed to open femto.xml." << std::endl;
		}
	}

	duds::time::interstellar::Femtoseconds now =
		duds::time::interstellar::FemtoClock::now().time_since_epoch();
	fs = now.count();
	sv = now;
	std::cout << "String visit femtos: " << boost::apply_visitor(
		duds::data::GenericValueStringVisitor(), sv) << std::endl;
	sv = now; // fs; //std::string("Yoyodyne");
	{
		std::ofstream ofs("femto.xml");
		/*
		duds::time::interstellar::Milliseconds now =
			duds::time::interstellar::MilliClock::now().time_since_epoch();
		std::int64_t ms = now.count();
		std::ofstream ofs("milli.xml");
		*/
		boost::archive::xml_oarchive oa(ofs);
		oa << boost::serialization::make_nvp("time", fs) <<
			boost::serialization::make_nvp("value", sv);
	}
	std::cout << "out time " << std::hex << fs << std::endl;
	{
		std::ifstream ifs("femto.xml");
		boost::archive::xml_iarchive ia(ifs);
		ia >> boost::serialization::make_nvp("time", fs) >>
			boost::serialization::make_nvp("value", sv);
	}
	std::cout << " in time " << std::hex << fs << std::endl;
	//__int128 ack = 4;
	//std::cout << ack;
	/**/
	std::ostringstream oss;
	{
		sv = duds::data::int128_w(15);
		boost::archive::xml_oarchive oa(oss);
		std::cout << "String visit 0: " << boost::apply_visitor(
			duds::data::GenericValueStringVisitor(), sv) << std::endl;
		oa << boost::serialization::make_nvp("sv0", sv);
		sv = 273.15;
		std::cout << "String visit 1: " << boost::apply_visitor(
			duds::data::GenericValueStringVisitor(), sv) << std::endl;
		oa << boost::serialization::make_nvp("sv1", sv);
		sv = 14L;
		std::cout << "String visit 2: " << boost::apply_visitor(
			duds::data::GenericValueStringVisitor(), sv) << std::endl;
		oa << boost::serialization::make_nvp("sv2", sv);
		sv = duds::time::interstellar::FemtoClock::now();
		std::cout << "String visit 3: " << boost::apply_visitor(
			duds::data::GenericValueStringVisitor(), sv) << std::endl;
		oa << boost::serialization::make_nvp("sv3", sv);
		sv = duds::data::Quantity(5.02, duds::data::units::Volt);
		std::cout << "String visit 4: " << boost::apply_visitor(
			duds::data::GenericValueStringVisitor(), sv) << std::endl;
		oa << boost::serialization::make_nvp("sv4", sv);
		/*
		duds::data::ExtendedUnit eu(duds::data::units::Kelvin);
		eu.offset(273.15f);
		duds::data::ExtendedQuantity<> eq;
		eq.unit = eu;
		eq.value = 22.375;
		sv = eq;
		std::cout << "String visit 5: " << boost::apply_visitor(
			duds::data::GenericValueStringVisitor(), sv) << std::endl;
		oa << boost::serialization::make_nvp("sv5", sv);
		*/
		std::array<double, 2> ad;
		ad[0] = 1.4; ad[1] = 2.8;
		sv = ad;
		std::cout << "String visit A: " << boost::apply_visitor(
			duds::data::GenericValueStringVisitor(), sv) << std::endl;
		oa << boost::serialization::make_nvp("svA", sv);
	}
	std::cout << oss.str() << std::endl;

	duds::data::ExtendedUnit eu;
	float cof = 273.15f;
	eu.offset(cof);
	std::cout << "EU float test: " << cof << ", " << eu.offsetf() << std::endl;
	double cod = 273.15;
	eu.offset(cod);
	std::cout << "EU double test: " << cod << ", " << eu.offset() << std::endl;

	return 0;
}
catch (...) {
	std::cerr << "ERROR: " << boost::current_exception_diagnostic_information()
	<< std::endl;
	return __LINE__;
}


#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/octree/octree_pointcloud.h>

#include <pcl/features/vfh.h>
#include <pcl/io/pcd_io.h>

#include <Eigen/Dense>

#include <pcl/features/integral_image_normal.h>

#include "minipcl.h"

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

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

void mpcl_compute_normals_PointXYZI(const pcl::PointCloud<pcl::PointXYZI>& cloud,
                          int ksearch,
                          double searchRadius,
                          pcl::PointCloud<pcl::Normal> &out)
{
    pcl::search::KdTree<pcl::PointXYZI>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZI> ());
    pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> ne;

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

void mpcl_compute_normals_PointXYZRGB(const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
                          int ksearch,
                          double searchRadius,
                          pcl::PointCloud<pcl::Normal> &out)
{
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;

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

void mpcl_compute_normals_PointXYZRGBA(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
                          int ksearch,
                          double searchRadius,
                          pcl::PointCloud<pcl::Normal> &out)
{
    pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
    pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;

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

// set ksearch and radius to < 0 to disable
void mpcl_sacnormal_set_axis(pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> &sac,
                             double ax, double ay, double az)
{
    Eigen::Vector3f vect(ax,ay,az);
    sac.setAxis(vect);
}

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

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

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

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

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

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

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

// EuclideanClusterExtraction
// Octree
// void mpcl_deleteVoxelAtPoint(pcl::octree::OctreePointCloud<pcl::PointXYZ>& inOctree, pcl::PointXYZ incloud)
// {
//     inOctree.deleteVoxelAtPoint(incloud);
// }
//
// void mpcl_deleteVoxelAtPoint(pcl::octree::OctreePointCloud<pcl::PointXYZI>& inOctree, pcl::PointXYZI incloud)
// {
//     inOctree.deleteVoxelAtPoint(incloud);
// }
//
// void mpcl_deleteVoxelAtPoint(pcl::octree::OctreePointCloud<pcl::PointXYZRGB>& inOctree, pcl::PointXYZRGB incloud)
// {
//     inOctree.deleteVoxelAtPoint(incloud);
// }
//
// void mpcl_deleteVoxelAtPoint(pcl::octree::OctreePointCloud<pcl::PointXYZRGBA>& inOctree, pcl::PointXYZRGBA incloud)
// {
//     inOctree.deleteVoxelAtPoint(incloud);
// }
//
//
// int mpcl_getOccupiedVoxelCenters(pcl::octree::OctreePointCloud<pcl::PointXYZ>& inOctree, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &incloud, vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ> > alignPoint)
// {
//     return inOctree.getOccupiedVoxelCenters(alignPoint);
// }
//
// int mpcl_getOccupiedVoxelCenters(pcl::octree::OctreePointCloud<pcl::PointXYZI>& inOctree, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &incloud, vector<pcl::PointXYZI, Eigen::aligned_allocator<pcl::PointXYZI> > alignPoint)
// {
//     return inOctree.getOccupiedVoxelCenters(alignPoint);
// }
//
// int mpcl_getOccupiedVoxelCenters(pcl::octree::OctreePointCloud<pcl::PointXYZRGB>& inOctree, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &incloud, vector<pcl::PointXYZRGB, Eigen::aligned_allocator<pcl::PointXYZRGB> > alignPoint)
// {
//     return inOctree.getOccupiedVoxelCenters(alignPoint);
// }
//
// int mpcl_getOccupiedVoxelCenters(pcl::octree::OctreePointCloud<pcl::PointXYZRGBA>& inOctree, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &incloud, vector<pcl::PointXYZRGBA, Eigen::aligned_allocator<pcl::PointXYZRGBA> > alignPoint)
// {
//     return inOctree.getOccupiedVoxelCenters(alignPoint);
// }
//

void mpcl_extract_VFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    // http://virtuemarket-lab.blogspot.jp/2015/03/viewpoint-feature-histogram.html
    // pcl::PointCloud<pcl::VFHSignature308>::Ptr Extract_VFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
    pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());

    // cloud_normals = surface_normals(cloud);
    vfh.setInputCloud (cloud);
    vfh.setInputNormals (cloud_normals);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    vfh.setSearchMethod (tree);

    vfh.compute (*vfhs);
    // return vfhs;
}

/*
// pcl1.6
#include <pcl/keypoints/harris_keypoint3D.h>
// use 1.7�`
#include <pcl/keypoints/harris_3d.h>

// HarrisKeypoint3D
// NG
// outcloud set pcl::PointXYZI
void mpcl_extract_HarrisKeypoint3D(pcl::PointCloud<pcl::PointXYZ>::Ptr &incloud,
                                   pcl::PointCloud<pcl::PointXYZ> *outcloud)
{
    // pcl::HarrisKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> detector;
    pcl::HarrisKeypoint3D<pcl::PointXYZ, pcl::PointXYZI> detector;

    detector.setInputCloud(incloud);

    detector.setNonMaxSupression (true);
    detector.setRadius (0.01);
    // detector.setRadiusSearch (100);
    // detector.setIndices(indicesptr);

    // NG
    // detector.compute(*outcloud);

    // OK
    pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints(new pcl::PointCloud<pcl::PointXYZI>());
    detector.compute(*keypoints);
}

// HarrisKeypoint3D
void mpcl_extract_HarrisKeypoint3D(pcl::PointCloud<pcl::PointXYZ>::Ptr &incloud,
                                   pcl::PointCloud<pcl::PointXYZI> *outcloud)
{
    pcl::HarrisKeypoint3D<pcl::PointXYZ, pcl::PointXYZI> detector;

    detector.setInputCloud(incloud);

    detector.setNonMaxSupression (true);
    detector.setRadius (0.01);
    //detector.setRadiusSearch (100);

    // detector.setIndices(indicesptr);
    // detector.compute(*outcloud);
    detector.compute(*outcloud);
}
*/

// features
// integral_image_normal.h
void mpcl_features_NormalEstimationMethod_AVERAGE_3D_GRADIENT(pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne)
{
    ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
}

void mpcl_features_NormalEstimationMethod_COVARIANCE_MATRIX(pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne)
{
    ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
}

void mpcl_features_NormalEstimationMethod_AVERAGE_DEPTH_CHANGE(pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne)
{
    ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
}

void mpcl_features_NormalEstimationMethod_SIMPLE_3D_GRADIENT(pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne)
{
    ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
}

void mpcl_features_NormalEstimationMethod_compute(pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne, pcl::PointCloud<pcl::Normal> &out)
{
    // NG : out Variant Function end error
    printf("compute start.\n");
    // �Q�ƃJ�E���g��NG?(�֐����������ɃG���[)
    ne.compute (out);
    // pcl 1.7.2 error
    // printf("out = %p.\n", out);
    printf("compute end.\n");
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IF_INC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IF_INC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object conditionally returns @c x+1 (respectively @c x)
    if @c c is @ref True (respectively  @ref False)

    @par Header <boost/simd/function/if_inc.hpp>

    @par Example:

      @snippet if_inc.cpp if_inc

    @par Possible output:

      @snippet if_inc.txt if_inc

  **/
  Value1 if_inc(Value0 const& c, Value1 const& x);
} }
#endif

#include <boost/simd/function/scalar/if_inc.hpp>
#include <boost/simd/function/simd/if_inc.hpp>

#endif


#define BOOST_TEST_MODULE gn_solver_unit_tests

#include <boost/test/included/unit_test.hpp>
#include <armadillo>
#include "../src/numerical/gauss-newton/gn_solver.h"
#include "test_utils.cpp"
#include "../src/logging/easylogging++.h"

INITIALIZE_EASYLOGGINGPP

GNSolver solver(SIGNALS, 100, 0.00001);

BOOST_AUTO_TEST_CASE(test_common_cases)
{
    auto tester = SolverTester<GNSolver>(solver);
    tester.test_common();
}