test filterByPosition

apps/function/multi_traj_functions.cpp

@@ -2,6 +2,7 @@
 #include <pcl/octree/octree.h>
 
 using namespace boost::filesystem;
+const float Squared30 = 30 * 30;
 
 vector<string> getActiveTraces(
         const string &activeTracePath)
@@ -213,13 +214,16 @@ void filterByPosition(
                     auto tarPeriod = tarOneRet.periods.at(tarPeriodIndex);
                     PointCloudExport tarPeriodCloud =
                             getCloudFromPeriod(pointClouds.at(tarOneRet.index), tarPeriod);
+                    uint32_t hitCnt = 0;
                     for(const PointExport& query : tarPeriodCloud.points){
                         int result_index;
                         float sqr_distance;
                         octree.approxNearestSearch(query, result_index, sqr_distance);
-                        if(sqrt(sqr_distance) < 30){
+                        if(sqr_distance < Squared30){
+                            hitCnt++;
+                        }
+                        if(hitCnt / tarPeriodCloud->size() < 0.8){
                             aroundIndexVec.push_back(make_pair(tarRetIndex, tarPeriodIndex));
-                            break;
                         }
                     }
                 }