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);
}
}
