C++ PointCloud::size方法代码示例

// TODO: add a way to specify the cost as a designed signed distance. 
void RadialPlan::updateNodeCosts(const pcl::PointCloud<pcl::PointXYZ> & pointCloud, Side side, float d_desired, float d_safety) {
#ifdef SAVE_OUTPUT
    FILE *pc = fopen("pc","w");
#endif
    bool ignore_glare_point = false;
    size_t count_glare = 0;
    if (filter_glare) {
        for (unsigned int i=0;i<pointCloud.size();i++) {
            double r = hypot(pointCloud[i].x,pointCloud[i].y);
            if (r < r_glare) {
                count_glare += 1;
            }
        }
        ignore_glare_point = (count_glare>0) && (count_glare < 4);
        ROS_INFO("%d glare point: ignoring %d",(int)count_glare,(int)ignore_glare_point);
    }

    std::vector<float> r_max(n_j,NAN);
    nns.reset();
    unsigned int j = 0, n_useful = pointCloud.size();
    if (ignore_glare_point) {
        n_useful -= count_glare;
    }
   	nns_cloud.resize(2, n_useful);
    for (unsigned int i=0;i<pointCloud.size();i++) {
        double r = hypot(pointCloud[i].x,pointCloud[i].y);
        if (ignore_glare_point && (r < r_glare)) {
            continue;
        }
        assert(j < n_useful);
        nns_cloud(0,j) = pointCloud[i].x;
        nns_cloud(1,j) = pointCloud[i].y;
        float alpha = round(atan2(pointCloud[i].y, pointCloud[i].x) * 2 * ang_range / angle_scale);
        int i_alpha = (int)alpha;
        if (abs(i_alpha) <= ang_range) {
            float r = hypot(pointCloud[i].y,pointCloud[i].x);
            if (isnan(r_max[ang_range + i_alpha]) || (r < r_max[ang_range + i_alpha])) {
                r_max[ang_range + i_alpha] = r;
            }
        }
#ifdef SAVE_OUTPUT
        fprintf(pc,"%e %e %e\n",pointCloud[i].x,pointCloud[i].y,0.0);
#endif
        j += 1;
    }
#ifdef SAVE_OUTPUT
    fclose(pc);
#endif

#ifdef SAVE_OUTPUT
    FILE * bd = fopen("bd", "w");
    float dalpha = angle_scale / (4*ang_range);
    for (int j=0;j<(signed)n_j;j++) {
        float alpha = (j - ang_range)*angle_scale / (2*ang_range);
        fprintf(bd,"%e %e\n%e %e\n%e %e\n",
                r_max[j]*cos(alpha-dalpha), r_max[j]*sin(alpha-dalpha), 
                r_max[j]*cos(alpha), r_max[j]*sin(alpha), 
                r_max[j]*cos(alpha+dalpha), r_max[j]*sin(alpha+dalpha));
    }
    fclose(bd);
#endif


    boost::shared_ptr<NaboFilter> filter(new NaboFilter(nns_cloud, nns_query));
    // Create a kd-tree for M, note that M must stay valid during the lifetime of the kd-tree.
    nns.reset(NNSearchF::createKDTreeLinearHeap(nns_cloud));

    // Look for the nearest neighbours of each query point, 
    // We do not want approximations but we want to sort by the distance,
    nns->knn(nns_query, indices, dists2, 1, 0, 0);
    for (int j=0;j < (signed)n_j; j++) {
        for (unsigned int r = 0; r < n_r; r ++ ) {
            unsigned int ix = r*n_j + j;
            float d = sqrt(dists2(0,ix));
            if (isnan(r_max[j]) || (r * r_scale < r_max[j])) {
                if (d > d_safety) {
                    node_safety(r,j) = 0.0;
                } else {
                    // Adding obstacle repulsion
                    node_safety(r,j) = d_safety/(d+1e-10) - 1;
                }
            } else {
                // Behind the point cloud
                node_safety(r,j) = NAN;
            }
        }
    }

#ifdef SAVE_OUTPUT
    FILE * fp = fopen("nodes","w");
    printf("Node costs\n");
#endif
    for (unsigned int k=0;k<n_k;k++) {
#ifdef SIGNED_DISTANCE
        float beta = (k-conn_range) * angle_scale / (2*ang_range);
        nns->setFilter(filter);
        switch (side) {
            case LEFT:
                filter->setOrientationOffset(beta + M_PI/2.);
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
    if(argc != 8){
      std::cout << "Usage: rosrun ndt_localizer local2global \"filename\" \"x\" \"y\" \"z\" \"roll\" \"pitch\" \"yaw\"" << std::endl;
      exit(1);
    }

    ros::init(argc, argv, "local2global");
    ros::NodeHandle n;

    std::string filename = argv[1];
    std::cout << filename << std::endl;

    if(pcl::io::loadPCDFile<pcl::PointXYZI> (filename, *additional_map_ptr) == -1){
      std::cout << "Couldn't read " << filename << "." << std::endl;
      return(-1);
    }
    std::cout << "Loaded " << additional_map_ptr->size() << " data points from " << filename << "." << std::endl;

    initial_x = std::stod(argv[2]);
    initial_y = std::stod(argv[3]);
    initial_z = std::stod(argv[4]);
    initial_roll = std::stod(argv[5]);
    initial_pitch = std::stod(argv[6]);
    initial_yaw = std::stod(argv[7]);

    std::cout << "initial_pose: "
	      << initial_x << " " << initial_y << " " << initial_z << " "
	      << initial_roll << " " << initial_pitch << " " << initial_yaw << std::endl;

    previous_pos.x = initial_x;
    previous_pos.y = initial_y;
    previous_pos.z = initial_z;
    previous_pos.roll = initial_roll;
    previous_pos.pitch = initial_pitch;
    previous_pos.yaw = initial_yaw;
    
    current_pos.x = initial_x;
    current_pos.y = initial_y;
    current_pos.z = initial_z;
    current_pos.roll = initial_roll;
    current_pos.pitch = initial_pitch;
    current_pos.yaw = initial_yaw;

    ndt_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/current_pose", 1000);

    control_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/control_pose", 1000);

    ndt_map_pub = n.advertise<sensor_msgs::PointCloud2>("/ndt_map", 1000, true);

    // subscribing parameter
    ros::Subscriber param_sub = n.subscribe("config/ndt", 10, param_callback);

    // subscribing gnss position
    ros::Subscriber gnss_sub = n.subscribe("gnss_pose", 10, gnss_callback);

    // subscribing map data (only once)
    ros::Subscriber map_sub = n.subscribe("points_map", 10, map_callback);

    // subscribing the velodyne data
    //    ros::Subscriber velodyne_sub = n.subscribe("points_raw", 1000, velodyne_callback);
    //    ros::Subscriber velodyne_sub = n.subscribe("cloud_pcd", 1000, velodyne_callback);

    ros::spin();

    return 0;
}

/**
 * Given a cloud of datapts (an in/out variable), a vector of dataPts, an index to a specific
 * point in datapts and a distanceThreshold, add the list of indices into dataPts which are
 * within the distanceThreshold  to dirtyPts.
 *
 * @param dataPTSreduced_cloud a reference to a vector of Points
 * @param point_cloud_for_reduction   the collection of points that we want to delete some points from
  * @param distanceThreshold an integer Euclidean distance
 */
pcl::PointCloud<pcl::PointXYZRGB> Track::removeClosestDataCloudPoints(pcl::PointCloud<pcl::PointXYZRGB> point_cloud_for_reduction,pcl::PointCloud<pcl::PointXYZRGB> removal_Cloud, int distanceThreshold){

    //TODO, MAKE THIS PARALLEL

        //NOTE: you cannot feed a KNN searcher clouds with 1 or fewer datapoints!

        //KDTREE SEARCH

        pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
        // Neighbors within radius search

        std::vector<int> pointIdxRadiusSearch;
        std::vector<float> pointRadiusSquaredDistance;

        double point_radius = distanceThreshold;

        PointCloud<pcl::PointXYZRGB> point_cloud_flattened;//Point cloud with extra Hue Dimension crushed to 0

        PointCloud<pcl::PointXYZRGB> point_cloud_for_return;
        point_cloud_for_return.reserve(point_cloud_for_reduction.size());


        if(point_cloud_for_reduction.size()>1){

            bool *marked= new bool[point_cloud_for_reduction.size()];
            memset(marked,false,sizeof(bool)*point_cloud_for_reduction.size());

            bool *parmarked= new bool[point_cloud_for_reduction.size()];
            memset(parmarked,false,sizeof(bool)*point_cloud_for_reduction.size());
            //Make a version of the original data cloud that is flattened to z=0 but with the same indice
            copyPointCloud( point_cloud_for_reduction,point_cloud_flattened);


            for(int q=0; q<point_cloud_flattened.size();q++){

                point_cloud_flattened.at(q).z=0;

            }

           pcl:: PointCloud<PointXYZRGB>::Ptr point_cloud_for_reduction_ptr (new pcl::PointCloud<PointXYZRGB> (point_cloud_flattened));

           // K nearest neighbor search with Radius

            kdtree.setInputCloud (point_cloud_for_reduction_ptr); //Needs to have more than 1 data pt or segfault




            double t = (double)getTickCount();


//The below has been parallelized,and runs about 2X faster
                /**
            // iterate over points in model and remove those points within a certain distance
            for (unsigned int c=0; c < removal_Cloud.size(); c++)
            {

                if(point_cloud_for_reduction.size()<2){
                    break;
                }


                pcl::PointXYZRGB searchPoint;

                searchPoint.x = removal_Cloud.points[c].x;
                searchPoint.y = removal_Cloud.points[c].y;
               //Need to take z as zero when using a flattened data point cloud
               searchPoint.z = 0;

                // qDebug() <<"Datapts before incremental remove"<< point_cloud_for_reduction.size();
                if ( kdtree.radiusSearch (searchPoint, point_radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
                {
                    for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){
                        if(point_cloud_for_reduction.size()>0) ///NOTE CHANGED FROM > 1

                        marked[pointIdxRadiusSearch[i]]=true;
    //                        point_cloud_for_reduction.erase(point_cloud_for_reduction.begin()+pointIdxRadiusSearch[i]);// point_cloud_for_reduction.points[ pointIdxRadiusSearch[i] ]
                    }
                }


            }

            //DESTROY ALL MARKED POINTS
            for(uint q=0; q< point_cloud_for_reduction.size(); q++){
                if(!marked[q]){
                    point_cloud_for_return.push_back(point_cloud_for_reduction.at(q));
    //                point_cloud_for_return.at(q) = point_cloud_for_reduction.at(q);

                }

//.........这里部分代码省略.........

void PbMapMaker::detectPlanesCloud( pcl::PointCloud<PointT>::Ptr &pointCloudPtr_arg,
                                    Eigen::Matrix4f &poseKF,
                                    double distThreshold, double angleThreshold, double minInliersF)
{
  unsigned minInliers = minInliersF * pointCloudPtr_arg->size();

  #ifdef _VERBOSE
    cout << "detectPlanes in a cloud with " << pointCloudPtr_arg->size() << " points " << minInliers << " minInliers\n";
  #endif

  pcl::PointCloud<PointT>::Ptr pointCloudPtr_arg2(new pcl::PointCloud<PointT>);
  pcl::copyPointCloud(*pointCloudPtr_arg,*pointCloudPtr_arg2);

  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr alignedCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>);
  pcl::transformPointCloud(*pointCloudPtr_arg,*alignedCloudPtr,poseKF);

  { mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);
    *mPbMap.globalMapPtr += *alignedCloudPtr;
  } // End CS

  // Downsample voxel map's point cloud
  static pcl::VoxelGrid<pcl::PointXYZRGBA> grid;
  grid.setLeafSize(0.02,0.02,0.02);
  pcl::PointCloud<pcl::PointXYZRGBA> globalMap;
  grid.setInputCloud (mPbMap.globalMapPtr);
  grid.filter (globalMap);
  mPbMap.globalMapPtr->clear();
  *mPbMap.globalMapPtr = globalMap;

  pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
  ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
  ne.setMaxDepthChangeFactor (0.02f); // For VGA: 0.02f, 10.0f
  ne.setNormalSmoothingSize (5.0f);
  ne.setDepthDependentSmoothing (true);

  pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
  mps.setMinInliers (minInliers);
  mps.setAngularThreshold (angleThreshold); // (0.017453 * 2.0) // 3 degrees
  mps.setDistanceThreshold (distThreshold); //2cm

  pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
  ne.setInputCloud (pointCloudPtr_arg2);
  ne.compute (*normal_cloud);

#ifdef _VERBOSE
  double plane_extract_start = pcl::getTime ();
#endif
  mps.setInputNormals (normal_cloud);
  mps.setInputCloud (pointCloudPtr_arg2);

  std::vector<pcl::PlanarRegion<PointT>, aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
  std::vector<pcl::ModelCoefficients> model_coefficients;
  std::vector<pcl::PointIndices> inlier_indices;
  pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
  std::vector<pcl::PointIndices> label_indices;
  std::vector<pcl::PointIndices> boundary_indices;
  mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);

  #ifdef _VERBOSE
    double plane_extract_end = pcl::getTime();
    std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl;
//    std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
    cout << regions.size() << " planes detected\n";
  #endif

  // Create a vector with the planes detected in this keyframe, and calculate their parameters (normal, center, pointclouds, etc.)
  // in the global reference
  vector<Plane> detectedPlanes;
  for (size_t i = 0; i < regions.size (); i++)
  {
    Plane plane;

    Vector3f centroid = regions[i].getCentroid ();
    plane.v3center = compose(poseKF, centroid);
    plane.v3normal = poseKF.block(0,0,3,3) * Vector3f(model_coefficients[i].values[0], model_coefficients[i].values[1], model_coefficients[i].values[2]);
//  assert(plane.v3normal*plane.v3center.transpose() <= 0);
//    if(plane.v3normal*plane.v3center.transpose() <= 0)
//      plane.v3normal *= -1;

    // Extract the planar inliers from the input cloud
    pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
    extract.setInputCloud (pointCloudPtr_arg2);
    extract.setIndices ( boost::make_shared<const pcl::PointIndices> (inlier_indices[i]) );
    extract.setNegative (false);
    extract.filter (*plane.planePointCloudPtr);    // Write the planar point cloud

    static pcl::VoxelGrid<pcl::PointXYZRGBA> plane_grid;
    plane_grid.setLeafSize(0.05,0.05,0.05);
    pcl::PointCloud<pcl::PointXYZRGBA> planeCloud;
    plane_grid.setInputCloud (plane.planePointCloudPtr);
    plane_grid.filter (planeCloud);
    plane.planePointCloudPtr->clear();
    pcl::transformPointCloud(planeCloud,*plane.planePointCloudPtr,poseKF);

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr contourPtr(new pcl::PointCloud<pcl::PointXYZRGBA>);
    contourPtr->points = regions[i].getContour();
//    plane.contourPtr->points = regions[i].getContour();
//    pcl::transformPointCloud(*plane.contourPtr,*plane.polygonContourPtr,poseKF);
    pcl::transformPointCloud(*contourPtr,*plane.polygonContourPtr,poseKF);
    plane_grid.setInputCloud (plane.polygonContourPtr);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    }
    if(conf.skip_pc || conf.add_pc)
    {
        if (conf.skip_pc)
        {
            conf.x = -1;
            conf.y = -0.3;
            conf.z = 0.78;
            conf.yaw = 0;
            conf.pitch = 30;
            conf.roll = 0;
        }
        else
        {
            conf.x = -0.1;
            conf.y = -0.8;
            conf.z = 0.019;
            conf.yaw = 175;
            conf.pitch = 0;
            conf.roll = 0;
        }

        noMoreUndo=false;
        conf.skip_pc = false;
        conf.add_pc =false;
        
        doUpdate = true;
        int count = 0;
        cloud_blob_prev = cloud_blob_new;
        cloud_blob_new = reader.getNextCloud();
        cout<<"header"<<cloud_blob_new->header<<endl;
//        cloud_blob_new->
        ros::M_string::iterator iter;
        //for(iter=cloud_blob_new->__connection_header->begin ();iter!=cloud_blob_new->__connection_header->end ();iter++)
         // cout<<iter->first<<","<<iter->second<<endl;
        
        
        while(count < skipNum && cloud_blob_prev != cloud_blob_new)
        {
            cloud_blob_prev = cloud_blob_new;
            cloud_blob_new = reader.getNextCloud();
            count ++;
        }
        if (cloud_blob_prev != cloud_blob_new) {
            pcl::fromROSMsg(*cloud_blob_new, *cloud_temp_ptr);
            cloud_temp_ptr->header;
            cout<<"numPoints="<<cloud_temp_ptr->size ()<<endl;
            appendCamIndexAndDistance (cloud_temp_ptr,cloud_new_ptr,globalFrameCount,VectorG(0,0,0));
            globalFrameCount++;
            cloud_new_ptr->width=1;
            cloud_new_ptr->height=cloud_new_ptr->size();
            writer.write (std::string("tempAppend.pcd"),*cloud_new_ptr,true);
  if (pcl::io::loadPCDFile (std::string("tempAppend.pcd"), cloud_blobc_new) == -1)
  {
    ROS_ERROR ("Couldn't read temp file ");
    return ;
  }
//  ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), argv[1] ,pcl::getFieldsList (cloud_blob).c_str ());

  // Convert to the templated message type
   pcl::fromROSMsg (cloud_blobc_new, *cloud_new_ptr);
//   pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud));
            
            if(ITpresent){
                cout<<"inside IT"<<endl;
                transformXYZYPR<PointT>(*cloud_new_ptr, *cloud_mod_ptr, InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI);
                *cloud_new_ptr = *cloud_mod_ptr;
             //   conf.pitch=0;
             //   conf.yaw=0;
             //   conf.roll=0;
            }
            //ROS_INFO("PointCloud with %d data points and frame %s (%f) received.", (int) cloud_new_ptr->points.size(), cloud_new_ptr->header.frame_id.c_str(), cloud_new_ptr->header.stamp.toSec());
            viewer.removePointCloud("new");
           // pcl::toROSMsg<PointT>(*cloud_new_ptr, cloud_blobc_new);
            color_handler_new.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_new));
            viewer.addPointCloud(*cloud_new_ptr, color_handler_new, "new", viewportOrig);
            ROS_INFO("displaying new point cloud");
          /*  if(Merged){
                viewer.removePointCloud("merged");
                pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged);
                color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged));
                viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig);
            }*/
        }else {
            ROS_INFO("Finised reading all pointclouds! Select done to save.");
        }
    }
    
    if(conf.set_skip){
        conf.set_skip = false;
        doUpdate = true;
        skipNum = (conf.skipNum);
    }
    if(conf.update)
    {
        conf.update = false;
        doUpdate = true;
        updateUI();
    }
}

void Main_process() {
    //    pcl::VoxelGrid<PointXYZRGB> VG_humanSampling;

    //    double clustering_voxel_size = 0.003;
    //    VG_humanSampling.setLeafSize (clustering_voxel_size, clustering_voxel_size, clustering_voxel_size);
    //    VG_humanSampling.setDownsampleAllData (false);

          person_cluster->clear();
          cv::Vec2b point;

        // for(int i = 0; i<imageG.rows; ++i)
        //     for(int j = 0; j<imageG.cols; ++j )
        //     {
        //         point = imageG.at<cv::Vec2b>(i,j);
        //       if((point[0] == 0)&&(point[1] == 0)&&(point[2] == 0))
        //           //&&(point[1] != 0)&&(point[2] != 0))
        //         {
        //             person_cluster->points.push_back(global_cloud->at(j,i));
        //         }
        //     }

      pcl::PointCloud<PointXYZRGB>::Ptr cloud_ptr (new pcl::PointCloud<PointXYZRGB>);
              pcl::PCDReader reader;
			  reader.read<pcl::PointXYZRGB> ("/home/shaghayegh/catkin_ws/src/mythesis_body/01.pcd", *cloud_ptr);
			  	// reader.read<pcl::PointXYZRGB> ("/home/shaghayegh/catkin_ws/src/mythesis_body/src/model_uniform/third_shot.pcd", *person_cluster);
              // pcl::io::savePCDFile("/home/shaghayegh/catkin_ws/pcdmamuli.pcd",*person_cluster);
              // pcl::io::savePCDFileASCII("/home/shaghayegh/catkin_ws/pcdascii.pcd",*person_cluster);
              // pcl::io::savePCDFileBinary("/home/shaghayegh/catkin_ws/pcdBinary.pcd",*person_cluster);

    //           for (size_t i = 0; i < person_cluster->points.size (); ++i)
    // std::cout << "    " << person_cluster->points[i].x
    //           << " "    << person_cluster->points[i].y
    //           << " "    << person_cluster->points[i].z << std::endl;
			  	cout<<"model_size "<<cloud_ptr->points.size()<<endl;
                boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1 (new pcl::visualization::PCLVisualizer ("3D Viewerman"));
                pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_ptr);
                viewer1->addPointCloud<pcl::PointXYZRGB> (cloud_ptr, rgb, "sample cloud");
                while (!viewer1->wasStopped ()) {
                    viewer1->spinOnce (1);
                    boost::this_thread::sleep (boost::posix_time::microseconds (100));
                }
                viewer1.reset();

    
      pcl::PointCloud<PointXYZRGB>::Ptr cloud_ptr2 (new pcl::PointCloud<PointXYZRGB>);

      *person_cluster = *cloud_ptr->makeShared();
      cout <<cloud_ptr->isOrganized()<<" organized "<<endl;
      cout<<cloud_ptr->header << " header "<<endl;
      waitKey(0);

      if(cloud_ptr->size()>0)
      {

          // std::vector<int> indices;
          // pcl::removeNaNFromPointCloud(*cloud_ptr, *cloud_ptr2, indices);
          // std::cout << "size: " << cloud_ptr2->points.size () << std::endl;//          pcl::removeNaNFromPointCloud()
        type_of_keypoint(cloud_ptr);
      }
    	ROS_INFO(" type of keypoint ");
                stringstream s2;
                string root_path_model = ros::package::getPath("mythesis_body")+"/src/model_uniform/";
                string endaddress = root_path_model; //ros::Pakeage::getPath(pick_and_place)+"src/Data/";
                s2 << endaddress << "third" << "_shot.pcd";
                pcl::io::savePCDFileBinary("/home/shaghayegh/catkin_ws/src/mythesis_body/01_uniform.pcd", *model_keypoints);
       cout << "*************keypoint size :  " << model_keypoints->size() << endl ;



      float descr_rad_(0.5);
      // calculat shot descriptor 
//        NormalEstimator norm;
//        model_normals = norm.get_normals(cloud_ptr);//?
//        pcl::io::savePCDFileASCII("/home/shaghayegh/catkin_ws/src/mythesis_body/01_shot_normalR9.pcd", *model_normals);


      pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
      ne.setInputCloud (cloud_ptr);
      pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree_normal (new pcl::search::KdTree<pcl::PointXYZRGB> ());
      ne.setSearchMethod (tree_normal);
//      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
      ne.setRadiusSearch (9);
      ne.compute (*model_normals);
                pcl::io::savePCDFileASCII("/home/shaghayegh/catkin_ws/src/mythesis_body/01_shot_normalR9.pcd", *model_normals);

      //         if(cloud_ptr->size()>0)
      // {
      //     std::vector<int> indices;
      //     pcl::removeNaNFromPointCloud(*person_cluster, *cloud_ptr2, indices);
      //     std::cout << "size: " << cloud_ptr2->points.size () << std::endl;//          pcl::removeNaNFromPointCloud()
      // }

     
        //calculate descriptor
        pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, PointNormal, pcl::SHOT1344> est;
        est.setInputCloud(model_keypoints);
        est.setSearchSurface(person_cluster);//?doroste? bayad kole satho midadam ye faghat adamaro
        est.setInputNormals(model_normals);

        pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
//.........这里部分代码省略.........

template <typename PointT> inline unsigned
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                              const Eigen::Vector4f &centroid,
                              Eigen::Matrix3f &covariance_matrix)
{
  if (cloud.points.empty ())
    return (0);

  // Initialize to 0
  covariance_matrix.setZero ();

  unsigned point_count;
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    point_count = cloud.size ();
    // For each point in the cloud
    for (size_t i = 0; i < point_count; ++i)
    {
      Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    point_count = 0;
    // For each point in the cloud
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      // Check if the point is invalid
      if (!isFinite (cloud [i]))
        continue;

      Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
      ++point_count;
    }
  }
  covariance_matrix (1, 0) = covariance_matrix (0, 1);
  covariance_matrix (2, 0) = covariance_matrix (0, 2);
  covariance_matrix (2, 1) = covariance_matrix (1, 2);

  return (point_count);
}

