void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
mtx_.lock ();
if (!cloud_ || !normals_)
{
mtx_.unlock ();
return;
}
CloudConstPtr temp_cloud;
pcl::PointCloud<pcl::Normal>::Ptr temp_normals;
temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
temp_normals.swap (normals_);
mtx_.unlock ();
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_)
{
viz.removePointCloud ("normalcloud");
viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, temp_normals, 100, 0.05f, "normalcloud");
new_cloud_ = false;
}
}
void Inspector::updateDepth(const openni_wrapper::Image& image,
const openni_wrapper::DepthImage& depth)
{
frame_.depth_->setZero();
ushort data[depth.getHeight() * depth.getWidth()];
depth.fillDepthImageRaw(depth.getWidth(), depth.getHeight(), data);
int i = 0;
for(size_t y = 0; y < depth.getHeight(); ++y) {
for(size_t x = 0; x < depth.getWidth(); ++x, ++i) {
if(data[i] == depth.getNoSampleValue() || data[i] == depth.getShadowValue())
continue;
frame_.depth_->coeffRef(y, x) = data[i];
}
}
if(dddm_ && use_intrinsics_)
dddm_->undistort(frame_.depth_.get());
frame_.img_ = oniToCV(image);
Cloud::Ptr cloud(new Cloud);
FrameProjector proj;
proj.width_ = 640;
proj.height_ = 480;
proj.cx_ = proj.width_ / 2;
proj.cy_ = proj.height_ / 2;
proj.fx_ = 525;
proj.fy_ = 525;
proj.frameToCloud(frame_, cloud.get());
pcd_vis_.updatePointCloud(cloud, "cloud");
pcd_vis_.spinOnce(5);
}
//Draw the current particles
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
if (particles && new_cloud_)
{
//Set pointCloud with particle's points
pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
for (size_t i = 0; i < particles->points.size (); i++)
{
pcl::PointXYZ point;
point.x = particles->points[i].x;
point.y = particles->points[i].y;
point.z = particles->points[i].z;
particle_cloud->points.push_back (point);
}
//Draw red particles
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color (particle_cloud, 250, 99, 71);
if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud"))
viz.addPointCloud (particle_cloud, red_color, "particle cloud");
}
return true;
}
else
{
return false;
}
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_)
{
boost::this_thread::sleep(boost::posix_time::seconds(1));
return;
}
CloudConstPtr temp_cloud;
temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_ && normals_)
{
viz.removePointCloud ("normalcloud");
viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, normals_, 200, 0.05, "normalcloud");
new_cloud_ = false;
}
}
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
if (particles)
{
if (visualize_particles_)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
for (size_t i = 0; i < particles->points.size (); i++)
{
pcl::PointXYZ point;
point.x = particles->points[i].x;
point.y = particles->points[i].y;
point.z = particles->points[i].z;
particle_cloud->points.push_back (point);
}
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color (particle_cloud, 250, 99, 71);
if (!viz.updatePointCloud (particle_cloud, blue_color, "particle cloud"))
viz.addPointCloud (particle_cloud, blue_color, "particle cloud");
}
}
return true;
}
else
{
PCL_WARN ("no particles\n");
return false;
}
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
if (!cloud_ || !new_cloud_)
{
boost::this_thread::sleep (boost::posix_time::milliseconds (1));
return;
}
{
boost::mutex::scoped_lock lock (mtx_);
FPS_CALC ("visualization");
CloudPtr temp_cloud;
temp_cloud.swap (cloud_pass_);
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_ && cloud_hull_)
{
viz.removePointCloud ("hull");
viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
}
new_cloud_ = false;
}
}
//visualization's callback function
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_pass_)
{
std::this_thread::sleep_for(1s);
return;
}
//Draw downsampled point cloud from sensor
if (new_cloud_ && cloud_pass_downsampled_)
{
CloudPtr cloud_pass;
cloud_pass = cloud_pass_downsampled_;
if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
{
viz.addPointCloud (cloud_pass, "cloudpass");
viz.resetCameraViewpoint ("cloudpass");
}
bool ret = drawParticles (viz);
if (ret)
drawResult (viz);
}
new_cloud_ = false;
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, bool firstRun)
{
if(firstRun)
{
visualizer.addPointCloud(cloud, "cloud");
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud");
}
else
{
visualizer.updatePointCloud(cloud, "cloud");
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud");
}
}
void viz_cb (pcl::visualization::PCLVisualizer& viz)
{
// cout << "PbMapMaker::viz_cb(...)\n";
if (cloud1->empty())
{
boost::this_thread::sleep (boost::posix_time::milliseconds (10));
return;
}
{
// boost::mutex::scoped_lock lock (viz_mutex);
// viz.removeAllShapes();
viz.removeAllPointClouds();
{ //mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);
boost::mutex::scoped_lock updateLock(visualizationMutex);
// if (!viz.updatePointCloud (cloud, "sphereCloud"))
// viz.addPointCloud (sphereCloud, "sphereCloud");
if (!viz.updatePointCloud (cloud1, "cloud1"))
viz.addPointCloud (cloud1, "cloud1");
if (!viz.updatePointCloud (cloud2, "cloud2"))
viz.addPointCloud (cloud2, "cloud2");
if (!viz.updatePointCloud (cloud3, "cloud3"))
viz.addPointCloud (cloud3, "cloud3");
if (!viz.updatePointCloud (cloud4, "cloud4"))
viz.addPointCloud (cloud4, "cloud4");
if (!viz.updatePointCloud (cloud5, "cloud5"))
viz.addPointCloud (cloud5, "cloud5");
if (!viz.updatePointCloud (cloud6, "cloud6"))
viz.addPointCloud (cloud6, "cloud6");
if (!viz.updatePointCloud (cloud7, "cloud7"))
viz.addPointCloud (cloud7, "cloud7");
if (!viz.updatePointCloud (cloud8, "cloud8"))
viz.addPointCloud (cloud8, "cloud8");
updateLock.unlock();
}
}
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
{
double pointSize = 1.0;
if(firstRun)
{
visualizer.addPointCloud(dispCloudPtr_, std::string("stuff"));
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::string("stuff"));
}
else
{
visualizer.updatePointCloud(dispCloudPtr_, std::string("stuff"));
visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::string("stuff"));
}
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
{
const std::string cloudname = this->name;
if(firstRun)
{
visualizer.addPointCloud(dispCloud, cloudname);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
}
else
{
visualizer.updatePointCloud(dispCloud, cloudname);
visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
}
}
//Draw model reference point cloud
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
//move close to camera a little for better visualization
transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
CloudPtr result_cloud (new Cloud ());
pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
//Draw blue model reference point cloud
{
pcl::visualization::PointCloudColorHandlerCustom<RefPointType> blue_color (result_cloud, 0, 0, 255);
if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud"))
viz.addPointCloud (result_cloud, blue_color, "resultcloud");
}
}
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
// move a little bit for better visualization
transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
RefCloudPtr result_cloud (new RefCloud ());
if (!visualize_non_downsample_)
pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
else
pcl::transformPointCloud<RefPointType> (*reference_, *result_cloud, transformation);
{
pcl::visualization::PointCloudColorHandlerCustom<RefPointType> red_color (result_cloud, 0, 0, 255);
if (!viz.updatePointCloud (result_cloud, red_color, "resultcloud"))
viz.addPointCloud (result_cloud, red_color, "resultcloud");
}
}
template <typename PointT> void
tviewer::PointCloudObject<PointT>::refreshDataInVisualizer (pcl::visualization::PCLVisualizer& v)
{
v.updatePointCloud (data_, name_);
}
void PbMapMaker::viz_cb (pcl::visualization::PCLVisualizer& viz)
{
if (mPbMap.globalMapPtr->empty())
{
mrpt::system::sleep(10);
return;
}
{ mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);
// Render the data
{
viz.removeAllShapes();
viz.removeAllPointClouds();
char name[1024];
if(graphRepresentation)
{
for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
{
pcl::PointXYZ center(2*mPbMap.vPlanes[i].v3center[0], 2*mPbMap.vPlanes[i].v3center[1], 2*mPbMap.vPlanes[i].v3center[2]);
double radius = 0.1 * sqrt(mPbMap.vPlanes[i].areaVoxels);
sprintf (name, "sphere%u", static_cast<unsigned>(i));
viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name);
if( !mPbMap.vPlanes[i].label.empty() )
viz.addText3D (mPbMap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], mPbMap.vPlanes[i].label);
else
{
sprintf (name, "P%u", static_cast<unsigned>(i));
viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
// Draw edges
if(!configPbMap.graph_mode) // Nearby neighbors
for(set<unsigned>::iterator it = mPbMap.vPlanes[i].nearbyPlanes.begin(); it != mPbMap.vPlanes[i].nearbyPlanes.end(); it++)
{
if(*it > mPbMap.vPlanes[i].id)
break;
sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it));
pcl::PointXYZ center_it(2*mPbMap.vPlanes[*it].v3center[0], 2*mPbMap.vPlanes[*it].v3center[1], 2*mPbMap.vPlanes[*it].v3center[2]);
viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
}
else
for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[i].neighborPlanes.begin(); it != mPbMap.vPlanes[i].neighborPlanes.end(); it++)
{
if(it->first > mPbMap.vPlanes[i].id)
break;
sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
pcl::PointXYZ center_it(2*mPbMap.vPlanes[it->first].v3center[0], 2*mPbMap.vPlanes[it->first].v3center[1], 2*mPbMap.vPlanes[it->first].v3center[2]);
viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
char commonObs[8];
sprintf (commonObs, "%u", it->second);
pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 );
viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name);
}
}
}
else
{ // Regular representation
if (!viz.updatePointCloud (mPbMap.globalMapPtr, "cloud"))
viz.addPointCloud (mPbMap.globalMapPtr, "cloud");
if(mpPbMapLocaliser != NULL)
if(mpPbMapLocaliser->alignedModelPtr){
if (!viz.updatePointCloud (mpPbMapLocaliser->alignedModelPtr, "model"))
viz.addPointCloud (mpPbMapLocaliser->alignedModelPtr, "model");}
sprintf (name, "PointCloud size %u", static_cast<unsigned>( mPbMap.globalMapPtr->size() ) );
viz.addText(name, 10, 20);
for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
{
Plane &plane_i = mPbMap.vPlanes[i];
sprintf (name, "normal_%u", static_cast<unsigned>(i));
pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization
pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]);
pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
// Draw Ppal diretion
// if( plane_i.elongation > 1.3 )
// {
// sprintf (name, "ppalComp_%u", static_cast<unsigned>(i));
// pcl::PointXYZ pt3 = pcl::PointXYZ ( plane_i.v3center[0] + (0.2f * plane_i.v3PpalDir[0]),
// plane_i.v3center[1] + (0.2f * plane_i.v3PpalDir[1]),
// plane_i.v3center[2] + (0.2f * plane_i.v3PpalDir[2]));
// viz.addArrow (pt3, plane_i.pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
// }
// if( !plane_i.label.empty() )
// viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label);
// else
{
sprintf (name, "n%u", static_cast<unsigned>(i));
// sprintf (name, "n%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(plane_i.semanticGroup));
viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
// sprintf (name, "planeRaw_%02u", static_cast<unsigned>(i));
// viz.addPointCloud (plane_i.planeRawPointCloudPtr, name);// contourPtr, planePointCloudPtr, polygonContourPtr
// if(!configPbMap.makeClusters)
// {
// sprintf (name, "plane_%02u", static_cast<unsigned>(i));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[i%10], grn[i%10], blu[i%10]);
//// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
// viz.addPointCloud (plane_i.planePointCloudPtr, color, name);
// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
// }
// else
// {
// sprintf (name, "plane_%02u", static_cast<unsigned>(i));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
// viz.addPointCloud (plane_i.planePointCloudPtr, color, name);
// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
// }
// sprintf (name, "planeBorder_%02u", static_cast<unsigned>(i));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color2 (plane_i.contourPtr, 255, 255, 255);
// viz.addPointCloud (plane_i.contourPtr, color2, name);// contourPtr, planePointCloudPtr, polygonContourPtr
// //Edges
// if(mPbMap.edgeCloudPtr->size() > 0)
// {
// sprintf (name, "planeEdge_%02u", static_cast<unsigned>(i));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color4 (mPbMap.edgeCloudPtr, 255, 255, 0);
// viz.addPointCloud (mPbMap.edgeCloudPtr, color4, name);// contourPtr, planePointCloudPtr, polygonContourPtr
// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
//
// sprintf (name, "edge%u", static_cast<unsigned>(i));
// viz.addLine (mPbMap.edgeCloudPtr->points.front(), mPbMap.edgeCloudPtr->points.back(), ared[3], agrn[3], ablu[3], name);
// }
sprintf (name, "approx_plane_%02d", int (i));
viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name);
}
//if(configPbMap.makeClusters)
// for(map<unsigned, std::vector<unsigned> >::iterator it=clusterize->groups.begin(); it != clusterize->groups.end(); it++)
// for(size_t i=0; i < it->second.size(); i++)
// {
// unsigned planeID = it->second[i];
// Plane &plane_i = mPbMap.vPlanes[planeID];
// sprintf (name, "plane_%02u", static_cast<unsigned>(planeID));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[planeID%10], grn[planeID%10], blu[planeID%10]);
// viz.addPointCloud (plane_i.planePointCloudPtr, color, name);// contourPtr, planePointCloudPtr, polygonContourPtr
// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, name);
// }
// Draw recognized plane labels
if(mpPbMapLocaliser != NULL)
for(map<string, pcl::PointXYZ>::iterator it = mpPbMapLocaliser->foundPlaces.begin(); it != mpPbMapLocaliser->foundPlaces.end(); it++)
viz.addText3D (it->first, it->second, 0.3, 0.9, 0.9, 0.9, it->first);
}
}
}
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_pass_)
{
boost::this_thread::sleep (boost::posix_time::seconds (1));
return;
}
if (new_cloud_ && cloud_pass_downsampled_)
{
CloudPtr cloud_pass;
if (!visualize_non_downsample_)
cloud_pass = cloud_pass_downsampled_;
else
cloud_pass = cloud_pass_;
if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
{
viz.addPointCloud (cloud_pass, "cloudpass");
viz.resetCameraViewpoint ("cloudpass");
}
}
if (new_cloud_ && reference_)
{
bool ret = drawParticles (viz);
if (ret)
{
drawResult (viz);
// draw some texts
viz.removeShape ("N");
viz.addText ((boost::format ("number of Reference PointClouds: %d") % tracker_->getReferenceCloud ()->points.size ()).str (),
10, 20, 20, 1.0, 1.0, 1.0, "N");
viz.removeShape ("M");
viz.addText ((boost::format ("number of Measured PointClouds: %d") % cloud_pass_downsampled_->points.size ()).str (),
10, 40, 20, 1.0, 1.0, 1.0, "M");
viz.removeShape ("tracking");
viz.addText ((boost::format ("tracking: %f fps") % (1.0 / tracking_time_)).str (),
10, 60, 20, 1.0, 1.0, 1.0, "tracking");
viz.removeShape ("downsampling");
viz.addText ((boost::format ("downsampling: %f fps") % (1.0 / downsampling_time_)).str (),
10, 80, 20, 1.0, 1.0, 1.0, "downsampling");
viz.removeShape ("computation");
viz.addText ((boost::format ("computation: %f fps") % (1.0 / computation_time_)).str (),
10, 100, 20, 1.0, 1.0, 1.0, "computation");
viz.removeShape ("particles");
viz.addText ((boost::format ("particles: %d") % tracker_->getParticles ()->points.size ()).str (),
10, 120, 20, 1.0, 1.0, 1.0, "particles");
}
}
new_cloud_ = false;
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
{
const std::string &cloudname = this->name;
if(firstRun)
{
visualizer.addPointCloud(cloud, cloudname);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
}
else
{
visualizer.updatePointCloud(cloud, cloudname);
visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
visualizer.removeAllShapes();
}
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0.2, 0, 0), 1, 0, 0, "X");
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0.2, 0), 0, 1, 0, "Y");
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0, 0.2), 0, 0, 1, "Z");
tf::Vector3 origin = worldToCam * tf::Vector3(0, 0, 0);
tf::Vector3 lineX = worldToCam * tf::Vector3(0.2, 0, 0);
tf::Vector3 lineY = worldToCam * tf::Vector3(0, 0.2, 0);
tf::Vector3 lineZ = worldToCam * tf::Vector3(0, 0, 0.2);
pcl::PointXYZ pclOrigin(origin.x(), origin.y(), origin.z());
pcl::PointXYZ pclLineX(lineX.x(), lineX.y(), lineX.z());
pcl::PointXYZ pclLineY(lineY.x(), lineY.y(), lineY.z());
pcl::PointXYZ pclLineZ(lineZ.x(), lineZ.y(), lineZ.z());
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pclOrigin, 1, 1, 1, "line");
visualizer.addLine(pclOrigin, pclLineX, 1, 0, 0, "lineX");
visualizer.addLine(pclOrigin, pclLineY, 0, 1, 0, "lineY");
visualizer.addLine(pclOrigin, pclLineZ, 0, 0, 1, "lineZ");
for(int i = 0; i < regions.size(); ++i)
{
const Region ®ion = regions[i];
tf::Transform transform = worldToCam * region.transform;
std::ostringstream oss;
oss << "region_" << i;
tf::Vector3 originB = transform * tf::Vector3(0, 0, 0);
tf::Vector3 lineXB = transform * tf::Vector3(0.2, 0, 0);
tf::Vector3 lineYB = transform * tf::Vector3(0, 0.2, 0);
tf::Vector3 lineZB = transform * tf::Vector3(0, 0, 0.2);
pcl::PointXYZ pclOriginB(originB.x(), originB.y(), originB.z());
pcl::PointXYZ pclLineXB(lineXB.x(), lineXB.y(), lineXB.z());
pcl::PointXYZ pclLineYB(lineYB.x(), lineYB.y(), lineYB.z());
pcl::PointXYZ pclLineZB(lineZB.x(), lineZB.y(), lineZB.z());
visualizer.addLine(pclOrigin, pclOriginB, 1, 1, 1, "line_" + oss.str());
visualizer.addLine(pclOriginB, pclLineXB, 1, 0, 0, "lineX_" + oss.str());
visualizer.addLine(pclOriginB, pclLineYB, 0, 1, 0, "lineY_" + oss.str());
visualizer.addLine(pclOriginB, pclLineZB, 0, 0, 1, "lineZ_" + oss.str());
Eigen::Vector3d translation;
Eigen::Quaterniond rotation;
tf::vectorTFToEigen(transform.getOrigin(), translation);
tf::quaternionTFToEigen(transform.getRotation(), rotation);
visualizer.addCube(translation.cast<float>(), rotation.cast<float>(), region.width, region.height, region.depth, oss.str());
}
}
void PbMapVisualizer::viz_cb (pcl::visualization::PCLVisualizer& viz)
{
if (pbmap.globalMapPtr->empty())
{
mrpt::system::sleep(10);
return;
}
// Render the data
{
viz.removeAllShapes();
viz.removeAllPointClouds();
char name[1024];
if(graphRepresentation)
{
// cout << "show graphRepresentation\n";
for(size_t i=0; i<pbmap.vPlanes.size(); i++)
{
pcl::PointXYZ center(2*pbmap.vPlanes[i].v3center[0], 2*pbmap.vPlanes[i].v3center[1], 2*pbmap.vPlanes[i].v3center[2]);
double radius = 0.1 * sqrt(pbmap.vPlanes[i].areaVoxels);
// cout << "radius " << radius << endl;
sprintf (name, "sphere%u", static_cast<unsigned>(i));
viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name);
if( !pbmap.vPlanes[i].label.empty() )
viz.addText3D (pbmap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], pbmap.vPlanes[i].label);
else
{
sprintf (name, "P%u", static_cast<unsigned>(i));
viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
// Draw edges
// for(set<unsigned>::iterator it = pbmap.vPlanes[i].nearbyPlanes.begin(); it != pbmap.vPlanes[i].nearbyPlanes.end(); it++)
// {
// if(*it > pbmap.vPlanes[i].id)
// break;
//
// sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it));
// pcl::PointXYZ center_it(2*pbmap.vPlanes[*it].v3center[0], 2*pbmap.vPlanes[*it].v3center[1], 2*pbmap.vPlanes[*it].v3center[2]);
// viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
// }
for(map<unsigned,unsigned>::iterator it = pbmap.vPlanes[i].neighborPlanes.begin(); it != pbmap.vPlanes[i].neighborPlanes.end(); it++)
{
if(it->first > pbmap.vPlanes[i].id)
break;
sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
pcl::PointXYZ center_it(2*pbmap.vPlanes[it->first].v3center[0], 2*pbmap.vPlanes[it->first].v3center[1], 2*pbmap.vPlanes[it->first].v3center[2]);
viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
char commonObs[8];
sprintf (commonObs, "%u", it->second);
pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 );
viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name);
}
}
}
else
{ // Regular representation
if (!viz.updatePointCloud (pbmap.globalMapPtr, "cloud"))
viz.addPointCloud (pbmap.globalMapPtr, "cloud");
sprintf (name, "PointCloud size %u", static_cast<unsigned>( pbmap.globalMapPtr->size() ) );
viz.addText(name, 10, 20);
for(size_t i=0; i<pbmap.vPlanes.size(); i++)
{
Plane &plane_i = pbmap.vPlanes[i];
// sprintf (name, "normal_%u", static_cast<unsigned>(i));
name[0] = *(mrpt::format("normal_%u", static_cast<unsigned>(i)).c_str());
pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization
pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]);
pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
if( !plane_i.label.empty() )
viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label);
else
{
sprintf (name, "n%u", static_cast<unsigned>(i));
viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
sprintf (name, "approx_plane_%02d", int (i));
viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name);
}
}
}
}
