cv::Mat ExtractColour::pcd2image(pcl::PointCloud<pcl::PointXYZRGBA> cloud)
{
unsigned char *rgb_buffer = new unsigned char[ cloud.height*cloud.width * 3]; // Create a vector of unsigned char values for save RGB values
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGBA> (cloud));
int j=0;
// fill the vector
#pragma omp parallel
{
#pragma omp for shared (cloud, rgb_buffer, j) private (i) reduction(+: sum)
{
for (int i=0; i<640*480; i++)
{
rgb_buffer[j]=cloudPtr->points[i].b; // save B parameter
rgb_buffer[j+1]=cloudPtr->points[i].g; // save G parameter
rgb_buffer[j+2]=cloudPtr->points[i].r; // save R parameter
j=j+3;
}
}
}
cv::Mat image(cv::Size(640,480), CV_8UC3, rgb_buffer, cv::Mat::AUTO_STEP);
std::cout << "Image creation finished" << std::endl;
return image;
}
cv::Mat ExtractColour::pcd2colourLab(pcl::PointCloud<pcl::PointXYZRGBA> cloud)
{
cv::Mat colour_extracted(cloud.height*cloud.width, 3, CV_32F);
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGBA> (cloud));
#pragma omp parallel
{
#pragma omp for shared (cloud, colour_extracted) private (i, j) reduction(+: sum)
{
for(int i=0; i<cloudPtr->width; i++)
for(int j = 0; j <cloudPtr->height; j++)
{
Conversion conversion;
std::vector<int> Lab=conversion.rgb2lab(cloudPtr->points[i].r,cloudPtr->points[i].g,cloudPtr->points[i].b); // perform RGB to Lab conversion
colour_extracted.at<float>(i + j*cloudPtr->width, 1) = Lab[0]; // save L parameter
colour_extracted.at<float>(i + j*cloudPtr->width, 2) = Lab[1]; // save a parameter
colour_extracted.at<float>(i + j*cloudPtr->width, 3) = Lab[2]; // save b parameter
}
}
}
std::cout << "Colour extraction finished" << std::endl;
return colour_extracted;
}
cv::Mat ExtractColour::pcd2colourHSV(pcl::PointCloud<pcl::PointXYZRGBA> cloud)
{
cv::Mat colour_extracted(cloud.height*cloud.width, 3, CV_32F);
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGBA> (cloud));
Conversion conversion;
#pragma omp parallel
{
#pragma omp for shared (colour_extracted, cloud) private (i, j) reduction(+: sum)
{
for(int i=0; i<cloudPtr->width; i++)
for(int j = 0; j <cloudPtr->height; j++)
{
std::vector<double> hsv=conversion.rgb2hsv(cloudPtr->points[i].r,cloudPtr->points[i].g,cloudPtr->points[i].b); // perform RGB to HSV conversion
colour_extracted.at<float>(i + j*cloudPtr->width, 1) = hsv[0]; // save H parameter
colour_extracted.at<float>(i + j*cloudPtr->width, 2) = hsv[1]; // save S parameter
colour_extracted.at<float>(i + j*cloudPtr->width, 3) = hsv[2]; // save V parameter
}
}
}
return colour_extracted;
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud);
// Perform the actual filtering
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.1, 0.1, 0.1);
sor.filter (cloud_filtered);
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
pcl_conversions::moveFromPCL(cloud_filtered, output);
// Publish the data
pub.publish (output);
}
int main (int argc, char *argv[]) {
ros::init(argc, argv, "IntentionRecognizer");
// ros::NodeHandle recognizerHandle;
// ros::Publisher speechPublisher = recognizerHandle.advertise<collab::Query>("CollabSpeechProcess", 2);
// ros::Publisher actionPublisher = recognizerHandle.advertise<collab::ObjectActionInfo>("CollabActionProcess", 2);
// ros::Rate loopRate(1);
// int limit = 5;
// int count = 0;
// while (count < limit) {
// collab::Query query;
// collab::ObjectActionInfo objActionInfo;
//
// if ((count + 1) == limit) {
// query.query = "STOP";
//
// } else {
// query.query = "Do you want to do something?";
//
// }
//
// ROS_INFO("%s", query.query.c_str());
// //ROS_INFO("%s", objActionInfo);
//
// speechPublisher.publish(query);
//
// ros::spinOnce();
// loopRate.sleep();
//
// ++count;
// }
/// Capture the point cloud
collab::PointCloudRetriever cap;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGB>);
cap.run(*cloudPtr);
/// Segment the point cloud to determine candidate objects
collab::ObjectModelGenerator<pcl::PointXYZRGB> generator(cloudPtr);
bool foundObjects = generator.generateObjectModels();
if (foundObjects) {
} else {
ROS_ERROR("No objects found!");
}
return 0;
}
void ObjectDescription::extractFPFHDescriptors(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampledCloudPtr (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB> objectPtCloudSampled;
*cloudPtr = *cloud;
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
grid.setInputCloud (cloudPtr);
grid.setLeafSize (0.002, 0.002, 0.002);
grid.filter (objectPtCloudSampled);
*sampledCloudPtr = objectPtCloudSampled;
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setInputCloud (sampledCloudPtr);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr normalsTree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
ne.setSearchMethod (normalsTree);
pcl::PointCloud<pcl::Normal>::Ptr cloudNormals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloudNormals);
pcl::FPFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> fpfh;
fpfh.setInputCloud (sampledCloudPtr);
fpfh.setInputNormals (cloudNormals);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr fpfhTree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
fpfh.setSearchMethod (fpfhTree);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr ptFeatHistograms (new pcl::PointCloud<pcl::FPFHSignature33> ());
fpfh.setRadiusSearch (0.05);
fpfh.compute (*ptFeatHistograms);
fpfhDescriptors = cv::Mat(ptFeatHistograms->size(), FPFH_LEN, CV_32F);
for (size_t i = 0; i < ptFeatHistograms->size(); i++) {
pcl::FPFHSignature33 feat = ptFeatHistograms->points[i];
float sum = 0.0;
for(int j = 0; j < FPFH_LEN; j++ ) {
fpfhDescriptors.at<float>(i, j) = feat.histogram[j];
sum += feat.histogram[j];
}
for(int j = 0; j < FPFH_LEN; j++ ) {
fpfhDescriptors.at<float>(i, j) /= sum;
}
}
}
std::vector<Matrix_double> ExtractColour::indexPCD2VectorRGB(pcl::PointCloud<pcl::PointXYZRGBA> cloud, std::vector<int> index)
{
std::vector <Matrix_double> colours(index.size());
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGBA> (cloud));
#pragma omp parallel
{
#pragma omp for shared (colours, index) private (i) reduction(+: sum)
{
for (int i=0; i<index.size(); i++)
{
colours[i].columns.push_back(cloudPtr->points[index[i]].r); // save R parameter
colours[i].columns.push_back(cloudPtr->points[index[i]].g); // save G parameter
colours[i].columns.push_back(cloudPtr->points[index[i]].b); // save B parameter
}
}
}
return colours;
}
cv::Mat ExtractColour::pcd2colourRGB(pcl::PointCloud<pcl::PointXYZRGBA> cloud)
{
cv::Mat colour_extracted(cloud.height*cloud.width, 3, CV_32F);
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGBA> (cloud));
#pragma omp parallel
{
#pragma omp for shared (colour_extracted, cloud) private (i, j) reduction(+: sum)
{
for(int i=0; i<cloudPtr->width; i++)
for(int j = 0; j <cloudPtr->height; j++)
{
colour_extracted.at<float>(i + j*cloudPtr->width, 1) = cloudPtr->points[i].r; // save R parameter
colour_extracted.at<float>(i + j*cloudPtr->width, 2) = cloudPtr->points[i].g; // save G parameter
colour_extracted.at<float>(i + j*cloudPtr->width, 3) = cloudPtr->points[i].b; // save B parameter
}
}
}
std::cout << "Colour extraction finished" << std::endl;
return colour_extracted;
}
std::vector<Matrix_double> ExtractColour::indexPCD2VectorLab(pcl::PointCloud<pcl::PointXYZRGBA> cloud, std::vector<int> index)
{
std::vector <Matrix_double> colours(3);
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGBA> (cloud));
Conversion conversion;
#pragma omp parallel
{
#pragma omp for shared (cloud, index, colours) private (i) reduction(+: sum)
{
for (int i=0; i<index.size(); i++)
{
std::vector<int> lab=conversion.rgb2lab(cloudPtr->points[index[i]].r,cloudPtr->points[index[i]].g,cloudPtr->points[index[i]].b); // perform RGB to Lab conversion
colours[i].columns.push_back(lab[0]); // save L parameter
colours[i].columns.push_back(lab[1]); // save a parameter
colours[i].columns.push_back(lab[2]); // save b parameter
}
}
}
return colours;
}
void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
// Instantiate various clouds
pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate);
pcl::PointCloud<pcl::PointXYZ> cloud;
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate);
// Apply Voxel Filter on PCLPointCloud2
vox.setInputCloud (cloudPtr);
vox.setInputCloud (cloudPtr);
vox.filter (*cloud_intermediate);
// Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ>
pcl::fromPCLPointCloud2(*cloud_intermediate, cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared();
// Apply Passthrough Filter
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.3, 1);
pass.setInputCloud (cloud_p);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_p);
// Apply Passthrough Filter
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.2, 0.2);
pass.setInputCloud (cloud_p);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 3.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_p);
// Apply Statistical Noise Filter
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
// Planar segmentation: Remove large planes? Or extract floor?
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
int nr_points = (int) cloud_p->points.size ();
Eigen::Vector3f lol (0, 1, 0);
seg.setEpsAngle( 30.0f * (3.14f/180.0f) );
seg.setAxis(lol);
//while(cloud_p->points.size () > 0.2 * nr_points) {
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_p);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//break;
}
else {
/*std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << "\n";*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_p);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
}
//}
Eigen::Vector3f lol_p (0.5f, 0, 0.5f);
seg.setAxis(lol_p);
while(cloud_p->points.size () > 0.1 * nr_points) {
seg.setInputCloud (cloud_p);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
break;
}
else {
/*std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << "\n";*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_p);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
}
}
// Apply Statistical Noise Filter
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
if(cloud_p->points.size() > 0) {
std::vector<pcl::PointIndices> cluster_indices;
tree->setInputCloud (cloud_p);
ec.setInputCloud (cloud_p);
ec.extract (cluster_indices);
std::cout << "Clusters detected: " << cluster_indices.size() << "\n";
// Convert to ROS data type
}
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_p, output);
// Publish the data
downsample_pub.publish(output);
}
void srs_env_model::COctoMapPlugin::insertCloud(const tPointCloud & cloud)
{
// PERROR("insertCloud: Try lock.");
// Lock data
boost::mutex::scoped_lock lock(m_lockData);
// PERROR("insertCloud: Locked.");
tPointCloud used_cloud;
pcl::copyPointCloud( cloud, used_cloud );
//*
Eigen::Matrix4f registration_transform( Eigen::Matrix4f::Identity() );
// Registration
{
if( m_registration.isRegistering() && cloud.size() > 0 && m_bNotFirst )
{
// pcl::copyPointCloud( *m_data, *m_bufferCloud );
tPointCloudPtr cloudPtr( new tPointCloud );
pcl::copyPointCloud( cloud, *cloudPtr );
if( m_registration.registerCloud( cloudPtr, m_mapParameters ) )
{
// std::cerr << "Starting registration process " << std::endl;
registration_transform = m_registration.getTransform();
// pcl::transformPointCloud( cloud, used_cloud, transform );
// std::cerr << "Registration succeeded" << std::endl;
}
else
{
// std::cerr << "reg failed." << std::endl;
return;
}
}
}
// ros::WallTime startTime = ros::WallTime::now();
tPointCloud pc_ground; // segmented ground plane
tPointCloud pc_nonground; // everything else
if (m_filterGroundPlane)
{
m_filterGround.setCloud(&used_cloud);
m_filterGround.filter(m_data->getTree());
pc_ground = *m_filterGround.getGroundPc();
pc_nonground = *m_filterGround.getNongroundPc();
// m_filterGround.writeLastRunInfo();
} else {
pc_nonground = used_cloud;
pc_ground.clear();
pc_ground.header = used_cloud.header;
pc_nonground.header = used_cloud.header;
}
tf::StampedTransform cloudToMapTf;
// PERROR("Get transforms.");
// Get transforms
try {
// Transformation - to, from, time, waiting time
m_tfListener.waitForTransform(m_mapParameters.frameId,
cloud.header.frame_id, cloud.header.stamp, ros::Duration(5));
m_tfListener.lookupTransform(m_mapParameters.frameId,
cloud.header.frame_id, cloud.header.stamp, cloudToMapTf);
} catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback");
PERROR( "Transform error.");
return;
}
// transform clouds to world frame for insertion
if (m_mapParameters.frameId != cloud.header.frame_id)
{
Eigen::Matrix4f c2mTM;
// PERROR("Transforming.");
pcl_ros::transformAsMatrix(cloudToMapTf, c2mTM);
pcl::transformPointCloud(pc_ground, pc_ground, c2mTM);
pcl::transformPointCloud(pc_nonground, pc_nonground, c2mTM);
}
// Use registration transform
pcl::transformPointCloud( pc_ground, pc_ground, registration_transform );
pc_ground.header = cloud.header;
pc_ground.header.frame_id = m_mapParameters.frameId;
pc_nonground.header = cloud.header;
pc_nonground.header.frame_id = m_mapParameters.frameId;
// 2012/12/14: Majkl (trying to solve problem with missing time stamps in all message headers)
m_DataTimeStamp = cloud.header.stamp;
ROS_DEBUG("COctoMapPlugin::insertCloud(): Stamp = %f", cloud.header.stamp.toSec());
insertScan(cloudToMapTf.getOrigin(), pc_ground, pc_nonground);
if (m_removeSpecles)
{
//degradeSingleSpeckles();
m_filterSingleSpecles.filter(m_data->getTree());
// m_filterSingleSpecles.writeLastRunInfo();
}
// PERROR("Outdated");
if (m_bRemoveOutdated && m_bFilterWithInput)
{
m_filterRaycast.setCloud(&cloud);
m_filterRaycast.filter(m_data->getTree());
// m_filterRaycast.writeLastRunInfo();
}
else
m_bNewDataToFilter = true;
// double total_elapsed = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG("Point cloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground).)", pc_ground.size(),
pc_nonground.size());
// PERROR("Filtered");
if (m_removeTester != 0) {
long removed = doObjectTesting(m_removeTester);
// PERROR( "Removed leafs: " << removed);
if (removed > 0)
m_data->getTree().prune();
--m_testerLifeCounter;
if (m_testerLifeCounter <= 0) {
delete m_removeTester;
m_removeTester = 0;
}
}
// Release lock
lock.unlock();
// PERROR("insertCloud: Unlocked.");
m_bNotFirst = true;
// Publish new data
invalidate();
// PERROR("insertCloud: End");
}
void
cloud_cb_white (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segment (new pcl::PointCloud<pcl::PointXYZ>);
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud);
// Perform downsampling
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.01, 0.01, 0.01);
sor.filter (cloud_filtered);
// convert PointCloud2 to pcl::PointCloud<pcl::PointXYZ>
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_filtered,*temp_cloud);
// segmenting area
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (temp_cloud);
// left - right
pass.setFilterFieldName ("x");
pass.setFilterLimits (-3, 3);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
pass.setInputCloud (cloud_segment);
// up - down
pass.setFilterFieldName ("y");
pass.setFilterLimits (-3, 3);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
pass.setInputCloud (cloud_segment);
// backward - forward
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.5, 4);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
// // Find centroid
float sum_x = 0;
float sum_y = 0;
float sum_z = 0;
if(cloud_segment->points.size() > 50) {
for(size_t i = 0; i < cloud_segment->points.size(); ++i) {
sum_x += cloud_segment->points[i].x;
sum_y += cloud_segment->points[i].y;
sum_z += cloud_segment->points[i].z;
}
sum_x = sum_x / cloud_segment->points.size();
sum_y = sum_y / cloud_segment->points.size();
sum_z = sum_z / cloud_segment->points.size();
geometry_msgs::PoseStamped sPose;
sPose.header.stamp = ros::Time::now();
sPose.pose.position.x = sum_z; // equals to z in pcl // backward - forward
sPose.pose.position.y = -(sum_x); // equals to -x in pcl // right - left
sPose.pose.position.z = -(sum_y); // equals to -y in pcl // down - up
sPose.pose.orientation.x = 0.0;
sPose.pose.orientation.y = 0.0;
sPose.pose.orientation.z = 0.0;
sPose.pose.orientation.w = 1.0;
cock_pose_white_pub.publish(sPose);
}
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
//pcl_conversions::fromPCL(cloud_filtered, output);
pcl::toROSMsg(*cloud_segment, output);
// Publish the data
cloud_pub_white.publish (output);
}
//####################################################################################
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
if(flag)
{
flag = false;
std::cout << "/original_pointcloud received..." << std::endl;
}
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr hsv_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PCLPointCloud2 cloud_filtered;
// convert from pointcloud2 to PCL
// pcl_conversions::toPCL(*cloud_msg,pcl_pc2);
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::fromPCLPointCloud2(*cloud,*hsv_cloud);
pcl::fromPCLPointCloud2(*cloud,*cloud_filtered);
std::cout << "/* point cloud size */" << cloud_filtered->size() << std::endl;
//#################################################################################### converting rgb to hsv -> xyz
double min, max, delta, r, g, b, h, s, v, x, y, z, dis_r, dis_g, dis_b, dis_w;
for (int pit = 0; pit < hsv_cloud->size() ; pit++)
{
// std::cout << "point " << pit << " : " << cloud_filtered->points[pit] << std::endl
// h=0;
// s=0;
// v=0;
r = hsv_cloud->points[pit].r;
g = hsv_cloud->points[pit].g;
b = hsv_cloud->points[pit].b;
min = r < g ? r : g;
min = min < b ? min : b;
max = r > g ? r : g;
max = max > b ? max : b;
v = max/255.0; // v
delta = max - min;
if (delta < 0.00001)
{
s = 0;
h = 0; // undefined, maybe nan?
}
if( max > 0.0 ) { // NOTE: if Max is == 0, this divide would cause a crash
s = (delta / max); // s
} else {
// if max is 0, then r = g = b = 0
// s = 0, v is undefined
s = 0.0;
h = 0.0; //NAN; // its now undefined
}
if( r >= max ) // > is bogus, just keeps compilor happy
h = ( g - b ) / delta; // between yellow & magenta
else
if( g >= max )
h = 2.0 + ( b - r ) / delta; // between cyan & yellow
else
h = 4.0 + ( r - g ) / delta; // between magenta & cyan
h *= 60.0; // degrees
if( h < 0.0 )
h += 360.0;
if (r==0)
if (g==0)
if (b==0)
h = s = v = 0.0;
if (r==254)
if (g==254)
if (b==254)
{
h = s = 0.0;
v = 1.0;
}
x = s*cos(h*PI/180.0);
y = s*sin(h*PI/180.0);
z = v;
hsv_cloud->points[pit].x = x;
hsv_cloud->points[pit].y = y;
hsv_cloud->points[pit].z = z;
// hsv_cloud->points[pit].r = (x+1.0)*255/2;
// hsv_cloud->points[pit].g = (y+1.0)*255/2;
// hsv_cloud->points[pit].b = z*255;
// the actual test
dis_r = sqrt(pow(x-0.38177621,2.0) + pow(y-0.10490212,2.0) + pow(z-0.58518914,2.0)); //red
dis_g = sqrt(pow(x+0.13,2.0) + pow(y-0.44343874,2.0) + pow(z-0.3631243,2.0)); //green
dis_b = sqrt(pow(x+.13,2.0) + pow(y+0.44343874,2.0) + pow(z-.3631243,2.0)); //blue
dis_w = sqrt(pow(x-0.0,2.0) + pow(y-0.0,2.0) + pow(z-0.7358909,2.0)); //white
// mydist = {dis_r,dis_g,dis_b,dis_w};
// std::cout << "/* message */" << mydist << std::endl;
if (dis_r<dis_b && dis_r<dis_g && dis_r<dis_w)
{
cloud_filtered->points[pit].r = 255;
cloud_filtered->points[pit].g = 0;
cloud_filtered->points[pit].b = 0;
}
else if(dis_b<dis_r && dis_b<dis_g && dis_b<dis_w)
{
cloud_filtered->points[pit].r = 0;
cloud_filtered->points[pit].g = 0;
cloud_filtered->points[pit].b = 255;
}
else if(dis_w<dis_r && dis_w<dis_g && dis_w<dis_b)
{
cloud_filtered->points[pit].r = 255;
cloud_filtered->points[pit].g = 255;
cloud_filtered->points[pit].b = 255;
}
else if(dis_g<dis_r && dis_g<dis_w && dis_g<dis_b)
{
cloud_filtered->points[pit].r = 0;
cloud_filtered->points[pit].g = 255;
cloud_filtered->points[pit].b = 0;
}
}
//#################################################################################### remove outlayers
// pcl::toPCLPointCloud2(*hsv_cloud,*cloud);
// // // Perform the actual filtering Remove outlayer, very slow !
// pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> outlayer2;
// outlayer2.setInputCloud (cloudPtr);
// outlayer2.setMeanK (8);
// outlayer2.setStddevMulThresh (.1);
// outlayer2.filter (*cloud);
// pcl::fromPCLPointCloud2(*cloud,*hsv_cloud);
//#################################################################################### add cylinders
if (cylinder_flag)
{
// Fill in the cloud data
cylinder.width = 500;
cylinder.height = 1;
cylinder.is_dense = false;
cylinder.points.resize (cylinder.width * cylinder.height);
for (size_t i = 0; i < cylinder.points.size (); ++i)
{
cylinder.points[i].x = 1.0*cos(2*PI*i/cylinder.points.size());
cylinder.points[i].y = 1.0*sin(2*PI*i/cylinder.points.size());
cylinder.points[i].z = -.02;
cylinder.points[i].r = 255;
cylinder.points[i].g = 0;
cylinder.points[i].b = 0;
}
// Fill in the cloud data
cylinder2.width = 500;
cylinder2.height = 1;
cylinder2.is_dense = false;
cylinder2.points.resize (cylinder2.width * cylinder2.height);
for (size_t i = 0; i < cylinder2.points.size (); ++i)
{
cylinder2.points[i].x = 1.0*cos(2*PI*i/cylinder2.points.size());
cylinder2.points[i].y = 1.0*sin(2*PI*i/cylinder2.points.size());
cylinder2.points[i].z = 1.02;
cylinder2.points[i].r = 0;
cylinder2.points[i].g = 0;
cylinder2.points[i].b = 255;
}
cylinder_flag = false;
}
*hsv_cloud += cylinder;
*hsv_cloud += cylinder2;
//#################################################################################### publishing results
pcl::toPCLPointCloud2(*hsv_cloud,*cloud);
// Convert to ROS data type
pcl_conversions::fromPCL(*cloud, output);
// Publish the data
pub.publish (output);
pcl::toPCLPointCloud2(*cloud_filtered,*cloud);
// Convert to ROS data type
pcl_conversions::fromPCL(*cloud, output);
// Publish the data
pub2.publish (output);
// pub.publish (output);
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
// pcl::PCLPointCloud2 cloud_filtered;
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Convert to PCL data type
pcl_conversions::toPCL(*input, *cloud_blob);
// Perform the actual filtering
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.05, 0.05, 0.05);
sor.setFilterFieldName("z");
sor.setFilterLimits(0.01, 0.3);
sor.filter (*cloud_filtered_blob);
// // Remove outlier X
// pcl::PCLPointCloud2::Ptr cloud_filtered_blobx (new pcl::PCLPointCloud2);
// pcl::VoxelGrid<pcl::PCLPointCloud2> sorx;
// sorx.setInputCloud(cloud_filtered_blobz);
// sorx.setFilterFieldName("x");
// sorx.setFilterLimits(-1, 1);
// sorx.filter(*cloud_filtered_blobx);
// // Remove outlier Y
// pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
// pcl::VoxelGrid<pcl::PCLPointCloud2> sory;
// sory.setInputCloud(cloud_filtered_blobx);
// sory.setFilterFieldName("y");
// sory.setFilterLimits(-1, 1);
// sory.filter(*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
Eigen::Vector3f upVector(0, 0, 1);
seg.setAxis(upVector);
seg.setEpsAngle(1.5708);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.05);
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
return;
}
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
// Publish inliers
// sensor_msgs::PointCloud2 inlierpc;
// pcl_conversions::fromPCL(cloud_p, inlierpc);
pub.publish (*cloud_p);
// Publish the model coefficients
pcl_msgs::ModelCoefficients ros_coefficients;
pcl_conversions::fromPCL(*coefficients, ros_coefficients);
pubCoef.publish (ros_coefficients);
// ==========================================
// // Convert to ROS data type
// sensor_msgs::PointCloud2 downpc;
// pcl_conversions::fromPCL(cloud_filtered, downpc);
// // Publish the data
// pub.publish (downpc);
// pcl::ModelCoefficients coefficients;
// pcl::PointIndices inliers;
// //.makeShared()
// // Create the segmentation object
// pcl::SACSegmentation<pcl::PointXYZ> seg;
// seg.setInputCloud (&cloudPtr);
// seg.setOptimizeCoefficients (true); // Optional
// seg.setModelType (pcl::SACMODEL_PLANE); // Mandatory
// seg.setMethodType (pcl::SAC_RANSAC);
// seg.setDistanceThreshold (0.1);
// seg.segment (inliers, coefficients);
// if (inliers.indices.size () == 0)
// {
// PCL_ERROR ("Could not estimate a planar model for the given dataset.");
// return;
// }
// std::cerr << "Model coefficients: " << coefficients.values[0] << " "
// << coefficients.values[1] << " "
// << coefficients.values[2] << " "
// << coefficients.values[3] << std::endl;
// // Publish the model coefficients
// pcl_msgs::ModelCoefficients ros_coefficients;
// pcl_conversions::fromPCL(coefficients, ros_coefficients);
// pubCoef.publish (ros_coefficients);
}
void rgb_pcl::cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
// Container for original & filtered data
#if PR2
if(target_frame.find(base_frame) == std::string::npos){
getTransformCloud(input, *input);
}
sensor_msgs::PointCloud2 in = *input;
sensor_msgs::PointCloud2 out;
pcl_ros::transformPointCloud(target_frame, net_transform, in, out);
#endif
// ROS_INFO("Cloud acquired...");
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
Mat displayImage = cv::Mat(Size(640, 480), CV_8UC3);
displayImage = Scalar(120);
// Convert to PCL data type
#if PR2
pcl_conversions::toPCL(out, *cloud);
#endif
#if !PR2
pcl_conversions::toPCL(*input, *cloud);
#endif
// ROS_INFO("\t=>Cloud rotated...");
// Perform the actual filtering
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.005, 0.005, 0.005);
sor.filter (*cloud_filtered_blob);
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
ModelCoefficientsPtr coefficients (new pcl::ModelCoefficients);
PointCloudPtr plane_points(new PointCloud), point_points_2d_hull(new PointCloud);
std::vector<PointCloudPtr> object_clouds;
pcl::PointCloud<pcl::PointXYZRGB> combinedCloud;
#if PR2
make_crop_box_marker(marker_publisher, base_frame, 0, 0.2, -1, 0.2, 1.3, 2, 1.3);
// Define your cube with two points in space:
Eigen::Vector4f minPoint;
minPoint[0]=0.2; // define minimum point x
minPoint[1]=-1; // define minimum point y
minPoint[2]=0.2; // define minimum point z
Eigen::Vector4f maxPoint;
maxPoint[0]=1.5; // define max point x
maxPoint[1]=1; // define max point y
maxPoint[2]=1.5; // define max point z
pcl::CropBox<pcl::PointXYZRGB> cropFilter;
cropFilter.setInputCloud (cloud_filtered);
cropFilter.setMin(minPoint);
cropFilter.setMax(maxPoint);
cropFilter.filter (*cloud_filtered);
#endif
#if !PR2
//Rotate the point cloud
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
float theta = M_PI; // The angle of rotation in radians
// Define a translation of 0 meters on the x axis
transform_1.translation() << 0.0, 0.0, 1.0;
// The same rotation matrix as before; tetha radians arround X axis
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
// Executing the transformation
pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, transform_1);
#endif
interpretTableScene(cloud_filtered, coefficients, plane_points, point_points_2d_hull, object_clouds);
int c = 0;
#if PUBLISH_CLOUDS
int ID_object = -1;
#endif
for(auto cloudCluster: object_clouds){
// get_cloud_matching(cloudCluster); //histogram matching
#if PUBLISH_CLOUDS
ID_object = c;
#endif
combinedCloud += *cloudCluster;
combinedCloud.header = cloud_filtered->header;
c++;
}
#if DISPLAY
drawPointCloud(combinedCloud, displayImage);
#endif
getTracker(object_clouds, displayImage);
stateDetection();
// ROS_INFO("\t=>Cloud analysed...");
#if PUBLISH_CLOUDS
sensor_msgs::PointCloud2 output;
if(object_clouds.size() >= ID_object && ID_object >= 0){
pcl::toROSMsg(combinedCloud, output);
// Publish the data
pub.publish (output);
}
#endif
end = ros::Time::now();
std::stringstream ss;
ss <<(end-begin);
string s_FPS = ss.str();
#if DISPLAY
cv::putText(displayImage, "FPS: "+to_string((int)1/(stof(s_FPS))) + " Desired: "+to_string(DESIRED_FPS), cv::Point(10, 10), CV_FONT_HERSHEY_COMPLEX, 0.4, Scalar(0,0,0));
imshow("RGB", displayImage);
#endif
waitKey(1);
begin = ros::Time::now();
}
void point_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr);
float leaf = 0.005;
sor.setLeafSize(leaf, leaf, leaf);
sor.filter(cloud_filtered);
sensor_msgs::PointCloud2 sensor_pcl;
pcl_conversions::moveFromPCL(cloud_filtered, sensor_pcl);
//publish pcl data
pub_voxel.publish(sensor_pcl);
global_counter++;
if((theta == 0.0 && y_offset == 0.0) || global_counter < 800 ){
// part for planar segmentation starts here ..
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_seg(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_rotated(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_transformed(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2 plane_sensor, plane_transf_sensor;
//convert sen
pcl::fromROSMsg(*cloud_msg, *cloud1);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.01);
seg.setInputCloud(cloud1);
seg.segment (*inliers, *coefficients);
Eigen::Matrix<float, 1, 3> surface_normal;
Eigen::Matrix<float, 1, 3> floor_normal;
surface_normal[0] = coefficients->values[0];
surface_normal[1] = coefficients->values[1];
surface_normal[2] = coefficients->values[2];
std::cout << surface_normal[0] << "\n" << surface_normal[1] << "\n" << surface_normal[2];
floor_normal[0] = 0.0;
floor_normal[1] = 1.0;
floor_normal[2] = 0.0;
theta = acos(surface_normal.dot(floor_normal));
//extract the inliers - copied from tutorials...
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud1);
extract.setIndices (inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
ROS_INFO("print cloud info %d", cloud_p->height);
pcl::toROSMsg(*cloud_p, plane_sensor);
pub_plane_simple.publish(plane_sensor);
// anti rotate the point cloud by first finding the angle of rotation
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
transform_1.translation() << 0.0, 0.0, 0.0;
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_rotated, transform_1);
double y_sum = 0;
// int num_points =
for (int i = 0; i < 20; i++){
y_sum = cloud_p_rotated->points[i].y;
}
y_offset = y_sum / 20;
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.translation() << 0.0, -y_offset, 0.0;
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_transformed, transform_2);
pcl::transformPointCloud(*cloud1, *cloud_transformed, transform_2);
//now remove the y offset because of the camera rotation
pcl::toROSMsg(*cloud_p_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud1, plane_transf_sensor);
pub_plane_transf.publish(plane_transf_sensor);
}
ras_msgs::Cam_transform cft;
cft.theta = theta;
cft.y_offset = y_offset;
pub_ctf.publish(cft);
// pub_tf.publish();
}
void PlaneDetector::applyFilter(const sensor_msgs::PointCloud2ConstPtr &input)
{
// boost::mutex::scoped_lock lock(mutex_);
std_msgs::Header header = input->header;
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2; // initialize object
pcl_conversions::toPCL(*input, *cloud); // from input, generate content of cloud
/* 1. Downsampling and Publishing voxel */
// LeafSize: should small enough to caputure a leaf of plants
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); // imutable pointer
pcl::PCLPointCloud2 cloud_voxel; // cloud_filtered to cloud_voxel
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr); // set input
sor.setLeafSize(0.02f, 0.02f, 0.02f); // 2cm, model equation
sor.filter(cloud_voxel); // set output
sensor_msgs::PointCloud2 output_voxel;
pcl_conversions::fromPCL(cloud_voxel, output_voxel);
// pub_voxel.publish(output_voxel);
/* 2. Filtering with RANSAC */
// RANSAC initialization
pcl::SACSegmentation<pcl::PointXYZ> seg; // Create the segmentation object
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
seg.setOptimizeCoefficients(true); // Optional
// seg.setModelType(pcl::SACMODEL_PLANE);
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); // Use SACMODEL_PERPENDICULAR_PLANE instead
seg.setMethodType(pcl::SAC_RANSAC);
// minimum number of points calculated from N and distanceThres
seg.setMaxIterations (1000); // N in RANSAC
// seg.setDistanceThreshold(0.025); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
seg.setDistanceThreshold(0.050); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
// param for perpendicular
seg.setAxis(Eigen::Vector3f (0.0, 0.0, 1.0));
seg.setEpsAngle(pcl::deg2rad (10.0f)); // 5.0 -> 10.0
// convert from PointCloud2 to PointXYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_voxel, *cloud_voxel_xyz);
// RANSAC application
seg.setInputCloud(cloud_voxel_xyz);
seg.segment(*inliers, *coefficients); // values are empty at beginning
// inliers.indices have array index of the points which are included as inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_xyz (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = inliers->indices.begin ();
pit != inliers->indices.end (); pit++) {
cloud_plane_xyz->points.push_back(cloud_voxel_xyz->points[*pit]);
}
// Organized as an image-structure
cloud_plane_xyz->width = cloud_plane_xyz->points.size ();
cloud_plane_xyz->height = 1;
/* insert code to set arbitary frame_id setting
such as frame_id ="/assemble_cloud_1"
with respect to "/odom or /base_link" */
// Conversions: PointCloud<T>, PCLPointCloud2, sensor_msgs::PointCloud2
pcl::PCLPointCloud2 cloud_plane_pcl;
pcl::toPCLPointCloud2(*cloud_plane_xyz, cloud_plane_pcl);
sensor_msgs::PointCloud2 cloud_plane_ros;
pcl_conversions::fromPCL(cloud_plane_pcl, cloud_plane_ros);
// cloud_plane_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_plane_ros.header.frame_id = header.frame_id;
cloud_plane_ros.header.stamp = header.stamp; // ros::Time::now() -> header.stamp
pub.publish(cloud_plane_ros);
}
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input) {
// std::cerr << "in cloud_cb" << std::endl;
/* 0. Importing input cloud */
std_msgs::Header header = input->header;
// std::string frame_id = input->header.frame_id;
// sensor_msgs::PointCloud2 input_cloud = *input;
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2; // initialize object
pcl_conversions::toPCL(*input, *cloud); // from input, generate content of cloud
/* 1. Downsampling and Publishing voxel */
// LeafSize: should small enough to caputure a leaf of plants
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); // imutable pointer
pcl::PCLPointCloud2 cloud_voxel; // cloud_filtered to cloud_voxel
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr); // set input
sor.setLeafSize(0.02f, 0.02f, 0.02f); // 2cm, model equation
sor.filter(cloud_voxel); // set output
sensor_msgs::PointCloud2 output_voxel;
pcl_conversions::fromPCL(cloud_voxel, output_voxel);
pub_voxel.publish(output_voxel);
/* 2. Filtering with RANSAC */
// RANSAC initialization
pcl::SACSegmentation<pcl::PointXYZ> seg; // Create the segmentation object
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
seg.setOptimizeCoefficients(true); // Optional
// seg.setModelType(pcl::SACMODEL_PLANE);
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); // Use SACMODEL_PERPENDICULAR_PLANE instead
seg.setMethodType(pcl::SAC_RANSAC);
// minimum number of points calculated from N and distanceThres
seg.setMaxIterations (1000); // N in RANSAC
// seg.setDistanceThreshold(0.025); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
seg.setDistanceThreshold(0.050); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
// param for perpendicular
seg.setAxis(Eigen::Vector3f (0.0, 0.0, 1.0));
seg.setEpsAngle(pcl::deg2rad (10.0f)); // 5.0 -> 10.0
// convert from PointCloud2 to PointXYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_voxel, *cloud_voxel_xyz);
// RANSAC application
seg.setInputCloud(cloud_voxel_xyz);
seg.segment(*inliers, *coefficients); // values are empty at beginning
// inliers.indices have array index of the points which are included as inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_xyz (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = inliers->indices.begin ();
pit != inliers->indices.end (); pit++) {
cloud_plane_xyz->points.push_back(cloud_voxel_xyz->points[*pit]);
}
// Organized as an image-structure
cloud_plane_xyz->width = cloud_plane_xyz->points.size ();
cloud_plane_xyz->height = 1;
/* insert code to set arbitary frame_id setting
such as frame_id ="/assemble_cloud_1"
with respect to "/odom or /base_link" */
// Conversions: PointCloud<T>, PCLPointCloud2, sensor_msgs::PointCloud2
pcl::PCLPointCloud2 cloud_plane_pcl;
pcl::toPCLPointCloud2(*cloud_plane_xyz, cloud_plane_pcl);
sensor_msgs::PointCloud2 cloud_plane_ros;
pcl_conversions::fromPCL(cloud_plane_pcl, cloud_plane_ros);
// cloud_plane_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_plane_ros.header.frame_id = header.frame_id;
cloud_plane_ros.header.stamp = header.stamp; // ros::Time::now() -> header.stamp
pub_plane.publish(cloud_plane_ros);
/* 3. PCA application to get eigen */
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloud_plane_xyz);
Eigen::Matrix3f eigen_vectors = pca.getEigenVectors(); // 3x3 eigen_vectors(n,m)
/* 4. PCA Visualization */
visualization_msgs::Marker points;
// points.header.frame_id = "/base_link"; // odom -> /base_link
points.header.frame_id = header.frame_id;
points.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
points.ns = "pca"; // namespace + id
points.id = 0; // pca/0
points.action = visualization_msgs::Marker::ADD;
points.type = visualization_msgs::Marker::ARROW;
points.pose.orientation.w = 1.0;
points.scale.x = 0.05;
points.scale.y = 0.05;
points.scale.z = 0.05;
points.color.g = 1.0f;
points.color.r = 0.0f;
points.color.b = 0.0f;
points.color.a = 1.0;
geometry_msgs::Point p_0, p_1;
p_0.x = 0; p_0.y = 0; p_0.z = 0; // get from tf
p_1.x = eigen_vectors(0,0);
p_1.y = eigen_vectors(0,1); // always negative
std::cerr << "y = " << eigen_vectors(0,1) << std::endl;
p_1.z = eigen_vectors(0,2);
points.points.push_back(p_0);
points.points.push_back(p_1);
pub_marker.publish(points);
/* 5. Point Cloud Rotation */
eigen_vectors(0,2) = 0; // ignore very small z-value
double norm = pow((pow(eigen_vectors(0,0), 2) + pow(eigen_vectors(0,1), 2)), 0.5);
double nx = eigen_vectors(0,0) / norm;
double ny = eigen_vectors(0,1) / norm;
Eigen::Matrix4d rot_z; // rotation inversed, convert to Matrix4f
rot_z(0,0) = nx; rot_z(0,1) = ny; rot_z(0,2) = 0; rot_z(0,3) = 0; // ny: +/-
rot_z(1,0) = -ny; rot_z(1,1) = nx; rot_z(1,2) = 0; rot_z(1,3) = 0; // ny: +/-
rot_z(2,0) = 0; rot_z(2,1) = 0; rot_z(2,2) = 1; rot_z(2,3) = 0;
rot_z(3,0) = 0; rot_z(3,1) = 0; rot_z(3,2) = 0; rot_z(3,3) = 1;
// Transformation: Rotation, Translation
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_rot (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*cloudPtr, *cloud_xyz); // from PointCloud2 to PointXYZ
pcl::transformPointCloud(*cloud_xyz, *cloud_xyz_rot, rot_z); // original, transformed, transformation
pcl::PCLPointCloud2 cloud_rot_pcl;
sensor_msgs::PointCloud2 cloud_rot_ros;
pcl::toPCLPointCloud2(*cloud_xyz_rot, cloud_rot_pcl);
pcl_conversions::fromPCL(cloud_rot_pcl, cloud_rot_ros);
pub_rot.publish(cloud_rot_ros);
/* 6. Point Cloud Reduction */
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr >
vector_cloud_separated_xyz = separate(cloud_xyz_rot, header);
pcl::PCLPointCloud2 cloud_separated_pcl;
sensor_msgs::PointCloud2 cloud_separated_ros;
pcl::toPCLPointCloud2(*vector_cloud_separated_xyz.at(1), cloud_separated_pcl);
pcl_conversions::fromPCL(cloud_separated_pcl, cloud_separated_ros);
// cloud_separated_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_separated_ros.header.frame_id = header.frame_id;
cloud_separated_ros.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
pub_red.publish(cloud_separated_ros);
// setMarker
// visualization_msgs::Marker width_min_line;
// width_min_line.header.frame_id = "/base_link";
// width_min_line.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// width_min_line.ns = "width_min";
// width_min_line.action = visualization_msgs::Marker::ADD;
// width_min_line.type = visualization_msgs::Marker::LINE_STRIP;
// width_min_line.pose.orientation.w = 1.0;
// width_min_line.id = 0;
// width_min_line.scale.x = 0.025;
// width_min_line.color.r = 0.0f;
// width_min_line.color.g = 1.0f;
// width_min_line.color.b = 0.0f;
// width_min_line.color.a = 1.0;
// width_min_line.points.push_back(point_vector.at(0));
// width_min_line.points.push_back(point_vector.at(2));
// pub_marker.publish(width_min_line);
// /* 6. Visualize center line */
// visualization_msgs::Marker line_strip;
// line_strip.header.frame_id = "/base_link"; // odom -> /base_link
// line_strip.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// line_strip.ns = "center";
// line_strip.action = visualization_msgs::Marker::ADD;
// line_strip.type = visualization_msgs::Marker::LINE_STRIP;
// line_strip.pose.orientation.w = 1.0;
// line_strip.id = 0; // set id
// line_strip.scale.x = 0.05;
// line_strip.color.r = 1.0f;
// line_strip.color.g = 0.0f;
// line_strip.color.b = 0.0f;
// line_strip.color.a = 1.0;
// // geometry_msgs::Point p_stitch, p_min;
// p_s.x = 0; p_s.y = 0; p_s.z = 0;
// p_e.x = p_m.x; p_e.y = p_m.y; p_e.z = 0;
// line_strip.points.push_back(p_s);
// line_strip.points.push_back(p_e);
// pub_marker.publish(line_strip);
/* PCA Visualization */
// geometry_msgs::Pose pose; tf::poseEigenToMsg(pca.getEigenVectors, pose);
/* to use Pose marker in rviz */
/* Automatic Measurement */
// 0-a. stitch measurement: -0.5 < z < -0.3
// 0-b. min width measurement: 0.3 < z < 5m
// 1. iterate
// 2. pick point if y < 0
// 3. compare point with all points if 0 < y
// 4. pick point-pare recording shortest distance
// 5. compare the point with previous point
// 6. update min
// 7. display value in text in between 2 points
}