bool StereoSensorProcessor::cleanPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud)
{
  pcl::PointCloud<pcl::PointXYZRGB> tempPointCloud;

  originalWidth_ = pointCloud->width;
  pcl::removeNaNFromPointCloud(*pointCloud, tempPointCloud, indices_);
  tempPointCloud.is_dense = true;
  pointCloud->swap(tempPointCloud);

  ROS_DEBUG("ElevationMap: cleanPointCloud() reduced point cloud to %i points.", static_cast<int>(pointCloud->size()));
  return true;
}

void
pcl::gpu::people::PeopleDetector::shs5 (const pcl::PointCloud<PointT> &cloud,
                                        const std::vector<int>& indices,
                                        unsigned char *mask)
{
  pcl::device::Intr intr (fx_, fy_, cx_, cy_);
  intr.setDefaultPPIfIncorrect (cloud.width, cloud.height);

  const float *hue = &hue_host_.points[0];
  double squared_radius = CLUST_TOL_SHS * CLUST_TOL_SHS;

  std::vector<std::vector<int> > storage (100);

  // Process all points in the indices vector
  int total = static_cast<int> (indices.size ());
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int k = 0; k < total; ++k)
  {
    int i = indices[k];
    if (mask[i])
      continue;

    mask[i] = 255;

    int id = 0;
#ifdef _OPENMP
    id = omp_get_thread_num();
#endif
    std::vector<int>& seed_queue = storage[id];
    seed_queue.clear ();
    seed_queue.reserve (cloud.size ());
    int sq_idx = 0;
    seed_queue.push_back (i);

    PointT p = cloud.points[i];
    float h = hue[i];

    while (sq_idx < (int) seed_queue.size ())
    {
      int index = seed_queue[sq_idx];
      const PointT& q = cloud.points[index];

      if (!pcl::isFinite (q))
        continue;

      // search window                  
      int left, right, top, bottom;
      getProjectedRadiusSearchBox (cloud.height, cloud.width, intr, q, squared_radius, left, right, top, bottom);

      int yEnd = (bottom + 1) * cloud.width + right + 1;
      int idx = top * cloud.width + left;
      int skip = cloud.width - right + left - 1;
      int xEnd = idx - left + right + 1;

      for (; xEnd < yEnd; idx += 2 * skip, xEnd += 2 * cloud.width)
        for (; idx < xEnd; idx += 2)
        {
          if (mask[idx])
            continue;

          if (sqnorm (cloud.points[idx], q) <= squared_radius)
          {
            float h_l = hue[idx];

            if (fabs (h_l - h) < DELTA_HUE_SHS)
            {
              seed_queue.push_back (idx);
              mask[idx] = 255;
            }
          }
        }

      sq_idx++;
    }
  }
}

