C++ Triangulation::error方法代码示例

/// Filter inconsistent correspondences by using 3-view correspondences on view triplets
void SfM_Data_Structure_Estimation_From_Known_Poses::filter(
  const SfM_Data & sfm_data,
  const Pair_Set & pairs,
  const std::shared_ptr<Regions_Provider> & regions_provider)
{
  // Compute triplets
  // Triangulate triplet tracks
  //  - keep valid one

  typedef std::vector< graph::Triplet > Triplets;
  const Triplets triplets = graph::tripletListing(pairs);

  C_Progress_display my_progress_bar( triplets.size(), std::cout,
    "Per triplet tracks validation (discard spurious correspondences):\n" );
#ifdef OPENMVG_USE_OPENMP
    #pragma omp parallel
#endif // OPENMVG_USE_OPENMP
  for( Triplets::const_iterator it = triplets.begin(); it != triplets.end(); ++it)
  {
#ifdef OPENMVG_USE_OPENMP
    #pragma omp single nowait
#endif // OPENMVG_USE_OPENMP
    {
      #ifdef OPENMVG_USE_OPENMP
        #pragma omp critical
      #endif // OPENMVG_USE_OPENMP
      {++my_progress_bar;}

      const graph::Triplet & triplet = *it;
      const IndexT I = triplet.i, J = triplet.j , K = triplet.k;

      openMVG::tracks::STLMAPTracks map_tracksCommon;
      openMVG::tracks::TracksBuilder tracksBuilder;
      {
        PairWiseMatches map_matchesIJK;
        if(putatives_matches.find(std::make_pair(I,J)) != putatives_matches.end())
          map_matchesIJK.insert(*putatives_matches.find(std::make_pair(I,J)));

        if(putatives_matches.find(std::make_pair(I,K)) != putatives_matches.end())
          map_matchesIJK.insert(*putatives_matches.find(std::make_pair(I,K)));

        if(putatives_matches.find(std::make_pair(J,K)) != putatives_matches.end())
          map_matchesIJK.insert(*putatives_matches.find(std::make_pair(J,K)));

        if (map_matchesIJK.size() >= 2) {
          tracksBuilder.Build(map_matchesIJK);
          tracksBuilder.Filter(3);
          tracksBuilder.ExportToSTL(map_tracksCommon);
        }

        // Triangulate the tracks
        for (tracks::STLMAPTracks::const_iterator iterTracks = map_tracksCommon.begin();
          iterTracks != map_tracksCommon.end(); ++iterTracks) {
          {
            const tracks::submapTrack & subTrack = iterTracks->second;
            Triangulation trianObj;
            for (tracks::submapTrack::const_iterator iter = subTrack.begin(); iter != subTrack.end(); ++iter) {
              const size_t imaIndex = iter->first;
              const size_t featIndex = iter->second;
              const View * view = sfm_data.GetViews().at(imaIndex).get();
              const IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
              const Pose3 pose = sfm_data.GetPoseOrDie(view);
              const Vec2 pt = regions_provider->regions_per_view.at(imaIndex)->GetRegionPosition(featIndex);
              trianObj.add(cam->get_projective_equivalent(pose), cam->get_ud_pixel(pt));
            }
            const Vec3 Xs = trianObj.compute();
            if (trianObj.minDepth() > 0 && trianObj.error() < 4.0)
            // TODO: Add an angular check ?
            {
              #ifdef OPENMVG_USE_OPENMP
                #pragma omp critical
              #endif // OPENMVG_USE_OPENMP
              {
                openMVG::tracks::submapTrack::const_iterator iterI, iterJ, iterK;
                iterI = iterJ = iterK = subTrack.begin();
                std::advance(iterJ,1);
                std::advance(iterK,2);

                triplets_matches[std::make_pair(I,J)].push_back(IndMatch(iterI->second, iterJ->second));
                triplets_matches[std::make_pair(J,K)].push_back(IndMatch(iterJ->second, iterK->second));
                triplets_matches[std::make_pair(I,K)].push_back(IndMatch(iterI->second, iterK->second));
              }
            }
          }
        }
      }
    }
  }
  // Clear putatives matches since they are no longer required
  matching::PairWiseMatches().swap(putatives_matches);
}

