//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_MAC_VALUE_HPP
#define CRYPTO3_MAC_VALUE_HPP

#include <type_traits>
#include <iterator>

#include <boost/assert.hpp>
#include <boost/concept_check.hpp>

#include <boost/range/concepts.hpp>

#include <boost/mpl/front.hpp>
#include <boost/mpl/apply.hpp>

#include <nil/crypto3/mac/accumulators/mac.hpp>

namespace nil {
    namespace crypto3 {
        namespace mac {
            namespace detail {
                template<typename MacAccumulator>
                struct ref_mac_impl {
                    typedef MacAccumulator accumulator_set_type;
                    typedef
                        typename boost::mpl::front<typename accumulator_set_type::features_type>::type accumulator_type;

                    ref_mac_impl(accumulator_set_type &&acc) : accumulator_set(acc) {
                    }

                    accumulator_set_type &accumulator_set;
                };

                template<typename MacAccumulator>
                struct value_mac_impl {
                    typedef MacAccumulator accumulator_set_type;
                    typedef
                        typename boost::mpl::front<typename accumulator_set_type::features_type>::type accumulator_type;

                    value_mac_impl(accumulator_set_type &&acc) :
                        accumulator_set(std::forward<accumulator_set_type>(acc)) {
                    }

                    mutable accumulator_set_type accumulator_set;
                };

                template<typename MacStateImpl>
                struct range_mac_impl : public MacStateImpl {
                    typedef MacStateImpl mac_state_impl_type;

                    typedef typename mac_state_impl_type::accumulator_type accumulator_type;
                    typedef typename mac_state_impl_type::accumulator_set_type accumulator_set_type;

                    typedef typename boost::mpl::apply<accumulator_set_type, accumulator_type>::type::result_type
                        result_type;

                    template<typename SinglePassRange>
                    range_mac_impl(const SinglePassRange &range, accumulator_set_type &&ise) :
                        MacStateImpl(std::forward<accumulator_set_type>(ise)) {
                        BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const SinglePassRange>));

                        this->accumulator_set(range);
                    }

                    template<typename InputIterator>
                    range_mac_impl(InputIterator first, InputIterator last, accumulator_set_type &&ise) :
                        MacStateImpl(std::forward<accumulator_set_type>(ise)) {
                        BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<InputIterator>));

                        this->accumulator_set(first, ::nil::crypto3::accumulators::iterator_last = last);
                    }

                    template<typename OutputRange>
                    operator OutputRange() const {
                        result_type result =
                            boost::accumulators::extract_result<accumulator_type>(this->accumulator_set);
                        return OutputRange(result.cbegin(), result.cend());
                    }

                    operator result_type() const {
                        return boost::accumulators::extract_result<accumulator_type>(this->accumulator_set);
                    }

                    operator accumulator_set_type &() const {
                        return this->accumulator_set;
                    }

#ifdef CRYPTO3_ASCII_STRING_CODEC_OUTPUT
                    template<typename Char, typename CharTraits, typename Alloc>
                    operator std::basic_string<Char, CharTraits, Alloc>() const {
                        return std::to_string(
                            boost::accumulators::extract_result<accumulator_type>(this->accumulator_set));
                    }
#endif
                };

                template<typename MacStateImpl, typename OutputIterator>
                struct itr_mac_impl : public MacStateImpl {
                private:
                    mutable OutputIterator out;

                public:
                    typedef MacStateImpl mac_state_impl_type;

                    typedef typename mac_state_impl_type::accumulator_type accumulator_type;
                    typedef typename mac_state_impl_type::accumulator_set_type accumulator_set_type;

                    typedef typename boost::mpl::apply<accumulator_set_type, accumulator_type>::type::result_type
                        result_type;

                    template<typename SinglePassRange>
                    itr_mac_impl(const SinglePassRange &range, OutputIterator out, accumulator_set_type &&ise) :
                        MacStateImpl(std::forward<accumulator_set_type>(ise)), out(std::move(out)) {
                        BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const SinglePassRange>));
                        BOOST_CONCEPT_ASSERT((boost::OutputIteratorConcept<OutputIterator, result_type>));

                        this->accumulator_set(range);
                    }

                    template<typename InputIterator>
                    itr_mac_impl(InputIterator first, InputIterator last, OutputIterator out,
                                 accumulator_set_type &&ise) :
                        MacStateImpl(std::forward<accumulator_set_type>(ise)),
                        out(std::move(out)) {
                        BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<InputIterator>));
                        BOOST_CONCEPT_ASSERT((boost::OutputIteratorConcept<OutputIterator, result_type>));

                        this->accumulator_set(first, ::nil::crypto3::accumulators::iterator_last = last);
                    }

                    operator OutputIterator() const {
                        *out++ = boost::accumulators::extract_result<accumulator_type>(this->accumulator_set);
                        return out;
                    }
                };
            }    // namespace detail
        }        // namespace mac
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_CODEC_POSTPROCESSOR_HPP


/*
 * Copyright (c) 2020, Eberty Alves
 */

// C++ standard library
#include <bits/stdc++.h>

// Point cloud library
#include <pcl/filters/crop_box.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// Boost library
#include <boost/program_options.hpp>
#include <boost/progress.hpp>

// Typedefs
typedef pcl::PointXYZRGBNormal PointT;
typedef pcl::PointCloud<PointT> PointC;

int main(int argc, char* argv[]) {
  pcl::console::setVerbosityLevel(pcl::console::L_ERROR);

  try {
    // Declaration of variables
    std::string src_file_name;
    std::string tgt_file_name;
    std::string output_file_name;

    double radius;
    bool negative;
    bool with_redundant;

    uint number_of_neighbors;
    double deviation_multiplier;

    // Parse command-line options
    namespace po = boost::program_options;

    // Define command-line options
    po::options_description desc("Options");
    desc.add_options()
    ("help,h", "Print help message")
    ("input,i", po::value<std::string>(&src_file_name)->required(), "Input cloud file (.ply)")
    ("target,t", po::value<std::string>(&tgt_file_name)->required(), "Input target file (.ply)")
    ("output,o", po::value<std::string>(&output_file_name)->required(), "Output file (.ply)")
    ("radius,r", po::value<double>(&radius)->default_value(0.05), "Standard radius to select point neighbours")
    ("clean_neighbors,c", po::value<uint>(&number_of_neighbors)->default_value(50), "N. of neighbors to analyze for each point to clean")
    ("dev_mult,d", po::value<double>(&deviation_multiplier)->default_value(1.0), "Standard deviation multiplier to clean")
    ("negative,n", "Saves the not redundant points in a .ply file")
    ("all,a", "Saves accumulated cloud with all points in a .ply file");

    // Use a parser to evaluate the command line
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    // Store the command-line options evaluated by the parser
    if (vm.count("help")) {
      std::cout << "Accumulate points clouds removing redundant points" << std::endl << std::endl;
      std::cout << desc << std::endl;
      return 0;
    }

    if (vm.count("input") && vm.count("target") && vm.count("output")) {
      src_file_name = vm["input"].as<std::string>();
      tgt_file_name = vm["target"].as<std::string>();
      output_file_name = vm["output"].as<std::string>();
    } else {
      throw std::logic_error("Correct mode of use: " + std::string(argv[0]) +
                             " -i input.ply -t target.ply -o output.ply [opts]");
    }

    number_of_neighbors = vm["clean_neighbors"].as<uint>();
    deviation_multiplier = vm["dev_mult"].as<double>();
    radius = vm["radius"].as<double>();
    negative = vm.count("negative");
    with_redundant = vm.count("all");

    PointC::Ptr cloud_src(new PointC);
    PointC::Ptr cloud_tgt(new PointC);

    // Load point clouds data from disk
    if (pcl::io::loadPLYFile<PointT>(src_file_name, *cloud_src) == -1) {
      throw std::runtime_error("Couldn't load input point cloud: " + src_file_name);
    }
    std::cout << "Loaded " << cloud_src->size() << " data points from " << src_file_name << std::endl;

    if (pcl::io::loadPLYFile<PointT>(tgt_file_name, *cloud_tgt) == -1) {
      throw std::runtime_error("Couldn't load target point cloud: " + tgt_file_name);
    }
    std::cout << "Loaded " << cloud_tgt->size() << " data points from " << tgt_file_name << std::endl;

    std::cout << "Cloud before accumulate: " << std::endl;
    std::cout << *cloud_tgt << std::endl;

    if (!with_redundant) {
      pcl::CropBox<PointT> crop_box;
      crop_box.setNegative(true);
      crop_box.setInputCloud(cloud_src);
      boost::progress_display show_progress(cloud_tgt->points.size());
      for (size_t i = 0; i < cloud_tgt->points.size(); i++) {
        PointT* point = &cloud_tgt->points[i];
        crop_box.setMin(Eigen::Vector4f(point->x - radius, point->y - radius, point->z - radius, 1.0));
        crop_box.setMax(Eigen::Vector4f(point->x + radius, point->y + radius, point->z + radius, 1.0));
        crop_box.filter(*cloud_src);
        ++show_progress;
      }

      // Create the filtering object
      pcl::StatisticalOutlierRemoval<PointT> sor;
      sor.setInputCloud(cloud_src);
      sor.setMeanK(number_of_neighbors);
      sor.setStddevMulThresh(deviation_multiplier);
      sor.filter(*cloud_src);
    }

    PointC::Ptr accumulated(new PointC);
    pcl::copyPointCloud(*cloud_tgt, *accumulated);
    *accumulated += *cloud_src;

    std::cout << "Cloud after accumulate: " << std::endl;
    std::cout << *accumulated << std::endl;
    pcl::io::savePLYFileBinary(output_file_name, *accumulated);

    // Save points of second cloud
    if (negative) {
      size_t pos = output_file_name.rfind(".ply");
      if (pos != std::string::npos) output_file_name.erase(pos, 4);
      pcl::io::savePLYFileBinary(output_file_name + "_negative.ply", *cloud_src);
    }

    return 0;
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
  } catch (...) {
    std::cerr << "An unknown error has occurred." << std::endl;
  }

  return -1;
}


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file suffix_array.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-08-29
 */
#ifndef PAAL_SUFFIX_ARRAY_HPP
#define PAAL_SUFFIX_ARRAY_HPP

#include "paal/utils/irange.hpp"

#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/numeric.hpp>

/*
 * algorithm from:
 *
 * http://www.cs.cmu.edu/~guyb/realworld/papersS04/KaSa03.pdf
 *
 */

