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