#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include "right_hand.h"
#include "left_hand.h"
#include<termios.h>

using namespace  std;
using namespace  Eigen;

ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;
ros::Publisher pub29 ;
ros::Publisher pub30 ;
ros::Publisher pub31 ;

int getch()
{
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt;
  newt.c_lflag &= ~(ICANON);                 // disable buffering
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

  int c = getchar();  // read character (non-blocking)

  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
}

void  SendGazebo(vector<double> q){

    std_msgs::Float64 data;
    data.data=q[1];
    pub1.publish(data);
    data.data=q[2];
    pub2.publish(data);
    data.data=q[3];
    pub3.publish(data);
    data.data=q[4];
    pub4.publish(data);
    data.data=q[5];
    pub5.publish(data);
    data.data=q[6];
    pub6.publish(data);
    data.data=q[7];
    pub7.publish(data);
    data.data=q[8];
    pub8.publish(data);
    data.data=q[9];
    pub9.publish(data);
    data.data=q[10];
    pub10.publish(data);
    data.data=q[11];
    pub11.publish(data);
    data.data=q[12];
    pub12.publish(data);
    data.data=q[13];
    pub13.publish(data);
    data.data=q[14];
    pub14.publish(data);
    data.data=q[15];
    pub15.publish(data);
    data.data=q[16];
    pub16.publish(data);
    data.data=q[17];
    pub17.publish(data);
    data.data=q[18];
    pub18.publish(data);
    data.data=q[19];
    pub19.publish(data);
    data.data=q[20];
    pub20.publish(data);
    data.data=q[21];
    pub21.publish(data);
    data.data=q[22];
    pub22.publish(data);
    data.data=q[23];
    pub23.publish(data);
    data.data=q[24];
    pub24.publish(data);
    data.data=q[25];
    pub25.publish(data);
    data.data=q[26];
    pub26.publish(data);
    data.data=q[27];
    pub27.publish(data);
    data.data=q[28];
    pub28.publish(data);
    data.data=q[29];
    pub29.publish(data);
    data.data=q[30];
    pub30.publish(data);
    data.data=q[31];
    pub31.publish(data);

}

int main(int argc, char **argv)
{
//hand_gui my_hand_gui;
//my_hand_gui.show();
    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);

    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",1000);
    pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",1000);
    pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",1000);
    pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",1000);
    pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",1000);
    pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",1000);
    pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",1000);
    pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",1000);
    pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",1000);
    pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",1000);
    pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",1000);
    pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",1000);
    pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",1000);
    pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",1000);
    pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",1000);
    pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",1000);
    pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",1000);
    pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",1000);
    pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",1000);
    pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",1000);
    pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",1000);
    pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",1000);
    pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",1000);
    pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",1000);
    pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",1000);
    pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",1000);
    pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",1000);
    pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",1000);
    pub29 = nh.advertise<std_msgs::Float64>("rrbot/joint29_position_controller/command",1000);
    pub30 = nh.advertise<std_msgs::Float64>("rrbot/joint30_position_controller/command",1000);
    pub31 = nh.advertise<std_msgs::Float64>("rrbot/joint31_position_controller/command",1000);

    right_hand hand_funcs;

    VectorXd r_target(3);
    MatrixXd R_target(3,3);
    VectorXd r_target_l(3);
    MatrixXd R_target_l(3,3);
        r_target<<.4,
                0.05,
                -0.2;
R_target=hand_funcs.rot(2,-120*M_PI/180,3)*hand_funcs.rot(3,-40*M_PI/180,3);

r_target_l<<.4,
        -0.05,
        -0.2;
R_target_l=hand_funcs.rot(2,-120*M_PI/180,3)*hand_funcs.rot(3,40*M_PI/180,3);

VectorXd q0(7);
VectorXd q0_l(7);
q0<<10*M_PI/180,
        -4*M_PI/180,
        0,
        -20*M_PI/180,
        0,
        0,
        0;

q0_l<<10*M_PI/180,
        4*M_PI/180,
        0,
        -20*M_PI/180,
        0,
        0,
        0;

right_hand hand0(q0,r_target,R_target,0,0);
left_hand hand0_l(q0_l,r_target_l,R_target_l,0,0);
double d0=hand0.dist;
double d=d0;
double d_des=hand0.d_des;
double theta=hand0.theta;
double theta_target=hand0.theta_target;
double sai=hand0.sai;
double sai_target=hand0.sai_target;
double phi=hand0.phi;
double phi_target=hand0.phi_target;

double d0_l=hand0.dist;
double d_l=d0;

double theta_l=hand0_l.theta;
double theta_target_l=hand0_l.theta_target;
double sai_l=hand0_l.sai;
double sai_target_l=hand0_l.sai_target;
double phi_l=hand0_l.phi;
double phi_target_l=hand0_l.phi_target;
//ROS_INFO("theta_target=%f,sai_target=%f,phi_target=%f",theta_target,sai_target,phi_target);
ROS_INFO("\nr_target=\n%f\n%f\n%f",r_target(0),r_target(1),r_target(2));
ROS_INFO("\nR_target=\n%f\t%f\t%f\n%f\t%f\t%f\n%f\t%f\t%f\n",R_target(0,0),R_target(0,1),R_target(0,2),R_target(1,0),R_target(1,1),R_target(1,2),R_target(2,0),R_target(2,1),R_target(2,2));
ROS_INFO("press any key to start!");
getch();

QVector<double> qr1;
QVector<double> qr2;
QVector<double> qr3;
QVector<double> qr4;
QVector<double> qr5;
QVector<double> qr6;
QVector<double> qr7;

QVector<double> ql1;
QVector<double> ql2;
QVector<double> ql3;
QVector<double> ql4;
QVector<double> ql5;
QVector<double> ql6;
QVector<double> ql7;

double x_r,y_r,z_r,x_l,y_l,z_l;

