/*
 Copyright 2012 Google Inc. All Rights Reserved.
 Author: blakely@google.com (Tim Blakely)

 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.
 You may obtain a copy of the License at

 http://www.apache.org/licenses/LICENSE-2.0

 Unless required by applicable law or agreed to in writing, software
 distributed under the License is distributed on an "AS IS" BASIS,
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.
 */

#include "topology/connector/gaussian_box.h"

#include <boost/random/taus88.hpp>
#include <stdlib.h>
#include <iostream>

#include "boost/random/uniform_01.hpp"
#include "network/objects/neuron.h"
#include "protos/basics.pb.h"
#include "protos/neuron.pb.h"
#include "protos/topology.pb.h"

namespace google {
namespace protobuf {
class Message;
}  // namespace protobuf
}  // namespace google

namespace bigbrain {

using std::vector;
using boost::random::uniform_01;

GaussianBoxConnector::GaussianBoxConnector() {
}

GaussianBoxConnector::~GaussianBoxConnector() {
}

void GaussianBoxConnector::MakeConnections(Neuron* source,
                                           vector<Neuron*>& potential_targets) {
  const Position& source_pos = source->get_common_params()->pos();

  double size_x = params_.width() * 2 + 1;
  double size_x2 = size_x / 2;
  double size_y = params_.height() * 2 + 1;
  double size_y2 = size_y / 2;
  double size_z = params_.depth() * 2 + 1;
  double size_z2 = size_y / 2;
  double scale_x = 2 * 3.14159265358 / size_x;
  double scale_y = 2 * 3.14159265358 / size_y;
  double scale_z = 2 * 3.14159265358 / size_z;

  uniform_01<> random;

  double origin[3];

  if (params_.use_origin()) {
    origin[0] = params_.origin().x();
    origin[1] = params_.origin().y();
    origin[2] = params_.origin().z();
  } else {
    origin[0] = source_pos.x();
    origin[1] = source_pos.y();
    origin[2] = source_pos.z();
  }

  // This can be GREATLY sped up with some form of spatial culling
  for (auto target : potential_targets) {
    // Make sure we don't connect to ourself if we don't allow it
    if (source->get_common_params()->uuid()
        == target->get_common_params()->uuid() && !allow_self_connections_) {
      continue;
    }

    const Position& target_pos = target->get_common_params()->pos();
    if (abs(target_pos.x() - origin[0]) <= params_.width()
        && abs(target_pos.y() - origin[1]) <= params_.height()
        && abs(target_pos.z() - origin[2]) <= params_.depth()) {
      // Since we're close enough, let's see if we can connect
      double relative[3];
      relative[0] = (target_pos.x() - origin[0]) * scale_x;
      relative[1] = (target_pos.y() - origin[1]) * scale_y;
      relative[2] = (target_pos.z() - origin[2]) * scale_z;

      double len = sqrt(
          pow(relative[0] * relative[0], 2) + pow(relative[1] * relative[1], 2)
              + pow(relative[2] * relative[2], 2));

      double gauss_val = params_.amplitude() / pow(2 * 3.14159265358, len / 2)
          * exp(
              -1 / 2.75 * relative[0] * relative[0] + relative[1] * relative[1]
                  + relative[2] * relative[2]);

      // The following is an implementation that Synergy used, and is kept here
      // for reference
      /*

       double gauss_val = params_.amplitude() * exp(-((
       pow(relative[0] * scale_x,2) + pow(relative[1] * scale_y,2))
       /params_.std()));

       */

      double randval = random(random_);
      if (fabs(gauss_val) > randval) {
        AddLink(source, target);
      }
    }
  }
}

google::protobuf::Message* GaussianBoxConnector::get_params() {
  return &params_;
}

}  // namespace bigbrain


#include <iostream>
#include <fstream>
#include <vector>
#include <ctime>
#include <stdlib.h>
#include <time.h>
#include <algorithm>
#include <deque>
#include <unordered_set>
#include <math.h>
//PCL
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include "patchwork.hpp"
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>//自定义点云类型时要加
#include <pcl/filters/conditional_removal.h>
//Eigen
#include <Eigen/Dense>
#include <queue>

#include <unordered_map>

#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>

#include "CVC_cluster.h"

using namespace std;

struct PointXYZILID
{
  PCL_ADD_POINT4D;                    // quad-word XYZ
  float    intensity;                 ///< laser intensity reading
  uint16_t label;                     ///< point label
  uint16_t id;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW     // ensure proper alignment
} EIGEN_ALIGN16;

// Register custom point struct according to PCL
POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, intensity, intensity)
                                  (uint16_t, label, label)
                                  (uint16_t, id, id))