namespace paal {

namespace detail {
/**
 * @param a1
 * @param a2
 * @param b1
 * @param b2
 * @brief return true if pair (a1,a2) is smaller than pair (b1,b2) in
 * lexicographic order
 * and false otherwise
 * @tparam Letter
 */
// class suffix_array{
template <typename Letter>
inline bool leq(Letter a1, int a2, Letter b1,
                int b2) { // lexic. order for pairs
    return (a1 < b1 || (a1 == b1 && a2 <= b2));
}
/**
 * @param a1
 * @param a2
 * @param a3
 * @param b1
 * @param b2
 * @param b3
 * @brief return true if triple (a1,a2,a3) is smaller than triple (b1,b2,b3) in
 * lexicographic order
 * and false otherwise
 * @tparam Letter
 */
template <typename Letter>
inline bool leq(Letter a1, Letter a2, int a3, Letter b1, Letter b2, int b3) {
    return (a1 < b1 || (a1 == b1 && leq(a2, a3, b2, b3)));
}

struct radix_pass {

    template <typename Letter>
    radix_pass(int max_letter, const std::vector<Letter> & text) {
        if (max_letter == 0) {
            max_letter = *boost::max_element(text);
        }
        c.resize(max_letter + 2);
    }

    // stably sort sortFrom[0..n-1] to sortTo[0..n-1] with keys in 0..K from r
    template <typename Iterator>
    void operator()(std::vector<int> const &sortFrom,
            std::vector<int> &sortTo, Iterator r, int n) { // count occurrences
        boost::fill(c, 0);
        for (auto i : irange(n)) {
            ++c[r[sortFrom[i]] + 1]; // count occurrences
        }

        boost::partial_sum(c, c.begin());

        for (auto i : irange(n)) {
            sortTo[c[r[sortFrom[i]]]++] = sortFrom[i]; // sort
        }
    }
private:
    std::vector<int> c;
};

/**
 *
 * @brief
 * require text[n]=text[n+1]=text[n+2]=0, n>=2
 * fill suffix_array
 * suffix_array[i] contains the starting position of the i-1'th smallest suffix
* in Word
 * @tparam Letter
 * @param text - text
 * @param SA place for suffix_array
 * @param max_letter optional parameter max_letter in alphabet
 */
// find the suffix array SA of text[0..n-1] in {1..max_letter}^n
template <typename Letter>
void suffix_array(std::vector<Letter> &text, std::vector<int> &SA,
                   Letter max_letter) {
    int n = text.size() - 3;
    assert(n >= 0);
    int n0 = (n + 2) / 3, n1 = (n + 1) / 3, n2 = n / 3, n02 = n0 + n2;
    text.resize(text.size() + 3);
    std::vector<int> text12;
    std::vector<int> SA12;
    std::vector<int> text0;
    std::vector<int> SA0;
    radix_pass radix{max_letter, text};
    // generate positions of mod 1 and mod  2 suffixes
    // the "+(n0-n1)" adds a dummy mod 1 suffix if n%3 == 1
    for (auto i : irange(n + (n0 - n1))) {
        if (i % 3 != 0) {
            text12.push_back(i);
        }
    }
    SA0.resize(n0);
    SA12.resize(n02 + 3);
    text12.resize(n02 + 3);
    // lsb radix sort the mod 1 and mod 2 triples
    radix(text12, SA12, text.begin() + 2, n02);
    radix(SA12, text12, text.begin() + 1, n02);
    radix(text12, SA12, text.begin(), n02);

    // find lexicographic names of triples
    int name = 0;
    Letter c0 = Letter{}, c1 = Letter{}, c2 = Letter{};
    for (auto i : irange(n02)) {
        if (text[SA12[i]] != c0 || text[SA12[i] + 1] != c1 ||
            text[SA12[i] + 2] != c2 || name == 0) {
            name++;
            c0 = text[SA12[i]];
            c1 = text[SA12[i] + 1];
            c2 = text[SA12[i] + 2];
        }
        if (SA12[i] % 3 == 1) {
            text12[SA12[i] / 3] = name; // left half
        } else {
            text12[SA12[i] / 3 + n0] = name; // right half
        }
    }

    // recurse if names are not yet unique
    if (name < n02) {
        suffix_array<int>(text12, SA12, name); // parametrized by int intentionally
        // store unique names in s12 using the suffix array
        for (auto i : irange(n02)) {
            text12[SA12[i]] = i + 1;
        }
    } else { // generate the suffix array of s12 directly
        for (auto i : irange(n02)) {
            SA12[text12[i] - 1] = i;
        }
    }

    // stably sort the mod 0 suffixes from SA12 by their first character
    for (auto i : irange(n02)) {
        if (SA12[i] < n0) {
            text0.push_back(3 * SA12[i]);
        }
    }
    radix(text0, SA0, text.begin(), n0);
    auto GetI = [&](int t)->int {
        return SA12[t] < n0 ? SA12[t] * 3 + 1 : (SA12[t] - n0) * 3 + 2;
    };

    // merge sorted SA0 suffixes and sorted SA12 suffixes
    auto p = SA0.begin();
    int t = n0 - n1;
    for (auto k = SA.begin(); k < SA.begin() + n; k++) {
        int i = GetI(t); // pos of current offset 12 suffix
        int j = (*p);    // pos of current offset 0  suffix
        if (SA12[t] < n0
                ? leq(text[i], text12[SA12[t] + n0], text[j], text12[j / 3])
                : leq(text[i], text[i + 1], text12[SA12[t] - n0 + 1], text[j],
                      text[j + 1], text12[j / 3 + n0])) { // suffix from SA12 is
                                                          // smaller
            (*k) = i;
            t++;
            if (t == n02) { // done --- only SA0 suffixes left
                k++;
                if (p < SA0.end()) {
                    k = std::copy(p, SA0.end(), k);
                    p = SA0.end();
                }
            }
        } else {
            (*k) = j;
            p++;
            if (p == SA0.end()) { // done --- only SA12 suffixes left
                for (k++; t < n02; t++, k++) {
                    (*k) = GetI(t);
                }
            }
        }
    }
}

}//!detail

/**
 *
 * @brief
 * require text.size()>=2
 * fill suffix_array
 * suffix_array[i] contains the starting position of the i-1'th smallest suffix
* in Word
 * @tparam Letter
 * @param text - text
 * @param SA place for suffix_array
 * @param max_letter optional parameter max_letter in alphabet
 */
// find the suffix array SA of text[0..n-1] in {1..max_letter}^n
template <typename Letter>
void suffix_array(std::vector<Letter> &text, std::vector<int> &SA,
                  Letter max_letter = 0) {
    text.resize(text.size() + 3);
    detail::suffix_array(text, SA, max_letter);
    text.resize(text.size() - 3);
}

} //!paal
#endif // PAAL_SUFFIX_ARRAY_HPP


// Class for creating Colorized SIFT Descriptors of images
//
// Author: Mark Desnoyer
// Date: March 2011

#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include "ColorSURFDescriptorGenerator.h"

#include "SURFDescriptor.h"

using namespace std;
using namespace boost;
using namespace cv;

namespace species_id {

ColorSURFDescriptorGenerator::ColorSURFDescriptorGenerator(
    boost::shared_ptr<ColorConverter> colorConverter,
    const FeatureDetector* keyGenerator,
    int nOctaves,
    int nOctaveLayers,
    bool extended) :
  SURFDescriptorGenerator(keyGenerator, nOctaves, nOctaveLayers, extended),
  colorConverter_(colorConverter) {
  needsMono_ = false;
}

ColorSURFDescriptorGenerator::~ColorSURFDescriptorGenerator() {}

void ColorSURFDescriptorGenerator::ExtractFromWholeImage(
    const Mat& image,
    const vector<KeyPoint>& keypoints,
    DescriptorCollection* descriptors) const {
  ROS_ASSERT(descriptors != NULL);

  // First convert the image into the color space we need
  Mat colorImage;
  //image.convertTo(colorImage, CV_32FC3);
  colorConverter_->ConvertImage(image, &colorImage);

  // Build up the collection of descriptors to return
  for (unsigned int i = 0; i < keypoints.size(); ++i) {
    descriptors->push_back(DescriptorPtr(new SURFDescriptor(keypoints[i])));
  }

  // Run the SURF algorithm for each color, this returns
  // the descriptors in descWrapper, which is a matrix where each row
  // defines a descriptor and each column is an entry in that
  // descriptor.
  vector<Mat> splitColorImage;
  split(colorImage, splitColorImage);
  for (vector<Mat>::iterator channelI = splitColorImage.begin();
       channelI != splitColorImage.end();
       ++channelI) {

    Mat descWrapper;
    surfImpl_.compute(*channelI,
                      const_cast<vector<KeyPoint>&>(keypoints),
                      descWrapper);

    // Now extract the descriptors into the SURFDescriptor objects.
    for (unsigned int i = 0; i < keypoints.size(); ++i) {
      Mat curRow(descWrapper.row(i));
      (*descriptors)[i]->AppendVals(curRow.begin<float>(),
                                    curRow.end<float>());
    }
  }
}

}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/all_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/contains.hpp>
#include <boost/hana/functional/curry.hpp>
#include <boost/hana/permutations.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto is_permutation_of = hana::curry<2>([](auto xs, auto perm) {
    return hana::contains(hana::permutations(xs), perm);
});

int main() {
    BOOST_HANA_CONSTEXPR_CHECK(
        hana::all_of(
            hana::make_tuple(
                hana::make_tuple('1', 2, 3.0),
                hana::make_tuple('1', 3.0, 2),
                hana::make_tuple(2, '1', 3.0),
                hana::make_tuple(2, 3.0, '1'),
                hana::make_tuple(3.0, '1', 2),
                hana::make_tuple(3.0, 2, '1')
            ),
            is_permutation_of(hana::make_tuple('1', 2, 3.0))
        )
    );
}


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

  @copyright 2017 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_ALGORITHM_NOT_ALL_OF_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_NOT_ALL_OF_HPP_INCLUDED

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/detail/identity.hpp>
#include <algorithm>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns true if pred returns false on any element in the range [first,last),
    and false otherwise (in particular when the range is empty).

    @param first  Beginning of the range of elements to test
    @param last   End of the range of elements
    @param f      predicate function object that will be applied.

    @par Requirement

      - @c first and @c last must be pointers to Vectorizable type.

      - @c f must be a polymorphic unary function object, i.e cnot_allable on generic types.
      - if @c f is not present the function test if any  element is zero

    @par Example;:

      @snippet not_all_of.cpp not_all_of

    @par Possible output:

      @snippet not_all_of.txt not_all_of

  **/
  template<typename T, typename Pred>
  bool not_all_of(T const* first, T const* last, Pred f)
  {
    auto pr = segmented_aligned_range(first,last);

    if(!std::all_of(pr.head.begin(), pr.head.end(), f)) return true;
    if(!std::all_of(pr.body.begin(), pr.body.end(), [&f](pack<T> const& x) { return all(f(x)); }))
      return true;
    if (!std::all_of(pr.tail.begin(), pr.tail.end(), f)) return true;

    return false;
  }

  template<typename T> BOOST_FORCEINLINE bool not_all_of(T const* first, T const* last)
  {
    return not_all_of(first, last, detail::identity());
  }
} }

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_INDIRECT_INCLUDE
#define MTL_MATRIX_INDIRECT_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/detail/range_generator.hpp>
#include <boost/numeric/mtl/utility/complexity.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/iset.hpp>
#include <boost/numeric/mtl/operation/is_negative.hpp>
#include <boost/numeric/mtl/matrix/mat_expr.hpp>

