server/algorithm/base/base_data_struct.h

#ifndef BASE_DATA_STRUCT_H
#define BASE_DATA_STRUCT_H

#include <memory>
#include <unordered_map>

namespace algorithm{
	namespace base{
		// 分站接收数据结构体
		struct ReceiveData{
			unsigned int reader_id;		// 分站号
			unsigned short antenna_id;  // 天线号
			long long rec_time_stamp;	// 分站接收时间，一个7字节的无符号数
			int special;
			double x;	//分站的x坐标
			double y;   //分站的y坐标
			double z;   //分站的z坐标

			ReceiveData(){
				reader_id = 0;
				antenna_id = 0;
				rec_time_stamp = 0;
				x = y = z = 0.0;
				special = 0;
			};
		};

		typedef unordered_map<unsigned long long ,std::shared_ptr<ReceiveData>> ReceiveDataUnorderedMap;

		template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
    class Functor
    {
    private:
        int m_inputs, m_values;
    public:
        typedef _Scalar Scalar;
        enum {
            InputsAtCompileTime = NX,
            ValuesAtCompileTime = NY
        };
        typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
        typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
        typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

        Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
        Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

        int inputs() const { return m_inputs; }
        int values() const { return m_values; }

    };

    class TDOAFunctor : public Functor<double>
    {
    private:
        Eigen::MatrixXd _data, _pos;
    public:
        TDOAFunctor(const Eigen::MatrixXd &data,const Eigen::MatrixXd &pos): Functor<double>(3,3),_data(data),_pos(pos) {}
        int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
        {
            for (int i = 0; i < values(); i++)
            {
                fvec(i) = sqrt(pow(_data(i,0) - x(0),2) + pow(_data(i,1) - x(1),2) + pow(_data(i,2) - x(2),2)) 
                        - sqrt(pow(_pos(0) - x(0),2) + pow(_pos(1) - x(1),2) + pow(_pos(2) - x(2),2)) 
                        - _data(i,3);
            }
            return 0;
        }
    };
	};

	const int MAX_READER_TDOA_PATH_NUMS = 10;	//主要是因为拐弯处可能存在多解的原因
	const double INVALID_COORDINATE = -1000.0;

	enum DIMENSION{
		_1D = 1,
		_2D,
		_3D
	};
	enum ALGO_LOC_TYPE{
		ALGO_LOC_SUCCESSED = 0,			//算法定位成功
		ALGO_FAILED_CONDITION_1,		//同步序号相差大于5
		ALGO_FAILED_CONDITION_2,		//相同卡序列号，时间同步序号却相差大于5
		ALGO_FAILED_CONDITION_3,		//时间同步戳值存在异常
		ALGO_FAILED_CONDITION_4,		//卡的上次时间戳大于卡的本次时间戳
		ALGO_FAILED_CONDITION_5,		//人卡加速度超限
		ALGO_FAILED_CONDITION_6,		//车卡加速度超限
		ALGO_FAILED_CONDITION_7,		//车卡速度超限
		ALGO_FAILED_CONDITION_8,		//卡尔曼连续2s定位失败等
		ALGO_FAILED_CONDITION_9,		//分站时间戳的距离差值大于分站之间的距离
		ALGO_FAILED_CONDITION_10,		//分站之间无地图集
		ALGO_FAILED_CONDITION_11,		//定位无解or解的个数为0
		ALGO_FAILED_CONDITION_12,		//分站附近（小于4m），分站不是特殊分站
		ALGO_FAILED_CONDITION_13,		//解与无地图集两分站之间的距离差（大于10m）
		ALGO_FAILED_CONDITION_14,		//定位结果距离定位分站之间的距离差大于4
		ALGO_FAILED_CONDITION_15,		//参与定位的数据条数小于2
		ALGO_LOC_TOTAL
	};

	struct POS{
		double posx; // 以米为单位坐标
		double posy;
		double posz;
		//误差范围
		double pos_radius;

		//以像素为单位的坐标，米/地图比例尺 = 像素
		double cx;
		double cy;
		double cz;
		//三个方向的速度
		double cvx;
		double cvy;
		double cvz;
		//加速度
		double av;

		//保存定位结果的两个分站信息
		int nFirstReader;
		int nSecondReader;

		//精度参考
		double dDiff[MAX_READER_TDOA_PATH_NUMS];
		double dis_diff;

		//定位失败的状态
		int reason;
		int status;
		int dimension;
		unsigned long long card_count;		//卡的ct号
		unsigned short card_timestamp; 
		bool update;		//是否需要保存到同步队列中
		bool is_back;
		bool is_fit;		//是否是拟合数据

		//当前分站同步序号与参考数据的分站同步序号时间差值
		double diff_reader_sync_num;

		double delta_time;			//时间差
		//参考数据的定位坐标
		double ref_x;
		double ref_y;
		double ref_z;
		double origin_speed;	//原始定位结果算的速度