struct PointXYZIR{
	PCL_ADD_POINT4D;
	float intensity;
	uint16_t ring;
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR,
		(float,x,x)
		(float,y,y)
		(float,z,z)
		(float,intensity,intensity)
		(uint16_t,ring,ring)
)

void PointXYZILID2XYZI(pcl::PointCloud<PointXYZILID>& src,
                       pcl::PointCloud<pcl::PointXYZI>::Ptr dst){
  dst->points.clear();
  for (const auto &pt: src.points){
    pcl::PointXYZI pt_xyzi;
    pt_xyzi.x = pt.x;
    pt_xyzi.y = pt.y;
    pt_xyzi.z = pt.z;
    pt_xyzi.intensity = pt.intensity;
    dst->points.push_back(pt_xyzi);
  }
}

using PointType = PointXYZILID;
boost::shared_ptr<PatchWork<PointType> > PatchworkGroundSeg;

int main(int argc, char**argv) {

       pcl::PointCloud<PointXYZIR>::Ptr cloud(new pcl::PointCloud<PointXYZIR>);//初始化点云
       pcl::io::loadPCDFile<PointXYZIR>(argv[1], *cloud);//加载pcd点云并放入cloud中

       PatchworkGroundSeg.reset(new PatchWork<PointXYZILID>());
       pcl::PointCloud<PointType>::Ptr pc_curr(new pcl::PointCloud<PointType>);
       pcl::PointCloud<PointType>::Ptr pc_ground(new pcl::PointCloud<PointType>);
       pcl::PointCloud<PointType>::Ptr pc_non_ground(new pcl::PointCloud<PointType>);
       double time_taken;

	for(int i=0; i<cloud->points.size(); ++i){
		PointType p;
		p.x = cloud->points[i].x;
		p.y = cloud->points[i].y;
		p.z = cloud->points[i].z;
		p.intensity = cloud->points[i].intensity;
		pc_curr->points.push_back(p);
	}

        PatchworkGroundSeg->estimate_ground(*pc_curr, *pc_ground, *pc_non_ground, time_taken);

	cout<<time_taken<<endl;

	vector<float> param(3,0);
	param[0] = 2;
	param[1] = 0.4;
	param[2] = 1.5;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_point(new pcl::PointCloud<pcl::PointXYZ>);

	for(int i=0; i<pc_non_ground->points.size(); ++i){
		pcl::PointXYZ p;
		p.x = pc_non_ground->points[i].x;
		p.y = pc_non_ground->points[i].y;
		p.z = pc_non_ground->points[i].z;
		cluster_point->points.push_back(p);
	}

	cout<<"cluster_point "<<cluster_point->points.size()<<endl;
	CVC Cluster(param);
	std::vector<PointAPR> capr;
	Cluster.calculateAPR(*cluster_point, capr);

	cout<<"capr "<<capr.size()<<endl;
	std::unordered_map<int, Voxel> hash_table;
	Cluster.build_hash_table(capr, hash_table);
	vector<int> cluster_indices;
	cluster_indices = Cluster.cluster(hash_table, capr);
	vector<int> cluster_id;
	Cluster.most_frequent_value(cluster_indices, cluster_id);

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
			new pcl::visualization::PCLVisualizer("pcd")); //PCLVisualizer 可视化类
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addCoordinateSystem(1);

	cv::RNG rng(12345);

        int r = rng.uniform(0, 200);
        int g = rng.uniform(0, 200);
        int b = rng.uniform(0, 200);
	pcl::visualization::PointCloudColorHandlerCustom <PointType> colorob(pc_ground, (r), (g),(b));
	string csob = "cloudfinalob";
	viewer->addPointCloud(pc_ground,colorob, csob);

	//pcl::visualization::PointCloudColorHandlerCustom <PointType> colorob2(pc_non_ground, (0), (255),(0));
	//string csob2 = "cloudfinalob1";
	//viewer->addPointCloud(pc_non_ground,colorob2, csob2);

	for(int j = 0; j < cluster_id.size(); ++j){
                int r = rng.uniform(20, 255);
                int g = rng.uniform(20, 255);
                int b = rng.uniform(20, 255);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudcluster(new pcl::PointCloud<pcl::PointXYZ>);//初始化
		for(int i =0; i<cluster_indices.size(); ++i){
			if(cluster_indices[i] == cluster_id[j]){
				 cloudcluster->points.push_back(cluster_point->points[i]);
			}
		}

		pcl::visualization::PointCloudColorHandlerCustom <pcl::PointXYZ> color(cloudcluster, (r), (g),(b));
		string text = "cloud" + toString(j);
		viewer->addPointCloud(cloudcluster,color, text);
	}

	while (!viewer->wasStopped()) {
		viewer->spin();
	}

    return 0;
}