template <typename PointT> void
pcl::io::pointCloudTovtkPolyData (const pcl::PointCloud<PointT>& cloud, vtkPolyData* const pdata)
{
  // Get a list of all the fields available
  std::vector<pcl::PCLPointField> fields;
  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));

  // Coordinates (always must have coordinates)
  vtkIdType nr_points = cloud.points.size ();
  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
  points->SetNumberOfPoints (nr_points);
  // Get a pointer to the beginning of the data array
  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);

  // Set the points
  if (cloud.is_dense)
  {
    for (vtkIdType i = 0; i < nr_points; ++i)
      memcpy (&data[i * 3], &cloud[i].x, 12);    // sizeof (float) * 3
  }
  else
  {
    vtkIdType j = 0;    // true point index
    for (vtkIdType i = 0; i < nr_points; ++i)
    {
      // Check if the point is invalid
      if (!std::isfinite (cloud[i].x) ||
          !std::isfinite (cloud[i].y) ||
          !std::isfinite (cloud[i].z))
        continue;

      memcpy (&data[j * 3], &cloud[i].x, 12);    // sizeof (float) * 3
      j++;
    }
    nr_points = j;
    points->SetNumberOfPoints (nr_points);
  }

  // Create a temporary PolyData and add the points to it
  vtkSmartPointer<vtkPolyData> temp_polydata = vtkSmartPointer<vtkPolyData>::New ();
  temp_polydata->SetPoints (points);

  // Check if Normals are present
  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
  for (size_t d = 0; d < fields.size (); ++d)
  {
    if (fields[d].name == "normal_x")
      normal_x_idx = fields[d].offset;
    else if (fields[d].name == "normal_y")
      normal_y_idx = fields[d].offset;
    else if (fields[d].name == "normal_z")
      normal_z_idx = fields[d].offset;
  }
  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
  {
    vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
    normals->SetNumberOfComponents (3); //3d normals (ie x,y,z)
    normals->SetNumberOfTuples (cloud.size ());
    normals->SetName ("Normals");

    for (size_t i = 0; i < cloud.size (); ++i)
    {
      float normal[3];
      pcl::getFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
      pcl::getFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
      pcl::getFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
      normals->SetTupleValue (i, normal);
    }
    temp_polydata->GetPointData ()->SetNormals (normals);
  }

  // Colors (optional)
  int rgb_idx = -1;
  for (size_t d = 0; d < fields.size (); ++d)
  {
    if (fields[d].name == "rgb" || fields[d].name == "rgba")
    {
      rgb_idx = fields[d].offset;
      break;
    }
  }
  if (rgb_idx != -1)
  {
    vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
    colors->SetNumberOfComponents (3);
    colors->SetNumberOfTuples (cloud.size ());
    colors->SetName ("RGB");

    for (size_t i = 0; i < cloud.size (); ++i)
    {
      unsigned char color[3];
      pcl::RGB rgb;
      pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba);
      color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
      colors->SetTupleValue (i, color);
    }
    temp_polydata->GetPointData ()->SetScalars (colors);
  }

  // Add 0D topology to every point
