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);
}