// Copyright 2014-2015 SDL plc
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//     http://www.apache.org/licenses/LICENSE-2.0
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/** \file

    randomly sample a high-probability path.
*/

#ifndef HYP__HYPERGRAPH_SAMPLE_PATH_HPP
#define HYP__HYPERGRAPH_SAMPLE_PATH_HPP
#pragma once

#include <sdl/Hypergraph/IHypergraph.hpp>
#include <sdl/Hypergraph/InsideAlgorithm.hpp>
#include <sdl/Hypergraph/MutableHypergraph.hpp>
#include <sdl/Hypergraph/Weight.hpp>
#include <sdl/Util/LogHelper.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <cstdlib>
#include <ctime>
#include <queue>

namespace sdl {
namespace Hypergraph {

/**
   Samples one of the incoming arcs that point into the
   specified state.
*/

template <typename Arc>
class Sampler {
 public:
  Sampler(unsigned long seed = time(0)) { srand(seed); }

  virtual ~Sampler() {}

  virtual void initialize(IHypergraph<Arc> const& hg) {}

  virtual Arc* sampleArc(IHypergraph<Arc> const& hg, StateId s) const = 0;
};

template <typename Arc>
class UniformInArcSampler : public Sampler<Arc> {
 public:
  typedef typename Arc::Weight Weight;
  UniformInArcSampler(unsigned long seed = time(0)) : Sampler<Arc>(seed) {}

  virtual Arc* sampleArc(IHypergraph<Arc> const& hg, StateId s) const {
    std::size_t numArcs = hg.numInArcs(s);
    if (numArcs == 0) {
      return NULL;
    }
    double r = rand() / (RAND_MAX + 1.0);
    ArcId arcid = static_cast<ArcId>(r * numArcs);
    return hg.inArc(s, arcid);
  }
};

template <typename Arc>
class ProbabilityArcSampler : public Sampler<Arc> {
 public:
  typedef typename Arc::Weight Weight;
  ProbabilityArcSampler(unsigned long seed = time(0)) : Sampler<Arc>(seed) {}

  virtual void initialize(IHypergraph<Arc> const& hg) { insideAlgorithm(hg, &insideDistances_); }
  virtual Arc* sampleArc(IHypergraph<Arc> const& hg, StateId s) const {
    std::size_t numArcs = hg.numInArcs(s);
    if (numArcs == 0) {
      return NULL;
    }
    if (numArcs == 1) {
      return hg.inArc(s, 0);
    }

    Weight totalWeight = Weight::zero();
    std::vector<Weight> weights;
    std::vector<Arc*> arcs;
    for (ArcId arcid : hg.inArcIds(s)) {
      Arc* arc = hg.inArc(s, arcid);
      Weight w = arc->weight();
      for (StateId tailId : arc->tails()) {
        if (!hg.hasLexicalLabel(tailId)) {
          timesBy(insideDistances_[tailId], w);
        }
      }
      plusBy(w, totalWeight);
      arcs.push_back(arc);
      weights.push_back(w);
    }

    double random = rand() % RAND_MAX / static_cast<double>(RAND_MAX) * exp(-totalWeight.getValue());
    int i = 0;
    Weight weightSum = weights[0];
    while (exp(-weightSum.getValue()) < random) {
      weightSum = Hypergraph::plus(weightSum, weights[++i]);
    }
    return arcs[i];
  }

 private:
  boost::ptr_vector<Weight> insideDistances_;
};

// TODO: Add other sample functors: (1) Local: Sample in proportion to
// the weights of the incoming arcs, (2) Global: Sample in proportion
// to the forward (i.e., inside) weights

/**
   Samples a path from hg, starting from the final state.
*/
template <class Arc>
void samplePath(IHypergraph<Arc> const& hg, Sampler<Arc>& sampler, IMutableHypergraph<Arc>* result) {
  if (!(hg.properties() & kStoreInArcs)) {
    SDL_THROW_LOG(Hypergraph, InvalidInputException, "HG must store incoming arcs");
  }
  result->setVocabulary(hg.getVocabulary());
  sampler.initialize(hg);
  std::queue<StateId> queue;
  std::set<StateId> onQueue;
  StateId finalStateId = hg.final();
  queue.push(finalStateId);
  onQueue.insert(finalStateId);
  result->addStateId(finalStateId, hg.inputLabel(finalStateId), hg.outputLabel(finalStateId));

  while (!queue.empty()) {
    StateId hgStateId = queue.front();
    queue.pop();
    if (hg.start() == hgStateId) {
      result->setStart(hgStateId);
    }

    if (Arc* arc = sampler.sampleArc(hg, hgStateId)) {
      for (StateId s : arc->tails()) {
        if (onQueue.insert(s).second) {
          result->addStateId(s, hg.labelPair(s));
          queue.push(s);
        }
      }
      result->addArc(new Arc(*arc));
    }
  }
  result->setFinal(finalStateId);
  SDL_DEBUG(Hypergraph.samplePath, "Result: " << *result);
}

}}

