// gestione evento di selezione di un punto della cloud in visuale doppia
    void VisualizationUtils::pointPickDoubleViewEvent(const pcl::visualization::PointPickingEvent& event, void* args) {
        POINT_TYPE current_point;
        isSecondCloud = false;

        cout << " -> selezionato il punto numero " << event.getPointIndex();
        event.getPoint(current_point.x, current_point.y, current_point.z);

        // verifico la validità del punto
        if ((current_point.z > TRANLSATION_Z_SECOND_CLOUD ? points_right : points_left).checkPointError(event.getPointIndex(), current_point)) {
            cout << " -> punto non valido!" << endl;
            return;
        }
        // se è un punto valido consento l'annullamento e procedo ad inserirlo
        canIRollBack = true;

        // identifico prima o seconda cloud
        if (current_point.z > TRANLSATION_Z_SECOND_CLOUD)
            isSecondCloud = true;

        cout << " - coordinate (" << current_point.x << ", " << current_point.y << ", " << current_point.z << ") ";
        cout << "cloud " << (isSecondCloud ? "destra." : "sinistra.") << endl;

        // coloro il punto
        current_point = (isSecondCloud ? color_right : color_left).colorize(current_point);
        // lo aggiungo alla nuovola dei selezionati
        (isSecondCloud ? clicked_points2 : clicked_points)->points.push_back(current_point);
        updateClickedPoints();
        // salvo i punti presi
        (isSecondCloud ? points_right : points_left).add(event.getPointIndex(), current_point);

        // passo al colore successivo
        (isSecondCloud ? color_right : color_left).nextColor();
    }
Esempio n. 2
0
 void pointpickingEventOccurred (const pcl::visualization::PointPickingEvent &event, void* viewer_void)
 {
 	int indx;
 	float x,y,z;
         indx = event.getPointIndex();
 	if(indx == -1)
          return;
 //get co-ordinates of point clicked
         event.getPoint(x,y,z);

// 	if(point_count%2 == 1)
// 	{
 		first.x = x;
 		first.y = y;
 		first.z = z;
 		std::cout<<"First point "<<first.x<<" , "<<first.y<<" , "<<first.z<<std::endl;
 		point_count++;
// 	}
// 	else if(point_count%2 == 0)
// 	{
// 		second.x = x;
// 		second.y = y;
// 		second.z = z;
// 		std::cout<<"Second point "<<second.x<<" , "<<second.y<<" , "<<second.z<<std::endl;
// 		std::cout<<"Euclidean Distance : "<<sqrt(pow(first.x - second.x,2) +  pow(first.y-second.y,2)  + pow(first.z-second.z,2))<<std::endl;
// 		point_count++;
// 	}
 }