//.........这里部分代码省略.........

/**
 * Gets the similarity between a cylinder and a given point cloud between 0.0 and 1.0
 * @param input_cloud_ptr the input cloud
 * @return likelihood
 */
double PointCloudDetection::getCylinderLikelihood(
        const pcl::PointCloud<PointRGBT>::Ptr input_cloud_ptr, pcl::ModelCoefficients& coefficients) {


    //The KD tree for the segmentation
    pcl::search::KdTree<PointRGBT>::Ptr tree(new pcl::search::KdTree<PointRGBT>());
    //Structure for normal estimation
    pcl::NormalEstimation<PointRGBT, pcl::Normal> ne; //Normal estimation

    //for the Ransac using Cylinder normals
    pcl::SACSegmentationFromNormals<PointRGBT, pcl::Normal> seg; // the ransac filter

    //the structure to store the cloud normals
    pcl::PointCloud<pcl::Normal> cloud_normals; // the cloud normals

    //for extraction
    pcl::ExtractIndices<PointRGBT> extract; //for point extraction from the filter
    pcl::ExtractIndices<pcl::Normal> extract_normals;

    //all points within a cylinder
    pcl::PointCloud<PointRGBT> cloud_out;

    //the sphere coefficents generated by segmentation
    pcl::PointIndices inliers;


  // Estimate point normals
    ne.setSearchMethod(tree);
    ne.setInputCloud(input_cloud_ptr);
    ne.setKSearch(50);
    ne.compute(cloud_normals);

    // Create the segmentation object for the planar model and set all the parameters
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CYLINDER);
    seg.setMethodType(pcl::SAC_RANSAC);

    seg.setNormalDistanceWeight(ransac_normal_dist_weight_);
    seg.setMaxIterations(ransac_iterations_);
    seg.setDistanceThreshold(ransac_dist_threshold_);
    seg.setRadiusLimits(ransac_min_radius_, ransac_max_radius_);

    seg.setInputCloud(input_cloud_ptr);
    seg.setInputNormals(boost::make_shared<pcl::PointCloud<pcl::Normal> >(cloud_normals));
    // Obtain the cylinder inliers and coefficients
    seg.segment(inliers, coefficients);

    if (debug_) PCL_WARN("Cylinder RanSac. Found %d inliers in a cloud of %d points\n",(int) inliers.indices.size(), (int) input_cloud_ptr->size());

    // Extract the inliers from the input cloud

    if (inliers.indices.size() > 0) {
        extract.setInputCloud(input_cloud_ptr);
        extract.setIndices(boost::make_shared<const pcl::PointIndices>(inliers));

        extract_normals.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::Normal> >(cloud_normals));
        extract_normals.setIndices(boost::make_shared<const pcl::PointIndices>(inliers));

        extract.setNegative(false);
        extract.filter(cloud_out);

        return static_cast<double>(cloud_out.points.size())/input_cloud_ptr->size();

    } else {
        return 0.0;
    }
}

	inline void
	remove_cluster_2d(	const image_geometry::PinholeCameraModel &camera_model,
								const pcl::PointCloud<PointT> &in,
								pcl::PointCloud<PointT> &out,
								int rows,
								int cols,
								int k_neighbors = 4,
								int border = 25)
	{
		std::vector<int> points2d_indices;
		pcl::PointCloud<pcl::PointXY> points2d;

#ifdef DEBUG
		std::cout << "in points: "<< in.size() << "\n";
#endif

		// Project points into image space
		for(unsigned int i=0; i < in.points.size();  ++i){
			const PointT* pt = &in.points.at(i);
			if( pt->z > 1) { // min distance from camera 1m

				cv::Point2i point_image = camera_model.project3dToPixel(cv::Point3d(pt->x, pt->y, pt->z));

				if( between<int>(0+border, point_image.x, cols-border )
					&& between<int>( 0+border, point_image.y, rows-border )
				)
				{
					pcl::PointXY p_image;
					p_image.x = point_image.x;
					p_image.y = point_image.y;

					points2d.push_back(p_image);
					points2d_indices.push_back(i);
				}
			}
		}

#ifdef DEBUG
		std::cout << "projected 2d points: "<< points2d.size() << " indices: " << points2d_indices.size() << "\n";
#endif

		pcl::search::KdTree<pcl::PointXY> tree_n;
		tree_n.setInputCloud(points2d.makeShared());
		tree_n.setEpsilon(0.1);

		for(unsigned int i=0; i < points2d.size(); ++i){
			std::vector<int> k_indices;
			std::vector<float> square_distance;

			//tree_n.nearestKSearch(points2d.at(i), k_neighbors, k_indices, square_distance);
			tree_n.radiusSearch(points2d.at(i), k_neighbors, k_indices, square_distance);

			if(k_indices.empty()) continue; // Dont add points without neighbors

			look_up_indices(points2d_indices, k_indices);

			float edginess = 0;
			if(is_edge_z(in, points2d_indices.at(i), k_indices, square_distance, edginess, 0.1)){
				out.push_back(in.points.at(points2d_indices.at(i)));
				out.at(out.size()-1).intensity = edginess;
			}
		}

#ifdef DEBUG
		std::cout << "out 2d points: "<< out.size() << "\n";
#endif
	}

