int main() {
PT val[] = {PT(0, 0), PT(1, 1), PT(2, 2), PT(-1, 0)};
vector<PT> puntuak;
for (int i = 0; i < 4; i++)
puntuak.push_back(val[i]);
ConvexHull(puntuak);
for (int i = 0; i < puntuak.size(); i++)
cout << puntuak[i].x << " " << puntuak[i].y << endl;
return 0;
}
void Polygon::CreateFromPoints(const VP& SomePoints)
{
m_pPosition = Sum(SomePoints)/SomePoints.size();
m_vPoints = ConvexHull(SomePoints);
for (auto& m_vPoint : m_vPoints)
{
m_vPoint -= m_pPosition;
}
}
ld max_distance(const VP &ps) {
assert (ps.size() > 1);
VP g = ConvexHull(ps);
int n = g.size(), a = 0, b = 1;
ld res = abs(g[0] - g[1]);
while (a < n) {
P p1 = g[a%n], p2 = g[(a+1)%n];
P q1 = g[b%n], q2 = g[(b+1)%n];
if (arg((p2 - p1) / (q1 - q2)) > 0) ++b; else ++a;
res = max(res, abs(p1 - q1));
}
return res;
}
/*
1.8 Close completes the cycle if this is not already the case and
pre-calculates the convex hull.
*/
void Face::Close() {
if (v.size() < 2) // Cannot close a face with only one (or even no) segment
return;
int i = v.size() - 1;
if (!(v[i].e == v[0].s)) {
// The cycle is not closed yet, we do that now.
Seg s(v[i].e, v[0].s);
AddSeg(s);
}
Sort();
ConvexHull();
Check();
}
void Reg::Close() {
int i = v.size() - 1;
if ((v[i].x2 != v[0].x1) || (v[i].y2 != v[0].y1)) {
Seg s = Seg(v[i].x2, v[i].y2, v[0].x1, v[0].y1);
AddSeg(s);
}
v = sortSegs(v);
cerr << "Dumping Reg\n"
<< ToString()
<< "Dumping Reg End\n";
ConvexHull();
}
const SHAPE_LINE_CHAIN SOLID::Hull( int aClearance, int aWalkaroundThickness ) const
{
int cl = aClearance + ( aWalkaroundThickness + 1 )/ 2;
switch( m_shape->Type() )
{
case SH_RECT:
{
SHAPE_RECT* rect = static_cast<SHAPE_RECT*>( m_shape );
return OctagonalHull( rect->GetPosition(), rect->GetSize(), cl + 1, 0.2 * cl );
}
case SH_CIRCLE:
{
SHAPE_CIRCLE* circle = static_cast<SHAPE_CIRCLE*>( m_shape );
int r = circle->GetRadius();
return OctagonalHull( circle->GetCenter() - VECTOR2I( r, r ), VECTOR2I( 2 * r, 2 * r ),
cl + 1, 0.52 * ( r + cl ) );
}
case SH_SEGMENT:
{
SHAPE_SEGMENT* seg = static_cast<SHAPE_SEGMENT*>( m_shape );
return SegmentHull( *seg, aClearance, aWalkaroundThickness );
}
case SH_SIMPLE:
{
SHAPE_SIMPLE* convex = static_cast<SHAPE_SIMPLE*>( m_shape );
return ConvexHull( *convex, cl );
}
default:
break;
}
return SHAPE_LINE_CHAIN();
}
Polygon::Polygon(const VP& SomePoints, bool alreadyconvex)
{
assert(!SomePoints.empty());
// cout << "Polygon::cctor" << endl;
if (alreadyconvex)
{
m_vPoints = SomePoints;
}
else
{
m_vPoints = ConvexHull(SomePoints);
}
// cout << "getting center of mass" << endl;
m_pPosition = Sum(m_vPoints)/m_vPoints.size();
// cout << "substracting" << endl;
for (auto& m_vPoint : m_vPoints)
{
m_vPoint -= m_pPosition;
}
// cout << "done" << endl;
}
void TraceManager::startNewWorker(int candidateIdx)
{
cout << "Starting worker...\n";
// find available worker
bool reuseWorker = false;
for (int i = 0; i < workers.size(); i++) {
if (workers[i]->status == TraceWorker::AVAILABLE) {
workers[i]->reset(refMesh, this->imageDatabase->getBary3DRefKeypoints(candidateIdx), this->imageDatabase->getKeypoints(candidateIdx), this->imageDatabase->getDescriptors(candidateIdx));
reuseWorker = true;
break;
}
}
// no available worker, create new worker
if (!reuseWorker) {
TraceWorker* worker = new TraceWorker(this, (int)workers.size());
workers.push_back(worker);
worker->reset(refMesh, this->imageDatabase->getBary3DRefKeypoints(candidateIdx), this->imageDatabase->getKeypoints(candidateIdx), this->imageDatabase->getDescriptors(candidateIdx));
hullMutex.push_back(new std::mutex);
hulls.push_back(ConvexHull());
}
}
int main(int argc, char *argv[])
{
int ntotal,external;
NodeList hull, intern, boundary, given;
HNN=MakeHashTable();
HEL=MakeHashTable();
HLI=MakeHashTable();
NEL=MakeListTable();
CM=MakeQueueTable();
SetOptions(argc,argv);
ReadFiles(argc,argv);
/* CheckBoundary(); */
ntotal = CheckInput();
hull = MakeNodeList(ntotal);
intern = MakeNodeList(ntotal);
printf("Making convex hull... %d\n",hull->count);
ConvexHull(hull,intern);
/*
CheckConvex(hull,intern);
PrintNodeList("hull",hull);
PrintNodeList("intern",intern);
*/
printf("Making maxi elements...\n");
MakeMaxiTriang(hull);
CheckNeibor(-1);
printf("Inserting convex hull... %d\n",hull->count);
InsertBoundaryNodes(hull);
CheckCircumCircle();
CheckNeibor(-2);
WriteAll("M_hull.grd",hull);
CheckNeibor(-3);
printf("Inserting internal boundary points... %d\n",intern->count);
InsertBoundaryNodes(intern);
CheckCircumCircle();
CheckNeibor(-4);
WriteAll("M_orgbound.grd",NULL);
SetResolution(hull);
CopyBoundaryLine();
printf("Recovering boundary lines 1...\n");
RecoverBoundary();
CheckCircumCircle();
CheckNeibor(-43);
WriteAll("M_bndrecover.grd",NULL);
printf("Refining boundary points 1...\n");
boundary = RefineBoundary();
if( boundary ) {
printf("Inserting new boundary points... %d\n",boundary->count);
InsertBoundaryNodes(boundary);
CheckCircumCircle();
CheckCircumCircleProperty();
CheckNeibor(-5);
}
TestVersion();
printf("Marking external elements...");
external=MarkExternalElements( hull );
printf(" %d / %d\n",external,NTotElems);
WriteGrd("M_finebound.grd");
/*
printf("Marking outer elements...");
external=MarkOuterElements();
printf(" %d / %d\n",external,NTotElems);
WriteGrd("M_test.grd");
*/
FreeNodeList(hull);
FreeNodeList(intern);
FreeNodeList(boundary);
given = GivenNodes();
printf("Inserting internal given points... %d\n",given->count);
InsertNodes(given);
FreeNodeList(given);
CheckCircumCircle();
CheckNeibor(-44);
WriteAll("M_given.grd",NULL);
printf("Recovering boundary lines 2...\n");
RecoverBoundary();
CheckCircumCircle();
CheckNeibor(-45);
WriteAll("M_intrecover.grd",NULL);
CheckArea();
printf("Inserting internal points...\n");
InsertInternalNodes();
CheckCircumCircle();
CheckCircumCircleProperty();
WriteGrd("M_insert.grd");
TestVersion();
CheckArea();
printf("Refining internal points... %f\n",OpAspect);
RefineInternalNodes();
CheckArea();
CheckCircumCircle();
CheckCircumCircleProperty();
WriteGrd("M_refine.grd");
CheckArea();
printf("Recovering boundary lines 3...\n");
RecoverBoundary();
printf("Recovering fault lines...\n");
RecoverInternalFault();
CheckCircumCircle();
CheckNeibor(-48);
WriteAll("M_intrecover2.grd",NULL);
printf("Marking outer elements...");
external=MarkOuterElements();
printf(" %d / %d\n",external,NTotElems);
printf("Marking outer nodes...");
external=MarkOuterNodes();
printf(" %d / %d\n",external,NTotNodes);
WriteGrd("M_test.grd");
TestVersion();
CheckArea();
printf("Smoothing internal points... %f\n",OpSmoothOmega);
SmoothInternalNodes();
CheckArea();
WriteGrd("final.grd");
return 0;
}
/*----------------------------------------------------------------------*
| perform clipping algorithm (private) mwgee 10/05|
*----------------------------------------------------------------------*/
bool MOERTEL::Overlap::Clipelements()
{
if (!havemxi_ || !havesxi_ || !havelines_ || !havesxim_ || !havelinem_)
{
std::cout << "***ERR*** MOERTEL::Overlap::Clipelements:\n"
<< "***ERR*** initialization of Overlap class missing\n"
<< "***ERR*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
exit(EXIT_FAILURE);
}
const int nmnode = mseg_.Nnode();
const int nsnode = sseg_.Nnode();
// put all mseg nodes in polygon
for (int i=0; i<nmnode; ++i)
AddPointtoPolygon(100+i,&mline_[i][0]);
//for (int i=0; i<nmnode; ++i)
// std::cout << "mline_[" << i << "][0] " << mline_[i][0] << " mline_[" << i << "][1] " << mline_[i][1]
// << " mline_[" << i << "][2] " << mline_[i][2] << " mline_[" << i << "][3] " << mline_[i][3] << endl;
//===========================================================================
// loop edges of slave segment and clip the master edges
// add all intersection points that can be found
for (int clipedge=0; clipedge<nsnode; ++clipedge)
{
// point on that clip edge (dim 2)
double* PE = &sline_[clipedge][0];
// the outward normal to the clip edge (dim 2)
double* N = &sn_[clipedge][0];
// loop edges of master segment and clip them against the clip edge
// add all intersections
for (int medge=0; medge<nmnode; ++medge)
{
bool ok;
// start point of medge with id 100+medge
double* P0 = &mline_[medge][0];
//int id0 = 100+medge;
// end point of medge has id 100+(medge+1 < 3)
double* P1 = &mline_[medge][2];
// find intersection between medge and clipedge
// if there's an intersection, put it in the polygon
// the id is 10*clipedge+medge
double xi[2];
ok = Clip_Intersect(N,PE,P0,P1,xi);
if (ok)
{
//std::cout << "OVERLAP Clipelements: adding intersection point " << 10*clipedge+medge << endl;
AddPointtoPolygon(10*clipedge+medge,xi);
}
} // for (int medge=0; medge<3; ++medge)
} // for (int clipedge=0; clipedge<3; ++clipedge)
//===========================================================================
// loop all clipedges and clip all points incl intersections
// that are in the polygon
// throw away all points that are not inside
{
int np = SizePointPolygon();
std::vector<Teuchos::RCP<MOERTEL::Point> > point;
PointView(point);
int p;
for (p=0; p<np; ++p)
{
bool ok = true;
//std::cout << "OVERLAP Clipelements: now clipping point " << point[p]->Id() << endl;
const double* P = point[p]->Xi();
for (int clipedge=0; clipedge<nsnode; ++clipedge)
{
// point on that clip edge (dim 2)
double* PE = &sline_[clipedge][0];
// the outward normal to the clip edge (dim 2)
double* N = &sn_[clipedge][0];
// clip point P against this edge
ok = Clip_TestPoint(N,PE,P,1.0e-10);
// leave point in
if (ok) continue;
// remove this point
else
{
ok = false;
break;
}
} // for (int clipedge=0; clipedge<3; ++clipedge)
// if not found inside, remove point
if (!ok)
{
//std::cout << "OVERLAP Clipelements: removing point " << point[p]->Id() << endl;
RemovePointfromPolygon(point[p]->Id(),P);
}
} // for (int p=0; p<np; ++p)
point.clear();
}
//===========================================================================
// loop all slave nodes and clip against master element.
// put those slave nodes that are inside master element into polygon
// Note that this works in master segment coords and the node that is put in
// is put in with slave segment coords as the polygon is completely in
// slave segment coords
// master point have ids 100,101,102
// slave points have ids 1000,1001,1002
// edge intersections have ids sedge*10+medge
// try only to put slave points in if the polygon is not empty
int np = SizePointPolygon();
if (np)
{
for (int i=0; i<nsnode; ++i)
{
bool ok = true;
// get the slave point in master coords
double* P = sxim_[i];
// loop master clip edges
for (int clipedge=0; clipedge<nmnode; ++clipedge)
{
// point on master clipedge
double* PE = &mlinem_[clipedge][0];
// the outward normal to the clip edge (dim 2)
double* N = &mn_[clipedge][0];
// clip point P against this edge
ok = Clip_TestPoint(N,PE,P,1.0e-5);
// put point in
if (ok) continue;
else
{
ok = false;
break;
}
} // for (int clipedge=0; clipedge<3; ++clipedge)
// don't put point in
if (!ok)
continue;
else
{
//std::cout << "OVERLAP Clipelements: inserting slave point " << 1000+i << " xi="
// << sxi_[i][0] << "/" << sxi_[i][1] << endl;
AddPointtoPolygon(1000+i,sxi_[i]);
}
} // for (int i=0; i<3; ++i)
}
//===========================================================================
//===========================================================================
//===========================================================================
#if 0
// make printout of the polygon so far
{
int np = SizePointPolygon();
std::vector<Teuchos::RCP<MOERTEL::Point> > point; PointView(point);
for (int p=0; p<np; ++p)
{
std::cout << "OVERLAP Clipelements: point " << setw(3) << point[p]->Id()
<< " xi " << point[p]->Xi()[0]
<< "/" << point[p]->Xi()[1] << endl;
}
point.clear();
}
#endif
//===========================================================================
// count how many corner nodes of mseg are in and how many
// intersections there are
np = SizePointPolygon();
//===========================================================================
// if there are no points, there is still the chance that all slave points
// are inside the master element
if (!np)
{
for (int i=0; i<3; ++i)
{
bool ok = true;
// get the slave point in master coords
double* P = sxim_[i];
for (int clipedge=0; clipedge<3; ++clipedge)
{
// point on master clipedge
double* PE = &mlinem_[clipedge][0];
// the outward normal to the clip edge (dim 2)
double* N = &mn_[clipedge][0];
// clip point P against this edge
ok = Clip_TestPoint(N,PE,P,1.0e-5);
// put point in
if (ok) continue;
else
{
ok = false;
break;
}
} // for (int clipedge=0; clipedge<3; ++clipedge)
// don't put point in
if (!ok)
continue;
else
{
//std::cout << "OVERLAP Clipelements: inserting slave point " << 1000+i << " xi="
// << sxi_[i][0] << "/" << sxi_[i][1] << endl;
AddPointtoPolygon(1000+i,sxi_[i]);
}
} // for (int i=0; i<3; ++i)
//=========================================================================
// check again how many points there are inside
np = SizePointPolygon();
if (np>2); // all slave points are in master segment, just continue
else if (np && np <= 2)
{
if (inter_.OutLevel()>8)
std::cout << "MOERTEL: ***WRN*** MOERTEL::Overlap::Clipelements:\n"
<< "MOERTEL: ***WRN*** " << np << " slave nodes seem to be in master segment but no overlap detected\n"
<< "MOERTEL: ***WRN*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
return false;
}
else // with none or less then 3 points in we assume no overlap
return false;
}
//===========================================================================
// maybe its a good idea to collapse intersections with nodes (that are in)
// that are really close to each other
if (p_.size()>3)
CollapsePoints(p_,1.0e-5);
#if 0
// make printout of the polygon so far
{
std::cout << "--------------------------------------------\n";
int np = SizePointPolygon();
std::vector<Teuchos::RCP<MOERTEL::Point> > point; PointView(point);
for (int p=0; p<np; ++p)
{
std::cout << "OVERLAP Clipelements: point " << setw(3) << point[p]->Id()
<< " xi " << point[p]->Xi()[0]
<< "/" << point[p]->Xi()[1] << endl;
}
point.clear();
}
#endif
//===========================================================================
// check again
np = SizePointPolygon();
if (np && np<3)
{
if (OutLevel()>8)
std::cout << "MOERTEL: ***WRN*** MOERTEL::Overlap::Clipelements:\n"
<< "MOERTEL: ***WRN*** " << np << " nodes in polygon but could not detect overlap\n"
<< "MOERTEL: ***WRN*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
return false;
}
else if (!np)
return false;
//===========================================================================
// finish the polygon
ConvexHull(p_);
return true;
}
void drawEdge(vector<PTARRAY> &final_edges, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr origin_cloud, bool hold,
boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer) {
#else
void drawEdge(vector<PTARRAY> &final_edges, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr origin_cloud, bool hold) {
#endif
printf("I am in\n");
bool disable_transform = false;
struct timeval tt1, tt2, tt3;
float color_importance = 0.2f;
float spatial_importance = 0.4f;
float normal_importance = 1.0f;
////////////////////////////// //////////////////////////////
////// This is how to use supervoxels
////////////////////////////// //////////////////////////////
gettimeofday(&tt1, NULL);
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
for (int i = 0; i < origin_cloud->size(); i++) {
if (origin_cloud->points[i].y > negdist && origin_cloud->points[i].y < posdist)
cloud->push_back(origin_cloud->points[i]);
}
pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution);
if (disable_transform)
super.setUseSingleCameraTransform (false);
super.setInputCloud (cloud);
super.setColorImportance (color_importance);
super.setSpatialImportance (spatial_importance);
super.setNormalImportance (normal_importance);
std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;
pcl::console::print_highlight ("Extracting supervoxels!\n");
super.extract (supervoxel_clusters);
pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ());
//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
//viewer->setBackgroundColor (0, 0, 0);
#if ShowEdgeResult
PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
viewer->addPointCloud (voxel_centroid_cloud, "raw centroids");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "raw centroids");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.95, "raw centroids");
PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud ();
viewer->addPointCloud (labeled_voxel_cloud, "labeled voxels");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "labeled voxels");
#endif
//PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
//We have this disabled so graph is easy to see, uncomment to see supervoxel normals
//viewer->addPointCloudNormals<PointNormal> (sv_normal_cloud,1,0.05f, "supervoxel_normals");
//std::vector< std::pair<PointTA,PointTA> > edge_lines;
pcl::console::print_highlight ("Getting supervoxel adjacency\n");
std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
super.getSupervoxelAdjacency (supervoxel_adjacency);
#if USE_SUPERVOXEL_CLUSTERS
int max_label = super.getMaxLabel();
printf("Max label %d\n", super.getMaxLabel());
int cluster_id = 1, current_label;
int *belong = new int[max_label+1];
memset(belong, 0, sizeof(int)*(max_label+1));
#endif
uint32_t supervoxel_label, adjacent_label;
//To make a graph of the supervoxel adjacency, we need to iterate through the supervoxel adjacency multimap
std::multimap<uint32_t,uint32_t>::iterator label_itr = supervoxel_adjacency.begin ();
for ( ; label_itr != supervoxel_adjacency.end (); ) {
//First get the label
supervoxel_label = label_itr->first;
#if USE_SUPERVOXEL_CLUSTERS
if (belong[supervoxel_label] == 0)
belong[supervoxel_label] = cluster_id++;
current_label = belong[supervoxel_label];
#endif
//Now get the supervoxel corresponding to the label
pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label);
//Now we need to iterate through the adjacent supervoxels and make a point cloud of them
PointCloudTA adjacent_supervoxel_centers; // RGBA
std::multimap<uint32_t,uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range (supervoxel_label).first;
for ( ; adjacent_itr!=supervoxel_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr) {
#if USE_SUPERVOXEL_CLUSTERS
adjacent_label = adjacent_itr->second;
if (belong[adjacent_label] == 0)
belong[adjacent_label] = current_label;
else if (belong[adjacent_label] != current_label) {
for (int i = 0; i < max_label+1; i++) {
if (belong[i] == belong[adjacent_label])
belong[i] = current_label;
}
}
#endif
pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at (adjacent_label);
adjacent_supervoxel_centers.push_back (neighbor_supervoxel->centroid_);
}
PointTA center = supervoxel->centroid_;
std::stringstream ss;
ss << "supervoxel_" << supervoxel_label;
/*if (hold) {
std::stringstream ss;
ss << "supervoxel_" << supervoxel_label;
addSupervoxelConnectionsToViewer (supervoxel->centroid_, adjacent_supervoxel_centers, ss.str (), viewer);
}*/
/*#if ! USE_SUPERVOXEL_CLUSTERS
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New ();
//if (center.y > negdist && center.y < posdist) {
if (center.y > negdist && center.y < posdist) { //} inAera(center.x, center.y, t, negdist, posdist)){
center.y = 0;
PointCloudTA::iterator adjacent_itr = adjacent_supervoxel_centers.begin ();
for ( ; adjacent_itr != adjacent_supervoxel_centers.end (); ++adjacent_itr) {
PointTA neighbor = *reinterpret_cast<PointTA*>(adjacent_itr->data);
// if (neighbor.y >negdist && neighbor.y < posdist) {
if (neighbor.y > negdist && neighbor.y < posdist) { //inAera(neighbor.x, neighbor.y, t, negdist, posdist)){
neighbor.y = 0;
edge_lines.push_back( std::pair<PointTA,PointTA>(center, neighbor) );
points->InsertNextPoint(center.data);
points->InsertNextPoint(neighbor.data);
}
}
}
// Create a polydata to store everything in
vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
// Add the points to the dataset
polyData->SetPoints (points);
polyLine->GetPointIds ()->SetNumberOfIds(points->GetNumberOfPoints ());
for(unsigned int i = 0; i < points->GetNumberOfPoints (); i++)
polyLine->GetPointIds ()->SetId (i,i);
cells->InsertNextCell (polyLine);
// Add the lines to the dataset
polyData->SetLines (cells);
viewer->addModelFromPolyData (polyData, ss.str());
#endif*/
//Move iterator forward to next label
label_itr = supervoxel_adjacency.upper_bound (supervoxel_label);
}
#if USE_SUPERVOXEL_CLUSTERS
/*for (int i = 0; i < max_label+1; i++) {
printf("(%d:%d) ", i, belong[i]);
if (!(i%50)) printf("\n");
}
printf("\n");*/
PointCloudT sum;
std::vector< std::pair<int,PointCloudT> > clusters;
int cells_count;
char clustername[20];
for (int i = 1; i < cluster_id; i++) {
printf("iter %d\n", i);
cells_count = 0;
PointCloudT single_cluster;
PointCloudTA::Ptr labeled_centroids;
for (int j = 1; j < max_label+1; j++) {
if (belong[j] == i) {
cells_count ++;
single_cluster += *((supervoxel_clusters.at(j))->voxels_);
//labeled_centroids->push_back( (supervoxel_clusters.at(j))->centroid_ );
}
}
if (cells_count > min_clusters_contains) {
sum += single_cluster; // TODO: sum zuihouyeyaofanhui
clusters.push_back( std::pair<int,PointCloudT>(cells_count, single_cluster) );
//sprintf(clustername, "cluster_%d", i);
//viewer->addPointCloud (labeled_centroids, clustername);
//viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,4.0, clustername);
//viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, clustername);
}
}
gettimeofday(&tt2, NULL);
printf("Time used for cluster = %lf\n", gettime(tt1, tt2));
final_edges.clear();//vector<PTARRAY> final_edges;
for (int i = 0; i < clusters.size(); i++) {
PointCloudT cluster = clusters[i].second;
final_edges.push_back( ConvexHull(cluster) );
}
gettimeofday(&tt3, NULL);
printf("Time used for Convex = %lf\n", gettime(tt2, tt3));
#if ShowEdgeResult
char final_edge_name[20];
for (int i = 0; i < final_edges.size(); i++) {
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New ();
#if EdgeDebug
char debug_name[20];
sprintf(debug_name, "EdgeViewer_%d_debug", i);
boost::shared_ptr<pcl::visualization::PCLVisualizer> edge_viewer_debug(new pcl::visualization::PCLVisualizer(debug_name));
edge_viewer_debug->setBackgroundColor (0, 0, 0);
sprintf(debug_name, "cloud_%d_debug", i);
edge_viewer_debug->addPointCloud(cloud, debug_name);
edge_viewer_debug->addPointCloud (voxel_centroid_cloud, "raw centroids");
edge_viewer_debug->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "raw centroids");
edge_viewer_debug->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.95, "raw centroids");
edge_viewer_debug->addPointCloud (labeled_voxel_cloud, "labeled voxels");
edge_viewer_debug->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "labeled voxels");
vtkSmartPointer<vtkPoints> points_debug = vtkSmartPointer<vtkPoints>::New ();
vtkSmartPointer<vtkCellArray> cells_debug = vtkSmartPointer<vtkCellArray>::New ();
vtkSmartPointer<vtkPolyLine> polyLine_debug = vtkSmartPointer<vtkPolyLine>::New ();
#endif
final_edges[i][0].y = 0;
for (int j = 0; j < final_edges[i].size() - 1; j++) {
final_edges[i][j+1].y = 0;
points->InsertNextPoint(final_edges[i][j].data);
points->InsertNextPoint(final_edges[i][j+1].data);
#if EdgeDebug
points_debug->InsertNextPoint(final_edges[i][j].data);
points_debug->InsertNextPoint(final_edges[i][j+1].data);
#endif
}
points->InsertNextPoint(final_edges[i][final_edges[i].size()-1].data);
points->InsertNextPoint(final_edges[i][0].data);
// Create a polydata to store everything in
vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
// Add the points to the dataset
polyData->SetPoints (points);
polyLine->GetPointIds ()->SetNumberOfIds(points->GetNumberOfPoints ());
for(unsigned int j= 0; j < points->GetNumberOfPoints (); j++)
polyLine->GetPointIds ()->SetId (j,j);
cells->InsertNextCell (polyLine);
// Add the lines to the dataset
polyData->SetLines (cells);
sprintf(final_edge_name, "final_edge_%d", i);
viewer->addModelFromPolyData (polyData, final_edge_name);
#if EdgeDebug
points_debug->InsertNextPoint(final_edges[i][final_edges[i].size()-1].data);
points_debug->InsertNextPoint(final_edges[i][0].data);
vtkSmartPointer<vtkPolyData> polyData_debug = vtkSmartPointer<vtkPolyData>::New ();
// Add the points to the dataset
polyData_debug->SetPoints (points_debug);
polyLine_debug->GetPointIds ()->SetNumberOfIds(points_debug->GetNumberOfPoints ());
for(unsigned int j = 0; j < points_debug->GetNumberOfPoints (); j++)
polyLine_debug->GetPointIds ()->SetId (j,j);
cells_debug->InsertNextCell (polyLine_debug);
// Add the lines to the dataset
polyData_debug->SetLines (cells_debug);
sprintf(debug_name, "edges_%d_debug", i);
edge_viewer_debug->addModelFromPolyData (polyData_debug, debug_name);
// while (!edge_viewer_debug->wasStopped());
#endif
}
#endif
#endif
return; // TODO: if USE_SUPERVOXEL_CLUSTERS, an empty vector would be returned
}
vector<City> startingTour(const vector<Point>& points) {
return ConvexHull(points);
}
void TraceManager::feed(cv::Mat &img)
{
if (this->imageDatabase->getSize() == 0) {
return;
}
if (Configuration::mode == Configuration::MODE::Extend) {
needNewWorker = false;
vector<std::thread> threads;
threads.resize(workers.size());
// first detect keypoints and descriptors
// TODO: make detect behind the worker to make convex test right
detecter->detect(img);
std::future<int> future;
// detecter->vote(img);
if (needDetect) {
future = std::async(std::launch::async, &DetectWorker::vote, detecter, std::ref(img));
}
// int candidateIdx = detecter->vote(img);
// reuse detector's descriptor
for (int i = 0; i < threads.size(); i++) {
if (workers[i]->status == TraceWorker::BUSY) {
threads[i] = thread(&TraceWorker::trace, workers[i], std::ref(img), detecter);
}
}
// wait for all tasks to finish
for (int i = 0; i < threads.size(); i++) {
if (threads[i].joinable()) {
threads[i].join();
}
}
if (needDetect) {
int candidateIdx = future.get();
needNewWorker = candidateIdx != -1;
if (needNewWorker) {
startNewWorker(candidateIdx);
needDetect = false;
}
} else {
needDetect = true;
}
} else {
if (indepWorkers.size() == 0) {
for (int i = 0; i < this->imageDatabase->getSize(); i++) {
indepWorkers.push_back(new IndepWorker(this, i));
hullMutex.push_back(new std::mutex);
hulls.push_back(ConvexHull());
}
}
vector<std::thread> threads;
threads.resize(indepWorkers.size());
for (int i = 0; i < threads.size(); i++) {
threads[i] = thread(&IndepWorker::trace, indepWorkers[i], std::ref(img));
}
for (int i = 0; i < threads.size(); i++) {
if (threads[i].joinable()) {
threads[i].join();
}
}
}
}
Geometry*
Geometry::convexHull() const
{
return ConvexHull(this).getConvexHull();
}
