int main(int argc, char *argv[]) { pcl::PointCloud<pcl::PointXYZHSV>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZHSV>); pcl::PointCloud<pcl::PointXYZRGBL>::Ptr label(new pcl::PointCloud<pcl::PointXYZRGBL>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGBL>::Ptr cloud_detected(new pcl::PointCloud<pcl::PointXYZRGBL>); pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>); pcl::search::Search<pcl::PointXYZHSV>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZHSV>>(new pcl::search::OrganizedNeighbor<pcl::PointXYZHSV>); pcl::PCDWriter writer; std::cout << "File Reader started" << std::endl; pcl::PCDReader reader; std::string filename = argv[1]; int pos = filename.find_last_of(".pcd"); std::string filename_no_ext(filename.substr(0,pos-3)); reader.read(filename_no_ext+"_hsv.pcd", *cloud); reader.read(filename_no_ext+".pcd",*cloud_color); reader.read(filename_no_ext+"_label.pcd",*label); ////////////////////////////////////////////////////////////////////////// pcl::IndicesPtr indices(new std::vector <int>); pcl::PassThrough<pcl::PointXYZHSV> pass; pass.setInputCloud(cloud); pass.setKeepOrganized(true); pass.setFilterFieldName("z"); pass.setFilterLimits(-50.0, 50.0); pass.filter(*indices); pcl::RegionGrowingHSV<pcl::PointXYZHSV, pcl::Normal> reg; reg.setInputCloud(cloud); reg.setSearchMethod(tree); reg.setIndices(indices); reg.setDistanceThreshold(0.05); reg.setPointColorThreshold(-80, -0.3, -0.1, 80, 0.3, 0.1); reg.setRegionColorThreshold(-10, -0.2, -0.01, 10, 0.2, 0.01); reg.setMinClusterSize(40); reg.setMaxClusterSize(800000); reg.setNumberOfRegionNeighbours(5); reg.setNumberOfNeighbours(5); //pcl::NormalEstimation<pcl::PointXYZHSV, pcl::Normal> ne; //ne.setInputCloud(cloud); //ne.setSearchMethod(tree); //ne.setRadiusSearch(0.03); //ne.compute(*normal); //reg.setInputNormals(normal); reg.setNormalTestFlag(false); reg.setCurvatureTestFlag(false); reg.setResidualTestFlag(false); std::cout << "Start Extract Clusters!" << std::endl; std::vector<pcl::PointIndices> clusters; // (new std::vector<pcl::PointIndices>()); reg.extract(clusters); ////////////////////////////////////////////////////////////////////////// //std::vector<pcl::PointIndices>::iterator itr; int clusters_count = 0; int num_of_clusters = clusters.size(); std::vector<pcl::PointIndices> new_clusters; for( int i = 0; i < num_of_clusters; i++) { std::vector<pcl::PointIndices>::pointer itr = &clusters.at(i); std::vector<int>::iterator idx; pcl::PointIndices plane_index_buffer; pcl::PointIndices object_index_buffer; for (idx = itr->indices.begin(); idx < itr->indices.end();idx++) { if (label->at(*idx).label == SD_OBJECT) { plane_index_buffer.indices.push_back(*idx); } else if(label->at(*idx).label == SD_UNDEFINED) { object_index_buffer.indices.push_back(*idx); } } if (!plane_index_buffer.indices.empty() && !object_index_buffer.indices.empty()) { while (!itr->indices.empty()) { itr->indices.pop_back(); } for (int j = 0; j < plane_index_buffer.indices.size(); j++) { itr->indices.push_back(plane_index_buffer.indices.at(j)); } clusters.push_back(object_index_buffer); } } reg.clusters_ = clusters; std::cout << "Got here!" << std::endl; ////////////////////////////////////////////////////////////////////////// // pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud(); // pcl::PCDWriter writer; // writer.write(filename.insert(0,"segmented_"), *colored_cloud); // reg.writeClustersToFile("clusters_statistics.txt", plane_labels); reg.readShadowLabelsFromFile("labels.txt"); cloud_detected = reg.getShadowCloud(cloud_color); ////////////////////////////////////////////////////////////////////////// writer.write(filename_no_ext+"_shadow_detected.pcd", *cloud_detected, true); return(0); }
void GeneralTransform::SegmentationAndUpdate(std::vector<cv::Mat>& prev_home_cloud, std::vector<cv::Mat>& home_cloud, cv::Mat& query_cloud, cv::Mat& feature, int iteration_count) { // all home cloud suppose to be the whole cloud thus same size... /************* nearest neighbor match part *********************/ cv::Mat target_cloud, transformed_cloud; int query_cloud_size = query_cloud.rows; int cloud_dim = home_cloud[0].cols; std::vector<cv::Mat> indices(num_joints_); std::vector<cv::Mat> min_dists(num_joints_); for(int i = 0; i < num_joints_; i++) { indices[i] = cv::Mat::zeros(query_cloud_size, 1, CV_32S); min_dists[i] = cv::Mat::zeros(query_cloud_size, 1, CV_32F); } // match different clouds, transformed by different weights... // for(int i = 0; i < num_joints_; i++) for(int i = 0; i < num_joints_; i++) { prev_home_cloud[i].convertTo(target_cloud, CV_32F); home_cloud[i].convertTo(transformed_cloud, CV_32F); cv::flann::Index kd_trees(target_cloud, cv::flann::KDTreeIndexParams(4), cvflann::FLANN_DIST_EUCLIDEAN); // build kd tree kd_trees.knnSearch(transformed_cloud, indices[i], min_dists[i], 1, cv::flann::SearchParams(64)); // kd tree search } // segment the clouds by minimum distance... // the two segments are of the same length which is the length of the previous home cloud // maybe use vector first and do a whole conversion at the end... that should be good... /************* segmentation based on closest neighbor part *********************/ std::vector<int> segmentation_count(num_joints_); std::vector<cv::Mat> segmented_target_cloud(num_joints_); std::vector<cv::Mat> segmented_transformed_cloud(num_joints_); std::vector<cv::Mat> segmented_query_cloud(num_joints_); std::vector<cv::Mat> segmented_idx(num_joints_); // pre allocate for(int i = 0; i < num_joints_; i++) { segmentation_count[i] = 0; // query_cloud.rows; segmented_idx[i] = cv::Mat::zeros(query_cloud_size, 2, CV_64F); // first column original idx, second column matched idx } // get the data... for(int i = 0; i < query_cloud_size; i++) { int min_idx = 0; double curr_min_dist = min_dists[0].at<float>(i, 0); for(int j = 1; j < num_joints_; j++) { // find the minimum... if(min_dists[j].at<float>(i, 0) < curr_min_dist) { min_idx = j; curr_min_dist = min_dists[j].at<float>(i, 0); } } int pos = segmentation_count[min_idx]; segmented_idx[min_idx].at<double>(pos, 0) = i; segmented_idx[min_idx].at<double>(pos, 1) = indices[min_idx].at<int>(i, 0); segmentation_count[min_idx]++; } for(int i = 0; i < num_joints_; i++) { segmented_target_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F); segmented_transformed_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F); segmented_query_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F); for(int j = 0; j < segmentation_count[i]; j++) { int query_pos = segmented_idx[i].at<double>(j, 0); int matched_pos = segmented_idx[i].at<double>(j, 1); home_cloud[i].rowRange(query_pos, query_pos + 1).copyTo(segmented_transformed_cloud[i].rowRange(j, j + 1)); query_cloud.rowRange(query_pos, query_pos + 1).copyTo(segmented_query_cloud[i].rowRange(j, j + 1)); prev_home_cloud[i].rowRange(matched_pos, matched_pos + 1).copyTo(segmented_target_cloud[i].rowRange(j, j + 1)); } } /******************* display segmented data... *********************/ if(iteration_count % 200 == 1) { // just display the query cloud... std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_segments(num_joints_); for(int i = 0; i < num_joints_; i++) { if(segmentation_count[i] != 0) { char cloud_name[10]; sprintf(cloud_name, "%d", i); COLOUR c = GetColour(i * 1.0 / (num_joints_ - 1) * num_joints_, 0, num_joints_); cloud_segments[i] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>); Mat2PCD_Trans(segmented_query_cloud[i], cloud_segments[i]); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color(cloud_segments[i], c.r * 255, c.g * 255, c.b * 255); if(iteration_count == 1) viewer_->addPointCloud<pcl::PointXYZ>(cloud_segments[i], cloud_color, cloud_name); else viewer_->updatePointCloud<pcl::PointXYZ>(cloud_segments[i], cloud_color, cloud_name); } } viewer_->spinOnce(1); } /************* weights update part **************/ // ReOrder_Trans(prev_home_cloud, segmented_target_cloud, indices); /*for(int i = 0; i < num_joints_; i++) query_cloud.copyTo(segmented_query_cloud[i]);*/ // CalcGradient(segmented_target_cloud, segmented_transformed_cloud, segmented_query_cloud, feature, segmentation_count); // CalcGradient(segmented_target_cloud, segmented_transformed_cloud, segmented_query_cloud, feature, segmentation_count); Update(iteration_count); }