///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;
}
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;
}
/// 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;
}