// from the resample demo
int main (int argc, char** argv) {
std::string infile = "../../recordings/mark1_pcd/1316652689.744386960.pcd";
std::string smoothfile = "scan-mls.pcd";
std::string outmesh = "scan.vtk"; // there's a ply export in pcl 1.2, but ... we don't have that
if(argc > 1) {
if(strcmp(argv[1], "-s") == 0) {
if(access(infile.c_str(), R_OK) >= 0) {
std::cout << "running Moving Least Squares smoothing, this takes about 30s\n";
mls(infile, smoothfile);
} else {
print_usage();
std::cout << " ! unable to find " << infile << std::endl;
}
} else if(strcmp(argv[1], "-v") == 0) {
if(access(smoothfile.c_str(), R_OK) >= 0) {
viz(smoothfile);
} else {
print_usage();
std::cout << " ! unable to find " << smoothfile << std::endl;
}
} else if(strcmp(argv[1], "-r") == 0) {
if(access(smoothfile.c_str(), R_OK) >= 0) {
std::cout << "running greedy projection, this takes about 4s\n";
recons(smoothfile, outmesh);
} else {
print_usage();
std::cout << " ! unable to find " << smoothfile << std::endl;
}
} else {
print_usage();
}
} else {
print_usage();
}
}
void CpuPointSet::construct (size_t num_probes) const
{
Vector<float> likes(size * num_probes, m_temp_likes_batch);
Vector<uint8_t> recons(dim * num_probes, m_temp_recons_batch);
Vector<float> work(Cpu::vq_construct_work_size(dim, num_probes), m_work);
ASSERT1_LE(sqr(mean(likes) - 1.0f), 1e-8f);
const float sum_likes = size;
const float tol = get_construct_tol() * sum_likes;
Cpu::vq_construct_batch(likes, m_points, recons, work, tol, num_probes);
}
ConstructStats CpuPointSet::construct_deriv (size_t num_probes) const
{
Vector<float> sd(size * num_probes, m_temp_squared_distances_batch);
Vector<float> likes(size * num_probes, m_temp_likes_batch);
Vector<uint8_t> probes(dim * num_probes, m_temp_probes_batch);
Vector<uint8_t> recons(dim * num_probes, m_temp_recons_batch);
Vector<float> work(
Cpu::vq_construct_deriv_work_size(dim, num_probes), m_work);
ASSERT1_LE(sqr(mean(likes) - 1.0f), 1e-8f);
const float sum_likes = size;
const float tol = get_construct_deriv_tol() * sum_likes;
return Cpu::vq_construct_deriv(
likes,
sd,
probes,
m_points,
recons,
work,
tol,
num_probes);
}
void CpuPointSet::fit_points (const Point & probes, size_t num_probes)
{
ASSERT_SIZE(probes, dim * num_probes);
ASSERT_LE(num_probes, max_batch_size);
const size_t num_points = size;
float max_fit_rate = 1.0f / num_probes;
float min_fit_rate = get_fit_rate_tol() * get_fit_rate();
Vector<uint8_t> recons(probes.size, m_temp_recons_batch);
Vector<float> sd(size * num_probes, m_temp_squared_distances_batch);
Vector<float> likes(size * num_probes, m_temp_likes_batch);
Vector<float> fit_rates(size * num_probes, m_fit_rates_batch);
Vector<float> work(Cpu::fit_points_work_size(dim, size), m_work);
Cpu::measure_batch(probes, m_points, sd, num_probes);
m_quantize_stats_accum += Cpu::quantize_batch(
get_radius(),
sd,
likes,
num_probes);
m_construct_stats_accum += construct_deriv(num_probes);
m_count_stats_accum += num_probes;
accum_prior(num_probes);
for (size_t i_probe = 0; i_probe < num_probes; ++i_probe) {
const float * restrict fit_rate_scales = m_fit_rate_scales;
const float * restrict likes = m_temp_likes_batch + num_points * i_probe;
float * restrict fit_rates = m_fit_rates_batch + num_points * i_probe;
for (size_t p = 0; p < num_points; ++p) {
fit_rates[p] = min(max_fit_rate, fit_rate_scales[p] * likes[p]);
}
}
if (not fitting_points_to_recon()) {
Cpu::fit_points_to_obs(
min_fit_rate,
probes,
fit_rates,
m_points,
work,
num_probes);
} else {
Cpu::fit_points_to_recon(
get_radius(),
min_fit_rate,
probes,
recons,
fit_rates,
m_points,
work,
num_probes);
}
}