#endif


//==================================================================================================
/**
  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_DETAIL_BITSET_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_BITSET_HPP_INCLUDED

#include <boost/simd/detail/dispatch/meta/make_integer.hpp>
#include <boost/simd/function/scalar/popcnt.hpp>
#include <type_traits>
#include <cstddef>
#include <cstdint>
#include <bitset>

namespace boost { namespace simd
{
  namespace detail
  {
    // Roudn to newt power of 2
    template<std::size_t N> struct nextpow2
    {
      static constexpr std::size_t v0 = N-1;
      static constexpr std::size_t v1 = v0 | v0 >> 1;
      static constexpr std::size_t v2 = v1 | v1 >> 2;
      static constexpr std::size_t v3 = v2 | v2 >> 4;
      static constexpr std::size_t v4 = v3 | v3 >> 8;
      static constexpr std::size_t v5 = v4 | v4 >> 16;
      static constexpr std::size_t value = v0 ? v5+1 : 1;
    };

    // Compute how many bytes is N bits
    template<std::size_t N> struct bitset_size
    {
      static constexpr std::size_t base = (N < 8) ? 1 : (N/8 + (N%8 ? 1 : 0));
      static constexpr std::size_t value = nextpow2<base>::value;
    };

    // Small N optimized bitset implementation
    template<std::size_t N> struct bitset
    {
      static constexpr std::size_t bitmask = (N == 64) ? ~0ULL : (1ULL<<N) - 1;
      using bits_type = bd::make_integer_t<bitset_size<N>::value,unsigned>;

      constexpr bitset()                BOOST_NOEXCEPT : bits_{} {}
      constexpr bitset(bitset const& v) BOOST_NOEXCEPT : bits_{v.bits_} {}
      constexpr bitset(std::size_t n)   BOOST_NOEXCEPT : bits_{static_cast<bits_type>(n & bitmask)} {}

      bitset& reset()               BOOST_NOEXCEPT { bits_ = 0U;                    return *this; }
      bitset& reset(std::size_t p)  BOOST_NOEXCEPT { bits_ = bits_ & ~((1ULL<<p));  return *this; }

      bitset& set() BOOST_NOEXCEPT { bits_ = ~0ULL; return *this; }
      bitset& set(std::size_t p, bool v = true) BOOST_NOEXCEPT
      {
        reset(p);
        bits_ |= (std::size_t(v)<<p);
        return *this;
      }

      bitset& flip()                { bits_ = ~bits_;     return *this; }
      bitset& flip(std::size_t p)   { bits_ ^= (1ULL<<p); return *this; }

      constexpr bool operator[]( std::size_t p ) const BOOST_NOEXCEPT { return bits_ & (1ULL<<p); }

      bool test( std::size_t p ) const
      {
        if(p>N) throw std::out_of_range{"Bit index is out of range"};
        return this->operator[](p);
      }

      static constexpr std::size_t size() BOOST_NOEXCEPT { return N; }

                std::size_t count() const BOOST_NOEXCEPT { return boost::simd::popcnt(bits_); }
      constexpr bool        all()   const BOOST_NOEXCEPT { return bits_ == bitmask;           }
      constexpr bool        any()   const BOOST_NOEXCEPT { return bits_ != 0;                 }
      constexpr bool        none()  const BOOST_NOEXCEPT { return bits_ == 0;                 }

      constexpr bool operator==(bitset const& other) const BOOST_NOEXCEPT { return bits_ == other.bits_; }
      constexpr bool operator!=(bitset const& other) const BOOST_NOEXCEPT { return bits_ != other.bits_; }

      bitset  operator>> (std::size_t pos) const { return bitset{bits_ >> pos}; }
      bitset  operator<< (std::size_t pos) const { return bitset{bits_ << pos}; }

      bitset& operator<<=(std::size_t pos) { bits_ <<= pos; return *this; }
      bitset& operator>>=(std::size_t pos) { bits_ >>= pos; return *this; }

      bitset& operator&=( const bitset<N>& other ) BOOST_NOEXCEPT { bits_ &= other.bits_; return *this; }
      bitset& operator|=( const bitset<N>& other ) BOOST_NOEXCEPT { bits_ |= other.bits_; return *this; }
      bitset& operator^=( const bitset<N>& other ) BOOST_NOEXCEPT { bits_ ^= other.bits_; return *this; }

      bitset  operator~() const BOOST_NOEXCEPT { return bitset( ~bits_ ); }

      operator std::bitset<N>() BOOST_NOEXCEPT { return std::bitset<N>{bits_}; }

      constexpr unsigned long      to_ulong()  const BOOST_NOEXCEPT { return bits_; }
      constexpr unsigned long long to_ullong() const BOOST_NOEXCEPT { return bits_; }

      private:
      bits_type bits_;
    };

    template<std::size_t N>
    std::ostream& operator<<( std::ostream& os, bitset<N> const& b)
    {
      os << b.to_ullong();
      return os;
    }

    template<std::size_t N>
    struct select_bitset : std::conditional<(N>64),std::bitset<N>,bitset<N>>
    {};
  }

  // Optimize small bitset if N <= 64bits - else use std::bitset
  template<std::size_t N> using bitset = typename detail::select_bitset<N>::type;
} }

#endif


#ifndef  EVALUATE_HPP_
#define  EVALUATE_HPP_

#include <cmath>
#include <cstdint>
#include <cstdio>
#include <fstream>
#include <iostream>
#include <limits>
#include <sstream>
#include <vector>
#include <map>
#include <exception>

#include <Eigen/Dense>

#include "yaml-cpp/yaml.h"

#include "io.hpp"
#include "types.hpp"

namespace ct_icp {

    struct errors {
        double t_err;
        double r_err;

        errors(double t_err, double r_err) :
                t_err(t_err), r_err(r_err) {}
    };

    struct seq_errors {
        std::vector<errors> tab_errors;
        double mean_rpe;
        double mean_ape;
        double max_ape;
        double mean_local_err;
        double max_local_err;
        double average_elapsed_ms = -1.0;
        int index_max_local_err;
        double mean_num_attempts;
    };

    inline double translationError(const Eigen::Matrix4d &pose_error) {
        return pose_error.block<3, 1>(0, 3).norm();
    }

    inline double rotationError(Eigen::Matrix4d &pose_error) {
        double a = pose_error(0, 0);
        double b = pose_error(1, 1);
        double c = pose_error(2, 2);
        double d = 0.5 * (a + b + c - 1.0);
        return acos(std::max(std::min(d, 1.0), -1.0));
    }

    inline std::vector<double> trajectoryDistances(const ArrayPoses &poses) {
        std::vector<double> dist(1, 0.0);
        for (size_t i = 1; i < poses.size(); i++)
            dist.push_back(dist[i - 1] + translationError(poses[i - 1] - poses[i]));
        return dist;
    }

    inline int lastFrameFromSegmentLength(const std::vector<double> &dist, int first_frame, double len) {
        for (int i = first_frame; i < dist.size(); i++)
            if (dist[i] > dist[first_frame] + len)
                return i;
        return -1;
    }

    double computeMeanRPE(const ArrayPoses &poses_gt, const ArrayPoses &poses_result,
                          seq_errors &seq_err);

    seq_errors eval(const ArrayPoses &poses_gt, const ArrayPoses &poses_estimated);

    inline void eval(const std::string &filename_poses_gt, const std::string &filename_poses_result,
                     std::list<seq_errors> &all_seq_errors) {

        ArrayPoses poses_gt = LoadPoses(filename_poses_gt);
        ArrayPoses poses_result = LoadPoses(filename_poses_result);

        auto seq_err = eval(poses_gt, poses_result);
        // Add to errors of all sequences
        all_seq_errors.push_back(seq_err);

    }

    // Saves Metrics to File
    void SaveMetrics(const std::map<std::string, seq_errors> &metrics,
                     const std::string &destination, bool success = true);
}

#endif


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#include <algorithm> // max, max_element
#include <cassert>
#include <unordered_map>
#include <utility>   // pair
#include <tuple>

#include <boost/range/algorithm/for_each.hpp>

#include "mc/classic/count_tokens.hh"

namespace pnmc { namespace mc { namespace classic {

/*------------------------------------------------------------------------------------------------*/