int i=0;
int i_l=0;
VectorXd q_ra;
VectorXd q_la;
q_ra=q0;
q_la=q0_l;

qr1.append(q_ra(0));
qr2.append(q_ra(1));
qr3.append(q_ra(2));
qr4.append(q_ra(3));
qr5.append(q_ra(4));
qr6.append(q_ra(5));
qr7.append(q_ra(6));

ql1.append(q_la(0));
ql2.append(q_la(1));
ql3.append(q_la(2));
ql4.append(q_la(3));
ql5.append(q_la(4));
ql6.append(q_la(5));
ql7.append(q_la(6));

vector<double> q_init(32);
for (int i = 0; i < 32; ++i) {
    q_init[i]=0;
}
q_init[15]=qr1[0];
q_init[16]=qr2[0];
q_init[17]=qr3[0];
q_init[18]=qr4[0];
q_init[19]=qr5[0];
q_init[20]=qr6[0];
q_init[21]=qr7[0];

q_init[22]=ql1[0];
q_init[23]=ql2[0];
q_init[24]=ql3[0];
q_init[25]=ql4[0];
q_init[26]=ql5[0];
q_init[27]=ql6[0];
q_init[28]=ql7[0];

SendGazebo(q_init);

int q_motor[16];
for (int var = 0; var < 16 ; ++var) {
    q_motor[var]=0;
}

vector<double> q(32);

ros::Rate loop_rate(200);
int count = 0;

    while (ros::ok())
    {

//        for (int k = 0; k < 7; ++k) {
//            if (k%2==1){r_target=r_shaking;}
//            else{r_target=r_shaking2;}
//right_hand hand00(q_ra,r_target,R_target,i,d0);
//d=hand00.dist;
//        while ((d>d0/2)  || (abs(theta-theta_target)>.02)   || (abs(sai-sai_target)>.02)  || (abs(phi-phi_target)>.02)  ) {
//while (1){
        if ((d>d_des)   || (abs(theta-theta_target)>.02)   || (abs(sai-sai_target)>.02)  || (abs(phi-phi_target)>.02)  )
{

        right_hand hand(q_ra,r_target,R_target,i,d0);

        hand.doQP(q_ra);
        q_ra=hand.q_next;

        qr1.append(q_ra(0));
        qr2.append(q_ra(1));
        qr3.append(q_ra(2));
        qr4.append(q_ra(3));
        qr5.append(q_ra(4));
        qr6.append(q_ra(5));
        qr7.append(q_ra(6));

        d=hand.dist;
        theta=hand.theta;
        sai=hand.sai;
        phi=hand.phi;

        i++;
//        ROS_INFO("q1=%f\tq2=%f\tq3=%f\tq4=%f\tq5=%f\tq6=%f\tq7=%f\t",180/M_PI*q[15],180/M_PI*q[16],180/M_PI*q[17],180/M_PI*q[18],180/M_PI*q[19],180/M_PI*q[20],180/M_PI*q[21]);
//        ROS_INFO("x=%f\ty=%f\tz=%f\t",hand.r_right_palm(0),hand.r_right_palm(1),hand.r_right_palm(2));
x_r=hand.r_right_palm(0);
y_r=hand.r_right_palm(1);
z_r=hand.r_right_palm(2);

        }
        else{
            //ROS_INFO("right reached!");
            qr1.append(qr1[qr1.size()-1]);
            qr2.append(qr2[qr2.size()-1]);
            qr3.append(qr3[qr3.size()-1]);
            qr4.append(qr4[qr4.size()-1]);
            qr5.append(qr5[qr5.size()-1]);
            qr6.append(qr6[qr6.size()-1]);
            qr7.append(qr7[qr7.size()-1]);

        }

    if ((d_l>d_des)   || (abs(theta_l-theta_target_l)>.02)   || (abs(sai_l-sai_target_l)>.02)  || (abs(phi-phi_target_l)>.02)  )
{

    left_hand hand_l(q_la,r_target_l,R_target_l,i_l,d0_l);

    hand_l.doQP(q_la);
    q_la=hand_l.q_next;

    ql1.append(q_la(0));
    ql2.append(q_la(1));
    ql3.append(q_la(2));
    ql4.append(q_la(3));
    ql5.append(q_la(4));
    ql6.append(q_la(5));
    ql7.append(q_la(6));

    d_l=hand_l.dist;
    theta_l=hand_l.theta;
    sai_l=hand_l.sai;
    phi_l=hand_l.phi;

    i_l++;
//    ROS_INFO("q1=%f\tq2=%f\tq3=%f\tq4=%f\tq5=%f\tq6=%f\tq7=%f\t",180/M_PI*q[15],180/M_PI*q[16],180/M_PI*q[17],180/M_PI*q[18],180/M_PI*q[19],180/M_PI*q[20],180/M_PI*q[21]);
//    ROS_INFO("x=%f\ty=%f\tz=%f\t",hand_l.r_left_palm(0),hand_l.r_left_palm(1),hand_l.r_left_palm(2));
    x_l=hand_l.r_left_palm(0);
    y_l=hand_l.r_left_palm(1);
    z_l=hand_l.r_left_palm(2);
    }
    else{

        //ROS_INFO("left reached!");
        ql1.append(ql1[ql1.size()-1]);
        ql2.append(ql2[ql2.size()-1]);
        ql3.append(ql3[ql3.size()-1]);
        ql4.append(ql4[ql4.size()-1]);
        ql5.append(ql5[ql5.size()-1]);
        ql6.append(ql6[ql6.size()-1]);
        ql7.append(ql7[ql7.size()-1]);

    }

        for (int i = 0; i < 32; ++i) {
            q[i]=0;
        }

//         q[15]=-M_PI/2;
//         q[16]=-10*M_PI/180;
//         q[17]=-10*M_PI/180;

        q[15]=qr1[count];
        q[16]=qr2[count];
        q[17]=qr3[count];
        q[18]=qr4[count];

        q[19]=qr5[count];
        q[20]=qr6[count];
        q[21]=qr7[count];

        q[22]=ql1[count];
        q[23]=ql2[count];
        q[24]=ql3[count];
        q[25]=ql4[count];

        q[26]=ql5[count];
        q[27]=ql6[count];
        q[28]=ql7[count];

        q[29]=20*M_PI/180*sin(double(count)*hand0.T*2);
        q[30]=0.0;
        q[31]=15*M_PI/180;

        ROS_INFO("\nqr=%f,%f,%f,%f,%f,%f,%f\nql=%f,%f,%f,%f,%f,%f,%f",
                 q[15]*180/M_PI,q[16]*180/M_PI,q[17]*180/M_PI,q[18]*180/M_PI,q[19]*180/M_PI,
                q[20]*180/M_PI,q[21]*180/M_PI,q[22]*180/M_PI,q[23]*180/M_PI,q[24]*180/M_PI,
                q[25]*180/M_PI,q[26]*180/M_PI,q[27]*180/M_PI,q[28]*180/M_PI);
        ROS_INFO("\nx_r=%f, y_r=%f, z_r=%f\nx_l=%f, y_l=%f, z_l=%f",x_r,y_r,z_r,x_l,y_l,z_l);

    //   ROS_INFO("T=%f upper:%f %f lower:%f %f qdot=%f q=%f %f",hand.T,(hand.toRad(10)+q_ra(1))/hand.T,hand.qdot_max,(hand.toRad(-90)+q_ra(1))/hand.T,-hand.qdot_max,hand.qdot(2),180/M_PI*q[16],180/M_PI*q_ra(1));

       SendGazebo(q);

       msg.data.clear();

      for(int  i = 0;i < 12;i++)
      {
          msg.data.push_back(0);
      }

      q_motor[0]=-int(6*(qr1[count]-q0(0))*360/M_PI);
      q_motor[1]=int(6*(qr2[count]-q0(1))*360/M_PI);
      q_motor[2]=-int(6*(qr3[count]-q0(2))*300/M_PI);
      q_motor[3]=int(6*(qr4[count]-q0(3))*300/M_PI);

      //right hand epose
      msg.data.push_back(q_motor[0]);//12 -y  a,z
      msg.data.push_back(q_motor[1]);//13 +x
      msg.data.push_back(q_motor[2]);//14 -z
      msg.data.push_back(q_motor[3]);//15 +y
      //right hand dynamixel + fingers
      msg.data.push_back(0);//16
      msg.data.push_back(0);//17
      msg.data.push_back(0);//18
      msg.data.push_back(0);//19
      //left hand epose
      msg.data.push_back(q_motor[4]);//20 +y
      msg.data.push_back(q_motor[5]);//21 +x
      msg.data.push_back(q_motor[6]);//22 -z
      msg.data.push_back(q_motor[7]);//23 -y
      //left hand dynamixel + fingers
      msg.data.push_back(0);//24
      msg.data.push_back(0);//25
      msg.data.push_back(0);//26
      msg.data.push_back(0);//27

        chatter_pub.publish(msg);

        ros::spinOnce();
        loop_rate.sleep();
        ++count;

}

    return 0;
}


