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;
}
void load_textures()
{
const size_t nbCams = vec_cameras.size();
m_image_vector.resize(nbCams);
C_Progress_display my_progress_bar( nbCams, std::cout,
"\n", " " , "Textures loading, Please wait...\n" );
for ( int i_cam=0; i_cam < nbCams; ++i_cam, ++my_progress_bar) {
const View * view = sfm_data.GetViews().at(vec_cameras[i_cam]).get();
const std::string srcImage = stlplus::create_filespec(sfm_data.s_root_path, view->s_Img_path);
std::vector<unsigned char> img;
int w,h,depth;
if (ReadImage(srcImage.c_str(), &img, &w, &h, &depth)) {
glEnable(GL_TEXTURE_2D);
//std::cout << "Read image : " << sImageName << "\n" << std::endl;
glDeleteTextures(1, &m_image_vector[i_cam].texture);
// Create texture
glGenTextures( 1, &m_image_vector[i_cam].texture);
// select our current texture
glBindTexture(GL_TEXTURE_2D, m_image_vector[i_cam].texture);
m_image_vector[i_cam].width = w;
m_image_vector[i_cam].height = h;
glTexImage2D(GL_TEXTURE_2D, 0, (depth == 1) ? GL_LUMINANCE : GL_RGB, m_image_vector[i_cam].width,
m_image_vector[i_cam].height, 0, (depth == 1) ? GL_LUMINANCE : GL_RGB, GL_UNSIGNED_BYTE,
&img[0]);
glBindTexture(GL_TEXTURE_2D, m_image_vector[i_cam].texture);
}
}
}
void Frustum_Filter::init_z_near_z_far_depth
(
const SfM_Data & sfm_data,
const double zNear,
const double zFar
)
{
// If z_near & z_far are -1 and structure if not empty,
// compute the values for each camera and the structure
const bool bComputed_Z = (zNear == -1. && zFar == -1.) && !sfm_data.structure.empty();
if (bComputed_Z) // Compute the near & far planes from the structure and view observations
{
for (Landmarks::const_iterator itL = sfm_data.GetLandmarks().begin();
itL != sfm_data.GetLandmarks().end(); ++itL)
{
const Landmark & landmark = itL->second;
const Vec3 & X = landmark.X;
for (Observations::const_iterator iterO = landmark.obs.begin();
iterO != landmark.obs.end(); ++iterO)
{
const IndexT id_view = iterO->first;
const View * view = sfm_data.GetViews().at(id_view).get();
if (!sfm_data.IsPoseAndIntrinsicDefined(view))
continue;
const Pose3 pose = sfm_data.GetPoseOrDie(view);
const double z = Depth(pose.rotation(), pose.translation(), X);
NearFarPlanesT::iterator itZ = z_near_z_far_perView.find(id_view);
if (itZ != z_near_z_far_perView.end())
{
if ( z < itZ->second.first)
itZ->second.first = z;
else
if ( z > itZ->second.second)
itZ->second.second = z;
}
else
z_near_z_far_perView[id_view] = {z,z};
}
}
}
else
{
// Init the same near & far limit for all the valid views
for (Views::const_iterator it = sfm_data.GetViews().begin();
it != sfm_data.GetViews().end(); ++it)
{
const View * view = it->second.get();
if (!sfm_data.IsPoseAndIntrinsicDefined(view))
continue;
if (z_near_z_far_perView.find(view->id_view) == z_near_z_far_perView.end())
z_near_z_far_perView[view->id_view] = {zNear, zFar};
}
}
}
/// 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());
}
}
virtual bool load(
const SfM_Data & sfm_data,
const std::string & feat_directory,
std::unique_ptr<features::Regions>& region_type)
{
C_Progress_display my_progress_bar( sfm_data.GetViews().size(),
std::cout, "\n- Features Loading -\n" );
// Read for each view the corresponding features and store them as PointFeatures
bool bContinue = true;
#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel
#endif
for (Views::const_iterator iter = sfm_data.GetViews().begin();
iter != sfm_data.GetViews().end() && bContinue; ++iter)
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp single nowait
#endif
{
const std::string sImageName = stlplus::create_filespec(sfm_data.s_root_path, iter->second.get()->s_Img_path);
const std::string basename = stlplus::basename_part(sImageName);
const std::string featFile = stlplus::create_filespec(feat_directory, basename, ".feat");
std::unique_ptr<features::Regions> regions(region_type->EmptyClone());
if (!regions->LoadFeatures(featFile))
{
std::cerr << "Invalid feature files for the view: " << sImageName << std::endl;
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif
bContinue = false;
}
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif
{
// save loaded Features as PointFeature
feats_per_view[iter->second.get()->id_view] = regions->GetRegionsPositions();
++my_progress_bar;
}
}
}
return bContinue;
}
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;
}
/// Triangulate a given track from a selection of observations
Vec3 SfM_Data_Structure_Computation_Robust::track_sample_triangulation(
const SfM_Data & sfm_data,
const Observations & obs,
const std::set<IndexT> & samples) const
{
Triangulation trianObj;
for (auto& it : samples)
{
const IndexT & idx = it;
Observations::const_iterator itObs = obs.begin();
std::advance(itObs, idx);
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);
trianObj.add(
cam->get_projective_equivalent(pose),
cam->get_ud_pixel(itObs->second.x));
}
return trianObj.compute();
}
bool CreateCameraFile( const SfM_Data & sfm_data,
const std::string & sCamerasFilename)
{
/* cameras.txt
# Camera list with one line of data per camera:
# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]
# Number of cameras: X
*/
std::ofstream camera_file( sCamerasFilename );
if ( ! camera_file )
{
std::cerr << "Cannot write file" << sCamerasFilename << std::endl;
return false;
}
camera_file << "# Camera list with one line of data per camera:\n";
camera_file << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n";
camera_file << "# Number of cameras: X\n";
std::vector<std::string> camera_lines;
C_Progress_display my_progress_bar( sfm_data.GetIntrinsics().size(), std::cout, "\n- CREATE CAMERA FILE -\n" );
for (Intrinsics::const_iterator iter = sfm_data.GetIntrinsics().begin();
iter != sfm_data.GetIntrinsics().end(); ++iter, ++my_progress_bar)
{
const IndexT camera_id = iter->first;
std::shared_ptr<openMVG::cameras::IntrinsicBase> intrinsic = iter->second;
std::string camera_line;
if (CreateLineCameraFile(camera_id, intrinsic, camera_line))
{
camera_lines.push_back(camera_line);
}
else
{
return false;
}
}
for (auto const& camera_line: camera_lines)
{
camera_file << camera_line << "\n";
}
return true;
}
/// 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;
}
/// Build a list of pair that share visibility content from the SfM_Data structure
Pair_Set BuildPairsFromStructureObservations(const SfM_Data & sfm_data)
{
Pair_Set pairs;
for (Landmarks::const_iterator itL = sfm_data.GetLandmarks().begin();
itL != sfm_data.GetLandmarks().end(); ++itL)
{
const Landmark & landmark = itL->second;
for (Observations::const_iterator iterI = landmark.obs.begin();
iterI != landmark.obs.end(); ++iterI)
{
const IndexT id_viewI = iterI->first;
Observations::const_iterator iterJ = landmark.obs.begin();
std::advance(iterJ, 1);
for (; iterJ != landmark.obs.end(); ++iterJ)
{
const IndexT id_viewJ = iterJ->first;
pairs.insert( std::make_pair(id_viewI,id_viewJ));
}
}
}
return pairs;
}
/// 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.GetPoseOrDie(view);
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;
}
// Init a frustum for each valid views of the SfM scene
void Frustum_Filter::initFrustum
(
const SfM_Data & sfm_data
)
{
for (NearFarPlanesT::const_iterator it = z_near_z_far_perView.begin();
it != z_near_z_far_perView.end(); ++it)
{
const View * view = sfm_data.GetViews().at(it->first).get();
if (!sfm_data.IsPoseAndIntrinsicDefined(view))
continue;
Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
if (!isPinhole(iterIntrinsic->second->getType()))
continue;
const Pose3 pose = sfm_data.GetPoseOrDie(view);
const Pinhole_Intrinsic * cam = dynamic_cast<const Pinhole_Intrinsic*>(iterIntrinsic->second.get());
if (!cam)
continue;
if (!_bTruncated) // use infinite frustum
{
const Frustum f(
cam->w(), cam->h(), cam->K(),
pose.rotation(), pose.center());
frustum_perView[view->id_view] = f;
}
else // use truncated frustum with defined Near and Far planes
{
const Frustum f(cam->w(), cam->h(), cam->K(),
pose.rotation(), pose.center(), it->second.first, it->second.second);
frustum_perView[view->id_view] = f;
}
}
}
void ImageList::sequentialReconstruct() {
string output_directory = "/home/colin/catkin_ws/src/aslam_project/SfM_PointCloudData";
string sfm_data = stlplus::create_filespec(output_directory, "sfm_data", ".json");
string cloud_data = stlplus::create_filespec(output_directory, "cloud_and_poses", ".ply");
string report_name = stlplus::create_filespec(output_directory, "Reconstruction_Report", ".html");
if (!stlplus::folder_exists(output_directory))
stlplus::folder_create(output_directory);
SequentialSfMReconstructionEngine sfmEngine(_sfm_data, output_directory, report_name);
shared_ptr<Features_Provider> feats_provider = std::make_shared<Features_Provider>();
shared_ptr<Matches_Provider> matches_provider = std::make_shared<Matches_Provider>();
feats_provider->load(_sfm_data, _directory, _regionsType);
matches_provider->load(_sfm_data, _matches_filtered);
sfmEngine.SetFeaturesProvider(feats_provider.get());
sfmEngine.SetMatchesProvider(matches_provider.get());
openMVG::Pair initialPairIndex;
Views::const_iterator it;
it = _sfm_data.GetViews().begin();
const View *v1 = it->second.get();
it++;
const View *v2 = it->second.get();
initialPairIndex.first = v1->id_view;
initialPairIndex.second = v2->id_view;
sfmEngine.setInitialPair(initialPairIndex);
sfmEngine.Set_bFixedIntrinsics(false);
sfmEngine.SetUnknownCameraType(EINTRINSIC(PINHOLE_CAMERA_RADIAL3));
sfmEngine.Process();
Save(sfmEngine.Get_SfM_Data(), sfm_data, ESfM_Data(openMVG::sfm::ALL));
Save(sfmEngine.Get_SfM_Data(), cloud_data, ESfM_Data(openMVG::sfm::ALL));
}
///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()), stl::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()), stl::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;
}
/// 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);
}
/// Use guided matching to find corresponding 2-view correspondences
void SfM_Data_Structure_Estimation_From_Known_Poses::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 Pose3 poseL = sfm_data.GetPoseOrDie(viewL);
const Intrinsics::const_iterator iterIntrinsicL = sfm_data.GetIntrinsics().find(viewL->id_intrinsic);
const View * viewR = sfm_data.GetViews().at(it->second).get();
const Pose3 poseR = sfm_data.GetPoseOrDie(viewR);
const Intrinsics::const_iterator iterIntrinsicR = sfm_data.GetIntrinsics().find(viewR->id_intrinsic);
if (sfm_data.GetIntrinsics().count(viewL->id_intrinsic) != 0 ||
sfm_data.GetIntrinsics().count(viewR->id_intrinsic) != 0)
{
const Mat34 P_L = iterIntrinsicL->second.get()->get_projective_equivalent(poseL);
const Mat34 P_R = iterIntrinsicR->second.get()->get_projective_equivalent(poseR);
const Mat3 F_lr = F_from_P(P_L, P_R);
const double thresholdF = 4.0;
#if defined(EXHAUSTIVE_MATCHING)
geometry_aware::GuidedMatching
<Mat3, openMVG::fundamental::kernel::EpipolarDistanceError>
(
F_lr,
iterIntrinsicL->second.get(),
*regions_provider->regions_per_view.at(it->first),
iterIntrinsicR->second.get(),
*regions_provider->regions_per_view.at(it->second),
Square(thresholdF), Square(0.8),
vec_corresponding_indexes
);
#else
const Vec3 epipole2 = epipole_from_P(P_R, poseL);
const features::Regions * regions = regions_provider->regions_per_view.at(it->first).get();
geometry_aware::GuidedMatching_Fundamental_Fast
<openMVG::fundamental::kernel::EpipolarDistanceError>
(
F_lr,
epipole2,
iterIntrinsicL->second.get(),
*regions_provider->regions_per_view.at(it->first),
iterIntrinsicR->second.get(),
*regions_provider->regions_per_view.at(it->second),
iterIntrinsicR->second->w(), iterIntrinsicR->second->h(),
Square(thresholdF), Square(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]);
}
}
}
}
}
/// Save SfM_Data in an ASCII BAF (Bundle Adjustment File).
// --Header
// #Intrinsics
// #Poses
// #Landmarks
// --Data
// Intrinsic parameters [foc ppx ppy, ...]
// Poses [angle axis, camera center]
// Landmarks [X Y Z #observations id_intrinsic id_pose x y ...]
//--
//- Export also a _imgList.txt file with View filename and id_intrinsic & id_pose.
// filename id_intrinsic id_pose
// The ids allow to establish a link between 3D point observations & the corresponding views
//--
// Export missing poses as Identity pose to keep tracking of the original id_pose indexes
static bool Save_BAF(
const SfM_Data & sfm_data,
const std::string & filename,
ESfM_Data flags_part)
{
std::ofstream stream(filename.c_str());
if (!stream.is_open())
return false;
bool bOk = false;
{
stream
<< sfm_data.GetIntrinsics().size() << '\n'
<< sfm_data.GetViews().size() << '\n'
<< sfm_data.GetLandmarks().size() << '\n';
const Intrinsics & intrinsics = sfm_data.GetIntrinsics();
for (Intrinsics::const_iterator iterIntrinsic = intrinsics.begin();
iterIntrinsic != intrinsics.end(); ++iterIntrinsic)
{
//get params
const std::vector<double> intrinsicsParams = iterIntrinsic->second.get()->getParams();
std::copy(intrinsicsParams.begin(), intrinsicsParams.end(),
std::ostream_iterator<double>(stream, " "));
stream << '\n';
}
const Poses & poses = sfm_data.GetPoses();
for (Views::const_iterator iterV = sfm_data.GetViews().begin();
iterV != sfm_data.GetViews().end();
++ iterV)
{
const View * view = iterV->second.get();
if (!sfm_data.IsPoseAndIntrinsicDefined(view))
{
const Mat3 R = Mat3::Identity();
const double * rotation = R.data();
std::copy(rotation, rotation+9, std::ostream_iterator<double>(stream, " "));
const Vec3 C = Vec3::Zero();
const double * center = C.data();
std::copy(center, center+3, std::ostream_iterator<double>(stream, " "));
stream << '\n';
}
else
{
// [Rotation col major 3x3; camera center 3x1]
const double * rotation = poses.at(view->id_pose).rotation().data();
std::copy(rotation, rotation+9, std::ostream_iterator<double>(stream, " "));
const double * center = poses.at(view->id_pose).center().data();
std::copy(center, center+3, std::ostream_iterator<double>(stream, " "));
stream << '\n';
}
}
const Landmarks & landmarks = sfm_data.GetLandmarks();
for (Landmarks::const_iterator iterLandmarks = landmarks.begin();
iterLandmarks != landmarks.end();
++iterLandmarks)
{
// Export visibility information
// X Y Z #observations id_cam id_pose x y ...
const double * X = iterLandmarks->second.X.data();
std::copy(X, X+3, std::ostream_iterator<double>(stream, " "));
const Observations & obs = iterLandmarks->second.obs;
stream << obs.size() << " ";
for (Observations::const_iterator iterOb = obs.begin();
iterOb != obs.end(); ++iterOb)
{
const IndexT id_view = iterOb->first;
const View * v = sfm_data.GetViews().at(id_view).get();
stream
<< v->id_intrinsic << ' '
<< v->id_pose << ' '
<< iterOb->second.x(0) << ' ' << iterOb->second.x(1) << ' ';
}
stream << '\n';
}
stream.flush();
bOk = stream.good();
stream.close();
}
// Export View filenames & ids as an imgList.txt file
{
const std::string sFile = stlplus::create_filespec(
stlplus::folder_part(filename), stlplus::basename_part(filename) + std::string("_imgList"), "txt");
stream.open(sFile.c_str());
if (!stream.is_open())
return false;
for (Views::const_iterator iterV = sfm_data.GetViews().begin();
iterV != sfm_data.GetViews().end();
++ iterV)
{
const std::string sView_filename = stlplus::create_filespec(sfm_data.s_root_path,
iterV->second->s_Img_path);
stream
<< sView_filename
<< ' ' << iterV->second->id_intrinsic
<< ' ' << iterV->second->id_pose << "\n";
}
stream.flush();
bOk = stream.good();
stream.close();
}
return bOk;
}
int main(int argc, char *argv[]) {
CmdLine cmd;
std::string sSfM_Data_Filename;
cmd.add( make_option('i', sSfM_Data_Filename, "sfmdata") );
try {
if (argc == 1) throw std::string("Invalid command line parameter.");
cmd.process(argc, argv);
} catch(const std::string& s) {
std::cerr << "Usage: " << argv[0] << '\n'
<< "[-i|--sfmdata filename, the SfM_Data file to read]\n"
<< std::endl;
std::cerr << s << std::endl;
return EXIT_FAILURE;
}
// Read the SfM scene
if (!Load(sfm_data, sSfM_Data_Filename, ESfM_Data(ALL))) {
std::cerr << std::endl
<< "The input SfM_Data file \""<< sSfM_Data_Filename << "\" cannot be read." << std::endl;
return EXIT_FAILURE;
}
// List valid camera (view that have a pose & a valid intrinsic data)
for(Views::const_iterator iter = sfm_data.GetViews().begin();
iter != sfm_data.GetViews().end(); ++iter)
{
const View * view = iter->second.get();
if (!sfm_data.IsPoseAndIntrinsicDefined(view))
continue;
vec_cameras.push_back(iter->first);
}
current_cam = 0;
std::cout << "Press left or right key to navigate between cameras ;-)" << std::endl
<< "Move viewpoint with Q,W,E,A,S,D" << std::endl
<< "Change Normalized focal (camera cones size) with '+' and '-'" << std::endl
<< "Reset viewpoint position with R" << std::endl
<< "Esc to quit" << std::endl;
//-- Create the GL window context
GLFWwindow* window;
int width, height;
if( !glfwInit() )
{
fprintf( stderr, "Failed to initialize GLFW\n" );
exit( EXIT_FAILURE );
}
glfwWindowHint(GLFW_DEPTH_BITS, 16);
window = glfwCreateWindow( 1000, 600, "SfmViewer", NULL, NULL );
if (!window)
{
fprintf( stderr, "Failed to open GLFW window\n" );
glfwTerminate();
exit( EXIT_FAILURE );
}
// Set callback functions
glfwSetWindowCloseCallback(window, window_close_callback);
glfwSetWindowSizeCallback(window, reshape);
glfwSetKeyCallback(window, key);
glfwMakeContextCurrent(window);
glfwSwapInterval( 1 );
glfwGetWindowSize(window, &width, &height);
reshape(window, width, height);
load_textures();
// Main loop
while( running )
{
// Draw SfM Scene
draw();
// Swap buffers
glfwSwapBuffers(window);
glfwPollEvents();
}
// Terminate GLFW
glfwTerminate();
// Exit program
exit( EXIT_SUCCESS );
}
/* OpenGL draw function & timing */
static void draw(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
{
// convert opengl coordinates into the document information coordinates
glPushMatrix();
glMultMatrixf((GLfloat*)m_convert);
// apply view offset
openMVG::Mat4 offset_w = l2w_Camera(Mat3::Identity(), Vec3(x_offset,y_offset,z_offset));
glMultMatrixd((GLdouble*)offset_w.data());
// then apply current camera transformation
const View * view = sfm_data.GetViews().at(vec_cameras[current_cam]).get();
const Pose3 pose = sfm_data.GetPoseOrDie(view);
const openMVG::Mat4 l2w = l2w_Camera(pose.rotation(), pose.translation());
glPushMatrix();
glMultMatrixd((GLdouble*)l2w.data());
glPointSize(3);
glDisable(GL_TEXTURE_2D);
glDisable(GL_LIGHTING);
//Draw Structure in GREEN (as seen from the current camera)
size_t nbPoint = sfm_data.GetLandmarks().size();
size_t cpt = 0;
glBegin(GL_POINTS);
glColor3f(0.f,1.f,0.f);
for (Landmarks::const_iterator iter = sfm_data.GetLandmarks().begin();
iter != sfm_data.GetLandmarks().end(); ++iter)
{
const Landmark & landmark = iter->second;
glVertex3d(landmark.X(0), landmark.X(1), landmark.X(2));
}
glEnd();
glDisable(GL_CULL_FACE);
for (int i_cam=0; i_cam < vec_cameras.size(); ++i_cam)
{
const View * view = sfm_data.GetViews().at(vec_cameras[i_cam]).get();
const Pose3 pose = sfm_data.GetPoseOrDie(view);
const IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
if (isPinhole(cam->getType()))
{
const Pinhole_Intrinsic * camPinhole = dynamic_cast<const Pinhole_Intrinsic*>(cam);
// Move frame to draw the camera i_cam by applying its inverse transformation
// Warning: translation has to be "fixed" to remove the current camera rotation
// Fix camera_i translation with current camera rotation inverse
const Vec3 trans = pose.rotation().transpose() * pose.translation();
// compute inverse transformation matrix from local to world
const openMVG::Mat4 l2w_i = l2w_Camera(pose.rotation().transpose(), -trans);
// stack it and use it
glPushMatrix();
glMultMatrixd((GLdouble*)l2w_i.data());
// 1. Draw optical center (RED) and image center (BLUE)
glPointSize(3);
glDisable(GL_TEXTURE_2D);
glDisable(GL_LIGHTING);
glBegin(GL_POINTS);
glColor3f(1.f,0.f,0.f);
glVertex3f(0, 0, 0); // optical center
glColor3f(0.f,0.f,1.f);
glVertex3f(0, 0, normalized_focal); // image center
glEnd();
// compute image corners coordinated with normalized focal (f=normalized_focal)
const int w = camPinhole->w();
const int h = camPinhole->h();
const double focal = camPinhole->focal();
// use principal point to adjust image center
const Vec2 pp = camPinhole->principal_point();
Vec3 c1( -pp[0]/focal * normalized_focal, (-pp[1]+h)/focal * normalized_focal, normalized_focal);
Vec3 c2((-pp[0]+w)/focal * normalized_focal, (-pp[1]+h)/focal * normalized_focal, normalized_focal);
Vec3 c3((-pp[0]+w)/focal * normalized_focal, -pp[1]/focal * normalized_focal, normalized_focal);
Vec3 c4( -pp[0]/focal * normalized_focal, -pp[1]/focal * normalized_focal, normalized_focal);
// 2. Draw thumbnail
if (i_cam == current_cam)
{
glEnable(GL_TEXTURE_2D);
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glBindTexture(GL_TEXTURE_2D, m_image_vector[i_cam].texture);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glDisable(GL_DEPTH_TEST);
if (i_cam == current_cam) {
glColor4f(0.5f,0.5f,0.5f, 0.7f);
} else {
glColor4f(0.5f,0.5f,0.5f, 0.5f);
}
glBegin(GL_QUADS);
glTexCoord2d(0.0,1.0); glVertex3d(c1[0], c1[1], c1[2]);
glTexCoord2d(1.0,1.0); glVertex3d(c2[0], c2[1], c2[2]);
glTexCoord2d(1.0,0.0); glVertex3d(c3[0], c3[1], c3[2]);
glTexCoord2d(0.0,0.0); glVertex3d(c4[0], c4[1], c4[2]);
glEnd();
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND); glEnable(GL_DEPTH_TEST);
}
// 3. Draw camera cone
if (i_cam == current_cam) {
glColor3f(1.f,1.f,0.f);
} else {
glColor3f(1.f,0.f,0.f);
}
glBegin(GL_LINES);
glVertex3d(0.0,0.0,0.0); glVertex3d(c1[0], c1[1], c1[2]);
glVertex3d(0.0,0.0,0.0); glVertex3d(c2[0], c2[1], c2[2]);
glVertex3d(0.0,0.0,0.0); glVertex3d(c3[0], c3[1], c3[2]);
glVertex3d(0.0,0.0,0.0); glVertex3d(c4[0], c4[1], c4[2]);
glVertex3d(c1[0], c1[1], c1[2]); glVertex3d(c2[0], c2[1], c2[2]);
glVertex3d(c2[0], c2[1], c2[2]); glVertex3d(c3[0], c3[1], c3[2]);
glVertex3d(c3[0], c3[1], c3[2]); glVertex3d(c4[0], c4[1], c4[2]);
glVertex3d(c4[0], c4[1], c4[2]); glVertex3d(c1[0], c1[1], c1[2]);
glEnd();
glPopMatrix(); // go back to current camera frame
}
}
glPopMatrix(); // go back to (document +offset) frame
glPopMatrix(); // go back to identity
}
}
void ImageList::computeMatches() {
float fDistRatio = 0.6f; // 0.6f dflt // Higher is stricter
openMVG::matching::PairWiseMatches map_PutativesMatches;
vector<string> vec_fileNames;
vector<pair<size_t, size_t> > vec_imagesSize;
for (Views::const_iterator iter = _sfm_data.GetViews().begin(); iter != _sfm_data.GetViews().end(); ++iter) {
const View * v = iter->second.get();
vec_fileNames.push_back(stlplus::create_filespec(_sfm_data.s_root_path, v->s_Img_path));
vec_imagesSize.push_back(make_pair( v->ui_width, v->ui_height) );
}
unique_ptr<Matcher_Regions_AllInMemory> collectionMatcher;
collectionMatcher.reset(new Matcher_Regions_AllInMemory(fDistRatio, openMVG::ANN_L2));
/*
//Sample code section updated to conform to latest library updates
//https://github.com/openMVG/openMVG/commit/6029189456152aa5dd4de0a822da5d2c06835a36
//no function loadData under collectionmatcher
if (collectionMatcher->loadData(_regionsType, vec_fileNames, _directory)) {
openMVG::Pair_Set pairs;
pairs = openMVG::exhaustivePairs(_sfm_data.GetViews().size());
//no function Match under collectionmatcher
collectionMatcher->Match(vec_fileNames, pairs, map_PutativesMatches);
ofstream file(_matches_full);
if (file.is_open())
PairedIndMatchToStream(map_PutativesMatches, file);
file.close();
}*/
openMVG::Pair_Set pairs;
shared_ptr<Regions_Provider> regions_provider = make_shared<Regions_Provider>(); //added
if (!regions_provider->load(_sfm_data, _directory, _regionsType)){ //added
ROS_ERROR("Regions provider load failed");
return;
}
//no longer checking if data loads
pairs = openMVG::exhaustivePairs(_sfm_data.GetViews().size());
collectionMatcher->Match(_sfm_data,regions_provider, pairs, map_PutativesMatches); //revised
ofstream file_full(_matches_full); //changed file object name to avoid conflict
if (file_full.is_open()){
PairedIndMatchToStream(map_PutativesMatches, file_full);
}
file_full.close();
shared_ptr<Features_Provider> feats_provider = make_shared<Features_Provider>();
if (!feats_provider->load(_sfm_data, _directory, _regionsType)){
ROS_ERROR("Features provider load failed");
return;
}
openMVG::PairWiseMatches map_GeometricMatches;
//no function collectionGeomFilter
//ImageCollectionGeometricFilter collectionGeomFilter(feats_provider.get());
std::unique_ptr<ImageCollectionGeometricFilter> filter_ptr(new ImageCollectionGeometricFilter(&_sfm_data, regions_provider));
const double maxResidualError = 1.0; // dflt 1.0; // Lower is stricter
//no member method "Filter" after library changes
//https://github.com/openMVG/openMVG/commit/fa8ab91c04dd0828ce60e9b7e74069f5cc54d4d6
/*
collectionGeomFilter.Filter(
GeometricFilter_FMatrix_AC(maxResidualError),
map_PutativesMatches,
map_GeometricMatches,
vec_imagesSize);
*/
filter_ptr->Robust_model_estimation(GeometricFilter_FMatrix_AC(maxResidualError), map_PutativesMatches); //added
map_GeometricMatches = filter_ptr->Get_geometric_matches(); //added
ofstream file_filtered(_matches_filtered); //changed file object name to avoid conflict
if (file_filtered.is_open()){
PairedIndMatchToStream(map_GeometricMatches, file_filtered);
}
file_filtered.close();
}
bool CreateImageFile( const SfM_Data & sfm_data,
const std::string & sImagesFilename)
{
/* images.txt
# Image list with two lines of data per image:
# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME
# POINTS2D[] as (X, Y, POINT3D_ID)
# Number of images: X, mean observations per image: Y
*/
// Header
std::ofstream images_file( sImagesFilename );
if ( ! images_file )
{
std::cerr << "Cannot write file" << sImagesFilename << std::endl;
return false;
}
images_file << "# Image list with two lines of data per image:\n";
images_file << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n";
images_file << "# POINTS2D[] as (X, Y, POINT3D_ID)\n";
images_file << "# Number of images: X, mean observations per image: Y\n";
std::map< IndexT, std::vector< std::tuple<double, double, IndexT> > > viewIdToPoints2D;
const Landmarks & landmarks = sfm_data.GetLandmarks();
{
for ( Landmarks::const_iterator iterLandmarks = landmarks.begin();
iterLandmarks != landmarks.end(); ++iterLandmarks)
{
const IndexT point3d_id = iterLandmarks->first;
// Tally set of feature observations
const Observations & obs = iterLandmarks->second.obs;
for ( Observations::const_iterator itObs = obs.begin(); itObs != obs.end(); ++itObs )
{
const IndexT currentViewId = itObs->first;
const Observation & ob = itObs->second;
viewIdToPoints2D[currentViewId].push_back(std::make_tuple(ob.x( 0 ), ob.x( 1 ), point3d_id));
}
}
}
{
C_Progress_display my_progress_bar( sfm_data.GetViews().size(), std::cout, "\n- CREATE IMAGE FILE -\n" );
for (Views::const_iterator iter = sfm_data.GetViews().begin();
iter != sfm_data.GetViews().end(); ++iter, ++my_progress_bar)
{
const View * view = iter->second.get();
if ( !sfm_data.IsPoseAndIntrinsicDefined( view ) )
{
continue;
}
const Pose3 pose = sfm_data.GetPoseOrDie( view );
const Mat3 rotation = pose.rotation();
const Vec3 translation = pose.translation();
const double Tx = translation[0];
const double Ty = translation[1];
const double Tz = translation[2];
Eigen::Quaterniond q( rotation );
const double Qx = q.x();
const double Qy = q.y();
const double Qz = q.z();
const double Qw = q.w();
const IndexT image_id = view->id_view;
// Colmap's camera_ids correspond to openMVG's intrinsic ids
const IndexT camera_id = view->id_intrinsic;
const std::string image_name = view->s_Img_path;
// first line per image
//IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME
images_file << image_id << " "
<< Qw << " "
<< Qx << " "
<< Qy << " "
<< Qz << " "
<< Tx << " "
<< Ty << " "
<< Tz << " "
<< camera_id << " "
<< image_name << " "
<< "\n";
// second line per image
//POINTS2D[] as (X, Y, POINT3D_ID)
for (auto point2D: viewIdToPoints2D[image_id])
{
images_file << std::get<0>(point2D) << " " <<
std::get<1>(point2D) << " " <<
std::get<2>(point2D) << " ";
}
images_file << "\n";
}
}
return true;
}
virtual void triangulate(SfM_Data & sfm_data) const
{
std::deque<IndexT> rejectedId;
std::unique_ptr<C_Progress_display> my_progress_bar;
if (_bConsoleVerbose)
my_progress_bar.reset( new C_Progress_display(
sfm_data.structure.size(),
std::cout,
"Blind triangulation progress:\n" ));
#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel
#endif
for(Landmarks::iterator iterTracks = sfm_data.structure.begin();
iterTracks != sfm_data.structure.end();
++iterTracks)
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp single nowait
#endif
{
if (_bConsoleVerbose)
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif
++(*my_progress_bar);
}
// Triangulate each landmark
Triangulation trianObj;
const Observations & obs = iterTracks->second.obs;
for(Observations::const_iterator itObs = obs.begin();
itObs != obs.end(); ++itObs)
{
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.poses.at(view->id_pose);
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
{
iterTracks->second.X = X;
}
else
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif
{
rejectedId.push_front(iterTracks->first);
}
}
}
}
// Erase the unsuccessful triangulated tracks
for (auto& it : rejectedId)
{
sfm_data.structure.erase(it);
}
}
/// Robustly try to estimate the best 3D point using a ransac Scheme
/// Return true for a successful triangulation
bool robust_triangulation(
const SfM_Data & sfm_data,
const Observations & obs,
Vec3 & X,
const IndexT min_required_inliers = 3,
const IndexT min_sample_index = 3) 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.poses.at(view->id_pose);
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.poses.at(view->id_pose);
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();
}
bool exportToCMPMVSFormat(
const SfM_Data & sfm_data,
const std::string & sOutDirectory // Output CMPMVS files directory
)
{
bool bOk = true;
// Create basis directory structure
if (!stlplus::is_folder(sOutDirectory))
{
stlplus::folder_create(sOutDirectory);
bOk = stlplus::is_folder(sOutDirectory);
}
if (!bOk)
{
std::cerr << "Cannot access to one of the desired output directory" << std::endl;
return false;
}
else
{
// Export data :
C_Progress_display my_progress_bar( sfm_data.GetViews().size()*2 );
// Since CMPMVS requires contiguous camera index, and that some views can have some missing poses,
// we reindex the poses to ensure a contiguous pose list.
Hash_Map<IndexT, IndexT> map_viewIdToContiguous;
// Export valid views as Projective Cameras:
for(Views::const_iterator iter = sfm_data.GetViews().begin();
iter != sfm_data.GetViews().end(); ++iter, ++my_progress_bar)
{
const View * view = iter->second.get();
if (!sfm_data.IsPoseAndIntrinsicDefined(view))
continue;
const Pose3 pose = sfm_data.GetPoseOrDie(view);
Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
// View Id re-indexing
map_viewIdToContiguous.insert(std::make_pair(view->id_view, map_viewIdToContiguous.size()));
// We have a valid view with a corresponding camera & pose
const Mat34 P = iterIntrinsic->second.get()->get_projective_equivalent(pose);
std::ostringstream os;
os << std::setw(5) << std::setfill('0') << map_viewIdToContiguous[view->id_view] << "_P";
std::ofstream file(
stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory),
os.str() ,"txt").c_str());
file << "CONTOUR" << os.widen('\n')
<< P.row(0) <<"\n"<< P.row(1) <<"\n"<< P.row(2) << os.widen('\n');
file.close();
}
// Export (calibrated) views as undistorted images
std::pair<unsigned int, unsigned int> w_h_image_size;
Image<RGBColor> image, image_ud;
for(Views::const_iterator iter = sfm_data.GetViews().begin();
iter != sfm_data.GetViews().end(); ++iter, ++my_progress_bar)
{
const View * view = iter->second.get();
if (!sfm_data.IsPoseAndIntrinsicDefined(view))
continue;
Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
// We have a valid view with a corresponding camera & pose
const std::string srcImage = stlplus::create_filespec(sfm_data.s_root_path, view->s_Img_path);
std::ostringstream os;
os << std::setw(5) << std::setfill('0') << map_viewIdToContiguous[view->id_view];
std::string dstImage = stlplus::create_filespec(
stlplus::folder_append_separator(sOutDirectory), os.str(),"jpg");
const IntrinsicBase * cam = iterIntrinsic->second.get();
if (map_viewIdToContiguous[view->id_view] == 0)
w_h_image_size = std::make_pair(cam->w(), cam->h());
else
{
// check that there is no image sizing change (CMPMVS support only images of the same size)
if (cam->w() != w_h_image_size.first ||
cam->h() != w_h_image_size.second)
{
std::cerr << "CMPMVS support only image having the same image size";
return false;
}
}
if (cam->have_disto())
{
// undistort the image and save it
ReadImage( srcImage.c_str(), &image);
UndistortImage(image, cam, image_ud, BLACK);
WriteImage(dstImage.c_str(), image_ud);
}
else // (no distortion)
{
// copy the image if extension match
if (stlplus::extension_part(srcImage) == "JPG" ||
stlplus::extension_part(srcImage) == "jpg")
{
stlplus::file_copy(srcImage, dstImage);
}
else
{
ReadImage( srcImage.c_str(), &image);
WriteImage( dstImage.c_str(), image);
}
}
}
// Write the mvs_firstRun script
std::ostringstream os;
os << "[global]" << os.widen('\n')
<< "dirName=\"" << stlplus::folder_append_separator(sOutDirectory) <<"\"" << os.widen('\n')
<< "prefix=\"\"" << os.widen('\n')
<< "imgExt=\"jpg\"" << os.widen('\n')
<< "ncams=" << map_viewIdToContiguous.size() << os.widen('\n')
<< "width=" << w_h_image_size.first << os.widen('\n')
<< "height=" << w_h_image_size.second << os.widen('\n')
<< "scale=2" << os.widen('\n')
<< "workDirName=\"_tmp_fast\"" << os.widen('\n')
<< "doPrepareData=TRUE" << os.widen('\n')
<< "doPrematchSifts=TRUE" << os.widen('\n')
<< "doPlaneSweepingSGM=TRUE" << os.widen('\n')
<< "doFuse=TRUE" << os.widen('\n')
<< "nTimesSimplify=10" << os.widen('\n')
<< os.widen('\n')
<< "[prematching]" << os.widen('\n')
<< "minAngle=3.0" << os.widen('\n')
<< os.widen('\n')
<< "[grow]" << os.widen('\n')
<< "minNumOfConsistentCams=6" << os.widen('\n')
<< os.widen('\n')
<< "[filter]" << os.widen('\n')
<< "minNumOfConsistentCams=2" << os.widen('\n')
<< os.widen('\n')
<< "#do not erase empy lines after this comment otherwise it will crash ... bug" << os.widen('\n')
<< os.widen('\n')
<< os.widen('\n');
std::ofstream file(
stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory),
"01_mvs_firstRun" ,"ini").c_str());
file << os.str();
file.close();
// limitedScale
os.str("");
os << "[global]" << os.widen('\n')
<< "dirName=\"" << stlplus::folder_append_separator(sOutDirectory) <<"\"" << os.widen('\n')
<< "prefix=\"\"" << os.widen('\n')
<< "imgExt=\"jpg\"" << os.widen('\n')
<< "ncams=" << map_viewIdToContiguous.size() << os.widen('\n')
<< "width=" << w_h_image_size.first << os.widen('\n')
<< "height=" << w_h_image_size.second << os.widen('\n')
<< "scale=2" << os.widen('\n')
<< "workDirName=\"_tmp_fast\"" << os.widen('\n')
<< "doPrepareData=FALSE" << os.widen('\n')
<< "doPrematchSifts=FALSE" << os.widen('\n')
<< "doPlaneSweepingSGM=FALSE" << os.widen('\n')
<< "doFuse=FALSE" << os.widen('\n')
<< os.widen('\n')
<< "[uvatlas]" << os.widen('\n')
<< "texSide=1024" << os.widen('\n')
<< "scale=1" << os.widen('\n')
<< os.widen('\n')
<< "[delanuaycut]" << os.widen('\n')
<< "saveMeshTextured=FALSE" << os.widen('\n')
<< os.widen('\n')
<< "[hallucinationsFiltering]" << os.widen('\n')
<< "useSkyPrior=FALSE" << os.widen('\n')
<< "doLeaveLargestFullSegmentOnly=FALSE" << os.widen('\n')
<< "doRemoveHugeTriangles=TRUE" << os.widen('\n')
<< os.widen('\n')
<< "[largeScale]" << os.widen('\n')
<< "doGenerateAndReconstructSpaceMaxPts=TRUE" << os.widen('\n')
<< "doGenerateSpace=TRUE" << os.widen('\n')
<< "planMaxPts=3000000" << os.widen('\n')
<< "doComputeDEMandOrtoPhoto=FALSE" << os.widen('\n')
<< "doGenerateVideoFrames=FALSE" << os.widen('\n')
<< os.widen('\n')
<< "[meshEnergyOpt]" << os.widen('\n')
<< "doOptimizeOrSmoothMesh=FALSE" << os.widen('\n')
<< os.widen('\n')
<< os.widen('\n')
<< "#EOF" << os.widen('\n')
<< os.widen('\n')
<< os.widen('\n');
std::ofstream file2(
stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory),
"02_mvs_limitedScale" ,"ini").c_str());
file2 << os.str();
file2.close();
}
return bOk;
}
/// Use guided matching to find corresponding 2-view correspondences
void SfM_Data_Structure_Estimation_From_Known_Poses::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 Pose3 poseL = sfm_data.GetPoseOrDie(viewL);
const Intrinsics::const_iterator iterIntrinsicL = sfm_data.GetIntrinsics().find(viewL->id_intrinsic);
const View * viewR = sfm_data.GetViews().at(it->second).get();
const Pose3 poseR = sfm_data.GetPoseOrDie(viewR);
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(poseL);
const Mat34 P_R = iterIntrinsicR->second.get()->get_projective_equivalent(poseR);
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, poseL);
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]);
}
}
}
}
bool CreatePoint3DFile( const SfM_Data & sfm_data,
const std::string & sPoints3DFilename)
{
/* points3D.txt
# 3D point list with one line of data per point:
# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)
# Number of points: X, mean track length: Y
*/
std::ofstream points3D_file( sPoints3DFilename );
if ( ! points3D_file )
{
std::cerr << "Cannot write file" << sPoints3DFilename << std::endl;
return false;
}
points3D_file << "# 3D point list with one line of data per point:\n";
points3D_file << "# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)\n";
points3D_file << "# Number of points: X, mean track length: Y\n";
const Landmarks & landmarks = sfm_data.GetLandmarks();
std::vector<Vec3> vec_3dPoints, vec_tracksColor;
if (!ColorizeTracks(sfm_data, vec_3dPoints, vec_tracksColor)) {
return false;
}
C_Progress_display my_progress_bar( landmarks.size(), std::cout, "\n- CREATE POINT3D FILE -\n" );
int point_index = 0;
for ( Landmarks::const_iterator iterLandmarks = landmarks.begin();
iterLandmarks != landmarks.end(); ++iterLandmarks, ++my_progress_bar )
{
const Vec3 exportPoint = iterLandmarks->second.X;
const IndexT point3d_id = iterLandmarks->first;
points3D_file << point3d_id << " "
<< exportPoint.x() << " "
<< exportPoint.y() << " "
<< exportPoint.z() << " "
<< static_cast<int>(vec_tracksColor.at(point_index)(0)) << " "
<< static_cast<int>(vec_tracksColor.at(point_index)(1)) << " "
<< static_cast<int>(vec_tracksColor.at(point_index)(2)) << " ";
++point_index;
const double error = 0.0; // Some error
points3D_file << error;
const Observations & obs = iterLandmarks->second.obs;
for ( Observations::const_iterator itObs = obs.begin(); itObs != obs.end(); ++itObs )
{
const IndexT viewId = itObs->first;
const IndexT featId = itObs->second.id_feat;
points3D_file << " "
<< viewId << " "
<< featId;
}
points3D_file << "\n";
}
return true;
}
inline bool Generate_SfM_Report
(
const SfM_Data & sfm_data,
const std::string & htmlFilename
)
{
// Compute mean,max,median residual values per View
IndexT residualCount = 0;
Hash_Map< IndexT, std::vector<double> > residuals_per_view;
for ( const auto & iterTracks : sfm_data.GetLandmarks() )
{
const Observations & obs = iterTracks.second.obs;
for ( const auto & itObs : obs )
{
const View * view = sfm_data.GetViews().at(itObs.first).get();
const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
const cameras::IntrinsicBase * intrinsic = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
// Use absolute values
const Vec2 residual = intrinsic->residual(pose, iterTracks.second.X, itObs.second.x).array().abs();
residuals_per_view[itObs.first].push_back(residual(0));
residuals_per_view[itObs.first].push_back(residual(1));
++residualCount;
}
}
using namespace htmlDocument;
// extract directory from htmlFilename
const std::string sTableBegin = "<table border=\"1\">",
sTableEnd = "</table>",
sRowBegin= "<tr>", sRowEnd = "</tr>",
sColBegin = "<td>", sColEnd = "</td>",
sNewLine = "<br>", sFullLine = "<hr>";
htmlDocument::htmlDocumentStream htmlDocStream("SFM report.");
htmlDocStream.pushInfo(
htmlDocument::htmlMarkup("h1", std::string("SFM report.")));
htmlDocStream.pushInfo(sFullLine);
htmlDocStream.pushInfo( "Dataset info:" + sNewLine );
std::ostringstream os;
os << " #views: " << sfm_data.GetViews().size() << sNewLine
<< " #poses: " << sfm_data.GetPoses().size() << sNewLine
<< " #intrinsics: " << sfm_data.GetIntrinsics().size() << sNewLine
<< " #tracks: " << sfm_data.GetLandmarks().size() << sNewLine
<< " #residuals: " << residualCount << sNewLine;
htmlDocStream.pushInfo( os.str() );
htmlDocStream.pushInfo( sFullLine );
htmlDocStream.pushInfo( sTableBegin);
os.str("");
os << sRowBegin
<< sColBegin + "IdView" + sColEnd
<< sColBegin + "Basename" + sColEnd
<< sColBegin + "#Observations" + sColEnd
<< sColBegin + "Residuals min" + sColEnd
<< sColBegin + "Residuals median" + sColEnd
<< sColBegin + "Residuals mean" + sColEnd
<< sColBegin + "Residuals max" + sColEnd
<< sRowEnd;
htmlDocStream.pushInfo( os.str() );
for (const auto & iterV : sfm_data.GetViews() )
{
const View * v = iterV.second.get();
const IndexT id_view = v->id_view;
os.str("");
os << sRowBegin
<< sColBegin << id_view << sColEnd
<< sColBegin + stlplus::basename_part(v->s_Img_path) + sColEnd;
// IdView | basename | #Observations | residuals min | residual median | residual max
if (sfm_data.IsPoseAndIntrinsicDefined(v))
{
if( residuals_per_view.find(id_view) != residuals_per_view.end() )
{
const std::vector<double> & residuals = residuals_per_view.at(id_view);
if (!residuals.empty())
{
double min, max, mean, median;
minMaxMeanMedian(residuals.begin(), residuals.end(), min, max, mean, median);
os << sColBegin << residuals.size()/2 << sColEnd // #observations
<< sColBegin << min << sColEnd
<< sColBegin << median << sColEnd
<< sColBegin << mean << sColEnd
<< sColBegin << max <<sColEnd;
}
}
}
os << sRowEnd;
htmlDocStream.pushInfo( os.str() );
}
htmlDocStream.pushInfo( sTableEnd );
htmlDocStream.pushInfo( sFullLine );
// combine all residual values into one vector
// export the SVG histogram
{
IndexT residualCount = 0;
for (Hash_Map< IndexT, std::vector<double> >::const_iterator
it = residuals_per_view.begin();
it != residuals_per_view.end();
++it)
{
residualCount += it->second.size();
}
// Concat per view residual values into one vector
std::vector<double> residuals(residualCount);
residualCount = 0;
for (Hash_Map< IndexT, std::vector<double> >::const_iterator
it = residuals_per_view.begin();
it != residuals_per_view.end();
++it)
{
std::copy(it->second.begin(),
it->second.begin()+it->second.size(),
residuals.begin()+residualCount);
residualCount += it->second.size();
}
if (!residuals.empty())
{
// RMSE computation
const Eigen::Map<Eigen::RowVectorXd> residuals_mapping(&residuals[0], residuals.size());
const double RMSE = std::sqrt(residuals_mapping.squaredNorm() / (double)residuals.size());
os.str("");
os << sFullLine << "SfM Scene RMSE: " << RMSE << sFullLine;
htmlDocStream.pushInfo(os.str());
const double maxRange = *max_element(residuals.begin(), residuals.end());
Histogram<double> histo(0.0, maxRange, 100);
histo.Add(residuals.begin(), residuals.end());
svg::svgHisto svg_Histo;
svg_Histo.draw(histo.GetHist(), std::pair<float,float>(0.f, maxRange),
stlplus::create_filespec(stlplus::folder_part(htmlFilename), "residuals_histogram", "svg"),
600, 200);
os.str("");
os << sNewLine<< "Residuals histogram" << sNewLine;
os << "<img src=\""
<< "residuals_histogram.svg"
<< "\" height=\"300\" width =\"800\">\n";
htmlDocStream.pushInfo(os.str());
}
}
std::ofstream htmlFileStream(htmlFilename.c_str());
htmlFileStream << htmlDocStream.getDoc();
const bool bOk = !htmlFileStream.bad();
return bOk;
}
/// 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);
}
}
}
}