void union_find(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud, int K, std::vector<int>& inliers){

	if (cloud->size()==0){
		return;
	}
	if(K==0){
		ROS_WARN("parameter K for K nearest neighbor is 0, aborting");
		return;
	}
	std::vector<union_find_node*> forest;
	std::vector<union_find_node*> model(cloud->size(),NULL);//init to NULL
	std::vector<int> knnindices(K); //stores indices of the last K nearest neighbors search
	std::vector<float> knnSqDistances(K); //sotres squared distance of the last KNN search
	int knncount=0;

	//KDTree
	pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;

	kdtree.setInputCloud (cloud);

	for (int i=0;i<cloud->size();i++){
		//if point is not yet visited
		if(model[i]==NULL){
			//find K nearest neighbors of the curent point
			knncount = kdtree.nearestKSearch(cloud->at(i),K,knnindices,knnSqDistances);
			//union them to the curent point
			for (int result_i=0;result_i<knncount;result_i++){
				//create the point in the model if it do not exists
				if (model[knnindices[result_i]]==NULL){
					model[knnindices[result_i]] = new union_find_node;
					makeSet(model[knnindices[result_i]]);
				}
				//actualy do the union
				union_nodes(model[i],model[knnindices[result_i]]);
			}
			//add the root to the forest (check if do not already exists)
			addToForest(forest,find(model[i]));
		}
	}

	//now we have to find the biggest tree
	if(forest.size()!=0){
		size_t max_size=0, biggest_tree_i=0;
		for (size_t i=0;i<forest.size();i++){
			if( forest[i]->children_n > max_size){
				max_size = forest[i]->children_n;
				biggest_tree_i = i;
			}
		}

		union_find_node* the_root = forest[biggest_tree_i];

		//we add to the inliers all the points that have the_root as root
		for (int i=0; i < cloud->size(); i++){
			if(find(model[i])==the_root){
				inliers.push_back(i);
			}
		}
		for (int i=0;i<model.size();i++)
			delete model[i];
	}
}

