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++;
// }
}
// 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();
}
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 ());
}
}
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);
}
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");
}
//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;
}
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;
}
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;
}
static void pointPickingEventHandler(const pcl::visualization::PointPickingEvent e) {
float x,y,z;
e.getPoint(x,y,z);
PointXYZ p2 = PointXYZ(x,y,z);
PointXYZ p1 = PointXYZ(x+100,y+100,z+100);
// Get random number as string to number arrows.
// Arrows must have different names.
int random_number_arrow = rand() % 1000;
std::ostringstream oss_arrow;
oss_arrow << random_number_arrow;
// Get random number as string to number arrows.
// Arrows must have different names.
int random_number_text = rand() % 1000;
std::ostringstream oss_text;
oss_text << random_number_text;
// Add text nearby marked point.
std::ostringstream oss2;
oss2 << "X: " << x << "Y: " << y << "Z: " << z ;
// Adds an arrow between points p1 and p2 and distance between them.
viewer->addArrow(p1,p2,0.5, 0.5, 0.0,oss_arrow.str());
viewer->addText3D(oss2.str(),p2,1.0,1.0,1.0,1.0,oss_text.str());
}
//remember to shift-click!
void getClickedPoint(const pcl::visualization::PointPickingEvent& event,
void* args){
/*int n = event.getPointIndex();
if (n == -1){
cout << "Got no point" << endl;
return;
}
desired_color = getCloudColorAt((size_t) n);*/
pcl::search::KdTree<pcl::PointXYZRGB> search;
search.setInputCloud(cloud);
pcl::PointXYZRGB picked_pt;
event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z);
vector<float> distances(1);
vector<int> search_indices(1);
search.nearestKSearch (picked_pt, 1, search_indices, distances);
desired_color = getCloudColorAt( (size_t) search_indices[0]);
cout << "Desired color: " << (int) desired_color.r << ", " <<
(int) desired_color.g << ", " << (int) desired_color.b << endl;
has_desired_color = true;
}
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 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 Viewer::pointPickingCallback( const pcl::visualization::PointPickingEvent& event, void* data ) {
pcl::PointXYZ p;
event.getPoint( p.x, p.y, p.z );
std::cout << "picked " << p.x << " " << p.y << " " << p.z << "\n";
selectedPoint = p;
viewer->removeShape( "selected_point" );
viewer->addSphere( p, 0.025, 0, 1.0, 0, "selected_point" );
}
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");
}
}
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();
}
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 ();
}
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);
//
}