float RangeImageBorderExtractor::getNeighborDistanceChangeScore(
const RangeImageBorderExtractor::LocalSurface& local_surface,
int x, int y, int offset_x, int offset_y, int pixel_radius) const
{
const PointWithRange& point = range_image_->getPoint(x, y);
PointWithRange neighbor;
range_image_->get1dPointAverage(x+offset_x, y+offset_y, offset_x, offset_y, pixel_radius, neighbor);
if (pcl_isinf(neighbor.range))
{
if (neighbor.range < 0.0f)
return 0.0f;
else
{
//cout << "INF edge -> Setting to 1.0\n";
return 1.0f; // TODO: Something more intelligent
}
}
float neighbor_distance_squared = squaredEuclideanDistance(neighbor, point);
if (neighbor_distance_squared <= local_surface.max_neighbor_distance_squared)
return 0.0f;
float ret = 1.0f - sqrtf(local_surface.max_neighbor_distance_squared / neighbor_distance_squared);
if (neighbor.range < point.range)
ret = -ret;
return ret;
}
bool RangeImageBorderExtractor::calculateMainPrincipalCurvature(int x, int y, int radius, int step_size, float& magnitude, Eigen::Vector3f& main_direction) const
{
magnitude = 0.0f;
int index = y*range_image_->width+x;
LocalSurface* local_surface = surface_structure_[index];
if (local_surface==NULL)
return false;
const PointWithRange& point = range_image_->getPointNoCheck(x,y);
float max_distance_squared = 2.0f*local_surface->max_neighbor_distance_squared;
Eigen::Vector3f& normal = local_surface->normal_no_jumps;
Eigen::Matrix3f to_tangent_plane = Eigen::Matrix3f::Identity() - normal*normal.transpose();
VectorAverage3f vector_average;
for (int y2=y-radius; y2<=y+radius; y2+=step_size)
{
for (int x2=x-radius; x2<=x+radius; x2+=step_size)
{
if (!range_image_->isValid(x2,y2))
continue;
int index2 = y2*range_image_->width + x;
const PointWithRange& point2 = range_image_->getPoint(index2);
LocalSurface* local_surface2 = surface_structure_[index2];
if (local_surface2==NULL)
continue;
Eigen::Vector3f& normal2 = local_surface2->normal_no_jumps;
float distance_squared = squaredEuclideanDistance(point, point2);
if (distance_squared > max_distance_squared)
continue;
vector_average.add(to_tangent_plane*normal2);
}
}
if (vector_average.getNoOfSamples() < 3)
return false;
Eigen::Vector3f eigen_values, eigen_vector1, eigen_vector2;
vector_average.doPCA(eigen_values, eigen_vector1, eigen_vector2, main_direction);
//magnitude = sqrtf(eigen_values[2]);
magnitude = eigen_values[2];
magnitude = 1.0f - powf(1.0f-magnitude, 3);
//magnitude += magnitude - powf(magnitude,2);
//magnitude += magnitude - powf(magnitude,2);
//magnitude = sqrtf(local_surface->eigen_values[0]/local_surface->eigen_values.sum());
//magnitude = sqrtf(local_surface->eigen_values_no_jumps[0]/local_surface->eigen_values_no_jumps.sum());
//if (surface_structure_[y*range_image_->width+x+1]==NULL||surface_structure_[y*range_image_->width+x-1]==NULL)
//{
//magnitude = -std::numeric_limits<float>::infinity ();
//return false;
//}
//float angle2 = acosf(surface_structure_[y*range_image_->width+x+1]->normal.dot(local_surface->normal)),
//angle1 = acosf(surface_structure_[y*range_image_->width+x-1]->normal.dot(local_surface->normal));
//magnitude = angle2-angle1;
return true;
}
bool HierarchicalClustering::train_(MatrixFloat &data){
trained = false;
clusters.clear();
distanceMatrix.clear();
if( data.getNumRows() == 0 || data.getNumCols() == 0 ){
return false;
}
//Set the rows and columns
M = data.getNumRows();
N = data.getNumCols();
//Build the distance matrix
distanceMatrix.resize(M,M);
//Build the distance matrix
for(UINT i=0; i<M; i++){
for(UINT j=0; j<M; j++){
if( i== j ) distanceMatrix[i][j] = grt_numeric_limits< Float >::max();
else{
distanceMatrix[i][j] = squaredEuclideanDistance(data[i], data[j]);
}
}
}
//Build the initial clusters, at the start each sample gets its own cluster
UINT uniqueClusterID = 0;
Vector< ClusterInfo > clusterData(M);
for(UINT i=0; i<M; i++){
clusterData[i].uniqueClusterID = uniqueClusterID++;
clusterData[i].addSampleToCluster(i);
}
trainingLog << "Starting clustering..." << std::endl;
//Create the first cluster level, each sample is it's own cluster
UINT level = 0;
ClusterLevel newLevel;
newLevel.level = level;
for(UINT i=0; i<M; i++){
newLevel.clusters.push_back( clusterData[i] );
}
clusters.push_back( newLevel );
//Move to level 1 and start the search
level++;
bool keepClustering = true;
while( keepClustering ){
//Find the closest two clusters within the cluster data
Float minDist = grt_numeric_limits< Float >::max();
Vector< Vector< UINT > > clusterPairs;
UINT K = (UINT)clusterData.size();
for(UINT i=0; i<K; i++){
for(UINT j=0; j<K; j++){
if( i != j ){
Float dist = computeClusterDistance( clusterData[i], clusterData[j] );
if( dist < minDist ){
minDist = dist;
Vector< UINT > clusterPair(2);
clusterPair[0] = i;
clusterPair[1] = j;
clusterPairs.clear();
clusterPairs.push_back( clusterPair );
}
}
}
}
if( minDist == grt_numeric_limits< Float >::max() ){
keepClustering = false;
warningLog << "train_(MatrixFloat &data) - Failed to find any cluster at level: " << level << std::endl;
return false;
}else{
//Merge the two closest clusters together and create a new level
ClusterLevel newLevel;
newLevel.level = level;
//Create the new cluster
ClusterInfo newCluster;
newCluster.uniqueClusterID = uniqueClusterID++;
const UINT numClusterPairs = clusterPairs.getSize();
for(UINT k=0; k<numClusterPairs; k++){
//Add all the samples in the first cluster to the new cluster
UINT numSamplesInClusterA = clusterData[ clusterPairs[k][0] ].getNumSamplesInCluster();
for(UINT i=0; i<numSamplesInClusterA; i++){
UINT index = clusterData[ clusterPairs[k][0] ][ i ];
newCluster.addSampleToCluster( index );
}
//Add all the samples in the second cluster to the new cluster
UINT numSamplesInClusterB = clusterData[ clusterPairs[k][1] ].getNumSamplesInCluster();
for(UINT i=0; i<numSamplesInClusterB; i++){
UINT index = clusterData[ clusterPairs[k][1] ][ i ];
newCluster.addSampleToCluster( index );
}
//Compute the cluster variance
newCluster.clusterVariance = computeClusterVariance( newCluster, data );
//Remove the two cluster pairs (so they will not be used in the next search
UINT idA = clusterData[ clusterPairs[k][0] ].getUniqueClusterID();
UINT idB = clusterData[ clusterPairs[k][1] ].getUniqueClusterID();
UINT numRemoved = 0;
Vector< ClusterInfo >::iterator iter = clusterData.begin();
while( iter != clusterData.end() ){
if( iter->getUniqueClusterID() == idA || iter->getUniqueClusterID() == idB ){
iter = clusterData.erase( iter );
if( ++numRemoved >= 2 ) break;
}else iter++;
}
}
//Add the merged cluster to the clusterData
clusterData.push_back( newCluster );
//Add the new level and cluster data to the main cluster buffer
newLevel.clusters.push_back( newCluster );
clusters.push_back( newLevel );
//Update the level
level++;
}
//Check to see if we should stop clustering
if( level >= M ){
keepClustering = false;
}
if( clusterData.size() == 0 ){
keepClustering = false;
}
trainingLog << "Cluster level: " << level << " Number of clusters: " << clusters.back().getNumClusters() << std::endl;
}
//Flag that the model is trained
trained = true;
//Setup the cluster labels
clusterLabels.resize(numClusters);
for(UINT i=0; i<numClusters; i++){
clusterLabels[i] = i+1;
}
clusterLikelihoods.resize(numClusters,0);
clusterDistances.resize(numClusters,0);
return true;
}
