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