#ifndef __PROPERTIES_HPP
#define __PROPERTIES_HPP

#include <unordered_map>
#include <armadillo>

/*
  List of all possible properties that could be produced by or
  requested of a QMInterface.
*/
enum class QMProperty{
   nacvector,        // mat
   nacvector_imag,   // mat
   wfoverlap,        // mat
   diabatic_rot_mat, //mat
   qmgradient,       // mat
   mmgradient,       // mat
   qmgradient_multi, // cube
   mmgradient_multi, // cube
   energies,         // vec
};

std::ostream& operator<<( std::ostream& oss, const QMProperty p);

/*
  A wrapper around pointers to various armadillo base types so that
  PropMap can store tensors of different rank. Implicitly converts
  back to the base type on assignement; will raise runtime exceptions
  if invalid conversions are attempted.

  To add support for another type:
  * Add a pointer member
  * Add a constructor
  * Add operators for & and *
  * Update the function std::string kind(void) with the new type
*/
class ArmaWrap{
public:
  ArmaWrap(std::nullptr_t x) {(void) x;};
  ArmaWrap(arma::vec  * x): v(x) {};
  ArmaWrap(arma::mat  * x): m(x) {};
  ArmaWrap(arma::cube * x): c(x) {};

  // FIXME: should the nullptr_t return be conditional?
  operator std::nullptr_t() const {return nullptr;};
  operator arma::vec  *() const {return notnull(v);};
  operator arma::mat  *() const {return notnull(m);};
  operator arma::cube *() const {return notnull(c);};

  operator arma::vec  &() const {return *notnull(v);};
  operator arma::mat  &() const {return *notnull(m);};
  operator arma::cube &() const {return *notnull(c);};

private:
  // Member types
  arma::vec  * v = nullptr;
  arma::mat  * m = nullptr;
  arma::cube * c = nullptr;

  template <typename T>
  T notnull(T p) const{
    if (!p){
      std::string msg = "Invalid conversion to " + (std::string) typeid(T).name() + "; I am a " + kind() + "!";
      throw std::logic_error(msg);
    }
    return p;
  }

  std::string kind(void) const{
    if      (v){return "vec";}
    else if (m){return "mat";}
    else if (c){return "cube";}
    else       {return "uninitialized wrapper";}
  }
};

class PropMap
{
public:
  PropMap() : prop{} {};
  ArmaWrap get(QMProperty key);
  const arma::uvec* get_idx(QMProperty key) const;
  arma::uvec & get_writable_idx(QMProperty key);

  void emplace(QMProperty p, ArmaWrap vec);
  void emplace(QMProperty p, arma::uvec iv, ArmaWrap vec);

  bool has(QMProperty key) const;
  bool has_idx(QMProperty key) const;

