void Candidate::Reindex(const MiscLib::Vector< int > &newIndices, int minInvalidIndex,
size_t mergedSubsets, const MiscLib::Vector< size_t > &subsetSizes,
const PointCloud &pc, size_t currentSize, float epsilon, float normalThresh,
float bitmapEpsilon)
{
size_t i = 0, j = 0;
for(; i < m_indices->size(); ++i)
if(newIndices[(*m_indices)[i]] < minInvalidIndex)
(*m_indices)[j++] = newIndices[(*m_indices)[i]];
if(m_subset <= mergedSubsets)
{
m_hasConnectedComponent = false;
m_subset = 0;
m_indices->clear();
m_lowerBound = 0;
m_upperBound = 0; // forget this candidate
m_score = 0;
return;
}
else
{
m_indices->resize(j);
m_subset -= mergedSubsets;
if(m_subset >= subsetSizes.size())
// do connected component if all subsets have been computed
ConnectedComponent(pc, bitmapEpsilon);
}
size_t sampledPoints = 0,
endi = std::min(m_subset, subsetSizes.size());;
for(i = 0; i < endi; ++i)
sampledPoints += subsetSizes[i];
GetBounds(sampledPoints, currentSize);
}
bool Sphere::Init(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 4)
return false;
// get center
size_t c = samples.size() / 2;
m_center = Vec3f(0, 0, 0);
size_t midCount = 0;
for(size_t i = 0; i < c - 1; ++i)
for(size_t j = i + 1; j < c; ++j)
{
Vec3f mid;
if(!Midpoint(samples[i], samples[i + c], samples[j], samples[j + c], &mid))
continue;
m_center += mid;
++midCount;
}
if(!midCount)
return false;
m_center /= midCount;
m_radius = 0;
for(size_t i = 0; i < c; ++i)
{
float d = (samples[i] - m_center).length();
m_radius += d;
}
m_radius /= c;
return true;
}
bool Cone::Init(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 6)
return false;
size_t c = samples.size() >> 1;
return Init(samples[0], samples[1], samples[2],
samples[c], samples[c + 1], samples[c + 2]);
}
size_t BitmapPrimitiveShape::AllConnectedComponents(const PointCloud &pc, float epsilon,
BitmapInfo& bitmapInfo, std::shared_ptr<MiscLib::Vector< size_t > >indices, MiscLib::Vector< int >& componentsImg,
MiscLib::Vector< std::pair< int, size_t > >& labels, bool doFiltering )
{
// first find the extent in the parametrization
// but remember the parametrized points for projection into a bitmap
size_t size = indices->size();
if(!size)
return 0;
// set up bitmap
MiscLib::Vector< std::pair< float, float > > extParams;
BuildBitmap(pc, &epsilon, indices->begin(), indices->end(), &bitmapInfo.params,
&bitmapInfo.bbox, &bitmapInfo.bitmap, &bitmapInfo.uextent, &bitmapInfo.vextent, &bitmapInfo.bmpIdx);
/*static int fname_int = 0;
std::ostringstream fn;
fn << "bitmapImg" << fname_int++ << ".txt";
std::ofstream file;
file.open(fn.str().c_str(), std::ios::out);
for(size_t j = 0; j < vextent; ++j)
{
for(size_t i = 0; i < uextent; ++i)
file << bitmap[j * uextent + i];
file << std::endl;
}
file.close();*/
// do a wrapping by copying pixels
PreWrapBitmap(bitmapInfo.bbox, epsilon, bitmapInfo.uextent, bitmapInfo.vextent, &bitmapInfo.bitmap);
MiscLib::Vector< char > tempBmp(bitmapInfo.bitmap.size()); // temporary bitmap object
bool uwrap, vwrap;
WrapBitmap(bitmapInfo.bbox, epsilon, &uwrap, &vwrap);
if (doFiltering)
{
// closing
DilateCross(bitmapInfo.bitmap, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &tempBmp);
ErodeCross(tempBmp, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &bitmapInfo.bitmap);
// opening
//ErodeCross(bitmap, uextent, vextent, uwrap, vwrap, &tempBmp);
//DilateCross(tempBmp, uextent, vextent, uwrap, vwrap, &bitmap);
}
Components(bitmapInfo.bitmap, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &componentsImg,
&labels);
if(labels.size() <= 1) // found no connected component!
{
return 0; // associate no points with this shape
}
WrapComponents(bitmapInfo.bbox, epsilon, bitmapInfo.uextent, bitmapInfo.vextent, &componentsImg, &labels);
return labels.size();
}
void Candidate::Reindex(const MiscLib::Vector< size_t > &reindex)
{
size_t reindexSize = reindex.size();
for(size_t i = 0; i < m_indices->size(); ++i)
if(m_indices->at(i) < reindexSize)
m_indices->at(i) = reindex[m_indices->at(i)];
}
bool Cylinder::Init(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 4)
return false;
// estimate axis from all pairs
m_axisDir = Vec3f(0, 0, 0);
size_t c = samples.size() / 2;
size_t axisCount = 0;
m_axisDir = samples[0 + c].cross(samples[1 + c]);
if(m_axisDir.normalize() < 1e-3)
return false;
m_axisPos = Vec3f(0, 0, 0);
m_radius = 0;
// project first normal into plane
float l = m_axisDir.dot(samples[0 + c]);
Vec3f xdir = samples[0 + c] - l * m_axisDir;
xdir.normalize();
Vec3f ydir = m_axisDir.cross(xdir);
ydir.normalize();
// xdir is the x axis in the plane (y = 0) samples[0] is the origin
float lineBnx = ydir.dot(samples[1 + c]);
if(abs(lineBnx) < 1e-6)
return false;
float lineBny = -xdir.dot(samples[1 + c]);
// origin of lineB
Vec3f originB = samples[1] - samples[0];
float lineBOx = xdir.dot(originB);
float lineBOy = ydir.dot(originB);
float lineBd = lineBnx * lineBOx + lineBny * lineBOy;
// lineB in the plane complete
// point of intersection is y = 0 and x = lineBd / lineBnx
float radius = lineBd / lineBnx;
m_axisPos += samples[0] + radius * xdir;
m_radius += abs(radius);
m_radius += std::sqrt((radius - lineBOx) * (radius - lineBOx) + lineBOy * lineBOy);
m_radius /= 2;
if(m_radius > 1e6)
return false;
// find point on axis closest to origin
float lambda = m_axisDir.dot(-m_axisPos);
m_axisPos = m_axisPos + lambda * m_axisDir;
m_hcs.FromNormal(m_axisDir);
m_angularRotatedRadians = 0;
return true;
}
bool Plane::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 1)
return false;
m_normal = Vec3f(0, 0, 0);
m_pos = Vec3f(0, 0, 0);
size_t c = samples.size() / 2;
MiscLib::Vector< GfxTL::Vector3Df > normals(c);
for(intptr_t i = 0; i < c; ++i)
normals[i] = GfxTL::Vector3Df(samples[i + c]);
GfxTL::Vector3Df meanNormal;
GfxTL::MeanOfNormals(normals.begin(), normals.end(), &meanNormal);
m_normal = Vec3f(meanNormal.Data());
GfxTL::Vector3Df mean;
GfxTL::Mean(samples.begin(), samples.begin() + c, &mean);
m_pos = Vec3f(mean.Data());
m_dist = m_pos.dot(m_normal);
return true;
}
void ConePrimitiveShape::BitmapExtent(float epsilon,
GfxTL::AABox< GfxTL::Vector2Df > *bbox,
MiscLib::Vector< std::pair< float, float > > *params,
size_t *uextent, size_t *vextent)
{
*uextent = std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon); // no wrappig along u direction
*vextent = std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon) + 1; // add one for wrapping
if((*vextent) * (*uextent) > 1e6 && m_cone.Angle() < float(M_PI / 4))
{
// try to reparameterize
// try to find cut in the outer regions
MiscLib::Vector< float > angularParams;//(params->size());
angularParams.reserve(params->size());
float outer = 3.f * std::max(abs(bbox->Min()[0]), abs(bbox->Max()[0])) / 4.f;
for(size_t i = 0; i < params->size(); ++i)
if((*params)[i].first > outer)
angularParams.push_back(((*params)[i].second
/ m_cone.RadiusAtLength((*params)[i].first)) + float(M_PI));
std::sort(angularParams.begin(), angularParams.end());
// try to find a large gap
float maxGap = 0;
float lower, upper;
for(size_t i = 1; i < angularParams.size(); ++i)
{
float gap = angularParams[i] - angularParams[i - 1];
if(gap > maxGap)
{
maxGap = gap;
lower = angularParams[i - 1];
upper = angularParams[i];
}
}
// reparameterize with new angular cut
float newCut = (lower + upper) / 2.f;
m_cone.RotateAngularDirection(newCut);
bbox->Min()[1] = std::numeric_limits< float >::infinity();
bbox->Max()[1] = -std::numeric_limits< float >::infinity();
for(size_t i = 0; i < params->size(); ++i)
{
float r = m_cone.RadiusAtLength((*params)[i].first);
(*params)[i].second = ((*params)[i].second / r) + float(M_PI) - newCut;
if((*params)[i].second < 0)
(*params)[i].second = 2 * float(M_PI) + (*params)[i].second;
(*params)[i].second = ((*params)[i].second - float(M_PI)) * r;
if((*params)[i].second < bbox->Min()[1])
bbox->Min()[1] = (*params)[i].second;
if((*params)[i].second > bbox->Max()[1])
bbox->Max()[1] = (*params)[i].second;
}
*vextent = std::floor((bbox->Max()[1] - bbox->Min()[1]) / epsilon) + 1;
}
}
void BitmapPrimitiveShape::BuildPolygons(const PointCloud &pc, float epsilon,
size_t begin, size_t end, GfxTL::AABox< GfxTL::Vector2Df > *bbox,
size_t *uextent, size_t *vextent,
std::deque< ComponentPolygons > *polys) const
{
// curves are extracted in the following way:
// first the bitmap is constructed
// then connected components are found
// for each component the curves are found
// constructing the bitmap is similar to ConnectedComponent
// -> use the same code
MiscLib::Vector< std::pair< float, float > > params;
MiscLib::Vector< char > bitmap;
MiscLib::Vector< size_t > bmpIdx;
BuildBitmap(pc, &epsilon, IndexIterator(begin), IndexIterator(end), ¶ms,
bbox, &bitmap, uextent, vextent, &bmpIdx);
// do closing
MiscLib::Vector< char > tempBmp(bitmap.size());
DilateCross(bitmap, *uextent, *vextent, false, false, &tempBmp);
ErodeCross(tempBmp, *uextent, *vextent, false, false, &bitmap);
// find connected components
MiscLib::Vector< int > componentsImg;
MiscLib::Vector< std::pair< int, size_t > > labels;
Components(bitmap, *uextent, *vextent, false, false, &componentsImg,
&labels);
if(labels.size() <= 1) // found no connected component!
return;
// for each component find all polygons
for(size_t i = 1; i < labels.size(); ++i)
{
polys->resize(polys->size() + 1);
ComponentLoops(componentsImg, *uextent, *vextent, labels[i].first,
false, false, &(*polys)[polys->size() - 1]);
}
}
bool Sphere::Interpolate(const MiscLib::Vector< Sphere > &spheres,
const MiscLib::Vector< float > &weights, Sphere *is)
{
Vec3f center(0, 0, 0);
float radius = 0;
for(size_t i = 0; i < spheres.size(); ++i)
{
center += weights[i] * spheres[i].Center();
radius += weights[i] * spheres[i].Radius();
}
is->Center(center);
is->Radius(radius);
return true;
}
bool Plane::Interpolate(const MiscLib::Vector< Plane > &planes,
const MiscLib::Vector< float > &weights, Plane *ip)
{
Vec3f normal(0, 0, 0);
Vec3f position(0, 0, 0);
for(size_t i = 0; i < planes.size(); ++i)
{
normal += weights[i] * planes[i].getNormal();
position += weights[i] * planes[i].getPosition();
}
normal.normalize();
*ip = Plane(position, normal);
return true;
}
bool Cylinder::Interpolate(const MiscLib::Vector< Cylinder > &cylinders,
const MiscLib::Vector< float > &weights, Cylinder *ic)
{
Vec3f axisPos(0, 0, 0);
Vec3f axisDir(0, 0, 0);
float r = 0;
for(size_t i = 0; i < cylinders.size(); ++i)
{
axisPos += weights[i] * cylinders[i].AxisPosition();
axisDir += weights[i] * cylinders[i].AxisDirection();
r += weights[i] * cylinders[i].Radius();
}
axisDir.normalize();
return ic->Init(axisDir, axisPos, r);
}
bool Cone::Interpolate(const MiscLib::Vector< Cone > &cones,
const MiscLib::Vector< float > &weights, Cone *ic)
{
Vec3f center(0, 0, 0);
Vec3f axisDir(0, 0, 0);
float omega = 0;
for(size_t i = 0; i < cones.size(); ++i)
{
center += weights[i] * cones[i].Center();
axisDir += weights[i] * cones[i].AxisDirection();
omega += weights[i] * cones[i].Angle();
}
axisDir.normalize();
return ic->Init(center, axisDir, omega);
}
void qRansacSD::doAction()
{
assert(m_app);
if (!m_app)
return;
const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
size_t selNum = selectedEntities.size();
if (selNum!=1)
{
m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccHObject* ent = selectedEntities[0];
assert(ent);
if (!ent || !ent->isA(CC_POINT_CLOUD))
{
m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccPointCloud* pc = static_cast<ccPointCloud*>(ent);
//input cloud
size_t count = (size_t)pc->size();
bool hasNorms = pc->hasNormals();
PointCoordinateType bbMin[3],bbMax[3];
pc->getBoundingBox(bbMin,bbMax);
//Convert CC point cloud to RANSAC_SD type
PointCloud cloud;
{
try
{
cloud.reserve(count);
}
catch(...)
{
m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
//default point & normal
Point Pt;
Pt.normal[0] = 0.0;
Pt.normal[1] = 0.0;
Pt.normal[2] = 0.0;
for (unsigned i=0; i<(unsigned)count; ++i)
{
const CCVector3* P = pc->getPoint(i);
Pt.pos[0] = P->x;
Pt.pos[1] = P->y;
Pt.pos[2] = P->z;
if (hasNorms)
{
const PointCoordinateType* N = pc->getPointNormal(i);
Pt.normal[0] = N[0];
Pt.normal[1] = N[1];
Pt.normal[2] = N[2];
}
cloud.push_back(Pt);
}
//manually set bounding box!
Vec3f cbbMin,cbbMax;
cbbMin[0] = bbMin[0];
cbbMin[1] = bbMin[1];
cbbMin[2] = bbMin[2];
cbbMax[0] = bbMax[0];
cbbMax[1] = bbMax[1];
cbbMax[2] = bbMax[2];
cloud.setBBox(cbbMin,cbbMax);
}
//cloud scale (useful for setting several parameters
const float scale = cloud.getScale();
//init dialog with default values
ccRansacSDDlg rsdDlg(m_app->getMainWindow());
rsdDlg.epsilonDoubleSpinBox->setValue(.01f * scale); // set distance threshold to .01f of bounding box width
// NOTE: Internally the distance threshold is taken as 3 * ransacOptions.m_epsilon!!!
rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.02f * scale); // set bitmap resolution to .02f of bounding box width
// NOTE: This threshold is NOT multiplied internally!
rsdDlg.supportPointsSpinBox->setValue(s_supportPoints);
rsdDlg.normThreshDoubleSpinBox->setValue(s_normThresh);
rsdDlg.probaDoubleSpinBox->setValue(s_proba);
rsdDlg.planeCheckBox->setChecked(s_primEnabled[0]);
rsdDlg.sphereCheckBox->setChecked(s_primEnabled[1]);
rsdDlg.cylinderCheckBox->setChecked(s_primEnabled[2]);
rsdDlg.coneCheckBox->setChecked(s_primEnabled[3]);
rsdDlg.torusCheckBox->setChecked(s_primEnabled[4]);
if (!rsdDlg.exec())
return;
//for parameters persistence
{
s_supportPoints = rsdDlg.supportPointsSpinBox->value();
s_normThresh = rsdDlg.normThreshDoubleSpinBox->value();
s_proba = rsdDlg.probaDoubleSpinBox->value();
//consistency check
{
unsigned char primCount = 0;
for (unsigned char k=0; k<5; ++k)
primCount += (unsigned)s_primEnabled[k];
if (primCount==0)
{
m_app->dispToConsole("No primitive type selected!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
}
s_primEnabled[0] = rsdDlg.planeCheckBox->isChecked();
s_primEnabled[1] = rsdDlg.sphereCheckBox->isChecked();
s_primEnabled[2] = rsdDlg.cylinderCheckBox->isChecked();
s_primEnabled[3] = rsdDlg.coneCheckBox->isChecked();
s_primEnabled[4] = rsdDlg.torusCheckBox->isChecked();
}
//import parameters from dialog
RansacShapeDetector::Options ransacOptions;
{
ransacOptions.m_epsilon = rsdDlg.epsilonDoubleSpinBox->value();
ransacOptions.m_bitmapEpsilon = rsdDlg.bitmapEpsilonDoubleSpinBox->value();
ransacOptions.m_normalThresh = rsdDlg.normThreshDoubleSpinBox->value();
ransacOptions.m_minSupport = rsdDlg.supportPointsSpinBox->value();
ransacOptions.m_probability = rsdDlg.probaDoubleSpinBox->value();
}
//progress dialog
ccProgressDialog progressCb(false,m_app->getMainWindow());
progressCb.setRange(0,0);
if (!hasNorms)
{
progressCb.setInfo("Computing normals (please wait)");
progressCb.start();
QApplication::processEvents();
cloud.calcNormals(.01f * scale);
if (pc->reserveTheNormsTable())
{
for (size_t i=0; i<count; ++i)
{
CCVector3 N(cloud[i].normal);
N.normalize();
pc->addNorm(N.u);
}
pc->showNormals(true);
//currently selected entities appearance may have changed!
pc->prepareDisplayForRefresh_recursive();
}
else
{
m_app->dispToConsole("Not enough memory to compute normals!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
}
// set which primitives are to be detected by adding the respective constructors
RansacShapeDetector detector(ransacOptions); // the detector object
if (rsdDlg.planeCheckBox->isChecked())
detector.Add(new PlanePrimitiveShapeConstructor());
if (rsdDlg.sphereCheckBox->isChecked())
detector.Add(new SpherePrimitiveShapeConstructor());
if (rsdDlg.cylinderCheckBox->isChecked())
detector.Add(new CylinderPrimitiveShapeConstructor());
if (rsdDlg.coneCheckBox->isChecked())
detector.Add(new ConePrimitiveShapeConstructor());
if (rsdDlg.torusCheckBox->isChecked())
detector.Add(new TorusPrimitiveShapeConstructor());
size_t remaining = count;
typedef std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > DetectedShape;
MiscLib::Vector< DetectedShape > shapes; // stores the detected shapes
// run detection
// returns number of unassigned points
// the array shapes is filled with pointers to the detected shapes
// the second element per shapes gives the number of points assigned to that primitive (the support)
// the points belonging to the first shape (shapes[0]) have been sorted to the end of pc,
// i.e. into the range [ pc.size() - shapes[0].second, pc.size() )
// the points of shape i are found in the range
// [ pc.size() - \sum_{j=0..i} shapes[j].second, pc.size() - \sum_{j=0..i-1} shapes[j].second )
{
progressCb.setInfo("Operation in progress");
progressCb.setMethodTitle("Ransac Shape Detection");
progressCb.start();
//run in a separate thread
s_detector = &detector;
s_shapes = &shapes;
s_cloud = &cloud;
QFuture<void> future = QtConcurrent::run(doDetection);
unsigned progress = 0;
while (!future.isFinished())
{
#if defined(_WIN32) || defined(WIN32)
::Sleep(500);
#else
sleep(500);
#endif
progressCb.update(++progress);
//Qtconcurrent::run can't be canceled!
/*if (progressCb.isCancelRequested())
{
future.cancel();
future.waitForFinished();
s_remainingPoints = count;
break;
}
//*/
}
remaining = s_remainingPoints;
progressCb.stop();
QApplication::processEvents();
}
//else
//{
// remaining = detector.Detect(cloud, 0, cloud.size(), &shapes);
//}
#ifdef _DEBUG
FILE* fp = fopen("RANS_SD_trace.txt","wt");
fprintf(fp,"[Options]\n");
fprintf(fp,"epsilon=%f\n",ransacOptions.m_epsilon);
fprintf(fp,"bitmap epsilon=%f\n",ransacOptions.m_bitmapEpsilon);
fprintf(fp,"normal thresh=%f\n",ransacOptions.m_normalThresh);
fprintf(fp,"min support=%i\n",ransacOptions.m_minSupport);
fprintf(fp,"probability=%f\n",ransacOptions.m_probability);
fprintf(fp,"\n[Statistics]\n");
fprintf(fp,"input points=%i\n",count);
fprintf(fp,"segmented=%i\n",count-remaining);
fprintf(fp,"remaining=%i\n",remaining);
if (shapes.size()>0)
{
fprintf(fp,"\n[Shapes]\n");
for (unsigned i=0; i<shapes.size(); ++i)
{
PrimitiveShape* shape = shapes[i].first;
unsigned shapePointsCount = shapes[i].second;
std::string desc;
shape->Description(&desc);
fprintf(fp,"#%i - %s - %i points\n",i+1,desc.c_str(),shapePointsCount);
}
}
fclose(fp);
#endif
if (remaining == count)
{
m_app->dispToConsole("Segmentation failed...",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
if (shapes.size() > 0)
{
ccHObject* group = 0;
for (MiscLib::Vector<DetectedShape>::const_iterator it = shapes.begin(); it != shapes.end(); ++it)
{
const PrimitiveShape* shape = it->first;
size_t shapePointsCount = it->second;
//too many points?!
if (shapePointsCount > count)
{
m_app->dispToConsole("Inconsistent result!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
break;
}
std::string desc;
shape->Description(&desc);
//new cloud for sub-part
ccPointCloud* pcShape = new ccPointCloud(desc.c_str());
//we fill cloud with sub-part points
if (!pcShape->reserve((unsigned)shapePointsCount))
{
m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
delete pcShape;
break;
}
bool saveNormals = pcShape->reserveTheNormsTable();
for (size_t j=0; j<shapePointsCount; ++j)
{
pcShape->addPoint(CCVector3(cloud[count-1-j].pos));
if (saveNormals)
pcShape->addNorm(cloud[count-1-j].normal);
}
//random color
unsigned char col[3]= { (unsigned char)(255.0*(float)rand()/(float)RAND_MAX),
(unsigned char)(255.0*(float)rand()/(float)RAND_MAX),
0
};
col[2]=255-(col[1]+col[2])/2;
pcShape->setRGBColor(col);
pcShape->showColors(true);
pcShape->showNormals(saveNormals);
pcShape->setVisible(true);
//convert detected primitive into a CC primitive type
ccGenericPrimitive* prim = 0;
switch(shape->Identifier())
{
case 0: //plane
{
const PlanePrimitiveShape* plane = static_cast<const PlanePrimitiveShape*>(shape);
Vec3f G = plane->Internal().getPosition();
Vec3f N = plane->Internal().getNormal();
Vec3f X = plane->getXDim();
Vec3f Y = plane->getYDim();
//we look for real plane extents
float minX,maxX,minY,maxY;
for (size_t j=0; j<shapePointsCount; ++j)
{
std::pair<float,float> param;
plane->Parameters(cloud[count-1-j].pos,¶m);
if (j!=0)
{
if (minX<param.first)
minX=param.first;
else if (maxX>param.first)
maxX=param.first;
if (minY<param.second)
minY=param.second;
else if (maxY>param.second)
maxY=param.second;
}
else
{
minX=maxX=param.first;
minY=maxY=param.second;
}
}
//we recenter plane (as it is not always the case!)
float dX = maxX-minX;
float dY = maxY-minY;
G += X * (minX+dX*0.5);
G += Y * (minY+dY*0.5);
//we build matrix from these vecctors
ccGLMatrix glMat(CCVector3(X.getValue()),
CCVector3(Y.getValue()),
CCVector3(N.getValue()),
CCVector3(G.getValue()));
//plane primitive
prim = new ccPlane(dX,dY,&glMat);
}
break;
case 1: //sphere
{
const SpherePrimitiveShape* sphere = static_cast<const SpherePrimitiveShape*>(shape);
float radius = sphere->Internal().Radius();
Vec3f CC = sphere->Internal().Center();
pcShape->setName(QString("Sphere (r=%1)").arg(radius,0,'f'));
//we build matrix from these vecctors
ccGLMatrix glMat;
glMat.setTranslation(CCVector3(CC.getValue()));
//sphere primitive
prim = new ccSphere(radius,&glMat);
prim->setEnabled(false);
}
break;
case 2: //cylinder
{
const CylinderPrimitiveShape* cyl = static_cast<const CylinderPrimitiveShape*>(shape);
Vec3f G = cyl->Internal().AxisPosition();
Vec3f N = cyl->Internal().AxisDirection();
Vec3f X = cyl->Internal().AngularDirection();
Vec3f Y = N.cross(X);
float r = cyl->Internal().Radius();
float hMin = cyl->MinHeight();
float hMax = cyl->MaxHeight();
float h = hMax-hMin;
G += N * (hMin+h*0.5);
pcShape->setName(QString("Cylinder (r=%1/h=%2)").arg(r,0,'f').arg(h,0,'f'));
//we build matrix from these vecctors
ccGLMatrix glMat(CCVector3(X.getValue()),
CCVector3(Y.getValue()),
CCVector3(N.getValue()),
CCVector3(G.getValue()));
//cylinder primitive
prim = new ccCylinder(r,h,&glMat);
prim->setEnabled(false);
}
break;
case 3: //cone
{
const ConePrimitiveShape* cone = static_cast<const ConePrimitiveShape*>(shape);
Vec3f CC = cone->Internal().Center();
Vec3f CA = cone->Internal().AxisDirection();
float alpha = cone->Internal().Angle();
//compute max height
CCVector3 maxP(CC.getValue());
float maxHeight = 0;
for (size_t j=0; j<shapePointsCount; ++j)
{
float h = cone->Internal().Height(cloud[count-1-j].pos);
if (h>maxHeight)
{
maxHeight=h;
maxP = CCVector3(cloud[count-1-j].pos);
}
}
pcShape->setName(QString("Cone (alpha=%1/h=%2)").arg(alpha,0,'f').arg(maxHeight,0,'f'));
float radius = tan(alpha)*maxHeight;
CCVector3 Z = CCVector3(CA.getValue());
CCVector3 C = CCVector3(CC.getValue()); //cone apex
//construct remaining of base
Z.normalize();
CCVector3 X = maxP - (C + maxHeight * Z);
X.normalize();
CCVector3 Y = Z * X;
//we build matrix from these vecctors
ccGLMatrix glMat(X,Y,Z,C+(maxHeight*0.5)*Z);
//cone primitive
prim = new ccCone(0,radius,maxHeight,0,0,&glMat);
prim->setEnabled(false);
}
break;
case 4: //torus
{
const TorusPrimitiveShape* torus = static_cast<const TorusPrimitiveShape*>(shape);
if (torus->Internal().IsAppleShaped())
{
m_app->dispToConsole("[qRansacSD] Apple-shaped torus are not handled by CloudCompare!",ccMainAppInterface::WRN_CONSOLE_MESSAGE);
}
else
{
Vec3f CC = torus->Internal().Center();
Vec3f CA = torus->Internal().AxisDirection();
float minRadius = torus->Internal().MinorRadius();
float maxRadius = torus->Internal().MajorRadius();
pcShape->setName(QString("Torus (r=%1/R=%2)").arg(minRadius,0,'f').arg(maxRadius,0,'f'));
CCVector3 Z = CCVector3(CA.getValue());
CCVector3 C = CCVector3(CC.getValue());
//construct remaining of base
CCVector3 X = Z.orthogonal();
CCVector3 Y = Z * X;
//we build matrix from these vecctors
ccGLMatrix glMat(X,Y,Z,C);
//torus primitive
prim = new ccTorus(maxRadius-minRadius,maxRadius+minRadius,M_PI*2.0,false,0,&glMat);
prim->setEnabled(false);
}
}
break;
}
//is there a primitive to add to part cloud?
if (prim)
{
prim->applyGLTransformation_recursive();
pcShape->addChild(prim);
prim->setDisplay(pcShape->getDisplay());
prim->setColor(col);
prim->showColors(true);
prim->setVisible(true);
}
if (!group)
{
group = new ccHObject(QString("Ransac Detected Shapes (%1)").arg(ent->getName()));
m_app->addToDB(group,true,0,false);
}
group->addChild(pcShape);
m_app->addToDB(pcShape,true,0,false);
count -= shapePointsCount;
QApplication::processEvents();
}
if (group)
{
assert(group->getChildrenNumber()!=0);
//we hide input cloud
pc->setEnabled(false);
m_app->dispToConsole("[qRansacSD] Input cloud has been automtically hidden!",ccMainAppInterface::WRN_CONSOLE_MESSAGE);
//we add new group to DB/display
group->setVisible(true);
group->setDisplay_recursive(pc->getDisplay());
group->prepareDisplayForRefresh_recursive();
m_app->refreshAll();
}
}
}
bool Cylinder::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 4)
return false;
// estimate axis from covariance of normal vectors
MiscLib::Vector< GfxTL::Vector3Df > normals;
size_t c = samples.size() / 2;
for(size_t i = c; i < samples.size(); ++i)
{
normals.push_back(GfxTL::Vector3Df(samples[i]));
normals.push_back(GfxTL::Vector3Df(-samples[i]));
}
GfxTL::MatrixXX< 3, 3, float > cov, eigenVectors;
GfxTL::Vector3Df eigenValues;
GfxTL::CovarianceMatrix(GfxTL::Vector3Df(0, 0, 0),
normals.begin(), normals.end(), &cov);
GfxTL::Jacobi(cov, &eigenValues, &eigenVectors);
// find the minimal eigenvalue and corresponding vector
float minEigVal = eigenValues[0];
unsigned int minEigIdx = 0;
for(unsigned int i = 1; i < 3; ++i)
if(eigenValues[i] < minEigVal)
{
minEigVal = eigenValues[i];
minEigIdx = i;
}
m_axisDir = Vec3f(eigenVectors[minEigIdx]);
// get a point on the axis from all pairs
m_axisPos = Vec3f(0, 0, 0);
m_radius = 0;
size_t pointCount = 0;
size_t pairCount = 0;
for(size_t i = 0; i < c - 1; ++i)
for(size_t j = i + 1; j < c; ++j)
{
// project first normal into plane
float l = m_axisDir.dot(samples[i + c]);
Vec3f xdir = samples[i + c] - l * m_axisDir;
xdir.normalize();
Vec3f ydir = m_axisDir.cross(xdir);
ydir.normalize();
// xdir is the x axis in the plane (y = 0) samples[i] is the origin
float lineBnx = ydir.dot(samples[j + c]);
if(abs(lineBnx) < .05f)
continue;
float lineBny = -xdir.dot(samples[j + c]);
// origin of lineB
Vec3f originB = samples[j] - samples[i];
float lineBOx = xdir.dot(originB);
float lineBOy = ydir.dot(originB);
float lineBd = lineBnx * lineBOx + lineBny * lineBOy;
// lineB in the plane complete
// point of intersection is y = 0 and x = lineBd / lineBnx
float radius = lineBd / lineBnx;
m_axisPos += samples[i] + radius * xdir;
m_radius += abs(radius);
m_radius += std::sqrt((radius - lineBOx) * (radius - lineBOx) + lineBOy * lineBOy);
++pointCount;
}
if(!pointCount)
return false;
m_axisPos /= pointCount;
m_radius /= pointCount * 2;
if(m_radius > 1e6)
return false;
// find point on axis closest to origin
float lambda = m_axisDir.dot(-m_axisPos);
m_axisPos = m_axisPos + lambda * m_axisDir;
m_hcs.FromNormal(m_axisDir);
m_angularRotatedRadians = 0;
return true;
}
void RansacShapeDetector::GenerateCandidates(
const IndexedOctreeType &globalOctree,
const MiscLib::Vector< ImmediateOctreeType * > &octrees,
const PointCloud &pc, ScoreVisitorT &scoreVisitor,
size_t currentSize, size_t numInvalid,
const MiscLib::Vector< double > &sampleLevelProbSum,
size_t *drawnCandidates,
MiscLib::Vector< std::pair< float, size_t > > *sampleLevelScores,
float *bestExpectedValue,
CandidatesType *candidates) const
{
size_t genCands = 0;
#ifdef DOPARALLEL
#pragma omp parallel
#endif
{
ScoreVisitorT scoreVisitorCopy(scoreVisitor);
#ifdef DOPARALLEL
#pragma omp for schedule(dynamic, 10) reduction(+:genCands)
#endif
for(int candIter = 0; candIter < 200; ++candIter)
{
// pick a sample level
double s = ((double)rand()) / (double)RAND_MAX;
size_t sampleLevel = 0;
for(; sampleLevel < sampleLevelProbSum.size() - 1; ++sampleLevel)
if(sampleLevelProbSum[sampleLevel] >= s)
break;
// draw samples on current sample level in octree
MiscLib::Vector< size_t > samples;
const IndexedOctreeType::CellType *node;
if(!DrawSamplesStratified(globalOctree, m_reqSamples, sampleLevel,
scoreVisitorCopy.GetShapeIndex(), &samples, &node))
continue;
++genCands;
// construct the candidates
size_t c = samples.size();
MiscLib::Vector< Vec3f > samplePoints(samples.size() << 1);
for(size_t i = 0; i < samples.size(); ++i)
{
samplePoints[i] = globalOctree.at(samples[i]).pos;
samplePoints[i + c] = globalOctree.at(samples[i]).normal;
}
// construct the different primitive shapes
PrimitiveShape *shape;
for(ConstructorsType::const_iterator i = m_constructors.begin(),
iend = m_constructors.end(); i != iend; ++i)
{
if((*i)->RequiredSamples() > samples.size()
|| !(shape = (*i)->Construct(samplePoints)))
continue;
// verify shape
std::pair< float, float > dn;
bool verified = true;
for(size_t i = 0; i < c; ++i)
{
shape->DistanceAndNormalDeviation(samplePoints[i], samplePoints[i + c], &dn);
if(!scoreVisitorCopy.PointCompFunc()(dn.first, dn.second))
{
verified = false;
break;
}
}
if(!verified)
{
shape->Release();
continue;
}
Candidate cand(shape, node->Level());
cand.Indices(new MiscLib::RefCounted< MiscLib::Vector< size_t > >);
cand.Indices()->Release();
shape->Release();
cand.ImproveBounds(octrees, pc, scoreVisitorCopy,
currentSize, m_options.m_bitmapEpsilon, 1);
if(cand.UpperBound() < m_options.m_minSupport)
{
#ifdef DOPARALLEL
#pragma omp critical
#endif
{
(*sampleLevelScores)[node->Level()].first += cand.ExpectedValue();
++(*sampleLevelScores)[node->Level()].second;
}
continue;
}
#ifdef DOPARALLEL
#pragma omp critical
#endif
{
(*sampleLevelScores)[node->Level()].first += cand.ExpectedValue();
++(*sampleLevelScores)[node->Level()].second;
candidates->push_back(cand);
if(cand.ExpectedValue() > *bestExpectedValue)
*bestExpectedValue = cand.ExpectedValue();
}
}
}
}
*drawnCandidates += genCands;
}
void RansacShapeDetector::UpdateLevelWeights(float factor,
const MiscLib::Vector< std::pair< float, size_t > > &levelScores,
MiscLib::Vector< double > *sampleLevelProbability) const
{
MiscLib::Vector< double > newSampleLevelProbability(
sampleLevelProbability->size());
double newSampleLevelProbabilitySum = 0;
for(size_t i = 0; i < newSampleLevelProbability.size(); ++i)
{
if((*sampleLevelProbability)[i] > 0)
newSampleLevelProbability[i] = (levelScores[i].first / (*sampleLevelProbability)[i]);
else
newSampleLevelProbability[i] = 0;
newSampleLevelProbabilitySum += newSampleLevelProbability[i];
}
double newSum = 0;
for(size_t i = 0; i < newSampleLevelProbability.size(); ++i)
{
newSampleLevelProbability[i] = .9f * newSampleLevelProbability[i] + .1f * newSampleLevelProbabilitySum/levelScores.size();
newSum += newSampleLevelProbability[i];
}
for(size_t i = 0; i < sampleLevelProbability->size(); ++i)
{
(*sampleLevelProbability)[i] = (1.f - factor) * (*sampleLevelProbability)[i] +
factor * (newSampleLevelProbability[i] / newSum);
}
}
bool RansacShapeDetector::FindBestCandidate(CandidatesType &candidates,
const MiscLib::Vector< ImmediateOctreeType * > &octrees, const PointCloud &pc,
ScoreVisitorT &scoreVisitor, size_t currentSize,
size_t drawnCandidates, size_t numInvalid, size_t minSize, float numLevels,
float *maxForgottenCandidate, float *candidateFailProb) const
{
if(!candidates.size())
return false;
size_t maxImproveSubsetDuringMaxSearch = octrees.size();
// sort by expected value
std::sort(candidates.begin(), candidates.end());
// check if max is smaller than forgotten candidate
if(candidates.size() && candidates.back().ExpectedValue() < *maxForgottenCandidate)
{
// drawn candidates is wrong!
// need to correct the value
drawnCandidates = std::max(candidates.size(), (size_t)1);
*maxForgottenCandidate = 0;
}
MiscLib::Vector< Candidate * > candHeap;
for(size_t i = candidates.size() - 1; i != -1; --i)
{
if(CandidateFailureProbability(
candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels) > m_options.m_probability)
break;
candHeap.push_back(&candidates[i]);
}
if(!candHeap.size())
{
return false;
}
std::make_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
MiscLib::Vector< Candidate * > beatenCands;
Candidate *trial = candHeap.front();
std::pop_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
candHeap.pop_back();
float bestCandidateFailureProbability;
while(candHeap.size())
{
if(trial->IsEquivalent(*candHeap.front(), pc, m_options.m_epsilon,
m_options.m_normalThresh))
{
std::pop_heap(candHeap.begin(), candHeap.end(),
CandidateHeapPred());
candHeap.pop_back();
continue;
}
bool isEquivalent = false;
for(size_t j = 0; j < beatenCands.size(); ++j)
{
if(beatenCands[j]->IsEquivalent(*candHeap.front(), pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
isEquivalent = true;
break;
}
}
if(isEquivalent)
{
std::pop_heap(candHeap.begin(), candHeap.end(),
CandidateHeapPred());
candHeap.pop_back();
continue;
}
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
while((bestCandidateFailureProbability <= m_options.m_probability)
&& (*trial >= *candHeap.front())
&& (trial->UpperBound() >= minSize)
&& trial->ImproveBounds(octrees, pc, scoreVisitor, currentSize,
m_options.m_bitmapEpsilon, octrees.size()))
{
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize
&& trial->ComputedSubsets() >= octrees.size()
&& *trial >= *candHeap.front())
break;
if(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize)
{
candHeap.push_back(trial);
std::push_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
}
else if((int)trial->ComputedSubsets()
> std::max(2, ((int)octrees.size()) - 2))
beatenCands.push_back(trial);
//nextCandidate
trial = candHeap.front();
std::pop_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
candHeap.pop_back();
}
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
while(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize
&& trial->ImproveBounds(octrees, pc, scoreVisitor, currentSize,
m_options.m_bitmapEpsilon, octrees.size()))
{
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if((bestCandidateFailureProbability > m_options.m_probability
|| trial->UpperBound() < minSize)
&& (!m_autoAcceptSize || trial->UpperBound() < m_autoAcceptSize))
{
return false;
}
std::sort(candidates.begin(), candidates.end());
size_t bestCandidate = candidates.size() - 1;
bestCandidateFailureProbability = CandidateFailureProbability(
candidates.back().ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
for(size_t i = bestCandidate - 1; i != -1; --i)
{
float iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(iFailProb > m_options.m_probability || candidates[i].UpperBound() < minSize
|| candidates[i].UpperBound() < candidates[bestCandidate].LowerBound())
break;
// check if this is an identical candidate
if(candidates[bestCandidate].IsEquivalent(candidates[i], pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
continue;
}
bool isEquivalent = false;
for(size_t j = 0; j < beatenCands.size(); ++j)
{
if(beatenCands[j]->IsEquivalent(candidates[i], pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
isEquivalent = true;
break;
}
}
if(isEquivalent)
{
continue;
}
do
{
if(candidates[i].UpperBound() > candidates[bestCandidate].UpperBound()
&& candidates[i].LowerBound() < candidates[bestCandidate].LowerBound())
{
bool dontBreak = candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
else if(candidates[bestCandidate].UpperBound() > candidates[i].UpperBound()
&& candidates[bestCandidate].LowerBound() < candidates[i].LowerBound())
{
bool dontBreak = candidates[bestCandidate].ImproveBounds(octrees, pc,
scoreVisitor, currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
else
{
bool dontBreak = candidates[bestCandidate].ImproveBounds(octrees, pc,
scoreVisitor, currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
dontBreak = candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch)
|| dontBreak;
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
}
while(bestCandidateFailureProbability <= m_options.m_probability
&& iFailProb <= m_options.m_probability
&& candidates[i].UpperBound() >= minSize
&& candidates[bestCandidate].UpperBound() >= minSize
&& candidates[i].UpperBound() > candidates[bestCandidate].LowerBound()
&& candidates[i].LowerBound() < candidates[bestCandidate].UpperBound()
);
if((
candidates[i] > candidates[bestCandidate]
|| bestCandidateFailureProbability > m_options.m_probability
|| candidates[bestCandidate].UpperBound() < minSize)
&& (iFailProb <= m_options.m_probability && candidates[i].UpperBound() >= minSize))
{
while(iFailProb <= m_options.m_probability && candidates[i].UpperBound() >= minSize
&& candidates[i] > candidates[bestCandidate]
&& candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon, octrees.size()))
{
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if(candidates[i] > candidates[bestCandidate])
{
beatenCands.push_back(&candidates[bestCandidate]);
bestCandidate = i;
bestCandidateFailureProbability = iFailProb;
}
else
beatenCands.push_back(&candidates[i]);
}
else
beatenCands.push_back(&candidates[i]);
if(bestCandidateFailureProbability > m_options.m_probability
|| candidates[bestCandidate].UpperBound() < minSize)
break;
} // end for
while(candidates[bestCandidate].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon, octrees.size()));
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if((bestCandidateFailureProbability <= m_options.m_probability
&& candidates[bestCandidate].UpperBound() >= minSize)
|| (m_autoAcceptSize && candidates[bestCandidate].UpperBound() >= m_autoAcceptSize))
{
std::swap(candidates.back(), candidates[bestCandidate]);
*candidateFailProb = bestCandidateFailureProbability;
return true;
}
std::sort(candidates.begin(), candidates.end()/*, std::greater< Candidate >()*/);
return false;
}
bool Plane::Init(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 6)
return false;
return Init(samples[0], samples[1], samples[2]);
}
void PreWrappedComponents(const MiscLib::Vector< char > &bitmap, size_t uextent,
size_t vextent, MiscLib::Vector< int > *preWrappedComponentsImg,
MiscLib::Vector< int > *relabelComponentsImg,
const MiscLib::Vector< std::pair< int, size_t > > &inLabels,
MiscLib::Vector< std::pair< int, size_t > > *labels)
{
MiscLib::Vector< std::pair< int, size_t > > tempLabels(inLabels);
tempLabels.reserve(bitmap.size() / 2 + 1); // this is the maximum of possible tempLabels
if(!tempLabels.size())
tempLabels.push_back(std::make_pair(0, size_t(0)));
int curLabel = tempLabels.size() - 1;
size_t prevRow, row = 0, nextRow = uextent;
for(size_t j = 1; j < vextent - 1; ++j)
{
prevRow = row;
row = nextRow;
nextRow = row + uextent;
for(size_t i = 1; i < uextent - 1; ++i)
{
if(!bitmap[row + i])
{
(*preWrappedComponentsImg)[row + i] = 0;
++tempLabels[0].second;
continue;
}
// get neighborhood
int n[8];
n[0] = (*preWrappedComponentsImg)[prevRow + i - 1];
n[1] = (*preWrappedComponentsImg)[prevRow + i];
n[2] = (*preWrappedComponentsImg)[prevRow + i + 1];
n[3] = (*preWrappedComponentsImg)[row + i - 1];
n[4] = (*preWrappedComponentsImg)[row + i + 1];
n[5] = (*preWrappedComponentsImg)[nextRow + i - 1];
n[6] = (*preWrappedComponentsImg)[nextRow + i];
n[7] = (*preWrappedComponentsImg)[nextRow + i + 1];
(*preWrappedComponentsImg)[row + i] = Label(n, 8,
&curLabel, &tempLabels);
}
}
// reduce the tempLabels
for(size_t i = tempLabels.size() - 1; i > 0; --i)
tempLabels[i].first = ReduceLabel(i, tempLabels);
MiscLib::Vector< int > condensed(tempLabels.size());
labels->clear();
labels->reserve(condensed.size());
int count = 0;
for(size_t i = 0; i < tempLabels.size(); ++i)
if(i == (size_t)tempLabels[i].first)
{
labels->push_back(std::make_pair(count, tempLabels[i].second));
condensed[i] = count;
++count;
}
else
(*labels)[condensed[tempLabels[i].first]].second
+= tempLabels[i].second;
// set new component ids
for(size_t i = 0; i < preWrappedComponentsImg->size(); ++i)
(*preWrappedComponentsImg)[i] =
condensed[tempLabels[(*preWrappedComponentsImg)[i]].first];
for(size_t i = 0; i < relabelComponentsImg->size(); ++i)
(*relabelComponentsImg)[i] =
condensed[tempLabels[(*relabelComponentsImg)[i]].first];
}
void ErodeCross(const MiscLib::Vector< char > &bitmap,
size_t uextent, size_t vextent, bool uwrap, bool vwrap,
MiscLib::Vector< char > *eroded)
{
// first pixel is special
(*eroded)[0] = bitmap[0] && bitmap[1] &&
bitmap[uextent];
if(vwrap)
(*eroded)[0] = (*eroded)[0] &&
bitmap[(vextent - 1) * uextent];
if(uwrap)
(*eroded)[0] = (*eroded)[0] && bitmap[uextent - 1];
// first row is special
if(!vwrap)
{
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[i] = bitmap[i - 1] && bitmap[i] && bitmap[i + 1] &&
bitmap[uextent + i];
}
}
else
{
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[i] = bitmap[i - 1] && bitmap[i] && bitmap[i + 1] &&
bitmap[uextent + i] && bitmap[(vextent - 1) * uextent + i];
}
}
// last pixel of first row is special
(*eroded)[uextent - 1] = bitmap[uextent - 1] && bitmap[uextent - 2] &&
bitmap[2 * uextent - 1];
if(vwrap)
(*eroded)[uextent - 1] = (*eroded)[uextent - 1] &&
bitmap[vextent * uextent - 1];
if(uwrap)
(*eroded)[uextent - 1] = (*eroded)[uextent - 1] && bitmap[0];
size_t row = 0, prevRow, nextRow = uextent;
for(size_t j = 1; j < vextent - 1; ++j)
{
prevRow = row;
row = nextRow;
nextRow = row + uextent;
// first pixel in row is special
(*eroded)[row] = bitmap[prevRow] &&
bitmap[row] && bitmap[row + 1] && bitmap[nextRow];
if(uwrap)
(*eroded)[row] = (*eroded)[row] && bitmap[nextRow - 1];
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[row + i] = bitmap[prevRow + i ] &&
bitmap[row + i - 1] && bitmap[row + i] &&
bitmap[row + i + 1] && bitmap[nextRow + i];
}
// last pixel in row is special
(*eroded)[row + uextent - 1] = bitmap[prevRow + uextent - 1] &&
bitmap[row + uextent - 2] && bitmap[row + uextent - 1] &&
bitmap[nextRow + uextent - 1];
if(uwrap)
(*eroded)[row + uextent - 1] = (*eroded)[row + uextent - 1] &&
bitmap[row];
}
// first pixel of last row is special
(*eroded)[(vextent - 1) * uextent] = bitmap[(vextent - 1) * uextent] &&
bitmap[(vextent - 1) * uextent + 1] &&
bitmap[(vextent - 2) * uextent];
if(vwrap)
(*eroded)[(vextent - 1) * uextent] =
(*eroded)[(vextent - 1) * uextent] && bitmap[0];
if(uwrap)
(*eroded)[(vextent - 1) * uextent] =
(*eroded)[(vextent - 1) * uextent] &&
bitmap[vextent * uextent - 1];
// last row is special
if(!vwrap)
{
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[(vextent - 1) * uextent + i] =
bitmap[(vextent - 1) * uextent + i] &&
bitmap[(vextent - 1) * uextent + i - 1] &&
bitmap[(vextent - 1) * uextent + i + 1] &&
bitmap[(vextent - 2) * uextent + i];
}
}
else
{
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[(vextent - 1) * uextent + i] =
bitmap[(vextent - 1) * uextent + i] &&
bitmap[(vextent - 1) * uextent + i - 1] &&
bitmap[(vextent - 1) * uextent + i + 1] &&
bitmap[(vextent - 2) * uextent + i] &&
bitmap[i];
}
}
// last pixel
(*eroded)[bitmap.size() - 1] = bitmap[bitmap.size() - 1] &&
bitmap[bitmap.size() - 2] && bitmap[bitmap.size() - uextent - 1];
if(vwrap)
(*eroded)[bitmap.size() - 1] = (*eroded)[bitmap.size() - 1] &&
bitmap[uextent - 1];
if(uwrap)
(*eroded)[bitmap.size() - 1] = (*eroded)[bitmap.size() - 1] &&
bitmap[bitmap.size() - uextent];
}
void Components(const MiscLib::Vector< char > &bitmap,
size_t uextent, size_t vextent, bool uwrap, bool vwrap,
MiscLib::Vector< int > *componentsImg,
MiscLib::Vector< std::pair< int, size_t > > *labels)
{
componentsImg->resize(uextent * vextent);
MiscLib::Vector< std::pair< int, size_t > > tempLabels;
tempLabels.reserve(componentsImg->size() / 2 + 1); // this is the maximum of possible tempLabels
tempLabels.push_back(std::make_pair(0, size_t(0)));
// use an eight neighborhood
// first row is special
// first pixel is special
// wrapping does not make sense in the first row: no pixels have been
// assigned components yet
int curLabel = 0;
if(bitmap[0])
{
(*componentsImg)[0] = ++curLabel;
tempLabels.push_back(std::make_pair(curLabel, size_t(1)));
}
else
{
(*componentsImg)[0] = 0;
++tempLabels[0].second;
}
// handle first row
for(size_t i = 1; i < uextent; ++i)
{
// in the first row only the previous pixel has to be considered
if(bitmap[i])
{
if((*componentsImg)[i - 1])
{
(*componentsImg)[i] = (*componentsImg)[i - 1];
++tempLabels[(*componentsImg)[i]].second;
}
else
{
(*componentsImg)[i] = ++curLabel;
tempLabels.push_back(std::make_pair(curLabel, size_t(1)));
}
}
else
{
(*componentsImg)[i] = 0;
++tempLabels[0].second;
}
}
size_t prevRow, row = 0;
size_t jend = vwrap? vextent - 1 : vextent;
for(size_t j = 1; j < jend; ++j)
{
prevRow = row;
row = prevRow + uextent;
// first pixel in row gets different treatment
if(bitmap[row])
{
if((*componentsImg)[prevRow]) // pixel above in component?
{
(*componentsImg)[row] = (*componentsImg)[prevRow];
++tempLabels[(*componentsImg)[row]].second;
}
else
{
int n[2];
n[0] = (*componentsImg)[prevRow + 1];
if(uwrap)
n[1] = (*componentsImg)[prevRow + uextent - 1];
(*componentsImg)[row] = Label(n, uwrap? 2 : 1, &curLabel,
&tempLabels);
}
}
else
{
(*componentsImg)[row] = 0;
++tempLabels[0].second;
}
for(size_t i = 1; i < uextent - 1; ++i)
{
if(!bitmap[row + i])
{
(*componentsImg)[row + i] = 0;
++tempLabels[0].second;
continue;
}
int n[4];
n[0] = (*componentsImg)[row + i - 1];
n[1] = (*componentsImg)[prevRow + i - 1];
n[2] = (*componentsImg)[prevRow + i];
n[3] = (*componentsImg)[prevRow + i + 1];
(*componentsImg)[row + i] = Label(n, 4, &curLabel, &tempLabels);
}
// last pixel in the row
if(!bitmap[row + uextent - 1])
{
(*componentsImg)[row + uextent - 1] = 0;
++tempLabels[0].second;
continue;
}
int n[5];
n[0] = (*componentsImg)[row + uextent - 2];
n[1] = (*componentsImg)[prevRow + uextent - 2];
n[2] = (*componentsImg)[prevRow + uextent - 1];
if(uwrap)
{
n[3] = (*componentsImg)[prevRow];
n[4] = (*componentsImg)[row];
}
(*componentsImg)[row + uextent - 1] = Label(n, uwrap? 5 : 3, &curLabel,
&tempLabels);
}
// last row
if(vwrap) // in case of vwrapping the last row is computed with almost full
// neighborhood
{
prevRow = (vextent - 2) * uextent;
row = (vextent - 1) * uextent;
// first pixel
if(bitmap[row])
{
int n[6];
n[0] = (*componentsImg)[prevRow];
n[1] = (*componentsImg)[prevRow + 1];
n[2] = (*componentsImg)[0];
n[3] = (*componentsImg)[1];
if(uwrap)
{
n[4] = (*componentsImg)[prevRow + uextent - 1];
n[5] = (*componentsImg)[uextent - 1];
}
(*componentsImg)[row] = Label(n, uwrap? 6 : 4, &curLabel,
&tempLabels);
}
else
{
(*componentsImg)[row] = 0;
++tempLabels[0].second;
}
for(size_t i = 1; i < uextent - 1; ++i)
{
if(!bitmap[row + i])
{
(*componentsImg)[row + i] = 0;
++tempLabels[0].second;
continue;
}
int n[7];
n[0] = (*componentsImg)[row + i - 1];
n[1] = (*componentsImg)[prevRow + i - 1];
n[2] = (*componentsImg)[prevRow + i];
n[3] = (*componentsImg)[prevRow + i + 1];
n[4] = (*componentsImg)[i - 1];
n[5] = (*componentsImg)[i];
n[6] = (*componentsImg)[i + 1];
(*componentsImg)[row + i] = Label(n, 7, &curLabel, &tempLabels);
}
// last pixel
if(!bitmap[row + uextent - 1])
{
(*componentsImg)[row + uextent - 1] = 0;
++tempLabels[0].second;
}
else
{
int n[8];
n[0] = (*componentsImg)[row + uextent - 2];
n[1] = (*componentsImg)[prevRow + uextent - 2];
n[2] = (*componentsImg)[prevRow + uextent - 1];
n[3] = (*componentsImg)[uextent - 2];
n[4] = (*componentsImg)[uextent - 1];
if(uwrap)
{
n[5] = (*componentsImg)[prevRow];
n[6] = (*componentsImg)[row];
n[7] = (*componentsImg)[0];
}
(*componentsImg)[row + uextent - 1] = Label(n, uwrap? 8 : 5,
&curLabel, &tempLabels);
}
}
// reduce the tempLabels
for(size_t i = tempLabels.size() - 1; i > 0; --i)
tempLabels[i].first = ReduceLabel(i, tempLabels);
MiscLib::Vector< int > condensed(tempLabels.size());
labels->clear();
labels->reserve(condensed.size());
int count = 0;
for(size_t i = 0; i < tempLabels.size(); ++i)
if(i == (size_t)tempLabels[i].first)
{
labels->push_back(std::make_pair(count, tempLabels[i].second));
condensed[i] = count;
++count;
}
else
(*labels)[condensed[tempLabels[i].first]].second
+= tempLabels[i].second;
// set new component ids
for(size_t i = 0; i < componentsImg->size(); ++i)
(*componentsImg)[i] =
condensed[tempLabels[(*componentsImg)[i]].first];
}
size_t BitmapPrimitiveShape::ConnectedComponent(
const PointCloud &pc, float epsilon,
std::shared_ptr<MiscLib::Vector< size_t > >indices, bool doFiltering, float* borderRatio )
{
MiscLib::Vector< int > componentsImg;
MiscLib::Vector< std::pair< int, size_t > > labels;
BitmapInfo bitmapInfo;
if( AllConnectedComponents( pc, epsilon, bitmapInfo, indices, componentsImg, labels, doFiltering ) <= 1 )
return 0;
size_t size = indices->size();
MiscLib::Vector< size_t >::iterator begin = indices->begin();
// find the largest component
size_t maxComp = 1;
for(size_t i = 2; i < labels.size(); ++i)
if(labels[maxComp].second < labels[i].second)
maxComp = i;
GfxTL::AABox< GfxTL::Vector2Df > bbox;
bbox.Min() = GfxTL::Vector2Df(
std::numeric_limits< float >::infinity(),
std::numeric_limits< float >::infinity());
bbox.Max() = -bbox.Min();
// compute bbox and update indices
size_t offset = 0;
for(size_t i = 0; i < size; ++i)
{
if(componentsImg[bitmapInfo.bmpIdx[i]] == labels[maxComp].first)
{
std::swap(begin[offset], begin[i]);
offset++;
// update bounding box
if(bbox.Min()[0] > bitmapInfo.params[i].first)
bbox.Min()[0] = bitmapInfo.params[i].first;
if(bbox.Max()[0] < bitmapInfo.params[i].first)
bbox.Max()[0] = bitmapInfo.params[i].first;
if(bbox.Min()[1] > bitmapInfo.params[i].second)
bbox.Min()[1] = bitmapInfo.params[i].second;
if(bbox.Max()[1] < bitmapInfo.params[i].second)
bbox.Max()[1] = bitmapInfo.params[i].second;
}
}
// ratio between border and connected-comp size should be calculated if borderRatio is a valid pointer
if( borderRatio )
{
int borderPixels = 0;
int maxLabel = labels[maxComp].first;
int row = bitmapInfo.uextent;
int pos = 0;
char numNeighbours = 0;
int ccSize = 0;
// test neightbourhood for all bitmappixels that are not marginal
for( size_t v = 1; v < bitmapInfo.vextent-1; ++v )
{
for( size_t u = 1; u < bitmapInfo.uextent-1; ++u )
{
pos = row + u;
if( componentsImg[pos] == maxLabel )
{
ccSize++;
numNeighbours = bitmapInfo.bitmap[pos-1] + bitmapInfo.bitmap[pos+1] +
bitmapInfo.bitmap[pos-bitmapInfo.uextent-1] + bitmapInfo.bitmap[pos-bitmapInfo.uextent+1] +
bitmapInfo.bitmap[pos+bitmapInfo.uextent-1] + bitmapInfo.bitmap[pos+bitmapInfo.uextent+1] +
bitmapInfo.bitmap[pos-bitmapInfo.uextent] + bitmapInfo.bitmap[pos+bitmapInfo.uextent];
if( (int)numNeighbours != 8 )
++borderPixels;
}
}
row += bitmapInfo.uextent;
}
// check left/right margins
row = bitmapInfo.uextent;
for( size_t v = 1; v < bitmapInfo.vextent-1; ++v )
{
ccSize++;
if( componentsImg[row] == maxLabel )
++borderPixels;
ccSize++;
if( componentsImg[row+bitmapInfo.uextent-1] == maxLabel )
++borderPixels;
row += bitmapInfo.uextent;
}
// check top/bottom margins
row = ( bitmapInfo.vextent-1 ) * bitmapInfo.uextent;
for( size_t u = 0; u < bitmapInfo.uextent; ++u )
{
ccSize++;
if( componentsImg[u] == maxLabel )
++borderPixels;
ccSize++;
if( componentsImg[row + u] == maxLabel )
++borderPixels;
}
*borderRatio = static_cast<float>( borderPixels ) / static_cast<float>( ccSize );
}
m_extBbox = bbox;
return offset;
}
bool Cone::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
// setup all the planes
size_t c = samples.size() / 2;
MiscLib::Vector< GfxTL::Vector4Df > planes(c);
#pragma omp parallel for schedule(static)
for(size_t i = 0; i < c; ++i)
{
for(unsigned int j = 0; j < 3; ++j)
planes[i][j] = samples[i][j];
planes[i][3] = samples[i].dot(samples[i + c]);
}
// compute center by intersecting the three planes given by (p1, n1)
// (p2, n2) and (p3, n3)
// set up linear system
double a[4 * 3];
double d1 = samples[0].dot(samples[c + 0]);
double d2 = samples[1].dot(samples[c + 1]);
double d3 = samples[2].dot(samples[c + 2]);
// column major
a[0 + 0 * 3] = samples[c + 0][0];
a[1 + 0 * 3] = samples[c + 1][0];
a[2 + 0 * 3] = samples[c + 2][0];
a[0 + 1 * 3] = samples[c + 0][1];
a[1 + 1 * 3] = samples[c + 1][1];
a[2 + 1 * 3] = samples[c + 2][1];
a[0 + 2 * 3] = samples[c + 0][2];
a[1 + 2 * 3] = samples[c + 1][2];
a[2 + 2 * 3] = samples[c + 2][2];
a[0 + 3 * 3] = d1;
a[1 + 3 * 3] = d2;
a[2 + 3 * 3] = d3;
if(dmat_solve(3, 1, a))
return false;
m_center[0] = a[0 + 3 * 3];
m_center[1] = a[1 + 3 * 3];
m_center[2] = a[2 + 3 * 3];
LevMarPlaneDistance planeDistance;
LevMar(planes.begin(), planes.end(), planeDistance,
(float *)m_center);
MiscLib::Vector< GfxTL::Vector3Df > spoints(c);
#pragma omp parallel for schedule(static)
for(size_t i = 0; i < c; ++i)
{
spoints[i] = GfxTL::Vector3Df(samples[i] - m_center);
spoints[i].Normalize();
}
GfxTL::Vector3Df axisDir;
GfxTL::MeanOfNormals(spoints.begin(), spoints.end(), &axisDir);
m_axisDir = GfxTL::Vector3Df(axisDir);
// make sure axis points in good direction
// the axis is defined to point into the interior of the cone
float heightSum = 0;
#pragma omp parallel for schedule(static) reduction(+:heightSum)
for(size_t i = 0; i < c; ++i)
heightSum += Height(samples[i]);
if(heightSum < 0)
m_axisDir *= -1;
float angleReduction = 0;
#pragma omp parallel for schedule(static) reduction(+:angleReduction)
for(size_t i = 0; i < c; ++i)
{
float angle = m_axisDir.dot(samples[i + c]);
if(angle < -1) // clamp angle to [-1, 1]
angle = -1;
else if(angle > 1)
angle = 1;
if(angle < 0)
// m_angle = omega + 90
angle = std::acos(angle) - float(M_PI) / 2;
else
// m_angle = 90 - omega
angle = float(M_PI) / 2 - std::acos(angle);
angleReduction += angle;
}
angleReduction /= c;
m_angle = angleReduction;
if(m_angle < 1.0e-6 || m_angle > float(M_PI) / 2 - 1.0e-6)
return false;
//if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
if(m_angle > 1.4835298641951801403851371532153f) // 85 degrees
return false;
m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
m_normalY = m_normal[1] * m_axisDir;
m_n2d[0] = std::cos(m_angle);
m_n2d[1] = -std::sin(m_angle);
m_hcs.FromNormal(m_axisDir);
m_angularRotatedRadians = 0;
return true;
}
// finds the loops around a connected component as polygons
void ComponentLoops(const MiscLib::Vector< int > &componentImg, size_t uextent,
size_t vextent, int label, bool uwrap, bool vwrap,
MiscLib::Vector< MiscLib::Vector< GfxTL::VectorXD< 2, size_t > > > *polys)
{
typedef GfxTL::VectorXD< 2, size_t > Vec2;
// find first point of component
size_t firsti = 0;
int x, y, prevx, prevy;
// the corners of our pixels will be the vertices of our polygons
// (x, y) is the upper left corner of the pixel y * uextent + x
HashGrid< bool, 4 > edges;
unsigned int edgesExtent[] = { uextent + 1, vextent + 1, 3, 3 };
edges.Extent(edgesExtent);
bool prevPixelWasWhite = true;
do
{
// find the first edge in the polygon
// edges are oriented so that the "black" pixels are on the right
// black pixels are pixels == label
for(; firsti < componentImg.size(); ++firsti)
{
if(prevPixelWasWhite && componentImg[firsti] == label)
{
prevPixelWasWhite = false;
x = firsti % uextent;
y = firsti / uextent;
break;
}
prevPixelWasWhite = componentImg[firsti] != label;
}
if(firsti >= componentImg.size()) // unable to find a pixel -> good bye
{
// if there is a uwrap, then the last row could be an outer loop
// this outer loop could be missed of all pixels in the last
// row are black
// to find that out we spawn another trial at the first
// pixel in the last row (if it is black)
// if the loop has already been detected, than this
// edge should already be in edges
if(!uwrap)
break;
if(componentImg[(vextent - 1) * uextent] == label)
{
x = 0;
y = vextent - 1;
}
}
MiscLib::Vector< Vec2 > poly;
// we initialize the path with an oriented edge
// since the black pixel is on the right the edge goes from
// bottom to top, i.e. from (x, y + 1) to (x, y)
if((x > 0 && (size_t)y < vextent - 1)
|| (!uwrap && !vwrap) || (vwrap && !uwrap && y == 0))
{
// on the left of pixel
// check if edge was visited already
unsigned int edgeIndex[] = { x, y, 1, 2 };
if(edges.find(edgeIndex))
continue;
prevx = 0;
prevy = 1;
}
else if(uwrap && !vwrap && x == 0 && (size_t)y != vextent - 1)
{
size_t dx, dy;
if(!IsEdge(componentImg, uextent, vextent, label, uwrap, vwrap,
x, y, 1, 0, &dx, &dy))
continue;
// check if edge was visited already
unsigned int edgeIndex[] = { x + 1, y, 0, 1 };
if(edges.find(edgeIndex))
continue;
// on top of pixel
prevx = -1;
prevy = 0;
++x;
}
else if(uwrap && !vwrap && x == 0 && (size_t)y == vextent - 1)
{
size_t dx, dy;
if(!IsEdge(componentImg, uextent, vextent, label, uwrap, vwrap,
x + 1, y + 1, -1, 0, &dx, &dy))
continue;
// on bottom of pixel
// check if edge was visited already
unsigned int edgeIndex[] = { x + 1, y + 1, 0, 1 };
if(edges.find(edgeIndex))
continue;
prevx = -1;
prevy = 0;
++y;
}
else if(!uwrap && vwrap && (size_t)x == uextent - 1)
{
// on right of pixel
size_t dx, dy;
if(!IsEdge(componentImg, uextent, vextent, label, uwrap, vwrap,
x + 1, y + 1, 0, -1, &dx, &dy))
continue;
// on bottom of pixel
// check if edge was visited already
unsigned int edgeIndex[] = { x + 1, y + 1, 1, 0 };
if(edges.find(edgeIndex))
continue;
prevx = 0;
prevy = 1;
++y;
}
else
continue; // we are unable to start a loop at this position
poly.push_back(Vec2(x + prevx, y + prevy));
edges[x][y][prevx + 1][prevy + 1] = true;
do
{
poly.push_back(Vec2(x, y));
// check the four neighbors of (x, y) from left to right
// starting from where we came from
// we take the first edge that we encounter
size_t nextx, nexty;
size_t checkEdge;
for(checkEdge = 0; checkEdge < 3; ++checkEdge)
{
std::swap(prevx, prevy);
prevx *= -1;
if(IsEdge(componentImg, uextent, vextent, label, uwrap, vwrap,
x, y, prevx, prevy, &nextx, &nexty))
break;
}
if(checkEdge > 3)
return;
x = nextx;
y = nexty;
prevx = -prevx;
prevy = -prevy;
edges[x][y][prevx + 1][prevy + 1] = true;
}
while(poly[0] != Vec2(x, y));
polys->push_back(poly);
}
while(firsti < componentImg.size());
#ifdef _DEBUG
static int fname_int = 0;
std::ostringstream fn;
fn << "ComponentLoopsInput" << fname_int << ".txt";
std::ofstream file;
file.open(fn.str().c_str(), std::ios::out);
for(size_t j = 0; j < vextent; ++j)
{
for(size_t i = 0; i < uextent; ++i)
file /*<< std::setw(3)*/ << componentImg[j * uextent + i]/* << " "*/;
file << std::endl;
}
file.close();
MiscLib::Vector< int > loopsImg((uextent + 1) * (vextent + 1), 0);
std::ostringstream fn2;
fn2 << "ComponentLoopsOutput" << fname_int++ << ".txt";
for(size_t i = 0; i < polys->size(); ++i)
for(size_t j = 0; j < (*polys)[i].size(); ++j)
loopsImg[(*polys)[i][j][1] * (uextent + 1) + (*polys)[i][j][0]] = i + 1;
file.open(fn2.str().c_str(), std::ios::out);
for(size_t j = 0; j < vextent + 1; ++j)
{
for(size_t i = 0; i < uextent + 1; ++i)
file /*<< std::setw(3)*/ << loopsImg[j * (uextent + 1) + i]/* << " "*/;
file << std::endl;
}
file.close();
#endif
}