namespace /* anonymous */{

/// @internal
struct count_tokens_visitor
{
  using result_type = std::pair<unsigned long, unsigned long>;

  /// @brief A cache is necessary to know if a node has already been encountered.
  ///
  /// We use the addresses of nodes as key. It's legit because nodes are unified and immutable.
  mutable std::unordered_map<const char*, result_type> cache_;

  /// @brief |0|.
  result_type
  operator()(const sdd::zero_terminal<sdd::conf1>&)
  const
  {
    assert(false && "|0|");
    __builtin_unreachable();
  }

  /// @brief |1|.
  result_type
  operator()(const sdd::one_terminal<sdd::conf1>&)
  const
  {
    return {0ul, 0ul};
  }

  /// @brief Flat SDD.
  result_type
  operator()(const sdd::flat_node<sdd::conf1>& n)
  const
  {
    auto insertion = cache_.emplace(reinterpret_cast<const char*>(&n), std::make_pair(0, 0));
    if (insertion.second)
    {
      auto markings = 0ul;
      auto places = 0ul;

      for (const auto& arc : n)
      {
        const auto max_value = *std::max_element(arc.valuation().cbegin(), arc.valuation().cend());
        const auto res = visit(*this, arc.successor());

        markings = std::max(markings, max_value + res.first);
        places = std::max({places, static_cast<unsigned long>(max_value), res.second});
      }

      insertion.first->second = std::make_pair(markings, places);
    }
    return insertion.first->second;
  }