namespace mtl { namespace matrix {

/// Class for indirect access to matrix
/** So far only constant access, mutable access for appropriate matrices might be added later. **/
template <typename Matrix>
struct indirect
  : mat_expr<indirect<Matrix> >,
    const_crtp_base_matrix<indirect<Matrix>, typename Collection<Matrix>::value_type, typename Collection<Matrix>::size_type>
{
    typedef indirect                                self;
    typedef Matrix                                  other;
    typedef typename Collection<Matrix>::value_type value_type;
    typedef typename Collection<Matrix>::size_type  size_type;
    // if implementation uses const_reference -> change collection accordingly

    /// Construct from constant matrix reference and isets for rows and columns
    indirect(const Matrix& ref, const iset& rows, const iset& cols) : ref(ref), rows(rows), cols(cols) {}

    friend size_type inline num_rows(const self& A) { return A.rows.size(); } ///< Number of rows
    friend size_type inline num_cols(const self& A) { return A.cols.size(); } ///< Number of colums
    size_type nnz() const { return num_rows(*this) * num_cols(*this); } ///< Number of non-zeros

    size_type dim1() const { return rows.size(); } ///< Dimension 1 is equal to number of rows
    size_type dim2() const { return cols.size(); } ///< Dimension 2 is equal to number of columns

    value_type operator() (size_type r, size_type c) const
    {   return (ref(rows[r], cols[c]));    } ///< Read A[r][c]

  private:
    const Matrix& ref;
    iset          rows, cols;
};

template <typename Matrix>
inline std::size_t size(const indirect<Matrix>& A)
{     return num_rows(A) * num_rows(A); }

}} // namespace mtl::matrix

// -- Range generators in utility/range_generator.hpp
// -- Property maps in utility/property_maps.hpp

#endif // MTL_MATRIX_INDIRECT_INCLUDE


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_OPERATOR_FUNCTIONS_IF_ELSE_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_OPERATOR_FUNCTIONS_IF_ELSE_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/proto/tags.hpp>

/*!
 * \ingroup boost_simd_operator
 * \defgroup boost_simd_operator_if_else if_else
 *
 * \par Description
 * return the elementwise selected element from the second and third operand
 * according to the non nullity of the first operand.
 * parameters 2 and 3 must share the same type and also the same element size
 * as parameter 1
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/if_else.hpp>
 * \endcode
 *
 * \par Alias
 * \arg where
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0,class A1,class A2>
 *     meta::call<tag::if_else_(A0,A1,A2)>::type
 *     if_else(const A0 & a0,const A1 & a1,const A2 & a2);
 * }
 * \endcode
 *
 * \param a0 the first parameter of if_else
 * \param a1 the second parameter of if_else
 * \param a2 the third parameter of if_else
 *
 * \return a value of the common type of the second and third  parameter
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag if_else_ of functor if_else
     *        in namespace boost::simd::tag for toolbox boost.simd.operator
    **/
    struct if_else_ : ext::elementwise_<if_else_> { typedef ext::elementwise_<if_else_> parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::if_else_          , if_else         , 3 )
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::if_else_          , where           , 3 )
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::if_else_          , select          , 3 )
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::if_else_          , sel             , 3 )

} }

namespace boost { namespace dispatch { namespace meta
{
  template<>
  struct hierarchy_of<boost::proto::tag::if_else_>
  {
    typedef boost::simd::tag::if_else_ type;
  };
  template<>
  struct proto_tag<boost::simd::tag::if_else_>
  {
    typedef boost::proto::tag::if_else_ type;
  };
} } }

#include <boost/simd/toolbox/operator/specific/common.hpp>

#endif


#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/obj_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <string>
#include <functional> // std::function
#include <map>

#include <pcl/registration/super4pcs.h>

#include <gr/shared.h>
#include "../demo-utils.h"

// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

using namespace gr;

using loadfunc = std::function<int(const std::string &, pcl::PointCloud<PointNT> &)>;
const std::map<std::string, loadfunc> loaders =
{
    { "obj", pcl::io::loadOBJFile<PointNT> },
    { "ply", pcl::io::loadPLYFile<PointNT> },
    { "pcd", pcl::io::loadPCDFile<PointNT> }
};

// src: https://en.cppreference.com/w/cpp/string/byte/tolower
std::string str_tolower(std::string s) {
    std::transform(s.begin(), s.end(), s.begin(),
                   [](unsigned char c){ return std::tolower(c); } // correct
                  );
    return s;
}

bool
load(const std::string& filename, PointCloudT::Ptr& pcloud){
    auto getFileExt = [] (const std::string& s) -> std::string {

       size_t i = s.rfind('.', s.length());
       return i != std::string::npos
                 ? str_tolower( s.substr(i+1, s.length() - i) )
                 : "";
    };

    std::string ext = getFileExt(filename);
    auto l = loaders.find(ext);
    if( l != loaders.end() ) {
        return (*l).second( filename, *pcloud ) >= 0;
    }
    pcl::console::print_error ("Unsupported file extension: %s\n", ext.c_str());
    return false;
}

// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
  // Point clouds
  PointCloudT::Ptr object (new PointCloudT);
  PointCloudT::Ptr object_aligned (new PointCloudT);
  PointCloudT::Ptr scene (new PointCloudT);

  // Get input object and scene
  if (argc < 3)
  {
    pcl::console::print_error ("Syntax is: %s scene.obj object.obj [PARAMS]\n", argv[0]);
    Demo::printParameterList();
    return (-1);
  }

  std::string objPath {argv[2]};
  std::string scenePath {argv[1]};

  // Load object and scene
  pcl::console::print_highlight ("Loading point clouds...\n");
  if ( ! ( load(objPath, object) && load(scenePath, scene) ) )
  {
    pcl::console::print_error ("Error loading object/scene file!\n");
    return (-1);
  }

  if(int c = Demo::getArgs(argc, argv) != 0)
    {
      Demo::printUsage(argc, argv);
      exit(std::max(c,0));
    }

  pcl::Super4PCS<PointNT,PointNT> align;
  auto &options = align.getOptions();
  Demo::setOptionsFromArgs(options);

  // Perform alignment
  pcl::console::print_highlight ("Starting alignment...\n");
  align.setInputSource (object);
  align.setInputTarget (scene);

  {
    pcl::ScopeTime t("Alignment");
    align.align (*object_aligned);
  }

  if (align.hasConverged ())
  {
    // Print results
    printf ("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation ();
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
    pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
    pcl::console::print_info ("\n");

    // Show alignment
    pcl::visualization::PCLVisualizer visu("Alignment - Super4PCS");
    visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
    visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");

    pcl::console::print_highlight ("Saving registered cloud to %s ...\n", Demo::defaultPlyOutput.c_str());
    pcl::io::savePLYFileBinary<PointNT>(Demo::defaultPlyOutput, *object_aligned);

    visu.spin ();
  }
  else
  {
    pcl::console::print_error ("Alignment failed!\n");
    return (-1);
  }

  return (0);
}


#include "BoostBasedPrimesCalculator.hpp"

#include <boost/math/special_functions/prime.hpp>
#include <boost/throw_exception.hpp>
#include <sstream>
#include <stdexcept>
#include <string>

namespace {
static constexpr primes::PrimesCalculator::Index minIndex = 1;
static constexpr primes::PrimesCalculator::Index maxIndex = 10000;
auto is_index_in_allowed_range(primes::PrimesCalculator::Index n) {
  return n >= minIndex && n <= maxIndex;
}

auto createIndexNotInRangeExplanation(primes::PrimesCalculator::Index actual) {
  std::stringstream ss;
  ss << "PrimesCalculator::Index index must be in range [" << minIndex << ", "
     << maxIndex << "]. Value passed: " << actual << ".";
  return ss.str();
}
}  // namespace

namespace primes {
unsigned BoostBasedPrimesCalculator::getPrime(PrimesCalculator::Index n) const {
  if (not is_index_in_allowed_range(n)) {
    BOOST_THROW_EXCEPTION(
        std::invalid_argument(createIndexNotInRangeExplanation(n)));
  }
  auto indexFromZero = n - 1u;  // boost::math calculates 1st prime at index 0.
  return boost::math::prime(indexFromZero);
}
}  // namespace primes


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     gateway_classifier_test.cpp
* \author   Collin Johnson
*
* Definition of GatewayClassifierTest.
*/

#include <hssh/local_topological/training/gateway_classifier_test.h>
#include <hssh/local_topological/training/gateway_errors.h>
#include <hssh/local_topological/training/labeled_gateway_data.h>
#include <hssh/local_topological/area_detection/gateways/classifier_based_generator.h>
#include <hssh/local_topological/area_detection/gateways/isovist_voronoi_gateway_generator.h>
#include <hssh/local_topological/area_detection/gateways/isovist_orientation_gateway_generator.h>
#include <hssh/local_topological/area_detection/gateways/gateway_classifier.h>
#include <hssh/local_topological/area_detection/gateways/gateway_locator.h>
#include <utils/histogram.h>
#include <gnuplot-iostream.h>
#include <boost/range/iterator_range.hpp>
#include <tuple>

