void StrColRoSeService::execute() { mBBThread.reset(new boost::thread(boost::bind(&StrColRoSeService::blackboardReadRun, this))); Planes detectedPlanes; Cylinders detectedCylinders; while (isRunning()&&ros::ok()) { RoSe::Mutex::AutoLocker lock(mMutex); mCondition.wait(); if (mLastPointCloud && isRunning() && ros::ok()) { detectedPlanes.clear(); detectedCylinders.clear(); //segment pointCloud mLastPointCloud PlanePtrs pointMapping(mLastPointCloud->points.size()); CylinderPtrs cylinderMapping; mStrCol.doSegmentation(mLastPointCloud, pointMapping, detectedPlanes, cylinderMapping, detectedCylinders); //send unsegmented points RosePointcloudPtr unsegmentedPoints(new RosePointcloud); extractUnmappedPointsToRosePC(unsegmentedPoints, mLastPointCloud, pointMapping); sendPoints(unsegmentedPoints); std::cout << unsegmentedPoints->points().size() << " points were not segmented and sent back" << std::endl; //send detected planes sendPlanes(detectedPlanes); mLastPointCloud.reset(); } } mBBThread->join(); }
~Impl() { // Ensure planes are cleared first (subsectors may include mappings). planes.clear(); delete [] self()._lookupPlanes; }
Planes ConvexHull_3D(Points &P) { Planes CH; int n = P.size(), i, j, k, l; for (i = 0; i < n; i++) for (j = i+1; j < n; j++) for (k = i+1; k < n; k++) { Plane pl = {P[i], P[j], P[k]}; Point p = pl.normal(); for (l = 0; l < n; l++) if (l != i && l != j && l != k && ! pl.same_side(p, P[l])) break; if (l == n) CH.push_back(pl); } return CH; }
void updatePlanesLookup() { delete [] self()._lookupPlanes; self()._lookupPlanes = new Plane *[planes.size()]; Plane **ptr = self()._lookupPlanes; for (Plane *p : planes) { *ptr++ = p; } }
void FrustumGrid::generateGridPlanes(Planes& xPlanes, Planes& yPlanes, Planes& zPlanes) { xPlanes.resize(dims.x + 1); yPlanes.resize(dims.y + 1); zPlanes.resize(dims.z + 1); float centerY = float(dims.y) * 0.5f; float centerX = float(dims.x) * 0.5f; for (int z = 0; z < (int) zPlanes.size(); z++) { ivec3 pos(0, 0, z); zPlanes[z] = glm::vec4(0.0f, 0.0f, 1.0f, -frustumGrid_clusterPosToEye(pos, vec3(0.0)).z); } for (int x = 0; x < (int) xPlanes.size(); x++) { auto slicePos = frustumGrid_clusterPosToEye(glm::vec3((float)x, centerY, 0.0)); auto sliceDir = glm::normalize(slicePos); xPlanes[x] = glm::vec4(sliceDir.z, 0.0, -sliceDir.x, 0.0); } for (int y = 0; y < (int) yPlanes.size(); y++) { auto slicePos = frustumGrid_clusterPosToEye(glm::vec3(centerX, (float)y, 0.0)); auto sliceDir = glm::normalize(slicePos); yPlanes[y] = glm::vec4(0.0, sliceDir.z, -sliceDir.y, 0.0); } }
void StrColRoSeService::sendPlanes(const Planes& planes) { for(Planes::const_iterator pit = planes.begin(); pit != planes.end(); ++pit){ Point3f normal; convertPoint(normal, (*pit)->getPlane3D().getPlaneNormal()); Points3f brVertices; convertPoints(brVertices, (*pit)->getBRVertices()); std::vector<uchar> encodedHeightmap, encodedTexturemap; std::vector<bool> alphamap; float widthRatio = (*pit)->getTextureWidthRatio(); float heightRatio = (*pit)->getTextureHeightRatio(); encodeHeightmap(encodedHeightmap, (*pit)->getHeightMap(), widthRatio, heightRatio); encodeTexturemap(encodedTexturemap, alphamap, (*pit)->getTextureMap(), widthRatio, heightRatio); RoSe::TexturedPlaneSegment planeForMsg(normal, (*pit)->getPlane3D().getPlaneDistanceToOrigin(), (*pit)->getDistanceThreshold(), brVertices, (*pit)->getHeightMap().type(), encodedHeightmap, (*pit)->getTextureMap().type(), encodedTexturemap, alphamap); RoSe::RefPtr<RoSe::MsgTexturedPlaneSegment> Msg(new RoSe::MsgTexturedPlaneSegment(planeForMsg)); RoSe::Message::Ptr msg(Msg); comm().sendTo(mSegmentedPlanesAndPointsRecieverSID, msg);//, RoSe::Message::TOS_ACK, true); } std::cout << "Textured Planes sent to " << mSegmentedPlanesAndPointsRecieverSID << "." << std::endl; }
void solve(Planes &CH) { // cout << CH.size() << endl; int m; // cin >> m; scanf("%d", &m); while (m--) { Point p; cin >> p.x >> p.y >> p.z; double ans = INF; for (int i = 0; i < CH.size(); ++ i) ans = min(ans, CH[i].dist2p(p)); // cout << ans << endl; printf("%.4lf\n", ans); } }