  /// @brief Hierarchical SDD.
  result_type
  operator()(const sdd::hierarchical_node<sdd::conf1>& n)
  const
  {
    auto insertion = cache_.emplace(reinterpret_cast<const char*>(&n), std::make_pair(0, 0));
    if (insertion.second)
    {
      auto markings = 0ul;
      auto places = 0ul;

      for (const auto& arc : n)
      {
        const auto val_res = visit(*this, arc.valuation());
        const auto succ_res = visit(*this, arc.successor());

        markings = std::max(markings, val_res.first + succ_res.first);
        places = std::max({places, val_res.second, succ_res.second});
      }

      insertion.first->second = std::make_pair(markings, places);
    }
    return insertion.first->second;
  }
}; // struct count_tokens_visitor

} // namespace anonymous

/*------------------------------------------------------------------------------------------------*/

void
count_tokens(results& res, const SDD& states, const pn::net& net)
{
  res.max_token_markings = 0;
  res.max_token_places = 0;

  std::tie(*res.max_token_markings, *res.max_token_places) = visit(count_tokens_visitor(), states);

  // Add markings of (useless) places that are connected.
  boost::range::for_each(net.places(), [&](const auto& place)
                                          {
                                            if (not place.connected())
                                            {
                                              *res.max_token_markings += place.marking;
                                            }
                                          });
}

/*------------------------------------------------------------------------------------------------*/

}}} // namespace pnmc::mc::classic


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/perspective_three_point.h"

#include <Eigen/Dense>
#include <algorithm>
#include <complex>
#include <glog/logging.h>
#include <math.h>

#include "theia/math/polynomial.h"
#include "theia/sfm/pose/util.h"