namespace vulcan
{
namespace hssh
{

const std::string kTestGwyClassifierTempFile("tmp_gwy_classifier_test");

constexpr int kThreshIdx = 0;
constexpr int kErrorIdx = 1;
constexpr int kFalseNegIdx = 2;
using ThresholdResult = std::tuple<double, double, int>;

constexpr int kRecallIdx = 0;
constexpr int kPrecisionIdx = 1;
using pr_pair_t = std::tuple<double, double>;

std::vector<pr_pair_t> pr_with_threshold(const LabeledGatewayData& data,
                                         const GatewayClassifier& classifier,
                                         GatewayClassifier::ClassifierType type);

GatewayClassifierTest::GatewayClassifierTest(const local_topology_params_t& params,
                                             const LabeledGatewayData& trainingData,
                                             const LabeledGatewayData& testData,
                                             const SkeletonMap& skeletons,
                                             const IsovistMap& isovists)
{
    int featureRadius = params.areaParams.locatorParams.generatorParams.classifierParams.featureRadius;
    learnClassifier(trainingData, skeletons, isovists, featureRadius);

    generators_[kClassifierBasedGeneratorType] = std::make_unique<GatewayLocator>(
        std::make_unique<ClassifierBasedGenerator>(featureRadius, kTestGwyClassifierTempFile)
    );

    generators_[kIsovistOrientationGatewayGeneratorType] = std::make_unique<GatewayLocator>(create_gateway_generator(
        kIsovistOrientationGatewayGeneratorType,
        "test",
        params.areaParams.locatorParams.generatorParams
    ));

    generators_[kIsovistVoronoiGatewayGeneratorType] = std::make_unique<GatewayLocator>(create_gateway_generator(
        kIsovistVoronoiGatewayGeneratorType,
        "test",
        params.areaParams.locatorParams.generatorParams
    ));

    for(auto& g : generators_)
    {
        results_.emplace_back(generateResults(g.first, *g.second, trainingData, testData, skeletons, isovists));

        // If we used the classifier gateways, then store the classifier with the results to allow for easier
        // interaction with the UI.
        if(g.first == kClassifierBasedGeneratorType)
        {
            results_.back().classifier = classifier_;
        }
    }

    generatePRCurve(trainingData, testData);
}

GatewayClassifierTest::~GatewayClassifierTest(void)
{
    // For std::unique_ptr
}

bool GatewayClassifierTest::saveClassifier(const std::string& basename) const
{
    return classifier_->save(basename);
}

void GatewayClassifierTest::learnClassifier(const LabeledGatewayData& data,
                                            const SkeletonMap& skeletons,
                                            const IsovistMap& isovists,
                                            int radius)
{
    std::vector<ThresholdResult> results;

    classifier_ = GatewayClassifier::LearnClassifier(data);
    classifier_->save(kTestGwyClassifierTempFile);

    // The classifier is learned, so now need to find the best threshold for this classifier
    // This is likely to be SLOW
    for(double minProb = 0.0; minProb <= 1.0; minProb += 0.05)
    {
        classifier_->setThreshold(minProb);
        classifier_->save(kTestGwyClassifierTempFile);
        GatewayLocator locator(std::make_unique<ClassifierBasedGenerator>(radius, kTestGwyClassifierTempFile));

        double totalTpError = 0.0;
        double totalFpError = 0.0;
        int totalFn = 0;

        for(auto& mapName : boost::make_iterator_range(data.beginMaps(), data.endMaps()))
        {
            LabeledGatewayData mapData = data.findMapExamples(mapName);
            VoronoiEdges edges(*skeletons.at(mapName), SKELETON_CELL_REDUCED_SKELETON);
            locator.locateGateways(*skeletons.at(mapName), *isovists.at(mapName));

            CellVector gatewayCells;
            for(auto& gw : locator.getGateways())
            {
                gatewayCells.push_back(gw.skeletonCell());
            }

            GatewayErrors errors(gatewayCells, mapData, edges);
            totalTpError += errors.truePosDist();
            totalFpError += errors.falsePosDist();
            totalFn += errors.numFalseNeg();
        }

        results.emplace_back(minProb, totalTpError + totalFpError, totalFn);
    }

    // Find the threshold that best balances the FP and TP error while having no FN (if possible)
    std::cout << "Classifier results:\nThresh\tError\t#fn\n";
    for(auto& res : results)
    {
        std::cout << std::get<kThreshIdx>(res) << '\t'
            << std::get<kErrorIdx>(res) << '\t'
            << std::get<kFalseNegIdx>(res) << '\n';
    }

    classifier_->setThreshold(0.53);
    classifier_->save(kTestGwyClassifierTempFile);
}

GeneratorResults GatewayClassifierTest::generateResults(
    const std::string& generatorName,
    GatewayLocator& generator,
    const LabeledGatewayData& trainingData,
    const LabeledGatewayData& testData,
    const SkeletonMap& skeletons,
    const IsovistMap& isovists
)
{
    GeneratorResults genResults;
    genResults.generatorName = generatorName;
    genResults.trainingResults = testGenerator(generator, trainingData, skeletons, isovists);
    genResults.testResults = testGenerator(generator, testData, skeletons, isovists);
    genResults.overallTest = calculateOverallResults(genResults.testResults);
    genResults.overallTraining = calculateOverallResults(genResults.trainingResults);
    return genResults;
}

std::vector<GatewayMapResults> GatewayClassifierTest::testGenerator(GatewayLocator& locator,
                                                                    const LabeledGatewayData& data,
                                                                    const SkeletonMap& skeletons,
                                                                    const IsovistMap& isovists)
{
    std::vector<GatewayMapResults> results;

    for(auto& mapName : boost::make_iterator_range(data.beginMaps(), data.endMaps()))
    {
        const auto& skeleton = skeletons.find(mapName)->second;
        const auto& field = isovists.find(mapName)->second;

        locator.clearGateways();
        locator.locateGateways(*skeleton, *field);

        VoronoiEdges edges(*skeleton, SKELETON_CELL_REDUCED_SKELETON);
        results.push_back(calculateResults(mapName,
                                           locator.getGateways(),
                                           edges,
                                           data));
    }

    return results;
}

GatewayMapResults GatewayClassifierTest::calculateResults(const std::string& mapName,
                                                          const std::vector<Gateway>& gateways,
                                                          const VoronoiEdges& edges,
                                                          const LabeledGatewayData& truth)
{
    GatewayMapResults results;
    results.mapName = mapName;

    LabeledGatewayData mapTruth = truth.findMapExamples(mapName);

    CellVector gatewayCells;
    for(auto& gwy : gateways)
    {
        gatewayCells.push_back(gwy.skeletonCell());
    }
    GatewayErrors errors(gatewayCells, mapTruth, edges);

    results.numTruePositives = errors.numTruePos();
    results.numFalsePositives = errors.numFalsePos();
    results.numFalseNegatives = errors.numFalseNeg();
    results.numActualGateways = results.numTruePositives + results.numFalseNegatives;

    return results;
}

GatewayMapResults GatewayClassifierTest::calculateOverallResults(const std::vector<GatewayMapResults>& results)
{
    GatewayMapResults overall;
    overall.mapName = "Overall";

    for(auto& r : results)
    {
        overall.numActualGateways += r.numActualGateways;
        overall.numTruePositives += r.numTruePositives;
        overall.numFalsePositives += r.numFalsePositives;
        overall.numFalseNegatives += r.numFalseNegatives;
    }

    return overall;
}

void GatewayClassifierTest::generatePRCurve(const LabeledGatewayData& trainingData,
                                            const LabeledGatewayData& testData)
{
//     std::vector<pr_pair_t> trainingSvm = pr_with_threshold(trainingData, *classifier_, GatewayClassifier::svm);
    std::vector<pr_pair_t> trainingLr = pr_with_threshold(trainingData, *classifier_, GatewayClassifier::adaboost);
//     std::vector<pr_pair_t> testSvm = pr_with_threshold(testData, *classifier_, GatewayClassifier::svm);
    std::vector<pr_pair_t> testLr = pr_with_threshold(testData, *classifier_, GatewayClassifier::adaboost);

    static Gnuplot plot;

    plot << "set yrange [0:1.01]\n";
    plot << "set xrange [0:1]\n";
    plot << "set title 'P-R Curve for gateway probability'\n";
    plot << "set xlabel 'Recall and (1 - threshold)'\n";
    plot << "set ylabel 'Precision'\n";
//     plot << "plot '-' with lines title 'Training SVM',";
    plot << "plot '-' with lines title 'Training LR',";
//     plot << "'-' with lines title 'Test SVM',";
    plot << "'-' with lines title 'Test LR'\n";
//     plot << "plot '-' with lines title 'Dist Weights', '-' with lines title 'Line Weights'\n";
//     plot.send1d(trainingSvm);
    plot.send1d(trainingLr);
//     plot.send1d(testSvm);
    plot.send1d(testLr);
}

std::vector<pr_pair_t> pr_with_threshold(const LabeledGatewayData& data,
                                         const GatewayClassifier& classifier,
                                         GatewayClassifier::ClassifierType type)
{
    const double kStepsize = 0.01;
    const int kNumSteps = (1.0 / kStepsize) + 1;

    constexpr int kTp = 0;
    constexpr int kFp = 1;
    constexpr int kFn = 2;

    using Results = std::tuple<int, int, int>;
    std::vector<Results> results(kNumSteps, std::make_tuple(0, 0, 0));

    utils::Histogram probHist(20);

    for(auto& example : data)
    {
        auto result = classifier.classifyGateway(example.features, type);
        probHist.addValue(result.probability);

        for(int n = 0; n < kNumSteps; ++n)
        {
            const double threshold = 1.0 - (n * kStepsize);
            if((result.probability >= threshold) && example.isGateway)
            {
                ++std::get<kTp>(results[n]);
            }
            else if((result.probability >= threshold) && !example.isGateway)
            {
                ++std::get<kFp>(results[n]);
            }
            else if(result.probability < threshold && example.isGateway)
            {
                ++std::get<kFn>(results[n]);
            }
        }
    }

//     std::cout << probHist << '\n';

    std::vector<pr_pair_t> prResults(kNumSteps);
    for(int n = 0; n < kNumSteps; ++n)
    {
        int totalTrue = std::get<kTp>(results[n]) + std::get<kFn>(results[n]);
        std::get<kRecallIdx>(prResults[n]) = (totalTrue > 0) ?
            static_cast<double>(std::get<kTp>(results[n])) / totalTrue : 0.0;

        int totalPositives = std::get<kTp>(results[n]) + std::get<kFp>(results[n]);
        std::get<kPrecisionIdx>(prResults[n]) = (totalPositives > 0) ?
            static_cast<double>(std::get<kTp>(results[n])) / totalPositives : 0.0;
    }

    return prResults;
}

} // namespace hssh
} // namespace vulcan


#ifndef GLMQUATSERIALIZE_H_
#define GLMQUATSERIALIZE_H_

#include <boost/serialization/serialization.hpp>

#define GLM_FORCE_RADIANS
#include "glm/gtc/quaternion.hpp"

namespace boost
{
namespace serialization
{

template<class Archive> void serialize(Archive& ar, glm::quat& q, const unsigned int version)
{
	ar & q.x & q.y & q.z & q.w;
}

}
}

#endif /* GLMQUATSERIALIZE_H_ */


/// @file observation.hpp A tuple type representing an observed feature

#ifndef BIGGLES_OBSERVATION_HPP__
#define BIGGLES_OBSERVATION_HPP__

#include <utility>
#include <functional>
#include <cmath>

#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>

#include "detail/fun.hpp"