  const std::vector<QMProperty> keys(void) const;

  friend std::ostream& operator<<(std::ostream& oss, const PropMap &pm){
    oss << "{ ";
    for (const auto& e: pm.prop){
      oss << "{" << e.first;
      if (pm.has_idx(e.first)){
        auto idx = pm.get_idx(e.first);
        oss << ": ";
        for (const auto& v: *idx){
          oss << v << " ";
        }
      }
      oss << "}, ";
    }
    oss << "}";
    return oss;
  }

private:
  std::unordered_map<QMProperty, ArmaWrap> prop{};
  std::unordered_map<QMProperty, arma::uvec> prop_vec{};
};

#endif


/* Copyright (c) 2016 - 2019, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef THERMAL_OPERATOR_HH
#define THERMAL_OPERATOR_HH

#include <MaterialProperty.hh>
#include <Operator.hh>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/matrix_free/cuda_matrix_free.h>
#include <deal.II/matrix_free/cuda_fe_evaluation.h>

namespace adamantine
{
  template <int dim, int fe_degree, typename NumberType>
  class ThermalOperatorQuad
{
public:
__device__ ThermalOperatorQuad(NumberType thermal_conductivity, NumberType alpha, NumberType beta):
_thermal_conductivity(thermal_conductivity),
_alpha(alpha),
_beta(beta)
{}

__device__ void
operator()(dealii::CUDAWrappers::FEEvaluation<dim, fe_degree, fe_degree+1, 1, NumberType> *fe_eval) const;

private:
NumberType _thermal_conductivity;
NumberType _alpha;
NumberType _beta;
};

template <int dim, int fe_degree, typename NumberType>
class LocalThermalOperator
{
public:
LocalThermalOperator(NumberType *thermal_conductivity, NumberType *alpha, NumberType *beta)
: _thermal_conductivity(thermal_conductivity),
_alpha(alpha),
_beta(beta)
{}

__device__ void operator()(
    const unsigned int cell,
    const typename dealii::CUDAWrappers::MatrixFree<dim, NumberType>::Data *gpu_data,
    dealii::CUDAWrappers::SharedData<dim, NumberType> * shared_data,
    const NumberType *src,
    NumberType* dst) const;

    static const unsigned int n_dofs_1d    = fe_degree + 1;
    static const unsigned int n_local_dofs = dealii::Utilities::pow(fe_degree + 1, dim);
    static const unsigned int n_q_points   = dealii::Utilities::pow(fe_degree + 1, dim);
  private:
    NumberType *_thermal_conductivity;
NumberType *_alpha;
NumberType *_beta;
  };

/**
 * This class is the operator associated with the heat equation, i.e., vmult
 * performs \f$ dst = -\nabla k \nabla src \f$.
 */
template <int dim, int fe_degree, typename NumberType>
class ThermalOperator : public Operator<NumberType>
{
public:
  ThermalOperator(MPI_Comm const &communicator,
                  std::shared_ptr<MaterialProperty<dim>> material_properties);

  /**
   * Associate the AffineConstraints<NumberType> and the MatrixFree objects to the
   * underlying Triangulation.
   */
  template <typename QuadratureType>
  void setup_dofs(dealii::DoFHandler<dim> const &dof_handler,
                  dealii::AffineConstraints<NumberType> const &affine_constraints,
                  QuadratureType const &quad);

  /**
   * Compute the inverse of the mass matrix and update the material properties.
   */
  void reinit(dealii::DoFHandler<dim> const &dof_handler,
              dealii::AffineConstraints<NumberType> const &affine_constraints);

  /**
   * Clear the MatrixFree object and resize the inverse of the mass matrix to
   * zero.
   */
  void clear();

  dealii::types::global_dof_index m() const override;

  dealii::types::global_dof_index n() const override;

  /**
   * Return a shared pointer to the inverse of the mass matrix.
   */
  std::shared_ptr<dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA>>
  get_inverse_mass_matrix() const;

  /**
   * Return a shared pointer to the underlying MatrixFree object.
   */
  dealii::CUDAWrappers::MatrixFree<dim, NumberType> const &get_matrix_free() const;

  void
  vmult(dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> &dst,
        dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &src) const override;

  void
  Tvmult(dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> &dst,
         dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &src) const override;

  void vmult_add(
      dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> &dst,
      dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &src) const override;

  void Tvmult_add(
      dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> &dst,
      dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &src) const override;

  void jacobian_vmult(
      dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> &dst,
      dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &src) const override;

  /**
   * Evaluate the material properties for a given state field.
   */
  void evaluate_material_properties(
      dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &state);

private:
  /**
   * Apply the operator on a given set of quadrature points.
   */
  void
  local_apply(dealii::CUDAWrappers::MatrixFree<dim, NumberType> const &data,
              dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> &dst,
              dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &src,
              std::pair<unsigned int, unsigned int> const &cell_range) const;

  /**
   * MPI communicator.
   */
  MPI_Comm const &_communicator;
  /**
   * Data to configure the MatrixFree object.
   */
  typename dealii::CUDAWrappers::MatrixFree<dim, NumberType>::AdditionalData
      _matrix_free_data;
  /**
   * Store the \f$ \alpha \f$ coefficient described in
   * MaterialProperty::compute_constants()
   */
  dealii::LinearAlgebra::CUDAWrappers::Vector<NumberType> _alpha;
  /**
   * Store the \f$ \beta \f$ coefficient described in
   * MaterialProperty::compute_constants()
   */
  dealii::LinearAlgebra::CUDAWrappers::Vector<NumberType> _beta;
  /**
   * Table of thermal conductivity coefficient.
   */
  dealii::LinearAlgebra::CUDAWrappers::Vector<NumberType> _thermal_conductivity;
  /**
   * Material properties associated with the domain.
   */
  std::shared_ptr<MaterialProperty<dim>> _material_properties;
  /**
   * Underlying MatrixFree object.
   */
  dealii::CUDAWrappers::MatrixFree<dim, NumberType> _matrix_free;
  /**
   * The inverse of the mass matrix is computed using an inexact Gauss-Lobatto
   * quadrature. This inexact quadrature makes the mass matrix and therefore
   * also its inverse, a diagonal matrix.
   */
  std::shared_ptr<dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA>>
      _inverse_mass_matrix;