namespace theia {
using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

namespace {

// Solves for cos(theta) that will describe the rotation of the plane from
// intermediate world frame to intermediate camera frame. The method returns the
// roots of a quartic (i.e. solutions to cos(alpha) ) and several factors that
// are needed for back-substitution.
int SolvePlaneRotation(const Vector3d normalized_image_points[3],
                       const Vector3d& intermediate_image_point,
                       const Vector3d& intermediate_world_point,
                       const double d_12,
                       double cos_theta[4],
                       double cot_alphas[4],
                       double* b) {
  // Calculate these parameters ahead of time for reuse and
  // readability. Notation for these variables is consistent with the notation
  // from the paper.
  const double f_1 = intermediate_image_point[0] / intermediate_image_point[2];
  const double f_2 = intermediate_image_point[1] / intermediate_image_point[2];
  const double p_1 = intermediate_world_point[0];
  const double p_2 = intermediate_world_point[1];
  const double cos_beta =
      normalized_image_points[0].dot(normalized_image_points[1]);
  *b = 1.0 / (1.0 - cos_beta * cos_beta) - 1.0;

  if (cos_beta < 0) {
    *b = -sqrt(*b);
  } else {
    *b = sqrt(*b);
  }

  // Definition of temporary variables for readability in the coefficients
  // calculation.
  const double f_1_pw2 = f_1 * f_1;
  const double f_2_pw2 = f_2 * f_2;
  const double p_1_pw2 = p_1 * p_1;
  const double p_1_pw3 = p_1_pw2 * p_1;
  const double p_1_pw4 = p_1_pw3 * p_1;
  const double p_2_pw2 = p_2 * p_2;
  const double p_2_pw3 = p_2_pw2 * p_2;
  const double p_2_pw4 = p_2_pw3 * p_2;
  const double d_12_pw2 = d_12 * d_12;
  const double b_pw2 = (*b) * (*b);

  // Computation of coefficients of 4th degree polynomial.
  Eigen::VectorXd coefficients(5);
  coefficients(0) = -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4;
  coefficients(1) = 2.0 * p_2_pw3 * d_12 * (*b) +
                    2.0 * f_2_pw2 * p_2_pw3 * d_12 * (*b) -
                    2.0 * f_2 * p_2_pw3 * f_1 * d_12;
  coefficients(2) =
      -f_2_pw2 * p_2_pw2 * p_1_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 -
      f_2_pw2 * p_2_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw4 + p_2_pw4 * f_1_pw2 +
      2.0 * p_1 * p_2_pw2 * d_12 +
      2.0 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * (*b) -
      p_2_pw2 * p_1_pw2 * f_1_pw2 + 2.0 * p_1 * p_2_pw2 * f_2_pw2 * d_12 -
      p_2_pw2 * d_12_pw2 * b_pw2 - 2.0 * p_1_pw2 * p_2_pw2;
  coefficients(3) =
      2.0 * p_1_pw2 * p_2 * d_12 * (*b) + 2.0 * f_2 * p_2_pw3 * f_1 * d_12 -
      2.0 * f_2_pw2 * p_2_pw3 * d_12 * (*b) - 2.0 * p_1 * p_2 * d_12_pw2 * (*b);
  coefficients(4) = -2 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * (*b) +
                    f_2_pw2 * p_2_pw2 * d_12_pw2 + 2.0 * p_1_pw3 * d_12 -
                    p_1_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw2 * p_1_pw2 - p_1_pw4 -
                    2.0 * f_2_pw2 * p_2_pw2 * p_1 * d_12 +
                    p_2_pw2 * f_1_pw2 * p_1_pw2 +
                    f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2;

  // Computation of roots.
  Eigen::VectorXd roots;
  FindPolynomialRoots(coefficients, &roots, NULL);

  // Calculate cot(alpha) needed for back-substitution.
  for (int i = 0; i < roots.size(); i++) {
    cos_theta[i] = roots(i);
    cot_alphas[i] = (-f_1 * p_1 / f_2 - cos_theta[i] * p_2 + d_12 * (*b)) /
                    (-f_1 * cos_theta[i] * p_2 / f_2 + p_1 - d_12);
  }

  return static_cast<int>(roots.size());
}

// Given the complete transformation between intermediate world and camera
// frames (parameterized by cos_theta and cot_alpha), back-substitute the
// solution and get an absolute camera pose.
void Backsubstitute(const Matrix3d& intermediate_world_frame,
                    const Matrix3d& intermediate_camera_frame,
                    const Vector3d& world_point_0,
                    const double cos_theta,
                    const double cot_alpha,
                    const double d_12,
                    const double b,
                    Vector3d* translation,
                    Matrix3d* rotation) {
  const double sin_theta = sqrt(1.0 - cos_theta * cos_theta);
  const double sin_alpha = sqrt(1.0 / (cot_alpha * cot_alpha + 1.0));
  double cos_alpha = sqrt(1.0 - sin_alpha * sin_alpha);

  if (cot_alpha < 0) {
    cos_alpha = -cos_alpha;
  }

  // Get the camera position in the intermediate world frame
  // coordinates. (Eq. 5 from the paper).
  const Vector3d c_nu(
      d_12 * cos_alpha * (sin_alpha * b + cos_alpha),
      cos_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha),
      sin_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha));

  // Transform c_nu into world coordinates. Use a Map to put the solution
  // directly into the output.
  *translation = world_point_0 + intermediate_world_frame.transpose() * c_nu;

  // Construct the transformation from the intermediate world frame to the
  // intermediate camera frame.
  Matrix3d intermediate_world_to_camera_rotation;
  intermediate_world_to_camera_rotation << -cos_alpha, -sin_alpha * cos_theta,
      -sin_alpha * sin_theta, sin_alpha, -cos_alpha * cos_theta,
      -cos_alpha * sin_theta, 0, -sin_theta, cos_theta;

  // Construct the rotation matrix.
  *rotation = (intermediate_world_frame.transpose() *
               intermediate_world_to_camera_rotation.transpose() *
               intermediate_camera_frame)
                  .transpose();

  // Adjust translation to account for rotation.
  *translation = -(*rotation) * (*translation);
}

}  // namespace

