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 #include <structureColoring/RoSeConnector/StrColRoSeService.h>
00037
00038 #include <algorithm>
00039 #include <structureColoring/StrColParams.h>
00040
00041
00042
00043 RoSe::Service::Ptr StrColRoSeService::create(const RoSe::SID &sid) {
00044 return new StrColRoSeService(sid);
00045 }
00046
00047
00048
00049 StrColRoSeService::StrColRoSeService(const RoSe::SID &sid) :
00050 RoSe::Service(sid, "StructureColoring_RoSeService"), mNH("~"), mCondition(mMutex) {
00051 }
00052
00053
00054
00055 StrColRoSeService::~StrColRoSeService() {
00056 dispose();
00057 }
00058
00059
00060
00061 void StrColRoSeService::terminate() {}
00062
00063
00064
00065 void StrColRoSeService::finish() {
00066 RoSe::Mutex::AutoLocker lock(mMutex);
00067 mCondition.signal();
00068 mNH.shutdown();
00069 }
00070
00071
00072
00073 void StrColRoSeService::execute() {
00074 mBBThread.reset(new boost::thread(boost::bind(&StrColRoSeService::blackboardReadRun, this)));
00075 Planes detectedPlanes;
00076 Cylinders detectedCylinders;
00077 while (isRunning()&&ros::ok()) {
00078 RoSe::Mutex::AutoLocker lock(mMutex);
00079 mCondition.wait();
00080 if (mLastPointCloud && isRunning() && ros::ok()) {
00081 detectedPlanes.clear();
00082 detectedCylinders.clear();
00083
00084 PlanePtrs pointMapping(mLastPointCloud->points.size());
00085 CylinderPtrs cylinderMapping;
00086 mStrCol.doSegmentation(mLastPointCloud, pointMapping, detectedPlanes, cylinderMapping, detectedCylinders);
00087
00088 RosePointcloudPtr unsegmentedPoints(new RosePointcloud);
00089 extractUnmappedPointsToRosePC(unsegmentedPoints, mLastPointCloud, pointMapping);
00090 sendPoints(unsegmentedPoints);
00091 std::cout << unsegmentedPoints->points().size() << " points were not segmented and sent back" << std::endl;
00092
00093 sendPlanes(detectedPlanes);
00094 mLastPointCloud.reset();
00095 }
00096 }
00097 mBBThread->join();
00098 }
00099
00100
00101
00102 void StrColRoSeService::initialize() {
00103
00104 #ifndef NDEBUG
00105 cv::setBreakOnError(true);
00106 #endif
00107
00108
00109 const RoSe::ConfigService& cfg = getConfiguration().getService(getID());
00110 std::cout << "own SID is: " << getID() << std::endl;
00111 mPointcloudBlackboardSenderSID = cfg.getParamSID("BlackboardSID");
00112 mSegmentedPlanesAndPointsRecieverSID = cfg.getParamSID("SegmentedPlanesAndPointsRecieverSID");
00113
00114 StrColParams params;
00115
00116
00117 params.mAngleEps = cfg.getParamFloat("AngleEps");
00118 params.mAngleEpsOnMinOctreeRes = cfg.getParamFloat("AngleEpsOnMinOctreeRes");
00119 params.mMinPointsInCC = cfg.getParamInteger("MinPointsInCC");
00120 params.mMinNodesInCC = cfg.getParamInteger("MinNodesInCC");
00121 params.mTriesOnEachDepth = cfg.getParamInteger("TriesOnEachDepth");
00122
00123 params.mMaxHTDistanceThreshold = cfg.getParamFloat("MaxHTDistanceThreshold");
00124 params.mHTOctreeBinSizeFactor = cfg.getParamFloat("HTOctreeBinSizeFactor");
00125 params.mHTDistanceDeviationFactor = cfg.getParamFloat("HTDistanceDeviationFactor");
00126
00127 params.mPhi_resolution = cfg.getParamInteger("PhiResolution");
00128
00129 params.mMaxSACDistanceThreshold = cfg.getParamFloat("MaxSACDistanceThreshold");
00130 params.mMinSACDistanceThreshold = cfg.getParamFloat("MinSACDistanceThreshold");
00131 params.mSACOctreeFactor = cfg.getParamFloat("SACOctreeFactor");
00132 params.mSACOutlierRatioThreshold = cfg.getParamFloat("SACOutlierRatioThreshold");
00133
00134 params.mMinPointsForNormal = cfg.getParamInteger("MinPointsForNormal");
00135 params.mCurvThreshold = cfg.getParamFloat("CurvThreshold");
00136 params.mCurv2Threshold = cfg.getParamFloat("Curv2Threshold");
00137 params.mPrincipalVarFactor = cfg.getParamFloat("PrincipalVarFactor");
00138 params.mMinOctreeNodes = cfg.getParamInteger("MinOctreeNodes");
00139 params.mMinOctreeResolution = cfg.getParamFloat("MinOctreeResolution");
00140 params.mSqDistFactor = cfg.getParamFloat("SqDistFactor");
00141
00142 params.mTexelSizeFactor = cfg.getParamFloat("TexelSizeFactor");
00143 params.mDilateIterations = cfg.getParamInteger("DilateIterations");
00144
00145 params.mDebugSteps = cfg.getParamInteger("DebugSteps");
00146 params.mPclSACmaxIter = cfg.getParamInteger("mPclSACmaxIter");
00147 params.mOnlyDepth = cfg.getParamInteger("mOnlyDepth");
00148
00149
00150 params.mPostProcessing = cfg.getParamInteger("mPostProcessing");
00151 params.mMergePlanesSimilarityFactor = cfg.getParamFloat("MergePlanesSimilarityFactor");
00152 params.mNodeToBBDistance = cfg.getParamFloat("NodeToBBDistance");
00153 params.mConnectionNeighbors = cfg.getParamInteger("ConnectionNeighbors");
00154 params.mRho_max = cfg.getParamFloat("RhoMax");
00155 params.mNodeSegmentedRatio = cfg.getParamFloat("NodeSegmentedRatio");
00156
00157
00158 params.mMinNodesInCylinder = cfg.getParamInteger("MinNodesInCylinder");
00159 params.mNormalDistanceWeight = cfg.getParamFloat("NormalDistanceWeight");
00160 params.mMinRadiusFactor = cfg.getParamFloat("MinRadiusFactor");
00161 params.mMaxRadiusFactor = cfg.getParamFloat("MaxRadiusFactor");
00162 params.mMinCylinderRadius = cfg.getParamFloat("MinCylinderRadius");
00163 params.mMaxCylinderRadius = cfg.getParamFloat("MaxCylinderRadius");
00164 params.mCylinderBins = cfg.getParamInteger("CylinderBins");
00165 params.mCylinderPairNeighbors = cfg.getParamInteger("CylinderPairNeighbors");
00166 params.mMidPointBinSizeFactor = cfg.getParamFloat("MidPointBinSizeFactor");
00167 params.mRadiusDevFactor = cfg.getParamFloat("RadiusDevFactor");
00168 params.mOccupiedRatio = cfg.getParamFloat("OccupiedRatio");
00169 params.mCylinderHeightDev = cfg.getParamFloat("CylinderHeightDev");
00170
00171
00172 params.mVerbose = cfg.getParamBoolean("Verbose");
00173 params.mKinect = cfg.getParamBoolean("Kinect");
00174 params.mLaser = cfg.getParamBoolean("Laser");
00175 params.mTextures = cfg.getParamBoolean("Textures");
00176 params.mCylinder = cfg.getParamBoolean("Cylinder");
00177 params.mPclSAC = cfg.getParamBoolean("PclSAC");
00178 params.mNoRansacStep = cfg.getParamBoolean("NoRansacStep");
00179 params.mLoadPCD = cfg.getParamBoolean("LoadPCD");
00180 params.mPCDnoRGB = cfg.getParamBoolean("LoadPCDnoRGB");
00181 params.mWriteRawPic = cfg.getParamBoolean("WriteRawPic");
00182 params.mRawPicFilename = cfg.getParamString("RawPicFilename");
00183 params.mRawPicCounter = cfg.getParamInteger("RawPicCounter");
00184 params.mRuntimeFilename = cfg.getParamString("RunTimeFilename");
00185
00186 mStrCol.setParams(params);
00187 }
00188
00189
00190
00191 void StrColRoSeService::receiveMsg(const RoSe::SID&, const RoSe::Message::Ptr) {
00192 }
00193
00194
00195
00196 void StrColRoSeService::extractUnmappedPointsToRosePC(RosePointcloudPtr unsegmentedPoints,
00197 PointCloudPtr pointCloud, PlanePtrs pointMapping) {
00198 union {
00199 float rgbfloat;
00200 uint8_t rgb[4];
00201 } ColorUnion;
00202 float origin3f[3] = {0.f, 0.f, 0.f};
00203 Point3fc origin(origin3f);
00204 origin.userdata() = RoSe::Color8(0, 0, 0, 0);
00205 unsegmentedPoints->origin() = origin;
00206 unsegmentedPoints->points().reserve(pointMapping.size());
00207 for (unsigned int i = 0; i < pointMapping.size(); ++i) {
00208
00209 if (pointMapping[i] != 0)
00210 continue;
00211
00212 Point3fc p;
00213 p[0] = pointCloud->points[i].x;
00214 p[1] = pointCloud->points[i].y;
00215 p[2] = pointCloud->points[i].z;
00216
00217 ColorUnion.rgbfloat = pointCloud->points[i].rgb;
00218 RoSe::Color8 color(ColorUnion.rgb[0], ColorUnion.rgb[1], ColorUnion.rgb[2], ColorUnion.rgb[3]);
00219 p.userdata() = color;
00220
00221 unsegmentedPoints->points().push_back(p);
00222 }
00223 }
00224
00225
00226
00227 void StrColRoSeService::encodeHeightmap(std::vector<unsigned char>& encodedHeightmap, const cv::Mat& heightmap,
00228 float widthRatio, float heightRatio){
00229 unsigned int smallWidth = (float) heightmap.cols * (float) widthRatio;
00230 unsigned int smallHeight = (float) heightmap.rows * (float) heightRatio;
00231 cv::Mat tempMat(heightmap, cv::Rect(0, 0, smallWidth, smallHeight));
00232 cv::Mat img;
00233 tempMat.convertTo(img, CV_8U, 255);
00234 std::vector<int> params(2);
00235 params[0] = CV_IMWRITE_PNG_COMPRESSION;
00236 params[1] = 9;
00237 cv::imencode(".png", img, encodedHeightmap, params);
00238 }
00239
00240
00241
00242 void StrColRoSeService::generateAlphamap(std::vector<bool>& alphamap, const cv::Mat& tempMat){
00243 alphamap.resize(tempMat.rows * tempMat.cols, false);
00244 for(int r = 0; r < tempMat.rows; ++r){
00245 for(int c = 0; c < tempMat.cols; ++c){
00246 if (tempMat.at<cv::Vec4f>(r, c)[3] > 0.5f)
00247 alphamap[r * tempMat.cols + c] = true;
00248 }
00249 }
00250 }
00251
00252
00253
00254 void StrColRoSeService::encodeTexturemap(std::vector<unsigned char>& encodedTexturemap,
00255 std::vector<bool>& alphamap, const cv::Mat& texturemap, float widthRatio, float heightRatio){
00256 unsigned int smallWidth = (float) texturemap.cols * (float) widthRatio;
00257 unsigned int smallHeight = (float) texturemap.rows * (float) heightRatio;
00258 cv::Mat tempMat(texturemap, cv::Rect(0, 0, smallWidth, smallHeight));
00259 cv::Mat img;
00260 tempMat.convertTo(img, CV_8U, 255);
00261 std::vector<int> params(2);
00262 params[0] = CV_IMWRITE_PNG_COMPRESSION;
00263 params[1] = 9;
00264 cv::imencode(".png", img, encodedTexturemap, params);
00265 generateAlphamap(alphamap, tempMat);
00266 }
00267
00268
00269
00270 void StrColRoSeService::sendPlanes(const Planes& planes) {
00271 for(Planes::const_iterator pit = planes.begin(); pit != planes.end(); ++pit){
00272 Point3f normal;
00273 convertPoint(normal, (*pit)->getPlane3D().getPlaneNormal());
00274 Points3f brVertices;
00275 convertPoints(brVertices, (*pit)->getBRVertices());
00276 std::vector<uchar> encodedHeightmap, encodedTexturemap;
00277 std::vector<bool> alphamap;
00278 float widthRatio = (*pit)->getTextureWidthRatio();
00279 float heightRatio = (*pit)->getTextureHeightRatio();
00280 encodeHeightmap(encodedHeightmap, (*pit)->getHeightMap(), widthRatio, heightRatio);
00281 encodeTexturemap(encodedTexturemap, alphamap, (*pit)->getTextureMap(), widthRatio, heightRatio);
00282 RoSe::TexturedPlaneSegment planeForMsg(normal, (*pit)->getPlane3D().getPlaneDistanceToOrigin(),
00283 (*pit)->getDistanceThreshold(), brVertices,
00284 (*pit)->getHeightMap().type(), encodedHeightmap,
00285 (*pit)->getTextureMap().type(), encodedTexturemap,
00286 alphamap);
00287 RoSe::RefPtr<RoSe::MsgTexturedPlaneSegment> Msg(new RoSe::MsgTexturedPlaneSegment(planeForMsg));
00288 RoSe::Message::Ptr msg(Msg);
00289 comm().sendTo(mSegmentedPlanesAndPointsRecieverSID, msg);
00290 }
00291 std::cout << "Textured Planes sent to " << mSegmentedPlanesAndPointsRecieverSID << "." << std::endl;
00292 }
00293
00294
00295
00296 void StrColRoSeService::sendPoints(RosePointcloudPtr pointCloud) {
00297 unsigned int stepsize = 100;
00298 const std::vector<Point3fc> points(pointCloud->points());
00299 RoSe::Quaternion orientation = pointCloud->orientation();
00300 Point3fc origin = pointCloud->origin();
00301 RoSePointCloudPoints::const_iterator start = pointCloud->points().begin();
00302 RoSePointCloudPoints::const_iterator stop = pointCloud->points().begin();
00303 unsigned int finished = 0;
00304 while(finished < points.size()){
00305 RosePointcloudPtr tmpPC(new RosePointcloud());
00306 tmpPC->orientation() = orientation;
00307 tmpPC->origin() = origin;
00308 if ((int)stepsize > (int)points.size() - (int)finished){
00309 stepsize = (int)points.size() - (int)finished;
00310 }
00311 stop += stepsize;
00312 finished += stepsize;
00313 std::copy(start, stop, std::back_inserter(tmpPC->points()));
00314 start += stepsize;
00315 RoSe::Message::Ptr msg = new RoSe::MsgPointCloud3fc(*tmpPC);
00316 comm().sendTo(mSegmentedPlanesAndPointsRecieverSID, msg);
00317 }
00318 }
00319
00320
00321
00322 bool StrColRoSeService::connectBlackboard()
00323 {
00324 try
00325 {
00326 mPCBlackboard = RoSe::Blackboard::fromSID(mPointcloudBlackboardSenderSID, false);
00327 std::cout << "Blackboard connected to: " << mPointcloudBlackboardSenderSID << "." << std::endl;
00328 return true;
00329 } catch (RoSe::IPCError &e) {
00330 ;
00331 }
00332 return false;
00333 }
00334
00335
00336
00337 void StrColRoSeService::readBlackboard()
00338 {
00339 union{
00340 float rgbfloat;
00341 uint8_t rgba[4];
00342 }colorUnion;
00343 if (mPCBlackboard.hasData())
00344 {
00345 RoSe::Blackboard::Reader reader = mPCBlackboard.getReader();
00346 RoSe::Message::Ptr msg = reader.getMessage();
00347 reader.finish();
00348 if (RoSe::Message::isType<RoSe::MsgPointCloud3fc>(msg))
00349 {
00350 RoSe::MsgPointCloud3fc &msgPC = msg->cast<RoSe::MsgPointCloud3fc>();
00351 RosePointcloud pointCloud = msgPC.getPayload();
00352 std::cout << "received " << pointCloud.points().size() << " points.\n";
00353
00354 RoSe::Mutex::AutoLocker lock(mMutex);
00355 mLastPointCloud.reset(new PointCloud());
00356 mLastPointCloud->points.reserve(pointCloud.points().size());
00357 mLastPointCloud->width = pointCloud.points().size();
00358 mLastPointCloud->height = 1;
00359 for (size_t i = 0; i < pointCloud.points().size(); ++i)
00360 {
00361 const Point3fc& p = pointCloud.points()[i];
00362 colorUnion.rgba[0] = p.userdata().red();
00363 colorUnion.rgba[1] = p.userdata().green();
00364 colorUnion.rgba[2] = p.userdata().blue();
00365 colorUnion.rgba[3] = p.userdata().alpha();
00366 PointT newP;
00367 newP.x = p[0];
00368 newP.y = p[1];
00369 newP.z = p[2];
00370 newP.rgb = colorUnion.rgbfloat;
00371 mLastPointCloud->points.push_back(newP);
00372 }
00373 std::cout << "\nNew Pointcloud recieved with " << mLastPointCloud->points.size() << " points.\n";
00374 mCondition.signal();
00375 }
00376 }
00377 }
00378
00379
00380
00381 void StrColRoSeService::blackboardReadRun()
00382 {
00383 while (isRunning()&&ros::ok())
00384 {
00385 if (mPCBlackboard.isValid())
00386 {
00387 try
00388 {
00389 mPCBlackboard.wait(100);
00390 this->readBlackboard();
00391 }
00392 catch (RoSe::TimeoutError e)
00393 { ; }
00394 }
00395 else
00396 {
00397 if (!this->connectBlackboard()) std::cout << "Could not connect to blackboard with SID: "<< mPointcloudBlackboardSenderSID << ".\n";
00398 usleep(1000);
00399 }
00400 }
00401 RoSe::Mutex::AutoLocker lock(mMutex);
00402 mCondition.signal();
00403 }