sure::Scalar sure::keypoints::calculateEntropyWithCrossproductsPairwise(const Octree& octree, Node* node, Scalar normalSamplingrate, Scalar radius, Scalar influenceRadius) { sure::normal::CrossProductHistogram histogram; histogram.setInfluenceRadius(influenceRadius); NodeVector nodes = octree.getNodes(node->fixed().getMeanPosition(), radius, normalSamplingrate); for(unsigned int i=0; i<nodes.size(); ++i) { Node* firstNode = nodes[i]; NormalPayload* firstPayload = static_cast<NormalPayload*>(firstNode->opt()); const Normal& firstNormal = firstPayload->normal_; if( !firstNormal.isStable() ) { continue; } for(unsigned int j=i+1; j<nodes.size(); ++j) { Node* secondNode = nodes[j]; CrossProductPayload* secondPayload = static_cast<CrossProductPayload*>(secondNode->opt()); const Normal& secondNormal = secondPayload->normal_; if( secondNormal.isStable() ) { histogram.insertCrossProduct(firstNormal.vector(), secondNormal.vector()); } } return histogram.calculateEntropy(); } return 0.0; }
sure::Scalar sure::keypoints::calculateEntropyWithCrossproducts(const Octree& octree, Node* node, Scalar normalSamplingrate, Scalar radius, Scalar influenceRadius) { FixedPayload mainNormalIntegrate; octree.integratePayload(node->fixed().getMeanPosition(), radius, mainNormalIntegrate); Normal mainNormal = mainNormalIntegrate.calculateNormal(); if( mainNormal.isStable() ) { sure::normal::CrossProductHistogram histogram; histogram.setInfluenceRadius(influenceRadius); NodeVector nodes = octree.getNodes(node->fixed().getMeanPosition(), radius, normalSamplingrate); for(unsigned int i=0; i<nodes.size(); ++i) { Node* currNode = nodes[i]; NormalPayload* currPayload = static_cast<CrossProductPayload*>(currNode->opt()); if( currPayload->normal_.isStable()) { histogram.insertCrossProduct(mainNormal.vector(), currPayload->normal_.vector()); } } return histogram.calculateEntropy(); } return 0.0; }
sure::Scalar sure::keypoints::calculateCornerness(const Octree& octree, Node* node, Scalar radius) { Octree::NodeVector vec; vec = octree.getNodes(node, octree.getUnitSize(radius)); Vector3 mean(Vector3::Zero()); Scalar weight(0.0); for(unsigned int i=0; i<vec.size(); ++i) { Node* currNode = vec[i]; sure::payload::EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt()); if( payload->entropy_ > 0.0 ) { mean += (payload->entropy_ * currNode->fixed().getMeanPosition()); // mean[0] += (payload->entropy_ * currNode->fixed().getMeanPosition()[0]); // mean[1] += (payload->entropy_ * currNode->fixed().getMeanPosition()[1]); // mean[2] += (payload->entropy_ * currNode->fixed().getMeanPosition()[2]); weight += payload->entropy_; } } if( weight > 0.0 ) { mean /= weight; } else { return 0.f; } Matrix3 covariance(Matrix3::Zero()); for(unsigned int i=0; i<vec.size(); ++i) { Node* currNode = vec[i]; EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt()); if( payload->entropy_ > 0.0 ) { // Vector3 d; // d[0] = mean[0] - currNode->fixed().getMeanPosition()[0]; // d[1] = mean[1] - currNode->fixed().getMeanPosition()[1]; // d[2] = mean[2] - currNode->fixed().getMeanPosition()[2]; covariance += payload->entropy_ * ( (mean - currNode->fixed().getMeanPosition()) * ((mean - currNode->fixed().getMeanPosition()).transpose()) ); // covariance += payload->entropy_ * ( d * d.transpose() ); } } covariance /= weight; Vector3 eigenValues; pcl::eigen33(covariance, eigenValues); return (Scalar) (eigenValues[0] / eigenValues[2]); }
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::improveLocalization(const Octree& octree, Scalar samplingrate, Scalar radius, Vector3& position) { for(unsigned iteration=0; iteration<NUMBER_OF_MEAN_SHIFT_ITERATIONS; ++iteration) { NodeVector neighbors = octree.getNodes(position, radius, samplingrate); Scalar summedMean(0.0); int count(0); for(unsigned int n=0; n<neighbors.size(); ++n) { Node* neighbor = neighbors[n]; EntropyPayload* neighborPayload = static_cast<EntropyPayload*>(neighbor->opt()); if( neighbor->fixed().getPointFlag() != ARTIFICIAL ) { summedMean += neighborPayload->entropy_; count++; } } if( count == 0 ) { break; } Scalar mean = summedMean / (Scalar) count; Scalar summedVariance(0.0); for(unsigned int n=0; n<neighbors.size(); ++n) { Node* neighbor = neighbors[n]; EntropyPayload* neighborPayload = static_cast<EntropyPayload*>(neighbor->opt()); if( neighbor->fixed().getPointFlag() != ARTIFICIAL ) { summedVariance += (neighborPayload->entropy_ - mean) * (neighborPayload->entropy_ - mean); } } Scalar variance = summedVariance / (Scalar) count; Vector3 shiftedPosition(0.0, 0.0, 0.0); Scalar summedShift(0.0); for(unsigned int n=0; n<neighbors.size(); ++n) { Node* neighbor = neighbors[n]; EntropyPayload* neighborPayload = static_cast<EntropyPayload*>(neighbor->opt()); if( neighborPayload->entropy_ > mean ) { Scalar entropyDifference = mean - neighborPayload->entropy_; Scalar kernelElements = (entropyDifference*entropyDifference) / (variance); shiftedPosition += neighbor->fixed().getMeanPosition() * exp(-0.5*kernelElements); summedShift += exp(-0.5*kernelElements); } } if( summedShift != 0 ) { shiftedPosition = shiftedPosition * (1.f / summedShift); } if( sure::eigen_is_finite(shiftedPosition) ) { // Scalar dist = (position - shiftedPosition).norm(); position = shiftedPosition; } } }