void computeDistanceMatrix(Matrix distance, FaceGraph* graphs, int mini, int maxi, int minj, int maxj, FaceGraphSimilarity distMeasure) {
int i,j;
static time_t current_time = 0;
if( (maxi - mini > MAX_BLOCK_SIZE) || (maxi - mini > MAX_BLOCK_SIZE) ) {
int sizei = maxi - mini;
int sizej = maxj - minj;
computeDistanceMatrix(distance, graphs, mini, mini+sizei/2, minj, minj+sizej/2, distMeasure);
computeDistanceMatrix(distance, graphs, mini+sizei/2, maxi, minj, minj+sizej/2, distMeasure);
computeDistanceMatrix(distance, graphs, mini, mini+sizei/2, minj+sizej/2, maxj, distMeasure);
computeDistanceMatrix(distance, graphs, mini+sizei/2, maxi, minj+sizej/2, maxj, distMeasure);
if(current_time + 10 < time(NULL)) {
/* Estimate the remaining time and print status report */
int hour, min, sec;
double remaining_time;
current_time = time(NULL);
remaining_time = ((double)total - completed)*((double)current_time - start_time) / completed;
hour = ((int)remaining_time)/3600;
min = (((int)remaining_time)%3600)/60;
sec = ((int)remaining_time)%60;
printf("Measuring: %010d of %010d (%5.2f%%) ETR = %02dh %02dm %02ds\r",completed, total, completed*100.0/total, hour, min, sec);
fflush(stdout);
}
return;
}
for(i = mini; i < maxi; i++) {
for(j = minj; j < maxj; j++) {
ME(distance,i,j) = distMeasure(graphs[i], graphs[j]);
completed++;
}
}
}
/**
* Gradient of cov(X) w.r.t. given parameter number
*/
void Matern5CF::getParameterPartialDerivative(mat& PD, const unsigned int parameterNumber, const mat& X) const
{
Transform* t = getTransform(parameterNumber);
double gradientModifier = t->gradientTransform(getParameter(parameterNumber));
switch(parameterNumber)
{
case 0 :
{
mat R2(PD.n_rows, PD.n_cols);
computeDistanceMatrix(R2, (sqrt(5.0) / lengthScale) * X);
mat R = sqrt(R2);
PD = (gradientModifier * (variance/(3.0*lengthScale)) * (R2 %(1.0+R)))
% exp(-R);
break;
}
case 1 :
{
computeSymmetric(PD, X);
PD *= (gradientModifier / variance);
break;
}
}
}
void ConfigStat::compute() {
deleteComputation();
initComputation();
computeDistanceMatrix();
computeCenter();
computeCentroid();
computeBetweennessCenter();
}
void EuclideanClustering::extract(
const sensor_msgs::PointCloud2ConstPtr &input)
{
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *cloud);
std::vector<pcl::PointIndices> cluster_indices;
// list up indices which are not NaN to use
// organized pointcloud
pcl::PointIndices::Ptr nonnan_indices (new pcl::PointIndices);
for (size_t i = 0; i < cloud->points.size(); i++) {
pcl::PointXYZ p = cloud->points[i];
if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
nonnan_indices->indices.push_back(i);
}
}
if (nonnan_indices->indices.size() == 0) {
// if input points is 0, publish empty data as result
jsk_recognition_msgs::ClusterPointIndices result;
result.header = input->header;
result_pub_.publish(result);
// do nothing and return it
jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped);
cluster_num_msg->header = input->header;
cluster_num_msg->data = 0;
cluster_num_pub_.publish(cluster_num_msg);
return;
}
EuclideanClusterExtraction<pcl::PointXYZ> impl;
{
jsk_topic_tools::ScopedTimer timer = kdtree_acc_.scopedTimer();
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
#endif
tree->setInputCloud (cloud);
impl.setClusterTolerance (tolerance);
impl.setMinClusterSize (minsize_);
impl.setMaxClusterSize (maxsize_);
impl.setSearchMethod (tree);
impl.setIndices(nonnan_indices);
impl.setInputCloud (cloud);
}
{
jsk_topic_tools::ScopedTimer timer = segmentation_acc_.scopedTimer();
impl.extract (cluster_indices);
}
// Publish result indices
jsk_recognition_msgs::ClusterPointIndices result;
result.cluster_indices.resize(cluster_indices.size());
cluster_counter_.add(cluster_indices.size());
result.header = input->header;
if (cogs_.size() != 0 && cogs_.size() == cluster_indices.size()) {
// tracking the labels
//ROS_INFO("computing distance matrix");
// compute distance matrix
// D[i][j] --> distance between the i-th previous cluster
// and the current j-th cluster
Vector4fVector new_cogs;
computeCentroidsOfClusters(new_cogs, cloud, cluster_indices);
double D[cogs_.size() * new_cogs.size()];
computeDistanceMatrix(D, cogs_, new_cogs);
std::vector<int> pivot_table = buildLabelTrackingPivotTable(D, cogs_, new_cogs, label_tracking_tolerance);
if (pivot_table.size() != 0) {
cluster_indices = pivotClusterIndices(pivot_table, cluster_indices);
}
}
Vector4fVector tmp_cogs;
computeCentroidsOfClusters(tmp_cogs, cloud, cluster_indices); // NB: not efficient
cogs_ = tmp_cogs;
for (size_t i = 0; i < cluster_indices.size(); i++) {
#if ROS_VERSION_MINIMUM(1, 10, 0)
// hydro and later
result.cluster_indices[i].header
= pcl_conversions::fromPCL(cluster_indices[i].header);
#else
// groovy
result.cluster_indices[i].header = cluster_indices[i].header;
#endif
result.cluster_indices[i].indices = cluster_indices[i].indices;
}
result_pub_.publish(result);
jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped);
cluster_num_msg->header = input->header;
cluster_num_msg->data = cluster_indices.size();
cluster_num_pub_.publish(cluster_num_msg);
diagnostic_updater_->update();
}
int main(int argc, char **argv)
{
// Read in the genomes
QString fileName = "data/bitvectors-genes.data.small";
if (argc == 2)
fileName = argv[1];
QFile file(fileName);
if (!file.open(QFile::ReadOnly)) {
qDebug() << file.errorString();
return 1;
}
QTextStream stream(&file);
while (!stream.atEnd()) {
const QString line = stream.readLine();
if (!line.isEmpty())
genomes.append(BitVector(line));
}
// Determine the maximum distance from the parent we will
// tolerate at this point in time.
int size = genomes[0].genome.size();
double mutationRate = MUTATION_RATE;
double perfectDifference = size * mutationRate;
double std = binomialstd(size, mutationRate);
maximumDifference = perfectDifference + (3 * std);
computeDistanceMatrix();
// If a node only has one connection to another genome set it as the parent.
// Repeat by remove nodes that are already child nodes finding new nodes
// that only have one parent
int c = 0;
do {
c = 0;
for (int i = 0; i < genomes.count(); ++i) {
if (genomes[i].findParents())
++c;
}
} while (c > 0);
// For the final nodes increase the maximum distance to catch
// the best match for outliers.
maximumDifference = perfectDifference + (5 * std);
for (int i = 0; i < genomes.count(); ++i)
if (genomes[i].parent == -1)
genomes[i].findUnlikelyParents(i);
// Final lost gnomes
// Try swapping child/parents of any two node graphs to find better combinations.
for (int i = 0; i < genomes.count(); ++i)
if (genomes[i].parent == -1) {
QList<int> children = findAllChildren(i);
if (children.count() == 1) {
int other = children[0];
genomes[i].parent = genomes[other].parent;
genomes[other].parent = -1;
genomes[other].findUnlikelyParents(other);
}
}
// Output results
QTextStream out(stdout);
for (int i = 0; i < genomes.count(); ++i)
out << genomes[i].parent << endl;
return 0;
}
int main(int argc, char** argv) {
ImageList *imagenames, *subject, *replicate;
int imageCount;
Matrix distance;
FaceGraph* graphs;
char** names;
int i, j;
Arguments args;
DistDirNode* distrec;
processCommand(argc, argv, &args);
MESSAGE("Reading in image names");
imagenames = getImageNames(args.imageNamesFile, &imageCount);
MESSAGE1ARG("Reading in graph files %d",imageCount);
/* Allocate space for imagenames, face graphs and distance matrix */
names = ALLOCATE(char*,imageCount);
graphs = ALLOCATE(FaceGraph, imageCount);
distance = makeZeroMatrix(imageCount,imageCount);
MESSAGE("Reading in graph files");
i = 0;
for(subject = imagenames; subject != NULL; subject = subject->next_subject) {
for( replicate = subject; replicate != NULL; replicate = replicate->next_replicate) {
printf("Reading in graph: %s\n", replicate->filename);
fflush(stdout);
names[i] = strdup(replicate->filename);
graphs[i] = loadFaceGraph(makePath(args.faceGraphDir,replicate->filename));
i++;
}
}
for(distrec = args.distList; distrec != NULL; distrec = distrec->next) {
/* Create distance matrix */
completed = 0;
total = imageCount*imageCount;
MESSAGE("Computing Distance Matrix");
start_time = time(NULL);
computeDistanceMatrix(distance, graphs, 0, imageCount, 0, imageCount, distrec->distMeasure);
/* Print out distance files to the distance directory */
for(i = 0; i < imageCount; i++) {
FILE* distfile = fopen(makePath(distrec->distDirectory,names[i]), "w");
if(!distfile) {
printf("Error opening distance file: %s\n",makePath(distrec->distDirectory,names[i]));
exit(1);
}
printf("Saving distances for image: %s\n",names[i]);
fflush(stdout);
for(j = 0; j < imageCount; j++) {
fprintf(distfile,"%s\t%16e\n",names[j], ME(distance,i,j));
}
fclose(distfile);
}
}
return 0;
}
