bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
{
Vector2d px(frame->w2c(point->pos_));
if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{
const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols
+ static_cast<int>(px[0]/grid_.cell_size);
grid_.cells.at(k)->push_back(Candidate(point, px));
return true;
}
return false;
}
bool Reprojector::reprojectPoint(FramePtr frame, Point* point, int print)
{
// point->pos_[2] = point->pos_[2] * 600;
Vector2d px(frame->w2c(point->pos_));
if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{
// cout << "point pos" << point->pos_ << endl;
// cout << "px" << px << endl;
if(print)
// cout << "repro" << endl;
if(print) {
outfile << round(px[0]) << " " << round(px[1]) << " " << point->pos_[2] << " ";
outfile2 << point->pos_[0] << " " << point->pos_[1] << " " << point->pos_[2] << "\n";
}
const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols
+ static_cast<int>(px[0]/grid_.cell_size);
grid_.cells.at(k)->push_back(Candidate(point, px));
return true;
}
return false;
}