void getRotations(const pcl::PointCloud<NormalType>::Ptr &cloud_normals,
                  const std::string &outName, Eigen::Vector3d &M1,
                  Eigen::Vector3d &M2, Eigen::Vector3d &M3) {

  if (!FLAGS_redo) {
    std::ifstream in(outName, std::ios::in | std::ios::binary);
    if (in.is_open()) {
      std::vector<Eigen::Matrix3d> R(NUM_ROTS);
      std::vector<Eigen::Vector3d> M(3);
      for (auto &r : R)
        in.read(reinterpret_cast<char *>(r.data()), sizeof(Eigen::Matrix3d));

      for (auto &m : M)
        in.read(reinterpret_cast<char *>(m.data()), sizeof(double) * 3);

      M1 = M[0];
      M2 = M[1];
      M3 = M[2];

      in.close();

      return;
    }
  }

  // NB: Convert pcl to eigen so linear algebra is easier
  std::vector<Eigen::Vector3d> normals;
  normals.reserve(cloud_normals->size());
  for (auto &n : *cloud_normals)
    normals.emplace_back(n.normal_x, n.normal_y, n.normal_z);

  if (!FLAGS_quietMode)
    std::cout << "N size: " << normals.size() << std::endl;

  std::vector<Eigen::Vector3d> M(3);
  satoshiRansacManhattan1(normals, M[0]);
  if (!FLAGS_quietMode) {
    std::cout << "D1: " << M[0] << std::endl << std::endl;
  }

  // NB: Select normals that are perpendicular to the first
  // dominate direction
  std::vector<Eigen::Vector3d> N2;
  for (auto &n : normals)
    if (std::asin(n.cross(M[0]).norm()) > PI / 2.0 - 0.02)
      N2.push_back(n);

  if (!FLAGS_quietMode)
    std::cout << "N2 size: " << N2.size() << std::endl;

  satoshiRansacManhattan2(N2, M[0], M[1], M[2]);

  if (!FLAGS_quietMode) {
    std::cout << "D2: " << M[1] << std::endl << std::endl;
    std::cout << "D3: " << M[2] << std::endl << std::endl;
  }

  if (std::abs(M[0][2]) > 0.5) {
    M1 = M[0];
    M2 = M[1];
  } else if (std::abs(M[1][2]) > 0.5) {
    M1 = M[1];
    M2 = M[0];
  } else {
    M1 = M[2];
    M2 = M[0];
  }

  if (M1[2] < 0)
    M1 *= -1.0;

  M3 = M1.cross(M2);

  M[0] = M1;
  M[1] = M2;
  M[2] = M3;

  std::vector<Eigen::Matrix3d> R(4);

  getMajorAngles(M1, M2, M3, R);

  if (!FLAGS_quietMode) {
    for (auto &r : R)
      std::cout << r << std::endl << std::endl;
  }

  if (FLAGS_save) {
    std::ofstream binaryWriter(outName, std::ios::out | std::ios::binary);
    for (auto &r : R)
      binaryWriter.write(reinterpret_cast<const char *>(r.data()),
                         sizeof(Eigen::Matrix3d));

    for (auto &m : M)
      binaryWriter.write(reinterpret_cast<const char *>(m.data()),
                         sizeof(double) * 3);

    binaryWriter.close();
  }
}