		double sumVariance;		// 保存各定位解之间差值的和

		POS(){
			card_timestamp = 0;
			nFirstReader = 0;
			nSecondReader = 0;
			pos_radius = 999999.9;	
			posx = posy = posz = INVALID_COORDINATE;
			cx = cy = cz = cvx = cvy = cvz = 0.0;
			av = 0.0;
			status = 0;
			card_count = 0;
			for (int i=0;i<MAX_READER_TDOA_PATH_NUMS;i++)
			{
				dDiff[i] = 0;
			}

			dis_diff = 999999.9;
			update = false;
			is_fit = false;
			is_back = false;
			origin_speed = 0;
			sumVariance = 0;
			diff_reader_sync_num = 0;
			delta_time = 0.0;
			ref_x = ref_y = ref_z = 0.0;
			dimension = _1D;
			reason = ALGO_LOC_SUCCESSED;
		}

		POS& operator=(POS& tmp){
			this->card_count = tmp.card_count;
			this->card_timestamp = tmp.card_timestamp;
			nFirstReader = tmp.nFirstReader;
			nSecondReader = tmp.nSecondReader;
			posx = tmp.posx;
			posy = tmp.posy;
			posz = tmp.posz;
			cx = tmp.cx;
			cy = tmp.cy;
			cz = tmp.cz;
			cvx = tmp.cvx;
			cvy = tmp.cvy;
			cvz = tmp.cvz;
			av = tmp.av;
			update = tmp.update;
			origin_speed = tmp.origin_speed;
			status = tmp.status;
			is_back = tmp.is_back;
			diff_reader_sync_num = tmp.diff_reader_sync_num;
			delta_time = tmp.delta_time;
			dis_diff = tmp.dis_diff;
			ref_x = tmp.ref_x;
			ref_y = tmp.ref_y;
			ref_z = tmp.ref_z;
			sumVariance = tmp.sumVariance;
			for (int i=0;i<MAX_READER_TDOA_PATH_NUMS;i++)
			{
				dDiff[i] = tmp.dDiff[i];
			}
			pos_radius = tmp.pos_radius;
			reason = ALGO_LOC_SUCCESSED;
			dimension = tmp.dimension;

			return *this;
		}
	};

	// 定位坐标
	struct _coordinate{
		_coordinate(){
			t = 0;
			reader_id = 0;
			tt = 0;
			d = 0.0;
			sync_num = 0;	

			x = 0.0;
			y = 0.0;
			z = 0.0;
			a = 0.0;
			v = 0.0;
			antenna_id = 0;
			d_offset = 0.0;
			special = 0;
			acceleration = 0;
			acce_state = 0;	
			acce_state_last = 0;
			ins_direction = 0;
			rangingtype = 1;		// 0 tof，1 toda
			cur_time = time(NULL);
			ins_request_check = 0;
			dimension = _1D;
			strength = 0;
		}
		int t; // 定位时间戳
		int reader_id; // 分站编号
		unsigned long long syncRootId; // tdoa主节点
		unsigned long long tt; // 分站接收的时间，为同步及线性插值后的时间
		double d; // 距离

		double x; // x坐标
		double y; // y坐标
		double z; // z坐标
		double a; // 平面角度
		double v; // 速度
		int antenna_id;

		int sync_num;			//同步序号
		int acce_state;			//加速度状态
		int acce_state_last;	//加速度计上一次状态
		int ins_direction;		//惯导方向
		int special;			//分站是否属于特殊分站，0属于特殊分站，1普通分站
		int rangingtype;
		int reason;
		int ins_request_check;
		int dimension;		//指定一维，二维，三维的类型
		int strength;

		double acceleration;	//加速度
		double d_offset;		// 与显示距离的偏移，用来修正单基站的情况
		double recv_signal_power; //接收信号功率，计算值

		time_t cur_time;
		_coordinate& operator=(_coordinate &tmp){
			t = tmp.t;
			reader_id = tmp.reader_id;
			tt = tmp.tt;
			d = tmp.d;
			sync_num = tmp.sync_num;
			x = tmp.x;
			y = tmp.y;
			z = tmp.z;
			a = tmp.a;
			v = tmp.v;
			antenna_id = tmp.antenna_id;
			d_offset = tmp.d_offset;
			special = tmp.special;
			acceleration = tmp.acceleration;
			acce_state = tmp.acce_state;
			acce_state_last = tmp.acce_state_last;
			ins_direction = tmp.ins_direction;
			rangingtype = tmp.rangingtype;
			reason = ALGO_LOC_SUCCESSED;
			cur_time = tmp.cur_time;
			ins_request_check = tmp.ins_request_check;
			dimension = tmp.dimension;
			strength = tmp.strength;

			return *this;
		}
		bool operator==(_coordinate &tmp)
		{
			if (x == tmp.x && y == tmp.y)
			{
				return true;
			}
			return false;
		}
	};
};

#endif