Cloud::Ptr SlamCalibrator::buildMap(StreamSequenceBase::ConstPtr sseq, const Trajectory& traj, double max_range, double vgsize)
{
std::cout<<"Building slam calibration map using max range of " << max_range << std::endl;
Cloud::Ptr map(new Cloud);
int num_used_frames = 0;
for(size_t i = 0; i < traj.size(); ++i) {
if(!traj.exists(i))
continue;
// if(i % traj.size() / 10 == 0)
// cout << "." << flush;
Frame frame;
sseq->readFrame(i, &frame);
filterFringe(&frame);
Cloud::Ptr tmp(new Cloud);
sseq->proj_.frameToCloud(frame, tmp.get(), max_range);
Cloud::Ptr nonans(new Cloud);
nonans->reserve(tmp->size());
for(size_t j = 0; j < tmp->size(); ++j)
if(isFinite(tmp->at(j)))
nonans->push_back(tmp->at(j));
pcl::transformPointCloud(*nonans, *nonans, traj.get(i).cast<float>());
*map += *nonans;
++num_used_frames;
// Added intermediate filtering to handle memory overload on huge maps
if(num_used_frames % 50 == 0)
{
//cout << "Filtering..." << endl;
HighResTimer hrt("filtering");
hrt.start();
pcl::VoxelGrid<Point> vg;
vg.setLeafSize(vgsize, vgsize, vgsize);
Cloud::Ptr tmp(new Cloud);
vg.setInputCloud(map);
vg.filter(*tmp);
*map = *tmp;
hrt.stop();
}
}
//cout << endl;
//cout << "Filtering..." << endl;
HighResTimer hrt("filtering");
hrt.start();
pcl::VoxelGrid<Point> vg;
vg.setLeafSize(vgsize, vgsize, vgsize);
Cloud::Ptr tmp(new Cloud);
vg.setInputCloud(map);
vg.filter(*tmp);
*map = *tmp;
hrt.stop();
//cout << hrt.reportMilliseconds() << endl;
cout << "Filtered map has " << map->size() << " points." << endl;
return map;
}
size_t SlamCalibrator::processMap(const StreamSequenceBase& sseq,
const Trajectory& traj, const Cloud& map,
DiscreteDepthDistortionModel* model) const
{
// -- Select which frame indices from the sequence to use.
// Consider only those with a pose in the Trajectory,
// and apply downsampling based on increment_.
vector<size_t> indices;
indices.reserve(traj.numValid());
int num = 0;
for(size_t i = 0; i < traj.size(); ++i) {
if(traj.exists(i)) {
++num;
if(num % increment_ == 0)
indices.push_back(i);
}
}
// -- For all selected frames, accumulate training examples
// in the distortion model.
VectorXi counts = VectorXi::Zero(indices.size());
#pragma omp parallel for
for(size_t i = 0; i < indices.size(); ++i) {
size_t idx = indices[i];
BOOST_ASSERT(traj.exists(idx));
cout << "." << flush;
Frame measurement;
sseq.readFrame(idx, &measurement);
Frame mapframe;
mapframe.depth_ = DepthMatPtr(new DepthMat);
sseq.proj_.estimateMapDepth(map, traj.get(idx).inverse().cast<float>(),
measurement, mapframe.depth_.get());
counts[i] = model->accumulate(*mapframe.depth_, *measurement.depth_);
// cv::imshow("map", mapframe.depthImage());
// cv::imshow("measurement", measurement.depthImage());
// cv::waitKey();
// -- Quick and dirty option for data inspection.
if(getenv("U") && getenv("V")) {
int u_center = atoi(getenv("U"));
int v_center = atoi(getenv("V"));
int radius = 1;
for(int u = max(0, u_center - radius); u < min(640, u_center + radius + 1); ++u) {
for(int v = max(0, v_center - radius); v < min(480, v_center + radius + 1); ++v) {
if(mapframe.depth_->coeffRef(v, u) == 0)
continue;
if(measurement.depth_->coeffRef(v, u) == 0)
continue;
cerr << mapframe.depth_->coeffRef(v, u) * 0.001
<< " "
<< measurement.depth_->coeffRef(v, u) * 0.001
<< endl;
}
}
}
}
cout << endl;
return counts.sum();
}