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;
}
}
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;
}
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];
}
// 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
}
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;
}
