void
cloud_callback (const CloudConstPtr& cloud)
{
FPS_CALC ("cloud callback");
boost::mutex::scoped_lock lock (cloud_mutex_);
cloud_ = cloud;
search_.setInputCloud (cloud);
// Subsequent frames are segmented by "tracking" the parameters of the previous frame
// We do this by using the estimated inliers from previous frames in the current frame,
// and refining the coefficients
if (!first_frame_)
{
if (!plane_indices_ || plane_indices_->indices.empty () || !search_.getInputCloud ())
{
PCL_ERROR ("Lost tracking. Select the object again to continue.\n");
first_frame_ = true;
return;
}
SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_PLANE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (search_.getInputCloud ());
seg.setIndices (plane_indices_);
ModelCoefficients coefficients;
PointIndices inliers;
seg.segment (inliers, coefficients);
if (inliers.indices.empty ())
{
PCL_ERROR ("No planar model found. Select the object again to continue.\n");
first_frame_ = true;
return;
}
// Visualize the object in 3D...
CloudPtr plane_inliers (new Cloud);
pcl::copyPointCloud (*search_.getInputCloud (), inliers.indices, *plane_inliers);
if (plane_inliers->points.empty ())
{
PCL_ERROR ("No planar model found. Select the object again to continue.\n");
first_frame_ = true;
return;
}
else
{
plane_.reset (new Cloud);
// Compute the convex hull of the plane
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (plane_inliers);
chull.reconstruct (*plane_);
}
}
}
static void DrawHull(const ConvexHull& hull)
{
const Point* ConvexVerts = hull.GetVerts();
udword NbPolys = hull.GetNbPolygons();
// printf("NbPolys: %d\n", NbPolys);
for(udword i=0;i<NbPolys;i++)
{
const HullPolygon& PolygonData = hull.GetPolygon(i);
glNormal3f(PolygonData.mPlane.n.x, PolygonData.mPlane.n.y, PolygonData.mPlane.n.z);
const udword NbVertsInPoly = PolygonData.mNbVerts;
const udword NbTris = NbVertsInPoly - 2;
const udword* Indices = PolygonData.mVRef;
udword Offset = 1;
for(udword i=0;i<NbTris;i++)
{
const udword VRef0 = Indices[0];
const udword VRef1 = Indices[Offset];
const udword VRef2 = Indices[Offset+1];
Offset++;
const Point av3LineEndpoints[] = {ConvexVerts[VRef0], ConvexVerts[VRef1], ConvexVerts[VRef2]};
glEnableClientState(GL_VERTEX_ARRAY);
glVertexPointer(3, GL_FLOAT, sizeof(Point), &av3LineEndpoints[0].x);
glDrawArrays(GL_TRIANGLES, 0, 3);
glDisableClientState(GL_VERTEX_ARRAY);
}
}
}
void reconstructMesh(const PointCloud<PointXYZ>::ConstPtr &cloud,
PointCloud<PointXYZ> &output_cloud, std::vector<Vertices> &triangles)
{
boost::shared_ptr<std::vector<int> > indices(new std::vector<int>);
indices->resize(cloud->points.size ());
for (size_t i = 0; i < indices->size (); ++i) { (*indices)[i] = i; }
pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>);
tree->setInputCloud(cloud);
PointCloud<PointXYZ>::Ptr mls_points(new PointCloud<PointXYZ>);
PointCloud<PointNormal>::Ptr mls_normals(new PointCloud<PointNormal>);
MovingLeastSquares<PointXYZ, PointNormal> mls;
mls.setInputCloud(cloud);
mls.setIndices(indices);
mls.setPolynomialFit(true);
mls.setSearchMethod(tree);
mls.setSearchRadius(0.03);
mls.process(*mls_normals);
ConvexHull<PointXYZ> ch;
ch.setInputCloud(mls_points);
ch.reconstruct(output_cloud, triangles);
}
ConvexHull inset_ch(ConvexHull ch, double dist) {
ConvexHull ret;
for(unsigned i = 0; i < ch.size(); i++) {
Point bisect = (rot90(ch[i+1] - ch[i]));
ret.merge(unit_vector(bisect)*dist+ch[i]);
ret.merge(unit_vector(bisect)*dist+ch[i+1]);
}
return ret;
}
bool HeightmapSampling::calculateConvexHull()
{
ConvexHull < PointXYZ > cv;
cv.setInputCloud(point_cloud_);
boost::shared_ptr < PointCloud<PointXYZ> > cv_points;
cv_points.reset(new PointCloud<PointXYZ> ());
cv.reconstruct(*cv_points, convex_hull_vertices_);
convex_hull_points_ = cv_points;
return true;
}
void COpenCVInterfaceDlg::OnMorphologyConvexhull()
{
if(mainImage.cols)
{
ConvexHull c;
Mat rez=mainImage.clone();
mainImage=Filters::gaussianFilter(mainImage,1);
Scalar meanVal,stdval;
meanStdDev(mainImage,meanVal,stdval);
Canny(mainImage,mainImage,meanVal.val[0]*1/3.,meanVal.val[0]);
//imshow("contours",mainImage);
vector<std::vector<cv::Point>> contours;
findContours(mainImage,contours,CV_RETR_LIST,CV_CHAIN_APPROX_NONE,Point(2,2));
vector<Point> Points;
//Points.resize(contours.size()*contours[0].size());
for(int i=0;i<contours.size();i++)
{
for(int j=0;j<contours[i].size();j++)
{
Points.push_back(contours[i][j]);
}
}
//for(int i=2;i<mainImage.rows-2;i++)
//{
// for(int j=2;j<mainImage.cols-2;j++)
// {
// if(mainImage.at<uchar>(i,j)==255)
// Points.push_back(Point(i,j));
// }
//}
std::vector<Point> chull;
c.graham(Points, chull);
for(int i=0;i<chull.size()-1;i++)
{
line(rez,chull[i],chull[i+1],Scalar(255,255,255),2);
}
line(rez,chull[chull.size()-1],chull[0],Scalar(255,255,255),2);
prelImage=rez.clone();
ShowResult(prelImage);
}
else
MessageBox("No image loaded");
}
ConvexHull findConvexHull(PointSet *pointSet) {
int pointCount = pointSet->getSize();
pointSet->sortPointsByAngle();
if (pointCount <= 3) { // The points are already a convex hull
ConvexHull hull;
for (int i = 0; i < pointCount; ++i) {
hull.addPoint(*pointSet->getPoint(i));
}
if (pointCount > 0) {
hull.addPoint(*pointSet->getPoint(0));
}
return hull;
}
std::stack<const Point *> candiates;
const Point *prev;
const Point *now;
candiates.push(pointSet->getLastPoint());
candiates.push(pointSet->getReferencePoint()); // Is always the first (idx 0)
// element in the point set
for (int i = 1; i < pointCount;) {
const Point *point = pointSet->getPoint(i);
now = candiates.top();
candiates.pop();
prev = candiates.top();
candiates.push(now);
if (isCCW(prev->pos(), now->pos(), point->pos())) {
if (point->pos() == now->pos() && USE_CUSTOM_ALGO_FIX) {
std::cout << "I saved your algorithm" << std::endl;
} else {
candiates.push(point);
}
++i;
} else {
candiates.pop();
}
}
ConvexHull hull(candiates);
return hull;
}
pointer PCL_CONVEX_HULL (register context *ctx, int n, pointer *argv) {
pointer in_cloud = argv[0];
int width = intval(get_from_pointcloud(ctx, in_cloud, K_EUSPCL_WIDTH));
int height = intval(get_from_pointcloud(ctx, in_cloud, K_EUSPCL_HEIGHT));
pointer points = get_from_pointcloud(ctx, in_cloud, K_EUSPCL_POINTS);
PointCloud< Point >::Ptr ptr =
make_pcl_pointcloud< Point > (ctx, points, NULL, NULL, NULL, width, height);
PointCloud< Point >::Ptr cloud_hull (new PointCloud<Point>);
ConvexHull< Point > chull;
chull.setInputCloud (ptr);
chull.reconstruct (*cloud_hull);
return make_pointcloud_from_pcl (ctx, *cloud_hull);
}
::testing::AssertionResult HullContainsPoint(ConvexHull const &h, Point const &p) {
if (h.contains(p)) {
return ::testing::AssertionSuccess();
} else {
return ::testing::AssertionFailure()
<< "Convex hull:\n"
<< h << "\ndoes not contain " << p;
}
}
ConvexHull graham_merge(ConvexHull a, ConvexHull b) {
ConvexHull result;
// we can avoid the find pivot step because of top_point_first
if(b.boundary[0] <= a.boundary[0])
std::swap(a, b);
result.boundary = a.boundary;
result.boundary.insert(result.boundary.end(),
b.boundary.begin(), b.boundary.end());
/** if we modified graham scan to work top to bottom as proposed in lect754.pdf we could replace the
angle sort with a simple merge sort type algorithm. furthermore, we could do the graham scan
online, avoiding a bunch of memory copies. That would probably be linear. -- njh*/
result.angle_sort();
result.graham_scan();
return result;
}
static ConvexHull* CreateConvexHull(udword nb_verts, const Point* verts)
{
ConvexHull* CH = ICE_NEW(ConvexHull);
ASSERT(CH);
CONVEXHULLCREATE Create;
Create.NbVerts = nb_verts;
Create.Vertices = verts;
Create.UnifyNormals = true;
Create.PolygonData = true;
bool status = CH->Compute(Create);
ASSERT(status);
if(!status)
{
DELETESINGLE(CH);
}
return CH;
}
void rot_cal(cairo_t* cr, ConvexHull ch) {
Point tb = ch.boundary.back();
for(unsigned i = 0; i < ch.boundary.size(); i++) {
Point tc = ch.boundary[i];
Point n = -rot90(tb-tc);
Point ta = *ch.furthest(n);
cairo_move_to(cr, tc);
cairo_line_to(cr, ta);
tb = tc;
}
}
int convex_plane(eusfloat_t *src, int ssize,
eusfloat_t *coeff, eusfloat_t *ret) {
typedef PointXYZ Point;
PointCloud<Point>::Ptr cloud_projected (new PointCloud<Point>);
PointCloud<Point>::Ptr cloud_filtered (new PointCloud<Point>);
floatvector2pointcloud(src, ssize, 1, *cloud_filtered);
ModelCoefficients::Ptr coefficients (new ModelCoefficients);
coefficients->values.resize(4);
coefficients->values[0] = coeff[0];
coefficients->values[1] = coeff[1];
coefficients->values[2] = coeff[2];
coefficients->values[3] = coeff[3] / 1000.0;
// Project the model inliers
ProjectInliers<Point> proj;
proj.setModelType (SACMODEL_PLANE);
proj.setInputCloud (cloud_filtered);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Create a Concave Hull representation of the projected inliers
PointCloud<Point>::Ptr cloud_hull (new PointCloud<Point>);
ConvexHull<Point> chull;
chull.setInputCloud (cloud_projected);
//chull.setAlpha (alpha);
chull.reconstruct (*cloud_hull);
for(int i = 0; i < cloud_hull->points.size(); i++) {
*ret++ = cloud_hull->points[i].x * 1000.0;
*ret++ = cloud_hull->points[i].y * 1000.0;
*ret++ = cloud_hull->points[i].z * 1000.0;
}
return cloud_hull->points.size();
}
HullState findConvexHullStep(PointSet *pointSet, int simulateUntilStep) {
int step = 0;
int pointCount = pointSet->getSize();
pointSet->sortPointsByAngle();
//++step;
if (step++ == simulateUntilStep) // sort done -> update numbers
{
return HullState::SortDone(step);
}
if (pointCount <= 3) { // The points are already a convex hull
ConvexHull hull;
for (int i = 0; i < pointCount; ++i) {
hull.addPoint(*pointSet->getPoint(i));
}
if (pointCount > 0) {
hull.addPoint(*pointSet->getPoint(0));
}
return HullState::HullFound(hull);
}
std::stack<const Point *> candiates;
const Point *prev;
const Point *now;
candiates.push(pointSet->getLastPoint());
candiates.push(pointSet->getReferencePoint()); // Is always the first (idx 0)
// element in the point set
//++step;
if (step++ == simulateUntilStep) // break print candidates
{
return HullState::CandidateAdded(candiates, step);
}
for (int i = 1; i < pointCount;) {
const Point *point = pointSet->getPoint(i);
now = candiates.top();
candiates.pop();
prev = candiates.top();
candiates.push(now);
if (isCCW(prev->pos(), now->pos(), point->pos())) {
candiates.push(point);
// std::cout << "adding " << i << std::endl;
++i;
//++step;
if (step++ == simulateUntilStep) // break print candidates
{
return HullState::CandidateAdded(candiates, step);
}
} else {
//++step;
if (step++ == simulateUntilStep) // break print candidates
{
return HullState::CandidatePoped(candiates, point, step);
}
// std::cout << "pop" << std::endl;
candiates.pop();
}
}
ConvexHull hull(candiates);
return HullState::HullFound(hull);
}
/** \brief Given a plane, and the set of inlier indices representing it,
* segment out the object of intererest supported by it.
*
* \param[in] picked_idx the index of a point on the object
* \param[in] cloud the full point cloud dataset
* \param[in] plane_indices a set of indices representing the plane supporting the object of interest
* \param[in] plane_boundary_indices a set of indices representing the boundary of the plane
* \param[out] object the segmented resultant object
*/
void
segmentObject (int picked_idx,
const CloudConstPtr &cloud,
const PointIndices::Ptr &plane_indices,
const PointIndices::Ptr &plane_boundary_indices,
Cloud &object)
{
CloudPtr plane_hull (new Cloud);
// Compute the convex hull of the plane
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud);
chull.setIndices (plane_boundary_indices);
chull.reconstruct (*plane_hull);
// Remove the plane indices from the data
PointIndices::Ptr everything_but_the_plane (new PointIndices);
if (indices_fullset_.size () != cloud->points.size ())
{
indices_fullset_.resize (cloud->points.size ());
for (int p_it = 0; p_it < static_cast<int> (indices_fullset_.size ()); ++p_it)
indices_fullset_[p_it] = p_it;
}
std::vector<int> indices_subset = plane_indices->indices;
std::sort (indices_subset.begin (), indices_subset.end ());
set_difference (indices_fullset_.begin (), indices_fullset_.end (),
indices_subset.begin (), indices_subset.end (),
inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
// Extract all clusters above the hull
PointIndices::Ptr points_above_plane (new PointIndices);
ExtractPolygonalPrismData<PointT> exppd;
exppd.setInputCloud (cloud);
exppd.setInputPlanarHull (plane_hull);
exppd.setIndices (everything_but_the_plane);
exppd.setHeightLimits (0.0, 0.5); // up to half a meter
exppd.segment (*points_above_plane);
// Use an organized clustering segmentation to extract the individual clusters
EuclideanClusterComparator<PointT, Normal, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Normal, Label>);
euclidean_cluster_comparator->setInputCloud (cloud);
euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
// Set the entire scene to false, and the inliers of the objects located on top of the plane to true
Label l; l.label = 0;
PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
// Mask the objects that we want to split into clusters
for (int i = 0; i < static_cast<int> (points_above_plane->indices.size ()); ++i)
scene->points[points_above_plane->indices[i]].label = 1;
euclidean_cluster_comparator->setLabels (scene);
vector<bool> exclude_labels (2); exclude_labels[0] = true; exclude_labels[1] = false;
euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
euclidean_segmentation.setInputCloud (cloud);
PointCloud<Label> euclidean_labels;
vector<PointIndices> euclidean_label_indices;
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
// For each cluster found
bool cluster_found = false;
for (size_t i = 0; i < euclidean_label_indices.size (); i++)
{
if (cluster_found)
break;
// Check if the point that we picked belongs to it
for (size_t j = 0; j < euclidean_label_indices[i].indices.size (); ++j)
{
if (picked_idx != euclidean_label_indices[i].indices[j])
continue;
//pcl::PointCloud<PointT> cluster;
pcl::copyPointCloud (*cloud, euclidean_label_indices[i].indices, object);
cluster_found = true;
break;
//object_indices = euclidean_label_indices[i].indices;
//clusters.push_back (cluster);
}
}
}
ConvexHull* Object::getConvexHull() {
ConvexHull *res = new ConvexHull(this->convexHull);
res->rotate(this->orientation);
res->translate(this->position);
return res;
}
int main() {
cout<<"Hello from c++ from linux \n";
cout<<"Hello from c++ from windows \n";
cout<<"fix some bug\n";
Point2D ptA(1.0,1.0);
Point2D ptB(2.0,3.0);
Point2D ptC,ptD;
double tmp;
cout<<"Point A x=";cin>>tmp;ptA.x=(tmp);
cout<<"Point A y=";cin>>tmp;ptA.y=(tmp);
cout<<"Point B x=";cin>>tmp;ptB.x=(tmp);
cout<<"Point B y=";cin>>tmp;ptB.y=(tmp);
wcout<<"Distance="<<ptA.DistanceTo(ptB)<<endl;
cout<<"Point C x=";cin>>tmp;ptC.x=(tmp);
cout<<"Point C y=";cin>>tmp;ptC.y=(tmp);
cout<<"Point D x=";cin>>tmp;ptD.x=(tmp);
cout<<"Point D y=";cin>>tmp;ptD.y=(tmp);
LineSegment l1(ptA,ptB);
LineSegment l2(ptC,ptD);
double x,y;
int result=l1.Intersection2Segment(l2,x,y);
cout<<"Result ABxCD ="<<result<<"="<<x<<";"<<y<<endl;
ConvexHull cvh;
cvh.AddPoint(ptA);
cvh.AddPoint(ptB);
cvh.AddPoint(ptC);
cvh.AddPoint(ptD);
if(result)
{
cvh.AddPoint(Point2D(x,y));
}
cvh.FindConvex_QuickHull();
CPolygon resultConvex= cvh.m_Convex;
int numOfPoint=resultConvex.GetNumPoint();
for (int i=0;i<numOfPoint;i++)
{
cout<<"Point at "<<i<<"="<<resultConvex.GetPoint(i).x<<";"<<resultConvex.GetPoint(i).y<<endl;
}
cin.get();
return 1;
}
void Light2D::findEdges(LightProperties light, ConvexHull object, int *index0, int *index1)
{
float m1, n1, m2, n2, cx;
float x1 = light.position.x, y1 = light.position.y;
int nextIndex, index, prevIndex;
int nV = object.getVertexCount();
Vector2 p1, p2, p3, p4;
Vector2 edges[2];
int curEdge = 0;
float al;
bool u = 0;
int in[2];
bool isBackFacing[CONVEX_HULL_MAX_VERTEX_COUNT];
for(int i = 0; i < CONVEX_HULL_MAX_VERTEX_COUNT; ++i) isBackFacing[i] = 0;
//int backFacingCount = 0;
for(int i = 0; i < nV + 1; ++i)
{
Vector2 firstVertex = Vector2(object.getVertex(i).x, object.getVertex(i).y);
int secondIndex = (i + 1) % nV;
Vector2 secondVertex = Vector2(object.getVertex(secondIndex).x, object.getVertex(secondIndex).y);
Vector2 middle = (firstVertex + secondVertex) / 2;
Vector2 L = light.position - middle;
Vector2 N;
N.x = firstVertex.y - secondVertex.y;
N.y = secondVertex.x - firstVertex.x;
isBackFacing[i] = (N.dotProduct(L) > 0);
//backFacingCount += isBackFacing[i];
}
//if(backFacingCount == nV) return 0;
int startingIndex = 0;
int endingIndex = 0;
for (int i = 0; i < nV; i++)
{
//graphics.fillEllipse(object.getVertex(i).x, object.getVertex(i).y, 5, 5, 0, 2 * pi, COLOR_RGB(255, 255, 255), - 5);
int currentIndex = i % nV;
int nextEdge = (i + 1) % nV;
/*if(isBackFacing[i])
{
graphics.fillEllipse(object.getVertex(i).x, object.getVertex(i).y, 20, 20, 0, 2 * pi, COLOR_RGB(255, 255, i * (255 / nV) ), - 10);
}*/
if (isBackFacing[currentIndex] && !isBackFacing[nextEdge])
{
endingIndex = nextEdge;
//break;
}
if (!isBackFacing[currentIndex] && isBackFacing[nextEdge])
{
startingIndex = nextEdge;
}
}
//*i1 = edges[0];
//*i2 = object.getVertex(1);
*index0 = startingIndex;
*index1 = endingIndex;
/*Vector2 i1 = object.getVertex(startingIndex);
Vector2 i2 = object.getVertex(endingIndex);
graphics.fillEllipse(i1.x, i1.y, 10, 10, 0, 2 * pi, COLOR_RGB(255, 0, 0), - 10);
graphics.fillEllipse(i2.x, i2.y, 20, 20, 0, 2 * pi, COLOR_RGB(0, 255, 0), - 10);*/
//console.print("StartingIndex : " + INT_TO_STRING(startingIndex) + ", EndingIndex : " + INT_TO_STRING(endingIndex));
}
//TODO: Regard some kind of aspect ration (input)
//(then also the rotation of a single component makes sense)
void ComponentSplitterLayout::reassembleDrawings(GraphAttributes& GA, const Array<List<node> > &nodesInCC)
{
int numberOfComponents = nodesInCC.size();
Array<IPoint> box;
Array<IPoint> offset;
Array<DPoint> oldOffset;
Array<double> rotation;
ConvexHull CH;
// rotate components and create bounding rectangles
//iterate through all components and compute convex hull
for (int j = 0; j < numberOfComponents; j++)
{
//todo: should not use std::vector, but in order not
//to have to change all interfaces, we do it anyway
std::vector<DPoint> points;
//collect node positions and at the same time center average
// at origin
double avg_x = 0.0;
double avg_y = 0.0;
for (node v : nodesInCC[j])
{
DPoint dp(GA.x(v), GA.y(v));
avg_x += dp.m_x;
avg_y += dp.m_y;
points.push_back(dp);
}
avg_x /= nodesInCC[j].size();
avg_y /= nodesInCC[j].size();
//adapt positions to origin
int count = 0;
//assume same order of vertices and positions
for (node v : nodesInCC[j])
{
//TODO: I am not sure if we need to update both
GA.x(v) = GA.x(v) - avg_x;
GA.y(v) = GA.y(v) - avg_y;
points.at(count).m_x -= avg_x;
points.at(count).m_y -= avg_y;
count++;
}
// calculate convex hull
DPolygon hull = CH.call(points);
double best_area = numeric_limits<double>::max();
DPoint best_normal;
double best_width = 0.0;
double best_height = 0.0;
// find best rotation by using every face as rectangle border once.
for (DPolygon::iterator j = hull.begin(); j != hull.end(); ++j) {
DPolygon::iterator k = hull.cyclicSucc(j);
double dist = 0.0;
DPoint norm = CH.calcNormal(*k, *j);
for (const DPoint &z : hull) {
double d = CH.leftOfLine(norm, z, *k);
if (d > dist) {
dist = d;
}
}
double left = 0.0;
double right = 0.0;
norm = CH.calcNormal(DPoint(0, 0), norm);
for (const DPoint &z : hull) {
double d = CH.leftOfLine(norm, z, *k);
if (d > left) {
left = d;
}
else if (d < right) {
right = d;
}
}
double width = left - right;
dist = max(dist, 1.0);
width = max(width, 1.0);
double area = dist * width;
if (area <= best_area) {
best_height = dist;
best_width = width;
best_area = area;
best_normal = CH.calcNormal(*k, *j);
}
}
if (hull.size() <= 1) {
best_height = 1.0;
best_width = 1.0;
best_area = 1.0;
best_normal = DPoint(1.0, 1.0);
}
double angle = -atan2(best_normal.m_y, best_normal.m_x) + 1.5 * Math::pi;
if (best_width < best_height) {
angle += 0.5f * Math::pi;
double temp = best_height;
best_height = best_width;
best_width = temp;
}
rotation.grow(1, angle);
double left = hull.front().m_x;
double top = hull.front().m_y;
double bottom = hull.front().m_y;
// apply rotation to hull and calc offset
for (DPoint tempP : hull) {
double ang = atan2(tempP.m_y, tempP.m_x);
double len = sqrt(tempP.m_x*tempP.m_x + tempP.m_y*tempP.m_y);
ang += angle;
tempP.m_x = cos(ang) * len;
tempP.m_y = sin(ang) * len;
if (tempP.m_x < left) {
left = tempP.m_x;
}
if (tempP.m_y < top) {
top = tempP.m_y;
}
if (tempP.m_y > bottom) {
bottom = tempP.m_y;
}
}
oldOffset.grow(1, DPoint(left + 0.5 * static_cast<double>(m_border), -1.0 * best_height + 1.0 * bottom + 0.0 * top + 0.5 * (double)m_border));
// save rect
int w = static_cast<int>(best_width);
int h = static_cast<int>(best_height);
box.grow(1, IPoint(w + m_border, h + m_border));
}// components
offset.init(box.size());
// call packer
m_packer.get().call(box, offset, m_targetRatio);
int index = 0;
// Apply offset and rebuild Graph
for (int j = 0; j < numberOfComponents; j++)
{
double angle = rotation[index];
// apply rotation and offset to all nodes
for (node v : nodesInCC[j])
{
double x = GA.x(v);
double y = GA.y(v);
double ang = atan2(y, x);
double len = sqrt(x*x + y*y);
ang += angle;
x = cos(ang) * len;
y = sin(ang) * len;
x += static_cast<double>(offset[index].m_x);
y += static_cast<double>(offset[index].m_y);
x -= oldOffset[index].m_x;
y -= oldOffset[index].m_y;
GA.x(v) = x;
GA.y(v) = y;
}// while nodes in component
index++;
} // for components
//now we center the whole graph again
//TODO: why?
//const Graph& G = GA.constGraph();
//for(node v : G.nodes)
//MLG.moveToZero();
}
ConvexHull rect2convexhull(Rect const & r) {
ConvexHull ch;
for(int i = 0; i < 4; i++)
ch.merge(r.corner(i));
return ch;
}
/** \brief Given a plane, and the set of inlier indices representing it,
* segment out the object of intererest supported by it.
*
* \param[in] picked_idx the index of a point on the object
* \param[in] cloud the full point cloud dataset
* \param[in] plane_indices a set of indices representing the plane supporting the object of interest
* \param[out] object the segmented resultant object
*/
void
segmentObject (int picked_idx,
const typename PointCloud<PointT>::ConstPtr &cloud,
const PointIndices::Ptr &plane_indices,
PointCloud<PointT> &object)
{
typename PointCloud<PointT>::Ptr plane_hull (new PointCloud<PointT>);
// Compute the convex hull of the plane
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud);
chull.setIndices (plane_indices);
chull.reconstruct (*plane_hull);
// Remove the plane indices from the data
typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
ExtractIndices<PointT> extract (true);
extract.setInputCloud (cloud);
extract.setIndices (plane_indices);
extract.setNegative (false);
extract.filter (*plane);
PointIndices::Ptr indices_but_the_plane (new PointIndices);
extract.getRemovedIndices (*indices_but_the_plane);
// Extract all clusters above the hull
PointIndices::Ptr points_above_plane (new PointIndices);
ExtractPolygonalPrismData<PointT> exppd;
exppd.setInputCloud (cloud);
exppd.setIndices (indices_but_the_plane);
exppd.setInputPlanarHull (plane_hull);
exppd.setViewPoint (cloud->points[picked_idx].x, cloud->points[picked_idx].y, cloud->points[picked_idx].z);
exppd.setHeightLimits (0.001, 0.5); // up to half a meter
exppd.segment (*points_above_plane);
vector<PointIndices> euclidean_label_indices;
// Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
if (cloud_->isOrganized ())
{
// Use an organized clustering segmentation to extract the individual clusters
typename EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
euclidean_cluster_comparator->setInputCloud (cloud);
euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
// Set the entire scene to false, and the inliers of the objects located on top of the plane to true
Label l; l.label = 0;
PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
// Mask the objects that we want to split into clusters
for (const int &index : points_above_plane->indices)
scene->points[index].label = 1;
euclidean_cluster_comparator->setLabels (scene);
boost::shared_ptr<std::set<uint32_t> > exclude_labels = boost::make_shared<std::set<uint32_t> > ();
exclude_labels->insert (0);
euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
euclidean_segmentation.setInputCloud (cloud);
PointCloud<Label> euclidean_labels;
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
}
else
{
print_highlight (stderr, "Extracting individual clusters from the points above the reference plane ");
TicToc tt; tt.tic ();
EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setSearchMethod (search_);
ec.setInputCloud (cloud);
ec.setIndices (points_above_plane);
ec.extract (euclidean_label_indices);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", euclidean_label_indices.size ()); print_info (" clusters]\n");
}
// For each cluster found
bool cluster_found = false;
for (const auto &euclidean_label_index : euclidean_label_indices)
{
if (cluster_found)
break;
// Check if the point that we picked belongs to it
for (size_t j = 0; j < euclidean_label_index.indices.size (); ++j)
{
if (picked_idx != euclidean_label_index.indices[j])
continue;
copyPointCloud (*cloud, euclidean_label_index.indices, object);
cluster_found = true;
break;
}
}
}
void
segment (const PointT &picked_point,
int picked_idx,
PlanarRegion<PointT> ®ion,
typename PointCloud<PointT>::Ptr &object)
{
object.reset ();
// Segment out all planes
vector<ModelCoefficients> model_coefficients;
vector<PointIndices> inlier_indices, boundary_indices;
vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > > regions;
// Prefer a faster method if the cloud is organized, over RANSAC
if (cloud_->isOrganized ())
{
print_highlight (stderr, "Estimating normals ");
TicToc tt; tt.tic ();
// Estimate normals
PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
estimateNormals (cloud_, *normal_cloud);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", normal_cloud->size ()); print_info (" points]\n");
OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
mps.setMinInliers (1000);
mps.setAngularThreshold (deg2rad (3.0)); // 3 degrees
mps.setDistanceThreshold (0.03); // 2 cm
mps.setMaximumCurvature (0.001); // a small curvature
mps.setProjectPoints (true);
mps.setComparator (plane_comparator_);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
// Use one of the overloaded segmentAndRefine calls to get all the information that we want out
PointCloud<Label>::Ptr labels (new PointCloud<Label>);
vector<PointIndices> label_indices;
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
}
else
{
SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_PLANE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.005);
// Copy XYZ and Normals to a new cloud
typename PointCloud<PointT>::Ptr cloud_segmented (new PointCloud<PointT> (*cloud_));
typename PointCloud<PointT>::Ptr cloud_remaining (new PointCloud<PointT>);
ModelCoefficients coefficients;
ExtractIndices<PointT> extract;
PointIndices::Ptr inliers (new PointIndices ());
// Up until 30% of the original cloud is left
int i = 1;
while (double (cloud_segmented->size ()) > 0.3 * double (cloud_->size ()))
{
seg.setInputCloud (cloud_segmented);
print_highlight (stderr, "Searching for the largest plane (%2.0d) ", i++);
TicToc tt; tt.tic ();
seg.segment (*inliers, coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", inliers->indices.size ()); print_info (" points]\n");
// No datasets could be found anymore
if (inliers->indices.empty ())
break;
// Save this plane
PlanarRegion<PointT> region;
region.setCoefficients (coefficients);
regions.push_back (region);
inlier_indices.push_back (*inliers);
model_coefficients.push_back (coefficients);
// Extract the outliers
extract.setInputCloud (cloud_segmented);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_remaining);
cloud_segmented.swap (cloud_remaining);
}
}
print_highlight ("Number of planar regions detected: %lu for a cloud of %lu points\n", regions.size (), cloud_->size ());
double max_dist = numeric_limits<double>::max ();
// Compute the distances from all the planar regions to the picked point, and select the closest region
int idx = -1;
for (size_t i = 0; i < regions.size (); ++i)
{
double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ());
if (dist < max_dist)
{
max_dist = dist;
idx = static_cast<int> (i);
}
}
// Get the plane that holds the object of interest
if (idx != -1)
{
plane_indices_.reset (new PointIndices (inlier_indices[idx]));
if (cloud_->isOrganized ())
{
approximatePolygon (regions[idx], region, 0.01f, false, true);
print_highlight ("Planar region: %lu points initial, %lu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ());
}
else
{
// Save the current region
region = regions[idx];
print_highlight (stderr, "Obtaining the boundary points for the region ");
TicToc tt; tt.tic ();
// Project the inliers to obtain a better hull
typename PointCloud<PointT>::Ptr cloud_projected (new PointCloud<PointT>);
ModelCoefficients::Ptr coefficients (new ModelCoefficients (model_coefficients[idx]));
ProjectInliers<PointT> proj;
proj.setModelType (SACMODEL_PLANE);
proj.setInputCloud (cloud_);
proj.setIndices (plane_indices_);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Compute the boundary points as a ConvexHull
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud_projected);
PointCloud<PointT> plane_hull;
chull.reconstruct (plane_hull);
region.setContour (plane_hull);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", plane_hull.size ()); print_info (" points]\n");
}
}
// Segment the object of interest
if (plane_indices_ && !plane_indices_->indices.empty ())
{
plane_.reset (new PointCloud<PointT>);
copyPointCloud (*cloud_, plane_indices_->indices, *plane_);
object.reset (new PointCloud<PointT>);
segmentObject (picked_idx, cloud_, plane_indices_, *object);
}
}
int main(int argc, char* argv[])
{
char *infile, *outfile;
if(argc == 5)
{
if(!strcmp(argv[1], "-in") && !strcmp(argv[3], "-out"))
{
infile = argv[2];
outfile = argv[4];
}
else if(!strcmp(argv[1], "-out") && !strcmp(argv[3], "-out"))
{
infile = argv[4];
outfile = argv[2];
}
}
else if((argc==2 && strcmp(argv[1], "-help")) || argc!=2)
{
cout << "help : -in [input file] -out [output file]" << endl;
return 0;
}
vector< Point2D<> > pointList;
clock_t start, end;
ifstream ifs;
ofstream ofs;
//input
ifs.open(infile, std::ios::in);
int numOfPoint;
ifs >> numOfPoint;
int tempX, tempY;
for(int i=0; i<numOfPoint; i++)
{
ifs >> tempX >> tempY;
pointList.push_back(Point2D<>(tempX, tempY));
}
//output
ofs.open(outfile, std::ios::out);
//convex hull
ConvexHull convexHull;
start = clock();
list<Point2D<>*> *ch = convexHull.getConVexHull(pointList);
end = clock();
ofs << "convex hull time" << endl;
ofs << (float)(end-start)/CLOCKS_PER_SEC << endl;
ofs << "convex hull set" << endl;
for(list<Point2D<>*>::iterator itr=ch->begin(); itr!=ch->end(); itr++)
ofs << (*itr)->X() << ' ' << (*itr)->Y() << endl;
ofs << -1 << endl;
//clesest pair
ClosestPair closestPair;
start = clock();
pair<Point2D<>, Point2D<> > cp = closestPair.getClosestPair(pointList);
end = clock();
ofs << "close pair time" << endl;
ofs << (float)(end-start)/CLOCKS_PER_SEC << endl;
ofs << "close pair" << endl;
ofs << cp.first.X() << ' ' << cp.first.Y() << endl;
ofs << cp.second.X() << ' ' << cp.second.Y();
return 0;
}