  dealii::types::global_dof_index _m;

  dealii::DoFHandler<dim>const * _dof_handler;
};

template <int dim, int fe_degree, typename NumberType>
inline dealii::types::global_dof_index
ThermalOperator<dim, fe_degree, NumberType>::m() const
{
  return _m;
}

template <int dim, int fe_degree, typename NumberType>
inline dealii::types::global_dof_index
ThermalOperator<dim, fe_degree, NumberType>::n() const
{
  return _m;
}

template <int dim, int fe_degree, typename NumberType>
inline std::shared_ptr<dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA>>
ThermalOperator<dim, fe_degree, NumberType>::get_inverse_mass_matrix() const
{
  return _inverse_mass_matrix;
}

template <int dim, int fe_degree, typename NumberType>
inline dealii::CUDAWrappers::MatrixFree<dim, NumberType> const &
ThermalOperator<dim, fe_degree, NumberType>::get_matrix_free() const
{
  return _matrix_free;
}

template <int dim, int fe_degree, typename NumberType>
inline void ThermalOperator<dim, fe_degree, NumberType>::jacobian_vmult(
    dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> &dst,
    dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &src) const
{
  vmult(dst, src);
}
} // namespace adamantine

#endif


/*=============================================================================
Copyright 2018 Pranam Lashkari <plashkari628@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_COORDINATE_SUPERGALACTIC_HPP
#define BOOST_ASTRONOMY_COORDINATE_SUPERGALACTIC_HPP

#include <boost/astronomy/coordinate/ref_frame/base_frame.hpp>
#include <boost/astronomy/coordinate/rep/spherical_representation.hpp>
#include <boost/astronomy/coordinate/diff/spherical_coslat_differential.hpp>

namespace boost { namespace astronomy { namespace coordinate {

template <typename Representation, typename Differential>
struct supergalactic : public base_frame<Representation, Differential>
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG((std::is_base_of
        <spherical_representation<typename Representation::type,
        typename Representation::quantity1, typename Representation::quantity2,
        typename Representation::quantity3>, Representation>::value),
        "argument type is expected to be a spherical_representation class");
    BOOST_STATIC_ASSERT_MSG((std::is_base_of
        <spherical_coslat_differential<typename Differential::type,
        typename Differential::quantity1, typename Differential::quantity2,
        typename Differential::quantity3>, Differential>::value),
        "argument type is expected to be a spherical_coslat_differential class");
    ///@endcond

public:
    //default constructor no initialization
    supergalactic() {}

    //!creates coordinate in supergalactic frame using any subclass of base_representation
    template <typename OtherRepresentation>
    supergalactic(OtherRepresentation const& representation_data)
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
         <boost::astronomy::coordinate::base_representation, OtherRepresentation>::value),
         "Invalid representation class");

        auto temp = make_spherical_representation
            <
            typename Representation::type,
            typename Representation::quantity1,
            typename Representation::quantity2,
            typename Representation::quantity3,
            typename OtherRepresentation::type,
            typename OtherRepresentation::quantity1,
            typename OtherRepresentation::quantity2,
            typename OtherRepresentation::quantity3
            >(representation_data);

        this->data = temp;
    }

    //!creates coordinate from given values
    //!sgb -> latitude, sgl -> longitude
    supergalactic
    (
        typename Representation::quantity1 const& sgb,
        typename Representation::quantity2 const& sgl,
        typename Representation::quantity3 const& distance
    )
    {
        this->data.set_lat_lon_dist(sgb, sgl, distance);
    }

    //!creates coordinate with motion from given values
    //!sgb -> latitude, sgl -> longitude
    //!pm_sgb -> proper motion in sgb, pm_sgl_cossgb -> proper motion in sgl including cos(sgb)
    supergalactic
    (
        typename Representation::quantity1 const& sgb,
        typename Representation::quantity2 const& sgl,
        typename Representation::quantity3 const& distance,
        typename Differential::quantity1 const& pm_sgb,
        typename Differential::quantity2 const& pm_sgl_cossgb,
        typename Differential::quantity3 const& radial_velocity
    ) : supergalactic(sgb, sgl, distance)
    {
        this->motion.set_dlat_dlon_coslat_ddist(pm_sgb, pm_sgl_cossgb, radial_velocity);
    }

    //!creates coordinate with motion
    //!representation class is used for coordinate data
    //!differential class is used for motion data
    template <typename OtherRepresentation, typename OtherDifferential>
    supergalactic
    (
        OtherRepresentation const& representation_data,
        OtherDifferential const& differential_data
    )
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
         <boost::astronomy::coordinate::base_representation, OtherRepresentation>::value),
         "argument type is expected to be a differential class");

        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_differential, OtherDifferential>::value),
            "argument type is expected to be a differential class");

        auto rep_temp = make_spherical_representation
            <
                typename Representation::type,
                typename Representation::quantity1,
                typename Representation::quantity2,
                typename Representation::quantity3,
                typename OtherRepresentation::type,
                typename OtherRepresentation::quantity1,
                typename OtherRepresentation::quantity2,
                typename OtherRepresentation::quantity3
            >(representation_data);
        this->data = rep_temp;

        auto dif_temp = make_spherical_coslat_differential
            <
                typename Differential::type,
                typename Differential::quantity1,
                typename Differential::quantity2,
                typename Differential::quantity3,
                typename OtherDifferential::type,
                typename OtherDifferential::quantity1,
                typename OtherDifferential::quantity2,
                typename OtherDifferential::quantity3
            >(differential_data);
        this->motion = dif_temp;
    }

    //copy constructor
    supergalactic(supergalactic<Representation, Differential> const& other)
    {
        this->data = other.get_data();
        this->motion = other.get_differential();
    }

    //!returns component sgb of the supergalactic coordinate
    typename Representation::quantity1 get_sgb() const
    {
        return this->data.get_lat();
    }

    //!returns component sgl of the supergalactic coordinate
    typename Representation::quantity2 get_sgl() const
    {
        return this->data.get_lon();
    }

    //!returns distance component of the supergalactic coordinate
    typename Representation::quantity3 get_distance() const
    {
        return this->data.get_dist();
    }

    //!returns the (sgb, sgl, dist) in the form of tuple
    std::tuple
    <
        typename Representation::quantity1,
        typename Representation::quantity2,
        typename Representation::quantity3
    > get_sgb_sgl_dist() const
    {
        return this->data.get_lat_lon_dist();
    }

    //!returns proper motion in supergalactic latitude
    typename Differential::quantity1 get_pm_sgb() const
    {
        return this->motion.get_dlat();
    }

    //!returns proper motion in supergalactic longitude including cos(b)
    typename Differential::quantity2 get_pm_sgl_cossgb() const
    {
        return this->motion.get_dlon_coslat();
    }

    //!returns radial_velocity
    typename Differential::quantity3 get_radial_velocity() const
    {
        return this->motion.get_ddist();
    }

    //!returns the proper motion in form of tuple including cos(b)
    std::tuple
    <
        typename Differential::quantity1,
        typename Differential::quantity2,
        typename Differential::quantity3
    > get_pm_sgb_sgl_radial() const
    {
        return this->motion.get_dlat_dlon_coslat_ddist();
    }

    //!sets value of component b of the supergalactic coordinate
    void set_sgb(typename Representation::quantity1 const& sgb)
    {
        this->data.set_lat(sgb);
    }

    //!sets value of component sgl of the supergalactic coordinate
    void set_sgl(typename Representation::quantity2 const& sgl) const
    {
        this->data.set_lon(sgl);
    }

    //!sets value of distance component of the supergalactic coordinate
    void set_distance(typename Representation::quantity3 const& distance)
    {
        this->data.set_dist(distance);
    }

    //!sets value of all component of the coordinate
    void set_sgb_sgl_dist
    (
        typename Representation::quantity1 const& sgb,
        typename Representation::quantity2 const& sgl,
        typename Representation::quantity3 const& dist
    )
    {
        this->data.set_lat_lon_dist(sgb, sgl, dist);
    }

    //!sets the proper motion in supergalactic latitude
    void set_pm_sgb(typename Differential::quantity1 const& pm_sgb)
    {
        this->motion.set_dlat(pm_sgb);
    }

    //!sets the proper motion in supergalactic longitude including cos(b)
    void set_pm_sgl_cossgb(typename Differential::quantity2 const& pm_sgl_cossgb)
    {
        this->motion.set_dlon_coslat(pm_sgl_cossgb);
    }

    //!sets the radial_velocity
    void set_radial_velocity(typename Differential::quantity3 const& radial_velocity)
    {
        this->motion.set_ddist(radial_velocity);
    }

    //!set value of motion including cos(b)
    void set_pm_sgb_sgl_radial
    (
        typename Differential::quantity1 const& pm_sgb,
        typename Differential::quantity2 const& pm_sgl_cosb,
        typename Differential::quantity3 const& radial_velocity
    )
    {
        this->motion.set_dlat_dlon_coslat_ddist(pm_sgb, pm_sgl_cosb, radial_velocity);
    }

};

}}} //namespace boost::astronomy::coordinate

#endif // !BOOST_ASTRONOMY_COORDINATE_SUPERGALACTIC_HPP


/*
ID: septicmk
LANG: C++
TASK: DT.cpp
*/

