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