void S2ObjectViewer::display() { LOG(LTRACE) << "S2ObjectViewer::display"; pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud_1_temp = in_cloud.read(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1(new pcl::PointCloud<pcl::PointXYZ>()); pcl::copyPointCloud(*in_cloud_1_temp, *cloud_1); pcl::PointCloud<PointXYZSHOT>::Ptr shots = in_shots.read(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>()); pcl::copyPointCloud(*shots, *cloud_2); pcl::PointCloud<PointXYZSIFT>::Ptr sifts = in_sifts.read(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_3(new pcl::PointCloud<pcl::PointXYZ>()); pcl::copyPointCloud(*sifts, *cloud_3); std::vector<int> indices; cloud_1->is_dense = false; pcl::removeNaNFromPointCloud(*cloud_1, *cloud_1, indices); viewer->removeAllPointClouds(); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue ( cloud_1, cloud_r, cloud_g, cloud_b); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green ( cloud_2, shot_r, shot_g, shot_b); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red ( cloud_3, sift_r, sift_g, sift_b); viewer->addPointCloud<pcl::PointXYZ> (cloud_1, blue, "cloud"); viewer->addPointCloud<pcl::PointXYZ> (cloud_2, green, "shots"); viewer->addPointCloud<pcl::PointXYZ> (cloud_3, red, "sifts"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, cloud_size, "cloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, sift_size, "sifts"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, shot_size, "shots"); }
void CorrespondencesViewer::display() { LOG(LTRACE) << "CorrespondencesViewer::display"; pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud_1_temp = in_cloud.read(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1(new pcl::PointCloud<pcl::PointXYZ>()); pcl::copyPointCloud(*in_cloud_1_temp, *cloud_1); pcl::PointCloud<PointXYZSHOT>::Ptr shots = in_shots.read(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>()); pcl::copyPointCloud(*shots, *cloud_2); std::vector<int> indices; cloud_1->is_dense = false; pcl::removeNaNFromPointCloud(*cloud_1, *cloud_1, indices); //pcl::VoxelGrid<pcl::PointXYZ> vg; //vg.setLeafSize (0.005, 0.005, 0.005); //vg.setInputCloud (cloud_1); //vg.filter (*cloud_1); /* pcl::PointCloud<pcl::PointXYZ>::Ptr in_src_temp = in_src.read(); pcl::PointCloud<pcl::PointXYZ>::Ptr src(new pcl::PointCloud<pcl::PointXYZ>(*in_src_temp)); pcl::PointCloud<pcl::PointXYZ>::Ptr in_tgt_temp = in_tgt.read(); pcl::PointCloud<pcl::PointXYZ>::Ptr tgt(new pcl::PointCloud<pcl::PointXYZ>(*in_tgt_temp)); pcl::CorrespondencesPtr in_corrs_1_temp = in_corrs_1.read(); pcl::CorrespondencesPtr corrs_1(new pcl::Correspondences(*in_corrs_1_temp)); pcl::CorrespondencesPtr in_corrs_2_temp = in_corrs_2.read(); pcl::CorrespondencesPtr corrs_2(new pcl::Correspondences(*in_corrs_2_temp)); */ viewer->removeAllPointClouds(); // viewer->removeAllPointClouds(v2); //viewer->removeAllShapes(); //viewer->removeAllShapes(v2); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green (cloud_1, 0, 255, 0); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red (cloud_2, 255, 0, 0); ////// viewer->addPointCloud<pcl::PointXYZ> (cloud_1, green, "cloud"); viewer->addPointCloud<pcl::PointXYZ> (cloud_2, red, "shots"); ////// /* viewer->addPointCloud<pcl::PointXYZ> (cloud_2, red, "new", v1); viewer->addPointCloud<pcl::PointXYZ> (cloud_2, red, "new2", v2); viewer->addPointCloud<pcl::PointXYZ> (src, "keypoints1_1", v1); viewer->addPointCloud<pcl::PointXYZ> (tgt, "keypoints1_2", v2); ////// viewer->addPointCloud<pcl::PointXYZ> (src, "keypoints2_1", v1); viewer->addPointCloud<pcl::PointXYZ> (tgt, "keypoints2_2", v2); */ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "shots"); /*viewer->addCorrespondences<pcl::PointXYZ>(src, tgt, *corrs_1, "corrs_1", v1); viewer->addCorrespondences<pcl::PointXYZ>(src, tgt, *corrs_2, "corrs_2", v2); */ //viewer->spinOnce (100); }
PointT locateCenterPoint(PointCloudPtr cloud) { int size = cloud->width * cloud->height; PointT weightPoint; std::vector<float> vX, vY; float minZ=10000; PointCloudPtr cloud_1(new PointCloud), cloud_2(new PointCloud); for (int i=0; i<size; ++i) { PointT point = cloud->points[i]; vX.push_back(point.x); vY.push_back(point.y); } weightPoint.x=centerAverage(vX); weightPoint.y=centerAverage(vY); pcl::PassThrough<PointT> pass; pass.setInputCloud(cloud); pass.setFilterFieldName("x"); pass.setFilterLimits(weightPoint.x-4.f, weightPoint.x+4.f); pass.filter(*cloud_1); pass.setInputCloud(cloud_1); pass.setFilterFieldName("y"); pass.setFilterLimits(weightPoint.y-1.9f, weightPoint.y+1.9f); pass.filter(*cloud_2); for (int i=0; i<cloud_2->width*cloud_2->height; ++i) if (cloud_2->points[i].z < minZ) minZ = cloud_2->points[i].z; weightPoint.z = minZ+RadiusZ; return weightPoint; }
PointCloudPtr cutNearCenter(PointCloudPtr cloud, PointT center) { PointCloudPtr cloud_1(new PointCloud), cloud_2(new PointCloud), cloud_3(new PointCloud); pcl::PassThrough<PointT> pass; pass.setInputCloud(cloud); pass.setFilterFieldName("x"); pass.setFilterLimits(center.x-maxWidth/2, center.x+maxWidth/2); pass.filter(*cloud_1); pass.setInputCloud(cloud_1); pass.setFilterFieldName("y"); pass.setFilterLimits(center.y, center.y+maxHeight+2); pass.filter(*cloud_2); pass.setInputCloud(cloud_2); pass.setFilterFieldName("z"); pass.setFilterLimits(center.z-maxWidth/2, center.z+plusDepth); pass.filter(*cloud_3); movePointCloud(cloud_3, -center.x, -center.y, -center.z); zoomPointCloud(cloud_3, 1, 1, zZoom); return cloud_3; }
int main(int argc, char* argv[]){ std::cout << std::endl; std::vector<double> distances; std::vector<rda::RPoint> rob_points; rda::cloudPtr dist_cloud (new rda::cloud); rda::cloudPtr ls_dist_cloud (new rda::cloud); rda::cloudPtr rdp_up_ls_dist_cloud (new rda::cloud); // y + some value rda::cloudPtr rdp_down_ls_dist_cloud (new rda::cloud); // y - some value pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2 (new pcl::PointCloud<pcl::PointXYZ> ); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ); rda::Console::readArgs(argc, argv); try{ double rot_angle = atof(rda::Console::getParam("-rot_angle").c_str()); //cloud = rda::readScene(rda::Console::getParam("-file"), distances); rda::readScene(rda::Console::getParam("-file"), distances, rob_points); std::cout << "Input points number : " << rob_points.size() << std::endl; int sma_window_size = atoi(rda::Console::getParam("-sma_window_size").c_str()); rda::computePointCloud(rob_points, distances, cloud); #pragma region Distances for(int i = 0; i < distances.size(); i++) dist_cloud->push_back(rda::Point(i, distances[i], 0)); double rdp_eps = atof(rda::Console::getParam("-rdp_eps").c_str()); clock_t ls_dist_clock; ls_dist_clock = clock(); std::vector<rda::CloudPart> dist_line_parts; dist_line_parts.push_back(rda::CloudPart(dist_cloud, rda::Range(1,2))); // end rda::rdp_minimization(rda::CloudPart(dist_cloud, rda::Range(0, dist_cloud->size()-1)),0, dist_cloud->size() - 1, rdp_eps, dist_line_parts); std::vector<double> ls_dists; for(int i = 0; i < dist_line_parts.size(); i++){ function::LeastSquares ls(sma_window_size); ls.init(dist_line_parts[i].cloud(), dist_line_parts[i].begin(), dist_line_parts[i].end()); ls.approximate(); for(int j = dist_line_parts[i].begin(); j < dist_line_parts[i].end(); j++ ){ ls_dist_cloud->push_back(rda::Point(j, ls.value(j), 0)); ls_dists.push_back(ls_dist_cloud->back().y); } } rda::computePointCloud(rob_points, ls_dists, cloud_2); //rda::simple_moving_avarage(dist_cloud, sma_window_size, ls_dist_cloud); std::cout << "Distances approximation time :" << ((float)(clock() - ls_dist_clock)) / CLOCKS_PER_SEC << "sec" << std::endl; std::vector<rda::Line> dist_lines; for(int i=0; i < dist_line_parts.size(); i++) dist_lines.push_back(rda::Line(dist_line_parts[i].first_point(), dist_line_parts[i].last_point())); /* #pragma endregion #pragma region Filteration clock_t filter_clock; filter_clock = clock(); rda::statisticalFilter(cloud, *cloud_filtered, 4, 0.1); std::cout << "Filtration time :" << ((float)(clock() - filter_clock)) / CLOCKS_PER_SEC << "sec" << std::endl; #pragma endregion #pragma region Segmentation Euclidean clock_t segm_clock; segm_clock = clock(); double eps = atof(rda::Console::getParam("-eps").c_str()); int minPts = atoi(rda::Console::getParam("-minPts").c_str()); std::vector<rda::cloudPtr> clusters; rda::euclideanClusterExctraction(cloud_filtered, clusters, eps, minPts, 999999); std::cout << "Segmentation Euclidean time :" << ((float)(clock() - segm_clock)) / CLOCKS_PER_SEC << "sec" << std::endl; #pragma endregion #pragma region Partitioning double max_dist = atof(rda::Console::getParam("-max_dist").c_str()); clock_t partitioning_clock; partitioning_clock = clock(); std::vector<rda::CloudPart> clusters_patrs; std::vector<rda::Line> min_clusters_patrs; std::vector<rda::CloudPart> min_clusters_parts_lines; std::vector<std::vector<rda::Line>> approximied_parts; std::vector<std::vector<rda::Line>> merged_cluster; double rdp_eps = atof(rda::Console::getParam("-rdp_eps").c_str()); //rdp_eps = rda::compute_rdp_threashold(dist_cloud, ls_dist_cloud); std::cout << "\t rdd_eps = " << rdp_eps << std::endl; for(auto i = ls_dist_cloud->begin(); i != ls_dist_cloud->end(); i++){ rdp_up_ls_dist_cloud->push_back(rda::Point(i->x, i->y + rdp_eps, i->z)); rdp_down_ls_dist_cloud->push_back(rda::Point(i->x, i->y - rdp_eps, i->z)); } double merge_dist = atof(rda::Console::getParam("-merge_dist").c_str()); double merge_angle = atof(rda::Console::getParam("-merge_angle").c_str()); int min_part_size = atoi(rda::Console::getParam("-min_part_size").c_str()); for(int i = 0; i < clusters.size(); i++){ std::vector<rda::CloudPart> parts; std::vector<rda::Line> min_partition_lines; std::vector<rda::CloudPart> min_parts; std::vector<rda::Line> approximied; std::vector<rda::Line> merged; rda::monotonePartitioning(clusters[i], max_dist, min_part_size, parts); //Here need to compute rdp_eps rda::lineSegmentation(parts, rdp_eps, min_parts); rda::lsLineApproximation(min_parts, approximied); rda::segmentsMerging(approximied, merge_dist, merge_angle, merged); for(int i = 0; i < parts.size();i++) clusters_patrs.push_back(parts[i]); for(int i = 0; i < min_parts.size();i++) min_clusters_patrs.push_back(rda::Line(min_parts[i].first_point(), min_parts[i].last_point())); approximied_parts.push_back(approximied); merged_cluster.push_back(merged); } std::cout << "Partitioning time :" << ((float)(clock() - partitioning_clock)) / CLOCKS_PER_SEC << "sec" << std::endl; #pragma endregion */ rda::Vizualizer::init(&argc, argv); rda::Vizualizer v; rda::Vizualizer v2; rda::Vizualizer v3; rda::Vizualizer v4; rda::Vizualizer v5; v.createWindow("partitioning", 600, 600, 20, 20); v2.createWindow("minimized_ramer", 600, 600, 640, 20); v3.createWindow("approximied_rumer", 600, 600, 640, 20); v4.createWindow("merged", 600, 600, 20, 20); v5.createWindow("distances", 600, 600, 20, 20); v5.addCloud(dist_cloud, rda::LINE_STRIP, 1.0, 0.0, 1.0, 1.0); v5.addCloud(dist_cloud, rda::POINTS, 1.0, 1.0, 1.0, 1.0, 1.0f); v5.addCloud(ls_dist_cloud, rda::POINTS, 0.0, 1.0, 0.0, 1.0); v5.addCloud(rdp_up_ls_dist_cloud, rda::LINE_STRIP, 1.0, 1.0, 1.0, 1.0); v5.addCloud(rdp_down_ls_dist_cloud, rda::LINE_STRIP, 1.0, 1.0, 1.0, 1.0); for(int i = 0; i < dist_lines.size(); i++) v5.addCloud(dist_lines[i], rda::LINE_STRIP, 0.0f, 0.0f, 1.0f, 1.0f); v4.addCloud(cloud, rda::POINTS, 1.0f, 1.0f, 1.0f, 0.5f); v.addCloud(cloud, rda::POINTS, 1.0f, 1.0f, 1.0f, 0.5f); v.addCloud(cloud_2, rda::POINTS, 0.0f, 1.0f, 0.0f, 0.5f); //v.addClouds(clusters, rda::POINTS, 1.0f); //v.addClouds(clusters_patrs, rda::LINE_STRIP, 0.5f); //v2.addClouds(min_clusters_patrs, rda::LINE_STRIP, 1.0f, 4.0f); v2.addCloud(cloud, rda::POINTS, 1.0f, 1.0f, 1.0f, 0.5f); v3.addCloud(cloud, rda::POINTS, 1.0f, 1.0f, 1.0f, 0.5f); /*for(int i = 0; i < merged_cluster.size(); i++) v4.addClouds(merged_cluster.at(i), rda::LINES, 1.0f); for(int i = 0; i < approximied_parts.size(); i++) v3.addClouds(approximied_parts.at(i), rda::LINES, 1.0f);*/ rda::Vizualizer::start(); } catch(rda::RdaException& e){ std::cout << e.what() << std:: endl; } return 0; }