C++ Pose3::depth方法代码示例

/// Robustly try to estimate the best 3D point using a ransac Scheme
/// Return true for a successful triangulation
bool SfM_Data_Structure_Computation_Robust::robust_triangulation(
  const SfM_Data & sfm_data,
  const Observations & obs,
  Vec3 & X,
  const IndexT min_required_inliers,
  const IndexT min_sample_index) const
{
  const double dThresholdPixel = 4.0; // TODO: make this parameter customizable

  const IndexT nbIter = obs.size(); // TODO: automatic computation of the number of iterations?

  // - Ransac variables
  Vec3 best_model;
  std::set<IndexT> best_inlier_set;
  double best_error = std::numeric_limits<double>::max();

  // - Ransac loop
  for (IndexT i = 0; i < nbIter; ++i)
  {
    std::vector<size_t> vec_samples;
    robust::UniformSample(min_sample_index, obs.size(), &vec_samples);
    const std::set<IndexT> samples(vec_samples.begin(), vec_samples.end());

    // Hypothesis generation.
    const Vec3 current_model = track_sample_triangulation(sfm_data, obs, samples);

    // Test validity of the hypothesis
    // - chierality (for the samples)
    // - residual error

    // Chierality (Check the point is in front of the sampled cameras)
    bool bChierality = true;
    for (auto& it : samples){
      Observations::const_iterator itObs = obs.begin();
      std::advance(itObs, it);
      const View * view = sfm_data.views.at(itObs->first).get();
      const IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
      const Pose3 pose = sfm_data.GetPoseOrDie(view);
      const double z = pose.depth(current_model); // TODO: cam->depth(pose(X));
      bChierality &= z > 0;
    }

    if (!bChierality)
      continue;

    std::set<IndexT> inlier_set;
    double current_error = 0.0;
    // Classification as inlier/outlier according pixel residual errors.
    for (Observations::const_iterator itObs = obs.begin();
        itObs != obs.end(); ++itObs)
    {
      const View * view = sfm_data.views.at(itObs->first).get();
      const IntrinsicBase * intrinsic = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
      const Pose3 pose = sfm_data.GetPoseOrDie(view);
      const Vec2 residual = intrinsic->residual(pose, current_model, itObs->second.x);
      const double residual_d = residual.norm();
      if (residual_d < dThresholdPixel)
      {
        inlier_set.insert(itObs->first);
        current_error += residual_d;
      }
      else
      {
        current_error += dThresholdPixel;
      }
    }
    // Does the hypothesis is the best one we have seen and have sufficient inliers.
    if (current_error < best_error && inlier_set.size() >= min_required_inliers)
    {
      X = best_model = current_model;
      best_inlier_set = inlier_set;
      best_error = current_error;
    }
  }
  return !best_inlier_set.empty();
}

//.........这里部分代码省略.........
      return EXIT_FAILURE;
    }

    //A. prepare the corresponding putatives points
    Mat xL(2, vec_PutativeMatches.size());
    Mat xR(2, vec_PutativeMatches.size());
    for (size_t k = 0; k < vec_PutativeMatches.size(); ++k)  {
      const PointFeature & imaL = featsL[vec_PutativeMatches[k]._i];
      const PointFeature & imaR = featsR[vec_PutativeMatches[k]._j];
      xL.col(k) = imaL.coords().cast<double>();
      xR.col(k) = imaR.coords().cast<double>();
    }

    //B. Compute the relative pose thanks to a essential matrix estimation
    std::pair<size_t, size_t> size_imaL(imageL.Width(), imageL.Height());
    std::pair<size_t, size_t> size_imaR(imageR.Width(), imageR.Height());
    sfm::RelativePose_Info relativePose_info;
    if (!sfm::robustRelativePose(K, K, xL, xR, relativePose_info, size_imaL, size_imaR, 256))
    {
      std::cerr << " /!\\ Robust relative pose estimation failure."
        << std::endl;
      return EXIT_FAILURE;
    }

    std::cout << "\nFound an Essential matrix:\n"
      << "\tprecision: " << relativePose_info.found_residual_precision << " pixels\n"
      << "\t#inliers: " << relativePose_info.vec_inliers.size() << "\n"
      << "\t#matches: " << vec_PutativeMatches.size()
      << std::endl;

    // Show Essential validated point
    svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
    svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
    svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
    for (size_t i = 0; i < relativePose_info.vec_inliers.size(); ++i)  {
      const SIOPointFeature & LL = regionsL->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._i];
      const SIOPointFeature & RR = regionsR->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._j];
      const Vec2f L = LL.coords();
      const Vec2f R = RR.coords();
      svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
      svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0));
      svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0));
    }
    const std::string out_filename = "04_ACRansacEssential.svg";
    std::ofstream svgFile( out_filename.c_str() );
    svgFile << svgStream.closeSvgFile().str();
    svgFile.close();

    //C. Triangulate and export inliers as a PLY scene
    std::vector<Vec3> vec_3DPoints;

    // Setup camera intrinsic and poses
    Pinhole_Intrinsic intrinsic0(imageL.Width(), imageL.Height(), K(0, 0), K(0, 2), K(1, 2));
    Pinhole_Intrinsic intrinsic1(imageR.Width(), imageR.Height(), K(0, 0), K(0, 2), K(1, 2));

    const Pose3 pose0 = Pose3(Mat3::Identity(), Vec3::Zero());
    const Pose3 pose1 = relativePose_info.relativePose;

    // Init structure by inlier triangulation
    const Mat34 P1 = intrinsic0.get_projective_equivalent(pose0);
    const Mat34 P2 = intrinsic1.get_projective_equivalent(pose1);
    std::vector<double> vec_residuals;
    vec_residuals.reserve(relativePose_info.vec_inliers.size() * 4);
    for (size_t i = 0; i < relativePose_info.vec_inliers.size(); ++i)  {
      const SIOPointFeature & LL = regionsL->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._i];
      const SIOPointFeature & RR = regionsR->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._j];
      // Point triangulation
      Vec3 X;
      TriangulateDLT(P1, LL.coords().cast<double>(), P2, RR.coords().cast<double>(), &X);
      // Reject point that is behind the camera
      if (pose0.depth(X) < 0 && pose1.depth(X) < 0)
        continue;
      const Vec2 residual0 = intrinsic0.residual(pose0, X, LL.coords().cast<double>());
      const Vec2 residual1 = intrinsic1.residual(pose1, X, RR.coords().cast<double>());
      vec_residuals.push_back(fabs(residual0(0)));
      vec_residuals.push_back(fabs(residual0(1)));
      vec_residuals.push_back(fabs(residual1(0)));
      vec_residuals.push_back(fabs(residual1(1)));
    }

    // Display some statistics of reprojection errors
    float dMin, dMax, dMean, dMedian;
    minMaxMeanMedian<float>(vec_residuals.begin(), vec_residuals.end(),
      dMin, dMax, dMean, dMedian);

    std::cout << std::endl
      << "Triangulation residuals statistics:" << "\n"
      << "\t-- Residual min:\t" << dMin << "\n"
      << "\t-- Residual median:\t" << dMedian << "\n"
      << "\t-- Residual max:\t " << dMax << "\n"
      << "\t-- Residual mean:\t " << dMean << std::endl;

      // Export as PLY (camera pos + 3Dpoints)
      std::vector<Vec3> vec_camPos;
      vec_camPos.push_back( pose0.center() );
      vec_camPos.push_back( pose1.center() );
      exportToPly(vec_3DPoints, vec_camPos, "EssentialGeometry.ply");
  }
  return EXIT_SUCCESS;
}