point_cloud.cpp
Go to the documentation of this file.
00001 #include "pointmatcher_ros/point_cloud.h"
00002 #include "ros/ros.h"
00003 #include "boost/detail/endian.hpp"
00004 #include "tf/transform_listener.h"
00005 #include <vector>
00006 #include <memory>
00007 
00008 namespace PointMatcher_ros
00009 {
00010         using namespace std;
00011         
00013         template<typename T>
00014         typename PointMatcher<T>::DataPoints rosMsgToPointMatcherCloud(const sensor_msgs::PointCloud2& rosMsg)
00015         {
00016                 typedef PointMatcher<T> PM;
00017                 typedef typename PM::DataPoints DataPoints;
00018                 typedef typename DataPoints::Label Label;
00019                 typedef typename DataPoints::Labels Labels;
00020                 typedef typename DataPoints::View View;
00021                 
00022                 if (rosMsg.fields.empty())
00023                         return DataPoints();
00024                 
00025                 // fill labels
00026                 // conversions of descriptor fields from pcl
00027                 // see http://www.ros.org/wiki/pcl/Overview
00028                 Labels featLabels;
00029                 Labels descLabels;
00030                 vector<bool> isFeature;
00031                 for(auto it(rosMsg.fields.begin()); it != rosMsg.fields.end(); ++it)
00032                 {
00033                         const string name(it->name);
00034                         const size_t count(std::max<size_t>(it->count, 1));
00035                         if (name == "x" || name == "y" || name == "z")
00036                         {
00037                                 featLabels.push_back(Label(name, count));
00038                                 isFeature.push_back(true);
00039                         }
00040                         else if (name == "rgb" || name == "rgba")
00041                         {
00042                                 descLabels.push_back(Label("color", (name == "rgba") ? 4 : 3));
00043                                 isFeature.push_back(false);
00044                         }
00045                         else if ((it+1) != rosMsg.fields.end() && it->name == "normal_x" && (it+1)->name == "normal_y")
00046                         {
00047                                 if ((it+2) != rosMsg.fields.end() && (it+2)->name == "normal_z")
00048                                 {
00049                                         descLabels.push_back(Label("normals", 3));
00050                                         it += 2;
00051                                         isFeature.push_back(false);
00052                                         isFeature.push_back(false);
00053                                 }
00054                                 else
00055                                 {
00056                                         descLabels.push_back(Label("normals", 2));
00057                                         it += 1;
00058                                         isFeature.push_back(false);
00059                                 }
00060                                 isFeature.push_back(false);
00061                         }
00062                         else 
00063                         {
00064                                 descLabels.push_back(Label(name, count));
00065                                 isFeature.push_back(false);
00066                         }
00067                 }
00068                 featLabels.push_back(Label("pad", 1));
00069                 assert(isFeature.size() == rosMsg.fields.size());
00070                 
00071                 // create cloud
00072                 const unsigned pointCount(rosMsg.width * rosMsg.height);
00073                 DataPoints cloud(featLabels, descLabels, pointCount);
00074                 cloud.getFeatureViewByName("pad").setConstant(1);
00075                 
00076                 // fill cloud
00077                 // TODO: support big endian, pass through endian-swapping method just after the *reinterpret_cast
00078                 typedef sensor_msgs::PointField PF;
00079                 size_t fieldId = 0;
00080                 for(auto it(rosMsg.fields.begin()); it != rosMsg.fields.end(); ++it, ++fieldId)
00081                 {
00082                         if (it->name == "rgb" || it->name == "rgba")
00083                         {
00084                                 // special case for colors
00085                                 if (((it->datatype != PF::UINT32) && (it->datatype != PF::INT32) && (it->datatype != PF::FLOAT32)) || (it->count != 1))
00086                                         throw runtime_error(
00087                                                 (boost::format("Colors in a point cloud must be a single element of size 32 bits, found %1% elements of type %2%") % it->count % unsigned(it->datatype)).str()
00088                                         );
00089                                 View view(cloud.getDescriptorViewByName("color"));
00090                                 int ptId(0);
00091                                 for (size_t y(0); y < rosMsg.height; ++y)
00092                                 {
00093                                         const uint8_t* dataPtr(&rosMsg.data[0] + rosMsg.row_step*y);
00094                                         for (size_t x(0); x < rosMsg.width; ++x)
00095                                         {
00096                                                 const uint32_t rgba(*reinterpret_cast<const uint32_t*>(dataPtr + it->offset));
00097                                                 const T colorA(T((rgba >> 24) & 0xff) / 255.);
00098                                                 const T colorR(T((rgba >> 16) & 0xff) / 255.);
00099                                                 const T colorG(T((rgba >> 8) & 0xff) / 255.);
00100                                                 const T colorB(T((rgba >> 0) & 0xff) / 255.);
00101                                                 view(0, ptId) = colorR;
00102                                                 view(1, ptId) = colorG;
00103                                                 view(2, ptId) = colorB;
00104                                                 if (view.rows() > 3)
00105                                                         view(3, ptId) = colorA;
00106                                                 dataPtr += rosMsg.point_step;
00107                                                 ptId += 1;
00108                                         }
00109                                 }
00110                         }
00111                         else
00112                         {
00113                                 // get view for editing data
00114                                 View view(
00115                                          (it->name == "normal_x") ? cloud.getDescriptorRowViewByName("normals", 0) :
00116                                         ((it->name == "normal_y") ? cloud.getDescriptorRowViewByName("normals", 1) :
00117                                         ((it->name == "normal_z") ? cloud.getDescriptorRowViewByName("normals", 2) :
00118                                         ((isFeature[fieldId]) ? cloud.getFeatureViewByName(it->name) :
00119                                         cloud.getDescriptorViewByName(it->name))))
00120                                 );
00121                                 // use view to read data
00122                                 int ptId(0);
00123                                 const size_t count(std::max<size_t>(it->count, 1));
00124                                 for (size_t y(0); y < rosMsg.height; ++y)
00125                                 {
00126                                         const uint8_t* dataPtr(&rosMsg.data[0] + rosMsg.row_step*y);
00127                                         for (size_t x(0); x < rosMsg.width; ++x)
00128                                         {
00129                                                 const uint8_t* fPtr(dataPtr + it->offset);
00130                                                 for (unsigned dim(0); dim < count; ++dim)
00131                                                 {
00132                                                         switch (it->datatype)
00133                                                         {
00134                                                                 case PF::INT8: view(dim, ptId) = T(*reinterpret_cast<const int8_t*>(fPtr)); fPtr += 1; break;
00135                                                                 case PF::UINT8: view(dim, ptId) = T(*reinterpret_cast<const uint8_t*>(fPtr)); fPtr += 1; break;
00136                                                                 case PF::INT16: view(dim, ptId) = T(*reinterpret_cast<const int16_t*>(fPtr)); fPtr += 2; break;
00137                                                                 case PF::UINT16: view(dim, ptId) = T(*reinterpret_cast<const uint16_t*>(fPtr)); fPtr += 2; break;
00138                                                                 case PF::INT32: view(dim, ptId) = T(*reinterpret_cast<const int32_t*>(fPtr)); fPtr += 4; break;
00139                                                                 case PF::UINT32: view(dim, ptId) = T(*reinterpret_cast<const uint32_t*>(fPtr)); fPtr += 4; break;
00140                                                                 case PF::FLOAT32: view(dim, ptId) = T(*reinterpret_cast<const float*>(fPtr)); fPtr += 4; break;
00141                                                                 case PF::FLOAT64: view(dim, ptId) = T(*reinterpret_cast<const double*>(fPtr)); fPtr += 8; break;
00142                                                                 default: abort();
00143                                                         }
00144                                                 }
00145                                                 dataPtr += rosMsg.point_step;
00146                                                 ptId += 1;
00147                                         }
00148                                 }
00149                         }
00150                 }
00151 
00152                 
00153                 shared_ptr<typename PM::DataPointsFilter> filter(PM::get().DataPointsFilterRegistrar.create("RemoveNaNDataPointsFilter"));
00154                 return filter->filter(cloud);
00155         }
00156         
00157         template
00158         PointMatcher<float>::DataPoints rosMsgToPointMatcherCloud<float>(const sensor_msgs::PointCloud2& rosMsg);
00159         template
00160         PointMatcher<double>::DataPoints rosMsgToPointMatcherCloud<double>(const sensor_msgs::PointCloud2& rosMsg);
00161         
00162         
00163         template<typename T>
00164         typename PointMatcher<T>::DataPoints rosMsgToPointMatcherCloud(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps)
00165         {
00166                 typedef PointMatcher<T> PM;
00167                 typedef typename PM::DataPoints DataPoints;
00168                 typedef typename DataPoints::Label Label;
00169                 typedef typename DataPoints::Labels Labels;
00170                 typedef typename DataPoints::View View;
00171                 
00172                 Labels featLabels;
00173                 featLabels.push_back(Label("x", 1));
00174                 featLabels.push_back(Label("y", 1));
00175                 if(force3D)
00176                         featLabels.push_back(Label("z", 1));
00177 
00178                 featLabels.push_back(Label("pad", 1));
00179                 
00180     // Build descriptors
00181                 Labels descLabels;
00182                 if (!rosMsg.intensities.empty())
00183                 {
00184                         descLabels.push_back(Label("intensity", 1));
00185                         assert(rosMsg.intensities.size() == rosMsg.ranges.size());
00186                 }
00187     if(addTimestamps)
00188     {
00189                         descLabels.push_back(Label("timestamp", 3));
00190     }
00191                 
00192                 // filter points based on range
00193     std::vector<size_t> ids(rosMsg.ranges.size());
00194     std::vector<double> ranges(rosMsg.ranges.size());
00195     std::vector<double> intensities(rosMsg.intensities.size());
00196 
00197                 size_t goodCount(0);
00198                 for (size_t i = 0; i < rosMsg.ranges.size(); ++i)
00199                 {
00200                         const float range(rosMsg.ranges[i]);
00201                         if (range >= rosMsg.range_min && range <= rosMsg.range_max)
00202       {
00203         ranges[goodCount] = range;
00204         ids[goodCount] = i;
00205         if(!rosMsg.intensities.empty())
00206         {
00207           intensities[goodCount] = rosMsg.intensities[i];
00208         }
00209                                 ++goodCount;
00210       }
00211                 }
00212                 if (goodCount == 0)
00213                         return DataPoints();
00214 
00215     ids.resize(goodCount);
00216     ranges.resize(goodCount);
00217     if(!rosMsg.intensities.empty())
00218       intensities.resize(goodCount);
00219 
00220                 DataPoints cloud(featLabels, descLabels, goodCount);
00221                 cloud.getFeatureViewByName("pad").setConstant(1);
00222                 
00223                 // fill features
00224                 const ros::Time& startTime(rosMsg.header.stamp);
00225                 const ros::Time endTime(startTime + ros::Duration(rosMsg.time_increment * (rosMsg.ranges.size() - 1)));
00226     
00227                 for (size_t i = 0; i < ranges.size(); ++i)
00228                 {
00229                         const T angle = rosMsg.angle_min + ids[i]*rosMsg.angle_increment;
00230                         const T range(ranges[i]);
00231       const T x = cos(angle) * range;
00232       const T y = sin(angle) * range;
00233 
00234       if (listener)
00235       {
00236         /* Note:
00237           We do an approximation, as some filters like
00238           ObservationDirectionDataPointsFilter should be applied per 
00239           point *before* correcting them using the tf transform, but
00240           as we expect the scan to be fast with respect to the speed 
00241           of the robot, we consider this approximation as being ok.
00242         */
00243         const ros::Time curTime(rosMsg.header.stamp + ros::Duration(ids[i] * rosMsg.time_increment));
00244         // wait for transform
00245         listener->waitForTransform(
00246           rosMsg.header.frame_id,
00247           fixedFrame,
00248           curTime,
00249           ros::Duration(0.1)
00250         );
00251 
00252         // transform data
00253         geometry_msgs::PointStamped pin, pout;
00254         pin.header.stamp = curTime;
00255         pin.header.frame_id = rosMsg.header.frame_id;
00256         pin.point.x = x;
00257         pin.point.y = y;
00258         pin.point.z = 0;
00259 
00260         try
00261         {
00262       
00263           listener->transformPoint(
00264             fixedFrame,
00265             curTime,
00266             pin,
00267             fixedFrame,
00268             pout
00269           );
00270         }
00271         catch (const tf::ExtrapolationException& e)
00272         {
00273           //ROS_WARN_STREAM("Couldn't transform point: " << e.what());
00274           return DataPoints();
00275         }
00276 
00277         //cout << "pin: " << pin.point.x << ", " << pin.point.y << ", " << pin.point.z << endl;
00278         //cout << "pout: " << pout.point.x << ", " << pout.point.y << ", " << pout.point.z << endl;
00279 
00280         // write back
00281         cloud.features(0,i) = pout.point.x;
00282         cloud.features(1,i) = pout.point.y;
00283         if(force3D)
00284           cloud.features(2,i) = pout.point.z;
00285                                 
00286                         }
00287                 }
00288 
00289                 // fill descriptors
00290                 if (!rosMsg.intensities.empty())
00291                 {
00292                         auto is(cloud.getDescriptorViewByName("intensity"));
00293                         for (size_t i = 0; i < intensities.size(); ++i)
00294                         {
00295                                         is(0,i) = intensities[i];
00296                         }
00297                 }
00298 
00299     if(addTimestamps)
00300     {
00301                         auto is(cloud.getDescriptorViewByName("timestamp"));
00302 
00303                         for (size_t i = 0; i < ranges.size(); ++i)
00304       {
00305         const ros::Time curTime(rosMsg.header.stamp + ros::Duration(ids[i] * rosMsg.time_increment));
00306 
00307         const T Msec = round(curTime.sec/1e6);
00308         const T sec  = round(curTime.sec - Msec*1e6);
00309         const T nsec = round(curTime.nsec);
00310 
00311         is(0,i) = Msec;
00312         is(1,i) = sec;
00313         is(2,i) = nsec;
00314       }
00315     }
00316                 
00317                 //cerr << "point cloud:\n" << cloud.features.leftCols(10) << endl;
00318                 return cloud;
00319         }
00320         
00321         template
00322         PointMatcher<float>::DataPoints rosMsgToPointMatcherCloud<float>(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps);
00323         template
00324         PointMatcher<double>::DataPoints rosMsgToPointMatcherCloud<double>(const sensor_msgs::LaserScan& rosMsg, const tf::TransformListener* listener, const std::string& fixedFrame, const bool force3D, const bool addTimestamps);
00325 
00326 
00327         template<typename T>
00328         sensor_msgs::PointCloud2 pointMatcherCloudToRosMsg(const typename PointMatcher<T>::DataPoints& pmCloud, const std::string& frame_id, const ros::Time& stamp)
00329         {
00330                 sensor_msgs::PointCloud2 rosCloud;
00331                 typedef sensor_msgs::PointField PF;
00332                 
00333                 // check type and get sizes
00334                 BOOST_STATIC_ASSERT(is_floating_point<T>::value);
00335                 BOOST_STATIC_ASSERT((is_same<T, long double>::value == false));
00336                 uint8_t dataType;
00337                 size_t scalarSize;
00338                 if (typeid(T) == typeid(float))
00339                 {
00340                         dataType = PF::FLOAT32;
00341                         scalarSize = 4;
00342                 }
00343                 else
00344                 {
00345                         dataType = PF::FLOAT64;
00346                         scalarSize = 8;
00347                 }
00348                 
00349                 // build labels
00350                 unsigned offset(0);
00351                 assert(!pmCloud.featureLabels.empty());
00352                 assert(pmCloud.featureLabels[pmCloud.featureLabels.size()-1].text == "pad");
00353                 for(auto it(pmCloud.featureLabels.begin()); it != pmCloud.featureLabels.end(); ++it)
00354                 {
00355                         // last label is padding
00356                         if ((it + 1) == pmCloud.featureLabels.end())
00357                                 break;
00358                         PF pointField;
00359                         pointField.name = it->text;
00360                         pointField.offset = offset;
00361                         pointField.datatype= dataType;
00362                         pointField.count = it->span;
00363                         rosCloud.fields.push_back(pointField);
00364                         offset += it->span * scalarSize;
00365                 }
00366                 bool addZ(false);
00367                 if (!pmCloud.featureLabels.contains("z"))
00368                 {
00369                         PF pointField;
00370                         pointField.name = "z";
00371                         pointField.offset = offset;
00372                         pointField.datatype= dataType;
00373                         pointField.count = 1;
00374                         rosCloud.fields.push_back(pointField);
00375                         offset += scalarSize;
00376                         addZ = true;
00377                 }
00378                 const bool isDescriptor(!pmCloud.descriptorLabels.empty());
00379                 bool hasColor(false);
00380                 unsigned colorPos(0);
00381                 unsigned colorCount(0);
00382                 unsigned inDescriptorPos(0);
00383                 for(auto it(pmCloud.descriptorLabels.begin()); it != pmCloud.descriptorLabels.end(); ++it)
00384                 {
00385                         PF pointField;
00386                         if (it->text == "normals")
00387                         {
00388                                 assert((it->span == 2) || (it->span == 3));
00389                                 pointField.datatype = dataType;
00390                                 pointField.name = "normal_x";
00391                                 pointField.offset = offset;
00392                                 pointField.count = 1;
00393                                 rosCloud.fields.push_back(pointField);
00394                                 offset += scalarSize;
00395                                 pointField.name = "normal_y";
00396                                 pointField.offset = offset;
00397                                 pointField.count = 1;
00398                                 rosCloud.fields.push_back(pointField);
00399                                 offset += scalarSize;
00400                                 if (it->span == 3)
00401                                 {
00402                                         pointField.name = "normal_z";
00403                                         pointField.offset = offset;
00404                                         pointField.count = 1;
00405                                         rosCloud.fields.push_back(pointField);
00406                                         offset += scalarSize;
00407                                 }
00408                         }
00409                         else if (it->text == "color")
00410                         {
00411                                 colorPos = inDescriptorPos;
00412                                 colorCount = it->span;
00413                                 hasColor = true;
00414                                 pointField.datatype = (colorCount == 4) ? uint8_t(PF::UINT32) : uint8_t(PF::FLOAT32);
00415                                 pointField.name = (colorCount == 4) ? "rgba" : "rgb";
00416                                 pointField.offset = offset;
00417                                 pointField.count = 1;
00418                                 rosCloud.fields.push_back(pointField);
00419                                 offset += 4;
00420                         }
00421                         else
00422                         {
00423                                 pointField.datatype = dataType;
00424                                 pointField.name = it->text;
00425                                 pointField.offset = offset;
00426                                 pointField.count = it->span;
00427                                 rosCloud.fields.push_back(pointField);
00428                                 offset += it->span * scalarSize;
00429                         }
00430                         inDescriptorPos += it->span;
00431                 }
00432                 
00433                 // fill cloud with data
00434                 rosCloud.header.frame_id = frame_id;
00435                 rosCloud.header.stamp = stamp;
00436                 rosCloud.height = 1;
00437                 rosCloud.width = pmCloud.features.cols();
00438                 #ifdef BOOST_BIG_ENDIAN
00439                 rosCloud.is_bigendian = true;
00440                 #else // BOOST_BIG_ENDIAN
00441                 rosCloud.is_bigendian = false;
00442                 #endif // BOOST_BIG_ENDIAN
00443                 rosCloud.point_step = offset;
00444                 rosCloud.row_step = rosCloud.point_step * rosCloud.width;
00445                 rosCloud.is_dense = true;
00446                 rosCloud.data.resize(rosCloud.row_step * rosCloud.height);
00447                 const unsigned featureDim(pmCloud.features.rows()-1);
00448                 const unsigned descriptorDim(pmCloud.descriptors.rows());
00449                 assert(descriptorDim == inDescriptorPos);
00450                 const unsigned postColorPos(colorPos + colorCount);
00451                 assert(postColorPos <= inDescriptorPos);
00452                 const unsigned postColorCount(descriptorDim - postColorPos);
00453                 for (unsigned pt(0); pt < rosCloud.width; ++pt)
00454                 {
00455                         uint8_t *fPtr(&rosCloud.data[pt * offset]);
00456                         memcpy(fPtr, reinterpret_cast<const uint8_t*>(&pmCloud.features(0, pt)), scalarSize * featureDim);
00457                         fPtr += scalarSize * featureDim;
00458                         if (addZ)
00459                         {
00460                                 memset(fPtr, 0, scalarSize);
00461                                 fPtr += scalarSize;
00462                         }
00463                         if (isDescriptor)
00464                         {
00465                                 if (hasColor)
00466                                 {
00467                                         // before color
00468                                         memcpy(fPtr, reinterpret_cast<const uint8_t*>(&pmCloud.descriptors(0, pt)), scalarSize * colorPos);
00469                                         fPtr += scalarSize * colorPos;
00470                                         // compact color
00471                                         uint32_t rgba;
00472                                         unsigned colorR(unsigned(pmCloud.descriptors(colorPos+0, pt) * 255.) & 0xFF);
00473                                         unsigned colorG(unsigned(pmCloud.descriptors(colorPos+1, pt) * 255.) & 0xFF);
00474                                         unsigned colorB(unsigned(pmCloud.descriptors(colorPos+2, pt) * 255.) & 0xFF);
00475                                         unsigned colorA(0);
00476                                         if (colorCount == 4)
00477                                                 colorA = unsigned(pmCloud.descriptors(colorPos+3, pt) * 255.) & 0xFF;
00478                                         rgba = colorA << 24 | colorR << 16 | colorG << 8 | colorB;
00479                                         memcpy(fPtr, reinterpret_cast<const uint8_t*>(&rgba), 4);
00480                                         fPtr += 4;
00481                                         // after color
00482                                         memcpy(fPtr, reinterpret_cast<const uint8_t*>(&pmCloud.descriptors(postColorPos, pt)), scalarSize * postColorCount);
00483                                 }
00484                                 else
00485                                 {
00486                                         memcpy(fPtr, reinterpret_cast<const uint8_t*>(&pmCloud.descriptors(0, pt)), scalarSize * descriptorDim);
00487                                 }
00488                         }
00489                 }
00490 
00491                 // fill remaining information
00492                 rosCloud.header.frame_id = frame_id;
00493                 rosCloud.header.stamp = stamp;
00494                 
00495                 return rosCloud;
00496         }
00497         
00498         template
00499         sensor_msgs::PointCloud2 pointMatcherCloudToRosMsg<float>(const PointMatcher<float>::DataPoints& pmCloud, const std::string& frame_id, const ros::Time& stamp);
00500         template
00501         sensor_msgs::PointCloud2 pointMatcherCloudToRosMsg<double>(const PointMatcher<double>::DataPoints& pmCloud, const std::string& frame_id, const ros::Time& stamp);
00502 
00503 } // PointMatcher_ros

libpointmatcher_ros
Author(s): Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:28:17