void
pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
{
  int idx = event.getPointIndex ();
  if (idx == -1)
    return;

  if (!cloud)
  {
    cloud = *reinterpret_cast<pcl::PCLPointCloud2::Ptr*> (cookie);
    xyzcloud.reset (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2 (*cloud, *xyzcloud);
    search.setInputCloud (xyzcloud);
  }
  // Return the correct index in the cloud instead of the index on the screen
  std::vector<int> indices (1);
  std::vector<float> distances (1);

  // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
  pcl::PointXYZ picked_pt;
  event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
  //TODO: Look into this.
  search.nearestKSearch (picked_pt, 1, indices, distances);

  PCL_INFO ("Point index picked: %d (real: %d) - [%f, %f, %f]\n", idx, indices[0], picked_pt.x, picked_pt.y, picked_pt.z);

  idx = indices[0];
  // If two points were selected, draw an arrow between them
  pcl::PointXYZ p1, p2;
  if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
  {
    std::stringstream ss;
    ss << p1 << p2;
    p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
    return;
  }

  // Else, if a single point has been selected
  std::stringstream ss;
  ss << idx;
  // Get the cloud's fields
  for (size_t i = 0; i < cloud->fields.size (); ++i)
  {
    if (!isMultiDimensionalFeatureField (cloud->fields[i]))
      continue;
    PCL_INFO ("Multidimensional field found: %s\n", cloud->fields[i].name.c_str ());
#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
    ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, ss.str ());
    ph_global.renderOnce ();
#endif
  }
  if (p)
  {
    pcl::PointXYZ pos;
    event.getPoint (pos.x, pos.y, pos.z);
    p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
  }
  
}
Esempio n. 4
0
void Ui::pointPickCallback(const pcl::visualization::PointPickingEvent& event, void* cookie)
{
    Ui *ui = (Ui*)cookie;
    float x,y,z;
    if (event.getPointIndex() == -1)
        ui->statusBar()->showMessage(tr("No point was clicked"));
    else
    {
        event.getPoint(x,y,z);
        ui->statusBar()->showMessage(QString("Point Clicked index: %1 x: %2 y: %3 z: %4")
                                 .arg(event.getPointIndex())
                                 .arg(x)
                                 .arg(y)
                                 .arg(z)
                                 );
    }
}
void
pp_callback (const pcl::visualization::PointPickingEvent &event, void *point)
{
  if (event.getPointIndex () == -1)
    return;
  PointType *idx;
  idx = static_cast<PointType *> (point);
  // A single point has been selected
  event.getPoint ( (*idx).x, (*idx).y, (*idx).z);
}
Esempio n. 6
0
void Ui::pointPickCallbackSelectComponent(const pcl::visualization::PointPickingEvent& event, void* cookie)
{
    Ui *ui = (Ui*)cookie;
    float x,y,z;
    if (event.getPointIndex() == -1)
        ui->statusBar()->showMessage(tr("No point was clicked"));
    else
    {
        event.getPoint(x,y,z);
        ui->statusBar()->showMessage(QString("Point Clicked index: %1 x: %2 y: %3 z: %4")
                                 .arg(event.getPointIndex())
                                 .arg(x)
                                 .arg(y)
                                 .arg(z)
                                 );
        QPushButton *colorbox = ui->getComponentDialog()->findChild<QPushButton *>("colorbox");
        colorbox->setStyleSheet(colorToStyleSheet(ui->getMotor()->getPointColor(event.getPointIndex())));
        ui->getMotor()->componentSelection(event.getPointIndex());
        ui->getDialogViewer()->updatePointCloud(ui->getMotor()->getNewComponentCloud(),"cloud");
    }
}
Esempio n. 7
0
void Inspector::pointPickingCallback(const pcl::visualization::PointPickingEvent& event, void* cookie)
{
  if(event.getPointIndex() == -1)
    return;
    
  Point pt;
  event.getPoint(pt.x, pt.y, pt.z);
  cout << "Selected point: " << pt.x << ", " << pt.y << ", " << pt.z << endl;
  pcd_vis_.removeAllShapes();
  
  Point origin(0, 0, 0);
  pcd_vis_.addArrow<Point, Point>(origin, pt, 0.0, 0.0, 0.8, 0.9, 0.9, 0.9, "line");
}
Esempio n. 8
0
void ICPWidget::pick(const pcl::visualization::PointPickingEvent & event, void*)
{
    if(currentState!=PICK_PATCH)return;
    if(segCloud->empty())return;
//    if(_PickedIndex.size()>=5)return;
    unsigned int idx = _PickedIndex.size();
    idx%=Pipe::FalseColorNum;
    if(-1==idmap.at<int>(0,event.getPointIndex()))
    {
        Pipe::inform("outlier picked");
        return;
    }
    _PickedIndex.push_back(event.getPointIndex());
    FullPoint p = segCloud->at(_PickedIndex[idx]);
    QString name;
    name = name.sprintf("%d",idx);
    float r,g,b;
    r = float(Pipe::FalseColor[idx][0])/255.0;
    g = float(Pipe::FalseColor[idx][1])/255.0;
    b = float(Pipe::FalseColor[idx][2])/255.0;
    v.addSphere (p,0.05,r,g,b,name.toStdString());
    widget.update();
}
Esempio n. 9
0
void
pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
{
  if (event.getPointIndex () == -1)
    return;
  sensor_msgs::PointCloud2::Ptr cloud = *static_cast<sensor_msgs::PointCloud2::Ptr*>(cookie);
  if (!cloud)
    return;

  // If two points were selected, draw an arrow between them
  pcl::PointXYZ p1, p2;
  if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
  {
    std::stringstream ss;
    ss << p1 << p2;
    p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
    return;
  }

  // Else, if a single point has been selected
  std::stringstream ss;
  ss << event.getPointIndex ();
  // Get the cloud's fields
  for (size_t i = 0; i < cloud->fields.size (); ++i)
  {
    if (!isMultiDimensionalFeatureField (cloud->fields[i]))
      continue;
    ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, event.getPointIndex (), ss.str ());
  }
  if (p)
  {
    pcl::PointXYZ pos;
    event.getPoint (pos.x, pos.y, pos.z);
    p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
  }
  ph_global.spinOnce ();
}
Esempio n. 10
0
void pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
{
	struct callback_args* data = (struct callback_args *)args;
	if (event.getPointIndex() == -1)
		return;
	PointT current_point;
	event.getPoint(current_point.x, current_point.y, current_point.z);
	data->clicked_points_3d->points.push_back(current_point);
	// Draw clicked points in red:
	pcl::visualization::PointCloudColorHandlerCustom<PointT> red(data->clicked_points_3d, 255, 0, 0);
	data->viewerPtr->removePointCloud("clicked_points");
	data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
	data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
	std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;
}
Esempio n. 11
0
//TF Customization added from example to print out xyz from picking of point from cloud
void pp_callback (const pcl::visualization::PointPickingEvent& event, void* viewer_void) //was (pp_callback (const pcl::visualization::PointPickingEvent& event, void* viewer_void)
{ 
  float x,y,z;
  cout << "pp_callback" << endl;       
  if (event.getPointIndex () == -1)
  {
    cout << "pointindex = -1" << endl;
    return;
  }
  //std::cout << event.getPointIndex (); 
  //TF  testing xyz out
  event.getPoint(x,y,z);
  cout << x << "  " << y << " " << z << endl;

}
template <typename PointT> void
open_ptrack::detection::GroundplaneEstimation<PointT>::pp_callback (const pcl::visualization::PointPickingEvent& event, void* args)
{
    struct callback_args_color* data = (struct callback_args_color *)args;
    if (event.getPointIndex () == -1)
        return;
    pcl::PointXYZRGB current_point;
    event.getPoint(current_point.x, current_point.y, current_point.z);
    data->clicked_points_3d->points.push_back(current_point);
    // Draw clicked points in red:
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red (data->clicked_points_3d, 255, 0, 0);
    data->viewerPtr->removePointCloud("clicked_points");
    data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
    data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
    std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;
}
void FindPickedPoint(const pcl::visualization::PointPickingEvent& event) {
	int idx = event.getPointIndex ();
	if (idx == -1)
	{
		std::cout << "Invalid pick!\n;";
		return;
	}
	search.setInputCloud(build_cloud_accurate_plane);

	// Return the correct index in the cloud instead of the index on the screen
	std::vector<int> indices (1);
	std::vector<float> distances (1);

	// Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
	pcl::PointXYZ picked_pt;
	event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
	search.nearestKSearch (picked_pt, 1, indices, distances);
	picked_points.push_back(picked_pt);
}
 void pointpickingEventOccurred (const pcl::visualization::PointPickingEvent &event, void* viewer_void)
 {
 	int indx;
 	float x,y,z;
    indx = event.getPointIndex();
 	if(indx == -1)
          return;
 	std::cout << indx << " " << std::endl;
    event.getPoint(x,y,z);

    first.x = x;
 	first.y = y;
 	first.z = z;
 	std::cout<<"First point "<<first.x<<" , "<<first.y<<" , "<<first.z<<std::endl;

 	// Green region detection
 	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr final_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
 	final_cloud->width    = cloud->width;
 	final_cloud->height   = cloud->height;
 	final_cloud->resize (cloud->width * cloud->height);
 	//


 }