pcl_conversion_util.h
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #ifndef JSK_RECOGNITION_UTILS_PCL_CONVERSION_UTIL_H_
00037 #define JSK_RECOGNITION_UTILS_PCL_CONVERSION_UTIL_H_
00038 #include <pcl/point_types.h>
00039 #include <pcl/point_cloud.h>
00040 #include <pcl_conversions/pcl_conversions.h>
00041 #include <geometry_msgs/Point32.h>
00042 #include <eigen_conversions/eigen_msg.h>
00043 #include <tf_conversions/tf_eigen.h>
00044 #include <pcl/range_image/range_image_planar.h>
00045 #include <visualization_msgs/Marker.h>
00046 #if ROS_VERSION_MINIMUM(1, 10, 0)
00047 // hydro and later
00048 typedef pcl_msgs::PointIndices PCLIndicesMsg;
00049 typedef pcl_msgs::ModelCoefficients PCLModelCoefficientMsg;
00050 #else
00051 // groovy
00052 typedef pcl::PointIndices PCLIndicesMsg;
00053 typedef pcl::ModelCoefficients PCLModelCoefficientMsg;
00054 #endif
00055 
00056 #include <opencv2/opencv.hpp>
00057 
00058 namespace jsk_recognition_utils
00059 {
00060 
00069   void rangeImageToCvMat(const pcl::RangeImage& range_image,
00070                          cv::Mat& mat);
00071   
00072   template<class FromT, class ToT>
00073   void pointFromXYZToVector(const FromT& msg,
00074                             ToT& p)
00075   {
00076     p[0] = msg.x; p[1] = msg.y; p[2] = msg.z;
00077   }
00078 
00079   template<class FromT, class ToT>
00080   void pointFromVectorToXYZ(const FromT& p,
00081                             ToT& msg)
00082   {
00083     msg.x = p[0]; msg.y = p[1]; msg.z = p[2];
00084   }
00085 
00086   template<class FromT, class ToT>
00087   void pointFromXYZToXYZ(const FromT& from,
00088                          ToT& to)
00089   {
00090     to.x = from.x; to.y = from.y; to.z = from.z;
00091   }
00092 
00093   template<class FromT, class ToT>
00094   void pointFromVectorToVector(const FromT& from,
00095                                ToT& to)
00096   {
00097     to[0] = from[0]; to[1] = from[1]; to[2] = from[2];
00098   }
00099 
00100   template<class FromT, class ToT>
00101   void convertMatrix4(const FromT& from,
00102                       ToT& to)
00103   {
00104     for (size_t i = 0; i < 4; i++) {
00105       for (size_t j = 0; j < 4; j++) {
00106         to(i, j) = from(i, j);
00107       }
00108     }
00109   }
00110 
00111   void convertEigenAffine3(const Eigen::Affine3d& from,
00112                            Eigen::Affine3f& to);
00113   void convertEigenAffine3(const Eigen::Affine3f& from,
00114                            Eigen::Affine3d& to);
00115 
00116   template <class PointT>
00117   void markerMsgToPointCloud(const visualization_msgs::Marker& input_marker,
00118                              int sample_nums,
00119                              pcl::PointCloud<PointT>& output_cloud)
00120   {
00121     std::vector<double> cumulative_areas;
00122     double total_area = 0;
00123 
00124     if (input_marker.points.size() != input_marker.colors.size()){
00125       ROS_ERROR("Color and Points nums is different in markerMsgToPointCloud");
00126       return;
00127     }
00128 
00129     //Gether the triangle areas
00130     for (int i = 0; i < (int)input_marker.points.size()/3; i++){
00131       geometry_msgs::Point p0_1;
00132       p0_1.x = input_marker.points[i*3].x - input_marker.points[i*3+2].x;
00133       p0_1.y = input_marker.points[i*3].y - input_marker.points[i*3+2].y;
00134       p0_1.z = input_marker.points[i*3].z - input_marker.points[i*3+2].z;
00135       geometry_msgs::Point p1_2;
00136       p1_2.x = input_marker.points[i*3+1].x - input_marker.points[i*3+2].x;
00137       p1_2.y = input_marker.points[i*3+1].y - input_marker.points[i*3+2].y;
00138       p1_2.z = input_marker.points[i*3+1].z - input_marker.points[i*3+2].z;
00139       geometry_msgs::Point outer_product;
00140       outer_product.x = p0_1.y * p1_2.z - p0_1.z * p1_2.y;
00141       outer_product.y = p0_1.z * p1_2.x - p0_1.x * p1_2.z;
00142       outer_product.z = p0_1.x * p1_2.y - p0_1.y * p1_2.x;
00143       double tmp_triangle_area = abs(sqrt( pow(outer_product.x*1000, 2) + pow(outer_product.y*1000, 2) + pow(outer_product.z*1000, 2)))/2;
00144       total_area += tmp_triangle_area;
00145       cumulative_areas.push_back(total_area);
00146     }
00147 
00148     //Gether Random sampling points in propotion to area size
00149     for(int i = 0; i < sample_nums; i++){
00150       double r = rand() * (1.0  / (RAND_MAX + 1.0)) * total_area;
00151       std::vector<double>::iterator low = std::lower_bound (cumulative_areas.begin (), cumulative_areas.end (), r);
00152       int index = int(low - cumulative_areas.begin ());
00153 
00154       //Get Target Triangle
00155       PointT p;
00156       std_msgs::ColorRGBA color;
00157       geometry_msgs::Point p0 = input_marker.points[index*3];
00158       geometry_msgs::Point p1 = input_marker.points[index*3+1];
00159       geometry_msgs::Point p2 = input_marker.points[index*3+2];
00160       std_msgs::ColorRGBA c0= input_marker.colors[index*3];
00161       std_msgs::ColorRGBA c1 = input_marker.colors[index*3+1];
00162       std_msgs::ColorRGBA c2 = input_marker.colors[index*3+2];
00163       r = rand() * (1.0  / (RAND_MAX + 1.0));
00164 
00165       geometry_msgs::Point point_on_p1_p2;
00166       point_on_p1_p2.x = p1.x*r + p2.x*(1.0 - r);
00167       point_on_p1_p2.y = p1.y*r + p2.y*(1.0 - r);
00168       point_on_p1_p2.z = p1.z*r + p2.z*(1.0 - r);
00169 
00170       color.r = c1.r*r + c2.r*(1.0 - r);
00171       color.g = c1.g*r + c2.g*(1.0 - r);
00172       color.b = c1.b*r + c2.b*(1.0 - r);
00173 
00174       r = sqrt(rand() * (1.0  / (RAND_MAX + 1.0)));
00175       geometry_msgs::Point target;
00176       target.x = point_on_p1_p2.x*r + p0.x*(1.0 - r);
00177       target.y = point_on_p1_p2.y*r + p0.y*(1.0 - r);
00178       target.z = point_on_p1_p2.z*r + p0.z*(1.0 - r);
00179       color.r = color.r*r + c0.r*(1.0 - r);
00180       color.g = color.g*r + c0.g*(1.0 - r);
00181       color.b = color.b*r + c0.b*(1.0 - r);
00182       p.x = target.x;
00183       p.y = target.y;
00184       p.z = target.z;
00185       p.r = color.r * 256;
00186       p.g = color.g * 256;
00187       p.b = color.b * 256;
00188       
00189       output_cloud.points.push_back(p);
00190     }
00191     output_cloud.width = sample_nums;
00192     output_cloud.height = 1;
00193   }
00194 
00195   inline bool isValidPoint(const pcl::PointXYZ& p)
00196   {
00197     return !std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z);
00198   }
00199 
00200   template <class PointT>
00201   inline bool isValidPoint(const PointT& p)
00202   {
00203     return !std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z);
00204   }
00205 
00206 }
00207 // extend pcl_conversions package's toPCL and fromPCL functions
00208 namespace pcl_conversions
00209 {  
00210   std::vector<pcl::PointIndices::Ptr>
00211   convertToPCLPointIndices(const std::vector<PCLIndicesMsg>& cluster_indices);
00212 
00213   std::vector<pcl::ModelCoefficients::Ptr>
00214   convertToPCLModelCoefficients(
00215     const std::vector<PCLModelCoefficientMsg>& coefficients);
00216   
00217   std::vector<PCLIndicesMsg>
00218   convertToROSPointIndices(
00219     const std::vector<pcl::PointIndices::Ptr> cluster_indices,
00220     const std_msgs::Header& header);
00221 
00222   std::vector<PCLIndicesMsg>
00223   convertToROSPointIndices(
00224     const std::vector<pcl::PointIndices> cluster_indices,
00225     const std_msgs::Header& header);
00226 
00227   std::vector<PCLModelCoefficientMsg>
00228   convertToROSModelCoefficients(
00229     const std::vector<pcl::ModelCoefficients::Ptr>& coefficients,
00230     const std_msgs::Header& header);
00231 
00232 }
00233 
00234 namespace tf
00235 {
00236   // for eigen float
00237   void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Affine3f& eigen);
00238   void poseEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Pose& msg);
00239   void transformMsgToEigen(const geometry_msgs::Transform& msg, Eigen::Affine3f& eigen);
00240   void transformEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Transform& msg);
00241   void transformTFToEigen(const tf::Transform& t, Eigen::Affine3f& eigen);
00242   void transformEigenToTF(Eigen::Affine3f& eigen , tf::Transform& t);
00243   void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3f& e);
00244   void vectorEigenToTF(const Eigen::Vector3f& e, tf::Vector3& t);
00245 }
00246 
00247 #endif


jsk_recognition_utils
Author(s):
autogenerated on Tue Jul 2 2019 19:40:37