new locate algo

card_message_handle.cpp

@@ -306,9 +306,9 @@ struct one_ct_message_handle
             return;
         }
 
-        log_info("[pdoa] calc_location_pdoa_begin: card_id=%d, ct=%d, m_ct=%d", m_card->m_id, v[0].m_card_ct, m_ct);
+        logn_info(3, "[pdoa] calc_location_pdoa_begin: card_id=%d, ct=%d, m_ct=%d", m_card->m_id, v[0].m_card_ct, m_ct);
         std::vector<point> rc = std::move(m_loc_tool.calc_location(v));
-        log_info("[pdoa] calc_location_pdoa: card_id=%d, size=%d", m_card->m_id, rc.size());
+        logn_info(3, "[pdoa] calc_location_pdoa: card_id=%d, size=%d", m_card->m_id, rc.size());
 
         if(!rc.empty()){
             m_card->on_location(std::move(rc), v);

loc_tool.cpp

@@ -120,7 +120,7 @@ bool loc_tool_pdoa_2_base::cal_position_pdoa(std::vector<pdoa_msg_ptr>& vps, con
 // pdoa一维定位
 std::vector<point> loc_tool_pdoa_1_base::calc_location(std::vector<loc_message>& loc)
 {
-    log_info("[pdoa] loc_tool_pdoa_1_base::calc_location, loc.size=%d", loc.size());
+    //logn_info(3, "[pdoa] loc_tool_pdoa_1_base::calc_location, card_id=%d, loc.size=%d", loc[0].m_card_id, loc.size());
     int32_t last_ct = -1;
     std::vector<point> vc;
     std::vector<loc_message> lm;

select_tool.h

@@ -128,6 +128,7 @@ struct select_point_object
 		{
 			att_initiate();
 		}
+		// 上次ct号
 		int last_ct()
         {
             return m_ct;
@@ -137,11 +138,11 @@ public:
 
 		zlist<loc_point,128> m_d;
 		line m_line;
-		int	 m_ct = -10;
+		int	 m_ct = -10;	// 计数号
 		fit_batch   m_fitk,m_fita;
-		fit_result  m_cur_fit;
-		loc_point*  m_begin;
-		loc_point*  m_last;
+		fit_result  m_cur_fit;	// 最新拟合结果
+		loc_point*  m_begin;	// 起始点
+		loc_point*  m_last;		// 结束点
 		//static opt_tool_main m_opt_tool;
 
 public:
@@ -156,10 +157,10 @@ public:
 		loc_point  select_solution_impl(const std::vector<point> p,const std::vector<loc_message>&lm);
 		bool select_pdoa_solution_impl(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
 		bool select_pdoa_solution(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point&lp, const float& pdoa);
+		bool select_pdoa_solution2(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point&lp, const float& pdoa);
 		bool select_pdoa_1d_solution(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
 		bool select_pdoa_2d_solution(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
 		bool select_pdoa_3d_solution(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
-		bool save_pdoa_1d(std::shared_ptr<card> card, const std::vector<point> vtp, const std::vector<loc_message> vtlm, loc_point& lp, double pdoa);
 		fit_result* best_fit_raw(int num_point=0,int start=0,int end=-1);
 		bool filter_by_acc(loc_point&c,std::array<solpoint,4>&v,double a);
 		bool filter_by_fit(loc_point & c,const std::vector<point> & vp,const double scale);
@@ -180,29 +181,36 @@ public:
 		virtual ~select_point_object(){}
 };
 
+/*
+ * 人员定位坐标过滤类
+ * 
+ * */
 struct person_point_filter:select_point_object
 {
 	person_point_filter(select_tool * owner)
 		:select_point_object(owner)
 		 ,m_filter_man_count(0)
 	{}
-	int m_filter_man_count=0;
+	int m_filter_man_count=0;	// 过滤计数
+	// 重置拟合
 	virtual void reset_fit(double d)
 	{
 		if(d>1)
 			m_filter_man_count++;
 		else 
 			m_filter_man_count = 0;
+		
 		if(m_filter_man_count==3)
 		{
 			m_fitk.reset_data();
 			m_fita.reset_data();
 			m_cur_fit.reset();
-			m_begin=m_last=nullptr;
+			m_begin = m_last = nullptr;
 			m_filter_man_count = 0;
 		}
 
 	}
+	
 	virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)
 	{
 		if(filter_by_fit(c,vp,scale))
@@ -221,14 +229,14 @@ struct person_point_filter:select_point_object
 		}
 		loc_point &refc=m_d(1);
 		if(c.m_type==4 &&  c.cl()<=20 && refc.is_same_site(c) && refc.time_off(c)<5 ){    
-			logn_info(3,"{test_lemon}first point:site=%d,card=%d,(%f,%f) else:(%f,%f),cred:%d,size:%d",c.m_sid,c.m_cid,c[0].x,c[0].y,c[1].x,c[1].y,c.cl(),vp.size());
+			logn_info(3,"{test}first point:site=%d,card=%d,(%f,%f) else:(%f,%f),cred:%d,size:%d",c.m_sid,c.m_cid,c[0].x,c[0].y,c[1].x,c[1].y,c.cl(),vp.size());
 			if(const auto & site_ptr=sit_list::instance()->get(c.m_sid)){
 				double d1=0,d2=0,d3=0;
 				d1=refc.m_dist1;
 				d2=site_ptr->dist_direct(c[0]);
 				d3=site_ptr->dist_direct(c[1]);
 				if(d1*d2<0 && d1*d3>0){
-					logn_info(3,"{test_lemon+++++++}first point:site=%d,card=%d,(%f,%f) else:(%f,%f),cred:%d,size:%d",c.m_sid,c.m_cid,c[0].x,c[0].y,c[1].x,c[1].y,c.cl(),vp.size());
+					logn_info(3,"{test}first point:site=%d,card=%d,(%f,%f) else:(%f,%f),cred:%d,size:%d",c.m_sid,c.m_cid,c[0].x,c[0].y,c[1].x,c[1].y,c.cl(),vp.size());
 					point t=c[0];
 					c[0]=c[1];
 					c[1]=t;
@@ -239,10 +247,12 @@ struct person_point_filter:select_point_object
 			}
 		}    
 	}
+
 	virtual fit_result * get_best_fit()
 	{
 		return best_fit_raw(4,4);
 	}
+	
 	virtual double getA(const fit_result * fit,const double scale,const double dt)
 	{
 		double a=2.5;
@@ -277,6 +287,7 @@ struct person_point_filter:select_point_object
 			m_owner->m_push_list.push(dp);
 			//dp.lp.debug_out("person");
 		}
+
 		return true;
 	}
 };
@@ -349,6 +360,7 @@ struct car_point_filter:select_point_object
 	{
 		return best_fit_raw(5);
 	}
+	
 	virtual double getA(const fit_result * fit,const double scale,const double dt)
 	{
 		double a=2.5;
@@ -456,15 +468,24 @@ struct car_point_filter:select_point_object
 		}
 	}
 
+	/* 根据历史数据估计定位点
+	 *
+	 * param
+	 *	const site* sit		分站
+	 *	double m_time_sec	秒数
+	 * return
+	 *	
+	 *
+	 */
 	double estimate_point_by_history(const site*sit, double m_time_sec)
 	{
 		double estimate_dist = m_last_fit_k * (m_time_sec-m_last_fit_xo) + m_last_fit_kb + m_last_fit_yo;
 		point pt(m_last_fit_md_x + estimate_dist * m_simple_rec_kx, m_last_fit_md_y + estimate_dist * m_simple_rec_ky);
 
-		int fidx=find_first(0);	
-		if(fidx>=0)
+		int fidx = find_first(0);	
+		if(fidx >= 0)
 		{
-			loc_point&f=m_d[fidx];
+			loc_point& f = m_d[fidx];
 			estimate_dist = f.loc_dist(pt);
 		}
 		else 
@@ -473,11 +494,15 @@ struct car_point_filter:select_point_object
 		return estimate_dist;
 	}
 
+	/*
+	 * 转换距离为坐标点
+	 * */
 	point convert_dist_to_pt(double dist, const site*sit)
 	{
-		int fidx=find_first(0);
-		if(fidx<0)return point(0, 0);
-		loc_point&f=m_d[fidx];
+		int fidx = find_first(0);
+		if(fidx < 0)
+			return point(0, 0);
+		loc_point& f = m_d[fidx];
 		return point(f.m_sol[0].x + dist * m_simple_rec_kx, f.m_sol[0].y + dist * m_simple_rec_ky);
 	}
 
@@ -546,10 +571,13 @@ struct car_point_filter:select_point_object
 		}
 	}
 
-	double convert_pt_to_dist(point &pt, const site*sit)
+	/*
+	 * 转换点为距离
+	 * */
+	double convert_pt_to_dist(point &pt, const site* sit)
 	{
-		double dist=0;
-		int fidx=find_first(0);
+		double dist = 0;
+		int fidx = find_first(0);
 		if(fidx>=0)
 		{
 			loc_point&f=m_d[fidx];		
@@ -674,6 +702,7 @@ struct car_point_filter:select_point_object
 		return flag; 
 	}
 
+	// 检测掉头或转弯
 	void detect_turning(point &mpt, const site*sit)
 	{
 		if(m_if_turning)return;
@@ -735,6 +764,7 @@ struct car_point_filter:select_point_object
 			}
 		}
 	}
+	// 计算掉头角度
 	double calc_turning_angle(point &a, point &b)
 	{
 		if(fabs(a.x-b.x)<0.001)
@@ -749,19 +779,20 @@ struct car_point_filter:select_point_object
 	}
 
 };