#include "DT.h"
#include "../Core/Core.h"
#include <boost/serialization/vector.hpp>
#include <boost/serialization/queue.hpp>
#include <boost/serialization/export.hpp>
#include <vector>
#include <queue>
#include <fstream>
#include <iostream>

BOOST_CLASS_EXPORT(dt::DT)
namespace dt{

    void DT::init(std::vector<size_t>feat_depth, size_t cls_number){
        this->feat_depth = feat_depth;
        this->feat_dim = feat_depth.size();
        this->cls_number = cls_number;
    }

    DT* DT::operator +(DT &rhs){
        DT* ret = new DT;
        ret->nodes = this->nodes;
        ret->feat_depth = this->feat_depth;
        ret->feat_dim = this->feat_dim;
        ret->cls_number = this->cls_number;
        for(size_t i = 0; i < nodes.size(); i++){
            if(nodes[i] == rhs.nodes[i]){
                ret->nodes[i] = nodes[i].merge(rhs.nodes[i]);
            }
        }
        return ret;
    }

    void DT::beginDataScan(std::vector<double> records, size_t feat_dim){
        Node head;
        head.index.resize(feat_dim-1);
        std::fill(head.index.begin(), head.index.end(), -1);
        head.init(feat_depth, cls_number);
        nodes.clear();
        nodes.push_back(head);
    }

