/// 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;
}
static bool eraseObservationsWithMissingPoses(SfM_Data & sfm_data, const IndexT min_points_per_landmark)
{
IndexT removed_elements = 0;
const Landmarks & landmarks = sfm_data.structure;
std::set<IndexT> pose_Index;
std::transform(sfm_data.poses.begin(), sfm_data.poses.end(),
std::inserter(pose_Index, pose_Index.begin()), std::RetrieveKey());
// For each landmark:
// - Check if we need to keep the observations & the track
Landmarks::iterator itLandmarks = sfm_data.structure.begin();
while (itLandmarks != sfm_data.structure.end())
{
Observations & obs = itLandmarks->second.obs;
Observations::iterator itObs = obs.begin();
while (itObs != obs.end())
{
const IndexT ViewId = itObs->first;
const View * v = sfm_data.getViews().at(ViewId).get();
if (pose_Index.count(v->id_pose) == 0)
{
itObs = obs.erase(itObs);
++removed_elements;
}
else
++itObs;
}
if (obs.empty() || obs.size() < min_points_per_landmark)
itLandmarks = sfm_data.structure.erase(itLandmarks);
else
++itLandmarks;
}
return removed_elements > 0;
}
///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]);
}
}
}
}
/// Filter inconsistent correspondences by using 3-view correspondences on view triplets
void 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< graphUtils::Triplet > Triplets;
const Triplets triplets = graphUtils::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 graphUtils::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.poses.at(view->id_pose);
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);
}
/// Find the color of the SfM_Data Landmarks/structure
void ColorizeTracks(
const SfM_Data & sfm_data,
std::vector<Vec3> & vec_3dPoints,
std::vector<Vec3> & vec_tracksColor)
{
// Colorize each track
// Start with the most representative image
// and iterate to provide a color to each 3D point
{
C_Progress_display my_progress_bar(sfm_data.getLandmarks().size(),
std::cout,
"\nCompute scene structure color\n");
vec_tracksColor.resize(sfm_data.getLandmarks().size());
vec_3dPoints.resize(sfm_data.getLandmarks().size());
//Build a list of contiguous index for the trackIds
std::map<IndexT, IndexT> trackIds_to_contiguousIndexes;
IndexT cpt = 0;
for (Landmarks::const_iterator it = sfm_data.getLandmarks().begin();
it != sfm_data.getLandmarks().end(); ++it, ++cpt)
{
trackIds_to_contiguousIndexes[it->first] = cpt;
vec_3dPoints[cpt] = it->second.X;
}
// The track list that will be colored (point removed during the process)
std::set<IndexT> remainingTrackToColor;
std::transform(sfm_data.getLandmarks().begin(), sfm_data.getLandmarks().end(),
std::inserter(remainingTrackToColor, remainingTrackToColor.begin()),
RetrieveKey() );
while( !remainingTrackToColor.empty() )
{
// Find the most representative image (for the remaining 3D points)
// a. Count the number of observation per view for each 3Dpoint Index
// b. Sort to find the most representative view index
std::map<IndexT, IndexT> map_IndexCardinal; // ViewId, Cardinal
for (std::set<IndexT>::const_iterator
iterT = remainingTrackToColor.begin();
iterT != remainingTrackToColor.end();
++iterT)
{
const size_t trackId = *iterT;
const Observations & obs = sfm_data.getLandmarks().at(trackId).obs;
for( Observations::const_iterator iterObs = obs.begin();
iterObs != obs.end(); ++iterObs)
{
const size_t viewId = iterObs->first;
if (map_IndexCardinal.find(viewId) == map_IndexCardinal.end())
map_IndexCardinal[viewId] = 1;
else
++map_IndexCardinal[viewId];
}
}
// Find the View index that is the most represented
std::vector<IndexT> vec_cardinal;
std::transform(map_IndexCardinal.begin(),
map_IndexCardinal.end(),
std::back_inserter(vec_cardinal),
RetrieveValue());
using namespace indexed_sort;
std::vector< sort_index_packet_descend< IndexT, IndexT> > packet_vec(vec_cardinal.size());
sort_index_helper(packet_vec, &vec_cardinal[0], 1);
// First image index with the most of occurence
std::map<IndexT, IndexT>::const_iterator iterTT = map_IndexCardinal.begin();
std::advance(iterTT, packet_vec[0].index);
const size_t view_index = iterTT->first;
const View * view = sfm_data.getViews().at(view_index).get();
const std::string sView_filename = stlplus::create_filespec(sfm_data.s_root_path,
view->s_Img_path);
Image<RGBColor> image;
ReadImage(sView_filename.c_str(), &image);
// Iterate through the remaining track to color
// - look if the current view is present to color the track
std::set<IndexT> set_toRemove;
for (std::set<IndexT>::const_iterator
iterT = remainingTrackToColor.begin();
iterT != remainingTrackToColor.end();
++iterT)
{
const size_t trackId = *iterT;
const Observations & obs = sfm_data.getLandmarks().at(trackId).obs;
Observations::const_iterator it = obs.find(view_index);
if (it != obs.end())
{
// Color the track
const Vec2 & pt = it->second.x;
const RGBColor color = image(pt.y(), pt.x());
vec_tracksColor[ trackIds_to_contiguousIndexes[trackId] ] = Vec3(color.r(), color.g(), color.b());
set_toRemove.insert(trackId);
++my_progress_bar;
}
}
// Remove colored track
for (std::set<IndexT>::const_iterator iter = set_toRemove.begin();
iter != set_toRemove.end(); ++iter)
{
remainingTrackToColor.erase(*iter);
}
}
}
}