bool PoseFromThreePoints(const std::vector<Vector2d>& feature_point,
                         const std::vector<Vector3d>& points_3d,
                         std::vector<Matrix3d>* solution_rotations,
                         std::vector<Vector3d>* solution_translations) {
  Vector3d normalized_image_points[3];
  // Store points_3d in world_points for ease of use. NOTE: we cannot use a
  // const ref or a Map because the world_points entries may be swapped later.
  Vector3d world_points[3];
  for (int i = 0; i < 3; ++i) {
    normalized_image_points[i] = feature_point[i].homogeneous().normalized();
    world_points[i] = points_3d[i];
  }

  // If the points are collinear, there are no possible solutions.
  double kTolerance = 1e-6;
  Vector3d world_1_0 = world_points[1] - world_points[0];
  Vector3d world_2_0 = world_points[2] - world_points[0];
  if (world_1_0.cross(world_2_0).squaredNorm() < kTolerance) {
    VLOG(2) << "The 3 world points are collinear! No solution for absolute "
               "pose exits.";
    return false;
  }

  // Create intermediate camera frame such that the x axis is in the direction
  // of one of the normalized image points, and the origin is the same as the
  // absolute camera frame. This is a rotation defined as the transformation:
  // T = [tx, ty, tz] where tx = f0, tz = (f0 x f1) / ||f0 x f1||, and
  // ty = tx x tz and f0, f1, f2 are the normalized image points.
  Matrix3d intermediate_camera_frame;
  intermediate_camera_frame.row(0) = normalized_image_points[0];
  intermediate_camera_frame.row(2) =
      normalized_image_points[0].cross(normalized_image_points[1]).normalized();
  intermediate_camera_frame.row(1) =
      intermediate_camera_frame.row(2).cross(intermediate_camera_frame.row(0));

  // Project the third world point into the intermediate camera frame.
  Vector3d intermediate_image_point =
      intermediate_camera_frame * normalized_image_points[2];

  // Enforce that the intermediate_image_point is in front of the intermediate
  // camera frame. If the point is behind the camera frame, recalculate the
  // intermediate camera frame by swapping which feature we align the x axis to.
  if (intermediate_image_point[2] > 0) {
    std::swap(normalized_image_points[0], normalized_image_points[1]);

    intermediate_camera_frame.row(0) = normalized_image_points[0];
    intermediate_camera_frame.row(2) = normalized_image_points[0]
                                           .cross(normalized_image_points[1])
                                           .normalized();
    intermediate_camera_frame.row(1) = intermediate_camera_frame.row(2).cross(
        intermediate_camera_frame.row(0));

    intermediate_image_point =
        intermediate_camera_frame * normalized_image_points[2];

    std::swap(world_points[0], world_points[1]);
    world_1_0 = world_points[1] - world_points[0];
    world_2_0 = world_points[2] - world_points[0];
  }

  // Create the intermediate world frame transformation that has the
  // origin at world_points[0] and the x-axis in the direction of
  // world_points[1]. This is defined by the transformation: N = [nx, ny, nz]
  // where nx = (p1 - p0) / ||p1 - p0||
  // nz = nx x (p2 - p0) / || nx x (p2 -p0) || and ny = nz x nx
  // Where p0, p1, p2 are the world points.
  Matrix3d intermediate_world_frame;
  intermediate_world_frame.row(0) = world_1_0.normalized();
  intermediate_world_frame.row(2) =
      intermediate_world_frame.row(0).cross(world_2_0).normalized();
  intermediate_world_frame.row(1) =
      intermediate_world_frame.row(2).cross(intermediate_world_frame.row(0));

  // Transform world_point[2] to the intermediate world frame coordinates.
  Vector3d intermediate_world_point = intermediate_world_frame * world_2_0;

  // Distance from world_points[1] to the intermediate world frame origin.
  double d_12 = world_1_0.norm();

  // Solve for the cos(theta) that will give us the transformation from
  // intermediate world frame to intermediate camera frame. We also get the
  // cot(alpha) for each solution necessary for back-substitution.
  double cos_theta[4];
  double cot_alphas[4];
  double b;
  const int num_solutions = SolvePlaneRotation(normalized_image_points,
                                               intermediate_image_point,
                                               intermediate_world_point,
                                               d_12,
                                               cos_theta,
                                               cot_alphas,
                                               &b);

  // Backsubstitution of each solution
  solution_translations->resize(num_solutions);
  solution_rotations->resize(num_solutions);
  for (int i = 0; i < num_solutions; i++) {
    Backsubstitute(intermediate_world_frame,
                   intermediate_camera_frame,
                   world_points[0],
                   cos_theta[i],
                   cot_alphas[i],
                   d_12,
                   b,
                   &solution_translations->at(i),
                   &solution_rotations->at(i));
  }

  return num_solutions > 0;
}

}  // namespace theia