    void DT::endDataScan(){
        std::vector<Node> next;
        next.clear();
        for(size_t i = 0; i < nodes.size(); i++){
            Node cur = nodes[i];
            if(cur.tag) continue;
            int pure = 0;
            int major = -1;
            int num = -1;
            for(size_t j = 0; j < cls_number; j++){
                if(cur.D[j] != 0) pure ++;
                if(cur.D[j] > num){
                    num = cur.D[j];
                    major = j;
                }
            }

            if(pure == 1){
                cur.vec.clear();
                cur.tag = true;
                cur.who = major;
                cur.D.clear();
                next.push_back(cur);
                continue;
            }
            int end = 0;
            for(size_t j = 0; j < cur.index.size(); j++){
                if(cur.index[j] == -1) end++;
            }
            int win = cur.split();
            //std::cout << pure << " "<<major  << std::endl;
            //std::cout << end << " "<<win  << std::endl;
            for(size_t k = 0; k < feat_depth[win]; k++){
                Node tmp;
                tmp.index = cur.index;
                tmp.index[win] = k;
                tmp.init(feat_depth, cls_number);
                //std::cout << "##"<<tmp.D.size() << std::endl;

                int tot = 0;
                for(size_t w = 0; w < cls_number; w++){
                    tot += cur.vec[win].get(k,w);
                }
                //std::cout << "**" << tot << " " << end << std::endl;
                if(tot == 0 || end == 1){
                    tmp.tag = true;
                    tmp.D.clear();
                    tmp.vec.clear();
                    tmp.who = major;
                }
                //std::cout << "$$"<<tmp.D.size() << std::endl;
                next.push_back(tmp);
            }
        }
        this->nodes = next;
    }

    DT* DT::processRecord(std::vector<double> records, size_t feat_dim){
        DT* ret = new DT;
        ret->nodes = this->nodes;
        ret->feat_depth = this->feat_depth;
        ret->feat_dim = this->feat_dim;
        ret->cls_number = this->cls_number;
        size_t n_records = records.size()/feat_dim;

        for(size_t k = 0; k < n_records; k++){
            std::vector<int> record;
            record.clear();
            for(size_t w = 0; w < feat_dim ; w++){
                record.push_back(records[w + k*feat_dim]);
            }

            for(size_t i = 0; i < ret->nodes.size(); i++){
                Node cur = ret->nodes[i];

                bool tag = true;
                for(size_t j = 0; j < cur.index.size(); j++){
                    if(cur.index[j] != -1 && cur.index[j] != record[j]){
                        tag = false;
                        break;
                    }
                }

                if(tag){
                    if(cur.tag) break;
                    cur.update(record);
                    ret->nodes[i] = cur;
                    break;
                }
            }
        }
        return ret;
    }

    bool DT::isConverged(DT* rhs, size_t feat_dim, double eps){
        for(size_t i = 0; i < nodes.size(); i ++){
            if(nodes[i].tag == false) return false;
        }
        return true;
    }

    void DT::finish(std::vector<double> records, size_t feat_dim){
        std::ofstream file;
        file.open("./DT.txt");

        for(size_t i = 0; i < nodes.size(); i++){
            for(size_t j = 0; j < nodes[i].index.size(); j ++){
                file <<  nodes[i].index[j] << ' ';
            }
            file << nodes[i].who << std::endl;
        }
    }

}


#ifndef MJOLNIR_TEST_UTIL_CHECK_FORCE_AND_VIRIAL_HPP
#define MJOLNIR_TEST_UTIL_CHECK_FORCE_AND_VIRIAL_HPP
#include <mjolnir/math/Vector.hpp>
#include <mjolnir/core/System.hpp>

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

namespace mjolnir
{
namespace test
{

// This checks the consistency of forces between `calc_force` and
// `calc_force_and_virial`.

template<typename traitsT, typename Interaction>
void check_force_and_virial(System<traitsT> ref,
                            const Interaction& interaction,
                            const typename traitsT::real_type tol)
{
    using coordinate_type = typename traitsT::coordinate_type;
    using matrix33_type   = typename traitsT::matrix33_type;

    for(std::size_t i=0; i<ref.size(); ++i)
    {
        ref.force(i) = math::make_coordinate<coordinate_type>(0,0,0);
    }
    ref.virial() = matrix33_type(0,0,0, 0,0,0, 0,0,0);

    System<traitsT> sys(ref);

    ref.preprocess_forces();
    interaction.calc_force(ref);
    ref.postprocess_forces();

    sys.preprocess_forces();
    interaction.calc_force_and_virial(sys);
    sys.postprocess_forces();

    for(std::size_t idx=0; idx<sys.size(); ++idx)
    {
        BOOST_TEST(math::X(sys.force(idx)) == math::X(ref.force(idx)), boost::test_tools::tolerance(tol));
        BOOST_TEST(math::Y(sys.force(idx)) == math::Y(ref.force(idx)), boost::test_tools::tolerance(tol));
        BOOST_TEST(math::Z(sys.force(idx)) == math::Z(ref.force(idx)), boost::test_tools::tolerance(tol));
    }
}

} // test
} // mjolnir
#endif// MJOLNIR_TEST_UTIL_CHECK_FORCE_HPP


/*******************************************************
 *
 * Copyright (C) 2022, Chen Jianqu, Shanghai University
 *
 * This file is part of detector_desktop.
 *
 * Licensed under the MIT License;
 * you may not use this file except in compliance with the License.
 *******************************************************/

#include "detector_desktop/Segmentor.h"
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include "detector_desktop/Types.h"

Segmentor::Segmentor()
{
    euc_cluster.reset(new pcl::EuclideanClusterExtraction<PointT>);
    euc_cluster->setClusterTolerance (0.01); // 2cm，在欧氏空间里设置空间聚类容差 tolerance ，其 实是在近邻搜索中所使用的半径。
    indices_extractor.reset(new pcl::ExtractIndices<PointT>);
    voxel_filter.reset(new pcl::ApproximateVoxelGrid<PointT>);
    voxel_filter->setDownsampleAllData(false);
    voxel_filter->setLeafSize(Config::kPointCloudResolution, Config::kPointCloudResolution, Config::kPointCloudResolution);
    cv::cv2eigen(Config::kOpticalToArm, Tcb.matrix());
}

void Segmentor::SetDetector(Detector *detector_) {
    detector.reset(detector_);
}

bool Segmentor::WaitForFrame(cv::Mat &vis,Instance::InstsPtr &insts,int time_out) {
    std::unique_lock<std::mutex> lock(queue_mutex);
    queue_cond.wait_for(lock, std::chrono::milliseconds(time_out));//等待time_out
    if(frame_queue.empty())
        return false;
    vis=frame_queue.front()->color;
    *insts=frame_queue.front()->insts;
    frame_queue.pop();
    return true;
}