namespace biggles
{

/// @brief A type used to represent an observation's time stamp or frame index.
typedef off_t time_stamp; // shall be signed integer type

/*
/// @brief An alias to \c boost::tuple representing an observation.
typedef boost::tuple<float, float, time_stamp> observation;
*/

class observation_t;
typedef boost::shared_ptr<observation_t> observation;

class observation_t {
    float x_, y_;
    time_stamp t_;
public:
    observation_t() : x_(0), y_(0), t_(0) {}
    observation_t(const float x, const float y, const float t) : x_(x), y_(y), t_(t) {}
    observation_t(const observation_t& o) : x_(o.x_), y_(o.y_), t_(o.t_) {}
    observation_t& operator=(const observation_t& o) {
        if (&o == this) return *this;
        x_ = o.x_;
        y_ = o.y_;
        t_ = o.t_;
        return *this;
    }
    //
    bool operator==(const observation_t& o) const {
        return x_ == o.x_ and y_ == o.y_ and t_ == o.t_;
    }
    bool operator<(const observation_t& o) const {
        return t_ < o.t_ or (t_ == o.t_ and x_ < o.x_) or (t_ == o.t_ and x_ == o.x_ and y_ < o.y_);
    }
    bool operator!=(const observation_t& o) const { return not (*this == o); }
    //
    friend const float x(const observation& o);
    friend const float y(const observation& o);
    friend const time_stamp t(const observation& o);
    void set_x(const float x) { x_ = x; }
    void set_y(const float y) { y_ = y; }
    void set_t(const float t) { t_ = t; }
};

/// @brief Extract a reference to a observation's x-co-ordinate (const).
inline const float x(const observation& o) { return o->x_; }

/// @brief Extract a reference to a observation's y-co-ordinate (const).
inline const float y(const observation& o) { return o->y_; }

/// @brief Extract a reference to a observation's time stamp (const).
inline const time_stamp t(const observation& o) { return o->t_; }

/*
/// @brief Extract a reference to a observation's x-co-ordinate (non-const).
inline float& x(observation& o) { return boost::tuples::get<0>(o); }

/// @brief Extract a reference to a observation's y-co-ordinate (non-const).
inline float& y(observation& o) { return boost::tuples::get<1>(o); }

/// @brief Extract a reference to a observation's time stamp (non-const).
inline time_stamp& t(observation& o) { return boost::tuples::get<2>(o); }
*/

/// \brief creates a new observation from cooridantes
inline observation new_obs(float x, float y, time_stamp t) {
    return observation(new observation_t(x, y, t));
}

/// \brief is the observation defined? If no, then it has no coordinates
inline bool noobs(const observation& o) { return o.use_count() == 0; }

/// \brief a compare functor for sets of observations
struct earlier { bool operator()(const observation& a, const observation& b) { return t(a) < t(b); } };

/// @brief Test if observations are in each others light cone
inline bool observations_see_each_other(const observation& o1, const observation& o2, const float& speed_of_light);
inline float observations_see_each_other2(const observation& o1, const observation& o2, const float& speed_of_light);

/// \brief Euclidean distance disregarding the time.
inline float dist(const observation& p, const observation& q) {
    return sqrtf(square(x(p) - x(q)) + square(y(p) - y(q)));
}

inline observation convex_combination(const observation& o1, const observation& o2, time_stamp t0) {
    const float a1 = float(t(o2) - t0) / float(t(o2) - t(o1));
    return new_obs(a1*x(o1) + (1.f-a1)*x(o2), a1*y(o1) + (1.f-a1)*y(o2), t0);
}

} // namespace biggles

#define WITHIN_BIGGLES_OBSERVATION_HPP__
#include "observation.tcc"
#undef WITHIN_BIGGLES_OBSERVATION_HPP__

#endif // BIGGLES_OBSERVATION_HPP__


//==================================================================================================
/*!
  @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_BITS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_BITS_HPP_INCLUDED

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

 /*!

    @ingroup group-bitwise
    Function object implementing bits capabilities

    Returns an unsigned integer value which has the same bits as the input

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = bits(x);
    @endcode

    is equivalent to:

    @code
    as_integer_t<T,unsigned> r = bitwise_cast< as_integer_t<T,unsigned>>(x);
    @endcode

  **/
  as_integer_t<T, unsigned> bits(Value const & v0);
} }
#endif

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

#endif


#pragma once

#include <chrono>
#include <map>
#include <random>
#include <string>

#include <dtl/dtl.hpp>
#include <dtl/filter/blocked_bloomfilter/block_addressing_logic.hpp>
#include <boost/algorithm/string.hpp>

#include "immintrin.h"

#include "cuckoofilter.hpp"
#include "cuckoofilter_tune.hpp"

#include "../model/calibration_data.hpp"
#include "../model/tuning_params.hpp"

namespace dtl {
namespace cuckoofilter {

namespace internal {

  struct cuckoofilter_tune_mock : cuckoofilter_tune {

    u32 unroll_factor;

    cuckoofilter_tune_mock(u32 unroll_factor) : unroll_factor(unroll_factor) {};

    $u32
    get_unroll_factor(u32 bits_per_tag,
                      u32 tags_per_bucket,
                      dtl::block_addressing addr_mode) const override {
      return unroll_factor;
    }
  };

} // namespace internal

//===----------------------------------------------------------------------===//
struct cuckoofilter_tune_impl : cuckoofilter_tune {

  using tuning_params = dtl::filter::model::tuning_params;
  using calibration_data = filter::model::calibration_data;

  static constexpr u32 max_k = 16;
  static constexpr u32 max_unroll_factor = 8;

  void
  set_unroll_factor(u32 bits_per_tag,
                    u32 tags_per_bucket,
                    dtl::block_addressing addr_mode,
                    u32 unroll_factor) override {
    config c { bits_per_tag, tags_per_bucket, addr_mode};
    tuning_params tp { unroll_factor };
    calibration_data::get_default_instance().put_tuning_params(c, tp);
  }

  void
  set_unroll_factor(u32 unroll_factor) {
    for ($u32 bits_per_tag : { 4u, 8u, 12u, 16u, 32u } ) {
      for ($u32 tags_per_bucket : { 1u, 2u, 4u } ) {
        for (auto addr_mode : {dtl::block_addressing::POWER_OF_TWO, dtl::block_addressing::MAGIC}) {
          set_unroll_factor(bits_per_tag, tags_per_bucket, addr_mode, unroll_factor);
        }
      }
    }
  }

  $u32
  get_unroll_factor(u32 bits_per_tag,
                    u32 tags_per_bucket,
                    dtl::block_addressing addr_mode) const override {
    config c { bits_per_tag, tags_per_bucket, addr_mode};
    try {
      auto tp = calibration_data::get_default_instance().get_tuning_params(c);
      return tp.unroll_factor;
    } catch (...) {
      std::cerr << "Missing calibrated tuning parameters." << std::endl;
      return 1; // default: SIMD, without unrolling
    }
  }

  $u32
  tune_unroll_factor(u32 bits_per_tag,
                     u32 tags_per_bucket,
                     dtl::block_addressing addr_mode,
                     u64 filter_size_bits) override {

    config c { bits_per_tag, tags_per_bucket, addr_mode};
    tuning_params tp = calibrate(c, filter_size_bits);
    calibration_data::get_default_instance().put_tuning_params(c, tp);
    return tp.unroll_factor;
  }

  void
  tune_unroll_factor(u64 filter_size_bits) {
    for ($u32 bits_per_tag : { 4u, 8u, 12u, 16u, 32u } ) {
      for ($u32 tags_per_bucket : { 1u, 2u, 4u } ) {
        for (auto addr_mode : {dtl::block_addressing::POWER_OF_TWO, dtl::block_addressing::MAGIC}) {
          tune_unroll_factor(bits_per_tag, tags_per_bucket, addr_mode, filter_size_bits);
        }
      }
    }
  }

  //===----------------------------------------------------------------------===//
  tuning_params
  calibrate(const config& c, u64 filter_size_bits) {
    using key_t = $u32;
    using word_t = $u32;

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<uint32_t> dis;

    static constexpr u32 data_size = 4u*1024*8;
    std::vector<key_t> random_data;
    random_data.reserve(data_size);
    for (std::size_t i = 0; i < data_size; i++) {
      random_data.push_back(dis(gen));
    }

    try {
      std::cerr << "t = " << std::setw(2) << c.bits_per_tag << ", "
                << "b = " << std::setw(2) << c.tags_per_bucket << ", "
                << "addr = " << std::setw(5) << (c.addr_mode == dtl::block_addressing::POWER_OF_TWO ? "pow2" : "magic")
                << ": " << std::flush;

      $f64 cycles_per_lookup_min = std::numeric_limits<$f64>::max();
      $u32 u_min = 1;

      std::size_t match_count = 0;
      uint32_t match_pos[dtl::BATCH_SIZE];

      // base lines
      $f64 cycles_per_lookup_scalar = 0.0;
      $f64 cycles_per_lookup_unroll_by_one = 0.0;

      for ($u32 u = 0; u <= max_unroll_factor; u = (u == 0) ? 1 : u * 2) {
        std::cerr << std::setw(2) << "u(" << std::to_string(u) + ") = "<< std::flush;

        u64 desired_filter_size_bits = dtl::next_power_of_two(filter_size_bits);
        const std::size_t m = desired_filter_size_bits
            + (128 * static_cast<u32>(c.addr_mode)); // enforce MAGIC addressing

        // Instantiate bloom filter logic.
        internal::cuckoofilter_tune_mock tune_mock { u };
        cuckoofilter cf(m , c.bits_per_tag, c.tags_per_bucket, tune_mock);

        // Allocate memory.
        dtl::mem::allocator_config alloc_config = dtl::mem::allocator_config::local();
        if (dtl::mem::hbm_available()) {
          // Use HBM if available
          const auto cpu_id = dtl::this_thread::get_cpu_affinity().find_first();
          const auto node_id = dtl::mem::get_node_of_cpu(cpu_id);
          const auto hbm_node_id = dtl::mem::get_nearest_hbm_node(node_id);
          alloc_config = dtl::mem::allocator_config::on_node(hbm_node_id);
        }

        dtl::mem::numa_allocator<word_t> alloc(alloc_config);
        std::vector<word_t, dtl::mem::numa_allocator<word_t>>
            filter_data(cf.size(), 0, alloc); // TODO how to pass different allocator (for KNL/HBM)

        // Note: No need to insert elements, as the BBF is branch-free.

        // Run the micro benchmark.
        $u64 rep_cntr = 0;
        auto start = std::chrono::high_resolution_clock::now();
        auto tsc_start = _rdtsc();
        while (true) {
          dtl::batch_wise(random_data.begin(), random_data.end(),
                          [&](const auto batch_begin, const auto batch_end) {
                            match_count += cf.batch_contains(&filter_data[0], &batch_begin[0], batch_end - batch_begin, match_pos, 0);
                          });

          rep_cntr++;
          std::chrono::duration<double> diff = std::chrono::high_resolution_clock::now() - start;
          if (diff.count() > 0.25) break; // Run micro benchmark for at least 250ms.
        }
        auto tsc_end = _rdtsc();

        auto cycles_per_lookup = (tsc_end - tsc_start) / (data_size * rep_cntr * 1.0);

        if (u == 0) cycles_per_lookup_scalar = cycles_per_lookup;
        if (u == 1) cycles_per_lookup_unroll_by_one = cycles_per_lookup;
        std::cerr << std::setprecision(3) << std::setw(4) << std::right << cycles_per_lookup << ", ";
        if (cycles_per_lookup < cycles_per_lookup_min) {
          cycles_per_lookup_min = cycles_per_lookup;
          u_min = u;
        }
        // Write CSV output to stdout.
        std::string filter_info = cf.name();
        boost::replace_all(filter_info, "\"", "\"\""); // escape JSON for CSV output
        std::cout
            << "\"" << filter_info << "\""
            << "," << m
            << "," << cycles_per_lookup
            << std::endl;

      }
      std::cerr << " picked u = " << u_min << " (" << cycles_per_lookup_min << " [cycles/lookup])"
                << ", speedup over u(0) = " << std::setprecision(3) << std::setw(4) << std::right << (cycles_per_lookup_scalar / cycles_per_lookup_min)
                << ", speedup over u(1) = " << std::setprecision(3) << std::setw(4) << std::right << (cycles_per_lookup_unroll_by_one / cycles_per_lookup_min)
                << " (chksum: " << match_count << ")" << std::endl;

      return tuning_params { u_min };

    } catch (...) {
      std::cerr<< " -> Failed to calibrate for t=" << c.bits_per_tag << " and b=" << c.tags_per_bucket << "." << std::endl;
    }
    return tuning_params { 1 }; // TODO throw exception (see bbf tune impl)
  }
  //===----------------------------------------------------------------------===//

};

} // namespace cuckoofilter
} // namespace dtl


