From 1f8a8a2b87620f2d0c3f8537bf1a32d73ea2e439 Mon Sep 17 00:00:00 2001 From: Michalis Logothetis Date: Fri, 2 Feb 2018 18:47:21 +0200 Subject: [PATCH] remove hidden files --- src/backup_kinect2_csl_tracker.cpp~ | 242 --------- src/csl_detection_node.cpp~ | 100 ---- src/csl_tracker.cpp~ | 786 ---------------------------- src/kinect2_csl_tracker.bak | 364 ------------- src/kinect2_csl_tracker.bak~ | 352 ------------- src/kinect2_csl_tracker.cpp~ | 345 ------------ src/kinect2_freenect_tracker.cpp~ | 405 -------------- src/kinect2_read_write_node.cpp~ | 121 ----- src/kinect2_tracking_node.cpp~ | 309 ----------- src/kinect_tracking_node.cpp~ | 48 -- src/openni2_test.cpp~ | 68 --- 11 files changed, 3140 deletions(-) delete mode 100644 src/backup_kinect2_csl_tracker.cpp~ delete mode 100644 src/csl_detection_node.cpp~ delete mode 100644 src/csl_tracker.cpp~ delete mode 100644 src/kinect2_csl_tracker.bak delete mode 100644 src/kinect2_csl_tracker.bak~ delete mode 100644 src/kinect2_csl_tracker.cpp~ delete mode 100644 src/kinect2_freenect_tracker.cpp~ delete mode 100644 src/kinect2_read_write_node.cpp~ delete mode 100644 src/kinect2_tracking_node.cpp~ delete mode 100644 src/kinect_tracking_node.cpp~ delete mode 100644 src/openni2_test.cpp~ diff --git a/src/backup_kinect2_csl_tracker.cpp~ b/src/backup_kinect2_csl_tracker.cpp~ deleted file mode 100644 index a489270..0000000 --- a/src/backup_kinect2_csl_tracker.cpp~ +++ /dev/null @@ -1,242 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define WITH_PCL -#include - -using namespace pcl::tracking; - -typedef pcl::PointXYZRGB PointType; -typedef pcl::PointXYZRGB RefPointType; - -typedef ParticleXYZRPY ParticleT; -typedef pcl::PointCloud Cloud; -typedef Cloud::Ptr CloudPtr; -typedef Cloud::ConstPtr CloudConstPtr; -typedef ParticleFilterTracker ParticleFilter; - -using namespace std; - -CloudPtr cloud_pass_; -CloudPtr cloud_pass_downsampled_; -CloudPtr target_cloud; - -boost::mutex mtx_; -boost::shared_ptr tracker_; -bool new_cloud_; -double downsampling_grid_size_; -int counter; - - -//Filter along a specified dimension -void filterPassThrough (const CloudConstPtr &cloud, Cloud &result) -{ - pcl::PassThrough pass; - pass.setFilterFieldName ("z"); - pass.setFilterLimits (0.0, 10.0); - pass.setKeepOrganized (false); - pass.setInputCloud (cloud); - pass.filter (result); -} - - -void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size) -{ - pcl::ApproximateVoxelGrid grid; - grid.setLeafSize (static_cast (leaf_size), static_cast (leaf_size), static_cast (leaf_size)); - grid.setInputCloud (cloud); - grid.filter (result); -} - -void -cloud_cb (const CloudPtr &cloud) -{ - boost::mutex::scoped_lock lock (mtx_); - cloud_pass_.reset (new Cloud); - cloud_pass_downsampled_.reset (new Cloud); - filterPassThrough (cloud, *cloud_pass_); - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - - if(counter < 10){ - counter++; - }else{ - //Track the object - CloudConstPtr const_cloud_pass_downsampled_ (cloud_pass_downsampled_); - tracker_->setInputCloud (&const_cloud_pass_downsampled_); - tracker_->compute (); - new_cloud_ = true; - } -} - - -int main (int argc, char **argv){ - - ros::init (argc, argv, "kinect2_tracking_node"); - ros::NodeHandle nh; - ros::Rate loop_rate(30); - - if (argc < 2) - { - PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]); - exit (1); - } - - //read pcd file - target_cloud.reset(new Cloud()); - if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){ - std::cout << "pcd file not found" << std::endl; - exit(-1); - } - - // TODO: Remove () - pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); - - - // Freenect - processor freenectprocessor = CPU; - K2G k2g(freenectprocessor); - - // Viewer - boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0, 0, 0); - pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - viewer->addPointCloud(scene, rgb, "sample cloud"); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); - - scene = k2g.getCloud();//Get once and set camera axes - scene->sensor_orientation_.w() = 1.0; - scene->sensor_orientation_.x() = 0.0; - scene->sensor_orientation_.y() = 0.0; - scene->sensor_orientation_.z() = 0.0; - - - std::string device_id = std::string (argv[1]); - - counter = 0; - - //Set parameters - new_cloud_ = false; - downsampling_grid_size_ = 0.002; - - std::vector default_step_covariance = std::vector (6, 0.015 * 0.015); - default_step_covariance[3] *= 40.0; - default_step_covariance[4] *= 40.0; - default_step_covariance[5] *= 40.0; - - std::vector initial_noise_covariance = std::vector (6, 0.00001); - std::vector default_initial_mean = std::vector (6, 0.0); - - boost::shared_ptr > tracker - (new KLDAdaptiveParticleFilterOMPTracker (8)); - - ParticleT bin_size; - bin_size.x = 0.1f; - bin_size.y = 0.1f; - bin_size.z = 0.1f; - bin_size.roll = 0.1f; - bin_size.pitch = 0.1f; - bin_size.yaw = 0.1f; - - - //Set all parameters for KLDAdaptiveParticleFilterOMPTracker - tracker->setMaximumParticleNum (1000); - tracker->setDelta (0.99); - tracker->setEpsilon (0.2); - tracker->setBinSize (bin_size); - - //Set all parameters for ParticleFilter - tracker_ = tracker; - tracker_->setTrans (Eigen::Affine3f::Identity ()); - tracker_->setStepNoiseCovariance (default_step_covariance); - tracker_->setInitialNoiseCovariance (initial_noise_covariance); - tracker_->setInitialNoiseMean (default_initial_mean); - tracker_->setIterationNum (1); - tracker_->setParticleNum (600); - tracker_->setResampleLikelihoodThr(0.00); - tracker_->setUseNormal (false); - - - //Setup coherence object for tracking - ApproxNearestPairPointCloudCoherence::Ptr coherence = ApproxNearestPairPointCloudCoherence::Ptr - (new ApproxNearestPairPointCloudCoherence ()); - - boost::shared_ptr > distance_coherence - = boost::shared_ptr > (new DistanceCoherence ()); - coherence->addPointCoherence (distance_coherence); - - boost::shared_ptr > search (new pcl::search::Octree (0.01)); - coherence->setSearchMethod (search); - coherence->setMaximumDistance (0.01); - - tracker_->setCloudCoherence (coherence); - - //prepare the model of tracker's target - Eigen::Vector4f c; - Eigen::Affine3f trans = Eigen::Affine3f::Identity (); - CloudPtr transed_ref (new Cloud); - CloudPtr transed_ref_downsampled (new Cloud); - - pcl::compute3DCentroid (*target_cloud, c); - trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); - pcl::transformPointCloud (*target_cloud, *transed_ref, trans.inverse()); - gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); - - //set reference model and trans - tracker_->setReferenceCloud (transed_ref_downsampled); - tracker_->setTrans (trans); - - while (ros::ok()){ - - viewer->spinOnce (); - - scene = k2g.getCloud(); - cloud_cb(scene); - - pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - viewer->updatePointCloud (scene, rgb, "sample cloud"); - ros::spinOnce(); - - } - - - k2g.shutDown(); - - return 0; -} - diff --git a/src/csl_detection_node.cpp~ b/src/csl_detection_node.cpp~ deleted file mode 100644 index 27e5255..0000000 --- a/src/csl_detection_node.cpp~ +++ /dev/null @@ -1,100 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; - -pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud); -pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); -bool saveCloud = false; -unsigned int filesSaved = 0; - - -void -keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, - void* nothing) -{ - if (event.getKeySym() == "space" && event.keyDown()){ - saveCloud = true; - } - else - saveCloud = false; - -} - - void cloud_cb(const boost::shared_ptr& input){ - - //Get the Point Cloud - pcl::PCLPointCloud2 pcl_pc2; - pcl_conversions::toPCL(*input,pcl_pc2); - pcl::fromPCLPointCloud2(pcl_pc2,*source_cloud); - - - //Transform the Cloud - Eigen::Affine3f transform = Eigen::Affine3f::Identity(); - //translation (none) - transform.translation() << 0.0, 0.0, 0.0; - //rotation matrix -PI radians arround X axis - transform.rotate (Eigen::AngleAxisf (-M_PI, Eigen::Vector3f::UnitX())); - // Executing the transformation - pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); - pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform); - - viewer.registerKeyboardCallback(keyboardEventOccurred); - viewer.showCloud (transformed_cloud); - - if (saveCloud) - { - stringstream stream; - //stream << "inputCloud" << filesSaved << ".ply"; - stream << "inputCloud" << filesSaved << ".pcd"; - string filename = stream.str(); - //if (pcl::io::savePLYFile(filename, *transformed_cloud, true) == 0) - if (pcl::io::savePCDFile(filename, *transformed_cloud, true) == 0) - { - filesSaved++; - cout << "Saved " << filename << "." << endl; - } - else PCL_ERROR("Problem saving %s.\n", filename.c_str()); - - saveCloud = false; - } -} - - - -int main (int argc, char **argv){ - - ros::init (argc, argv, "csl_detection_node"); - ros::NodeHandle nh; - ros::Rate loop_rate( 10.0 ); - ros::Subscriber sub = nh.subscribe("/camera/depth_registered/points", 1, cloud_cb); - - while (ros::ok()){ - - - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} - - - - - - - diff --git a/src/csl_tracker.cpp~ b/src/csl_tracker.cpp~ deleted file mode 100644 index 96cbfce..0000000 --- a/src/csl_tracker.cpp~ +++ /dev/null @@ -1,786 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2012-, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include - -#include -#include - -#include - -#define FPS_CALC_BEGIN \ - static double duration = 0; \ - double start_time = pcl::getTime (); \ - -#define FPS_CALC_END(_WHAT_) \ - { \ - double end_time = pcl::getTime (); \ - static unsigned count = 0; \ - if (++count == 10) \ - { \ - std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(duration) << " Hz" << std::endl; \ - count = 0; \ - duration = 0.0; \ - } \ - else \ - { \ - duration += end_time - start_time; \ - } \ - } - -using namespace pcl::tracking; - -geometry_msgs::Vector3 obj_position; - -template -class OpenNISegmentTracking -{ -public: - //typedef pcl::PointXYZRGBANormal RefPointType; - typedef pcl::PointXYZRGBA RefPointType; - //typedef pcl::PointXYZ RefPointType; - typedef ParticleXYZRPY ParticleT; - - typedef pcl::PointCloud Cloud; - typedef pcl::PointCloud RefCloud; - typedef typename RefCloud::Ptr RefCloudPtr; - typedef typename RefCloud::ConstPtr RefCloudConstPtr; - typedef typename Cloud::Ptr CloudPtr; - typedef typename Cloud::ConstPtr CloudConstPtr; - //typedef KLDAdaptiveParticleFilterTracker ParticleFilter; - //typedef KLDAdaptiveParticleFilterOMPTracker ParticleFilter; - //typedef ParticleFilterOMPTracker ParticleFilter; - typedef ParticleFilterTracker ParticleFilter; - typedef typename ParticleFilter::CoherencePtr CoherencePtr; - typedef typename pcl::search::KdTree KdTree; - typedef typename KdTree::Ptr KdTreePtr; - ros::Publisher obj_pub; - ros::NodeHandle nh; - - OpenNISegmentTracking (const std::string& device_id, int thread_nr, double downsampling_grid_size, - bool use_convex_hull, - bool visualize_non_downsample, bool visualize_particles, - bool use_fixed) - : viewer_ ("PCL OpenNI Tracking Viewer") - , device_id_ (device_id) - , new_cloud_ (false) - , ne_ (thread_nr) - , counter_ (0) - , use_convex_hull_ (use_convex_hull) - , visualize_non_downsample_ (visualize_non_downsample) - , visualize_particles_ (visualize_particles) - , downsampling_grid_size_ (downsampling_grid_size) - { - - //Create Publishers - obj_pub = nh.advertise("/csl_tracker/position", 1); - - KdTreePtr tree (new KdTree (false)); - ne_.setSearchMethod (tree); - ne_.setRadiusSearch (0.03); - - std::vector default_step_covariance = std::vector (6, 0.015 * 0.015); - default_step_covariance[3] *= 40.0; - default_step_covariance[4] *= 40.0; - default_step_covariance[5] *= 40.0; - - std::vector initial_noise_covariance = std::vector (6, 0.00001); - std::vector default_initial_mean = std::vector (6, 0.0); - if (use_fixed) - { - boost::shared_ptr > tracker - (new ParticleFilterOMPTracker (thread_nr)); - tracker_ = tracker; - } - else - { - boost::shared_ptr > tracker - (new KLDAdaptiveParticleFilterOMPTracker (thread_nr)); - tracker->setMaximumParticleNum (500); - tracker->setDelta (0.99); - tracker->setEpsilon (0.2); - ParticleT bin_size; - bin_size.x = 0.1f; - bin_size.y = 0.1f; - bin_size.z = 0.1f; - bin_size.roll = 0.1f; - bin_size.pitch = 0.1f; - bin_size.yaw = 0.1f; - tracker->setBinSize (bin_size); - tracker_ = tracker; - } - - tracker_->setTrans (Eigen::Affine3f::Identity ()); - tracker_->setStepNoiseCovariance (default_step_covariance); - tracker_->setInitialNoiseCovariance (initial_noise_covariance); - tracker_->setInitialNoiseMean (default_initial_mean); - tracker_->setIterationNum (1); - - tracker_->setParticleNum (400); - tracker_->setResampleLikelihoodThr(0.00); - tracker_->setUseNormal (false); - // setup coherences - ApproxNearestPairPointCloudCoherence::Ptr coherence = ApproxNearestPairPointCloudCoherence::Ptr - (new ApproxNearestPairPointCloudCoherence ()); - // NearestPairPointCloudCoherence::Ptr coherence = NearestPairPointCloudCoherence::Ptr - // (new NearestPairPointCloudCoherence ()); - - boost::shared_ptr > distance_coherence - = boost::shared_ptr > (new DistanceCoherence ()); - coherence->addPointCoherence (distance_coherence); - - boost::shared_ptr > color_coherence - = boost::shared_ptr > (new HSVColorCoherence ()); - color_coherence->setWeight (0.1); - coherence->addPointCoherence (color_coherence); - - //boost::shared_ptr > search (new pcl::search::KdTree (false)); - boost::shared_ptr > search (new pcl::search::Octree (0.01)); - //boost::shared_ptr > search (new pcl::search::OrganizedNeighbor); - coherence->setSearchMethod (search); - coherence->setMaximumDistance (0.01); - tracker_->setCloudCoherence (coherence); - } - - bool - drawParticles (pcl::visualization::PCLVisualizer& viz) - { - ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles (); - if (particles) - { - if (visualize_particles_) - { - pcl::PointCloud::Ptr particle_cloud (new pcl::PointCloud ()); - for (size_t i = 0; i < particles->points.size (); i++) - { - pcl::PointXYZ point; - - point.x = particles->points[i].x; - point.y = particles->points[i].y; - point.z = particles->points[i].z; - particle_cloud->points.push_back (point); - } - - Eigen::Vector4f centroid; - pcl::compute3DCentroid (*particle_cloud, centroid); - - obj_position.x = centroid[0]; - obj_position.y = centroid[1]; - obj_position.z = centroid[2]; - - obj_pub.publish(obj_position); - //ros::spinOnce(); - - std::cout << "Centroid X:" << centroid[0] << " " << "Centroid Y:" << centroid[1] << " " << "Centroid Z:" << centroid[2] << std::endl; - - - { - pcl::visualization::PointCloudColorHandlerCustom blue_color (particle_cloud, 250, 99, 71); - if (!viz.updatePointCloud (particle_cloud, blue_color, "particle cloud")) - viz.addPointCloud (particle_cloud, blue_color, "particle cloud"); - } - } - - return true; - } - else - { - PCL_WARN ("no particles\n"); - return false; - } - } - - void - drawResult (pcl::visualization::PCLVisualizer& viz) - { - ParticleXYZRPY result = tracker_->getResult (); - Eigen::Affine3f transformation = tracker_->toEigenMatrix (result); - // move a little bit for better visualization - transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f); - RefCloudPtr result_cloud (new RefCloud ()); - - if (!visualize_non_downsample_) - pcl::transformPointCloud (*(tracker_->getReferenceCloud ()), *result_cloud, transformation); - else - pcl::transformPointCloud (*reference_, *result_cloud, transformation); - - { - pcl::visualization::PointCloudColorHandlerCustom red_color (result_cloud, 0, 0, 255); - if (!viz.updatePointCloud (result_cloud, red_color, "resultcloud")) - viz.addPointCloud (result_cloud, red_color, "resultcloud"); - } - - } - - void - viz_cb (pcl::visualization::PCLVisualizer& viz) - { - boost::mutex::scoped_lock lock (mtx_); - - if (!cloud_pass_) - { - boost::this_thread::sleep (boost::posix_time::seconds (1)); - return; - } - - if (new_cloud_ && cloud_pass_downsampled_) - { - CloudPtr cloud_pass; - if (!visualize_non_downsample_) - cloud_pass = cloud_pass_downsampled_; - else - cloud_pass = cloud_pass_; - - if (!viz.updatePointCloud (cloud_pass, "cloudpass")) - { - viz.addPointCloud (cloud_pass, "cloudpass"); - viz.resetCameraViewpoint ("cloudpass"); - } - } - - if (new_cloud_ && reference_) - { - bool ret = drawParticles (viz); - if (ret) - { - drawResult (viz); - - // draw some texts - viz.removeShape ("N"); - viz.addText ((boost::format ("number of Reference PointClouds: %d") % tracker_->getReferenceCloud ()->points.size ()).str (), - 10, 20, 20, 1.0, 1.0, 1.0, "N"); - - viz.removeShape ("M"); - viz.addText ((boost::format ("number of Measured PointClouds: %d") % cloud_pass_downsampled_->points.size ()).str (), - 10, 40, 20, 1.0, 1.0, 1.0, "M"); - - viz.removeShape ("tracking"); - viz.addText ((boost::format ("tracking: %f fps") % (1.0 / tracking_time_)).str (), - 10, 60, 20, 1.0, 1.0, 1.0, "tracking"); - - viz.removeShape ("downsampling"); - viz.addText ((boost::format ("downsampling: %f fps") % (1.0 / downsampling_time_)).str (), - 10, 80, 20, 1.0, 1.0, 1.0, "downsampling"); - - viz.removeShape ("computation"); - viz.addText ((boost::format ("computation: %f fps") % (1.0 / computation_time_)).str (), - 10, 100, 20, 1.0, 1.0, 1.0, "computation"); - - viz.removeShape ("particles"); - viz.addText ((boost::format ("particles: %d") % tracker_->getParticles ()->points.size ()).str (), - 10, 120, 20, 1.0, 1.0, 1.0, "particles"); - - } - } - new_cloud_ = false; - } - - void filterPassThrough (const CloudConstPtr &cloud, Cloud &result) - { - FPS_CALC_BEGIN; - pcl::PassThrough pass; - pass.setFilterFieldName ("z"); - pass.setFilterLimits (0.0, 10.0); - //pass.setFilterLimits (0.0, 1.5); - //pass.setFilterLimits (0.0, 0.6); - pass.setKeepOrganized (false); - pass.setInputCloud (cloud); - pass.filter (result); - //FPS_CALC_END("filterPassThrough"); - } - - void euclideanSegment (const CloudConstPtr &cloud, - std::vector &cluster_indices) - { - FPS_CALC_BEGIN; - pcl::EuclideanClusterExtraction ec; - KdTreePtr tree (new KdTree ()); - - ec.setClusterTolerance (0.05); // 2cm - ec.setMinClusterSize (50); - ec.setMaxClusterSize (25000); - //ec.setMaxClusterSize (400); - ec.setSearchMethod (tree); - ec.setInputCloud (cloud); - ec.extract (cluster_indices); - //FPS_CALC_END("euclideanSegmentation"); - } - - void gridSample (const CloudConstPtr &cloud, Cloud &result, double leaf_size = 0.01) - { - FPS_CALC_BEGIN; - double start = pcl::getTime (); - pcl::VoxelGrid grid; - //pcl::ApproximateVoxelGrid grid; - grid.setLeafSize (float (leaf_size), float (leaf_size), float (leaf_size)); - grid.setInputCloud (cloud); - grid.filter (result); - //result = *cloud; - double end = pcl::getTime (); - downsampling_time_ = end - start; - //FPS_CALC_END("gridSample"); - } - - void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size = 0.01) - { - FPS_CALC_BEGIN; - double start = pcl::getTime (); - //pcl::VoxelGrid grid; - pcl::ApproximateVoxelGrid grid; - grid.setLeafSize (static_cast (leaf_size), static_cast (leaf_size), static_cast (leaf_size)); - grid.setInputCloud (cloud); - grid.filter (result); - //result = *cloud; - double end = pcl::getTime (); - downsampling_time_ = end - start; - //FPS_CALC_END("gridSample"); - } - - void planeSegmentation (const CloudConstPtr &cloud, - pcl::ModelCoefficients &coefficients, - pcl::PointIndices &inliers) - { - FPS_CALC_BEGIN; - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients (true); - seg.setModelType (pcl::SACMODEL_PLANE); - seg.setMethodType (pcl::SAC_RANSAC); - seg.setMaxIterations (1000); - seg.setDistanceThreshold (0.03); - seg.setInputCloud (cloud); - seg.segment (inliers, coefficients); - //FPS_CALC_END("planeSegmentation"); - } - - void planeProjection (const CloudConstPtr &cloud, - Cloud &result, - const pcl::ModelCoefficients::ConstPtr &coefficients) - { - FPS_CALC_BEGIN; - pcl::ProjectInliers proj; - proj.setModelType (pcl::SACMODEL_PLANE); - proj.setInputCloud (cloud); - proj.setModelCoefficients (coefficients); - proj.filter (result); - //FPS_CALC_END("planeProjection"); - } - - void convexHull (const CloudConstPtr &cloud, - Cloud &, - std::vector &hull_vertices) - { - FPS_CALC_BEGIN; - pcl::ConvexHull chull; - chull.setInputCloud (cloud); - chull.reconstruct (*cloud_hull_, hull_vertices); - //FPS_CALC_END("convexHull"); - } - - void normalEstimation (const CloudConstPtr &cloud, - pcl::PointCloud &result) - { - FPS_CALC_BEGIN; - ne_.setInputCloud (cloud); - ne_.compute (result); - //FPS_CALC_END("normalEstimation"); - } - - void tracking (const RefCloudConstPtr &cloud) - { - double start = pcl::getTime (); - FPS_CALC_BEGIN; - tracker_->setInputCloud (cloud); - tracker_->compute (); - double end = pcl::getTime (); - //FPS_CALC_END("tracking"); - tracking_time_ = end - start; - } - - void addNormalToCloud (const CloudConstPtr &cloud, - const pcl::PointCloud::ConstPtr &, - RefCloud &result) - { - result.width = cloud->width; - result.height = cloud->height; - result.is_dense = cloud->is_dense; - for (size_t i = 0; i < cloud->points.size (); i++) - { - RefPointType point; - point.x = cloud->points[i].x; - point.y = cloud->points[i].y; - point.z = cloud->points[i].z; - point.rgba = cloud->points[i].rgba; - // point.normal[0] = normals->points[i].normal[0]; - // point.normal[1] = normals->points[i].normal[1]; - // point.normal[2] = normals->points[i].normal[2]; - result.points.push_back (point); - } - } - - void extractNonPlanePoints (const CloudConstPtr &cloud, - const CloudConstPtr &cloud_hull, - Cloud &result) - { - pcl::ExtractPolygonalPrismData polygon_extract; - pcl::PointIndices::Ptr inliers_polygon (new pcl::PointIndices ()); - polygon_extract.setHeightLimits (0.01, 10.0); - polygon_extract.setInputPlanarHull (cloud_hull); - polygon_extract.setInputCloud (cloud); - polygon_extract.segment (*inliers_polygon); - { - pcl::ExtractIndices extract_positive; - extract_positive.setNegative (false); - extract_positive.setInputCloud (cloud); - extract_positive.setIndices (inliers_polygon); - extract_positive.filter (result); - } - } - - void removeZeroPoints (const CloudConstPtr &cloud, - Cloud &result) - { - for (size_t i = 0; i < cloud->points.size (); i++) - { - PointType point = cloud->points[i]; - if (!(fabs(point.x) < 0.01 && - fabs(point.y) < 0.01 && - fabs(point.z) < 0.01) && - !pcl_isnan(point.x) && - !pcl_isnan(point.y) && - !pcl_isnan(point.z)) - result.points.push_back(point); - } - - result.width = static_cast (result.points.size ()); - result.height = 1; - result.is_dense = true; - } - - void extractSegmentCluster (const CloudConstPtr &cloud, - const std::vector cluster_indices, - const int segment_index, - Cloud &result) - { - pcl::PointIndices segmented_indices = cluster_indices[segment_index]; - for (size_t i = 0; i < segmented_indices.indices.size (); i++) - { - PointType point = cloud->points[segmented_indices.indices[i]]; - result.points.push_back (point); - } - result.width = pcl::uint32_t (result.points.size ()); - result.height = 1; - result.is_dense = true; - } - - void - cloud_cb (const CloudConstPtr &cloud) - { - boost::mutex::scoped_lock lock (mtx_); - double start = pcl::getTime (); - FPS_CALC_BEGIN; - cloud_pass_.reset (new Cloud); - cloud_pass_downsampled_.reset (new Cloud); - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); - pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); - filterPassThrough (cloud, *cloud_pass_); - if (counter_ < 10) - { - gridSample (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - } - else if (counter_ == 10) - { - //gridSample (cloud_pass_, *cloud_pass_downsampled_, 0.01); - cloud_pass_downsampled_ = cloud_pass_; - CloudPtr target_cloud; - if (use_convex_hull_) - { - planeSegmentation (cloud_pass_downsampled_, *coefficients, *inliers); - if (inliers->indices.size () > 3) - { - CloudPtr cloud_projected (new Cloud); - cloud_hull_.reset (new Cloud); - nonplane_cloud_.reset (new Cloud); - - planeProjection (cloud_pass_downsampled_, *cloud_projected, coefficients); - convexHull (cloud_projected, *cloud_hull_, hull_vertices_); - - extractNonPlanePoints (cloud_pass_downsampled_, cloud_hull_, *nonplane_cloud_); - target_cloud = nonplane_cloud_; - } - else - { - PCL_WARN ("cannot segment plane\n"); - } - } - else - { - PCL_WARN ("without plane segmentation\n"); - target_cloud = cloud_pass_downsampled_; - } - - if (target_cloud != NULL) - { - PCL_INFO ("segmentation, please wait...\n"); - std::vector cluster_indices; - euclideanSegment (target_cloud, cluster_indices); - if (cluster_indices.size () > 0) - { - // select the cluster to track - CloudPtr temp_cloud (new Cloud); - extractSegmentCluster (target_cloud, cluster_indices, 0, *temp_cloud); - Eigen::Vector4f c; - pcl::compute3DCentroid (*temp_cloud, c); - int segment_index = 0; - double segment_distance = c[0] * c[0] + c[1] * c[1]; - for (size_t i = 1; i < cluster_indices.size (); i++) - { - temp_cloud.reset (new Cloud); - extractSegmentCluster (target_cloud, cluster_indices, int (i), *temp_cloud); - pcl::compute3DCentroid (*temp_cloud, c); - double distance = c[0] * c[0] + c[1] * c[1]; - if (distance < segment_distance) - { - segment_index = int (i); - segment_distance = distance; - } - } - - segmented_cloud_.reset (new Cloud); - extractSegmentCluster (target_cloud, cluster_indices, segment_index, *segmented_cloud_); - //pcl::PointCloud::Ptr normals (new pcl::PointCloud); - //normalEstimation (segmented_cloud_, *normals); - RefCloudPtr ref_cloud (new RefCloud); - //addNormalToCloud (segmented_cloud_, normals, *ref_cloud); - ref_cloud = segmented_cloud_; - RefCloudPtr nonzero_ref (new RefCloud); - removeZeroPoints (ref_cloud, *nonzero_ref); - - PCL_INFO ("calculating cog\n"); - - RefCloudPtr transed_ref (new RefCloud); - pcl::compute3DCentroid (*nonzero_ref, c); - Eigen::Affine3f trans = Eigen::Affine3f::Identity (); - trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); - - //pcl::transformPointCloudWithNormals (*ref_cloud, *transed_ref, trans.inverse()); - pcl::transformPointCloud (*nonzero_ref, *transed_ref, trans.inverse()); - CloudPtr transed_ref_downsampled (new Cloud); - gridSample (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); - tracker_->setReferenceCloud (transed_ref_downsampled); - tracker_->setTrans (trans); - reference_ = transed_ref; - tracker_->setMinIndices (int (ref_cloud->points.size ()) / 2); - } - else - { - PCL_WARN ("euclidean segmentation failed\n"); - } - } - } - else - { - //normals_.reset (new pcl::PointCloud); - //normalEstimation (cloud_pass_downsampled_, *normals_); - //RefCloudPtr tracking_cloud (new RefCloud ()); - //addNormalToCloud (cloud_pass_downsampled_, normals_, *tracking_cloud); - //tracking_cloud = cloud_pass_downsampled_; - - //*cloud_pass_downsampled_ = *cloud_pass_; - //cloud_pass_downsampled_ = cloud_pass_; - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - tracking (cloud_pass_downsampled_); - } - - new_cloud_ = true; - double end = pcl::getTime (); - computation_time_ = end - start; - //FPS_CALC_END("computation"); - counter_++; - } - - void - run () - { - pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id_); - boost::function f = - boost::bind (&OpenNISegmentTracking::cloud_cb, this, _1); - interface->registerCallback (f); - - viewer_.runOnVisualizationThread (boost::bind(&OpenNISegmentTracking::viz_cb, this, _1), "viz_cb"); - - interface->start (); - - while (!viewer_.wasStopped ()) - boost::this_thread::sleep(boost::posix_time::seconds(1)); - interface->stop (); - } - - pcl::visualization::CloudViewer viewer_; - pcl::PointCloud::Ptr normals_; - CloudPtr cloud_pass_; - CloudPtr cloud_pass_downsampled_; - CloudPtr plane_cloud_; - CloudPtr nonplane_cloud_; - CloudPtr cloud_hull_; - CloudPtr segmented_cloud_; - CloudPtr reference_; - std::vector hull_vertices_; - - std::string device_id_; - boost::mutex mtx_; - bool new_cloud_; - pcl::NormalEstimationOMP ne_; // to store threadpool - boost::shared_ptr tracker_; - int counter_; - bool use_convex_hull_; - bool visualize_non_downsample_; - bool visualize_particles_; - double tracking_time_; - double computation_time_; - double downsampling_time_; - double downsampling_grid_size_; - }; - -void -usage (char** argv) -{ - std::cout << "usage: " << argv[0] << " [-C] [-g]\n\n"; - std::cout << " -C: initialize the pointcloud to track without plane segmentation" - << std::endl; - std::cout << " -D: visualizing with non-downsampled pointclouds." - << std::endl; - std::cout << " -P: not visualizing particle cloud." - << std::endl; - std::cout << " -fixed: use the fixed number of the particles." - << std::endl; - std::cout << " -d : specify the grid size of downsampling (defaults to 0.01)." - << std::endl; -} - -int -main (int argc, char** argv) -{ - - ros::init (argc, argv, "csl_tracker_node"); - - bool use_convex_hull = true; - bool visualize_non_downsample = false; - bool visualize_particles = true; - bool use_fixed = false; - - double downsampling_grid_size = 0.01; - - if (pcl::console::find_argument (argc, argv, "-C") > 0) - use_convex_hull = false; - if (pcl::console::find_argument (argc, argv, "-D") > 0) - visualize_non_downsample = true; - if (pcl::console::find_argument (argc, argv, "-P") > 0) - visualize_particles = false; - if (pcl::console::find_argument (argc, argv, "-fixed") > 0) - use_fixed = true; - pcl::console::parse_argument (argc, argv, "-d", downsampling_grid_size); - if (argc < 2) - { - usage (argv); - exit (1); - } - - std::string device_id = std::string (argv[1]); - - if (device_id == "--help" || device_id == "-h") - { - usage (argv); - exit (1); - } - - // open kinect - OpenNISegmentTracking v (device_id, 8, downsampling_grid_size, - use_convex_hull, - visualize_non_downsample, visualize_particles, - use_fixed); - v.run (); - - while (ros::ok()){ - //std::cout << "Hello" << std::endl; - //obj_pub.publish(obj_position); - //ros::spinOnce(); - //loop_rate.sleep(); - } -} diff --git a/src/kinect2_csl_tracker.bak b/src/kinect2_csl_tracker.bak deleted file mode 100644 index 69a632c..0000000 --- a/src/kinect2_csl_tracker.bak +++ /dev/null @@ -1,364 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define WITH_PCL -#include - -using namespace std; - -string pcd_file_path = "/home/michalis/Downloads/coke.ply"; - -using namespace pcl::tracking; - -typedef pcl::PointXYZRGB PointType; -typedef pcl::PointXYZRGBA RefPointType; -typedef ParticleXYZRPY ParticleT; - -typedef pcl::PointCloud Cloud; -typedef Cloud::Ptr CloudPtr; -typedef Cloud::ConstPtr CloudConstPtr; -typedef ParticleFilterTracker ParticleFilter; - -CloudPtr cloud_pass_; -CloudPtr cloud_pass_downsampled_; -// Target Cloud Variable -CloudPtr target_cloud; - -boost::mutex mtx_; -boost::shared_ptr tracker_; -bool new_cloud_; -double downsampling_grid_size_; -int counter; - - -//Filter along a specified dimension -void filterPassThrough (const CloudConstPtr &cloud, Cloud &result) -{ - pcl::PassThrough pass; - pass.setFilterFieldName ("z"); - pass.setFilterLimits (0.0, 10.0); - pass.setKeepOrganized (false); - pass.setInputCloud (cloud); - pass.filter (result); -} - - -void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size) -{ - pcl::ApproximateVoxelGrid grid; - grid.setLeafSize (static_cast (leaf_size), static_cast (leaf_size), static_cast (leaf_size)); - grid.setInputCloud (cloud); - grid.filter (result); -} - -//Draw the current particles -bool -drawParticles (pcl::visualization::PCLVisualizer& viz) -{ - ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles (); - if (particles && new_cloud_) - { - //Set pointCloud with particle's points - pcl::PointCloud::Ptr particle_cloud (new pcl::PointCloud ()); - for (size_t i = 0; i < particles->points.size (); i++) - { - pcl::PointXYZ point; - - point.x = particles->points[i].x; - point.y = particles->points[i].y; - point.z = particles->points[i].z; - particle_cloud->points.push_back (point); - } - - //Draw red particles - { - pcl::visualization::PointCloudColorHandlerCustom red_color (particle_cloud, 250, 99, 71); - - if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud")) - viz.addPointCloud (particle_cloud, red_color, "particle cloud"); - } - return true; - } - else - { - return false; - } -} - -//Draw model reference point cloud -void -drawResult (pcl::visualization::PCLVisualizer& viz) -{ - ParticleXYZRPY result = tracker_->getResult (); - Eigen::Affine3f transformation = tracker_->toEigenMatrix (result); - - //move close to camera a little for better visualization - transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f); - CloudPtr result_cloud (new Cloud ()); - pcl::transformPointCloud (*(tracker_->getReferenceCloud ()), *result_cloud, transformation); - - //Draw blue model reference point cloud - { - pcl::visualization::PointCloudColorHandlerCustom blue_color (result_cloud, 0, 0, 255); - - if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud")) - viz.addPointCloud (result_cloud, blue_color, "resultcloud"); - } -} - -//visualization's callback function -void -viz_cb (pcl::visualization::PCLVisualizer& viz) -{ - //boost::mutex::scoped_lock lock (mtx_); - - if (!cloud_pass_) - { - boost::this_thread::sleep (boost::posix_time::seconds (1)); - return; - } - - //Draw downsampled point cloud from sensor - if (new_cloud_ && cloud_pass_downsampled_) - { - CloudPtr cloud_pass; - cloud_pass = cloud_pass_downsampled_; - - if (!viz.updatePointCloud (cloud_pass, "cloudpass")) - { - viz.addPointCloud (cloud_pass, "cloudpass"); - viz.resetCameraViewpoint ("cloudpass"); - } - bool ret = drawParticles (viz); - if (ret) - drawResult (viz); - } - new_cloud_ = false; -} - -//OpenNI Grabber's cloud Callback function -void -cloud_cb (const CloudConstPtr &cloud) -{ - //boost::mutex::scoped_lock lock (mtx_); - cloud_pass_.reset (new Cloud); - cloud_pass_downsampled_.reset (new Cloud); - filterPassThrough (cloud, *cloud_pass_); - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - - if(counter < 10){ - counter++; - }else{ - //Track the object - tracker_->setInputCloud (cloud_pass_downsampled_); - tracker_->compute (); - new_cloud_ = true; - } -} - -int main (int argc, char **argv){ - - ros::init (argc, argv, "kinect2_tracking_node"); - ros::NodeHandle nh; - ros::Rate loop_rate(30); - - // TODO: Remove () - pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); - pcl::PointCloud::Ptr sceneA (new pcl::PointCloud ()); - - // Read Cloud RGB Type - pcl::PointCloud::Ptr read_cloud (new pcl::PointCloud()); - - //read pcd file - target_cloud.reset(new Cloud()); - if(pcl::io::loadPLYFile (pcd_file_path, *read_cloud) == -1){ - std::cout << "pcl file not found" << std::endl; - exit(-1); - } - - // Conver Target Point Cloud RGB to RGBA - pcl::copyPointCloud(*read_cloud, *target_cloud); - - //Initialize Freenect Class - processor freenectprocessor = CPU; - K2G k2g(freenectprocessor); - - // Viewer - boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer ("3D Viewer")); - - viewer->setBackgroundColor (0, 0, 0); - pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - //pcl::visualization::PointCloudColorHandlerCustom target_rgb(target_cloud, 255,0,0); - - viewer->addPointCloud(scene, rgb, "sample cloud"); - //viewer->addPointCloud(target_cloud, target_rgb, "target cloud"); - - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); - //viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud"); - - //Read Point Cloud using freenect2 Driver - scene = k2g.getCloud();//Get once and set camera axes - - - scene->sensor_orientation_.w() = 1.0; - scene->sensor_orientation_.x() = 0.0; - scene->sensor_orientation_.y() = 0.0; - scene->sensor_orientation_.z() = 0.0; - - - counter = 0; - - //Set parameters - new_cloud_ = false; - downsampling_grid_size_ = 1.002; - - std::vector default_step_covariance = std::vector (6, 0.015 * 0.015); - default_step_covariance[3] *= 40.0; - default_step_covariance[4] *= 40.0; - default_step_covariance[5] *= 40.0; - - std::vector initial_noise_covariance = std::vector (6, 0.00001); - std::vector default_initial_mean = std::vector (6, 0.0); - - boost::shared_ptr > tracker - (new KLDAdaptiveParticleFilterOMPTracker (8)); - - ParticleT bin_size; - bin_size.x = 0.1f; - bin_size.y = 0.1f; - bin_size.z = 0.1f; - bin_size.roll = 0.1f; - bin_size.pitch = 0.1f; - bin_size.yaw = 0.1f; - - //Set all parameters for KLDAdaptiveParticleFilterOMPTracker - tracker->setMaximumParticleNum (1000); - tracker->setDelta (0.99); - tracker->setEpsilon (0.2); - tracker->setBinSize (bin_size); - - //Set all parameters for ParticleFilter - tracker_ = tracker; - tracker_->setTrans (Eigen::Affine3f::Identity ()); - tracker_->setStepNoiseCovariance (default_step_covariance); - tracker_->setInitialNoiseCovariance (initial_noise_covariance); - tracker_->setInitialNoiseMean (default_initial_mean); - tracker_->setIterationNum (1); - tracker_->setParticleNum (600); - tracker_->setResampleLikelihoodThr(0.00); - tracker_->setUseNormal (false); - cout<<" Hello 1"<::Ptr coherence = ApproxNearestPairPointCloudCoherence::Ptr - (new ApproxNearestPairPointCloudCoherence ()); - - boost::shared_ptr > distance_coherence - = boost::shared_ptr > (new DistanceCoherence ()); - - coherence->addPointCoherence (distance_coherence); - - boost::shared_ptr > search (new pcl::search::Octree (0.01)); - - coherence->setSearchMethod (search); - coherence->setMaximumDistance (0.01); - - cout<<" Hello 2"<setCloudCoherence (coherence); - - cout<<" Hello 3"< (*target_cloud, c); - - trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); - - pcl::transformPointCloud (*target_cloud, *transed_ref, trans.inverse()); - - gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); - - cout<<" Hello 4"<setReferenceCloud (transed_ref_downsampled); - - cout<<" Hello 5"<setTrans (trans); - - cout<<" Hello 6"<spinOnce (); - - //Read Point Cloud using freenect2 Driver - scene = k2g.getCloud(); - pcl::copyPointCloud(*scene, *sceneA); - cout<<" Hello 8"< rgb(sceneA); - - viewer->updatePointCloud (sceneA, rgb, "sample cloud"); - //viewer->updatePointCloud(target_cloud, target_rgb, "target cloud"); - //ros::Duration(0.02).sleep(); - ros::spinOnce(); - - } - - - k2g.shutDown(); - - return 0; -} - diff --git a/src/kinect2_csl_tracker.bak~ b/src/kinect2_csl_tracker.bak~ deleted file mode 100644 index 122b89a..0000000 --- a/src/kinect2_csl_tracker.bak~ +++ /dev/null @@ -1,352 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define WITH_PCL -#include - -using namespace std; - -string pcd_file_path = "/home/michalis/Downloads/coke.ply"; - -using namespace pcl::tracking; - -typedef pcl::PointXYZRGB PointType; -typedef pcl::PointXYZRGBA RefPointType; -typedef ParticleXYZRPY ParticleT; - -typedef pcl::PointCloud Cloud; -typedef Cloud::Ptr CloudPtr; -typedef Cloud::ConstPtr CloudConstPtr; -typedef ParticleFilterTracker ParticleFilter; - -CloudPtr cloud_pass_; -CloudPtr cloud_pass_downsampled_; -// Target Cloud Variable -CloudPtr target_cloud; - -boost::mutex mtx_; -boost::shared_ptr tracker_; -bool new_cloud_; -double downsampling_grid_size_; -int counter; - - -//Filter along a specified dimension -void filterPassThrough (const CloudConstPtr &cloud, Cloud &result) -{ - pcl::PassThrough pass; - pass.setFilterFieldName ("z"); - pass.setFilterLimits (0.0, 10.0); - pass.setKeepOrganized (false); - pass.setInputCloud (cloud); - pass.filter (result); -} - - -void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size) -{ - pcl::ApproximateVoxelGrid grid; - grid.setLeafSize (static_cast (leaf_size), static_cast (leaf_size), static_cast (leaf_size)); - grid.setInputCloud (cloud); - grid.filter (result); -} - -//Draw the current particles -bool -drawParticles (pcl::visualization::PCLVisualizer& viz) -{ - ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles (); - if (particles && new_cloud_) - { - //Set pointCloud with particle's points - pcl::PointCloud::Ptr particle_cloud (new pcl::PointCloud ()); - for (size_t i = 0; i < particles->points.size (); i++) - { - pcl::PointXYZ point; - - point.x = particles->points[i].x; - point.y = particles->points[i].y; - point.z = particles->points[i].z; - particle_cloud->points.push_back (point); - } - - //Draw red particles - { - pcl::visualization::PointCloudColorHandlerCustom red_color (particle_cloud, 250, 99, 71); - - if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud")) - viz.addPointCloud (particle_cloud, red_color, "particle cloud"); - } - return true; - } - else - { - return false; - } -} - -//Draw model reference point cloud -void -drawResult (pcl::visualization::PCLVisualizer& viz) -{ - ParticleXYZRPY result = tracker_->getResult (); - Eigen::Affine3f transformation = tracker_->toEigenMatrix (result); - - //move close to camera a little for better visualization - transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f); - CloudPtr result_cloud (new Cloud ()); - pcl::transformPointCloud (*(tracker_->getReferenceCloud ()), *result_cloud, transformation); - - //Draw blue model reference point cloud - { - pcl::visualization::PointCloudColorHandlerCustom blue_color (result_cloud, 0, 0, 255); - - if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud")) - viz.addPointCloud (result_cloud, blue_color, "resultcloud"); - } -} - -//visualization's callback function -void -viz_cb (pcl::visualization::PCLVisualizer& viz) -{ - //boost::mutex::scoped_lock lock (mtx_); - - if (!cloud_pass_) - { - boost::this_thread::sleep (boost::posix_time::seconds (1)); - return; - } - - //Draw downsampled point cloud from sensor - if (new_cloud_ && cloud_pass_downsampled_) - { - CloudPtr cloud_pass; - cloud_pass = cloud_pass_downsampled_; - - if (!viz.updatePointCloud (cloud_pass, "cloudpass")) - { - viz.addPointCloud (cloud_pass, "cloudpass"); - viz.resetCameraViewpoint ("cloudpass"); - } - bool ret = drawParticles (viz); - if (ret) - drawResult (viz); - } - new_cloud_ = false; -} - -//OpenNI Grabber's cloud Callback function -void -cloud_cb (const CloudConstPtr &cloud) -{ - //boost::mutex::scoped_lock lock (mtx_); - cloud_pass_.reset (new Cloud); - cloud_pass_downsampled_.reset (new Cloud); - filterPassThrough (cloud, *cloud_pass_); - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - - if(counter < 10){ - counter++; - }else{ - //Track the object - tracker_->setInputCloud (cloud_pass_downsampled_); - tracker_->compute (); - new_cloud_ = true; - } -} - -int main (int argc, char **argv){ - - ros::init (argc, argv, "kinect2_tracking_node"); - ros::NodeHandle nh; - ros::Rate loop_rate(30); - - // TODO: Remove () - pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); - pcl::PointCloud::Ptr sceneA (new pcl::PointCloud ()); - - // Read Cloud RGB Type - pcl::PointCloud::Ptr read_cloud (new pcl::PointCloud()); - - //read pcd file - target_cloud.reset(new Cloud()); - if(pcl::io::loadPLYFile (pcd_file_path, *read_cloud) == -1){ - std::cout << "pcl file not found" << std::endl; - exit(-1); - } - - // Conver Target Point Cloud RGB to RGBA - pcl::copyPointCloud(*read_cloud, *target_cloud); - - //Initialize Freenect Class - processor freenectprocessor = CPU; - K2G k2g(freenectprocessor); - - // Viewer - boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer ("3D Viewer")); - - viewer->setBackgroundColor (0, 0, 0); - pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - //pcl::visualization::PointCloudColorHandlerCustom target_rgb(target_cloud, 255,0,0); - - viewer->addPointCloud(scene, rgb, "sample cloud"); - //viewer->addPointCloud(target_cloud, target_rgb, "target cloud"); - - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); - //viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud"); - - //Read Point Cloud using freenect2 Driver - scene = k2g.getCloud();//Get once and set camera axes - - - scene->sensor_orientation_.w() = 1.0; - scene->sensor_orientation_.x() = 0.0; - scene->sensor_orientation_.y() = 0.0; - scene->sensor_orientation_.z() = 0.0; - - - counter = 0; - - //Set parameters - new_cloud_ = false; - downsampling_grid_size_ = 0.002; - - std::vector default_step_covariance = std::vector (6, 0.015 * 0.015); - default_step_covariance[3] *= 40.0; - default_step_covariance[4] *= 40.0; - default_step_covariance[5] *= 40.0; - - std::vector initial_noise_covariance = std::vector (6, 0.00001); - std::vector default_initial_mean = std::vector (6, 0.0); - - boost::shared_ptr > tracker - (new KLDAdaptiveParticleFilterOMPTracker (8)); - - ParticleT bin_size; - bin_size.x = 0.1f; - bin_size.y = 0.1f; - bin_size.z = 0.1f; - bin_size.roll = 0.1f; - bin_size.pitch = 0.1f; - bin_size.yaw = 0.1f; - - //Set all parameters for KLDAdaptiveParticleFilterOMPTracker - tracker->setMaximumParticleNum (1000); - tracker->setDelta (0.99); - tracker->setEpsilon (0.2); - tracker->setBinSize (bin_size); - - //Set all parameters for ParticleFilter - tracker_ = tracker; - tracker_->setTrans (Eigen::Affine3f::Identity ()); - tracker_->setStepNoiseCovariance (default_step_covariance); - tracker_->setInitialNoiseCovariance (initial_noise_covariance); - tracker_->setInitialNoiseMean (default_initial_mean); - tracker_->setIterationNum (1); - tracker_->setParticleNum (600); - tracker_->setResampleLikelihoodThr(0.00); - tracker_->setUseNormal (false); - cout<<" Hello 1"<::Ptr coherence = ApproxNearestPairPointCloudCoherence::Ptr - (new ApproxNearestPairPointCloudCoherence ()); - - boost::shared_ptr > distance_coherence - = boost::shared_ptr > (new DistanceCoherence ()); - coherence->addPointCoherence (distance_coherence); - - boost::shared_ptr > search (new pcl::search::Octree (0.01)); - coherence->setSearchMethod (search); - coherence->setMaximumDistance (0.01); - cout<<" Hello 2"<setCloudCoherence (coherence); - cout<<" Hello 3"< (*target_cloud, c); - trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); - pcl::transformPointCloud (*target_cloud, *transed_ref, trans.inverse()); - gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); - - cout<<" Hello 4"<setReferenceCloud (transed_ref_downsampled); - - cout<<" Hello 5"<setTrans (trans); - - cout<<" Hello 6"<spinOnce (); - - //Read Point Cloud using freenect2 Driver - scene = k2g.getCloud(); - pcl::copyPointCloud(*scene, *sceneA); - cout<<" Hello 8"< rgb(sceneA); - - viewer->updatePointCloud (sceneA, rgb, "sample cloud"); - //viewer->updatePointCloud(target_cloud, target_rgb, "target cloud"); - //ros::Duration(0.02).sleep(); - ros::spinOnce(); - - } - - - k2g.shutDown(); - - return 0; -} - diff --git a/src/kinect2_csl_tracker.cpp~ b/src/kinect2_csl_tracker.cpp~ deleted file mode 100644 index be46543..0000000 --- a/src/kinect2_csl_tracker.cpp~ +++ /dev/null @@ -1,345 +0,0 @@ -#include -#include -//#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace pcl::tracking; - -string pcd_file_path = "/home/michalis/Downloads/coke.ply"; - - -typedef pcl::PointXYZRGBA RefPointType; -typedef ParticleXYZRPY ParticleT; -typedef pcl::PointCloud Cloud; -typedef Cloud::Ptr CloudPtr; -typedef Cloud::ConstPtr CloudConstPtr; -typedef ParticleFilterTracker ParticleFilter; - -CloudPtr cloud_pass_; -CloudPtr cloud_pass_downsampled_; -CloudPtr target_cloud; - -boost::mutex mtx_; -boost::shared_ptr tracker_; -bool new_cloud_; -double downsampling_grid_size_; -int counter; - - -//Filter along a specified dimension -void filterPassThrough (const CloudConstPtr &cloud, Cloud &result) -{ - pcl::PassThrough pass; - pass.setFilterFieldName ("z"); - pass.setFilterLimits (0.0, 10.0); - pass.setKeepOrganized (false); - pass.setInputCloud (cloud); - pass.filter (result); -} - - -void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size) -{ - pcl::ApproximateVoxelGrid grid; - grid.setLeafSize (static_cast (leaf_size), static_cast (leaf_size), static_cast (leaf_size)); - grid.setInputCloud (cloud); - grid.filter (result); -} - - -//Draw the current particles -bool -drawParticles (pcl::visualization::PCLVisualizer& viz) -{ - ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles (); - if (particles && new_cloud_) - { - //Set pointCloud with particle's points - pcl::PointCloud::Ptr particle_cloud (new pcl::PointCloud ()); - for (size_t i = 0; i < particles->points.size (); i++) - { - pcl::PointXYZ point; - - point.x = particles->points[i].x; - point.y = particles->points[i].y; - point.z = particles->points[i].z; - particle_cloud->points.push_back (point); - } - - //Draw red particles - { - pcl::visualization::PointCloudColorHandlerCustom red_color (particle_cloud, 250, 99, 71); - - if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud")) - viz.addPointCloud (particle_cloud, red_color, "particle cloud"); - } - return true; - } - else - { - return false; - } -} - -//Draw model reference point cloud -void -drawResult (pcl::visualization::PCLVisualizer& viz) -{ - ParticleXYZRPY result = tracker_->getResult (); - Eigen::Affine3f transformation = tracker_->toEigenMatrix (result); - - //move close to camera a little for better visualization - transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f); - CloudPtr result_cloud (new Cloud ()); - pcl::transformPointCloud (*(tracker_->getReferenceCloud ()), *result_cloud, transformation); - - //Draw blue model reference point cloud - { - pcl::visualization::PointCloudColorHandlerCustom blue_color (result_cloud, 0, 0, 255); - - if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud")) - viz.addPointCloud (result_cloud, blue_color, "resultcloud"); - } -} - -//visualization's callback function -void -viz_cb (pcl::visualization::PCLVisualizer& viz) -{ - boost::mutex::scoped_lock lock (mtx_); - - if (!cloud_pass_) - { - boost::this_thread::sleep (boost::posix_time::seconds (1)); - return; - } - - //Draw downsampled point cloud from sensor - if (new_cloud_ && cloud_pass_downsampled_) - { - CloudPtr cloud_pass; - cloud_pass = cloud_pass_downsampled_; - - if (!viz.updatePointCloud (cloud_pass, "cloudpass")) - { - viz.addPointCloud (cloud_pass, "cloudpass"); - viz.resetCameraViewpoint ("cloudpass"); - } - bool ret = drawParticles (viz); - if (ret) - drawResult (viz); - } - new_cloud_ = false; -} - -//OpenNI Grabber's cloud Callback function -void -cloud_cb (const CloudConstPtr &cloud) -{ - boost::mutex::scoped_lock lock (mtx_); - cloud_pass_.reset (new Cloud); - cloud_pass_downsampled_.reset (new Cloud); - filterPassThrough (cloud, *cloud_pass_); - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - - if(counter < 10){ - counter++; - }else{ - //Track the object - tracker_->setInputCloud (cloud_pass_downsampled_); - tracker_->compute (); - new_cloud_ = true; - } -} - -int -main (int argc, char** argv) -{ - - - /* - if (argc < 3) - { - PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]); - exit (1); - } - - - //read pcd file - target_cloud.reset(new Cloud()); - if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){ - std::cout << "pcd file not found" << std::endl; - exit(-1); - } - - std::string device_id = std::string (argv[1]); - */ - - // Read Cloud RGB Type - pcl::PointCloud::Ptr read_cloud (new pcl::PointCloud()); - - cout<<"Hello 1"< default_step_covariance = std::vector (6, 0.015 * 0.015); - default_step_covariance[3] *= 40.0; - default_step_covariance[4] *= 40.0; - default_step_covariance[5] *= 40.0; - - std::vector initial_noise_covariance = std::vector (6, 0.00001); - std::vector default_initial_mean = std::vector (6, 0.0); - - boost::shared_ptr > tracker - (new KLDAdaptiveParticleFilterOMPTracker (8)); - - cout<<"Hello 3"<setMaximumParticleNum (1000); - tracker->setDelta (0.99); - tracker->setEpsilon (0.2); - tracker->setBinSize (bin_size); - - //Set all parameters for ParticleFilter - tracker_ = tracker; - tracker_->setTrans (Eigen::Affine3f::Identity ()); - tracker_->setStepNoiseCovariance (default_step_covariance); - tracker_->setInitialNoiseCovariance (initial_noise_covariance); - tracker_->setInitialNoiseMean (default_initial_mean); - tracker_->setIterationNum (1); - tracker_->setParticleNum (600); - tracker_->setResampleLikelihoodThr(0.00); - tracker_->setUseNormal (false); - - - cout<<"Hello 4"<::Ptr coherence = ApproxNearestPairPointCloudCoherence::Ptr - (new ApproxNearestPairPointCloudCoherence ()); - - - cout<<"Hello 5"< > distance_coherence - = boost::shared_ptr > (new DistanceCoherence ()); - coherence->addPointCoherence (distance_coherence); - - boost::shared_ptr > search (new pcl::search::Octree (0.01)); - coherence->setSearchMethod (search); - coherence->setMaximumDistance (0.01); - - - - cout<<"Hello 6"<setCloudCoherence (coherence); - - //prepare the model of tracker's target - Eigen::Vector4f c; - Eigen::Affine3f trans = Eigen::Affine3f::Identity (); - CloudPtr transed_ref (new Cloud); - CloudPtr transed_ref_downsampled (new Cloud); - - pcl::compute3DCentroid (*target_cloud, c); - trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); - pcl::transformPointCloud (*target_cloud, *transed_ref, trans.inverse()); - gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); - - cout<<"Hello 7"<setReferenceCloud (transed_ref_downsampled); - tracker_->setTrans (trans); - - //Setup OpenNIGrabber and viewer - pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer"); - //pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id); - cout<<"Hello 8"< f = - boost::bind (&cloud_cb, _1); - cout<<"Hello 10"<registerCallback (f); - - cout<<"Hello 11"<runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb"); - - cout<<"Hello 12"<start(); - - cout<<"Hello 13"<wasStopped ()) - boost::this_thread::sleep(boost::posix_time::seconds(1)); - interface->stop(); -} diff --git a/src/kinect2_freenect_tracker.cpp~ b/src/kinect2_freenect_tracker.cpp~ deleted file mode 100644 index 5bec60d..0000000 --- a/src/kinect2_freenect_tracker.cpp~ +++ /dev/null @@ -1,405 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define WITH_PCL -#include - -using namespace std; - -string pcd_file_path = "/home/michalis/Downloads/coke.pcd"; - -using namespace pcl::tracking; - -typedef pcl::PointXYZRGB PointType; -typedef pcl::PointXYZRGBA RefPointType; -typedef ParticleXYZRPY ParticleT; - -typedef pcl::PointCloud Cloud; -typedef Cloud::Ptr CloudPtr; -typedef Cloud::ConstPtr CloudConstPtr; -typedef ParticleFilterTracker ParticleFilter; - -CloudPtr cloud_pass_; -CloudPtr cloud_pass_downsampled_; -// Target Cloud Variable -CloudPtr target_cloud; - -boost::mutex mtx_; -boost::shared_ptr tracker_; -bool new_cloud_; -double downsampling_grid_size_; -int counter; - - -//Filter along a specified dimension -void filterPassThrough (const CloudConstPtr &cloud, Cloud &result) -{ - pcl::PassThrough pass; - pass.setFilterFieldName ("z"); - pass.setFilterLimits (0.0, 10.0); - pass.setKeepOrganized (false); - pass.setInputCloud (cloud); - pass.filter (result); -} - - -void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size) -{ - pcl::ApproximateVoxelGrid grid; - grid.setLeafSize (static_cast (leaf_size), static_cast (leaf_size), static_cast (leaf_size)); - grid.setInputCloud (cloud); - grid.filter (result); -} - -//Draw the current particles -bool -drawParticles (pcl::visualization::PCLVisualizer& viz) -{ - ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles (); - if (particles && new_cloud_) - { - //Set pointCloud with particle's points - pcl::PointCloud::Ptr particle_cloud (new pcl::PointCloud ()); - for (size_t i = 0; i < particles->points.size (); i++) - { - pcl::PointXYZ point; - - point.x = particles->points[i].x; - point.y = particles->points[i].y; - point.z = particles->points[i].z; - particle_cloud->points.push_back (point); - } - - //Draw red particles - { - pcl::visualization::PointCloudColorHandlerCustom red_color (particle_cloud, 250, 99, 71); - - if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud")) - viz.addPointCloud (particle_cloud, red_color, "particle cloud"); - } - return true; - } - else - { - return false; - } -} - -//Draw model reference point cloud -void -drawResult (pcl::visualization::PCLVisualizer& viz) -{ - ParticleXYZRPY result = tracker_->getResult (); - Eigen::Affine3f transformation = tracker_->toEigenMatrix (result); - - //move close to camera a little for better visualization - transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f); - CloudPtr result_cloud (new Cloud ()); - pcl::transformPointCloud (*(tracker_->getReferenceCloud ()), *result_cloud, transformation); - - //Draw blue model reference point cloud - { - pcl::visualization::PointCloudColorHandlerCustom blue_color (result_cloud, 0, 0, 255); - - if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud")) - viz.addPointCloud (result_cloud, blue_color, "resultcloud"); - } -} - -//visualization's callback function -void -viz_cb (pcl::visualization::PCLVisualizer& viz) -{ - //boost::mutex::scoped_lock lock (mtx_); - - if (!cloud_pass_) - { - boost::this_thread::sleep (boost::posix_time::seconds (1)); - return; - } - - //Draw downsampled point cloud from sensor - if (new_cloud_ && cloud_pass_downsampled_) - { - CloudPtr cloud_pass; - cloud_pass = cloud_pass_downsampled_; - - if (!viz.updatePointCloud (cloud_pass, "cloudpass")) - { - viz.addPointCloud (cloud_pass, "cloudpass"); - viz.resetCameraViewpoint ("cloudpass"); - } - bool ret = drawParticles (viz); - if (ret) - drawResult (viz); - } - new_cloud_ = false; -} - -//OpenNI Grabber's cloud Callback function -void -cloud_cb (const CloudConstPtr &cloud) -{ - //boost::mutex::scoped_lock lock (mtx_); - cloud_pass_.reset (new Cloud); - cloud_pass_downsampled_.reset (new Cloud); - filterPassThrough (cloud, *cloud_pass_); - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - - if(counter < 10){ - counter++; - }else{ - //Track the object - //TODO - //tracker_->setInputCloud (cloud_pass_downsampled_); - tracker_->setInputCloud (cloud); - tracker_->compute (); - new_cloud_ = true; - } -} - -int main (int argc, char **argv){ - - ros::init (argc, argv, "kinect2_tracking_node"); - ros::NodeHandle nh; - ros::Rate loop_rate(30); - - // TODO: Remove () - pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); - pcl::PointCloud::Ptr sceneA (new pcl::PointCloud ()); - - // Read Cloud RGB Type - pcl::PointCloud::Ptr read_cloud (new pcl::PointCloud()); - - //read pcd file - - target_cloud.reset(new Cloud()); - /* - if(pcl::io::loadPLYFile (pcd_file_path, *read_cloud) == -1){ - std::cout << "pcl file not found" << std::endl; - exit(-1); - } - */ - if(pcl::io::loadPCDFile (pcd_file_path, *target_cloud) == -1){ - std::cout << "pcd file not found" << std::endl; - exit(-1); - } - - // Conver Target Point Cloud RGB to RGBA - //pcl::copyPointCloud(*read_cloud, *target_cloud); - - //Initialize Freenect Class - processor freenectprocessor = CPU; - K2G k2g(freenectprocessor); - - // Viewer - boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer ("3D Viewer")); - - viewer->setBackgroundColor (0, 0, 0); - pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - //pcl::visualization::PointCloudColorHandlerCustom target_rgb(target_cloud, 255,0,0); - - viewer->addPointCloud(scene, rgb, "sample cloud"); - //viewer->addPointCloud(target_cloud, target_rgb, "target cloud"); - - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); - //viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud"); - - //Read Point Cloud using freenect2 Driver - scene = k2g.getCloud();//Get once and set camera axes - - -// scene->sensor_orientation_.w() = 1.0; -// scene->sensor_orientation_.x() = 0.0; -// scene->sensor_orientation_.y() = 0.0; -// scene->sensor_orientation_.z() = 0.0; -// - - counter = 0; - - //Set parameters - new_cloud_ = false; - downsampling_grid_size_ = 0.002; - - std::vector default_step_covariance = std::vector (6, 0.015 * 0.015); - default_step_covariance[3] *= 40.0; - default_step_covariance[4] *= 40.0; - default_step_covariance[5] *= 40.0; - - std::vector initial_noise_covariance = std::vector (6, 0.00001); - std::vector default_initial_mean = std::vector (6, 0.0); - - boost::shared_ptr > tracker - (new KLDAdaptiveParticleFilterOMPTracker (8)); - - ParticleT bin_size; - bin_size.x = 0.1f; - bin_size.y = 0.1f; - bin_size.z = 0.1f; - bin_size.roll = 0.1f; - bin_size.pitch = 0.1f; - bin_size.yaw = 0.1f; - - //Set all parameters for KLDAdaptiveParticleFilterOMPTracker - tracker->setMaximumParticleNum (1000); - tracker->setDelta (0.99); - tracker->setEpsilon (0.2); - tracker->setBinSize (bin_size); - - //Set all parameters for ParticleFilter - tracker_ = tracker; - tracker_->setTrans (Eigen::Affine3f::Identity ()); - tracker_->setStepNoiseCovariance (default_step_covariance); - tracker_->setInitialNoiseCovariance (initial_noise_covariance); - tracker_->setInitialNoiseMean (default_initial_mean); - tracker_->setIterationNum (1); - tracker_->setParticleNum (600); - tracker_->setResampleLikelihoodThr(0.00); - tracker_->setUseNormal (false); - cout<<" Hello 1"<::Ptr coherence = ApproxNearestPairPointCloudCoherence::Ptr - (new ApproxNearestPairPointCloudCoherence ()); - - boost::shared_ptr > distance_coherence - = boost::shared_ptr > (new DistanceCoherence ()); - - coherence->addPointCoherence (distance_coherence); - - boost::shared_ptr > search (new pcl::search::Octree (0.01)); - - coherence->setSearchMethod (search); - coherence->setMaximumDistance (0.01); - - cout<<" Hello 2"<setCloudCoherence (coherence); - - cout<<" Hello 3"< (*target_cloud, c); - - trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); - - pcl::transformPointCloud (*target_cloud, *transed_ref, trans.inverse()); - - // Downsample Reference Point Cloud - gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); - - cout<<" Hello 4"<size()<size()<setReferenceCloud (transed_ref); - tracker_->setReferenceCloud (transed_ref_downsampled); - - cout<<" Hello 5"<setTrans (trans); - - cout<<" Hello 6"<spinOnce (); - - //Read Point Cloud using freenect2 Driver - scene = k2g.getCloud(); - pcl::copyPointCloud(*scene, *sceneA); - cout<<" Hello 8"<setInputCloud (cloud_pass_downsampled_); - - cout<<"Kinect point cloud size: "<size() <size()<size()<setInputCloud (cloud); - - cout<<" Tracker Compute"<compute (); - new_cloud_ = true; - } - - cout<<" Hello 9"< rgb(sceneA); - - viewer->updatePointCloud (sceneA, rgb, "sample cloud"); - //viewer->updatePointCloud(target_cloud, target_rgb, "target cloud"); - //ros::Duration(0.02).sleep(); - ros::spinOnce(); - loop_rate.sleep(); - - } - - - k2g.shutDown(); - - return 0; -} - diff --git a/src/kinect2_read_write_node.cpp~ b/src/kinect2_read_write_node.cpp~ deleted file mode 100644 index 7accc1b..0000000 --- a/src/kinect2_read_write_node.cpp~ +++ /dev/null @@ -1,121 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define WITH_PCL -#include - -typedef pcl::PointXYZRGB PointType; - -using namespace std; - -string model_filename_ = "/home/karrasg/Downloads/libfreenect2pclgrabber/build/test_cloud.ply"; - - - -int main (int argc, char **argv){ - - ros::init (argc, argv, "kinect2_read_write_node"); - ros::NodeHandle nh; - ros::Rate loop_rate(30); - - processor freenectprocessor = CPU; - - - pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); - pcl::PointCloud::Ptr model (new pcl::PointCloud ()); - - - K2G k2g(freenectprocessor); - - boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer ("3D Viewer")); - - - scene = k2g.getCloud();//Get once and set camera axes - scene->sensor_orientation_.w() = 0.0; - scene->sensor_orientation_.x() = 1.0; - scene->sensor_orientation_.y() = 0.0; - scene->sensor_orientation_.z() = 0.0; - - viewer->setBackgroundColor (0, 0, 0); - pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - viewer->addPointCloud(scene, rgb, "sample cloud"); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); - - - - - //Reading Model From File - if (pcl::io::loadPLYFile (model_filename_, *model) < 0){ - std::cout << "Error loading model cloud." << std::endl; - return (-1); - } - - - //Create Cube Model - //generate cloud (CUBE) - /*int side = 10 + 1; - model->points.resize (pow (side, 3)); - model->width = model->points.size (); - model->height = 1; - - int p = 0; - for (size_t i = 0; i < side; i++) - for (size_t j = 0; j < side; j++) - for (size_t k = 0; k < side; k++, p++) - { - model->points[p].getVector3fMap () = Eigen::Vector3f (i, j, k); - } */ - - - int i = 0; - while (i < 500000){ - - i++; - viewer->spinOnce (); - std::chrono::high_resolution_clock::time_point tnow = std::chrono::high_resolution_clock::now(); - - //scene = k2g.getCloud(); - - std::chrono::high_resolution_clock::time_point tpost = std::chrono::high_resolution_clock::now(); - //std::cout << "delta " << std::chrono::duration_cast>(tpost-tnow).count() * 1000 << std::endl; - //pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - //viewer->updatePointCloud (scene, rgb, "sample cloud"); - - pcl::visualization::PointCloudColorHandlerRGBField rgb(model); - viewer->updatePointCloud (model, rgb, "sample cloud"); - - - //pcl::io::savePCDFileASCII ("test_pcd.pcd", *scene); - //std::cerr << "Saved " << std::endl; - } - - - k2g.shutDown(); - - return 0; -} - diff --git a/src/kinect2_tracking_node.cpp~ b/src/kinect2_tracking_node.cpp~ deleted file mode 100644 index 0487534..0000000 --- a/src/kinect2_tracking_node.cpp~ +++ /dev/null @@ -1,309 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define WITH_PCL -#include - -typedef pcl::PointXYZRGB PointType; -typedef pcl::Normal NormalType; -typedef pcl::ReferenceFrame RFType; -typedef pcl::SHOT352 DescriptorType; - -using namespace std; - -string model_filename_ = "/home/michalis/Downloads/coke.ply" ; //"/home/karrasg/Downloads/libfreenect2pclgrabber/build/test_cloud.ply"; -//string model_filename_ = "/home/karrasg/Downloads/libfreenect2pclgrabber/build/test_cloud2.ply" ; - -//Algorithm params -bool show_keypoints_ (false); -bool show_correspondences_ (false); -bool use_cloud_resolution_ (false); -bool use_hough_ (true); -float model_ss_ (0.01f); -float scene_ss_ (0.03f); -float rf_rad_ (0.015f); -float descr_rad_ (0.02f); -float cg_size_ (0.001f); -float cg_thresh_ (.5f); - -int main (int argc, char **argv){ - - ros::init (argc, argv, "kinect2_tracking_node"); - ros::NodeHandle nh; - ros::Rate loop_rate(30); - - processor freenectprocessor = CPU; - - - pcl::PointCloud::Ptr model (new pcl::PointCloud ()); - pcl::PointCloud::Ptr model_keypoints (new pcl::PointCloud ()); - pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); - pcl::PointCloud::Ptr scene_keypoints (new pcl::PointCloud ()); - pcl::PointCloud::Ptr model_normals (new pcl::PointCloud ()); - pcl::PointCloud::Ptr scene_normals (new pcl::PointCloud ()); - pcl::PointCloud::Ptr model_descriptors (new pcl::PointCloud ()); - pcl::PointCloud::Ptr scene_descriptors (new pcl::PointCloud ()); - - - K2G k2g(freenectprocessor); - - boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer ("3D Viewer")); - - - scene = k2g.getCloud();//Get once and set camera axes - scene->sensor_orientation_.w() = 0.0; - scene->sensor_orientation_.x() = 1.0; - scene->sensor_orientation_.y() = 0.0; - scene->sensor_orientation_.z() = 0.0; - - viewer->setBackgroundColor (0, 0, 0); - pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - viewer->addPointCloud(scene, rgb, "sample cloud"); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); - - //Reading Model From File - if (pcl::io::loadPLYFile (model_filename_, *model) < 0){ - std::cout << "Error loading model cloud." << std::endl; - return (-1); - } - - - - // Compute Normals - pcl::NormalEstimationOMP norm_est; - norm_est.setKSearch (10); - norm_est.setInputCloud (model);//Model - norm_est.compute (*model_normals); - - // Downsample Model Cloud to Extract keypoints - pcl::UniformSampling uniform_sampling; - uniform_sampling.setInputCloud (model); - uniform_sampling.setRadiusSearch (model_ss_); - pcl::PointCloud keypointIndices1; - uniform_sampling.compute(keypointIndices1); - pcl::copyPointCloud(*model, keypointIndices1.points, *model_keypoints); - //std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl; - - - //Compute Descriptor for keypoints - pcl::SHOTEstimationOMP descr_est; - descr_est.setRadiusSearch (descr_rad_); - //Model first - descr_est.setInputCloud (model_keypoints); - descr_est.setInputNormals (model_normals); - descr_est.setSearchSurface (model); - descr_est.compute (*model_descriptors); - - - while (ros::ok()){ - - viewer->spinOnce (); - std::chrono::high_resolution_clock::time_point tnow = std::chrono::high_resolution_clock::now(); - - scene = k2g.getCloud(); - - std::chrono::high_resolution_clock::time_point tpost = std::chrono::high_resolution_clock::now(); - //std::cout << "delta " << std::chrono::duration_cast>(tpost-tnow).count() * 1000 << std::endl; - pcl::visualization::PointCloudColorHandlerRGBField rgb(scene); - viewer->updatePointCloud (scene, rgb, "sample cloud"); - - - //Compute Normals Scene - norm_est.setInputCloud (scene); - norm_est.compute (*scene_normals); - - //Downsample scene to extract keypoints - uniform_sampling.setInputCloud (scene); - uniform_sampling.setRadiusSearch (scene_ss_); - pcl::PointCloud keypointIndices2; - uniform_sampling.compute(keypointIndices2); - pcl::copyPointCloud(*scene, keypointIndices2.points, *scene_keypoints); - //std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl; - - //Compute Descriptors for scene - descr_est.setInputCloud (scene_keypoints); - descr_est.setInputNormals (scene_normals); - descr_est.setSearchSurface (scene); - descr_est.compute (*scene_descriptors); - - - // - //Find Model-Scene Correspondences with KdTree - pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ()); - - pcl::KdTreeFLANN match_search; - match_search.setInputCloud (model_descriptors); - - // For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector. - for (size_t i = 0; i < scene_descriptors->size (); ++i) - { - std::vector neigh_indices (1); - std::vector neigh_sqr_dists (1); - if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs - { - continue; - } - int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists); - if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design) - { - pcl::Correspondence corr (neigh_indices[0], static_cast (i), neigh_sqr_dists[0]); - model_scene_corrs->push_back (corr); - } - } - std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl; - - - // - // Actual Clustering - std::vector > rototranslations; - std::vector clustered_corrs; - - // Using Hough3D - - // - // Compute (Keypoints) Reference Frames only for Hough - // - pcl::PointCloud::Ptr model_rf (new pcl::PointCloud ()); - pcl::PointCloud::Ptr scene_rf (new pcl::PointCloud ()); - - pcl::BOARDLocalReferenceFrameEstimation rf_est; - rf_est.setFindHoles (true); - rf_est.setRadiusSearch (rf_rad_); - - rf_est.setInputCloud (model_keypoints); - rf_est.setInputNormals (model_normals); - rf_est.setSearchSurface (model); - rf_est.compute (*model_rf); - - rf_est.setInputCloud (scene_keypoints); - rf_est.setInputNormals (scene_normals); - rf_est.setSearchSurface (scene); - rf_est.compute (*scene_rf); - - // Clustering - pcl::Hough3DGrouping clusterer; - clusterer.setHoughBinSize (cg_size_); - clusterer.setHoughThreshold (cg_thresh_); - clusterer.setUseInterpolation (true); - clusterer.setUseDistanceWeight (false); - - clusterer.setInputCloud (model_keypoints); - clusterer.setInputRf (model_rf); - clusterer.setSceneCloud (scene_keypoints); - clusterer.setSceneRf (scene_rf); - clusterer.setModelSceneCorrespondences (model_scene_corrs); - - //clusterer.cluster (clustered_corrs); - clusterer.recognize (rototranslations, clustered_corrs); - - // - // Output results - // - std::cout << "Model instances found: " << rototranslations.size () << std::endl; - for (size_t i = 0; i < rototranslations.size (); ++i) - { - std::cout << "\n Instance " << i + 1 << ":" << std::endl; - std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl; - - // Print the rotation matrix and translation vector - Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0); - Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3); - - printf ("\n"); - printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2)); - printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2)); - printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2)); - printf ("\n"); - printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2)); - } - /* - // - // Visualization - // - pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping"); - viewer.addPointCloud (scene, "scene_cloud"); - - pcl::PointCloud::Ptr off_scene_model (new pcl::PointCloud ()); - pcl::PointCloud::Ptr off_scene_model_keypoints (new pcl::PointCloud ()); - - if (show_correspondences_ || show_keypoints_) - { - // We are translating the model so that it doesn't end in the middle of the scene representation - pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0)); - pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0)); - - pcl::visualization::PointCloudColorHandlerCustom off_scene_model_color_handler (off_scene_model, 255, 255, 128); - viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model"); - } - - if (show_keypoints_) - { - pcl::visualization::PointCloudColorHandlerCustom scene_keypoints_color_handler (scene_keypoints, 0, 0, 255); - viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints"); - viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints"); - - pcl::visualization::PointCloudColorHandlerCustom off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255); - viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints"); - viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints"); - } - - for (size_t i = 0; i < rototranslations.size (); ++i) - { - pcl::PointCloud::Ptr rotated_model (new pcl::PointCloud ()); - pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]); - - std::stringstream ss_cloud; - ss_cloud << "instance" << i; - - pcl::visualization::PointCloudColorHandlerCustom rotated_model_color_handler (rotated_model, 255, 0, 0); - viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ()); - - if (show_correspondences_) - { - for (size_t j = 0; j < clustered_corrs[i].size (); ++j) - { - std::stringstream ss_line; - ss_line << "correspondence_line" << i << "_" << j; - PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query); - PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match); - - // We are drawing a line for each pair of clustered correspondences found between the model and the scene - viewer.addLine (model_point, scene_point, 0, 255, 0, ss_line.str ()); - } - } - }*/ - - - } - - - k2g.shutDown(); - - return 0; -} - diff --git a/src/kinect_tracking_node.cpp~ b/src/kinect_tracking_node.cpp~ deleted file mode 100644 index 6c13f3c..0000000 --- a/src/kinect_tracking_node.cpp~ +++ /dev/null @@ -1,48 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; - - - - - - - - -int main (int argc, char **argv){ - - ros::init (argc, argv, "kinect_tracking_node"); - ros::NodeHandle nh; - ros::Rate loop_rate( 10.0 ); - ros::Subscriber sub = nh.subscribe("/kinect2/sd/points", 1, cloud_cb); - - while (ros::ok()){ - - - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} - - - - - - - diff --git a/src/openni2_test.cpp~ b/src/openni2_test.cpp~ deleted file mode 100644 index f1a8c9c..0000000 --- a/src/openni2_test.cpp~ +++ /dev/null @@ -1,68 +0,0 @@ -#include - -#include -#include -#include - -using namespace std ; -typedef pcl::PointXYZ T; - -class SimpleOpenNIViewer { -public: - SimpleOpenNIViewer() - : viewer(new pcl::visualization::PCLVisualizer("PCL OpenNI Viewer")) {} - - void cloud_cb_(const pcl::PointCloud::ConstPtr &cloud) { - if (!viewer->wasStopped()) { - mtx.lock(); - viewer->updatePointCloud(cloud, "openni"); - mtx.unlock(); - } - } - - void run() { - - cout<<"Hello 1"<::Ptr cloud(new pcl::PointCloud); - viewer->addPointCloud(cloud, "openni", 0); - - cout<<"Hello 3"<::ConstPtr &)> f = - boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1); - - cout<<"Hello 4"<registerCallback(f); - - cout<<"Hello 5"<start(); - - cout<<"Hello 6"<wasStopped()) { - boost::this_thread::sleep(boost::posix_time::millisec(50)); - mtx.lock(); - viewer->spinOnce(); - mtx.unlock(); - } - - interface->stop(); - } - - std::mutex mtx; - pcl::visualization::PCLVisualizer::Ptr viewer; -}; - -int main() { - SimpleOpenNIViewer v; - v.run(); - return 0; -} -