コード例 #1
0
  void execVisualizerThread()
  {
    initVisualizer();

    while(!visualizer_->wasStopped())
    {
      render(1);
    }
    visualizer_.reset();
  }
コード例 #2
0
  PclCameraTrajectoryVisualizerImpl(bool render_thread = true)
  {
    if(render_thread)
    {
      visualizer_thread_.reset(new boost::thread(boost::bind(&PclCameraTrajectoryVisualizerImpl::execVisualizerThread, this)));

      // wait for visualizer_ being created
      while(!visualizer_)
      {
        boost::this_thread::yield();
      }
    }
    else
    {
      initVisualizer();
    }
  }
コード例 #3
0
ファイル: main.c プロジェクト: iscumd/IGVC2016
int main(){
    int i = 0;
    int mapCount = 0, clearMapCount = 0, dumpCount=0;
    int revFrameCount = 0;

#ifdef USE_NORTH
    targetsGPS[maxTargets].lat = ADVANCED5LAT;
    targetsGPS[maxTargets].lon = ADVANCED5LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED6LAT;
    targetsGPS[maxTargets].lon = ADVANCED6LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED7LAT;
    targetsGPS[maxTargets].lon = ADVANCED7LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED8LAT;
    targetsGPS[maxTargets].lon = ADVANCED8LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED2LAT;
    targetsGPS[maxTargets].lon = ADVANCED2LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED1LAT;
    targetsGPS[maxTargets].lon = ADVANCED1LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED3LAT;
    targetsGPS[maxTargets].lon = ADVANCED3LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED12LAT;
    targetsGPS[maxTargets].lon = ADVANCED12LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED4LAT;
    targetsGPS[maxTargets].lon = ADVANCED4LON;
    maxTargets++;
#else
    targetsGPS[maxTargets].lat = ADVANCED4LAT;
    targetsGPS[maxTargets].lon = ADVANCED4LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED1LAT;
    targetsGPS[maxTargets].lon = ADVANCED1LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED2LAT;
    targetsGPS[maxTargets].lon = ADVANCED2LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED3LAT;
    targetsGPS[maxTargets].lon = ADVANCED3LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED11LAT;
    targetsGPS[maxTargets].lon = ADVANCED11LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED8LAT;
    targetsGPS[maxTargets].lon = ADVANCED8LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED7LAT;
    targetsGPS[maxTargets].lon = ADVANCED7LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED6LAT;
    targetsGPS[maxTargets].lon = ADVANCED6LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED11LAT;
    targetsGPS[maxTargets].lon = ADVANCED11LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED5LAT;
    targetsGPS[maxTargets].lon = ADVANCED5LON;
    maxTargets++;
#endif

    maxTargetIndex=maxTargets-1;

    for(i=0;i<maxTargets;i++){// this is converting all GPS point data to XY data.
        targetListXY[i].x = GPSX(targetsGPS[i].lon, startLongitude);
        targetListXY[i].y = GPSY(targetsGPS[i].lat, startLatitude);
    }
    currentXY.x = GPSX(gpsvar.longitude,startLongitude);// converts current robot X location compared to start longitude
    currentXY.y = GPSY(gpsvar.latitude,startLatitude);// converts current robot Y location compared to start latitude

    targetXY = targetListXY[currentTargetIndex];//sets first target GPS point
    nextTargetIndex = (currentTargetIndex + 1)%maxTargets;//sets next target GPS point
    nextXY = targetListXY[nextTargetIndex];// ??
    previousXY.x = GPSX(startLongitude, startLongitude);// why?
    previousXY.y = GPSY(startLatitude, startLatitude);//Why?

    initRoboteq();  /* Initialize roboteq */
    initGuide();//what is guide?
#ifdef USE_VISION // if USE_vision is defined, then initialize vision.
    initVision();
#endif //USE_VISION
#ifdef USE_GPS// if USE_GPS is defined, then initialize GPS.
    initGPS();
    initParser();
#endif //USE_GPS
#ifdef USE_LIDAR// if USE_LIDAR is defined, then initialize LIDAR.
    initObjects();
    initSICK();
#endif //USE_LIDAR
#ifdef DEBUG_VISUALIZER// if defined, then use visualizer.
    initVisualizer();
#endif //DEBUG_VISUALIZER
#ifdef USE_MAP//////>>>>>>>>>>>????
    initMap(0,0,0);
#endif //USE_MAP
#ifdef DUMP_GPS// dump GPS data into file
    FILE *fp;
    fp = fopen("gpsdump.txt", "w");
#endif // DUMP_GPS
    while(1){
        double dir = 1.0;
        double speed = 0.0, turn = 0.0;
        static double turnBoost = 0.750;//Multiplier for turn. Adjust to smooth jerky motions. Usually < 1.0
        static int lSpeed = 0, rSpeed = 0;//Wheel Speed Variables
        if (joystick() != 0) {// is joystick is connected
            if (joy0.buttons & LB_BTN) {// deadman switch, but what does joy0.buttons do?????????????????????????????????
                speed = -joy0.axis[1]; //Up is negative on joystick negate so positive when going forward
                turn = joy0.axis[0];

                lSpeed = (int)((speed + turnBoost*turn)*maxSpeed);//send left motor speed
                rSpeed = (int)((speed - turnBoost*turn)*maxSpeed);//send right motor speed
                }else{ //stop the robot
                     rSpeed=lSpeed=0;
            }
            if(((joy0.buttons & B_BTN)||autoOn)&& (saveImage==0)){//what is the single & ???????????????????
                saveImage =DEBOUNCE_FOR_SAVE_IMAGE;//save each image the camera takes, save image is an int declared in vision_nav.h
            }else{
                if (saveImage) saveImage--; // turn off if button wasn't pressed?
            }
            if(joy0.buttons & RB_BTN){//turn on autonmous mode if start??? button is pressed
                autoOn = 1;
                mode=1;
            }
            if(joy0.buttons & Y_BTN){ // turn off autonomous mode
                autoOn = 0;
                mode =0;
            }
            lastButtons = joy0.buttons;//is this just updating buttons?
        } else{
//            printf("No Joystick Found!\n");
            rSpeed=lSpeed=0;
        }
//
//        printf("3: %f %f\n",BASIC3LAT,BASIC3LON);
//        printf("4: %f %f\n",BASIC4LAT,BASIC4LON);
//        printf("5: %f %f\n",BASIC5LAT,BASIC5LON);
//        getchar();
#ifdef AUTO_SWAP//what is this
        if((currentTargetIndex>1&&targetIndexMem!=currentTargetIndex)||!autoOn||!mode==3){
            startTime=currentTime=(float)(clock()/CLOCKS_PER_SEC);
            targetIndexMem = currentTargetIndex;
        }else{
            currentTime=(float)(clock()/CLOCKS_PER_SEC);
        }
        totalTime = currentTime-startTime;
        if(totalTime>=SWAPTIME&&autoOn){
            swap();
            targetIndexMem = 0;
        }
#endif //AUTO_SWAP

#ifdef USE_GPS
        readGPS();
        currentXY.x = GPSX(gpsvar.longitude,startLongitude);
        currentXY.y = GPSY(gpsvar.latitude,startLatitude);
        robotTheta = ADJUST_RADIANS(DEG2RAD(gpsvar.course));
#else
        currentXY.x = 0.0;
        currentXY.y = 0.0;
        robotTheta = 0.0;
#endif //USE_GPS

        if(autoOn&&!flagPointSet){//this whole thing?????
            flagXY.x=currentXY.x+FLAG_X_ADJUST;
            flagXY.y=currentXY.y;
            flagPointSet=1;
            startAutoTime=currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
        }
        if(autoOn){
            currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
            totalAutoTime = currentAutoTime-startAutoTime;
            if(totalAutoTime>=MODE2DELAY){
                mode1TimeUp=1;//what is mode1 time up?
            }
            printf("TIMEING\n");
        }

//        if(currentTargetIndex <= OPEN_FIELD_INDEX || currentTargetIndex >= maxTargetIndex){
        if(currentTargetIndex <= OPEN_FIELD_INDEX){//if you are on your last target, then set approaching thresh, and dest thresh to larger values?
                //OPEN_FIELD_INDEX is set to 0 above...?
            approachingThresh=4.0;
            destinationThresh=3.0;
        }else{//otherwise set your thresholds to a bit closer.
//            destinationThresh=1.0;
            destinationThresh=0.75;
            approachingThresh=2.5;
        }
//mode1 = lane tracking and obstacle avoidance. mode 2 = vision, lane tracking, but guide to gps. its not primary focus.
//mode3= gps mode in open field, but vision is toned down to not get distracted by random grass.
//mode 4= flag tracking

       if(guide(currentXY, targetXY, previousXY, nextXY, robotTheta, robotWidth, 1)&& !allTargetsReached){//If target reached and and not all targets reached
            printf("REACHED TARGET\n");
            initGuide();// reset PID control stuff. problably resets all control variables.
            previousXY = targetXY;//update last target
            if(currentTargetIndex == maxTargetIndex){ //seeing if you are done with all targets.
                 allTargetsReached = 1;
            }else{//otherwise update all the target information
                currentTargetIndex = (currentTargetIndex + 1);
                nextTargetIndex = (currentTargetIndex + 1)% maxTargets;
                targetXY = targetListXY[currentTargetIndex];
                nextXY = targetListXY[nextTargetIndex];
            }
        }
        if((autoOn&&(currentTargetIndex == 0&&!approachingTarget&&!mode1TimeUp))||allTargetsReached){
                //if autonomous, and on first target, and not not approaching target, and not mode 1 time up, or reached last target.
            mode =1;//wtf is mode
            distanceMultiplier = 50;//wthis is how heavily to rely on vision
        } else if((autoOn&&currentTargetIndex == 0&&mode1TimeUp)||(autoOn&&approachingTarget&&(currentTargetIndex<=OPEN_FIELD_INDEX||currentTargetIndex>=maxTargetIndex-END_LANE_INDEX))){
            mode =2;
            distanceMultiplier = 50;
        } else if((autoOn&&currentTargetIndex!=0)){
            mode =3;
            distanceMultiplier = 12;
        }
        flagPointDistance = D((currentXY.x-flagXY.x),(currentXY.y-flagXY.y));// basically the distance formula, but to what? what flags GPS point?
        if(allTargetsReached&&flagPointDistance<FLAG_DIST_THRESH){
            mode =4;// what is mode
        }
#ifdef FLAG_TESTING
        /*FLAG TESTING*/
        mode=4;
#endif //FLAG_TESTING

        /*Current Target Heading PID Control Adjustment*/
        cvar.lookAhead = 0.00;//?
        cvar.kP = 0.20; cvar.kI = 0.000; cvar.kD = 0.15;

        turn = cvar.turn;


        int bestVisGpsMask = 99;
        int h = 0;
        double minVisGpsTurn = 9999;
        for(h=0;h<11;h++){
            if(fabs((cvar.turn-turn_angle[h]))<minVisGpsTurn){
                minVisGpsTurn=fabs((cvar.turn-turn_angle[h]));
                bestVisGpsMask = h;
            }
        }
        bestGpsMask = bestVisGpsMask;
//        printf("bvg: %d \n", bestVisGpsMask);
//        printf("vgt: %f cv3: %f\n", minVisGpsTurn,cvar3.turn);

#ifdef USE_VISION
//        double visTurnBoost = 0.50;
        double visTurnBoost = 1.0;
        if(imageProc(mode) == -1) break;
        if(mode==1||mode==2){
            turn = turn_angle[bestmask];
            turn *= visTurnBoost;
        }else if(mode==3 && fabs(turn_angle[bestmask])>0.70){
            turn = turn_angle[bestmask];
            turn *= visTurnBoost;
        }
#endif //USE_VISION
#ifdef USE_LIDAR
        updateSick();
//        findObjects();
#endif //USE_LIDAR

#ifdef USE_COMBINED_BUFFER//??????????
#define WORSTTHRESH 10
#define BESTTHRESH 3
        if(mode==4){
#ifdef USE_NORTH
            turn = (0.5*turn_angle[bestBlueMask]+0.5*turn_angle[bestRedMask]);
#else
            turn = (0.65*turn_angle[bestBlueMask]+0.35*turn_angle[bestRedMask]);
#endif
            turn *= 0.75;
        }
        combinedTargDist = cvar.targdist;
        if(((approachingTarget||inLastTarget)&&currentTargetIndex>OPEN_FIELD_INDEX
            &&currentTargetIndex<maxTargetIndex-END_LANE_INDEX)||(MAG(howbad[worstmask]-howbad[bestmask]))<BESTTHRESH||mode==4){
            getCombinedBufferAngles(0,0);//Don't Use Vision Radar Data
        }else{
            getCombinedBufferAngles(0,1);//Use Vision Radar Data
        }
        if(combinedBufferAngles.left != 0 || combinedBufferAngles.right !=0){
            if(mode == 1 || mode==2 || mode==3 || mode==4){
//            if(mode == 1 || mode==2 || mode==3){
//            if(mode==2 || mode==3){
//            if(mode==3){
                if(fabs(combinedBufferAngles.right)==fabs(combinedBufferAngles.left)){
                    double revTurn;
                    double revDistLeft, revDistRight;
                    int revIdx;
                    if(fabs(turn)<0.10) dir = -1.0;
                    if(fabs(combinedBufferAngles.left)>1.25) dir = -1.0;
                    if(dir<0){
                        revIdx = 540-RAD2DEG(combinedBufferAngles.left)*4;
                        revIdx = MIN(revIdx,1080);
                        revIdx = MAX(revIdx,0);
                        revDistLeft = LMSdata[revIdx];

                        revIdx = 540-RAD2DEG(combinedBufferAngles.right)*4;
                        revIdx = MIN(revIdx,1080);
                        revIdx = MAX(revIdx,0);
                        revDistRight = LMSdata[revIdx];
                        if(revDistLeft>=revDistRight){
                            revTurn = combinedBufferAngles.left;
                        }else {
                            revTurn = combinedBufferAngles.right;
                        }
                        turn = revTurn;
                    }else{
                        turn = turn_angle[bestmask];
                    }
                } else if(fabs(combinedBufferAngles.right-turn)<fabs(combinedBufferAngles.left-turn)){
//                } else if(turn<=0){
                    turn = combinedBufferAngles.right;
                }else {
                    turn = combinedBufferAngles.left;
                }
            }
        }
#endif //USE_COMBINED_BUFFER
        if(dir<0||revFrameCount!=0){
            dir = -1.0;
            revFrameCount = (revFrameCount+1)%REVFRAMES;
        }
        //        turn *= dir;
        turn = SIGN(turn) * MIN(fabs(turn), 1.0);
        speed = 1.0/(1.0+1.0*fabs(turn))*dir;
        speed = SIGN(speed) * MIN(fabs(speed), 1.0);
        if(!autoOn){
            maxSpeed = 60;
            targetIndexMem = 0;
        }else if(dir<0){
            maxSpeed = 30;
        }else if(mode<=2||(mode==3 && fabs(turn_angle[bestmask])>0.25)){
            maxSpeed = 60 - 25*fabs(turn);
//            maxSpeed = 70 - 35*fabs(turn);
//            maxSpeed = 90 - 50*fabs(turn);
//            maxSpeed = 100 - 65*fabs(turn);
        }else if(mode==4){
            maxSpeed = 45-20*fabs(turn);
        }else{
            maxSpeed = 85 - 50*fabs(turn);
//            maxSpeed = 100 - 65*fabs(turn);
//            maxSpeed = 110 - 70*fabs(turn);
//            maxSpeed = 120 - 85*fabs(turn);
        }
        if(autoOn){
            lSpeed = (speed + turnBoost*turn) * maxSpeed;
            rSpeed = (speed - turnBoost*turn) * maxSpeed;
        }
#ifdef DEBUG_MAIN
        printf("s:%.4f t: %.4f m: %d vt:%f dir:%f tmr: %f\n", speed, turn, mode, turn_angle[bestmask], flagPointDistance, totalAutoTime);
#endif //DEBUG_MAIN
#ifdef DUMP_GPS
    if(dumpCount==0){
        if (fp != NULL) {
                fprintf(fp, "%f %f %f %f %f\n",gpsvar.latitude,gpsvar.longitude, gpsvar.course, gpsvar.speed, gpsvar.time);
            }
    }
        dumpCount = dumpCount+1%DUMPGPSDELAY;

#endif //DUMP_GPS
#ifdef DEBUG_TARGET
        debugTarget();
#endif //DEBUG_TARGET
#ifdef DEBUG_GUIDE
        debugGuide();
#endif //DEBUG_GUIDE
#ifdef DEBUG_GPS
        debugGPS();
#endif //DEBUG_GPS
#ifdef DEBUG_LIDAR
        debugSICK();
#endif //DEBUG_LIDAR
#ifdef DEBUG_BUFFER
        debugCombinedBufferAngles();
#endif //DEBUG_BUFFE
#ifdef DEBUG_VISUALIZER
        robotX = currentXY.x;
        robotY = currentXY.y;
        robotTheta = robotTheta;//redundant I know....
        targetX = targetXY.x;
        targetY = targetXY.y;
//        should probably pass the above to the function...
        paintPathPlanner(robotX,robotY,robotTheta);
        showPlot();
#endif //VISUALIZER

#ifdef USE_MAP
       if(mapCount==0){
//            mapRobot(currentXY.x,currentXY.y,robotTheta);
            if(clearMapCount==0) clearMapSection(currentXY.x,currentXY.y,robotTheta);
            else clearMapCount = (clearMapCount+1)%CLEARMAPDELAY;
            mapVSICK(currentXY.x,currentXY.y,robotTheta);
//            mapVSICK(0,0,0);
#ifdef USE_LIDAR
            mapSICK(currentXY.x,currentXY.y,robotTheta);
#endif
            showMap();
//            printf("MAPPING\n");
       }
            mapCount= (mapCount+1)%MAPDELAY;

#endif //USE_MAP
        sendSpeed(lSpeed,rSpeed);
        Sleep(5);
    }
#ifdef DUMP_GPS
    fclose(fp);
#endif
    return 0;
}
/** \brief The machine learning classifier, results are stored in the ClusterData structs.
  * \param[in] cloud_in A pointer to the input point cloud.
  * \param[in/out] clusters_data An array of information holders for each cluster
  */
