/// List the view indexes that have valid camera intrinsic and pose.
static std::set<IndexT> Get_Valid_Views
(
const SfM_Data & sfm_data
)
{
std::set<IndexT> valid_idx;
for (Views::const_iterator it = sfm_data.getViews().begin();
it != sfm_data.getViews().end(); ++it)
{
const View * v = it->second.get();
const IndexT id_view = v->id_view;
const IndexT id_intrinsic = v->id_intrinsic;
const IndexT id_pose = v->id_pose;
bool bDefined =
id_intrinsic != UndefinedIndexT &&
sfm_data.getIntrinsics().find(id_intrinsic) != sfm_data.getIntrinsics().end() &&
id_pose != UndefinedIndexT &&
sfm_data.getPoses().find(id_pose) != sfm_data.getPoses().end();
if (bDefined)
{
valid_idx.insert(id_view);
}
}
return valid_idx;
}
/// Export camera poses positions as a Vec3 vector
void GetCameraPositions(const SfM_Data & sfm_data, std::vector<Vec3> & vec_camPosition)
{
const Poses & poses = sfm_data.getPoses();
for (Poses::const_iterator iterPose = poses.begin();
iterPose != poses.end(); ++iterPose)
{
vec_camPosition.push_back(iterPose->second.center());
}
}
///Check that each pose have a valid intrinsic and pose id in the existing View ids
bool ValidIds(const SfM_Data & sfm_data, ESfM_Data flags_part)
{
const bool bCheck_Intrinsic = (flags_part & INTRINSICS) == INTRINSICS;
const bool bCheck_Extrinsic = (flags_part & EXTRINSICS) == EXTRINSICS;
std::set<IndexT> set_id_intrinsics;
transform(sfm_data.getIntrinsics().begin(), sfm_data.getIntrinsics().end(),
std::inserter(set_id_intrinsics, set_id_intrinsics.begin()), std::RetrieveKey());
std::set<IndexT> set_id_extrinsics; //unique so can use a set
transform(sfm_data.getPoses().begin(), sfm_data.getPoses().end(),
std::inserter(set_id_extrinsics, set_id_extrinsics.begin()), std::RetrieveKey());
// Collect existing id_intrinsic && id_extrinsic from views
std::set<IndexT> reallyDefined_id_intrinsics;
std::set<IndexT> reallyDefined_id_extrinsics;
for (Views::const_iterator iter = sfm_data.getViews().begin();
iter != sfm_data.getViews().end();
++iter)
{
// If a pose is defined, at least the intrinsic must be valid,
// In order to generate a valid camera.
const IndexT id_pose = iter->second.get()->id_pose;
const IndexT id_intrinsic = iter->second.get()->id_intrinsic;
if (set_id_extrinsics.count(id_pose))
reallyDefined_id_extrinsics.insert(id_pose); //at least it exists
if (set_id_intrinsics.count(id_intrinsic))
reallyDefined_id_intrinsics.insert(id_intrinsic); //at least it exists
}
// Check if defined intrinsic & extrinsic are at least connected to views
bool bRet = true;
if (bCheck_Intrinsic)
bRet &= set_id_intrinsics.size() == reallyDefined_id_intrinsics.size();
if (bCheck_Extrinsic)
bRet &= set_id_extrinsics.size() == reallyDefined_id_extrinsics.size();
if (bRet == false)
std::cout << "There is orphan intrinsics data or poses (do not depend on any view)" << std::endl;
return bRet;
}
static bool eraseMissingPoses(SfM_Data & sfm_data, const IndexT min_points_per_pose)
{
IndexT removed_elements = 0;
const Landmarks & landmarks = sfm_data.structure;
// Count the observation poses occurence
Hash_Map<IndexT, IndexT> map_PoseId_Count;
// Init with 0 count (in order to be able to remove non referenced elements)
for (Poses::const_iterator itPoses = sfm_data.getPoses().begin();
itPoses != sfm_data.getPoses().end(); ++itPoses)
{
map_PoseId_Count[itPoses->first] = 0;
}
// Count occurence of the poses in the Landmark observations
for (Landmarks::const_iterator itLandmarks = landmarks.begin();
itLandmarks != landmarks.end(); ++itLandmarks)
{
const Observations & obs = itLandmarks->second.obs;
for (Observations::const_iterator itObs = obs.begin();
itObs != obs.end(); ++itObs)
{
const IndexT ViewId = itObs->first;
const View * v = sfm_data.getViews().at(ViewId).get();
if (map_PoseId_Count.count(v->id_pose))
map_PoseId_Count[v->id_pose] += 1;
else
map_PoseId_Count[v->id_pose] = 0;
}
}
// If usage count is smaller than the threshold, remove the Pose
for (Hash_Map<IndexT, IndexT>::const_iterator it = map_PoseId_Count.begin();
it != map_PoseId_Count.end(); ++it)
{
if (it->second < min_points_per_pose)
{
sfm_data.poses.erase(it->first);
++removed_elements;
}
}
return removed_elements > 0;
}
/// Compute the Root Mean Square Error of the residuals
double RMSE(const SfM_Data & sfm_data)
{
// Compute residuals for each observation
std::vector<double> vec;
for(Landmarks::const_iterator iterTracks = sfm_data.getLandmarks().begin();
iterTracks != sfm_data.getLandmarks().end();
++iterTracks)
{
const Observations & obs = iterTracks->second.obs;
for(Observations::const_iterator itObs = obs.begin();
itObs != obs.end(); ++itObs)
{
const View * view = sfm_data.getViews().find(itObs->first)->second.get();
const Pose3 & pose = sfm_data.getPoses().find(view->id_pose)->second;
const std::shared_ptr<IntrinsicBase> intrinsic = sfm_data.getIntrinsics().find(view->id_intrinsic)->second;
const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x);
vec.push_back( residual(0) );
vec.push_back( residual(1) );
}
}
const Eigen::Map<Eigen::RowVectorXd> residuals(&vec[0], vec.size());
const double RMSE = std::sqrt(residuals.squaredNorm() / vec.size());
return RMSE;
}
/// Use guided matching to find corresponding 2-view correspondences
void match(
const SfM_Data & sfm_data,
const Pair_Set & pairs,
const std::shared_ptr<Regions_Provider> & regions_provider)
{
C_Progress_display my_progress_bar( pairs.size(), std::cout,
"Compute pairwise fundamental guided matching:\n" );
#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel
#endif // OPENMVG_USE_OPENMP
for (Pair_Set::const_iterator it = pairs.begin(); it != pairs.end(); ++it)
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp single nowait
#endif // OPENMVG_USE_OPENMP
{
// --
// Perform GUIDED MATCHING
// --
// Use the computed model to check valid correspondences
// - by considering geometric error and descriptor distance ratio.
std::vector<IndMatch> vec_corresponding_indexes;
const View * viewL = sfm_data.getViews().at(it->first).get();
const Poses::const_iterator iterPoseL = sfm_data.getPoses().find(viewL->id_pose);
const Intrinsics::const_iterator iterIntrinsicL = sfm_data.getIntrinsics().find(viewL->id_intrinsic);
const View * viewR = sfm_data.getViews().at(it->second).get();
const Poses::const_iterator iterPoseR = sfm_data.getPoses().find(viewR->id_pose);
const Intrinsics::const_iterator iterIntrinsicR = sfm_data.getIntrinsics().find(viewR->id_intrinsic);
Mat xL, xR;
PointsToMat(iterIntrinsicL->second.get(), regions_provider->regions_per_view.at(it->first)->GetRegionsPositions(), xL);
PointsToMat(iterIntrinsicR->second.get(), regions_provider->regions_per_view.at(it->second)->GetRegionsPositions(), xR);
const Mat34 P_L = iterIntrinsicL->second.get()->get_projective_equivalent(iterPoseL->second);
const Mat34 P_R = iterIntrinsicR->second.get()->get_projective_equivalent(iterPoseR->second);
const Mat3 F_lr = F_from_P(P_L, P_R);
const double thresholdF = 4.0;
#if defined(EXHAUSTIVE_MATCHING)
// Guided matching considering geometric error and descriptor distance ratio
geometry_aware::GuidedMatching
<Mat3, openMVG::fundamental::kernel::EpipolarDistanceError,
DescriptorT, L2_Vectorized<DescriptorT::bin_type> >(
F_lr, xL, desc_provider.at(it->first), xR, desc_provider.at(it->second),
Square(thresholdF), Square(0.8),
vec_corresponding_indexes);
#else
const Vec3 epipole2 = epipole_from_P(P_R, iterPoseL->second);
const features::Regions * regions = regions_provider->regions_per_view.at(it->first).get();
if (regions->IsScalar())
{
// L2 Metric (Handle descriptor internal type)
if(regions->Type_id() == typeid(unsigned char).name())
{
geometry_aware::GuidedMatching_Fundamental_Fast<
openMVG::fundamental::kernel::EpipolarDistanceError,
L2_Vectorized<unsigned char> >
( F_lr,
epipole2,
regions_provider->regions_per_view.at(it->first).get(),
iterIntrinsicR->second.get()->w(), iterIntrinsicR->second.get()->h(),
regions_provider->regions_per_view.at(it->second).get(),
Square(thresholdF), Square(0.8),
vec_corresponding_indexes);
}
else
if(regions->Type_id() == typeid(float).name())
{
geometry_aware::GuidedMatching_Fundamental_Fast<
openMVG::fundamental::kernel::EpipolarDistanceError,
L2_Vectorized<float> >
( F_lr,
epipole2,
regions_provider->regions_per_view.at(it->first).get(),
iterIntrinsicR->second.get()->w(), iterIntrinsicR->second.get()->h(),
regions_provider->regions_per_view.at(it->second).get(),
Square(thresholdF), Square(0.8),
vec_corresponding_indexes);
}
else
if(regions->Type_id() == typeid(double).name())
{
geometry_aware::GuidedMatching_Fundamental_Fast<
openMVG::fundamental::kernel::EpipolarDistanceError,
L2_Vectorized<double> >
( F_lr,
epipole2,
regions_provider->regions_per_view.at(it->first).get(),
iterIntrinsicR->second.get()->w(), iterIntrinsicR->second.get()->h(),
regions_provider->regions_per_view.at(it->second).get(),
Square(thresholdF), Square(0.8),
vec_corresponding_indexes);
}
}
else
if (regions->IsBinary() && regions->Type_id() == typeid(unsigned char).name())
{
// Hamming metric
geometry_aware::GuidedMatching_Fundamental_Fast<
openMVG::fundamental::kernel::EpipolarDistanceError,
Hamming<unsigned char> >
( F_lr,
epipole2,
regions_provider->regions_per_view.at(it->first).get(),
iterIntrinsicR->second.get()->w(), iterIntrinsicR->second.get()->h(),
regions_provider->regions_per_view.at(it->second).get(),
Square(thresholdF), 0.8,
vec_corresponding_indexes);
}
#endif
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif // OPENMVG_USE_OPENMP
{
++my_progress_bar;
for (size_t i = 0; i < vec_corresponding_indexes.size(); ++i)
putatives_matches[*it].push_back(vec_corresponding_indexes[i]);
}
}
}
}