Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
00048 typedef pcl_msgs::PointIndices PCLIndicesMsg;
00049 typedef pcl_msgs::ModelCoefficients PCLModelCoefficientMsg;
00050 #else
00051
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
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
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
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
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
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