void execute()
{
set_samples_frequency();
get_annotation();
getsc_min_max();
get_sample_intervals();
init_samples();
qrs_detection_algorithm();
}
std::vector<CylindricalShell> Sampling::searchAffordances(const PointCloud::Ptr &cloud,
const PointCloudRGB::Ptr &cloudrgb, double target_radius)
{
double start_time = omp_get_wtime();
double sigma = 2.0 * target_radius;
// find initial affordances
std::vector<int> indices = this->affordances.createRandomIndices(cloud, num_init_samples);
std::vector<CylindricalShell> all_shells = this->affordances.searchAffordances(cloud, indices);
// visualize
if (this->is_visualized)
{
Eigen::MatrixXd init_samples(3, num_init_samples);
for (std::size_t i = 0; i < num_init_samples; i++)
{
init_samples(0, i) = cloudrgb->points[indices[i]].x;
init_samples(1, i) = cloudrgb->points[indices[i]].y;
init_samples(2, i) = cloudrgb->points[indices[i]].z;
}
std::vector<CylindricalShell> no_shells;
no_shells.resize(0);
SamplingVisualizer visualizer;
visualizer.createViewerRGB(cloudrgb, no_shells, init_samples, target_radius);
visualizer.getViewer()->setSize(800, 600);
visualizer.getViewer()->setCameraPosition(0.255106, -0.903705, -0.538521, 0.255053, -0.902864, -0.537242,
-0.0206334, -0.835517, 0.549076);
while (!visualizer.getViewer()->wasStopped())
{
visualizer.getViewer()->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
int num_rand_samples = prob_rand_samples * num_samples;
int num_gauss_samples = num_samples - num_rand_samples;
Eigen::Matrix3d diag_sigma = Eigen::Matrix3d::Zero();
diag_sigma.diagonal() << sigma, sigma, sigma;
Eigen::Matrix3d inv_sigma = diag_sigma.inverse();
double term = 1.0 / sqrt(pow(2.0 * M_PI, 3.0) * pow(sigma, 3.0));
// create random generator for normal distribution
boost::mt19937 *rng = new boost::mt19937();
rng->seed(time(NULL));
boost::normal_distribution<> distribution(0.0, 1.0);
boost::variate_generator<boost::mt19937, boost::normal_distribution<> > generator(*rng, distribution);
Eigen::MatrixXd samples(3, num_samples);
// find affordances using importance sampling
for (std::size_t i = 0; i < num_iterations; i++)
{
double iteration_start_time = omp_get_wtime();
// draw samples close to affordances (importance sampling)
if (this->method == SUM) // sum of Gaussians
{
for (std::size_t j = 0; j < num_gauss_samples; j++)
{
int idx = rand() % all_shells.size();
samples(0, j) = all_shells[idx].getCentroid()(0) + generator() * sigma;
samples(1, j) = all_shells[idx].getCentroid()(1) + generator() * sigma;
samples(2, j) = all_shells[idx].getCentroid()(2) + generator() * sigma;
}
}
else // max of Gaussians
{
int j = 0;
while (j < num_gauss_samples) // draw samples using rejection sampling
{
// draw from sum of Gaussians
int idx = rand() % all_shells.size();
Eigen::Vector3d x;
x(0) = all_shells[idx].getCentroid()(0) + generator() * sigma;
x(1) = all_shells[idx].getCentroid()(1) + generator() * sigma;
x(2) = all_shells[idx].getCentroid()(2) + generator() * sigma;
double maxp = 0;
for (std::size_t k = 0; k < all_shells.size(); k++)
{
double p = (x - all_shells[k].getCentroid()).transpose() * (x - all_shells[k].getCentroid());
p = term * exp((-1.0 / (2.0 * sigma)) * p);
if (p > maxp)
maxp = p;
}
double p = (x - all_shells[idx].getCentroid()).transpose() * (x - all_shells[idx].getCentroid());
p = term * exp((-1.0 / (2.0 * sigma)) * p);
if (p >= maxp)
{
samples.col(j) = x;
j++;
}
}
}
// draw random samples
for (int j = num_samples - num_rand_samples; j < num_samples; j++)
{
int r = std::rand() % cloud->points.size();
while (!pcl::isFinite((*cloud)[r])
|| !this->affordances.isPointInWorkspace(cloud->points[r].x, cloud->points[r].y, cloud->points[r].z))
r = std::rand() % cloud->points.size();
samples.col(j) = cloud->points[r].getVector3fMap().cast<double>();
}
// visualize
if (is_visualized)
{
SamplingVisualizer visualizer;
// visualizer.createViewer(cloud, all_shells, samples, target_radius);
visualizer.createViewerRGB(cloudrgb, all_shells, samples.block(0, 0, 3, num_gauss_samples), target_radius);
visualizer.getViewer()->setSize(800, 600);
visualizer.getViewer()->setCameraPosition(0.255106, -0.903705, -0.538521, 0.255053, -0.902864, -0.537242,
-0.0206334, -0.835517, 0.549076);
visualizer.getViewer()->addText(
"Iteration " + boost::lexical_cast < std::string > (i) + ", shells: " + boost::lexical_cast < std::string
> (all_shells.size()) + ", num_gauss_samples: " + boost::lexical_cast < std::string
> (num_gauss_samples) + ", num_rand_samples: " + boost::lexical_cast < std::string > (num_rand_samples),
10, 20, 16, 1.0, 1.0, 1.0);
while (!visualizer.getViewer()->wasStopped())
{
visualizer.getViewer()->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
// find affordances
std::vector<CylindricalShell> shells = this->affordances.searchAffordancesTaubin(cloud, samples);
all_shells.insert(all_shells.end(), shells.begin(), shells.end());
printf("ELAPSED TIME (ITERATION %i): %.3f\n", (int)i, omp_get_wtime() - iteration_start_time);
}
printf("elapsed time (affordance search): %.3f sec, total # of affordances found: %i\n", omp_get_wtime() - start_time,
(int)all_shells.size());
return all_shells;
}
void init_test( // Initialise test data
TEST_DATA_S * tst_data_sp // Pointer to Structure containing test data
)
{
init_samples( tst_data_sp ,tst_data_sp->inps );
} // init_test
