void PCLView::showRGBPoints(cv::Mat &frame,
std::vector<cv::Point2f> &p2s,
std::vector<cv::Point3f> &p3s){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
std::cout << "Genarating example point clouds.\n\n";
for(int i = 0, _end = p2s.size(); i<_end;i++){
pcl::PointXYZRGB point;
point.x = p3s[i].x;
point.y = p3s[i].y;
point.z = p3s[i].z;
int col = p2s[i].x, row = p2s[i].y;
unsigned char* ptr= frame.ptr<unsigned char>(row);
unsigned char b = ptr[col*3], g = ptr[col*3+1], r = ptr[col*3+2];
uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
point.rgb = *reinterpret_cast<float*>(&rgb);
point_cloud_ptr->points.push_back (point);
}
point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
point_cloud_ptr->height = 1;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(point_cloud_ptr);
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
/* ******************************************************************************************** */
int main (int argc, char** argv) {
// Compute the transformation
T1.block<3,3>(0,0) = Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,0,1)).matrix();
T2.block<3,3>(0,0) = Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,0,1)).matrix() *
Eigen::AngleAxis<double>(M_PI_2, Eigen::Vector3d(0,1,0)).matrix();
T2.block<3,1>(0,3) = Eigen::Vector3d(2.05, 0.4, 2.55);
T3.block<3,3>(0,0) = Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,0,1)).matrix() *
Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,1,0)).matrix();
T3.block<3,1>(0,3) = Eigen::Vector3d(0.0, 0.0, 0.0);
T4.block<3,3>(0,0) = Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,0,1)).matrix() *
Eigen::AngleAxis<double>(3*M_PI_2, Eigen::Vector3d(0,1,0)).matrix();
T4.block<3,1>(0,3) = Eigen::Vector3d(0.0, 0.0, 0.0);
Cloud::Ptr c1 (new Cloud);
Cloud::Ptr c2 (new Cloud);
Cloud::Ptr c3 (new Cloud);
Cloud::Ptr c4 (new Cloud);
assert(pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *c1) != -1);
double distLimit = 6.5;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = rgbVis(Cloud::Ptr(new Cloud));
while (!viewer->wasStopped ()) {
Cloud::Ptr point_cloud_ptr (new Cloud);
// First cloud
for (int h=0; h < c1->height; h++) {
for (int w=0; w < c1->width; w++) {
pcl::PointXYZRGBA point = c1->at(w, h);
if(point.x != point.x) continue;
if(point.z > atof(argv[2])) continue;
if(fabs(point.x) > atof(argv[3])) continue;
Eigen::Vector4d p (point.x, point.y, point.z, 1);
Eigen::Vector4d Tp = T1 * p;
point.x = Tp(0); point.y = Tp(1); point.z = Tp(2);
point_cloud_ptr->push_back(point);
}
}
viewer->removePointCloud("sample cloud");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(point_cloud_ptr);
viewer->addPointCloud<pcl::PointXYZRGBA> (point_cloud_ptr, rgb, "sample cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "sample cloud");
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (10000));
}
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
bool simple(false), rgb(false), custom_c(false), normals(false),
shapes(false), viewports(false), interaction_customization(false);
if (pcl::console::find_argument (argc, argv, "-s") >= 0)
{
simple = true;
std::cout << "Simple visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-c") >= 0)
{
custom_c = true;
std::cout << "Custom colour visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-r") >= 0)
{
rgb = true;
std::cout << "RGB colour visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-n") >= 0)
{
normals = true;
std::cout << "Normals visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-a") >= 0)
{
shapes = true;
std::cout << "Shapes visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-v") >= 0)
{
viewports = true;
std::cout << "Viewports example\n";
}
else if (pcl::console::find_argument (argc, argv, "-i") >= 0)
{
interaction_customization = true;
std::cout << "Interaction Customization example\n";
}
else
{
printUsage (argv[0]);
return 0;
}
// ------------------------------------
// -----Create example point cloud-----
// ------------------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
std::cout << "Genarating example point clouds.\n\n";
// We're going to make an ellipse extruded along the z-axis. The colour for
// the XYZRGB cloud will gradually go from red to green to blue.
uint8_t r(255), g(15), b(15);
for (float z(-1.0); z <= 1.0; z += 0.05)
{
for (float angle(0.0); angle <= 360.0; angle += 5.0)
{
pcl::PointXYZ basic_point;
basic_point.x = 0.5 * cosf (pcl::deg2rad(angle));
basic_point.y = sinf (pcl::deg2rad(angle));
basic_point.z = z;
basic_cloud_ptr->points.push_back(basic_point);
pcl::PointXYZRGB point;
point.x = basic_point.x;
point.y = basic_point.y;
point.z = basic_point.z;
uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
point.rgb = *reinterpret_cast<float*>(&rgb);
point_cloud_ptr->points.push_back (point);
}
if (z < 0.0)
{
r -= 12;
g += 12;
}
else
{
g -= 12;
b += 12;
}
}
basic_cloud_ptr->width = (int) basic_cloud_ptr->points.size ();
basic_cloud_ptr->height = 1;
point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
point_cloud_ptr->height = 1;
// ----------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.05-----
// ----------------------------------------------------------------
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setInputCloud (point_cloud_ptr);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.05);
ne.compute (*cloud_normals1);
// ---------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.1-----
// ---------------------------------------------------------------
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.1);
ne.compute (*cloud_normals2);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (simple)
{
viewer = simpleVis(basic_cloud_ptr);
}
else if (rgb)
{
viewer = rgbVis(point_cloud_ptr);
}
else if (custom_c)
{
viewer = customColourVis(basic_cloud_ptr);
}
else if (normals)
{
viewer = normalsVis(point_cloud_ptr, cloud_normals2);
}
else if (shapes)
{
viewer = shapesVis(point_cloud_ptr);
}
else if (viewports)
{
viewer = viewportsVis(point_cloud_ptr, cloud_normals1, cloud_normals2);
}
else if (interaction_customization)
{
viewer = interactionCustomizationVis();
}
//--------------------
// -----Main loop-----
//--------------------
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
int main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_original(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter0(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter1(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2_planar(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud3_non_planar(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud4_cylinder(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud4_cylinder_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud5_non_planar_non_cylinder(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PCDReader reader;
//reader.read<pcl::PointXYZRGB> ("/home/niladri-64/plc_kinect_workspace/robot/build/two_objects/saved_file4.pcd",*cloud_original);
reader.read<pcl::PointXYZRGB> ("/home/niladri-64/plc_kinect_workspace/openni_grabber/build/saved_file2.pcd",*cloud_original);
// TRansforming
Eigen::Matrix4f temp_trans;
temp_trans << -0.819538 , -0.26171 , 0.50977, -0.316881 ,
-0.572858, 0.352719, -0.73988 ,0.944997 ,
0.013828, -0.898386, -0.438989 , 0.737386,
0, 0, 0, 1;
pcl::transformPointCloud(*cloud_original, *cloud, temp_trans );
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(cloud);
while (!viewer->wasStopped())
{
viewer->spinOnce(1000);
}
}
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setFilterFieldName ("z");
pass.setFilterLimits (-0.18, 0.5);
pass.setInputCloud (cloud);
pass.filter (*result_pass_filter0);
pass.setFilterFieldName ("x");
pass.setFilterLimits (0.77, 1.2);
pass.setInputCloud (result_pass_filter0);
pass.filter (*result_pass_filter1);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.5, 0.5);
pass.setInputCloud (result_pass_filter1);
pass.filter (*result_pass_filter2);
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(result_pass_filter2);
while (!viewer->wasStopped())
{
viewer->spinOnce(1000);
}
}
// Planar Segmentation
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.02);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
int i = 0, nr_points = (int) result_pass_filter2->points.size ();
// While 30% of the original cloud is still there
while (result_pass_filter2->points.size () > 0.5 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (result_pass_filter2);
seg.segment (*inliers, *coefficients);
// std::cerr << "Model coefficients: " << coefficients->values[0] << " "
// << coefficients->values[1] << " "
// << coefficients->values[2] << " "
// << coefficients->values[3] << std::endl;
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the inliers
extract.setInputCloud (result_pass_filter2);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud2_planar);
std::cerr << "PointCloud representing the planar component: " << cloud2_planar->width * cloud2_planar->height << " data points." << std::endl;
extract.setNegative (true);
extract.filter (*cloud3_non_planar);
std::stringstream ss;
ss << "table_scene_lms400_plane_" << i << ".pcd";
//writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = viewportsVis(cloud3_non_planar, result_pass_filter2);
while (!viewer->wasStopped())
{
viewer->spinOnce (1000);
}
}
i++;
result_pass_filter2.swap (cloud3_non_planar);
} // while loop
result_pass_filter2.swap (cloud3_non_planar);
// cloud3_non_planar is again the non-planar component
{ // cylinder segmentation
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (cloud3_non_planar);
ne.setKSearch (50);
ne.compute (*cloud_normals3);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
// pcl::SACSegmentation<pcl::PointXYZRGB> seg2;
pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> seg2;
// Optional
seg2.setOptimizeCoefficients (true);
seg2.setModelType (pcl::SACMODEL_CYLINDER);
seg2.setMethodType (pcl::SAC_RANSAC);
seg2.setNormalDistanceWeight (0.1);
seg2.setMaxIterations (10000);
seg2.setDistanceThreshold (0.07);
seg2.setRadiusLimits (0, 0.11);
seg2.setInputCloud (cloud3_non_planar);
seg2.setInputNormals (cloud_normals3);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
int i = 0, nr_points = (int) cloud3_non_planar->points.size ();
// While 30% of the original cloud is still there
// while (result2->points.size () > 0.5 * nr_points)
// {
// Segment the largest planar component from the remaining cloud
seg2.setInputCloud (cloud3_non_planar);
seg2.segment (*inliers, *coefficients);
std::cerr << "Model coefficients of Cylinder : " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << " "
<< coefficients->values[4] << " "
<< coefficients->values[5] << " "
<< coefficients->values[6] << " " <<std::endl;
std::cout << "Orientation :" << coefficients->values[3] << " " << coefficients->values[4] << " "<< coefficients->values[5] << std::endl;
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
// break;
}
// Extract the inliers
extract.setInputCloud (cloud3_non_planar);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud4_cylinder);
std::cerr << "PointCloud representing the planar component: " << cloud4_cylinder->width * cloud4_cylinder->height << " data points." << std::endl;
std::stringstream ss;
ss << "table_scene_lms400_plane_" << i << ".pcd";
//writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
{ // visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = viewportsVis(cloud4_cylinder, cloud3_non_planar);
while (!viewer->wasStopped())
{
viewer->spinOnce (1000);
}
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud5_non_planar_non_cylinder);
// cloud3_non_planar.swap (cloud5_non_planar_non_cylinder); we are not running a loop
//i++;
} // cylinder segmentation ()
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud4_cylinder);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud4_cylinder_filtered);
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = viewportsVis(cloud4_cylinder_filtered, cloud4_cylinder);
while (!viewer->wasStopped())
{
viewer->spinOnce (1000);
}
}
pcl::PointXYZRGB minPt, maxPt;
pcl::getMinMax3D (*cloud4_cylinder_filtered, minPt, maxPt);
std::cout << "Max x: " << maxPt.x << std::endl;
std::cout << "Max y: " << maxPt.y << std::endl;
std::cout << "Max z: " << maxPt.z << std::endl;
std::cout << "Min x: " << minPt.x << std::endl;
std::cout << "Min y: " << minPt.y << std::endl;
std::cout << "Min z: " << minPt.z << std::endl;
std::cout << "Position :" << minPt.x << " " << (maxPt.y + minPt.y)/2 << " "<< (maxPt.z + minPt.z)/2 << std::endl;
//std::cout << "Orientation :" << coefficients->values[4] << " " << coefficients->values[5] << " "<< coefficients->values[6] << std::endl;
return 0;
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
//preparation du socket
int sockfd, newsockfd, portno, pid, visualizerpid;
socklen_t clilen;
struct sockaddr_in serv_addr, cli_addr;
sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd < 0)
error("ERROR opening socket");
bzero((char *) &serv_addr, sizeof(serv_addr));
portno = 60588;
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(portno);
if (bind(sockfd, (struct sockaddr *) &serv_addr,
sizeof(serv_addr)) < 0)
error("ERROR on binding");
listen(sockfd,5);
clilen = sizeof(cli_addr);
update = (int *)mmap(NULL, sizeof *update, PROT_READ | PROT_WRITE,
MAP_SHARED | MAP_ANONYMOUS, -1, 0);
*update = 0;
shared_buffer_size = (int*)mmap(0, sizeof(int), PROT_READ|PROT_WRITE,
MAP_SHARED | MAP_ANONYMOUS, -1, 0);
*shared_buffer_size=50000;
sharedbuffer = (float*)mmap(0, (*shared_buffer_size)*sizeof(float), PROT_READ|PROT_WRITE,
MAP_SHARED | MAP_ANONYMOUS, -1, 0);
memset((void *)sharedbuffer, 0, (*shared_buffer_size)*sizeof(float));
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = rgbVis();
visualizerpid = fork();
if (visualizerpid < 0)
error("ERROR on fork");
if (visualizerpid == 0) {
// std::cout << "entier par le fils: " << point_cloud_ptr->points[0].x << std::endl;
//son prepare for close window event
signal(SIGHUP, sigHandler);
while (1)
{
newsockfd = accept(sockfd,
(struct sockaddr *) &cli_addr, &clilen);
if (newsockfd < 0)
error("ERROR on accept");
pid = fork();
if (pid < 0)
error("ERROR on fork");
if (pid == 0) {
close(sockfd);
dostuff(newsockfd);
*update = 1;
exit(0);
}
else{ close(newsockfd);}
}
exit(0);
}else{
while (!viewer->wasStopped ())
{
if(*update){
std::cout<<"first value "<<sharedbuffer[2]<<std::endl;
//std::cout << "test0: " << update << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr=createPointCloud();
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(point_cloud_ptr);
viewer->addPointCloud<pcl::PointXYZRGB> (point_cloud_ptr, rgb, "sample cloud");
*update=0;
}
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
kill(visualizerpid, SIGHUP);
close(sockfd);
}
return 0;
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
bool simple(false), rgb(false), custom_c(false), normals(false),
shapes(false), viewports(false), interaction_customization(false);
if (pcl::console::find_argument (argc, argv, "-s") >= 0)
{
simple = true;
std::cout << "Simple visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-c") >= 0)
{
custom_c = true;
std::cout << "Custom colour visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-r") >= 0)
{
rgb = true;
std::cout << "RGB colour visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-n") >= 0)
{
normals = true;
std::cout << "Normals visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-a") >= 0)
{
shapes = true;
std::cout << "Shapes visualisation example\n";
}
else if (pcl::console::find_argument (argc, argv, "-v") >= 0)
{
viewports = true;
std::cout << "Viewports example\n";
}
else if (pcl::console::find_argument (argc, argv, "-i") >= 0)
{
interaction_customization = true;
std::cout << "Interaction Customization example\n";
}
else
{
printUsage (argv[0]);
return 0;
}
// ------------------------------------
// -----Create example point cloud-----
// ------------------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
std::cout << "Genarating example point clouds.\n\n";
// We're going to make an ellipse extruded along the z-axis. The colour for
// the XYZRGB cloud will gradually go from red to green to blue.
uint8_t r(255), g(15), b(15);
for (float z(-1.0); z <= 1.0; z += 0.05)
{
for (float angle(0.0); angle <= 360.0; angle += 5.0)
{
pcl::PointXYZ basic_point;
basic_point.x = 0.5 * cosf (pcl::deg2rad(angle));
basic_point.y = sinf (pcl::deg2rad(angle));
basic_point.z = z;
basic_cloud_ptr->points.push_back(basic_point);
pcl::PointXYZRGB point;
point.x = basic_point.x;
point.y = basic_point.y;
point.z = basic_point.z;
uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
point.rgb = *reinterpret_cast<float*>(&rgb);
point_cloud_ptr->points.push_back (point);
}
if (z < 0.0)
{
r -= 12;
g += 12;
}
else
{
g -= 12;
b += 12;
}
}
basic_cloud_ptr->width = (int) basic_cloud_ptr->points.size ();
basic_cloud_ptr->height = 1;
point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
point_cloud_ptr->height = 1;
// ----------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.05-----
// ----------------------------------------------------------------
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setInputCloud (point_cloud_ptr);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.05);
ne.compute (*cloud_normals1);
// ---------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.1-----
// ---------------------------------------------------------------
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.1);
ne.compute (*cloud_normals2);
// ----------------------------------------------------------------
// -----Load PCD file from Kinect ---------------------------------
// - TF -----------------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr_kinect (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>& point_cloud_kinect = *point_cloud_ptr_kinect;
pcl::io::loadPCDFile ("/home/taylor/src/data_pcd/top/kinect_top_rgb.pcd", point_cloud_kinect);
// --end import --
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (simple)
{
viewer = simpleVis(basic_cloud_ptr);
}
else if (rgb)
{
viewer = rgbVis(point_cloud_ptr_kinect);
}
else if (custom_c)
{
viewer = customColourVis(point_cloud_ptr_kinect);
}
else if (normals)
{
viewer = normalsVis(point_cloud_ptr, cloud_normals2);
}
else if (shapes)
{
viewer = shapesVis(point_cloud_ptr);
}
else if (viewports)
{
viewer = viewportsVis(point_cloud_ptr, cloud_normals1, cloud_normals2);
}
else if (interaction_customization)
{
viewer = interactionCustomizationVis();
}
//--------------------
// -----Main loop-----
//--------------------
//TF custom
cout << "begin custom" << endl;
viewer->spinOnce(1000);
viewer->setCameraPosition(0.00, 0.00, -1.25, 0.00, 0.00, 0.625, -0.00, -0.99999, 0.000);
viewer->setCameraFieldOfView(0.523599);
viewer->setCameraClipDistances(0.0, 4.0);
viewer->setSize(1000,1000);
viewer->updateCamera();
viewer->spinOnce(1000);
cout << " " << endl;
cout << "drawing sphere..." << endl;
pcl::PointXYZ p1;
p1.x = -0.031;
p1.y = 0.021;
p1.z = 0.602;
viewer->addSphere(p1, 0.01, 1.0, 0.0, 1.0, "PickedPoint", 0);
cout << "end custom" << endl;
//end TF Custom
while (!viewer->wasStopped ())
{
viewer->spinOnce (10000);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
#include <string>
#include <conv.h>
#include <iostream>
#include <boost/thread.hpp>
#include <boost/date_time.hpp>
uint32_t ColorR []={ 255, 0, 0, 255, 50,0,0};
uint32_t ColorG []={ 0, 255, 0, 255, 0,0,50};
uint32_t ColorB []={ 0, 0, 255, 0 , 0,50,0};
boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis (std::string);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewerinitial = rgbVis("Default PointCloud");
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = rgbVis("Treated PointCloud");
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
static bool update=false;
void error(const char *msg)
{
perror(msg);
exit(1);
}
void testkeyboard(){
// --------------
// -----Main-----
// --------------
int main(int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl17::console::find_argument(argc, argv, "-h") >= 0)
{
printUsage(argv[0]);
return 0;
}
bool simple(false), rgb(false), custom_c(false), normals(false), shapes(false), viewports(false),
interaction_customization(false);
if (pcl17::console::find_argument(argc, argv, "-s") >= 0)
{
simple = true;
std::cout << "Simple visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-c") >= 0)
{
custom_c = true;
std::cout << "Custom colour visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-r") >= 0)
{
rgb = true;
std::cout << "RGB colour visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-n") >= 0)
{
normals = true;
std::cout << "Normals visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-a") >= 0)
{
shapes = true;
std::cout << "Shapes visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-v") >= 0)
{
viewports = true;
std::cout << "Viewports example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-i") >= 0)
{
interaction_customization = true;
std::cout << "Interaction Customization example\n";
}
else
{
printUsage(argv[0]);
return 0;
}
// ------------------------------------
// -----Create example point cloud-----
// ------------------------------------
pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr basic_cloud_ptr(new pcl17::PointCloud<pcl17::PointXYZRGB>);
pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr point_cloud_ptr(new pcl17::PointCloud<pcl17::PointXYZRGB>);
pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr cloud_f(new pcl17::PointCloud<pcl17::PointXYZRGB>);
pcl17::VoxelGrid<pcl17::PointXYZRGB > sor;
//pcl17::PLYReader reader;
//reader.read("box.ply",*point_cloud_ptr,0);
//pcl17::PointXYZRGB point;
if (pcl17::io::loadPCDFile<pcl17::PointXYZRGB>(argv[1], *point_cloud_ptr) == -1) //* load the file
{
PCL17_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
basic_cloud_ptr = point_cloud_ptr;
/*
std::cout << "Loaded " << point_cloud_ptr->width * point_cloud_ptr->height << " data points" << std::endl;
cout << "gobbel" << endl;
sor.setInputCloud(point_cloud_ptr);
std::cerr << "PointCloud before filtering: " << point_cloud_ptr->width * point_cloud_ptr->height << " data points ("
<< pcl17::getFieldsList(*point_cloud_ptr) << ").";
std::cout << std::endl;
sor.setLeafSize(0.01, 0.01, 0.01);
sor.filter(*cloud_f);
pcl17::io::savePCDFileASCII("BoxFiltered.pcd", *cloud_f); */
/* for (size_t i = 0; i < point_cloud_ptr->points.size (); ++i)
std::cout << " " << point_cloud_ptr->points[i].x
<< " " << point_cloud_ptr->points[i].y
<< " " << point_cloud_ptr->points[i].z << std::endl;
cout << point_cloud_ptr->width << endl;
cout << point_cloud_ptr->height << endl;
for(int i=0;i<point_cloud_ptr->width * point_cloud_ptr->height;i++)
{
point=point_cloud_ptr->points.at(i);
point.r = 255;
point.g = 0;
point.b = 0;
basic_cloud_ptr->points.at(i)=point;
}
cout << basic_cloud_ptr->width << endl;
cout << basic_cloud_ptr->height << endl;
pcl17::PCDWriter writer;
writer.write<pcl17::PointXYZRGB> (argv[1], *basic_cloud_ptr, false);
point_cloud_ptr=basic_cloud_ptr; */
// ----------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.05-----
// ----------------------------------------------------------------
pcl17::NormalEstimation<pcl17::PointXYZRGB, pcl17::Normal> ne;
ne.setInputCloud(point_cloud_ptr);
pcl17::search::KdTree<pcl17::PointXYZRGB>::Ptr tree(new pcl17::search::KdTree<pcl17::PointXYZRGB>());
ne.setSearchMethod(tree);
pcl17::PointCloud<pcl17::Normal>::Ptr cloud_normals1(new pcl17::PointCloud<pcl17::Normal>);
ne.setRadiusSearch(0.05);
ne.compute(*cloud_normals1);
// ---------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.1-----
// ---------------------------------------------------------------
pcl17::PointCloud<pcl17::Normal>::Ptr cloud_normals2(new pcl17::PointCloud<pcl17::Normal>);
ne.setRadiusSearch(0.1);
ne.compute(*cloud_normals2);
boost::shared_ptr<pcl17::visualization::PCLVisualizer> viewer;
if (simple)
{
viewer = simpleVis(basic_cloud_ptr);
}
else if (rgb)
{
viewer = rgbVis(point_cloud_ptr);
}
else if (custom_c)
{
viewer = customColourVis(basic_cloud_ptr);
}
else if (normals)
{
viewer = normalsVis(point_cloud_ptr, cloud_normals2);
}
else if (shapes)
{
viewer = shapesVis(point_cloud_ptr);
}
else if (viewports)
{
viewer = viewportsVis(point_cloud_ptr, cloud_normals1, cloud_normals2);
}
else if (interaction_customization)
{
viewer = interactionCustomizationVis();
}
//--------------------
// -----Main loop-----
//--------------------
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
int
main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
//Removes part_of_cloud but retain the original full_cloud
//extract.setNegative(true); // Removes part_of_cloud from full cloud and keep the rest
extract.filter(*cloud_plane);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud(cloud_plane);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_filtered);
//cloud.swap(cloud_plane);
/*
pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(cloud_plane);
while (!viewer.wasStopped())
{
}
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis;
fitted_plane_norm[0] = coefficients->values[0];
fitted_plane_norm[1] = coefficients->values[1];
fitted_plane_norm[2] = coefficients->values[2];
xyaxis_plane_norm[0] = 0.0;
xyaxis_plane_norm[1] = 0.0;
xyaxis_plane_norm[2] = 1.0;
rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm);
float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm));
//float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm));
transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud_recentered, *cloud_xyz);
///////////////////////////////////////////////////////////////////
Eigen::Vector4f pcaCentroid;
pcl::compute3DCentroid(*cloud_xyz, pcaCentroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
std::cout << eigenVectorsPCA.size() << std::endl;
if(eigenVectorsPCA.size()!=9)
eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform);
// Get the minimum and maximum points of the transformed cloud.
pcl::PointXYZ minPoint, maxPoint;
pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(cloud);
// viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud");
// viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2");
viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z);
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
/*
pcl::PCA< pcl::PointXYZ > pca;
pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud;
pcl::PointCloud< pcl::PointXYZ > proj;
pca.setInputCloud(cloud);
pca.project(*cloud, proj);
Eigen::Vector4f proj_min;
Eigen::Vector4f proj_max;
pcl::getMinMax3D(proj, proj_min, proj_max);
pcl::PointXYZ min;
pcl::PointXYZ max;
pca.reconstruct(proj_min, min);
pca.reconstruct(proj_max, max);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z);
*/
return (0);
}
