void addCentroids(pcl::visualization::PCLVisualizer &visualizer, const std::string &name) { // pcl::PointCloud< pcl::PointXYZRGBA>::Ptr voxel_centroid_cloud = supervoxels->getVoxelCentroidCloud(); visualizer.addPointCloud(supervoxelcloud, name); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, name); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.95, name); }
void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*) { if (event.keyUp()) { double opacity; switch (event.getKeyCode()) { case '1': viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges"); break; case '2': viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges"); break; case '3': viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges"); break; case '4': viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges"); break; case '5': viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges"); break; } } }
void addVoxels(pcl::visualization::PCLVisualizer &visualizer, const std::string &name) { pcl::PointCloud< pcl::PointXYZRGBA>::Ptr colored_voxel_cloud = supervoxels->getColoredVoxelCloud(); visualizer.addPointCloud(colored_voxel_cloud, name); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, name); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.8, name); }
void viz_cb (pcl::visualization::PCLVisualizer& viz) { static bool first_time = true; double psize = 1.0,opacity = 1.0,linesize =1.0; std::string cloud_name ("cloud"); boost::mutex::scoped_lock l(m_mutex); if (new_cloud) { //typedef pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> ColorHandler; typedef pcl::visualization::PointCloudColorHandlerGenericField <pcl::PointXYZRGBNormal> ColorHandler; //ColorHandler Color_handler (normal_cloud); ColorHandler Color_handler (normal_cloud,"curvature"); if (!first_time) { viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name); viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name); viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name); //viz.removePointCloud ("normalcloud"); viz.removePointCloud ("cloud"); } else first_time = false; //viz.addPointCloudNormals<pcl::PointXYZRGBNormal> (normal_cloud, 139, 0.1, "normalcloud"); viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, std::string("cloud"), 0); viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name); viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name); viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name); new_cloud = false; } }
template <typename PointT> void tviewer::PointCloudObject<PointT>::addDataToVisualizer (pcl::visualization::PCLVisualizer& v) { v.addPointCloud (data_, name_); v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, name_); v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, visibility_, name_); if (use_fixed_color_ != 0) { float r, g, b; std::tie (r, g, b) = getRGBFromColor (color_); v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, r, g, b, name_); } }
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 OnNewMapFrame(pcl::PointCloud< pcl::PointXYZ > mapFrame) { _viewer.removeAllPointClouds(0); _viewer.addPointCloud(mapFrame.makeShared(), "map"); _viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "map"); _viewer.spinOnce(); }
void filterAdjacency(pcl::visualization::PCLVisualizer &visualizer) { visualizer.removeAllPointClouds(); visualizer.removeAllShapes(); std::multimap<uint32_t, uint32_t> supervoxel_adjacency; supervoxels->getSupervoxelAdjacency(supervoxel_adjacency); pcl::PointCloud< pcl::PointXYZRGBA>::Ptr filtered_supervoxel(new pcl::PointCloud< pcl::PointXYZRGBA>); for(std::multimap<uint32_t, uint32_t>::iterator label_itr = supervoxel_adjacency.begin(); label_itr != supervoxel_adjacency.end();) { //First get the label uint32_t supervoxel_label = label_itr->first; //Now get the supervoxel corresponding to the label pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr supervoxel = supervoxel_clusters.at(supervoxel_label); int threshold = 7 - supervoxel->centroid_.z * 0.375; int count = supervoxel_adjacency.count(supervoxel_label); if(count >= threshold) { //Now we need to iterate through the adjacent supervoxels and make a point cloud of them pcl::PointCloud< pcl::PointXYZRGBA>::Ptr adjacent_supervoxel_centers(new pcl::PointCloud< pcl::PointXYZRGBA>); std::multimap<uint32_t, uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range(supervoxel_label).first; for(; adjacent_itr != supervoxel_adjacency.equal_range(supervoxel_label).second; ++adjacent_itr) { uint32_t snd_label = adjacent_itr->second; int snd_count = supervoxel_adjacency.count(snd_label); pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr neighbor_supervoxel = supervoxel_clusters.at(snd_label); int snd_threshold = 7 - neighbor_supervoxel->centroid_.z * 0.375; if(snd_count >= snd_threshold) { adjacent_supervoxel_centers->push_back(neighbor_supervoxel->centroid_); filtered_supervoxel->push_back(neighbor_supervoxel->centroid_); } } //Now we make a name for this polygon std::stringstream ss; ss << "supervoxel_" << supervoxel_label; //This function generates a "star" polygon mesh from the points given addSupervoxelConnectionsToViewer(supervoxel->centroid_, *adjacent_supervoxel_centers, ss.str(), visualizer); } label_itr = supervoxel_adjacency.upper_bound(supervoxel_label); } const std::string foo = "fassdfasdf"; visualizer.addPointCloud(filtered_supervoxel, foo); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, foo); }
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); } }
void viewerUpdate(pcl::visualization::PCLVisualizer& viewer) { std::stringstream ss; ss << "Rawlog entry: " << rawlogEntry; viewer.removeShape ("text", 0); viewer.addText (ss.str(), 10,50, "text", 0); static mrpt::system::TTimeStamp last_time = INVALID_TIMESTAMP; { // Mutex protected std::lock_guard<std::mutex> lock(td_cs); if (td.new_timestamp!=last_time) { last_time = td.new_timestamp; viewer.removePointCloud("cloud", 0); viewer.addPointCloud (td.new_cloud,"cloud",0); viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3.0); const size_t N = td.new_cloud->size(); std::cout << "Showing new point cloud of size=" << N << std::endl; static bool first = true; if (N && first) { first = false; //viewer.resetCameraViewpoint("cloud"); } #if 0 std::cout << mrpt::format( "camera: clip = %f %f\n" " focal = %f %f %f\n" " pos = %f %f %f\n" " view = %f %f %f\n", viewer.camera_.clip[0],viewer.camera_.clip[1], viewer.camera_.focal[0],viewer.camera_.focal[1],viewer.camera_.focal[2], viewer.camera_.pos[0],viewer.camera_.pos[1],viewer.camera_.pos[2], viewer.camera_.view[0],viewer.camera_.view[1],viewer.camera_.view[2]); #endif } } }
void viz_cb (pcl::visualization::PCLVisualizer& viz) { boost::mutex::scoped_lock lock (mtx_); if (!keypoints_ && !cloud_) { boost::this_thread::sleep(boost::posix_time::seconds(1)); return; } FPS_CALC ("visualization"); viz.removePointCloud ("raw"); pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color_handler (cloud_); viz.addPointCloud<pcl::PointXYZRGBA> (cloud_, color_handler, "raw"); if (!viz.updatePointCloud<pcl::PointXYZ> (keypoints_, "keypoints")) { viz.addPointCloud<pcl::PointXYZ> (keypoints_, "keypoints"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5.0, "keypoints"); viz.resetCameraViewpoint ("keypoints"); } }
void compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, float th_dd, int max_search) { CloudPtr cloud (new Cloud); fromROSMsg (*input, *cloud); pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>); pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setNormalSmoothingSize (10.0f); ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR); ne.setInputCloud (cloud); ne.compute (*normal); TicToc tt; tt.tic (); //OrganizedEdgeBase<PointXYZRGBA, Label> oed; //OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed; //OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed; OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed; oed.setInputNormals (normal); oed.setInputCloud (cloud); oed.setDepthDisconThreshold (th_dd); oed.setMaxSearchNeighbors (max_search); oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY); PointCloud<Label> labels; std::vector<PointIndices> label_indices; oed.compute (labels, label_indices); print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n"); // Make gray point clouds for (int idx = 0; idx < (int)cloud->points.size (); idx++) { uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3; cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray; } // Display edges in PCLVisualizer viewer.setSize (640, 480); viewer.addCoordinateSystem (0.2f); viewer.addPointCloud (cloud, "original point cloud"); viewer.registerKeyboardCallback(&keyboard_callback); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges); pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges); pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges); pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges); pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges); const int point_size = 2; viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges"); while (!viewer.wasStopped ()) { viewer.spinOnce (); pcl_sleep(0.1); } // Combine point clouds and edge labels sensor_msgs::PointCloud2 output_edges; toROSMsg (labels, output_edges); concatenateFields (*input, output_edges, output); }
void addNormals(pcl::visualization::PCLVisualizer &visualizer, const std::string &name) { pcl::PointCloud< pcl::PointNormal>::Ptr sv_normal_cloud = supervoxels->makeSupervoxelNormalCloud(supervoxel_clusters); visualizer.addPointCloudNormals<pcl::PointNormal> (sv_normal_cloud, 1, 0.05f, name); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, normalsColor[0], normalsColor[1], normalsColor[2], name); }
void run(pcl::RFFaceDetectorTrainer & fdrf, typename pcl::PointCloud<PointInT>::Ptr & scene_vis, pcl::visualization::PCLVisualizer & vis, bool heat_map, bool show_votes, const std::string & filename) { pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud (*scene_vis, *scene); fdrf.setInputCloud (scene); if (heat_map) { pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>); fdrf.setFaceHeatMapCloud (intensity_cloud); } fdrf.detectFaces (); typedef typename pcl::traits::fieldList<PointInT>::type FieldListM; double rgb_m; bool exists_m; pcl::for_each_type < FieldListM > (pcl::CopyIfFieldExists<PointInT, double> (scene_vis->points[0], "rgb", exists_m, rgb_m)); std::cout << "Color exists:" << static_cast<int> (exists_m) << std::endl; if (exists_m) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr to_visualize (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::copyPointCloud (*scene_vis, *to_visualize); pcl::visualization::PointCloudColorHandlerRGBField < pcl::PointXYZRGB > handler_keypoints (to_visualize); vis.addPointCloud < pcl::PointXYZRGB > (to_visualize, handler_keypoints, "scene_cloud"); } else { vis.addPointCloud (scene_vis, "scene_cloud"); } if (heat_map) { pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>); fdrf.getFaceHeatMap (intensity_cloud); pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity"); vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map"); } if (show_votes) { //display votes_ /*pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud(new pcl::PointCloud<pcl::PointXYZ>()); fdrf.getVotes(votes_cloud); pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes(votes_cloud, 255, 0, 0); vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud"); vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud"); vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud"); vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");*/ pcl::PointCloud<pcl::PointXYZI>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZI> ()); fdrf.getVotes2 (votes_cloud); pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_votes (votes_cloud, "intensity"); vis.addPointCloud < pcl::PointXYZI > (votes_cloud, handler_votes, "votes_cloud"); vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud"); } vis.addCoordinateSystem (0.1, "global"); std::vector<Eigen::VectorXd> heads; fdrf.getDetectedFaces (heads); face_detection_apps_utils::displayHeads (heads, vis); if (SHOW_GT) { //check if there is ground truth data std::string pose_file (filename); boost::replace_all (pose_file, ".pcd", "_pose.txt"); Eigen::Matrix4d pose_mat; pose_mat.setIdentity (4, 4); bool result = face_detection_apps_utils::readMatrixFromFile (pose_file, pose_mat); if (result) { Eigen::Vector3d ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2); Eigen::Vector3d trans_vector = Eigen::Vector3d (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3)); std::cout << ea << std::endl; std::cout << trans_vector << std::endl; pcl::PointXYZ center_point; center_point.x = trans_vector[0]; center_point.y = trans_vector[1]; center_point.z = trans_vector[2]; vis.addSphere (center_point, 0.05, 255, 0, 0, "sphere"); pcl::ModelCoefficients cylinder_coeff; cylinder_coeff.values.resize (7); // We need 7 values cylinder_coeff.values[0] = center_point.x; cylinder_coeff.values[1] = center_point.y; cylinder_coeff.values[2] = center_point.z; cylinder_coeff.values[3] = ea[0]; cylinder_coeff.values[4] = ea[1]; cylinder_coeff.values[5] = ea[2]; Eigen::Vector3d vec = Eigen::Vector3d::UnitZ () * -1.; Eigen::Matrix3d matrixxx; matrixxx = Eigen::AngleAxisd (ea[0], Eigen::Vector3d::UnitX ()) * Eigen::AngleAxisd (ea[1], Eigen::Vector3d::UnitY ()) * Eigen::AngleAxisd (ea[2], Eigen::Vector3d::UnitZ ()); //matrixxx = pose_mat.block<3,3>(0,0); vec = matrixxx * vec; cylinder_coeff.values[3] = vec[0]; cylinder_coeff.values[4] = vec[1]; cylinder_coeff.values[5] = vec[2]; cylinder_coeff.values[6] = 0.01; vis.addCylinder (cylinder_coeff, "cylinder"); } } vis.setRepresentationToSurfaceForAllActors (); if (VIDEO) { vis.spinOnce (50, true); } else { vis.spin (); } vis.removeAllPointClouds (); vis.removeAllShapes (); }
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()); } }