void sure::keypoints::allocateEntropyPayload(Octree& octree, Scalar samplingrate, sure::memory::FixedSizeAllocatorWithDirectAccess<EntropyPayload>& allocator)
{
unsigned depth = octree.getDepth(samplingrate);
allocator.resizeIfSmaller(octree[depth].size());
for(unsigned int i=0; i<octree[depth].size(); ++i)
{
Node* node = octree[depth][i];
EntropyPayload* payload = allocator.allocate(i);
node->setOptionalPayload(static_cast<sure::payload::Payload*>(payload));
}
}
unsigned sure::keypoints::extractKeypoints(Octree& octree, Scalar samplingrate, Scalar searchRadius, Scalar featureRadius, std::vector<Feature>& features, std::vector<Node*>& keypointNodes)
{
unsigned samplingDepth = octree.getDepth(samplingrate);
unsigned keypoints(0);
for(unsigned int i=0; i<octree[samplingDepth].size(); ++i)
{
Node* currNode = octree[samplingDepth][i];
EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt());
if( payload->flag_ != POSSIBLE )
{
continue;
}
NodeVector neighbors = octree.getNodes(currNode, octree.getUnitSize(searchRadius));
for(unsigned int j=0; j<neighbors.size(); ++j)
{
Node* currNeighbor = neighbors[j];
EntropyPayload* neighborPayload = static_cast<EntropyPayload*>(currNeighbor->opt());
if( neighborPayload->flag_ == IS_MAXIMUM )
{
payload->flag_ = SUPPRESSED;
break;
}
else if( neighborPayload->flag_ == POSSIBLE )
{
if( payload->entropy_ < neighborPayload->entropy_ )
{
payload->flag_ = SUPPRESSED;
break;
}
}
else
{
continue;
}
}
if( payload->flag_ == POSSIBLE )
{
payload->flag_ = IS_MAXIMUM;
sure::feature::Feature f;
f.radius() = featureRadius;
f.position() = currNode->fixed().getMeanPosition();
features.push_back(f);
keypointNodes.push_back(currNode);
keypoints++;
}
}
return keypoints;
}
void sure::keypoints::resetFeatureFlags(Octree& octree, Scalar samplingrate)
{
unsigned depth = octree.getDepth(samplingrate);
for(unsigned int i=0; i<octree[depth].size(); ++i)
{
Node* node = octree[depth][i];
EntropyPayload* payload = static_cast<EntropyPayload*>(node->opt());
if( payload->flag_ < DISTANCE_TOO_HIGH )
{
payload->flag_ = NOT_CALCULATED;
}
}
}
void sure::keypoints::flagBackgroundPoints(Octree& octree, Scalar samplingrate)
{
unsigned depth = octree.getDepth(samplingrate);
for(unsigned int i=0; i<octree[depth].size(); ++i)
{
Node* node = octree[depth][i];
EntropyPayload* payload = static_cast<EntropyPayload*>(node->opt());
if( ((node->fixed().getPointFlag() & BACKGROUND_BORDER) == BACKGROUND_BORDER) )
{
payload->flag_ = BACKGROUND_EDGE;
continue;
}
}
}
void sure::keypoints::flagArtificialPoints(Octree& octree, Scalar samplingrate)
{
unsigned depth = octree.getDepth(samplingrate);
for(unsigned int i=0; i<octree[depth].size(); ++i)
{
Node* node = octree[depth][i];
EntropyPayload* payload = static_cast<EntropyPayload*>(node->opt());
if( node->fixed().getPointFlag() == ARTIFICIAL )
{
payload->flag_ = ARTIFICIAL_POINTS;
continue;
}
}
}
void sure::keypoints::flagDistantPoints(Octree& octree, Scalar samplingrate, Scalar threshold, const Vector3& sensorPosition)
{
unsigned depth = octree.getDepth(samplingrate);
threshold *= threshold;
for(unsigned int i=0; i<octree[depth].size(); ++i)
{
Node* node = octree[depth][i];
EntropyPayload* payload = static_cast<EntropyPayload*>(node->opt());
if( (node->fixed().getMeanPosition() - sensorPosition).squaredNorm() > threshold )
{
payload->flag_ = DISTANCE_TOO_HIGH;
continue;
}
}
}
void sure::keypoints::calculateCornerness(Octree& octree, Scalar samplingRate, Scalar radius, Scalar threshold)
{
unsigned samplingDepth = octree.getDepth(samplingRate);
for(unsigned int i=0; i<octree[samplingDepth].size(); ++i)
{
Node* currNode = octree[samplingDepth][i];
EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt());
if( payload->flag_ == POSSIBLE )
{
payload->cornerness_ = sure::keypoints::calculateCornerness(octree, currNode, radius);
if( payload->cornerness_ < threshold )
{
payload->flag_ = CORNERNESS_TOO_LOW;
}
}
}
}
void sure::keypoints::calculateEntropy(Octree& octree, Scalar samplingrate, Scalar normalSamplingrate, Scalar radius, Scalar threshold, EntropyCalculationMode mode, Scalar influenceRadius)
{
unsigned samplingDepth = octree.getDepth(samplingrate);
for(unsigned int i=0; i<octree[samplingDepth].size(); ++i)
{
Node* node = octree[samplingDepth][i];
EntropyPayload* payload = static_cast<EntropyPayload*>(node->opt());
if( payload->flag_ != NOT_CALCULATED )
{
continue;
}
switch( mode )
{
default:
case NORMALS:
payload->entropy_ = calculateEntropyWithNormals(octree, node, normalSamplingrate, radius, influenceRadius);
break;
case CROSS_PRODUCTS_W_MAIN:
payload->entropy_ = calculateEntropyWithCrossproducts(octree, node, normalSamplingrate, radius, influenceRadius);
break;
case CROSS_PRODUCTS_PAIRWISE:
payload->entropy_ = calculateEntropyWithCrossproductsPairwise(octree, node, normalSamplingrate, radius, influenceRadius);
break;
}
if( payload->entropy_ < threshold )
{
payload->flag_ = ENTROPY_TOO_LOW;
continue;
}
payload->flag_ = POSSIBLE;
}
}
