Go to the documentation of this file.00001 #include<hrl_phri_2011/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 &pc, const std::string& topic, double rate)
00052 {
00053 ros::NodeHandle nh;
00054 ros::Publisher pub_pc = nh.advertise<PCRGB>(topic, 1);
00055 ros::Rate r(rate);
00056 while(ros::ok()) {
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)
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()) {
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 }