void cloud_detect (const sensor_msgs::PointCloud2ConstPtr& input){
sensor_msgs::PointCloud2 output;
// Do data processing here...
usleep (10000);
viewer->spinOnce (10);
// if no new messages appear, just stop processing
if (!cloud_msg || cloud_msg == old_cloud_msg)
return;
old_cloud_msg = cloud_msg;
// get message from ROS and convert to point cloud
PointCloud<PointXYZRGB> point_cloud;
fromROSMsg(*cloud_msg, point_cloud);
// if the sensor point cloud provides "far range data", use it to compute the sensor_pose
PointCloud<PointWithViewpoint> far_ranges;
RangeImage::extractFarRanges(*cloud_msg, far_ranges);
if (pcl::getFieldIndex(*cloud_msg, "vp_x")>=0) {
PointCloud<PointWithViewpoint> tmp_pc;
fromROSMsg(*cloud_msg, tmp_pc);
Eigen::Vector3f average_viewpoint = RangeImage::getAverageViewPoint(tmp_pc);
sensor_pose = Eigen::Translation3f(average_viewpoint) * sensor_pose;
}
PointCloud<PointXYZRGB>::Ptr point_cloud_ptr (new PointCloud<PointXYZRGB>(point_cloud));
PointCloud<PointXYZRGB>::Ptr point_cloudfilt_ptr (new PointCloud<PointXYZRGB>); // filtered pc
// Filter clouds in Z dimension (min, max)
FilterPointCloudZ(point_cloud_ptr, output, 0.0f, 10.0f);
//publish filtered point cloud
pub.publish((*output));
}
void Renderer::visualize(Vector3f *vertices, Vector3f *vectors, int numberOfVertices) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_vectors_ptr(new pcl::PointCloud<pcl::Normal>);
uint8_t r(255), g(255), b(255);
for (uint i = 0; i < numberOfVertices; i++) {
if(vertices[i](0) != 0) {
pcl::PointXYZRGB point(255, 255, 255);
point.x = vertices[i](0);
point.y = vertices[i](1);
point.z = vertices[i](2);
point_cloud_ptr->points.push_back(point);
pcl::Normal n(vectors[i](0), vectors[i](1), vectors[i](2));
cloud_vectors_ptr->push_back(n);
}
}
point_cloud_ptr->width = (int) point_cloud_ptr->points.size();
point_cloud_ptr->height = 1;
cloud_vectors_ptr->width = (int) cloud_vectors_ptr->points.size();
cloud_vectors_ptr->height = 1;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
viewer->setBackgroundColor(0, 0, 0);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(point_cloud_ptr);
viewer->addPointCloud<pcl::PointXYZRGB>(point_cloud_ptr, rgb, "sample cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(point_cloud_ptr, cloud_vectors_ptr, 1, 0.05, "normals");
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
while (!viewer->wasStopped() ) {
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
//TF Begin customization and import pcd file
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>& point_cloud = *point_cloud_ptr;
pcl::io::loadPCDFile ("/home/taylor/src/data_pcd/top/kinect_top_rgb.pcd", point_cloud);
//TF End customization
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ> (point_cloud_ptr, "sample cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
//viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
//TF add mouse click customization to this setup
viewer->registerPointPickingCallback (pp_callback, (void*)&viewer); //was viewer->registerPointPickingCallback (pp_callback, (void*)&viewer);
return (viewer);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
createPointCloud(){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
uint8_t r(255), g(15), b(15);
for (int z=0; z < *shared_buffer_size; z +=3)
{
pcl::PointXYZRGB point;
point.x = sharedbuffer[z];
point.y = sharedbuffer[z+1];
point.z = sharedbuffer[z+2];
std::cout<<point.x<<" "<<point.y<<" "<<point.z<<std::endl;
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);
r -= 12;
g += 12;
}
point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
point_cloud_ptr->height = 1;
return point_cloud_ptr;
}
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));
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
convertBytesToPointCloud(char* buffer,int buffer_point_size){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
uint8_t r(255), g(15), b(15);
for(int k=0;k<buffer_point_size;k+=12) {
pcl::PointXYZRGB point;
point.x = bytesToFloat(buffer[k], buffer[k+1], buffer[k+2], buffer[k+3]);
point.y = bytesToFloat(buffer[k+4], buffer[k+5], buffer[k+6], buffer[k+7]);
point.z = bytesToFloat(buffer[k+8], buffer[k+9], buffer[k+10], buffer[k+11]);
printf("%f,%f,%f\n ", point.x,point.y,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);
r -= 5;
g += 5;
}
point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
point_cloud_ptr->height = 1;
return point_cloud_ptr;
}
/* ******************************************************************************************** */
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));
}
}
Test::Test()
{
std::cout<<"Test Obj created";
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>);
uint8_t r(255), g(15), b(15);
pcl::PointXYZ basic_point;
basic_point.x = 0.5;
basic_point.y = 0.5;
basic_point.z = 0.5;
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);
basic_point.x = -0.5;
basic_point.y = -0.5;
basic_point.z = -0.5;
rgb = (static_cast<uint32_t>(15) << 16 |
static_cast<uint32_t>(255) << 8 | static_cast<uint32_t>(15));
point.rgb = *reinterpret_cast<float*>(&rgb);
point_cloud_ptr->points.push_back (point);
//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;
pcl::visualization::CloudViewer viz("My cloud viewer");
viz.showCloud(point_cloud_ptr);
//viz.spin();
while(!viz.wasStopped())
{
}
}
int main( int argc, char** argv )
{
if (argc != 4)
{
//Check arguments
std::cerr << "Usage: " << argv[0] << " <rgb-image-filename> <disparity-image-filename> <path-to-Q-matrix>" << std::endl;
return 1;
}
//Load Matrix Q
cv::FileStorage fs(argv[3], cv::FileStorage::READ);
cv::Mat Q;
fs["Q"] >> Q;
//If size of Q is not 4x4 exit
if (Q.cols != 4 || Q.rows != 4)
{
std::cerr << "ERROR: Could not read matrix Q (doesn't exist or size is not 4x4)" << std::endl;
return 1;
}
#ifdef CUSTOM_REPROJECT
//Get the interesting parameters from Q
double Q03, Q13, Q23, Q32, Q33;
Q03 = Q.at<double>(0,3);
Q13 = Q.at<double>(1,3);
Q23 = Q.at<double>(2,3);
Q32 = Q.at<double>(3,2);
Q33 = Q.at<double>(3,3);
std::cout << "Q(0,3) = "<< Q03 <<"; Q(1,3) = "<< Q13 <<"; Q(2,3) = "<< Q23 <<"; Q(3,2) = "<< Q32 <<"; Q(3,3) = "<< Q33 <<";" << std::endl;
#endif
std::cout << "Read matrix in file " << argv[3] << std::endl;
//Show the values inside Q (for debug purposes)
/*
for (int y = 0; y < Q.rows; y++)
{
const double* Qy = Q.ptr<double>(y);
for (int x = 0; x < Q.cols; x++)
{
std::cout << "Q(" << x << "," << y << ") = " << Qy[x] << std::endl;
}
}
*/
//Load rgb-image
cv::Mat img_rgb = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
if (img_rgb.data == NULL)
{
std::cerr << "ERROR: Could not read rgb-image: " << argv[1] << std::endl;
return 1;
}
//Load disparity image
cv::Mat img_disparity = cv::imread(argv[2], CV_LOAD_IMAGE_GRAYSCALE);
if (img_disparity.data == NULL)
{
std::cerr << "ERROR: Could not read disparity-image: " << argv[2] << std::endl;
return 1;
}
//Both images must be same size
if (img_rgb.size() != img_disparity.size())
{
std::cerr << "ERROR: rgb-image and disparity-image have different sizes " << std::endl;
return 1;
}
//Show both images (for debug purposes)
cv::namedWindow("rgb-image");
cv::namedWindow("disparity-image");
cv::imshow("rbg-image", img_rgb);
cv::imshow("disparity-image", img_disparity);
std::cout << "Press a key to continue..." << std::endl;
cv::waitKey(0);
cv::destroyWindow("rgb-image");
cv::destroyWindow("disparity-image");
#ifndef CUSTOM_REPROJECT
//Create matrix that will contain 3D corrdinates of each pixel
cv::Mat recons3D(img_disparity.size(), CV_32FC3);
//Reproject image to 3D
std::cout << "Reprojecting image to 3D..." << std::endl;
cv::reprojectImageTo3D( img_disparity, recons3D, Q, false, CV_32F );
#endif
//Create point cloud and fill it
std::cout << "Creating Point Cloud..." <<std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
double px, py, pz;
uchar pr, pg, pb;
for (int i = 0; i < img_rgb.rows; i++)
{
uchar* rgb_ptr = img_rgb.ptr<uchar>(i);
#ifdef CUSTOM_REPROJECT
uchar* disp_ptr = img_disparity.ptr<uchar>(i);
#else
double* recons_ptr = recons3D.ptr<double>(i);
#endif
for (int j = 0; j < img_rgb.cols; j++)
{
//Get 3D coordinates
#ifdef CUSTOM_REPROJECT
uchar d = disp_ptr[j];
if ( d == 0 ) continue; //Discard bad pixels
double pw = -1.0 * static_cast<double>(d) * Q32 + Q33;
px = static_cast<double>(j) + Q03;
py = static_cast<double>(i) + Q13;
pz = Q23;
px = px/pw;
py = py/pw;
pz = pz/pw;
#else
px = recons_ptr[3*j];
py = recons_ptr[3*j+1];
pz = recons_ptr[3*j+2];
#endif
//Get RGB info
pb = rgb_ptr[3*j];
pg = rgb_ptr[3*j+1];
pr = rgb_ptr[3*j+2];
//Insert info into point cloud structure
pcl::PointXYZRGB point;
point.x = px;
point.y = py;
point.z = pz;
uint32_t rgb = (static_cast<uint32_t>(pr) << 16 |
static_cast<uint32_t>(pg) << 8 | static_cast<uint32_t>(pb));
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;
//Create visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = createVisualizer( point_cloud_ptr );
// Write to .PLY file - added by Brian Morgan
// There is an option to set the origin when you write
cout << "Writing to pclout.ply...";
pcl::PLYWriter plyWriter; // <pcl::PointXYZRGB>
int writeSuccess = plyWriter.write("pclout.ply", *point_cloud_ptr);
if (!writeSuccess)
cout << "File Write failed.";
//Main loop
while ( !viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
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);
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::PointXYZRGBA>::Ptr final_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBA>); // point cloud to be saved here after loading
// pcl::PointCloud<pcl::PointXYZRGBA> nil_cloud ;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBA>); // point cloud to be saved here after loading
int error = pcl::io::loadPCDFile ("/home/xwam/plc_kinect_workspace/nil_seg_basic/build/saved_file4.pcd", *point_cloud_ptr);
if (error == -1) //* load the file
{
PCL_ERROR ("Couldn't read file...error \n");
return (-1);
}
//
// nil_cloud.width = point_cloud_ptr->width;
// nil_cloud.height = point_cloud_ptr->height;
// nil_cloud.is_dense = false;
// initialise the final cloud
// final_cloud_ptr.width = point_cloud_ptr->width;
// final_cloud_ptr.height = point_cloud_ptr->height;
// final_cloud_ptr.is_dense = false;
//
// std::cout << final_cloud_ptr->width << "\n" << std::endl;
// std::cout << final_cloud_ptr->height << "\n" << std::endl;
// main filtering loop
for (int i = 0; i < static_cast<int>(point_cloud_ptr->height); i++ )
{
for (int j = 0; j < static_cast<int>(point_cloud_ptr->width); j++)
{
// final_cloud_ptr->points.push_back (pcl::PointXYZRGBA (nan, nan, nan, 0));
//debug pointer
// std::cout << "correct upto this five " << std::endl;
// just iteratively storing one point cloud data to another
// std::cout << i*point_cloud_ptr->width + j << std::endl;
// std::cout << point_cloud_ptr->points[i*point_cloud_ptr->width + j].x << std::endl;
// std::cout << point_cloud_ptr->points[i*point_cloud_ptr->width + j].y << std::endl;
// std::cout << point_cloud_ptr->points[i*point_cloud_ptr->width + j].z << std::endl;
// std::cout << point_cloud_ptr->points[i*point_cloud_ptr->width + j].rgba << std::endl;
// std::cout << point_cloud_ptr->points[i*point_cloud_ptr->width + j].x<< std::endl;
// size_t hh= point_cloud_ptr->points[i*point_cloud_ptr->width + j].x;
// nil_cloud.points[0].x =33;
// std::cout << nil_cloud.points[0].x<< std::endl;
if(point_cloud_ptr->points[i*point_cloud_ptr->width + j].x <= - 0.5 || point_cloud_ptr->points[i*point_cloud_ptr->width + j].x >= 0.5 || point_cloud_ptr->points[i*point_cloud_ptr->width + j].y <= -0.3 || point_cloud_ptr->points[i*point_cloud_ptr->width + j].y >= 0.5 || point_cloud_ptr->points[i*point_cloud_ptr->width + j].z <=0 || point_cloud_ptr->points[i*point_cloud_ptr->width + j].z >= 1.5)
{
point_cloud_ptr->points[i*point_cloud_ptr->width + j].x = NAN;
point_cloud_ptr->points[i*point_cloud_ptr->width + j].y = NAN;
point_cloud_ptr->points[i*point_cloud_ptr->width + j].z = NAN;
point_cloud_ptr->points[i*point_cloud_ptr->width + j].rgba = 0;
}
// final_cloud_ptr.points[0].x =33;
// std::cout << final_cloud_ptr.points[0].x<< std::endl;
// final_cloud_ptr->points[i*(point_cloud_ptr->width) + j].x = point_cloud_ptr->points[i*point_cloud_ptr->width + j].x;
// final_cloud_ptr->points[i*point_cloud_ptr->width + j].y = point_cloud_ptr->points[i*point_cloud_ptr->width + j].y;
// final_cloud_ptr->points[i*point_cloud_ptr->width + j].z = point_cloud_ptr->points[i*point_cloud_ptr->width + j].z;
// final_cloud_ptr->points[i*point_cloud_ptr->width + j].rgba = point_cloud_ptr->points[i*point_cloud_ptr->width + j].rgba;
// if(point_cloud_ptr->points[i*point_cloud_ptr->width + j].x >= - 0.5 && point_cloud_ptr->points[i*point_cloud_ptr->width + j].x <= 0.5 && point_cloud_ptr->points[i*point_cloud_ptr->width + j].y >= -0.3 && point_cloud_ptr->points[i*point_cloud_ptr->width + j].y <= 0.5 && point_cloud_ptr->points[i*point_cloud_ptr->width + j].z >=0 && point_cloud_ptr->points[i*point_cloud_ptr->width + j].z <= 1.5)
// if(point_cloud_ptr->points[i*point_cloud_ptr->width + j].z >=0.0 && point_cloud_ptr->points[i*point_cloud_ptr->width + j].z <= 1.5)
//
// {
// final_cloud_ptr.points[i*point_cloud_ptr->width + j].x = point_cloud_ptr->points[i*point_cloud_ptr->width + j].x;
// final_cloud_ptr.points[i*point_cloud_ptr->width + j].y = point_cloud_ptr->points[i*point_cloud_ptr->width + j].y;
// final_cloud_ptr.points[i*point_cloud_ptr->width + j].z = point_cloud_ptr->points[i*point_cloud_ptr->width + j].z;
// final_cloud_ptr.points[i*point_cloud_ptr->width + j].rgba = point_cloud_ptr->points[i*point_cloud_ptr->width + j].rgba;
//
// //debug pointer
// std::cout << "correct upto this one " << std::endl;
// }
//
//
//
// else
// {
//
//
// final_cloud_ptr.points[i*point_cloud_ptr->width + j].x =point_cloud_ptr->points[i*point_cloud_ptr->width + j].x;
// final_cloud_ptr.points[i*point_cloud_ptr->width + j].y = point_cloud_ptr->points[i*point_cloud_ptr->width + j].y;
// //debug pointer
// std::cout << "correct upto this four" << std::endl;
//
// final_cloud_ptr.points[i*point_cloud_ptr->width + j].z = NAN;
// final_cloud_ptr.points[i*point_cloud_ptr->width + j].rgba = static_cast<uint32_t>(0);
// //debug pointer
// std::cout << "correct upto this two" << std::endl;
// }
//debug pointer
// std::cout << "correct upto this three" << std::endl;
}
}
// saving module
std::stringstream oss;
oss<<"saved_file2"<<".pcd";
pcl::io::savePCDFileASCII (oss.str(), *point_cloud_ptr);
}
// --------------
// -----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;
}
if (pcl::console::find_argument (argc, argv, "-l") >= 0)
{
live_update = true;
std::cout << "Live update is on.\n";
}
if (pcl::console::parse (argc, argv, "-rx", angular_resolution_x) >= 0)
std::cout << "Setting angular resolution in x-direction to "<<angular_resolution_x<<"deg.\n";
if (pcl::console::parse (argc, argv, "-ry", angular_resolution_y) >= 0)
std::cout << "Setting angular resolution in y-direction to "<<angular_resolution_y<<"deg.\n";
int tmp_coordinate_frame;
if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
{
coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
}
angular_resolution_x = pcl::deg2rad (angular_resolution_x);
angular_resolution_y = pcl::deg2rad (angular_resolution_y);
// ------------------------------------------------------------------
// -----Read pcd file or create example point cloud if not given-----
// ------------------------------------------------------------------
pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
if (!pcd_filename_indices.empty ())
{
std::string filename = argv[pcd_filename_indices[0]];
if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
{
std::cout << "Was not able to open file \""<<filename<<"\".\n";
printUsage (argv[0]);
return 0;
}
scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2])) *
Eigen::Affine3f (point_cloud.sensor_orientation_);
}
else
{
std::cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
for (float x=-0.5f; x<=0.5f; x+=0.01f)
{
for (float y=-0.5f; y<=0.5f; y+=0.01f)
{
PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
point_cloud.points.push_back (point);
}
}
point_cloud.width = (int) point_cloud.points.size (); point_cloud.height = 1;
}
// -----------------------------------------------
// -----Create RangeImage from the PointCloud-----
// -----------------------------------------------
float noise_level = 0.0;
float min_range = 0.0f;
int border_size = 1;
boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.setBackgroundColor (1, 1, 1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
//viewer.addCoordinateSystem (1.0f, "global");
//PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
viewer.initCameraParameters ();
setViewerPose(viewer, range_image.getTransformationToWorldSystem ());
// --------------------------
// -----Show range image-----
// --------------------------
pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
range_image_widget.showRangeImage (range_image);
//--------------------
// -----Main loop-----
//--------------------
while (!viewer.wasStopped ())
{
range_image_widget.spinOnce ();
viewer.spinOnce ();
pcl_sleep (0.01);
if (live_update)
{
scene_sensor_pose = viewer.getViewerPose();
range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size);
range_image_widget.showRangeImage (range_image);
}
}
}
int
main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
setUnseenToMaxRange = true;
std::cout << "Setting unseen values in range image to maximum range readings.\n";
}
if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)
std::cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\n";
int tmp_coordinate_frame;
if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
{
coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
}
if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
std::cout << "Setting support size to "<<support_size<<".\n";
if (pcl::console::parse (argc, argv, "-d", descriptor_size) >= 0)
std::cout << "Setting descriptor size to "<<descriptor_size<<".\n";
if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
std::cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution = pcl::deg2rad (angular_resolution);
// -----------------------
// -----Read pcd file-----
// -----------------------
pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
if (!pcd_filename_indices.empty ())
{
std::string filename = argv[pcd_filename_indices[0]];
if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
{
std::cout << "Was not able to open file \""<<filename<<"\".\n";
printUsage (argv[0]);
return 0;
}
scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2])) *
Eigen::Affine3f (point_cloud.sensor_orientation_);
std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)
std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
}
else
{
std::cout << "\nNo *.pcd file for scene given.\n\n";
printUsage (argv[0]);
return 1;
}
// -----------------------------------------------
// -----Create RangeImage from the PointCloud-----
// -----------------------------------------------
float noise_level = 0.0;
float min_range = 0.0f;
int border_size = 1;
pcl::RangeImage::Ptr range_image_ptr (new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
range_image.integrateFarRanges (far_ranges);
if (setUnseenToMaxRange)
range_image.setUnseenToMaxRange ();
// Extract NARF features:
std::cout << "Now extracting NARFs in every image point.\n";
std::vector<std::vector<pcl::Narf*> > narfs;
narfs.resize (range_image.points.size ());
int last_percentage=-1;
for (unsigned int y=0; y<range_image.height; ++y)
{
for (unsigned int x=0; x<range_image.width; ++x)
{
int index = y*range_image.width+x;
int percentage = (int) ((100*index) / range_image.points.size ());
if (percentage > last_percentage)
{
std::cout << percentage<<"% "<<std::flush;
last_percentage = percentage;
}
pcl::Narf::extractFromRangeImageAndAddToList (range_image, x, y, descriptor_size,
support_size, rotation_invariant != 0, narfs[index]);
//std::cout << "Extracted "<<narfs[index].size ()<<" features for pixel "<<x<<","<<y<<".\n";
}
}
std::cout << "100%\n";
std::cout << "Done.\n\n Now you can click on points in the image to visualize how the descriptor is "
<< "extracted and see the descriptor distances to every other point..\n";
//---------------------
// -----Show image-----
// --------------------
pcl::visualization::RangeImageVisualizer range_image_widget ("Scene range image"),
surface_patch_widget("Descriptor's surface patch"),
descriptor_widget("Descriptor"),
descriptor_distances_widget("descriptor distances");
range_image_widget.showRangeImage (range_image);
//range_image_widget.visualize_selected_point = true;
//--------------------
// -----Main loop-----
//--------------------
while (true)
{
range_image_widget.spinOnce (); // process GUI events
surface_patch_widget.spinOnce (); // process GUI events
descriptor_widget.spinOnce (); // process GUI events
pcl_sleep(0.01);
//if (!range_image_widget.mouse_click_happened)
continue;
//range_image_widget.mouse_click_happened = false;
//float clicked_pixel_x_f = range_image_widget.last_clicked_point_x,
//clicked_pixel_y_f = range_image_widget.last_clicked_point_y;
int clicked_pixel_x, clicked_pixel_y;
//range_image.real2DToInt2D (clicked_pixel_x_f, clicked_pixel_y_f, clicked_pixel_x, clicked_pixel_y);
if (!range_image.isValid (clicked_pixel_x, clicked_pixel_y))
continue;
//Vector3f clicked_3d_point;
//range_image.getPoint (clicked_pixel_x, clicked_pixel_y, clicked_3d_point);
//surface_patch_widget.show (false);
//descriptor_widget.show (false);"
int selected_index = clicked_pixel_y*range_image.width + clicked_pixel_x;
pcl::Narf narf;
if (!narf.extractFromRangeImage (range_image, clicked_pixel_x, clicked_pixel_y,
descriptor_size, support_size))
{
continue;
}
int surface_patch_pixel_size = narf.getSurfacePatchPixelSize ();
float surface_patch_world_size = narf.getSurfacePatchWorldSize ();
surface_patch_widget.showFloatImage (narf.getSurfacePatch (), surface_patch_pixel_size, surface_patch_pixel_size,
-0.5f*surface_patch_world_size, 0.5f*surface_patch_world_size, true);
float surface_patch_rotation = narf.getSurfacePatchRotation ();
float patch_middle = 0.5f* (float (surface_patch_pixel_size-1));
float angle_step_size = pcl::deg2rad (360.0f)/narf.getDescriptorSize ();
float cell_size = surface_patch_world_size/float (surface_patch_pixel_size),
cell_factor = 1.0f/cell_size,
max_dist = 0.5f*surface_patch_world_size,
line_length = cell_factor* (max_dist-0.5f*cell_size);
for (int descriptor_value_idx=0; descriptor_value_idx<narf.getDescriptorSize (); ++descriptor_value_idx)
{
float angle = descriptor_value_idx*angle_step_size + surface_patch_rotation;
//surface_patch_widget.markLine (patch_middle, patch_middle, patch_middle+line_length*sinf (angle),
//patch_middle+line_length*-cosf (angle), pcl::visualization::Vector3ub (0,255,0));
}
std::vector<float> rotations, strengths;
narf.getRotations (rotations, strengths);
float radius = 0.5f*surface_patch_pixel_size;
for (unsigned int i=0; i<rotations.size (); ++i)
{
//surface_patch_widget.markLine (radius-0.5, radius-0.5, radius-0.5f + 2.0f*radius*sinf (rotations[i]),
//radius-0.5f - 2.0f*radius*cosf (rotations[i]), pcl::visualization::Vector3ub (255,0,0));
}
descriptor_widget.showFloatImage (narf.getDescriptor (), narf.getDescriptorSize (), 1, -0.1f, 0.3f, true);
//===================================
//=====Compare with all features=====
//===================================
const std::vector<pcl::Narf*>& narfs_of_selected_point = narfs[selected_index];
if (narfs_of_selected_point.empty ())
continue;
//descriptor_distances_widget.show (false);
float* descriptor_distance_image = new float[range_image.points.size ()];
for (unsigned int point_index=0; point_index<range_image.points.size (); ++point_index)
{
float& descriptor_distance = descriptor_distance_image[point_index];
descriptor_distance = std::numeric_limits<float>::infinity ();
std::vector<pcl::Narf*>& narfs_of_current_point = narfs[point_index];
if (narfs_of_current_point.empty ())
continue;
for (unsigned int i=0; i<narfs_of_selected_point.size (); ++i)
{
for (unsigned int j=0; j<narfs_of_current_point.size (); ++j)
{
descriptor_distance = (std::min)(descriptor_distance,
narfs_of_selected_point[i]->getDescriptorDistance (*narfs_of_current_point[j]));
}
}
}
descriptor_distances_widget.showFloatImage (descriptor_distance_image, range_image.width, range_image.height,
-std::numeric_limits<float>::infinity (), std::numeric_limits<float>::infinity (), true);
delete[] descriptor_distance_image;
}
}
// --------------
// -----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));
}
}
// --------------
// -----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;
}
if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
setUnseenToMaxRange = true;
cout << "Setting unseen values in range image to maximum range readings.\n";
}
if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)
cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\n";
int tmp_coordinate_frame;
if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
{
coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
}
if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
cout << "Setting support size to "<<support_size<<".\n";
if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution = pcl::deg2rad (angular_resolution);
// ------------------------------------------------------------------
// -----Read pcd file or create example point cloud if not given-----
// ------------------------------------------------------------------
pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
if (!pcd_filename_indices.empty ())
{
std::string filename = argv[pcd_filename_indices[0]];
if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
{
cerr << "Was not able to open file \""<<filename<<"\".\n";
printUsage (argv[0]);
return 0;
}
scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2])) *
Eigen::Affine3f (point_cloud.sensor_orientation_);
std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)
std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
}
else
{
setUnseenToMaxRange = true;
cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
for (float x=-0.5f; x<=0.5f; x+=0.01f)
{
for (float y=-0.5f; y<=0.5f; y+=0.01f)
{
PointType point;
point.x = x;
point.y = y;
point.z = 2.0f - y;
point_cloud.points.push_back (point);
}
}
point_cloud.width = (int) point_cloud.points.size ();
point_cloud.height = 1;
}
// -----------------------------------------------
// -----Create RangeImage from the PointCloud-----
// -----------------------------------------------
float noise_level = 0.0;
float min_range = 0.0f;
int border_size = 1;
boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
range_image.integrateFarRanges (far_ranges);
if (setUnseenToMaxRange)
range_image.setUnseenToMaxRange ();
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.setBackgroundColor (1, 1, 1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
//viewer.addCoordinateSystem (1.0f);
//PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
viewer.initCameraParameters ();
setViewerPose (viewer, range_image.getTransformationToWorldSystem ());
// --------------------------
// -----Show range image-----
// --------------------------
pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
range_image_widget.showRangeImage (range_image);
// --------------------------------
// -----Extract NARF keypoints-----
// --------------------------------
pcl::RangeImageBorderExtractor range_image_border_extractor;
pcl::NarfKeypoint narf_keypoint_detector;
narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);
narf_keypoint_detector.setRangeImage (&range_image);
narf_keypoint_detector.getParameters ().support_size = support_size;
pcl::PointCloud<int> keypoint_indices;
narf_keypoint_detector.compute (keypoint_indices);
std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";
// ----------------------------------------------
// -----Show keypoints in range image widget-----
// ----------------------------------------------
//for (size_t i=0; i<keypoint_indices.points.size (); ++i)
//range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
//keypoint_indices.points[i]/range_image.width);
// -------------------------------------
// -----Show keypoints in 3D viewer-----
// -------------------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
keypoints.points.resize (keypoint_indices.points.size ());
for (size_t i=0; i<keypoint_indices.points.size (); ++i)
keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
// ------------------------------------------------------
// -----Extract NARF descriptors for interest points-----
// ------------------------------------------------------
std::vector<int> keypoint_indices2;
keypoint_indices2.resize (keypoint_indices.points.size ());
for (unsigned int i=0; i<keypoint_indices.size (); ++i) // This step is necessary to get the right vector type
keypoint_indices2[i]=keypoint_indices.points[i];
pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);
narf_descriptor.getParameters ().support_size = support_size;
narf_descriptor.getParameters ().rotation_invariant = rotation_invariant;
pcl::PointCloud<pcl::Narf36> narf_descriptors;
narf_descriptor.compute (narf_descriptors);
cout << "Extracted "<<narf_descriptors.size ()<<" descriptors for "
<<keypoint_indices.points.size ()<< " keypoints.\n";
//--------------------
// -----Main loop-----
//--------------------
while (!viewer.wasStopped ())
{
range_image_widget.spinOnce (); // process GUI events
viewer.spinOnce ();
pcl_sleep(0.01);
}
}
// --------------
// -----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;
}
if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
setUnseenToMaxRange = true;
cout << "Setting unseen values in range image to maximum range readings.\n";
}
int tmp_coordinate_frame;
if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
{
coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
}
if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution = pcl::deg2rad (angular_resolution);
// ------------------------------------------------------------------
// -----Read pcd file or create example point cloud if not given-----
// ------------------------------------------------------------------
pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
if (!pcd_filename_indices.empty ())
{
std::string filename = argv[pcd_filename_indices[0]];
if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
{
cout << "Was not able to open file \""<<filename<<"\".\n";
printUsage (argv[0]);
return 0;
}
scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2])) *
Eigen::Affine3f (point_cloud.sensor_orientation_);
std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
}
else
{
cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
for (float x=-0.5f; x<=0.5f; x+=0.01f)
{
for (float y=-0.5f; y<=0.5f; y+=0.01f)
{
PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
point_cloud.points.push_back (point);
}
}
point_cloud.width = (int) point_cloud.points.size (); point_cloud.height = 1;
}
// -----------------------------------------------
// -----Create RangeImage from the PointCloud-----
// -----------------------------------------------
float noise_level = 0.0;
float min_range = 0.0f;
int border_size = 1;
boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
range_image.integrateFarRanges (far_ranges);
if (setUnseenToMaxRange)
range_image.setUnseenToMaxRange ();
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.setBackgroundColor (1, 1, 1);
viewer.addCoordinateSystem (1.0f);
pcl::visualization::PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 0, 0, 0);
viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
//PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 150, 150, 150);
//viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
//viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 2, "range image");
// -------------------------
// -----Extract borders-----
// -------------------------
pcl::RangeImageBorderExtractor border_extractor (&range_image);
pcl::PointCloud<pcl::BorderDescription> border_descriptions;
border_extractor.compute (border_descriptions);
// ----------------------------------
// -----Show points in 3D viewer-----
// ----------------------------------
pcl::PointCloud<pcl::PointWithRange>::Ptr border_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
veil_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
shadow_points_ptr(new pcl::PointCloud<pcl::PointWithRange>);
pcl::PointCloud<pcl::PointWithRange>& border_points = *border_points_ptr,
& veil_points = * veil_points_ptr,
& shadow_points = *shadow_points_ptr;
for (int y=0; y< (int)range_image.height; ++y)
{
for (int x=0; x< (int)range_image.width; ++x)
{
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])
border_points.points.push_back (range_image.points[y*range_image.width + x]);
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT])
veil_points.points.push_back (range_image.points[y*range_image.width + x]);
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER])
shadow_points.points.push_back (range_image.points[y*range_image.width + x]);
}
}
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> border_points_color_handler (border_points_ptr, 0, 255, 0);
viewer.addPointCloud<pcl::PointWithRange> (border_points_ptr, border_points_color_handler, "border points");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "border points");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> veil_points_color_handler (veil_points_ptr, 255, 0, 0);
viewer.addPointCloud<pcl::PointWithRange> (veil_points_ptr, veil_points_color_handler, "veil points");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "veil points");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> shadow_points_color_handler (shadow_points_ptr, 0, 255, 255);
viewer.addPointCloud<pcl::PointWithRange> (shadow_points_ptr, shadow_points_color_handler, "shadow points");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "shadow points");
//-------------------------------------
// -----Show points on range image-----
// ------------------------------------
pcl::visualization::RangeImageVisualizer* range_image_borders_widget = NULL;
range_image_borders_widget =
pcl::visualization::RangeImageVisualizer::getRangeImageBordersWidget (range_image, -std::numeric_limits<float>::infinity (), std::numeric_limits<float>::infinity (), false,
border_descriptions, "Range image with borders");
// -------------------------------------
//--------------------
// -----Main loop-----
//--------------------
while (!viewer.wasStopped ())
{
range_image_borders_widget->spinOnce ();
viewer.spinOnce ();
pcl_sleep(0.01);
}
}
// --------------
// -----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));
}
}