#ifndef SLIC_HPP
#define SLIC_HPP
#include <SFML/Graphics.hpp> //Contains sf::Image, which is our image loader/writer.
#include <list>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>

#include <cstdlib>
#include <algorithm>

#define _USE_MATH_DEFINES
#include <cmath> //std::abs std::sqrt std::pow pi

static const double deg_to_rad = M_PI / 180.0;

struct HSV {
	double hue;
	double saturation;
	double value;

};

struct RGBA{
	double r;
	double g;
	double b;
	double a;
};

//Vertex data is simple, we just need HSV values for comparison, and rgba values to write later if we want to restore the original image without conversion.
struct vertex_data {
	RGBA rgba;
	HSV hsv;
	std::vector<int> cluster_ids; ///< ID of the cluster in the vector we're associated with. -1 is unassociated.
};

struct edge_data {
	double distance;
	int cluster_id;
};

//I am so used to using glm::vec2 at this point that not having it bothers me.
template <typename T>
struct vec2 {
	T x;
	T y;
};

//For our clusters, we don't need anything too crazy. We just need to store the HSV color value, and the x/y coordinates(so we can )
struct Cluster {
	HSV color; ///Average color of the cluster.
	vec2<int> cpos; ///Center position of the cluster.
	int id;
};

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS, vertex_data, edge_data> PIXEL_GRAPH;

class Slic {
  private:
    //Image data
    sf::Image image; ///< Proper image.
    std::string if_path; ///< Input file path.
    unsigned int num_cuts; ///< Number of graph cuts.
    unsigned int total_pixels; ///< Total pixels in the original image.
    unsigned int height; ///< Image height
    unsigned int width;  ///< Image width
    bool greyscale; ///< Did we convert to greyscale?

    //Graph data
    PIXEL_GRAPH graph; ///< Our active working graph.
    unsigned int vertex_count; ///< Our graph's vertex count.

		//Class flags.
    bool ready; ///< Did our file load correctly and are we ready to start working on it?
		bool graph_loaded; ///< Did we load our pixels to the graph?
    //Private utility functions.
    HSV rgb2hsv(RGBA rgb); ///< Converts a pixel's color data from RGBA format to HSV. This algorithm is an implementation of the one documented on wikipedia.
    double getColorDistance(HSV c1, HSV c2); ///< Returns the distance between two pixels in HSV colorspace.

		void updateClusterAverages(); ///< Updates the cluster average colors by using a connected components algorithm to determine which pixels are associated, then averaging.
		void cleanEdges(); ///< Removes extra edges from each vertex.
		std::vector<Cluster> clusters; ///< Vector of our clusters in the image. It's strictly used for the super pixel routine.
		bool isEdge(unsigned int x, unsigned int y);
		bool inBounds(unsigned int x, unsigned int y);

		bool clusterPass(int radius, unsigned int error);
		bool updateClusterPositions(unsigned int error);

		unsigned int K; ///< Number of initial clusters.
		unsigned int S; ///< Grid intervals
  public:
    Slic(std::string filename); ///< Takes a file path input and loads the image.
    ~Slic();
    void writeToDisk(std::string out_dir); ///< Writes our graph to file.
    void convertToGreyscale(); ///< Converts our image to greyscale
    void loadToGraph(); ///< Loads the current data in pixel_data data pointer to our graph.
		void generateSuperpixels(unsigned int K, unsigned int error);
};

#endif


#include "localization-summary-map/localization-summary-map-creation.h"

#include <fstream>  // NOLINT

#include <Eigen/Core>
#include <descriptor-projection/descriptor-projection.h>
#include <loopclosure-common/flags.h>
#include <loopclosure-common/types.h>
#include <map-sparsification/sampler-base.h>
#include <map-sparsification/sampler-factory.h>
#include <maplab-common/binary-serialization.h>
#include <maplab-common/eigen-proto.h>
#include <vi-map-helpers/vi-map-queries.h>
#include <vi-map/vi-map.h>

#include "localization-summary-map/localization-summary-map-cache.h"
#include "localization-summary-map/localization-summary-map.h"

