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 _StructureColoring_h_
00037 #define _StructureColoring_h_
00038
00039 #include <structureColoring/OcTree.h>
00040 #include <structureColoring/grids/GridMap.h>
00041 #include <structureColoring/grids/ConnectedComponent.h>
00042 #include <structureColoring/structures/CylinderPatch.h>
00043 #include <structureColoring/structures/PlanePatch.h>
00044 #include <structureColoring/segcomp/rangeimageio.h>
00045 #include <structureColoring/StrColParams.h>
00046 #include <ros/ros.h>
00047 #include <sensor_msgs/PointCloud.h>
00048 #include <sensor_msgs/point_cloud_conversion.h>
00049 #include <pcl/point_types.h>
00050 #include <pcl/PointIndices.h>
00051 #include <opencv/highgui.h>
00052 #include <boost/shared_ptr.hpp>
00053 #include <string>
00054 #include <vector>
00055 #include <utility>
00056 #include <QtCore/QMutex>
00057 #include <omp.h>
00058 #include <Eigen/Dense>
00059 #include <Eigen/StdVector>
00060
00061
00062 class RosVisualization;
00063 class SphereUniformSampling;
00064 class NodePair;
00065
00066 class StructureColoring {
00067 public:
00068 typedef pcl::PointXYZRGB PointT;
00069 typedef pcl::PointCloud<PointT> PointCloud;
00070 typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00071
00072 typedef Plane3D::Plane3DPtr Plane3DPtr;
00073 typedef PlanePatch::PlanePatches PlanePatches;
00074 typedef PlanePatch::PlanePatchPtr PlanePatchPtr;
00075 typedef std::vector<PlanePatchPtr> PlanePatchVector;
00076
00077 typedef Cylinder3D::Cylinder3DPtr Cylinder3DPtr;
00078 typedef CylinderPatch::CylinderPatches CylinderPatches;
00079 typedef CylinderPatch::CylinderPatchPtr CylinderPatchPtr;
00080 typedef std::vector<CylinderPatchPtr> CylinderPatchVector;
00081
00082 typedef std::vector<NodePair> NodePairs;
00083
00089 StructureColoring(ros::NodeHandle& n, int argc, char* argv[]);
00090
00095 StructureColoring(int argc, char* argv[]): mVis(NULL){init(argc, argv);}
00096
00099 StructureColoring(): mVis(NULL){init();}
00100
00101 void init(ros::NodeHandle& n, int argc, char* argv[]);
00102
00103 void init(int argc, char* argv[]);
00104
00105 void init();
00106
00109 virtual ~StructureColoring();
00110
00113 bool waitOnKinectMsgs(){return mParams.mKinect;}
00114
00117 bool waitOnLaserMsgs(){return mParams.mLaser;}
00118
00122 void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& msg);
00123
00127 void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& msg);
00128
00129 void readPointCloudFromPCD(PointCloud& pointCloud, const std::string& filename, unsigned int& width,
00130 unsigned int& height, std::vector<unsigned int>& undefPoints, const float& zMin, const float& zMax);
00131 void filterCloud(PointCloud& pointCloud, std::vector<unsigned int>& undefPoints, const float& zMin, const float& zMax);
00132
00135 void getPointsFromFile();
00136
00137 void doSegmentation(PointCloudPtr pointCloud, PlanePatchVector& pointMapping, PlanePatches& extractedPlanes,
00138 CylinderPatchVector& pointMappingCylinders, CylinderPatches& extractedCylinders);
00139
00140 void filterMsgSetupCloud(PointCloud& pointCloud, const sensor_msgs::PointCloud2ConstPtr& pMsg, double zMin, double zMax, bool fromKinect, bool writePic = false, const std::string& picFilename = "", unsigned int picCounter = 0);
00141
00142 void setParams(StrColParams params){ mParams = params; }
00143 protected:
00144 typedef boost::shared_ptr<GridMap> GridMapPtr;
00145 typedef OcTree::NodePtr NodePtr;
00146 typedef OcTree::ValueClass ValueClass;
00147 typedef OcTree::OctreePtr OctreePtr;
00148 typedef OcTree::Allocator OctreeAllocator;
00149 typedef OcTree::SamplingMap OctreeSamplingMap;
00150 typedef OcTree::NodePointers NodePointers;
00151 typedef OcTree::NodePointerList NodePointerList;
00152 typedef std::vector<int> NodeIndices;
00153 typedef std::vector<int> PointIndices;
00154 typedef Eigen::Vector3f Vec3;
00155 typedef Eigen::Matrix3f Mat3;
00156 typedef std::vector<Vec3, Eigen::aligned_allocator<Vec3> > Points;
00157 typedef std::pair<PlanePatches::iterator, bool> PatchIterBoolPair;
00158 typedef std::vector<PatchIterBoolPair> PatchIterBoolPairVector;
00159 typedef std::vector<unsigned int> uints;
00160 typedef std::pair<PlanePatches::iterator, NodePointers> PatchIterNodePointersPair;
00161 typedef std::vector<PatchIterNodePointersPair> PatchIterNodePointersPairs;
00162 typedef std::list<NodePtr> NodePtrList;
00163 typedef std::vector<NodePtrList::iterator> NodePtrListIters;
00164 typedef std::pair<unsigned int, unsigned int> Pair2ui;
00165 typedef pcl::PointIndices pclPointIndices;
00166 typedef std::pair<PlanePatches::iterator, PointIndices > PlanePatchIterPointIndicesPair;
00167 typedef std::vector<PlanePatchIterPointIndicesPair> PlanePatchIterPointIndicesPairs;
00168 typedef std::map<PlanePatchPtr, NodePointers, CompPlanePtr> PatchToNodePointersMap;
00169 typedef std::vector<PlanePatchVector> NodeIndexToPlanePatchPointers;
00170 typedef std::map<PlanePatchPtr, NodeIndices, CompPlanePtr> PatchToNodeIndicesMap;
00171 typedef std::map<PlanePatchPtr, PointIndices, CompPlanePtr> PatchToPointIndicesMap;
00172 typedef std::vector<SphereUniformSampling> SphereUniformSamplings;
00173 typedef std::vector<int> PairIndices;
00174
00175 struct SphereBin{
00176 unsigned int rho;
00177 unsigned int i;
00178 unsigned int j;
00179 std::list<int>::iterator pit;
00180 std::list<float>::iterator wit;
00181 };
00182 typedef std::vector<SphereBin> SphereBins;
00183 typedef std::vector<SphereBins> SphereBinsVec;
00184
00190 static NodePointers getNodesFromIndices(const NodeIndices& nodeIndices, const NodePointers& nodes);
00191
00197 static Points getPointsFromIndices(const PointIndices& pointIndices, const PointCloud& cloud);
00198
00204 static void markAsSegmented(NodePtr currentNode, NodePtr nextNode = NULL, void* data = NULL);
00205
00210 static void flipToViewport(Vec3& vec, const Vec3& viewport);
00211
00212 void parseInput(std::string& paramFilename, std::string& inputFilename, std::string& outputFilename, int argc, char* argv[]);
00213 void initNode(ros::NodeHandle& n);
00214 void initParams();
00215 void initOctree();
00216
00217
00218 template<typename PointMappingT>
00219 void initSegmentation(PointMappingT& pointMapping, const PointCloud& pointCloud);
00220
00221 double segmentPlanes(PlanePatches& extractedPlanes, PlanePatchVector& pointMapping, OcTree& octree, const PointCloudPtr& pointCloud);
00222 double segmentPlanesSAC(PlanePatches& extractedPlanes, PlanePatchVector& pointMapping, const PointCloudPtr& pointCloud);
00223 double segmentCylinders(CylinderPatches& cylinderPatches, CylinderPatchVector& pointMapping, OcTree& octree, const PointCloudPtr& pointCloud);
00224 double segmentCylindersSAC(CylinderPatches& cylinderPatches, CylinderPatchVector& pointMapping, OcTree& octree, const PointCloudPtr& pointCloud);
00225 void generateTextures(const PlanePatches& extractedPlanes, const PointCloud& pointCloud);
00226 void callPublisher(const OcTree& octree, PlanePatches& extractedPlanes, const CylinderPatches& extractedCylinders , const PointCloudPtr pointCloud,
00227 const PlanePatchVector& pointMapping, const CylinderPatchVector& cylinderPointMapping);
00228
00229 void filterConnectedNodes(std::vector<NodePointers>& CCInlierNodes, const Plane3D& pp, const NodePointers& octreeNodes,
00230 float cellSize, GridMap* oldGridMap = NULL, NodePointers* notConnectedNodesOutput = NULL);
00231
00232 void estimatePlane(const pclPointIndices::Ptr& inliers, PlanePatchPtr& planePatch, const PointCloudPtr& pointCloud,
00233 const float& sacDist);
00234 void estimateCylinder(pclPointIndices& inliers, CylinderPatchPtr& cylinder, unsigned int& numPointsBefRansac, const NodePointers& octreeNodes,
00235 const OcTree& octree, const PointCloudPtr& pointCloud, const float& sacDist,
00236 const float& normalDistanceWeight, const float& minRadius, const float& maxRadius);
00237 void estimateCylinderNNNormals(CylinderPatchPtr& cylinder,
00238 const PointCloudPtr& pointCloud, const float& sacDist,
00239 const float& normalDistanceWeight, const float& minRadius,
00240 const float& maxRadius, float searchRadius, const Vec3& initialAxis);
00241 void estimateCylinderFromNodes(NodeIndices& inliers, CylinderPatchPtr& cylinder, const NodePointers& octreeNodes,
00242 const OcTree& octree, const PointCloudPtr& pointCloud, const float& sacDist, const float& normalDistanceWeight,
00243 const float& minRadius, const float& maxRadius);
00244 void generateSingleSphereOctreeHTBins(SphereUniformSamplings& houghBins, SphereBinsVec& pointNeighborBins,
00245 const NodePointers& octreeNodes);
00246 void estimatePlaneSingleSphereOctreeHT(pclPointIndices& inliers, Plane3DPtr& planePatch,
00247 const SphereUniformSamplings& houghBins, const SphereBinsVec& pointNeighborBins, const NodePointers& octreeNodes,
00248 NodeIndices& inlierNodes, const PointCloud& pointCloud, const OcTree& octree, const unsigned int& depth,
00249 const PlanePatchVector& pointMapping);
00250 void updateHTBins(const NodeIndices& inliers, SphereUniformSamplings& houghBins, SphereBinsVec& pointNeighborBins);
00251
00259 void generateNeighboringNodePairs(NodePairs& nodePairs, NodePointers& octreeNodes, const unsigned int neighborhoodSize,
00260 const unsigned int& currentOctreeDepth, const OcTree& octree);
00261
00267 void generateCylinderOrientationHB(SphereUniformSampling& cylinderOrientationBins, SphereBinsVec& binNeighborhood,
00268 const NodePairs& nodePairs);
00269
00278 void estimateCylinderHT(const SphereUniformSampling& cylinderOrientationBins, const NodePairs& nodePairs,
00279 PairIndices& inlierIndices, CylinderPatchPtr& cylinder, const unsigned int& depth, const OcTree& octree,
00280 const CylinderPatchVector& pointMapping, const PointCloud& pointCloud, const float& minRadius, const float& maxRadius);
00281
00287 void updateCylinderHTBins(SphereUniformSampling& cylinderOrientationBins, const PairIndices& inlierIndices,
00288 SphereBinsVec& binNeighborhood);
00289
00290 template<typename StructurePtrT>
00291 void updatePointMappingAndInliers(std::vector<StructurePtrT>& pointMapping, PointIndices& pointIndices, const StructurePtrT& structurePointer);
00292
00293 template<typename StructurePtrT>
00294 void updatePointMapping(std::vector<StructurePtrT>& pointMapping, const PointIndices& pointIndices, const StructurePtrT& structurePointer);
00295
00296 template<typename StructurePtrT, typename Structures>
00297 void rebuildPointMapping(std::vector<StructurePtrT>& pointMapping, const Structures& structures);
00298
00305 template<typename PointMappingT>
00306 void getPointsFromNodes(PointIndices& pointIndices, const NodePointers& octreeNodes,
00307 const OcTree& octree, const PointCloud& pointCloud, const PointMappingT& pointMapping);
00314 void getAllPointsFromNodes(PointIndices& pointIndices, const NodePointers& octreeNodes, const OcTree& octree,
00315 const PointCloud& pointCloud);
00316
00324 void getAllPointsFromNodes(PointIndices& pointIndices, const NodeIndices& nodeIndices, const NodePointers& octreeNodes,
00325 const OcTree& octree, const PointCloud& pointCloud);
00326
00331 void addNodesToPlane(PlanePatchPtr& plane, const NodePointers& nodes, const OcTree& octree,
00332 const PointCloud& pointCloud, PlanePatchVector& pointMapping);
00333
00339 void markNodesAsSegmented(const PlanePatch& pp, const NodePointers& nodes, const OcTree& octree,
00340 const PointCloud& pointCloud, const float& maxDistance, const float& ratio);
00341
00351 void assignNodesToPlanes(NodePointers& notAssignedOctreeNodes, const unsigned int& octreeDepth,
00352 const OcTree& octree, PlanePatches& extractedPlanes, const PointCloud& pointCloud, PlanePatchVector& pointMapping);
00353
00361 void extractOctreePlanesFromCloud(PlanePatches& extractedPlanes, PlanePatchVector& pointMapping,
00362 const OcTree& octree, const PointCloudPtr& pointCloud);
00363
00368 void assignNodesToCylinders(NodePointers& octreeNodes, CylinderPatches& extractedCylinders,
00369 CylinderPatchVector& pointMapping, const OcTree& octree, const PointCloud& pointCloud);
00370
00378 void extractOctreeCylindersFromCloud(CylinderPatches& extractedCylinders, CylinderPatchVector& pointMapping,
00379 const OcTree& octree, const PointCloudPtr& pointCloud);
00380
00381 void getNodesFromPairs(NodePointers& nodePointers, const PairIndices& pairIndices, const NodePairs& nodePairs);
00382
00389 void spreadNodes(const OcTree& octree, PlanePatches& extractedPlanes, PlanePatchVector& pointMapping,
00390 const PointCloud& pointCloud);
00391 void spreadPoints();
00392 void splitIntoConnectedCompononents();
00393 void mergePlanes(PlanePatches& extractedPlanes, PlanePatchVector& pointMapping, const PointCloud& pointCloud);
00394
00400 bool planesAreConnected(const PlanePatch& pp1, const PlanePatch& pp2, const PointCloud& pointCloud);
00401 void addPlanePatchesAndUpdateSegmented(PlanePatches& extractedPlanes, PlanePatchVector& pointMapping,
00402 const PlanePatches& planePatches);
00403
00404 template<typename PointMappingT, typename StructureContainerT>
00405 void writeSegments(const unsigned int& width, const unsigned int& height,
00406 const std::vector<unsigned int>& undefPoints, const PointMappingT& pointMapping, const std::string& outFilename,
00407 const StructureContainerT& extractedStructures);
00408
00415 float getSACDistanceFromDepth(const unsigned int& depth, const OcTree& octree);
00416
00423 float getHTDistanceFromDepth(const unsigned int& depth, const OcTree& octree);
00424
00425 void getMinMaxRadiusFromDepth(float& minRadius, float& maxRadius, const OcTree& octree, const float& depth);
00426
00433 float getAngleEpsFromDepth(const unsigned int& depth, const OcTree& octree);
00434
00441 void determineCandidatePlanes(PatchToNodeIndicesMap& outIndicesMap, PatchToNodePointersMap& outPointersMap,
00442 const NodePointers& octreeNodes, PlanePatches& extractedPlanes);
00443 void determineCandidatePlanes(NodeIndexToPlanePatchPointers& nodeToPlaneCandidates, const NodePointers& octreeNodes,
00444 PlanePatches& extractedPlanes);
00445
00455 void refineAssignmentWithCCs(NodeIndexToPlanePatchPointers& nodeToPlaneCandidates, const PatchToNodeIndicesMap& nodeIndicesMap,
00456 const PatchToNodePointersMap& nodePointersMap, const NodePointers& octreeNodes, PlanePatches& extractedPlanes);
00457
00463 void calculateMiddlePlane(Plane3D& outParams, const PlanePatch& firstPPP, const PlanePatch& secondPPP);
00464
00472 void refineEdges(PatchToNodePointersMap& finalPlaneNodeAssignmentMap, PatchToPointIndicesMap& finalPlanePointAssignment,
00473 const NodeIndexToPlanePatchPointers& nodeToPlaneCandidates, const NodePointers& octreeNodes, const OcTree& octree,
00474 const PointCloud& pointCloud);
00475
00476 bool checkCylinder(const CylinderPatchPtr& cylinder, const PointCloudPtr& pointCloud) const;
00477 bool checkCylinderDimensions(const CylinderPatchPtr& cylinder) const;
00478
00479 ros::NodeHandle* mNodeHandle;
00480 std::string mKinectTopic;
00481 ros::Subscriber mPointCloud2Subscriber;
00482 ros::Subscriber mPointCloudSubscriber;
00483
00484 PointCloudPtr mPointCloud;
00485 PlanePatchVector mPointMapping;
00486 QMutex mPointCloudMutex;
00487 PlanePatches mPlanePatches;
00488 QMutex mPlanePatchesMutex;
00489 CylinderPatches mCylinderPatches;
00490 QMutex mCylinderPatchesMutex;
00491
00492 typedef boost::shared_ptr<OcTree> OcTreePtr;
00493 OcTreePtr mOcTree;
00494
00495 RosVisualization* mVis;
00496
00497 SphereBinsVec mPointNeighborBins;
00498
00499 StrColParams mParams;
00500 float mLastCellSizeWithNormals;
00501 };
00502
00503
00504
00505 inline StructureColoring::NodePointers StructureColoring::getNodesFromIndices(const NodeIndices& nodeIndices, const NodePointers& nodes){
00506 NodePointers outNodes;
00507 for(NodeIndices::const_iterator nit = nodeIndices.begin(); nit != nodeIndices.end(); ++nit)
00508 outNodes.push_back(nodes[*nit]);
00509 return outNodes;
00510 }
00511
00512
00513
00514 inline StructureColoring::Points StructureColoring::getPointsFromIndices(const PointIndices& pointIndices, const PointCloud& cloud){
00515 Points outPoints;
00516 for(PointIndices::const_iterator pit = pointIndices.begin(); pit != pointIndices.end(); ++pit)
00517 outPoints.push_back(cloud.points[*pit].getVector3fMap());
00518 return outPoints;
00519 }
00520
00521
00522
00523 inline void StructureColoring::markAsSegmented(NodePtr currentNode, NodePtr nextNode, void* data){
00524 currentNode->value_.segmented = true;
00525 (void)nextNode;
00526 (void)data;
00527 }
00528
00529
00530
00531 template<typename PointMappingT>
00532 void StructureColoring::getPointsFromNodes(PointIndices& pointIndices, const NodePointers& octreeNodes,
00533 const OcTree& octree, const PointCloud& pointCloud, const PointMappingT& pointMapping) {
00534 pointIndices.clear();
00535 pointIndices.reserve(pointCloud.points.size());
00536 for (NodePointers::const_iterator node_it = octreeNodes.begin(); node_it != octreeNodes.end(); node_it++) {
00537 NodePointers leaf_nodes;
00538 octree.getAllLeafs(leaf_nodes, (*node_it));
00539 for (NodePointers::const_iterator leaf_it = leaf_nodes.begin(); leaf_it != leaf_nodes.end(); leaf_it++) {
00540 const PointIndices& points = octree.getPointsFromNodePtr(*leaf_it);
00541 for (std::vector<int>::const_iterator point_it = points.begin(); point_it != points.end(); point_it++) {
00542 if (!pointMapping[*point_it])
00543 pointIndices.push_back(*point_it);
00544 }
00545 }
00546 }
00547 }
00548
00549
00550
00551 template<typename PointMappingT>
00552 void StructureColoring::initSegmentation(PointMappingT& pointMapping, const PointCloud& pointCloud) {
00553 pointMapping.resize(pointCloud.points.size());
00554 }
00555
00556
00557
00558 template<typename PointMappingT, typename StructureContainerT>
00559 void StructureColoring::writeSegments(const unsigned int& width, const unsigned int& height,
00560 const std::vector<unsigned int>& undefPoints, const PointMappingT& pointMapping, const std::string& outFilename,
00561 const StructureContainerT& extractedStructures) {
00562 size_t id = 10;
00563 for (typename StructureContainerT::const_iterator it = extractedStructures.begin(); it != extractedStructures.end(); ++it) {
00564 if ((*it)->getInliers().empty())
00565 ROS_ERROR("writeSegments() empty structure!");
00566 else {
00567 (*it)->setId(id++);
00568 }
00569 }
00570 std::vector<unsigned char> outData;
00571 outData.resize(width * height, 0);
00572 unsigned int up_counter = 0;
00573 for (unsigned int j = 0; j < height; j++) {
00574 for (unsigned int i = 0; i < width; i++) {
00575 unsigned int pos = j * width + i;
00576 unsigned int outDataPos = pos + up_counter;
00577 while ((up_counter < undefPoints.size()) && (outDataPos == undefPoints[up_counter])) {
00578 up_counter++;
00579 outDataPos++;
00580 }
00581 if ((pos < pointMapping.size()) && (pointMapping[pos])) {
00582 outData[outDataPos] = pointMapping[pos]->getId();
00583 }
00584 }
00585 }
00586 if (mParams.mPGM)
00587 writePGM(outFilename, width, height, outData);
00588 else
00589 writeRasterfile(outFilename, width, height, outData);
00590 }
00591
00592
00593
00594 template<typename StructurePtrT>
00595 void StructureColoring::updatePointMappingAndInliers(std::vector<StructurePtrT>& pointMapping, PointIndices& pointIndices, const StructurePtrT& structurePointer){
00596 if (!structurePointer)return;
00597 PointIndices indicesToAdd;
00598 indicesToAdd.reserve(pointIndices.size());
00599 for(PointIndices::const_iterator pit = pointIndices.begin(); pit != pointIndices.end(); ++pit){
00600 assert((size_t)*pit < pointMapping.size());
00601 if (pointMapping[*pit] != structurePointer){
00602 pointMapping[*pit] = structurePointer;
00603 indicesToAdd.push_back(*pit);
00604 }
00605 }
00606 pointIndices.swap(indicesToAdd);
00607 }
00608
00609
00610
00611 template<typename StructurePtrT>
00612 void StructureColoring::updatePointMapping(std::vector<StructurePtrT>& pointMapping, const PointIndices& pointIndices, const StructurePtrT& structurePointer){
00613 if (!structurePointer) return;
00614
00615 #pragma omp parallel for schedule(dynamic,1)
00616 for(size_t i = 0; i < pointIndices.size(); ++i){
00617 assert((size_t)pointIndices[i] < pointMapping.size());
00618 pointMapping[pointIndices[i]] = structurePointer;
00619 }
00620 }
00621
00622
00623
00624 template<typename StructurePtrT, typename Structures>
00625 void StructureColoring::rebuildPointMapping(std::vector<StructurePtrT>& pointMapping, const Structures& structures)
00626 {
00627 for (size_t i = 0; i < pointMapping.size(); ++i)
00628 pointMapping[i].reset();
00629 for (typename Structures::const_iterator it = structures.begin();
00630 it != structures.end(); ++it)
00631 {
00632 updatePointMapping(pointMapping, (*it)->getInliers(), *it);
00633 }
00634 }
00635
00636
00637 #endif