bool Segmentor::WaitForFrame(Frame::Ptr &frame,int time_out) {
    std::unique_lock<std::mutex> lock(queue_mutex);
    if(queue_cond.wait_for(lock, std::chrono::milliseconds(time_out)) == std::cv_status::timeout){//等待time_out
        return false;
    }
    if(frame_queue.empty())
        return false;
    frame=frame_queue.front();
    //frameQueue.front()->clone(frame);
    frame_queue.pop();
    return true;
}

bool Segmentor::TryPop(Frame::Ptr &frame) {
    std::unique_lock<std::mutex> lock(queue_mutex);
    if(frame_queue.empty())
        return false;
    frame=frame_queue.front();
    frame_queue.pop();
    return true;
}

void Segmentor::BuildPointCloud(const cv::Mat& color,const cv::Mat& depth,int rows_start,int rows_end,
                                int cols_start,int cols_end,PointCloud::Ptr &pc)
{
    for ( int i=rows_start; i<rows_end; i++ ){
        for ( int j=cols_start; j<cols_end; j++ ){
            unsigned short d = depth.ptr<unsigned short> (i)[j]; // 深度值
            if ( d<100 ) continue; // 为0表示没有测量到
            if ( d >= 2500 ) continue; // 深度太大时不稳定，去掉
            PointT p;
            p.z=float(d)/cfg::kDepthFactor;
            p.x= (float(j)-cfg::kCX) * p.z / cfg::kFX;
            p.y= (float(i)-cfg::kCY) * p.z / cfg::kFY;
            p.b = color.data[ i*color.step+j*color.channels() ];
            p.g = color.data[ i*color.step+j*color.channels()+1 ];
            p.r = color.data[ i*color.step+j*color.channels()+2 ];
            pc->points.push_back(p);
        }
    }
}

PointCloud::Ptr Segmentor::BuildObjectPointCloud(const cv::Mat &color, const cv::Mat &depth, Box2D &box)
{
    PointCloud::Ptr cpc(new PointCloud);
    BuildPointCloud(color,depth,box.top,box.bottom,box.left,box.right,cpc);//本线程
    /*
        //两个并行线程构建点云
        PointCloud::Ptr pc1(new PointCloud);
        int left=box.left,top=box.top,right=box.right,bottom=box.bottom;
        int half_cols=left+(right-left)/2;
        std::thread t1(&Segmentor::BuildPointCloud, this,color,depth,top,bottom,left,half_cols,std::ref(pc1));//另一个线程
        BuildPointCloud(color,depth,top,bottom,half_cols,right,cpc);//本线程
        t1.join();
        *cpc+=*pc1;
        */
    if(cpc->size()<1000){
        return PointCloud::Ptr(new PointCloud);
    }
    PointCloud::Ptr cloud_cluster (new PointCloud);
    //体素滤波
    voxel_filter->setInputCloud(cpc );
    voxel_filter->filter(*cpc );
    //聚类分割
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
    tree->setInputCloud (cpc);
    euc_cluster->setMinClusterSize (int(cpc->size() / 3));//最小簇
    euc_cluster->setMaxClusterSize (cpc->size());//最大簇
    euc_cluster->setSearchMethod (tree);//设置搜索时所用的搜索机制，参数 tree 指向搜索时所用的搜索对象，例如kd-tree octree 等对
    euc_cluster->setInputCloud (cpc);
    euc_cluster->extract (cluster_indices);
    if(cluster_indices.empty()){
        return PointCloud::Ptr(new PointCloud);
    }
    auto itp = cluster_indices.begin ();
    pcl::PointIndices::Ptr pi_ptr(new pcl::PointIndices(*itp));
    indices_extractor->setInputCloud (cpc);
    indices_extractor->setIndices (pi_ptr);
    indices_extractor->setNegative (false);
    indices_extractor->filter (*cloud_cluster);
    //对点云进行变换
    pcl::transformPointCloud(*cloud_cluster, *cloud_cluster, Tcb);
    //对将点云从相机坐标系转换到世界坐标系
    //Eigen::Affine3d T=Eigen::Affine3d::Identity();
    //T.translate(-Tcb.translation());
    //T.rotate(Tcb.rotation().inverse());
    //pcl::transformPointCloud(*cloud_cluster, *cloud_cluster, T);

    /*
    auto m=Tcb.matrix();
    for(auto &p : cloud_cluster->points){
        double x=p.x,y=p.y,z=p.z;
        p.x=m(0,0)*x+m(0,1)*y+m(0,2)*z;
        p.y=m(1,0)*x+m(1,1)*y+m(1,2)*z;
        p.z=m(2,0)*x+m(2,1)*y+m(2,2)*z;
    }
     */
    return cloud_cluster;
}

void Segmentor::Run()
{
    while(cfg::is_running)
    {
        Frame::Ptr frame;
        detector->WaitForResult(frame);
        TimeCounter timeCounter;
        Instance::Insts insts;
        for(auto &box : frame->boxes){
            PointCloud::Ptr cpc= BuildObjectPointCloud(frame->color, frame->depth, box);
            insts.emplace_back(Instance(cpc,box));
            //在图片上绘制框
            if(!cpc->empty()){
                auto color=cv::Scalar(255,0,0);
                box.VisBox(frame->drawedImg,color);
            }
            else{
                auto color=cv::Scalar(0,255,0);
                box.VisBox(frame->drawedImg,color);
            }
        }
        frame->insts=insts;
        frame->MergePointCloud();
        {
            std::unique_lock<std::mutex> lock(queue_mutex);
            if(frame_queue.size() < Config::kSegmentorQueueSize){
                frame_queue.push(frame);
            }
            queue_cond.notify_one();
        }
    }
    Debugt("Segmentor线程结束");
}