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