Fix and improve RANSAC step
I believe there is an error in the call to findEssentialMatrix() in the VO processor. The code now reads:
cv::Mat cvMask;
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
double focal = (sen_cam_->getIntrinsicMatrix()(0,0) +
sen_cam_->getIntrinsicMatrix()(1,1)) / 2;
_E = cv::findEssentialMat(p2d_prev,
p2d_curr,
Kcv_,
cv::RANSAC,
params_visual_odometry_->ransac.prob,
params_visual_odometry_->ransac.thresh / focal,
cvMask);And it is wrong because p2d_prev and p2d_curr are not in pixel units, they are expressed in the canonical camera system. So I replaced the code with this:
cv::Mat cvMask;
// cv::Mat K = cv::Mat::eye(3,3,CV_32F);
double focal = (sen_cam_->getIntrinsicMatrix()(0,0) +
sen_cam_->getIntrinsicMatrix()(1,1)) / 2;
_E = cv::findEssentialMat(p2d_prev, // pixel coordinates in canonical camera
p2d_curr, // pixel coordinates in canonical camera
1.0, // focal of canonical camera
cv::Point2d(0, 0), // principal point of canonical camera
// K,
cv::RANSAC, params_visual_odometry_->ransac.prob,
params_visual_odometry_->ransac.thresh / focal,
cvMask);Otherwise (and I think more elegant) is to avoid converting the points to the canonical space, and just use plain OpenCV API to do the job. I paste here the resulting code which I think is more logical than what we have now:
bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev,
const KeyPointsMap _mwkps_curr,
TracksMap &_tracks_prev_curr,
cv::Mat &_E)
{
// We need at least five tracks
if (_tracks_prev_curr.size() < 5) return false;
// We need to build lists of pt2d for openCV function
std::vector<cv::Point2d> p2d_prev, p2d_curr;
std::vector<size_t> all_indices;
for (auto & track : _tracks_prev_curr){
all_indices.push_back(track.first);
p2d_prev.push_back(_mwkps_prev.at(track.first).getCvPoint());
p2d_curr.push_back(_mwkps_curr.at(track.second).getCvPoint());
}
cv::Mat cvMask;
_E = cv::findEssentialMat(p2d_prev,
p2d_curr,
Kcv_,
cv::RANSAC,
params_visual_odometry_->ransac.prob,
params_visual_odometry_->ransac.thresh,
cvMask);
// Let's remove outliers from tracksMap
for (size_t k=0; k<all_indices.size(); k++){
if (cvMask.at<bool>(k) == 0){
_tracks_prev_curr.erase(all_indices.at(k));
}
}
return true;
}