double
dist_bounding_box(carmen_point_t particle_pose, pcl::PointCloud<pcl::PointXYZ> &point_cloud, object_geometry_t model_geometry,
		object_geometry_t obj_geometry, carmen_vector_3D_t car_global_position, double x_pose, double y_pose)
{
	double sum = 0.0;
	long unsigned int pcl_size = point_cloud.size();
	carmen_position_t new_pt; //(x,y)
	double width_normalizer = norm_factor_y/model_geometry.width;
	double length_normalizer = norm_factor_x/model_geometry.length;
	double height_normalizer = norm_factor_x/model_geometry.height;

	pcl::PointCloud<pcl::PointXYZ>::iterator end = point_cloud.points.end();
	/*** BOX POSITIONING ***/

//	#pragma omp parallel for reduction(+ : sum)
//	for (pcl::PointCloud<pcl::PointXYZ>::iterator it = point_cloud.points.begin(); it < end; ++it)
//	{
//		new_pt = transf2d_bounding_box(car_global_position.x + it->x - particle_pose.x, car_global_position.y + it->y - particle_pose.y, -particle_pose.theta);
//		sum += dist_btw_point_and_box(new_pt, width_normalizer, length_normalizer);
//	}

	#pragma omp parallel for reduction(+ : sum)
	for (long unsigned int i = 0; i < pcl_size; i++)
	{
		new_pt = transf2d_bounding_box(car_global_position.x + point_cloud.points[i].x - particle_pose.x, car_global_position.y + point_cloud.points[i].y - particle_pose.y, -particle_pose.theta);
		sum += dist_btw_point_and_box(new_pt, width_normalizer, length_normalizer);
	}

	double penalty = 0.0;
	// Centroid's coordinates already global
	new_pt = transf2d_bounding_box(x_pose - particle_pose.x, y_pose - particle_pose.y, -particle_pose.theta);
	if (new_pt.x > 0.5*model_geometry.length || new_pt.y > 0.5*model_geometry.width)
	{
		new_pt.x *= length_normalizer;//4.5;
		new_pt.y *= width_normalizer;//2.1;
		penalty = sqrt(new_pt.x*new_pt.x + new_pt.y*new_pt.y);//new_pt.x + new_pt.y;//
	}

	/*** BOX DIMENSIONING ***/
	// Penalize based on the differences between dimensions of point cloud and model box
	double diff_length = (model_geometry.length - obj_geometry.length)*length_normalizer;
	double diff_width = (model_geometry.width - obj_geometry.width)*width_normalizer;
	double diff_height = (model_geometry.height - obj_geometry.height)*height_normalizer;
	double object_diagonal_xy = sqrt(obj_geometry.length*length_normalizer + obj_geometry.width*width_normalizer);
	double model_diagonal_xy = sqrt(model_geometry.length*length_normalizer + model_geometry.width*width_normalizer);
	double diff_diagonal = (model_diagonal_xy - object_diagonal_xy);
//	penalty += 2*diff_diagonal + diff_height;
	penalty += diff_length*diff_length + diff_width*diff_width + diff_height*diff_height + diff_diagonal*diff_diagonal;

	/*
	// aqui penaiza de acordo com o ponto de vista do objeto em relação ao carro.
	double local_x = x_pose - car_global_position.x;
	double local_y = y_pose - car_global_position.y;
	double local_theta = atan2(local_y,local_x);

	if((local_theta < M_PI/36 && local_theta > -M_PI/36) || (local_theta > 35*M_PI/36 && local_theta < -35*M_PI/36))
	{
		//caso 1
		diff_width = (model_geometry.width - obj_geometry.length)*width_normalizer;
		penalty += diff_width*diff_width + diff_height*diff_height;
	} else if( (local_theta > M_PI/36 && local_theta < 3*M_PI/8) || (local_theta > 5*M_PI/8 && local_theta < 35*M_PI/36) ||
			(local_theta < -M_PI/36 && local_theta > -3*M_PI/8) || (local_theta < -5*M_PI/8 && local_theta > -35*M_PI/36))
	{
		//caso 2
		penalty += diff_length*diff_length + diff_width*diff_width + diff_height*diff_height + diff_diagonal*diff_diagonal;
	} else if( (local_theta > 3*M_PI/8 && local_theta < 5*M_PI/8) || (local_theta < -3*M_PI/8 && local_theta > -5*M_PI/8) )
	{
		//caso 3
		penalty += diff_length*diff_length + diff_height*diff_height;
	}
	*/

	// Avoid division by zero
	if (pcl_size != 0)
		return sum/pcl_size + 0.2*penalty; //return normalized distance with penalty

	/* Else return very big distance */
	return 999999.0;
}