namespace summary_map {

void createLocalizationSummaryMapForWellConstrainedLandmarks(
    const vi_map::VIMap& map,
    summary_map::LocalizationSummaryMap* summary_map)
    {
  vi_map_helpers::VIMapQueries queries(map);
  vi_map::LandmarkIdList landmark_ids;
  queries.getAllWellConstrainedLandmarkIds(&landmark_ids);

  createLocalizationSummaryMapFromLandmarkList(
      map, landmark_ids, nullptr, summary_map);
}

void createLocalizationSummaryMapForSummarizedLandmarks(
    const vi_map::VIMap& map, const double landmark_keep_fraction,
    summary_map::LocalizationSummaryMap* summary_map) {
  using map_sparsification::SamplerBase;
  SamplerBase::Ptr sampler = map_sparsification::createSampler(
      SamplerBase::Type::kLpsolvePartitionIlp);

  vi_map_helpers::VIMapQueries queries(map);
  vi_map::LandmarkIdList good_landmark_ids;
  queries.getAllWellConstrainedLandmarkIds(&good_landmark_ids);
  const size_t num_good_landmarks = good_landmark_ids.size();
  const size_t desired_num_landmarks =
      landmark_keep_fraction * num_good_landmarks;

  vi_map::LandmarkIdSet landmarks_to_keep;
  sampler->sample(map, desired_num_landmarks, &landmarks_to_keep);

  vi_map::LandmarkIdList landmarks_to_keep_list(
      landmarks_to_keep.begin(), landmarks_to_keep.end());
  createLocalizationSummaryMapFromLandmarkList(
      map, landmarks_to_keep_list, summary_map);
}

void createLocalizationSummaryMapFromLandmarkList(
    const vi_map::VIMap& map, const vi_map::LandmarkIdList& landmark_ids,
    summary_map::LocalizationSummaryMap* summary_map) {
  createLocalizationSummaryMapFromLandmarkList(
      map, landmark_ids, nullptr, summary_map);
}

//从地图点到定位用地图
void createLocalizationSummaryMapFromLandmarkList(
    const vi_map::VIMap& map,
    const vi_map::LandmarkIdList& landmark_ids,
    LocalizationSummaryMapCache* summary_map_cache,
    summary_map::LocalizationSummaryMap* summary_map) {
  CHECK_NOTNULL(summary_map);
  /// The position of the landmarks in the global frame of reference.
  //地图点在世界坐标系中的坐标
  Eigen::Matrix3Xd G_landmark_position;
  /// The position of the observers in the global frame of reference.
  //
  Eigen::Matrix3Xd G_observer_position;
  /// A set of projected_descriptors from observations of landmarks.
  Eigen::MatrixXf projected_descriptors;
  /// An index of a key-frame for every observation (descriptor).
  Eigen::Matrix<unsigned int, Eigen::Dynamic, 1> observer_indices;
  /// A mapping from observation (descriptor) to index in G_landmark_position.
  Eigen::Matrix<unsigned int, Eigen::Dynamic, 1> observation_to_landmark_index;

  CHECK(!landmark_ids.empty());
  G_landmark_position.resize(Eigen::NoChange, landmark_ids.size());//所有的地图点的坐标

  std::vector<vi_map::KeypointIdentifier> observations;
  // Best guess reserve.
  observations.reserve(landmark_ids.size() * 4);

  std::vector<unsigned int> observation_to_landmark;
  observation_to_landmark.reserve(landmark_ids.size() * 4);

  // We first collect all observations to the landmarks in question.
  for (size_t landmark_index = 0; landmark_index < landmark_ids.size();
       ++landmark_index)
  {//遍历所有的地图点
    const vi_map::LandmarkId& landmark_id = landmark_ids[landmark_index];//地图点id
    const vi_map::Landmark& landmark = map.getLandmark(landmark_id);//地图点
    G_landmark_position.col(landmark_index) =//地图点坐标
        map.getLandmark_G_p_fi(landmark_id);

    const std::vector<vi_map::KeypointIdentifier>& landmark_observations =
        landmark.getObservations();//所有地图点的观测信息，都是第几个节点第几个相机去观测到的
    observations.insert(
        observations.end(), landmark_observations.begin(),
        landmark_observations.end());

    // Push the index of the landmark for all the observations.
    for (size_t i = 0; i < landmark_observations.size(); ++i) {
      observation_to_landmark.push_back(landmark_index);//方便索引
    }
  }
  CHECK(!observations.empty()) << "No landmark observations for summary map.";

  // Copy all the observation to landmark indices into the summary-map format.
  observation_to_landmark_index =
      Eigen::Map<const Eigen::Matrix<unsigned int, Eigen::Dynamic, 1> >(
          observation_to_landmark.data(), observation_to_landmark.size(), 1);

  const char* loop_closure_files_path = getenv("MAPLAB_LOOPCLOSURE_DIR");
  CHECK_NE(loop_closure_files_path, static_cast<char*>(NULL))
      << "MAPLAB_LOOPCLOSURE_DIR environment variable is not set.\n"
         "Source the MapLab environment from your workspace:\n"
         "  . devel/setup.bash";

  if (FLAGS_feature_descriptor_type == loop_closure::kFeatureDescriptorFREAK) {
    if (FLAGS_lc_projection_matrix_filename == "") {
      FLAGS_lc_projection_matrix_filename =
          std::string(loop_closure_files_path) + "/projection_matrix_freak.dat";
    }
  } else {
    if (FLAGS_lc_projection_matrix_filename == "") {
      FLAGS_lc_projection_matrix_filename =
          std::string(loop_closure_files_path) + "/projection_matrix_brisk.dat";
    }
  }
  Eigen::MatrixXf projection_matrix;
  std::ifstream deserializer(FLAGS_lc_projection_matrix_filename);
  CHECK(deserializer.is_open()) << "Cannot load projection matrix from file: "
                                << FLAGS_lc_projection_matrix_filename;
  common::Deserialize(&projection_matrix, &deserializer);

  projected_descriptors.resize(
      FLAGS_lc_target_dimensionality, observations.size());
  observer_indices.resize(observations.size());
  Aligned<std::vector, Eigen::Vector3d> G_observer_positions;

  std::unordered_map<vi_map::VisualFrameIdentifier, int> frame_id_to_index;//观测到这个地图点的是哪个相机的第几帧,它首次看到的第一个地图点
  int observer_index = 0;
  for (size_t observation_index = 0; observation_index < observations.size();//所有地图点的观测信息
       ++observation_index)
  {
    // We store the observer index for covisibility graph based filtering.
    const vi_map::KeypointIdentifier& observation =//当前的观测数据结构
        observations[observation_index];
    std::unordered_map<vi_map::VisualFrameIdentifier, int>::const_iterator it =
        frame_id_to_index.find(observation.frame_id);//
    if (it == frame_id_to_index.end()) {//这个啥意思？每一个相机的每一个帧只存储一个它对应的地图点？
      it = frame_id_to_index
               .insert(std::make_pair(observation.frame_id, observer_index))
               .first;
      Eigen::Vector3d G_observer_position =
          map.getVertex_G_p_I(observation.frame_id.vertex_id);
      G_observer_positions.push_back(G_observer_position);
      ++observer_index;
    }
    observer_indices(observation_index, 0) = it->second;
    //如果在之后的地图点找到这个节点的这个相机，那么就通过observer_indices索引上
    //第几个观测信息，还观测到了谁

    CHECK_LT(observation_index, observation_to_landmark.size());
    const size_t landmark_index = observation_to_landmark[observation_index];
    CHECK_LT(landmark_index, landmark_ids.size());
    const vi_map::LandmarkId& landmark_id = landmark_ids[landmark_index];
    if (summary_map_cache == nullptr ||
        !summary_map_cache->getProjectedDescriptorForLandmark(
            observation, landmark_id,
            projected_descriptors.col(observation_index))) {
      // No projected descriptor is stored yet, need to compute first.
      const aslam::VisualFrame& frame =
          map.getVertex(observation.frame_id.vertex_id)
              .getVisualFrame(observation.frame_id.frame_index);

      Eigen::Map<const Eigen::Matrix<unsigned char, Eigen::Dynamic, 1> >
          raw_descriptor(
              frame.getDescriptor(observation.keypoint_index),
              frame.getDescriptorSizeBytes(), 1);

      // Project the descriptors directly into the descriptor storage.
      descriptor_projection::ProjectDescriptor(
          raw_descriptor, projection_matrix, FLAGS_lc_target_dimensionality,
          projected_descriptors.col(observation_index));

      if (summary_map_cache != nullptr) {
        summary_map_cache->addProjectedDescriptor(
            observation, landmark_id,
            projected_descriptors.col(observation_index));
      }
    }
  }
  G_observer_position.resize(Eigen::NoChange, G_observer_positions.size());
  for (size_t i = 0; i < G_observer_positions.size(); ++i) {
    G_observer_position.col(i) = G_observer_positions[i];
  }

  summary_map->setGLandmarkPosition(G_landmark_position);
  summary_map->setGObserverPosition(G_observer_position);
  summary_map->setProjectedDescriptors(projected_descriptors);
  summary_map->setObserverIndices(observer_indices);
  summary_map->setObservationToLandmarkIndex(observation_to_landmark_index);
}
}  // namespace summary_map


// Copyright 2017 ProjectQ-Framework (www.projectq.ch)
//
// 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 FUSION_MPI_HPP
#define FUSION_MPI_HPP

#include <glog/logging.h>

#include <algorithm>
#include <boost/format.hpp>
#include <complex>
#include <iostream>
#include <set>
#include <vector>

#include "alignedallocator.hpp"
#include "funcs.hpp"

class Item
{
public:
     using Index = int64_t;
     using IndexVector = std::vector<Index>;
     using Complex = std::complex<double>;
     using Matrix
         = std::vector<std::vector<Complex, aligned_allocator<Complex, 64>>>;

     Item(Matrix mat, uint64_t aFlags, IndexVector idx)
         : mat_(mat), idx_(idx), m_flags(aFlags)
     {}

     ~Item()
     {}

     Matrix& get_matrix()
     {
          return mat_;
     }
     IndexVector& get_indices()
     {
          return idx_;
     }
     uint64_t flags()
     {
          return m_flags;
     }

     Matrix mat_;
     IndexVector idx_;
     uint64_t m_flags;
};

class Fusion
{
public:
     using Index = int64_t;
     using IndexSet = std::set<Index>;
     using IndexVector = std::vector<Index>;
     using Complex = std::complex<double>;
     using Matrix
         = std::vector<std::vector<Complex, aligned_allocator<Complex, 64>>>;
     using ItemVector = std::vector<Item>;

     unsigned num_qubits()
     {
          return set_.size();
     }

     std::size_t size() const
     {
          return items_.size();
     }

     void insert(Matrix matrix, uint64_t flags, IndexVector index_list,
                 IndexVector const& ctrl_list = {})
     {
          if (matrix.size() == 1) {
               factor *= matrix[0][0];
               return;
          }

          for (auto idx: index_list)
               set_.emplace(idx);

          handle_controls(matrix, index_list, ctrl_list);
          Item item(matrix, flags, index_list);
          items_.push_back(item);
     }

     void perform_fusion(Matrix& fused_matrix, IndexVector& index_list,
                         IndexVector& ctrl_list, uint64_t& out_flags)
     {
          index_list.insert(index_list.end(), set_.begin(), set_.end());

          std::size_t N = num_qubits();
          fused_matrix = Matrix(
              1UL << N,
              std::vector<Complex, aligned_allocator<Complex, 64>>(1UL << N));
          auto& M = fused_matrix;

          for (std::size_t i = 0; i < (1UL << N); ++i)
               M[i][i] = factor;

          DLOG(INFO)
              << boost::format(
                     "perform_fusion(): items_.size(): %d, num qubits: %d")
                     % items_.size() % N;
          out_flags |= MatProps::IS_DIAG;
          for (auto& item: items_) {
               uint64_t flags = item.flags();
               if (!(flags & MatProps::IS_DIAG))
                    out_flags &= ~MatProps::IS_DIAG;

               auto const& idx = item.get_indices();
               IndexVector idx2mat(idx.size());
               for (std::size_t i = 0; i < idx.size(); ++i)
                    idx2mat[i] = ((std::equal_range(index_list.begin(),
                                                    index_list.end(), idx[i]))
                                      .first
                                  - index_list.begin());

               for (std::size_t k = 0; k < (1UL << N);
                    ++k) {  // loop over big matrix columns
                    // check if column index satisfies control-mask
                    // if not: leave it unchanged
                    std::vector<Complex> oldcol(1UL << N);
                    for (std::size_t i = 0; i < (1UL << N); ++i)
                         oldcol[i] = M[i][k];

                    for (std::size_t i = 0; i < (1UL << N); ++i) {
                         std::size_t local_i = 0;
                         for (std::size_t l = 0; l < idx.size(); ++l)
                              local_i |= ((i >> idx2mat[l]) & 1UL) << l;

                         Complex res = 0.;
                         for (std::size_t j = 0; j < (1UL << idx.size()); ++j) {
                              std::size_t locidx = i;
                              for (std::size_t l = 0; l < idx.size(); ++l)
                                   if (((j >> l) & 1UL)
                                       != ((i >> idx2mat[l]) & 1UL))
                                        locidx ^= (1UL << idx2mat[l]);
                              res += oldcol[locidx]
                                     * item.get_matrix()[local_i][j];
                         }
                         M[i][k] = res;
                    }
               }
          }
          ctrl_list.reserve(ctrl_set_.size());
          for (auto ctrl: ctrl_set_)
               ctrl_list.push_back(ctrl);

          factor = 1.0;
     }

     static void add_controls(Matrix& matrix, IndexVector& indexList,
                              IndexVector const& new_ctrls)
     {
          indexList.reserve(indexList.size() + new_ctrls.size());
          indexList.insert(indexList.end(), new_ctrls.begin(), new_ctrls.end());

          std::size_t F = (1UL << new_ctrls.size());
          Matrix newmatrix(F * matrix.size(),
                           std::vector<Complex, aligned_allocator<Complex, 64>>(
                               F * matrix.size(), 0.));

          std::size_t Offset = newmatrix.size() - matrix.size();

          for (std::size_t i = 0; i < Offset; ++i)
               newmatrix[i][i] = 1.;
          for (std::size_t i = 0; i < matrix.size(); ++i) {
               for (std::size_t j = 0; j < matrix.size(); ++j)
                    newmatrix[Offset + i][Offset + j] = matrix[i][j];
          }
          matrix = std::move(newmatrix);
     }

private:
     void handle_controls(Matrix& matrix, IndexVector& indexList,
                          IndexVector const& ctrlList)
     {
          auto unhandled_ctrl = ctrl_set_;  // will contain all ctrls that are
                                            // not part of the new command
          // --> need to be removed from the global mask and the controls
          // incorporated into the old commands (the ones already in the list).

          for (auto ctrlIdx: ctrlList) {
               if (ctrl_set_.count(ctrlIdx)
                   == 0) {  // need to either add it to the list or to the
                            // command
                    if (items_.size() > 0) {  // add it to the command
                         add_controls(matrix, indexList, {ctrlIdx});
                         set_.insert(ctrlIdx);
                    }
                    else  // add it to the list
                         ctrl_set_.emplace(ctrlIdx);
               }
               else
                    unhandled_ctrl.erase(ctrlIdx);
          }
          // remove global controls which are no longer global (because the
          // current command didn't have it)
          if (unhandled_ctrl.size() > 0) {
               IndexVector new_ctrls;
               new_ctrls.reserve(unhandled_ctrl.size());
               for (auto idx: unhandled_ctrl) {
                    new_ctrls.push_back(idx);
                    ctrl_set_.erase(idx);
                    set_.insert(idx);
               }
               for (auto& item: items_)
                    add_controls(item.get_matrix(), item.get_indices(),
                                 new_ctrls);
          }
     }

public:
     IndexSet set_;
     ItemVector items_;
     IndexSet ctrl_set_;