+
 //---------------------smooth
 struct smooth_tool
 {
-	std::unique_ptr<select_tool>&m_st;
-	bool smooth_initial_setting;
-	double smooth_speed;
+	std::unique_ptr<select_tool>& m_st;	// 选择工具类
+	bool smooth_initial_setting;		// 平滑初始化状态
+	double smooth_speed;				// 平滑速度
 	double smooth_speed_presentation;
 	int smooth_speed_presentation_cnt;
-	point smooth_last_position;
-	double smooth_last_time_sec;
-	point smooth_last_true_position;
+	point smooth_last_position;			// 上一个平滑位置
+	double smooth_last_time_sec;		// 上一个平滑时间
+	point smooth_last_true_position;	// 上一个真实位置
 
-	line smooth_line;
+	line smooth_line;					// 平滑线
 	bool smooth_line_reset=false;	//if line reset
 
 	bool smooth_halt_condition;	//if halting
@@ -781,6 +812,7 @@ struct smooth_tool
 		 ,smooth_halt_condition(0)
 		 ,smooth_halt_count(0)
 	{}
+	
 	void smooth_set_loc_point(double t, int ct, const site*sit, loc_point *lp)
 	{
 		point pt;
@@ -789,11 +821,12 @@ struct smooth_tool
 		else
 			pt.set(smooth_last_position);
 
-		lp->m_dist2=sit->dist_direct(pt);
-		lp->m_smooth_x=pt.x;
-		lp->m_smooth_y=pt.y;
-		lp->m_time=(int64_t)(t*1000);
-		lp->m_ct=ct;
+		lp->m_dist2 = sit->dist_direct(pt);
+		lp->m_smooth_x = pt.x;
+		lp->m_smooth_y = pt.y;
+		lp->m_time = (int64_t)(t*1000);
+		lp->m_ct = ct;
+		
 		if(smooth_halt_condition)
 		{
 			lp->m_speed=0;
@@ -801,8 +834,8 @@ struct smooth_tool
 		}
 		else
 		{
-			lp->m_speed=smooth_speed_presentation * (3.6*sit->m_scale) ; //(m/s) to (km/h)
-			if(smooth_speed<0)
+			lp->m_speed = smooth_speed_presentation * (3.6*sit->m_scale) ; //(m/s) to (km/h)
+			if(smooth_speed < 0)
 				lp->m_speed = -lp->m_speed;
 			if(std::isnan(lp->m_speed))
 				lp->m_speed=0;
@@ -812,15 +845,16 @@ struct smooth_tool
 
 	void smooth_reset()
 	{
-		smooth_initial_setting=false;
-		smooth_speed=0;							//smoothed speed
-		smooth_speed_presentation_cnt=0;
-		smooth_last_position=point(0,0);		//last position of smoothed point
-		smooth_last_true_position=point(0,0);	//last position of true point
-		smooth_last_time_sec=0;					//last time second
+		smooth_initial_setting = false;
+		smooth_speed = 0;							//smoothed speed
+		smooth_speed_presentation_cnt = 0;
+		smooth_last_position = point(0,0);		//last position of smoothed point
+		smooth_last_true_position = point(0,0);	//last position of true point
+		smooth_last_time_sec = 0;					//last time second
 		smooth_halt_condition = false;
-		smooth_halt_count=0;
+		smooth_halt_count = 0;
 	}
+	
 	bool smooth_initiate(point &pt, double t, const site*sit)
 	{
 		smooth_initial_setting=true;
@@ -836,6 +870,7 @@ struct smooth_tool
 		smooth_halt_position_minus=pt;
 		return true;
 	}
+	
 	virtual void set(point &pt,loc_point &lp)=0;
 	loc_point smooth_strategy()
 	{
@@ -845,20 +880,20 @@ struct smooth_tool
 			return lp;
 		point pt;
 		pt.set(dpt);
-		double current_t=time(NULL);
+		double current_t = time(NULL);
 		const auto & sit = sit_list::instance()->get(dpt.lp.m_sid);
 		if(dpt.valid)
 		{
 			smooth_dist(pt, current_t, dpt.ct, sit.get(), dpt.dstp, &lp);
 			set(pt,lp);
 
-			dpt.lp.m_dist2=lp.m_dist2;
-			dpt.lp.m_time=lp.m_time;
-			dpt.lp.m_ct=lp.m_ct;	
+			dpt.lp.m_dist2 = lp.m_dist2;
+			dpt.lp.m_time = lp.m_time;
+			dpt.lp.m_ct = lp.m_ct;	
 			dpt.lp.set(lp);
-			dpt.lp.m_dist=sit->dist_direct(dpt);
-			dpt.lp.m_speed=lp.m_speed;
-			dpt.lp.m_stat=lp.m_stat;
+			dpt.lp.m_dist = sit->dist_direct(dpt);
+			dpt.lp.m_speed = lp.m_speed;
+			dpt.lp.m_stat = lp.m_stat;
 			dpt.lp.debug_out("get_point");
 		}
 		else
@@ -866,16 +901,17 @@ struct smooth_tool
 			smooth_set_loc_point(current_t, dpt.ct, sit.get(), &lp);
 			if(dpt.stop_cnt<=0)
 			{
-				lp.m_speed=0;
-				lp.m_stat=0;
+				lp.m_speed = 0;
+				lp.m_stat = 0;
 			}
-			set(pt,lp);
+			set(pt, lp);
 		}
 		return lp;
 	}
 	virtual void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)=0;
 	virtual ~smooth_tool(){}
 };
+
 struct smooth_tool_person_1:smooth_tool
 {
 	smooth_tool_person_1(std::unique_ptr<select_tool>&m)