void MainWindow::registerProject()
{
  if (m_doc._sfm_data.control_points.size() < 3)
  {
    QMessageBox msgBox;
    msgBox.setText("At least 3 control points are required.");
    msgBox.exec();
    return;
  }

  // Assert that control points can be triangulated
  for (Landmarks::const_iterator iterL = m_doc._sfm_data.control_points.begin();
    iterL != m_doc._sfm_data.control_points.end(); ++iterL)
  {
    if (iterL->second.obs.size() < 2)
    {
      QMessageBox msgBox;
      msgBox.setText("Each control point must be defined in at least 2 pictures.");
      msgBox.exec();
      return;
    }
  }

  //---
  // registration (coarse):
  // - compute the 3D points corresponding to the control point observation for the SfM scene
  // - compute a coarse registration between the controls points & the triangulated point
  // - transform the scene according the found transformation
  //---
  std::map<IndexT, Vec3> vec_control_points, vec_triangulated;
  std::map<IndexT, double> vec_triangulation_errors;
  for (Landmarks::iterator iterCP = m_doc._sfm_data.control_points.begin();
    iterCP != m_doc._sfm_data.control_points.end(); ++iterCP)
  {
    Landmark & landmark = iterCP->second;
    //Triangulate the point:
    Triangulation trianObj;
    const Observations & obs = landmark.obs;
    for(Observations::const_iterator itObs = obs.begin();
      itObs != obs.end(); ++itObs)
    {
      const View * view = m_doc._sfm_data.views.at(itObs->first).get();
      if (!m_doc._sfm_data.IsPoseAndIntrinsicDefined(view))
        continue;
      const openMVG::cameras::IntrinsicBase * cam = m_doc._sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
      const openMVG::geometry::Pose3 pose = m_doc._sfm_data.GetPoseOrDie(view);
      trianObj.add(
        cam->get_projective_equivalent(pose),
        cam->get_ud_pixel(itObs->second.x));
    }
    // Compute the 3D point
    const Vec3 X = trianObj.compute();
    if (trianObj.minDepth() > 0) // Keep the point only if it have a positive depth
    {
      vec_triangulated[iterCP->first] = X;
      vec_control_points[iterCP->first] = landmark.X;
      vec_triangulation_errors[iterCP->first] = trianObj.error()/(double)trianObj.size();
    }
    else
    {
      std::cout << "Invalid triangulation" << std::endl;
      return;
    }
  }

  if (vec_control_points.size() < 3)
  {
    QMessageBox msgBox;
    msgBox.setText("Insufficient number of triangulated control points.");
    msgBox.exec();
    return;
  }

  // compute the similarity
  {
    // data conversion to appropriate container
    Mat x1(3, vec_control_points.size()),
        x2(3, vec_control_points.size());
    for (int i=0; i < vec_control_points.size(); ++i)
    {
      x1.col(i) = vec_triangulated[i];
      x2.col(i) = vec_control_points[i];
    }

    std::cout
      << "Control points observation triangulations:\n"
      << x1 << std::endl << std::endl
      << "Control points coords:\n"
      << x2 << std::endl << std::endl;

    Vec3 t;
    Mat3 R;
    double S;
    if (openMVG::geometry::FindRTS(x1, x2, &S, &t, &R))
    {
      openMVG::geometry::Refine_RTS(x1,x2,&S,&t,&R);
      std::cout << "Found transform:\n"
        << " scale: " << S << "\n"
        << " rotation:\n" << R << "\n"
        << " translation: "<< t.transpose() << std::endl;
//.........这里部分代码省略.........