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();
}