void
applyObjectClassification (const pcl::PointCloud<PointType>::Ptr cloud_in, boost::shared_ptr<std::vector<ClusterData> > &clusters_data)
{
  // Set up the machine learnin class
  pcl::SVMTrain ml_svm_training; // To train the classifier
  pcl::SVMClassify ml_svm_classify; // To classify

  std::vector<pcl::SVMData> featuresSet; // Create the input vector for the SVM class
  std::vector<std::vector<double> > predictionOut; // Prediction output vector
  // If the input model_filename exists, it starts the classification.
  // Otherwise it starts a new machine learning training.
  if (global_data.model.size() > 0)
  {
    if (!ml_svm_classify.loadClassifierModel (global_data.model.data()))
      return;
    pcl::console::print_highlight (stderr, "Loaded ");
    pcl::console::print_value (stderr, "%s ", global_data.model.data());

    // Copy the input vector for the SVM classification
    for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
      featuresSet.push_back ( (*clusters_data) [c_it].features);

    ml_svm_classify.setInputTrainingSet (featuresSet); // Set input clusters set
    ml_svm_classify.saveNormClassProblem ("data_input"); //Save clusters features
    ml_svm_classify.setProbabilityEstimates (1); // Estimates the probabilities
    ml_svm_classify.classification ();
  }
  else
  {
    // Currently: analyze on voxels of 0.08 x 0.08 x 0.08 meter with slight alteration based on cluster aggressiveness
    float resolution = 0.08 * global_data.scale / pow (0.5 + global_data.cagg, 2);
    // Create the viewer
    pcl::visualization::PCLVisualizer viewer ("cluster viewer");

    // Output classifier model name
    global_data.model.assign (global_data.cloud_name.data());
    global_data.model.append (".model");

    std::vector<bool> lab_cluster;// save whether a cluster is labelled
    std::vector<int> pt_clst_pos; // used to memorize in the total cloud, the point affiliation to the original cluster

    // fill the vector (1 = labelled, 0 = unlabelled)
    lab_cluster.resize ( (std::size_t) clusters_data->size ());
    for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
    {
      if ( (*clusters_data) [c_it].is_isolated)
      {
        (*clusters_data) [c_it].features.label = 0;
        lab_cluster[c_it] = 1;
      }
      else
        lab_cluster[c_it] = 0;
    }

    // Build a cloud with unlabelled clusters
    pcl::PointCloud<PointType>::Ptr fragm_cloud (new pcl::PointCloud<PointType>);

    // Initialize the viewer
    initVisualizer (viewer);
    PointType picked_point; // changed whether a mouse click occours. It saves the selected cluster index
    viewer.registerPointPickingCallback (&pp_callback, (void *) &picked_point);
    
    // Create a cloud with unlabelled clusters
    for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
      if (!lab_cluster[c_it])
      {
        pcl::PointCloud<PointType>::Ptr cluster (new pcl::PointCloud<PointType>);
        pcl::copyPointCloud (*cloud_in, * (*clusters_data) [c_it].indices, *cluster);
	
	// Downsample cluster
	pcl::VoxelGrid<PointType> sor;
	sor.setInputCloud (cluster);
        sor.setLeafSize (resolution, resolution, resolution);
	sor.filter (*cluster);
	
	// Copy cluster into a global cloud
        fragm_cloud->operator+= (*cluster);

        // Fill a vector to memorize the original affiliation of a point to the cluster
        for (int clust_pt = 0; clust_pt < cluster->size(); clust_pt++)
          pt_clst_pos.push_back (c_it);

        // Add cluster to the viewer
        std::stringstream cluster_name;
        cluster_name << "cluster" << c_it;
        pcl::visualization::PointCloudColorHandlerGenericField<PointType> rgb (cluster, "intensity");// Get color handler for the cluster cloud
        viewer.addPointCloud<PointType> (cluster, rgb, cluster_name.str().data());
      }
    
    // Create a tree for point searching in the total cloud
    pcl::KdTreeFLANN<pcl::PointXYZI> tree_;
    tree_.setInputCloud (fragm_cloud);

    // Visualize the whole cloud
    int selected = -1; // save the picked cluster
    bool stop = 0;
    while (!viewer.wasStopped())
    {
      viewer.registerKeyboardCallback (keyboardEventOccurred, (void*) &stop);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
      viewer.spinOnce (500);
      
      if (picked_point.x != 0.0f || picked_point.y != 0.0f || picked_point.z != 0.0f)  // if a point is clicked
      {
        std::vector<int> pointIdxNKNSearch (1);
        std::vector<float> pointNKNSquaredDistance (1);
	pcl::PointCloud<PointType>::Ptr cluster (new pcl::PointCloud<PointType>);
        
        tree_.nearestKSearch (picked_point, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
	selected = pt_clst_pos[pointIdxNKNSearch[0]];
	
	viewer.removePointCloud("cluster");
	pcl::copyPointCloud (*cloud_in, * (*clusters_data) [selected].indices, *cluster);
	pcl::visualization::PointCloudColorHandlerGenericField<PointType> rgb (cluster, "intensity");
	viewer.addPointCloud<PointType> (cluster, rgb, "cluster");

        picked_point.x = 0.0f;
        picked_point.y = 0.0f;
        picked_point.z = 0.0f;
      }
      
      if(selected != -1 && stop)
      {
	std::stringstream cluster_name;
        cluster_name << "cluster" << selected;
        lab_cluster[ selected ] = 1; // cluster is marked as labelled
        (*clusters_data) [ selected ].features.label = 1; // the cluster is set as a noise
        viewer.removePointCloud(cluster_name.str().data());
	viewer.removePointCloud("cluster");
        stop = 0;
	selected = -1;
      }
    }

    // Close the viewer
    viewer.close();

    // The remaining unlabelled clusters are marked as "good"
    for (int c_it = 0; c_it < lab_cluster.size(); c_it++)
    {
      if (!lab_cluster[c_it])
        (*clusters_data) [c_it].features.label = 0; // Mark remaining clusters as good
    }

    // Copy the input vector for the SVM classification
    for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
      featuresSet.push_back ( (*clusters_data) [c_it].features);

    // Setting the training classifier
    pcl::SVMParam trainParam;
    trainParam.probability = 1; // Estimates the probabilities
    trainParam.C = 512; // Initial C value of the classifier
    trainParam.gamma = 2; // Initial gamma value of the classifier

    ml_svm_training.setInputTrainingSet (featuresSet);  // Set input training set
    ml_svm_training.setParameters (trainParam);
    ml_svm_training.trainClassifier(); // Train the classifier
    ml_svm_training.saveClassifierModel (global_data.model.data()); // Save classifier model
    pcl::console::print_highlight (stderr, "Saved ");
    pcl::console::print_value (stderr, "%s ", global_data.model.data());
    ml_svm_training.saveTrainingSet ("data_input"); // Save clusters features normalized

    // Test the current classification
    ml_svm_classify.loadClassifierModel (global_data.model.data());
    ml_svm_classify.setInputTrainingSet (featuresSet);
    ml_svm_classify.setProbabilityEstimates (1);
    ml_svm_classify.classificationTest ();
  }

  ml_svm_classify.saveClassificationResult ("prediction"); // save prediction in outputtext file
  ml_svm_classify.getClassificationResult (predictionOut);

  // Get labels order
  std::vector<int> labels;
  ml_svm_classify.getLabel (labels);

  // Store the boolean output inside clusters_data
  for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
    switch ( (int) predictionOut[c_it][0])
    {
      case 0:
        (*clusters_data) [c_it].is_good = true;
        break;
      case 1:
        (*clusters_data) [c_it].is_ghost = true;
        break;
      case 2:
        (*clusters_data) [c_it].is_tree = true;
        break;
    }

  // Store the percentage output inside cluster_data
  for (size_t lab = 0; lab < labels.size (); lab++)
    switch (labels[lab])
    {
      case 0:
        for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
        {
          (*clusters_data) [c_it].is_good_prob = predictionOut[c_it][lab + 1];
        }
        break;
      case 1:
        for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
        {
          (*clusters_data) [c_it].is_ghost_prob = predictionOut[c_it][lab + 1];
        }
        break;
      case 2:
        for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
        {
          (*clusters_data) [c_it].is_tree_prob = predictionOut[c_it][lab + 1];
        }
        break;
    }
};