src/frontend/StereoFrame.cpp
/* -------------------------------------------------------------------------- */
// TODO: Clean up RGBD
// TODO: this should be in StereoMatcher
Clean up RGBD
TODO: this should be in StereoMatcher
Clean up RGBD TODO : this should be in StereoMatcher
void StereoFrame::sparseStereoMatching(const int verbosity) {
  if (verbosity > 0) {
    cv::Mat leftImgWithKeypoints =
        UtilsOpenCV::DrawCircles(left_frame_.img_, left_frame_.keypoints_);
    showImagesSideBySide(leftImgWithKeypoints, right_frame_.img_,
                         "unrectifiedLeftWithKeypoints_", verbosity);
  }
  CHECK(is_rectified_);
  StatusKeypointsCV left_keypoints_rectified;
  undistortRectifyPoints(left_frame_.keypoints_, left_frame_.cam_param_,
                         left_undistRectCameraMatrix_,
                         &left_keypoints_rectified);
  double fx = left_undistRectCameraMatrix_.fx();
  StatusKeypointsCV right_keypoints_rectified;
  switch (sparse_stereo_params_.vision_sensor_type_) {
  case VisionSensorType::STEREO:
    right_keypoints_rectified =
        getRightKeypointsRectified(left_img_rectified_, right_img_rectified_,
                                   left_keypoints_rectified, fx, getBaseline());
    break;
  case VisionSensorType::RGBD: 
    right_keypoints_rectified = getRightKeypointsRectifiedRGBD(
        left_img_rectified_, right_img_rectified_, left_keypoints_rectified, fx,
        getBaseline(), getMapDepthFactor(), getMinDepthFactor());
    break;
  default:
    LOG(FATAL) << "sparseStereoMatching: only works when "
                  "VisionSensorType::STEREO or RGBD";
    break;
  }
  keypoints_depth_ = getDepthFromRectifiedMatches(
      left_keypoints_rectified, right_keypoints_rectified, fx, getBaseline());
  if (verbosity > 0) {
    cv::Mat left_rectifiedWithKeypoints =
        UtilsOpenCV::DrawCircles(left_img_rectified_, left_keypoints_rectified);
    drawEpipolarLines(left_rectifiedWithKeypoints, right_img_rectified_, 20,
                      verbosity);
    cv::Mat right_rectifiedWithKeypoints = UtilsOpenCV::DrawCircles(
        right_img_rectified_, right_keypoints_rectified, keypoints_depth_);
    showImagesSideBySide(left_rectifiedWithKeypoints,
                         right_rectifiedWithKeypoints,
                         "rectifiedWithKeypointsAndDepth_", verbosity);
  }
  std::tie(right_frame_.keypoints_, right_keypoints_status_) =
      distortUnrectifyPoints(right_keypoints_rectified,
                             right_frame_.cam_param_.undistRect_map_x_,
                             right_frame_.cam_param_.undistRect_map_y_);
  CHECK_EQ(keypoints_depth_.size(), left_frame_.versors_.size())
      << "sparseStereoMatching: keypoints_depth_ & versors_ sizes are wrong!";
  keypoints_3d_.clear();
  keypoints_3d_.reserve(right_keypoints_rectified.size());
  left_keypoints_rectified_.clear();
  left_keypoints_rectified_.reserve(right_keypoints_rectified.size());
  right_keypoints_rectified_.clear();
  right_keypoints_rectified_.reserve(right_keypoints_rectified.size());
  gtsam::Rot3 camLrect_R_camL =
      UtilsOpenCV::cvMatToGtsamRot3(left_frame_.cam_param_.R_rectify_);
  for (size_t i = 0; i < right_keypoints_rectified.size(); i++) {
    left_keypoints_rectified_.push_back(left_keypoints_rectified.at(i).second);
    right_keypoints_rectified_.push_back(
        right_keypoints_rectified.at(i).second);
    if (right_keypoints_rectified[i].first == KeypointStatus::VALID) {
      Vector3 versor = camLrect_R_camL.rotate(left_frame_.versors_[i]);
      CHECK_GE(versor(2), 1e-3)
          << "sparseStereoMatching: found point with nonpositive depth!";
      keypoints_3d_.push_back(versor * keypoints_depth_[i] / versor(2));
    } else {
      keypoints_3d_.push_back(Vector3::Zero());
    }
  }
  if (VLOG_IS_ON(20)) {
    displayKeypointStats(right_keypoints_rectified);
  }
  VLOG(10) << "Sanity check stereo frame...";
  checkStereoFrame();
  VLOG(10) << "Finished sanity check stereo frame.";
}
void StereoFrame :: sparseStereoMatching ( const int verbosity ) { if ( verbosity > 0 ) { cv :: Mat leftImgWithKeypoints = UtilsOpenCV :: DrawCircles ( left_frame_ . img_ , left_frame_ . keypoints_ ) ; showImagesSideBySide ( leftImgWithKeypoints , right_frame_ . img_ , " " , verbosity ) ; } CHECK ( is_rectified_ ) ; StatusKeypointsCV left_keypoints_rectified ; undistortRectifyPoints ( left_frame_ . keypoints_ , left_frame_ . cam_param_ , left_undistRectCameraMatrix_ , & left_keypoints_rectified ) ; double fx = left_undistRectCameraMatrix_ . fx ( ) ; StatusKeypointsCV right_keypoints_rectified ; switch ( sparse_stereo_params_ . vision_sensor_type_ ) { case VisionSensorType :: STEREO : right_keypoints_rectified = getRightKeypointsRectified ( left_img_rectified_ , right_img_rectified_ , left_keypoints_rectified , fx , getBaseline ( ) ) ; break ; case VisionSensorType :: RGBD : right_keypoints_rectified = getRightKeypointsRectifiedRGBD ( left_img_rectified_ , right_img_rectified_ , left_keypoints_rectified , fx , getBaseline ( ) , getMapDepthFactor ( ) , getMinDepthFactor ( ) ) ; break ; default : LOG ( FATAL ) << " " " " ; break ; } keypoints_depth_ = getDepthFromRectifiedMatches ( left_keypoints_rectified , right_keypoints_rectified , fx , getBaseline ( ) ) ; if ( verbosity > 0 ) { cv :: Mat left_rectifiedWithKeypoints = UtilsOpenCV :: DrawCircles ( left_img_rectified_ , left_keypoints_rectified ) ; drawEpipolarLines ( left_rectifiedWithKeypoints , right_img_rectified_ , 20 , verbosity ) ; cv :: Mat right_rectifiedWithKeypoints = UtilsOpenCV :: DrawCircles ( right_img_rectified_ , right_keypoints_rectified , keypoints_depth_ ) ; showImagesSideBySide ( left_rectifiedWithKeypoints , right_rectifiedWithKeypoints , " " , verbosity ) ; } std :: tie ( right_frame_ . keypoints_ , right_keypoints_status_ ) = distortUnrectifyPoints ( right_keypoints_rectified , right_frame_ . cam_param_ . undistRect_map_x_ , right_frame_ . cam_param_ . undistRect_map_y_ ) ; CHECK_EQ ( keypoints_depth_ . size ( ) , left_frame_ . versors_ . size ( ) ) << " " ; keypoints_3d_ . clear ( ) ; keypoints_3d_ . reserve ( right_keypoints_rectified . size ( ) ) ; left_keypoints_rectified_ . clear ( ) ; left_keypoints_rectified_ . reserve ( right_keypoints_rectified . size ( ) ) ; right_keypoints_rectified_ . clear ( ) ; right_keypoints_rectified_ . reserve ( right_keypoints_rectified . size ( ) ) ; gtsam :: Rot3 camLrect_R_camL = UtilsOpenCV :: cvMatToGtsamRot3 ( left_frame_ . cam_param_ . R_rectify_ ) ; for ( size_t i = 0 ; i < right_keypoints_rectified . size ( ) ; i ++ ) { left_keypoints_rectified_ . push_back ( left_keypoints_rectified . at ( i ) . second ) ; right_keypoints_rectified_ . push_back ( right_keypoints_rectified . at ( i ) . second ) ; if ( right_keypoints_rectified [ i ] . first == KeypointStatus :: VALID ) { Vector3 versor = camLrect_R_camL . rotate ( left_frame_ . versors_ [ i ] ) ; CHECK_GE ( versor ( 2 ) , 1e-3 ) << " " ; keypoints_3d_ . push_back ( versor * keypoints_depth_ [ i ] / versor ( 2 ) ) ; } else { keypoints_3d_ . push_back ( Vector3 :: Zero ( ) ) ; } } if ( VLOG_IS_ON ( 20 ) ) { displayKeypointStats ( right_keypoints_rectified ) ; } VLOG ( 10 ) << " " ; checkStereoFrame ( ) ; VLOG ( 10 ) << " " ; }
TODO: Clean up RGBD
TODO: this should be in StereoMatcher
TODO : Clean up RGBD TODO : this should be in StereoMatcher
// Get rectified left keypoints. // TODO (actually this is compensated later on in the pipeline): This should // be correct but largely hinders the performance of RANSAC compensate versors // for rectification //  gtsam::Rot3 camLrect_R_camL = //  UtilsOpenCV::Cvmat2rot(left_frame_.cam_param_.R_rectify_); std::cout << //  "left_frame_.cam_param_.R_rectify_ << " << //  camLrect_R_camL.matrix().determinant() << std::endl; for(size_t i = 0; i < //  left_frame_.versors_.size(); i++){ // for each versor //    left_frame_.versors_.at(i) = //    camLrect_R_camL.rotate(gtsam::Unit3(left_frame_.versors_.at(i))).unitVector(); //    // rotate versor //    // R_rectify = camL_R_camLrect' = camL_R_camLrect => camLrect_versor = //    camLrect_R_camL * camL_versor double normV = //    left_frame_.versors_.at(i).norm(); left_frame_.versors_.at(i) = //    left_frame_.versors_.at(i) / normV; //    // renormalize to reduce error propagation //  } // **** now we get corresponding keypoints in right image // *************************** ////////////////////////////////////////////////////////////////////////// // another way is to find for sparse correspondences in right image using // optical flow COMMENT: this does not work, probably the baseline is too // large getRightKeypointsLKunrectified(); ////////////////////////////////////////////////////////////////////////// // another way is to extract corners in right and try to match with left // COMMENT: unfortunately, from visual inspection it seems that we do not get // the same matches KeypointsCV right_keypoints = // UtilsOpenCV::ExtractCorners(right_frame_.img_); cv::Mat // rightImgWithKeypoints = UtilsOpenCV::DrawCircles(right_frame_.img_, // right_keypoints); // showImagesSideBySide(leftImgWithKeypoints,rightImgWithKeypoints,"unrectifiedWithKeypoints", // verbosity); ////////////////////////////////////////////////////////////////////////// // the way we use is to find for sparse correspondences in right image using // patch correlation along (horizontal epipolar lines) // std::cout << "nr of template matching calls: " << // left_frame_.getNrValidKeypoints() << std::endl; // Options for stereo and RGB-D // just use depth to "fake right pixel // matches" // Compute the depth for each keypoints. // Display. // Store point pixels and statuses: for visualization // and to populate the statuses. // TODO remove tie, potential copies being made. // Sanity check. // Get 3D points and populate structures. // IMPORTANT: keypoints_3d_ are expressed in the rectified left frame, so we // have to compensate for rectification. We do not do this for the versors to // avoid adding numerical errors (we are using very tight thresholds on // 5-point RANSAC) // keypoints_depth_ is not the norm of the vector, it is the z component. // Visualize statistics on the performance of the sparse stereo matching. // Sanity check.