|
|
@@ -1,5 +1,6 @@
|
|
|
#ifndef __SELECT_TOOL__H
|
|
|
#define __SELECT_TOOL__H
|
|
|
+#include <mutex>
|
|
|
#include "loc_point.h"
|
|
|
#include "line.h"
|
|
|
#include <memory>
|
|
|
@@ -32,72 +33,137 @@ struct solpoint:point
|
|
|
return m_score;
|
|
|
}
|
|
|
};
|
|
|
+struct push_data_point:point
|
|
|
+{
|
|
|
+ const site *sit;
|
|
|
+ point dstp;
|
|
|
+ int ct;
|
|
|
+ bool valid; // if valid
|
|
|
+ int stop_cnt; // if calculate speed
|
|
|
+ loc_point lp;
|
|
|
+};
|
|
|
+
|
|
|
+class select_point_object;
|
|
|
+struct select_tool
|
|
|
+{
|
|
|
+ std::mutex m_mtx;
|
|
|
+ zlist<push_data_point,16> m_push_list;
|
|
|
+ select_point_object *m_spo=nullptr;
|
|
|
+ virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)=0;
|
|
|
+ push_data_point getDpt()
|
|
|
+ {
|
|
|
+ push_data_point dpt;
|
|
|
+ std::lock_guard<std::mutex> lock(m_mtx);
|
|
|
+ if (m_push_list.empty()) //empty
|
|
|
+ {
|
|
|
+ return dpt;
|
|
|
+ }
|
|
|
+ else if(m_push_list.size()==1) //size=1
|
|
|
+ {
|
|
|
+ dpt = m_push_list[0];
|
|
|
+ m_push_list[0].valid=false;
|
|
|
+ if(m_push_list[0].stop_cnt>5)m_push_list[0].stop_cnt=5;
|
|
|
+ if(m_push_list[0].stop_cnt>0)m_push_list[0].stop_cnt--;
|
|
|
+ }
|
|
|
+ else{ //size>1
|
|
|
+ dpt = m_push_list[0];
|
|
|
+ m_push_list.skip(1);
|
|
|
+ }
|
|
|
+ return dpt;
|
|
|
+ }
|
|
|
+ virtual ~select_tool();
|
|
|
|
|
|
+};
|
|
|
|
|
|
-struct select_point_object
|
|
|
+//--------------person------solution one--------
|
|
|
+struct select_tool_person_1:select_tool
|
|
|
{
|
|
|
-public:
|
|
|
- select_point_object()
|
|
|
- :m_ct(-10)
|
|
|
- {
|
|
|
- att_initiate();
|
|
|
- }
|
|
|
- inline int last_ct(){return m_ct;}
|
|
|
-public:
|
|
|
- const static int max_histime=60; //±£Áô×îºó60sµÄÊý¾Ý
|
|
|
-
|
|
|
- zlist<loc_point,128> m_d;
|
|
|
- line m_line;
|
|
|
- int m_ct = -10;
|
|
|
- fit_batch m_fitk,m_fita;
|
|
|
- fit_result m_cur_fit;
|
|
|
- loc_point* m_begin;
|
|
|
- loc_point* m_last;
|
|
|
- struct push_data_point:point
|
|
|
+ virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm);
|
|
|
+ ~select_tool_person_1()
|
|
|
+ {
|
|
|
+ }
|
|
|
+};
|
|
|
+
|
|
|
+struct select_tool_person_2:select_tool
|
|
|
+{
|
|
|
+ virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)
|
|
|
{
|
|
|
- const site *sit;
|
|
|
- point dstp;
|
|
|
- int ct;
|
|
|
- bool valid; // if valid
|
|
|
- int stop_cnt; // if calculate speed
|
|
|
loc_point lp;
|
|
|
- };
|
|
|
- zlist<push_data_point,16> m_push_list;
|
|
|
+ return lp;
|
|
|
+ }
|
|
|
|
|
|
-public:
|
|
|
- int find_last(int start);
|
|
|
- int find_first(int start);
|
|
|
- void save_k();
|
|
|
- void att_initiate();
|
|
|
- void remove_history();
|
|
|
- bool make_line();
|
|
|
- point select_solution0(std::vector<point> &vp,const double scale);
|
|
|
- bool select_solution(const std::vector<point> &vp,const site*sit,loc_point &p);
|
|
|
- loc_point select_solution_impl(const std::vector<point> p,const std::vector<loc_message>&lm);
|
|
|
- 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);
|
|
|
- void select_one_ant(loc_point &c,const std::vector<point> & vp);
|
|
|
-public:
|
|
|
- virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)=0;
|
|
|
- virtual fit_result * get_best_fit()=0;
|
|
|
- virtual double getA(const fit_result * fit,const double scale,const double dt)=0;
|
|
|
- virtual void reset_fit(double d)=0;
|
|
|
- virtual bool revise_by_history(point & pt, const site*sit, int64_t m_time)=0;
|
|
|
- virtual ~select_point_object(){}
|
|
|
+};
|
|
|
+//----------------------car----------
|
|
|
+struct select_tool_car_1:select_tool
|
|
|
+{
|
|
|
+ virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm);
|
|
|
+};
|
|
|
+//---------------------drivingfaceCar
|
|
|
+struct select_tool_drivingface_car_1:select_tool
|
|
|
+{
|
|
|
+ virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)
|
|
|
+ {
|
|
|
+ loc_point lp;
|
|
|
+ return lp;
|
|
|
+ }
|
|
|
+};
|
|
|
+//----------------------------------------
|
|
|
+struct select_point_object
|
|
|
+{
|
|
|
+ select_tool * m_owner;
|
|
|
+ public:
|
|
|
+ select_point_object(select_tool * owner)
|
|
|
+ :m_owner(owner)
|
|
|
+ ,m_ct(-10)
|
|
|
+ {
|
|
|
+ att_initiate();
|
|
|
+ }
|
|
|
+ inline int last_ct(){return m_ct;}
|
|
|
+ public:
|
|
|
+ const static int max_histime=60; //±£Áô×îºó60sµÄÊý¾Ý
|
|
|
+
|
|
|
+ zlist<loc_point,128> m_d;
|
|
|
+ line m_line;
|
|
|
+ int m_ct = -10;
|
|
|
+ fit_batch m_fitk,m_fita;
|
|
|
+ fit_result m_cur_fit;
|
|
|
+ loc_point* m_begin;
|
|
|
+ loc_point* m_last;
|
|
|
+ public:
|
|
|
+ int find_last(int start);
|
|
|
+ int find_first(int start);
|
|
|
+ void save_k();
|
|
|
+ void att_initiate();
|
|
|
+ void remove_history();
|
|
|
+ bool make_line();
|
|
|
+ point select_solution0(std::vector<point> &vp,const double scale);
|
|
|
+ bool select_solution(const std::vector<point> &vp,const site*sit,loc_point &p);
|
|
|
+ loc_point select_solution_impl(const std::vector<point> p,const std::vector<loc_message>&lm);
|
|
|
+ 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);
|
|
|
+ void select_one_ant(loc_point &c,const std::vector<point> & vp);
|
|
|
+ public:
|
|
|
+ virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)=0;
|
|
|
+ virtual fit_result * get_best_fit()=0;
|
|
|
+ virtual double getA(const fit_result * fit,const double scale,const double dt)=0;
|
|
|
+ virtual void reset_fit(double d)=0;
|
|
|
+ virtual bool revise_by_history(point & pt, const site*sit, int64_t m_time)=0;
|
|
|
+ virtual ~select_point_object(){}
|
|
|
};
|
|
|
struct person_point_filter:select_point_object
|
|
|
{
|
|
|
- person_point_filter()
|
|
|
- :m_filter_man_count(0)
|
|
|
- {}
|
|
|
+ person_point_filter(select_tool * owner)
|
|
|
+ :select_point_object(owner)
|
|
|
+ ,m_filter_man_count(0)
|
|
|
+ {}
|
|
|
int m_filter_man_count=0;
|
|
|
- virtual void reset_fit(double d)
|
|
|
- {
|
|
|
- if(d>1)
|
|
|
- m_filter_man_count++;
|
|
|
+ virtual void reset_fit(double d)
|
|
|
+ {
|
|
|
+ if(d>1)
|
|
|
+ m_filter_man_count++;
|
|
|
else
|
|
|
- m_filter_man_count = 0;
|
|
|
+ m_filter_man_count = 0;
|
|
|
if(m_filter_man_count==3)
|
|
|
{
|
|
|
m_fitk.reset_data();
|
|
|
@@ -107,44 +173,44 @@ struct person_point_filter:select_point_object
|
|
|
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))
|
|
|
- {
|
|
|
- c.inc_cl(40);
|
|
|
- }
|
|
|
- else if(c.cl()>0 && vp.size()==2)
|
|
|
- {
|
|
|
- c[0]=vp[0];
|
|
|
- c[1]=vp[1];
|
|
|
- c.inc_cl(10);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- select_one_ant(c,vp);
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
- 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;
|
|
|
+ }
|
|
|
+ virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)
|
|
|
+ {
|
|
|
+ if(filter_by_fit(c,vp,scale))
|
|
|
+ {
|
|
|
+ c.inc_cl(40);
|
|
|
+ }
|
|
|
+ else if(c.cl()>0 && vp.size()==2)
|
|
|
+ {
|
|
|
+ c[0]=vp[0];
|
|
|
+ c[1]=vp[1];
|
|
|
+ c.inc_cl(10);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ select_one_ant(c,vp);
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ 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;
|
|
|
if(fabs(fit->k)<2/(3.6*scale) && fabs(fit->k)>0.5/(3.6*scale) && dt<10)
|
|
|
{
|
|
|
a=0.3;
|
|
|
}
|
|
|
- return a;
|
|
|
- }
|
|
|
+ return a;
|
|
|
+ }
|
|
|
|
|
|
- virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
|
|
|
- {
|
|
|
- point dstp = sit->get_dstp(pt);
|
|
|
- if(dstp.empty())
|
|
|
- std_error("error.here....");
|
|
|
+ virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
|
|
|
+ {
|
|
|
+ point dstp = sit->get_dstp(pt);
|
|
|
+ if(dstp.empty())
|
|
|
+ std_error("error.here....");
|
|
|
|
|
|
push_data_point dp;
|
|
|
dp.sit=sit;
|
|
|
@@ -155,29 +221,32 @@ struct person_point_filter:select_point_object
|
|
|
dp.lp=m_d(0);
|
|
|
dp.set(pt);
|
|
|
{ //don't delete the braces
|
|
|
- m_push_list.clear();
|
|
|
- m_push_list.push(dp);
|
|
|
+ std::lock_guard<std::mutex> lock(m_owner->m_mtx);
|
|
|
+ m_owner->m_push_list.clear();
|
|
|
+ m_owner->m_push_list.push(dp);
|
|
|
+ //dp.lp.debug_out("person");
|
|
|
}
|
|
|
- return true;
|
|
|
- }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
|
|
|
};
|
|
|
struct car_point_filter:select_point_object
|
|
|
{
|
|
|
- car_point_filter()
|
|
|
- :m_last_fit_valid(false)
|
|
|
- ,m_last_fit_k(0)
|
|
|
- ,m_last_fit_kb(0)
|
|
|
- ,m_last_fit_ka(0)
|
|
|
- ,m_last_fit_xo(0)
|
|
|
- ,m_last_fit_yo(0)
|
|
|
- ,m_last_fit_md_x(0)
|
|
|
- ,m_last_fit_md_y(0)
|
|
|
- ,m_last_fit_time_sec(0)
|
|
|
- ,m_last_fit_nearest_time_sec(0)
|
|
|
- ,m_last_time_sec(0)
|
|
|
- ,m_if_turning(false)
|
|
|
- {}
|
|
|
+ car_point_filter(select_tool * owner)
|
|
|
+ :select_point_object(owner)
|
|
|
+ ,m_last_fit_valid(false)
|
|
|
+ ,m_last_fit_k(0)
|
|
|
+ ,m_last_fit_kb(0)
|
|
|
+ ,m_last_fit_ka(0)
|
|
|
+ ,m_last_fit_xo(0)
|
|
|
+ ,m_last_fit_yo(0)
|
|
|
+ ,m_last_fit_md_x(0)
|
|
|
+ ,m_last_fit_md_y(0)
|
|
|
+ ,m_last_fit_time_sec(0)
|
|
|
+ ,m_last_fit_nearest_time_sec(0)
|
|
|
+ ,m_last_time_sec(0)
|
|
|
+ ,m_if_turning(false)
|
|
|
+ {}
|
|
|
bool m_last_fit_valid = false;
|
|
|
double m_last_fit_k ;
|
|
|
double m_last_fit_kb;
|
|
|
@@ -193,60 +262,60 @@ struct car_point_filter:select_point_object
|
|
|
bool m_if_turning=false;
|
|
|
point m_turning_pt;
|
|
|
point m_turning_ept;
|
|
|
- double m_simple_rec_kx,m_simple_rec_ky;
|
|
|
+ double m_simple_rec_kx,m_simple_rec_ky;
|
|
|
const double m_fit_differ=4;
|
|
|
const double m_pos_differ=8;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
- virtual void reset_fit(double d){}
|
|
|
- virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)
|
|
|
- {
|
|
|
- //two ants.
|
|
|
- if(c.cl()>0 && vp.size()==2)//m_card->smoothFlag() )
|
|
|
- {
|
|
|
- c[0]=vp[0];
|
|
|
- c[1]=vp[1];
|
|
|
- c.inc_cl(50);
|
|
|
- }
|
|
|
- else if(filter_by_fit(c,vp,scale))
|
|
|
- {
|
|
|
- c.inc_cl(40);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- select_one_ant(c,vp);
|
|
|
- }
|
|
|
-
|
|
|
+ virtual void reset_fit(double d){}
|
|
|
+ virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)
|
|
|
+ {
|
|
|
+ //two ants.
|
|
|
+ if(c.cl()>0 && vp.size()==2)//m_card->smoothFlag() )
|
|
|
+ {
|
|
|
+ c[0]=vp[0];
|
|
|
+ c[1]=vp[1];
|
|
|
+ c.inc_cl(50);
|
|
|
+ }
|
|
|
+ else if(filter_by_fit(c,vp,scale))
|
|
|
+ {
|
|
|
+ c.inc_cl(40);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ select_one_ant(c,vp);
|
|
|
+ }
|
|
|
|
|
|
- }
|
|
|
+
|
|
|
+ }
|
|
|
|
|
|
const fit_result* best_fit()const
|
|
|
{
|
|
|
if(m_cur_fit.k==0 && m_cur_fit.ke==0)
|
|
|
- return nullptr;
|
|
|
+ return nullptr;
|
|
|
|
|
|
return &m_cur_fit;
|
|
|
}
|
|
|
|
|
|
- virtual fit_result * get_best_fit()
|
|
|
- {
|
|
|
- return best_fit_raw(5);
|
|
|
- }
|
|
|
- virtual double getA(const fit_result * fit,const double scale,const double dt)
|
|
|
- {
|
|
|
- double a=2.5;
|
|
|
+ virtual fit_result * get_best_fit()
|
|
|
+ {
|
|
|
+ return best_fit_raw(5);
|
|
|
+ }
|
|
|
+ virtual double getA(const fit_result * fit,const double scale,const double dt)
|
|
|
+ {
|
|
|
+ double a=2.5;
|
|
|
if(fabs(fit->k)<10/(3.6*scale) && fabs(fit->k)>1/(3.6*scale) && dt<20)
|
|
|
{
|
|
|
a=1;
|
|
|
}
|
|
|
if(fabs(fit->k)<=1/(3.6*scale) && dt<20)
|
|
|
{
|
|
|
- a=-1;
|
|
|
+ a=-1;
|
|
|
}
|
|
|
- return a;
|
|
|
- }
|
|
|
+ return a;
|
|
|
+ }
|
|
|
|
|
|
void reset_turning()
|
|
|
{
|
|
|
@@ -271,9 +340,9 @@ struct car_point_filter:select_point_object
|
|
|
|
|
|
point sit_location; //projection
|
|
|
//sit_location.set(sit->x,sit->y);
|
|
|
- sit_location = sit->get_dstp(m_turning_pt);
|
|
|
- if(sit_location.empty())
|
|
|
- std_error("get_dstp point error.....");
|
|
|
+ sit_location = sit->get_dstp(m_turning_pt);
|
|
|
+ if(sit_location.empty())
|
|
|
+ std_error("get_dstp point error.....");
|
|
|
double dist1=sit_location.dist(rpt);
|
|
|
double dist2=sit_location.dist(m_turning_pt);
|
|
|
double dist3=m_turning_pt.dist(rpt); // dist1 is supposed to be = dist2+dist3
|
|
|
@@ -295,7 +364,7 @@ struct car_point_filter:select_point_object
|
|
|
|
|
|
|
|
|
printf("turning mapping:(%.2f,%.2f)->(%.2f,%.2f),d1:%f,d2:%f,d3:%f\n",
|
|
|
- rpt.x,rpt.y,turning_x,turning_y,dist1,dist2,dist3);
|
|
|
+ rpt.x,rpt.y,turning_x,turning_y,dist1,dist2,dist3);
|
|
|
|
|
|
rpt.set(turning_x,turning_y);
|
|
|
}
|
|
|
@@ -311,9 +380,9 @@ struct car_point_filter:select_point_object
|
|
|
{
|
|
|
lp.debug_out();
|
|
|
printf("out of path:t=%ld,sit=%d,card=l,ct=%d,"
|
|
|
- "tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
|
|
|
- m_d(0).m_time, m_d(0).m_sid,m_d(0).m_ct,
|
|
|
- m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
|
|
|
+ "tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
|
|
|
+ m_d(0).m_time, m_d(0).m_sid,m_d(0).m_ct,
|
|
|
+ m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
@@ -321,9 +390,9 @@ struct car_point_filter:select_point_object
|
|
|
if(!m_if_turning)
|
|
|
{
|
|
|
//dstp.set(sit->x,sit->y);
|
|
|
- dstp = sit->get_dstp(pt);
|
|
|
- if(dstp.empty())
|
|
|
- std_error("error.here....");
|
|
|
+ dstp = sit->get_dstp(pt);
|
|
|
+ if(dstp.empty())
|
|
|
+ std_error("error.here....");
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
@@ -339,8 +408,10 @@ struct car_point_filter:select_point_object
|
|
|
dp.lp=lp;
|
|
|
dp.set(rpt);
|
|
|
{ //don't delete the braces
|
|
|
- m_push_list.clear();
|
|
|
- m_push_list.push(dp);
|
|
|
+ std::lock_guard<std::mutex> lock(m_owner->m_mtx);
|
|
|
+ m_owner->m_push_list.clear();
|
|
|
+ m_owner->m_push_list.push(dp);
|
|
|
+ //dp.lp.debug_out("single point .");
|
|
|
}
|
|
|
}
|
|
|
double estimate_point_by_history(const site*sit, double m_time_sec)
|
|
|
@@ -378,18 +449,18 @@ struct car_point_filter:select_point_object
|
|
|
{
|
|
|
lp.debug_out();
|
|
|
printf("out of path:t=%ld,sit=%d,card=0,ct=%d,"
|
|
|
- "tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
|
|
|
- m_d(0).m_time, m_d(0).m_sid,m_d(0).m_ct,
|
|
|
- m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
|
|
|
+ "tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
|
|
|
+ m_d(0).m_time, m_d(0).m_sid,m_d(0).m_ct,
|
|
|
+ m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
point dstp; //projection
|
|
|
if(!m_if_turning)
|
|
|
{
|
|
|
- dstp = sit->get_dstp(pt);
|
|
|
- if(dstp.empty())
|
|
|
- std_error("error.here....");
|
|
|
+ dstp = sit->get_dstp(pt);
|
|
|
+ if(dstp.empty())
|
|
|
+ std_error("error.here....");
|
|
|
//dstp.set(sit->x,sit->y);
|
|
|
}
|
|
|
else
|
|
|
@@ -398,7 +469,7 @@ struct car_point_filter:select_point_object
|
|
|
}
|
|
|
|
|
|
int size = 0;
|
|
|
- push_data_point dp[10];
|
|
|
+ push_data_point dp[13];
|
|
|
dp[size].sit=sit;
|
|
|
dp[size].dstp.set(dstp);
|
|
|
dp[size].ct=ct;
|
|
|
@@ -408,7 +479,7 @@ struct car_point_filter:select_point_object
|
|
|
dp[size].set(rpt);
|
|
|
double missing_time = m_d(0).m_time/1000.;
|
|
|
size++;
|
|
|
- for(;size<10;size++)
|
|
|
+ for(;size<13;size++)
|
|
|
{
|
|
|
dp[size].sit=sit;
|
|
|
dp[size].dstp.set(dstp);
|
|
|
@@ -431,11 +502,13 @@ struct car_point_filter:select_point_object
|
|
|
dp[size].lp.m_sid = sit->m_id;
|
|
|
//dp[size].lp.m_cid = m_d(0).m_cid;
|
|
|
}
|
|
|
- {
|
|
|
- m_push_list.clear();
|
|
|
+ {
|
|
|
+ std::lock_guard<std::mutex> lock(m_owner->m_mtx);
|
|
|
+ m_owner->m_push_list.clear();
|
|
|
for(int i=0;i<size;i++)
|
|
|
{
|
|
|
- m_push_list.push(dp[i]);
|
|
|
+ m_owner->m_push_list.push(dp[i]);
|
|
|
+ //dp[i].lp.debug_out("push_list");
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
@@ -460,7 +533,7 @@ struct car_point_filter:select_point_object
|
|
|
m_simple_rec_kx = 0;
|
|
|
m_simple_rec_ky = 0;
|
|
|
}
|
|
|
- //printf("convert_pt_to_dist:(%f,%f),%f\n",pt.x,pt.y,dist);
|
|
|
+ //printf("convert_pt_to_dist:(%f,%f),%f\n",pt.x,pt.y,dist);
|
|
|
//double dist = sit->dist_direct(pt);
|
|
|
//if(dist == 0)return 0;
|
|
|
//m_simple_rec_kx = (pt.x - (*sit).x) / dist;
|
|
|
@@ -468,13 +541,11 @@ struct car_point_filter:select_point_object
|
|
|
return dist;
|
|
|
}
|
|
|
|
|
|
- virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
|
|
|
- {
|
|
|
- std_info("revise_____-before:%f,%f,%llu",pt.x,pt.y,timestamp);
|
|
|
- bool flag =false;
|
|
|
+ virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
|
|
|
+ {
|
|
|
+ bool flag =false;
|
|
|
if(m_line.empty() || !m_line.contain(m_d(0),0.1))
|
|
|
{
|
|
|
- std_info("m_line .empty() ......");
|
|
|
m_last_fit_valid = false;
|
|
|
m_last_fit_time_sec = 0;
|
|
|
m_last_fit_k = 0;
|
|
|
@@ -504,17 +575,13 @@ struct car_point_filter:select_point_object
|
|
|
// choose data by fit
|
|
|
const fit_result*fit=best_fit();
|
|
|
bool if_change_fit=false;
|
|
|
- if(fit !=nullptr)
|
|
|
- std_info("fit != nullptr...m_time_sec:%f,fitK.x():%f",m_time_sec,m_fitk.x(0));
|
|
|
- else
|
|
|
- std_info("fit ==nullptr...");
|
|
|
if(fit!=nullptr && fit->ke<=1 && m_time_sec - m_fitk.x(0) <= 15 && fabs(fit->k) < m_pos_differ)
|
|
|
{ //printf("change fit time:%f,%f,%f\n",m_time_sec, fit->d.x(0), m_time_sec - fit->d.x(0));
|
|
|
-
|
|
|
+
|
|
|
// put m_acccumulate_acc into consideration
|
|
|
// fit->k - m_last_fit_k < m_accumulate_acc
|
|
|
if(m_last_fit_valid == true && m_last_fit_k * fit->k > -0.6)
|
|
|
- //if((sit->dist(pt)<20 ||m_last_fit_k * fit->k > -0.6))
|
|
|
+ //if((sit->dist(pt)<20 ||m_last_fit_k * fit->k > -0.6))
|
|
|
{ //if point is too near the sit: do not not judge the backwards
|
|
|
|
|
|
double est1 = estimate_point_by_history(sit, m_last_fit_time_sec);
|
|
|
@@ -523,10 +590,10 @@ struct car_point_filter:select_point_object
|
|
|
// m_last_fit_nearest_time_sec,est1,m_time_sec,est2,m_time_sec-m_last_fit_nearest_time_sec,est2-est1);
|
|
|
//if(fabs(est1-est2)>40)printf("change fit:%f,%f,%f\n",fabs(est1-est2),est1,est2);
|
|
|
if(fabs(est1-est2)< (m_time_sec - m_last_fit_time_sec) * 5) // large jump is not allowed
|
|
|
- if_change_fit=true;
|
|
|
+ if_change_fit=true;
|
|
|
}
|
|
|
else if(m_last_fit_valid==false)
|
|
|
- if_change_fit=true;
|
|
|
+ if_change_fit=true;
|
|
|
}
|
|
|
if(if_change_fit)
|
|
|
{
|
|
|
@@ -538,7 +605,7 @@ struct car_point_filter:select_point_object
|
|
|
m_last_fit_xo = fit->xo;
|
|
|
m_last_fit_yo = fit->yo;
|
|
|
m_last_fit_nearest_time_sec = m_fitk.x(0);
|
|
|
-
|
|
|
+
|
|
|
int fidx=find_first(0);
|
|
|
if(fidx<0)
|
|
|
{
|
|
|
@@ -559,17 +626,16 @@ struct car_point_filter:select_point_object
|
|
|
|
|
|
// revise
|
|
|
double estimate_dist = estimate_point_by_history(sit, m_time_sec);
|
|
|
- //printf("revise:est:%f, d:%f, fitvalid:%d, timesecdiffer:%f\n",
|
|
|
- //estimate_dist, dist, m_last_fit_valid, m_time_sec - m_last_fit_time_sec);
|
|
|
+ //printf("revise:est:%f, d:%f, fitvalid:%d, timesecdiffer:%f\n",
|
|
|
+ //estimate_dist, dist, m_last_fit_valid, m_time_sec - m_last_fit_time_sec);
|
|
|
if(m_last_fit_valid && m_time_sec - m_last_fit_time_sec < 20)
|
|
|
{
|
|
|
- std_info("here ....m_time_sec:%llu,m_last_fit_time_sec:%llu",m_time_sec,m_last_fit_time_sec);
|
|
|
if(fabs(m_last_fit_k) > 0.5 && fabs(estimate_dist-dist)>m_fit_differ)
|
|
|
- dist=estimate_dist;
|
|
|
+ dist=estimate_dist;
|
|
|
else if(fabs(m_last_fit_k) <= 0.5 && fabs(estimate_dist-dist)>m_fit_differ * 2)
|
|
|
- dist=estimate_dist;
|
|
|
+ dist=estimate_dist;
|
|
|
else flag = true;
|
|
|
- //m_last_fit_nearest_time_sec = m_time_sec; //need more tests to uncomment this sentence
|
|
|
+ //m_last_fit_nearest_time_sec = m_time_sec; //need more tests to uncomment this sentence
|
|
|
}
|
|
|
else m_last_fit_nearest_time_sec = m_time_sec;
|
|
|
m_last_time_sec = m_time_sec;
|
|
|
@@ -583,19 +649,18 @@ struct car_point_filter:select_point_object
|
|
|
// create the list
|
|
|
//if(m_accumulate_acc<-10)generate(mpt, sit,false); generate single point
|
|
|
if(m_last_fit_valid && timestamp/1000. - m_last_fit_time_sec < 20 && fabs(m_last_fit_k) > 0.5)
|
|
|
- generate_list(mpt, sit, true); //generate the whole list
|
|
|
+ generate_list(mpt, sit, true); //generate the whole list
|
|
|
else
|
|
|
- generate_list(mpt, sit, false); //generate single point
|
|
|
+ generate_list(mpt, sit, false); //generate single point
|
|
|
|
|
|
//turning map
|
|
|
turning_mapping(mpt, sit);
|
|
|
|
|
|
pt = mpt;
|
|
|
- std_info("revise_____-end:%f,%f,useless:%s",pt.x,pt.y,flag?"true":"false");
|
|
|
|
|
|
|
|
|
- return flag;
|
|
|
- }
|
|
|
+ return flag;
|
|
|
+ }
|
|
|
|
|
|
void detect_turning(point &mpt, const site*sit)
|
|
|
{
|
|
|
@@ -626,9 +691,9 @@ struct car_point_filter:select_point_object
|
|
|
|
|
|
double angle1;
|
|
|
if(m_last_fit_k * dist>0)
|
|
|
- angle1=calc_turning_angle(pt1, mpt);
|
|
|
+ angle1=calc_turning_angle(pt1, mpt);
|
|
|
else
|
|
|
- angle1=calc_turning_angle(mpt, pt1);
|
|
|
+ angle1=calc_turning_angle(mpt, pt1);
|
|
|
if(angle1<0)return;
|
|
|
|
|
|
//finding
|
|
|
@@ -643,12 +708,12 @@ struct car_point_filter:select_point_object
|
|
|
if(delta<-180)delta=delta+360;
|
|
|
if(fabs(delta)<5)continue;
|
|
|
if(fabs(delta)>175)continue;
|
|
|
- //printf("angle:%f, delta:%f. mul:%f\n",angle, delta, delta*detect_para);
|
|
|
+ //printf("angle:%f, delta:%f. mul:%f\n",angle, delta, delta*detect_para);
|
|
|
// turning angle must be correct
|
|
|
if(angle*delta>0 && fabs(angle)>fabs(delta)*detect_para)
|
|
|
{
|
|
|
printf("turning:(%.5f,%.5f)(%.5f,%.5f)(%.5f,%.5f),a1:%f,a2:%f,delta:%f,angle:%f\n",
|
|
|
- pt1.x,pt1.y,l.v[0].x,l.v[0].y,l.v[1].x,l.v[1].y,angle1,angle2,delta,angle);
|
|
|
+ pt1.x,pt1.y,l.v[0].x,l.v[0].y,l.v[1].x,l.v[1].y,angle1,angle2,delta,angle);
|
|
|
m_if_turning=true;
|
|
|
m_turning_pt.set(l.v[0]);
|
|
|
m_turning_ept.set(l.v[1]);
|
|
|
@@ -670,54 +735,11 @@ struct car_point_filter:select_point_object
|
|
|
}
|
|
|
|
|
|
};
|
|
|
-struct select_tool
|
|
|
-{
|
|
|
- select_point_object *m_spo=nullptr;
|
|
|
- virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)=0;
|
|
|
- virtual ~select_tool()
|
|
|
- {
|
|
|
- if(m_spo !=nullptr)
|
|
|
- delete m_spo;
|
|
|
- }
|
|
|
-};
|
|
|
-
|
|
|
-//--------------person------solution one--------
|
|
|
-struct select_tool_person_1:select_tool
|
|
|
-{
|
|
|
- virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm);
|
|
|
- ~select_tool_person_1()
|
|
|
- {
|
|
|
- }
|
|
|
-};
|
|
|
-
|
|
|
-struct select_tool_person_2:select_tool
|
|
|
-{
|
|
|
- virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)
|
|
|
- {
|
|
|
- loc_point lp;
|
|
|
- return lp;
|
|
|
- }
|
|
|
-
|
|
|
-};
|
|
|
-//----------------------car----------
|
|
|
-struct select_tool_car_1:select_tool
|
|
|
-{
|
|
|
- virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm);
|
|
|
-};
|
|
|
-//---------------------drivingfaceCar
|
|
|
-struct select_tool_drivingface_car_1:select_tool
|
|
|
-{
|
|
|
- virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)
|
|
|
- {
|
|
|
- loc_point lp;
|
|
|
- return lp;
|
|
|
- }
|
|
|
-};
|
|
|
//---------------------smooth
|
|
|
struct smooth_tool
|
|
|
{
|
|
|
- std::shared_ptr<select_tool> m_st=nullptr;
|
|
|
- bool smooth_initial_setting;
|
|
|
+ std::unique_ptr<select_tool>&m_st;
|
|
|
+ bool smooth_initial_setting;
|
|
|
double smooth_speed;
|
|
|
double smooth_speed_presentation;
|
|
|
int smooth_speed_presentation_cnt;
|
|
|
@@ -735,24 +757,24 @@ struct smooth_tool
|
|
|
point smooth_halt_position_minus;
|
|
|
|
|
|
|
|
|
- smooth_tool()=default;
|
|
|
- smooth_tool(std::shared_ptr<select_tool> st)
|
|
|
- :m_st(st)
|
|
|
- ,smooth_initial_setting(false)
|
|
|
- ,smooth_speed(0)
|
|
|
- ,smooth_speed_presentation(0)
|
|
|
- ,smooth_speed_presentation_cnt(0)
|
|
|
- ,smooth_last_time_sec(0)
|
|
|
- ,smooth_halt_condition(0)
|
|
|
- ,smooth_halt_count(0)
|
|
|
- {}
|
|
|
+ smooth_tool()=default;
|
|
|
+ smooth_tool(std::unique_ptr<select_tool>&st)
|
|
|
+ :m_st(st)
|
|
|
+ ,smooth_initial_setting(false)
|
|
|
+ ,smooth_speed(0)
|
|
|
+ ,smooth_speed_presentation(0)
|
|
|
+ ,smooth_speed_presentation_cnt(0)
|
|
|
+ ,smooth_last_time_sec(0)
|
|
|
+ ,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;
|
|
|
if(smooth_halt_condition)
|
|
|
- pt.set(smooth_halt_position);
|
|
|
+ pt.set(smooth_halt_position);
|
|
|
else
|
|
|
- pt.set(smooth_last_position);
|
|
|
+ pt.set(smooth_last_position);
|
|
|
|
|
|
lp->m_dist2=sit->dist_direct(pt);
|
|
|
lp->m_smooth_x=pt.x;
|
|
|
@@ -768,9 +790,9 @@ struct smooth_tool
|
|
|
{
|
|
|
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;
|
|
|
+ lp->m_speed = -lp->m_speed;
|
|
|
if(std::isnan(lp->m_speed))
|
|
|
- lp->m_speed=0;
|
|
|
+ lp->m_speed=0;
|
|
|
lp->m_stat=1;
|
|
|
//if(fabs(smooth_speed_presentation) < 0.1)
|
|
|
// lp->m_speed=0;
|
|
|
@@ -805,17 +827,54 @@ struct smooth_tool
|
|
|
smooth_halt_position_minus=pt;
|
|
|
return true;
|
|
|
}
|
|
|
-
|
|
|
- virtual void smooth_strategy() = 0;
|
|
|
+ virtual void set(point &pt,loc_point &lp)=0;
|
|
|
+ loc_point smooth_strategy()
|
|
|
+ {
|
|
|
+ loc_point lp;
|
|
|
+ push_data_point dpt = m_st->getDpt();
|
|
|
+ point pt;
|
|
|
+ pt.set(dpt);
|
|
|
+ if(pt.empty())
|
|
|
+ return lp;
|
|
|
+ double current_t=time(NULL);
|
|
|
+ if(dpt.valid)
|
|
|
+ {
|
|
|
+ smooth_dist(pt, current_t, dpt.ct, dpt.sit, 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.set(lp);
|
|
|
+ dpt.lp.m_dist=dpt.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
|
|
|
+ {
|
|
|
+ smooth_set_loc_point(current_t, dpt.ct, dpt.sit, &lp);
|
|
|
+ if(dpt.stop_cnt<=0)
|
|
|
+ {
|
|
|
+ lp.m_speed=0;
|
|
|
+ lp.m_stat=0;
|
|
|
+ }
|
|
|
+ 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::shared_ptr<select_tool> m)
|
|
|
+ smooth_tool_person_1(std::unique_ptr<select_tool>&m)
|
|
|
:smooth_tool(m)
|
|
|
{}
|
|
|
- virtual void smooth_strategy(){}
|
|
|
+ virtual void set(point &pt,loc_point &lp)
|
|
|
+ {
|
|
|
+ lp.set(pt);
|
|
|
+ }
|
|
|
void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)
|
|
|
{
|
|
|
if(smooth_line.empty() || !smooth_line.contain(pt,0.1))
|
|
|
@@ -888,10 +947,13 @@ struct smooth_tool_person_1:smooth_tool
|
|
|
};
|
|
|
struct smooth_tool_car_1:smooth_tool
|
|
|
{
|
|
|
- smooth_tool_car_1(std::shared_ptr<select_tool> m)
|
|
|
+ smooth_tool_car_1(std::unique_ptr<select_tool>&m)
|
|
|
:smooth_tool(m)
|
|
|
{}
|
|
|
- virtual void smooth_strategy(){}
|
|
|
+ virtual void set(point &pt,loc_point &lp)
|
|
|
+ {
|
|
|
+ lp.set(lp.m_smooth_x,lp.m_smooth_y);
|
|
|
+ }
|
|
|
void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)
|
|
|
{
|
|
|
point init_pt(pt.x, pt.y);
|
|
|
@@ -1060,37 +1122,36 @@ struct smooth_tool_car_1:smooth_tool
|
|
|
};
|
|
|
struct smooth_tool_drivingface_car_1:smooth_tool
|
|
|
{
|
|
|
- smooth_tool_drivingface_car_1(std::shared_ptr<select_tool> m)
|
|
|
+ smooth_tool_drivingface_car_1(std::unique_ptr<select_tool>&m)
|
|
|
:smooth_tool(m)
|
|
|
{}
|
|
|
- virtual void smooth_strategy()
|
|
|
- {}
|
|
|
+ virtual void set(point&p,loc_point&lp){}
|
|
|
virtual void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr){}
|
|
|
};
|
|
|
//---------------------------------
|
|
|
struct select_tool_manage
|
|
|
{
|
|
|
- void create_tool(const std::string &s,std::shared_ptr<select_tool> &set,std::shared_ptr<smooth_tool> &smt)
|
|
|
+ void create_tool(const std::string &s,std::unique_ptr <select_tool> &set,std::unique_ptr <smooth_tool> &smt)
|
|
|
{
|
|
|
if(!s.compare(std::string{"person1"}))
|
|
|
{
|
|
|
- set=std::make_shared<select_tool_person_1>();
|
|
|
- smt=std::make_shared<smooth_tool_person_1>(set);
|
|
|
+ set.reset(new select_tool_person_1());
|
|
|
+ smt.reset(new smooth_tool_person_1(set));
|
|
|
}
|
|
|
else if(!s.compare(std::string{"person2"}))
|
|
|
{
|
|
|
- set=std::make_shared<select_tool_person_2>();
|
|
|
- smt=std::make_shared<smooth_tool_person_1>(set);
|
|
|
+ set.reset(new select_tool_person_2());
|
|
|
+ smt.reset(new smooth_tool_person_1(set));
|
|
|
}
|
|
|
else if(!s.compare(std::string{"car1"}))
|
|
|
{
|
|
|
- set=std::make_shared<select_tool_car_1>();
|
|
|
- smt=std::make_shared<smooth_tool_car_1>(set);
|
|
|
+ set.reset(new select_tool_car_1());
|
|
|
+ smt.reset(new smooth_tool_car_1(set));
|
|
|
}
|
|
|
else if(!s.compare(std::string{"drivingface1"}))
|
|
|
{
|
|
|
- set=std::make_shared<select_tool_drivingface_car_1>();
|
|
|
- smt=std::make_shared<smooth_tool_drivingface_car_1>(set);
|
|
|
+ set.reset(new select_tool_drivingface_car_1());
|
|
|
+ smt.reset(new smooth_tool_drivingface_car_1(set));
|
|
|
}
|
|
|
}
|
|
|
static select_tool_manage * instance();
|
zzj