change detection/detection_lidar_cnntogrid direct use pointcloud point.

src/detection/detection_lidar_cnntogrid/main.cpp

@@ -54,6 +54,15 @@ namespace iv {
 }
 
 
+namespace iv {
+
+struct roundpos
+{
+  int x;
+  int y;
+};
+}
+
 static void cnntogird(iv::lidar::objectarray * pobjarray)
 {
     int i;
@@ -63,47 +72,87 @@ static void cnntogird(iv::lidar::objectarray * pobjarray)
 
         iv::lidar::lidarobject * pobj = pobjarray->mutable_obj(i);
 
-        if((fabs(pobj->centroid().x())<0.9)&&(fabs(pobj->centroid().y())<2.3))
-        {
-            continue;
-        }
-        double fdis;
- //       fdis = sqrt(per.location.x*per.location.x + per.location.y*per.location.y);
- //       if((fdis>30)&&(per.size.z<0.5))continue;
-        if((pobj->dimensions().x()>0)&&(pobj->dimensions().y()>0))
+//        if((fabs(pobj->centroid().x())<0.9)&&(fabs(pobj->centroid().y())<2.3))
+//        {
+//            continue;
+//        }
+//        double fdis;
+
+       std::vector<iv::roundpos> xvectorpos;
+//        std::vector<iv::ObstacleBasic> xvectortemp1,xvectortemp2;
+        int j;
+        int npsize = pobj->cloud_size();
+        iv::roundpos rp;
+        for(j=0;j<npsize;j++)
         {
-            int xp = (int)((pobj->dimensions().x()/0.2)/2.0);
-            if(xp == 0)xp = 1;
-            int yp = (int)((pobj->dimensions().y()/0.2)/2.0);
-            if(yp == 0)yp = 1;
-            iv::ObstacleBasic * p;
-      //      p = new iv::ObstacleBasic[xp*yp*4];
-            int ix,iy;
-            for(ix=0;ix<(xp*2);ix++)
+            iv::ObstacleBasic * pobs = new iv::ObstacleBasic;
+
+            pobs->nomal_x = pobj->cloud(j).x();
+            pobs->nomal_y = pobj->cloud(j).y();
+            pobs->nomal_z = pobj->dimensions().z();
+            pobs->speed_x = pobj->velocity_linear_x() * cos(pobj->tyaw());
+            pobs->speed_y = pobj->velocity_linear_x() * sin(pobj->tyaw());
+            int posx = pobs->nomal_x/0.2;
+            int posy = pobs->nomal_y/0.2;
+
+            bool bHaveSame = false;
+            int k;
+            for(k=0;k<xvectorpos.size();k++)
             {
-                for(iy=0;iy<(yp*2);iy++)
+                if((posx == xvectorpos.at(k).x)&&(posy == xvectorpos.at(k).y))
                 {
-    //              iv::ObstacleBasic * pobs = &p[iy*xp*2 +ix];
-                  iv::ObstacleBasic * pobs = new iv::ObstacleBasic;
-                  pobs->nomal_x = ix*0.2 - xp*0.2;
-                  pobs->nomal_y = iy*0.2 - yp*0.2;
-                  pobs->nomal_z = pobj->dimensions().z();
-                  double s,t;
-                  s = pobs->nomal_x*cos(pobj->tyaw()) - pobs->nomal_y * sin(pobj->tyaw());
-                  t = pobs->nomal_x*sin(pobj->tyaw()) + pobs->nomal_y*cos(pobj->tyaw());
-                  pobs->nomal_x = pobj->centroid().x() + s;
-                  pobs->nomal_y = pobj->centroid().y() + t;
-                  pobs->speed_x = pobj->velocity_linear_x() * cos(pobj->tyaw());
-                  pobs->speed_y = pobj->velocity_linear_x() * sin(pobj->tyaw());
-
-                  lidar_obs->push_back(*pobs);
-                  delete pobs;
+                    bHaveSame = true;
+                    break;
                 }
             }
+            if(bHaveSame == false)
+            {
+                rp.x = posx;
+                rp.y = posy;
+                xvectorpos.push_back(rp);
+            }
+            if(bHaveSame == false)lidar_obs->push_back(*pobs);
+            delete pobs;
+        }
+
+ //       fdis = sqrt(per.location.x*per.location.x + per.location.y*per.location.y);
+ //       if((fdis>30)&&(per.size.z<0.5))continue;
+//        if((pobj->dimensions().x()>0)&&(pobj->dimensions().y()>0))
+//        {
+//            int xp = (int)((pobj->dimensions().x()/0.2)/2.0);
+//            if(xp == 0)xp = 1;
+//            int yp = (int)((pobj->dimensions().y()/0.2)/2.0);
+//            if(yp == 0)yp = 1;
+//            iv::ObstacleBasic * p;
+//      //      p = new iv::ObstacleBasic[xp*yp*4];
+//            int ix,iy;
+//            for(ix=0;ix<(xp*2);ix++)
+//            {
+//                for(iy=0;iy<(yp*2);iy++)
+//                {
+
+//    //              iv::ObstacleBasic * pobs = &p[iy*xp*2 +ix];
+//                  iv::ObstacleBasic * pobs = new iv::ObstacleBasic;
+//                  pobs->nomal_x = ix*0.2 - xp*0.2;
+//                  pobs->nomal_y = iy*0.2 - yp*0.2;
+//                  pobs->nomal_z = pobj->dimensions().z();
+//                  double s,t;
+//                  s = pobs->nomal_x*cos(pobj->tyaw()) - pobs->nomal_y * sin(pobj->tyaw());
+//                  t = pobs->nomal_x*sin(pobj->tyaw()) + pobs->nomal_y*cos(pobj->tyaw());
+//                  pobs->nomal_x = pobj->centroid().x() + s;
+//                  pobs->nomal_y = pobj->centroid().y() + t;
+//                  pobs->speed_x = pobj->velocity_linear_x() * cos(pobj->tyaw());
+//                  pobs->speed_y = pobj->velocity_linear_x() * sin(pobj->tyaw());
+
+
+//                  lidar_obs->push_back(*pobs);
+//                  delete pobs;
+//                }
+//            }
 
 
 //            delete p;
-        }
+//        }
 
     }