     Complex factor = 1.0;
};

#endif


#define BOOST_TEST_MODULE pcraster geo point_value
#include <boost/test/unit_test.hpp>
#include "geo_point.h"
#include "geo_pointvalue.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace geo;

  {
    typedef Point<double, 2> Point;
    typedef double Value;

    Point point(2.2, 3.3);
    Value value(5.5);

    PointValue<Point, Value> height(point, value);
    BOOST_CHECK(height.point() == point);
    BOOST_CHECK(height.value() == value);

    point += 8.8;
    height.setPoint(point);
    BOOST_CHECK(height.point() == point);

    value += 9.9;
    height.setValue(value);
    BOOST_CHECK(height.value() == value);
  }
}


/*
 * Copyright (C) 2005-2020 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * 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 otbResetMarginFilter_hxx
#define otbResetMarginFilter_hxx

#include "otbResetMarginFilter.h"
#include "otbInterval.h"
#include "otbMacro.h"
#include "otbLogHelpers.h"
#include "itkImageScanlineConstIterator.h"
#include "itkImageScanlineIterator.h"
#include "itkProgressReporter.h"
#include <boost/numeric/interval.hpp>
#include <algorithm>
#include <cassert>
#include <ostream>

template <typename T, typename P>
inline
std::ostream & operator<<(std::ostream & os, boost::numeric::interval<T,P> const& v)
{
  return os << '[' << v.lower() << ".." << v.upper() << '[';
}

namespace otb
{

template<typename TImage>
void
ResetMarginFilter<TImage>
::ThreadedGenerateData(
    OutputImageRegionType const& outputRegionForThread,
    itk::ThreadIdType            threadId)
{
  // otbMsgDevMacro("ThreadedGenerateData begin("<<NeatRegionLogger{outputRegionForThread}<<")");
  using InputIterator   = itk::ImageScanlineConstIterator<InputImageType const>;
  using OutputIterator  = itk::ImageScanlineIterator<OutputImageType>;

  auto const* input  = this->GetInput();
  auto      * output = this->GetOutput();
  assert(input);
  assert(output);
  InputIterator  inputIterator (input,  OutputRegionToInputRegion(outputRegionForThread));
  OutputIterator outputIterator(output, outputRegionForThread);

  auto const& imgRegion = output->GetLargestPossibleRegion();
  auto const  imgSizeX  = imgRegion.GetSize()[0];
  auto const  imgSizeY  = imgRegion.GetSize()[1];
  itk::IndexValueType const  imgEndX   = imgRegion.GetIndex()[0] + imgSizeX;
  itk::IndexValueType const  imgEndY   = imgRegion.GetIndex()[1] + imgSizeY;

  auto const& size      = outputRegionForThread.GetSize();
  auto const& index     = outputRegionForThread.GetIndex();
  auto const  sizeX     = size[0];
  auto const  sizeY     = size[1];
  auto const  startX    = index[0];
  auto const  startY    = index[1];
  itk::IndexValueType const  endX      = startX + sizeX;
  itk::IndexValueType const  endY      = startY + sizeY;
  auto const  thrX1     = std::min<itk::IndexValueType>(endX, m_thresholdX);
  auto const  thrX2     = std::min<itk::IndexValueType>(endX, imgEndX - m_thresholdX);
  auto const  thrY1     = std::min<itk::IndexValueType>(endY, m_thresholdYtop);
  auto const  thrY2     = std::min<itk::IndexValueType>(endY, imgEndY - m_thresholdYbot);

  assert(thrX1 <= endX && "Iterations shall stay within requested region");
  assert(thrX2 <= endX && "Iterations shall stay within requested region");
  assert(thrY1 <= endY && "Iterations shall stay within requested region");
  assert(thrY2 <= endY && "Iterations shall stay within requested region");

  // using interval_t = boost::numeric::interval<long>;
  using interval_t = Interval;
  auto const region      = interval_t{startX, endX};
  auto const zero_left   = intersect(interval_t{startX, thrX1}, region);
  auto const copy_middle = intersect(interval_t{thrX1, thrX2},  region);
  auto const zero_right  = intersect(interval_t{thrX2, endX},   region);
  otbMsgDevMacro("X in " << zero_left   << " <<-- 0");
  otbMsgDevMacro("X in " << copy_middle << " <<-- copy input");
  otbMsgDevMacro("X in " << zero_right  << " <<-- 0");
  otbMsgDevMacro("Y in ["<<startY<<".."<<thrY1<<"[  <<--- 0");

  auto const nb_z_l = zero_left.upper()   - zero_left.lower();
  auto const nb_c_m = copy_middle.upper() - copy_middle.lower();
  auto const nb_z_r = zero_right.upper()  - zero_right.lower();
  assert(nb_z_l + nb_c_m + nb_z_r == sizeX);

  itk::ProgressReporter progress( this, threadId, outputRegionForThread.GetNumberOfPixels() / sizeY );
  outputIterator.GoToBegin();

  // TODO: Can we consider that lines are contiguous in memory?
  // If so, we could have only 2 fill_n and one copy_n!
  auto y = startY;
  for (
      ; y < thrY1
      ; ++y, outputIterator.NextLine())
  {
    // If there is any trimming of first lines, the inputIterator iterator will
    // directly point to the right region. we shall not increment it!
    // otbMsgDevMacro("o(" << y << ") <-- 0");
    assert(! outputIterator.IsAtEnd());
    outputIterator.GoToBeginOfLine();
    std::fill_n(&outputIterator.Value(), sizeX, OutputPixelType{});
    progress.CompletedPixel(); // Completed...Line()
  }
  assert(y == thrY1 || y == startY);
  otbMsgDevMacro("Y in ["<<thrY1<<".."<<thrY2<<"[  <<--- Input");
  inputIterator.GoToBegin();
  for (
      ; y < thrY2
      ; ++y, inputIterator.NextLine(), outputIterator.NextLine())
  {
    // otbMsgDevMacro("o(" << y << ") <-- Input");
    assert(! inputIterator.IsAtEnd());
    assert(! outputIterator.IsAtEnd());
    inputIterator.GoToBeginOfLine();
    outputIterator.GoToBeginOfLine();
    auto const t1 = std::fill_n(&outputIterator.Value(), nb_z_l, OutputPixelType{});
    // If there is any trimming of first columns, the inputIterator iterator
    // will directly point to the right region. we shall not apply an offset!
    auto const t2 = std::copy_n(&inputIterator.Value(), nb_c_m, t1);
    std::fill_n(t2, nb_z_r, OutputPixelType{});
    progress.CompletedPixel(); // Completed...Line()
  }
  assert(y == thrY2 || y == startY);
  otbMsgDevMacro("Y in ["<<thrY2<<".."<<endY<<"[  <<--- 0");
  for (
      ; y < endY
      ; ++y, outputIterator.NextLine())
  {
    // If there is any trimming of last lines, the inputIterator iterator will
    // directly point to the right region. we shall not increment it!
    // otbMsgDevMacro("o(" << y << ") <-- 0");
    assert(! outputIterator.IsAtEnd());
    outputIterator.GoToBeginOfLine();
    std::fill_n(&outputIterator.Value(), sizeX, OutputPixelType{});
    progress.CompletedPixel(); // Completed...Line()
  }
  assert(y == endY);
  otbMsgDevMacro("ThreadedGenerateData end");
}

template<typename TImage>
typename ResetMarginFilter<TImage>::InputImageRegionType
ResetMarginFilter<TImage>
::OutputRegionToInputRegion(OutputImageRegionType const& srcRegion)
{
  auto const* output = this->GetOutput();
  assert(output);

  auto const& maxRegion = output->GetLargestPossibleRegion();
  auto const& maxSize   = maxRegion.GetSize();
  auto const& maxStart  = maxRegion.GetIndex();

  auto const& reqRegion = srcRegion;
  auto const& reqSize   = reqRegion.GetSize();
  auto const& reqStart  = reqRegion.GetIndex();

  // using interval_t = boost::numeric::interval<long>;
  using interval_t = Interval;
  auto const maxRegionX = interval_t{
    maxStart[0]+m_thresholdX,
    static_cast<itk::IndexValueType>(maxStart[0]+maxSize[0]-m_thresholdX)
  };
  auto const maxRegionY = interval_t{
    maxStart[1]+m_thresholdYtop,
    static_cast<itk::IndexValueType>(maxStart[1]+maxSize[1]-m_thresholdYbot)
  };

  auto const reqRegionX = interval_t::OfLength(reqStart[0], reqSize[0]);
  auto const reqRegionY = interval_t::OfLength(reqStart[1], reqSize[1]);
#if 0
  otbMsgDevMacro("OutputRegionToInputRegion: "
      << "out="<< NeatRegionLogger{reqRegion}
      << ";    max: x="<<maxRegionX << "  y="<<maxRegionY
      << ";    req: x="<<reqRegionX << "  y="<<reqRegionY
      );
#endif

  auto const inRegionX = intersect(reqRegionX, maxRegionX);
  auto const inRegionY = intersect(reqRegionY, maxRegionY);
  // otbMsgDevMacro(" --> ∩X: " << inRegionX << " ∩Y: " << inRegionY);

  const InputIndexType inStart{inRegionX.lower(), inRegionY.lower()};
  assert(inRegionX.lower() <= inRegionX.upper());
  assert(inRegionY.lower() <= inRegionY.upper());
  const InputSizeType inSize{
    static_cast<unsigned long>(inRegionX.upper()-inRegionX.lower()),
    static_cast<unsigned long>(inRegionY.upper()-inRegionY.lower())
  };
  auto const inRegion = InputImageRegionType{inStart, inSize};
  otbMsgDevMacro("OutputRegionToInputRegion: out="<< NeatRegionLogger{reqRegion}<<"   --> in="<<NeatRegionLogger{inRegion});
  return inRegion;
}

} // otb namespace

#endif  // otbResetMarginFilter_hxx