|
@@ -45,7 +45,7 @@ void obs_mobieye::InterpolationTrace(std::vector<iv::Point2D> *pgpsTrace, std::v
|
|
|
double fdis;
|
|
|
fdis = sqrt(pow(xgpstrace[k].x - xgpstrace[k+1].x,2)
|
|
|
+pow(xgpstrace[k].y - xgpstrace[k+1].y,2));
|
|
|
- if(fdis > 0.3)
|
|
|
+ if(fdis > 0.16)
|
|
|
{
|
|
|
qDebug("fdis %f is big, insert a point2d ",fdis);
|
|
|
iv::Point2D xpoint;
|
|
@@ -75,15 +75,15 @@ int obs_mobieye::GetCandidataObs(std::vector<iv::Point2D> &xgpstrace, iv::mobile
|
|
|
int index = -1;
|
|
|
for(j=0;j<ntracecount;j++)
|
|
|
{
|
|
|
- if(fabs(pobs->pos_x() - xgpstrace[j].y) > 10.0)
|
|
|
+ if(fabs((pobs->pos_x()*(-1)) - xgpstrace[j].y) > 10.0)
|
|
|
{
|
|
|
continue;
|
|
|
}
|
|
|
- if(fabs(pobs->pos_x() - xgpstrace[j].x) > 10.0)
|
|
|
+ if(fabs(pobs->pos_x() - xgpstrace[j].y) > 10.0)
|
|
|
{
|
|
|
continue;
|
|
|
}
|
|
|
- double fdis = sqrt(pow(xgpstrace[j].x - pobs->pos_y(),2)
|
|
|
+ double fdis = sqrt(pow(xgpstrace[j].x - (pobs->pos_y()*(-1.0)),2)
|
|
|
+pow(xgpstrace[j].y - pobs->pos_x(),2));
|
|
|
if(fdis<fdismin)
|
|
|
{
|
|
@@ -112,7 +112,7 @@ bool obs_mobieye::PointInObs(iv::mobileye::obs *pobs, iv::Point2D xpoint)
|
|
|
x = xpoint.x;
|
|
|
y = xpoint.y;
|
|
|
double rel_x,rel_y;
|
|
|
- rel_x = x - pobs->pos_y();
|
|
|
+ rel_x = x - (pobs->pos_y()*(-1));
|
|
|
rel_y = y - pobs->pos_x();
|
|
|
double beta = pobs->obsang() *M_PI/180.0;
|
|
|
double xrot,yrot;
|