pcl_basic.cpp
Go to the documentation of this file.
00001 #include<hrl_head_registration/pcl_basic.h>
00002 #include <limits>
00003 
00004 void transformPC(const PCRGB &in_pc, PCRGB &out_pc, 
00005                  const Eigen::Affine3d& transform) 
00006                  {
00007     MatrixXd pt_mat = MatrixXd::Constant(4, in_pc.points.size(), 1.0);
00008     uint32_t i = 0;
00009     vector<bool> nans_list(in_pc.points.size());
00010     BOOST_FOREACH(PRGB const pt, in_pc.points) {
00011         if(pt.x == pt.x && pt.y == pt.y && pt.z == pt.z) {
00012             pt_mat(0, i) = pt.x; pt_mat(1, i) = pt.y; pt_mat(2, i) = pt.z; 
00013         }
00014         else
00015             nans_list[i] = true;
00016         i++;
00017     }
00018     MatrixXd trans_pt_mat = transform.matrix() * pt_mat;
00019     for(i=0;i<in_pc.points.size();i++) {
00020         PRGB pt;
00021         if(!nans_list[i]) {
00022             pt.x = trans_pt_mat(0, i); pt.y = trans_pt_mat(1, i); pt.z = trans_pt_mat(2, i); 
00023         } else {
00024             pt.x = numeric_limits<float>::quiet_NaN();
00025             pt.y = numeric_limits<float>::quiet_NaN();
00026             pt.z = numeric_limits<float>::quiet_NaN();
00027         }
00028         pt.rgb = in_pc.points[i].rgb;
00029         out_pc.points.push_back(pt);
00030     }
00031 }
00032 
00033 void extractIndices(const PCRGB::Ptr& in_pc, pcl::IndicesPtr& inds, PCRGB::Ptr& out_pc, bool is_negative) 
00034 {
00035     vector<bool> inds_bool(in_pc->points.size(), false);
00036     for(size_t i=0;i<inds->size();i++) {
00037         inds_bool[inds->operator[](i)] = true;
00038         if(inds->operator[](i) >= in_pc->points.size()) 
00039             ROS_ERROR("Bad index: %d", inds->operator[](i));
00040     }
00041     for(size_t i=0;i<in_pc->points.size();i++) {
00042         if(inds_bool[i] && is_negative || !inds_bool[i] && !is_negative)
00043             continue;
00044         const PRGB pt = in_pc->points[i];
00045         PRGB npt;
00046         npt.x = pt.x; npt.y = pt.y; npt.z = pt.z; npt.rgb = pt.rgb;
00047         out_pc->points.push_back(npt);
00048     }
00049 }
00050 
00051 void pubLoop(PCRGB::Ptr& pc, const std::string& topic, double rate, int number_runs) 
00052 {
00053     ros::NodeHandle nh;
00054     ros::Publisher pub_pc = nh.advertise<PCRGB>(topic, 1);
00055     ros::Rate r(rate);
00056     while(ros::ok() && number_runs-- > 0) {
00057         pc->header.stamp = ros::Time::now();
00058         pub_pc.publish(pc);
00059         r.sleep();
00060     }
00061 }
00062 
00063 void pubLoop(vector<PCRGB::Ptr> &pcs, const vector<std::string>& topics, double rate, int number_runs) 
00064 {
00065     ros::NodeHandle nh;
00066     vector<ros::Publisher> pub_pcs;
00067     for(size_t i=0;i<topics.size();i++) 
00068         pub_pcs.push_back(nh.advertise<PCRGB>(topics[i], 1));
00069     ros::Rate r(rate);
00070     while(ros::ok() && number_runs-- > 0) {
00071         for(size_t i=0;i<pcs.size();i++) {
00072             pcs[i]->header.stamp = ros::Time::now();
00073             pub_pcs[i].publish(*pcs[i]);
00074         }
00075         r.sleep();
00076     }
00077 }
00078 
00079 void boxFilter(const PCRGB &in_pc, PCRGB &out_pc,
00080                double min_x, double max_x, double min_y, double max_y, double min_z, double max_z) 
00081                {
00082     pcl::ConditionAnd<PRGB>::Ptr near_cond(new pcl::ConditionAnd<PRGB>());
00083     PCRGB::Ptr near_pts(new PCRGB());
00084     pcl::ConditionalRemoval<PRGB> near_extract;
00085     near_cond->addComparison(pcl::FieldComparison<PRGB>::Ptr(new pcl::FieldComparison<PRGB>(
00086                              "x", pcl::ComparisonOps::GT, min_x)));
00087     near_cond->addComparison(pcl::FieldComparison<PRGB>::Ptr(new pcl::FieldComparison<PRGB>(
00088                              "x", pcl::ComparisonOps::LT, max_x)));
00089     near_cond->addComparison(pcl::FieldComparison<PRGB>::Ptr(new pcl::FieldComparison<PRGB>(
00090                              "y", pcl::ComparisonOps::GT, min_y)));
00091     near_cond->addComparison(pcl::FieldComparison<PRGB>::Ptr(new pcl::FieldComparison<PRGB>(
00092                              "y", pcl::ComparisonOps::LT, max_y)));
00093     near_cond->addComparison(pcl::FieldComparison<PRGB>::Ptr(new pcl::FieldComparison<PRGB>(
00094                              "z", pcl::ComparisonOps::GT, min_z)));
00095     near_cond->addComparison(pcl::FieldComparison<PRGB>::Ptr(new pcl::FieldComparison<PRGB>(
00096                              "z", pcl::ComparisonOps::LT, max_z)));
00097     near_extract.setCondition(near_cond);
00098     near_extract.setKeepOrganized(true);
00099     near_extract.setInputCloud(in_pc.makeShared());
00100     near_extract.filter(out_pc);
00101 }
00102 
00103 void readPCBag(const string& filename, PCRGB::Ptr& pc) 
00104 {
00105     rosbag::Bag bag;
00106     bag.open(filename, rosbag::bagmode::Read);
00107     rosbag::View view(bag, rosbag::TopicQuery("/kinect_head/rgb/points"));
00108     BOOST_FOREACH(rosbag::MessageInstance const msg, view) {
00109         pc = msg.instantiate< PCRGB >();
00110         break;
00111     }
00112     bag.close();
00113 }
00114 
00115 void savePCBag(const string& filename, PCRGB::Ptr& pc)
00116 {
00117     rosbag::Bag bag;
00118     bag.open(filename, rosbag::bagmode::Write);
00119     bag.write("/kinect_head/rgb/points", ros::Time::now(), pc);
00120     bag.close();
00121 }


hrl_head_registration
Author(s): Kelsey Hawkins
autogenerated on Wed Nov 27 2013 11:45:27