31 void TSequenceFeatureObservations::saveToTextFile(
32 const std::string& filName,
bool skipFirstCommentLine)
const
36 ofstream f(filName.c_str());
40 if (!skipFirstCommentLine)
41 f <<
"% FRAME_ID FEAT_ID X Y \n"
42 "%-------------------------------------\n";
44 for (
const auto& it : *
this)
45 f << setw(7) << it.id_frame << setw(7) << it.id_feature << setw(13)
46 << it.px.x << setw(11) << it.px.y << endl;
52 void TSequenceFeatureObservations::loadFromTextFile(
const std::string& filName)
58 ifstream f(filName.c_str());
62 unsigned int linNum = 0;
69 if (lin.empty() || lin[0] ==
'%')
continue;
78 if (!(s >> camID >> featID >> px.
x >> px.
y))
80 "%s:%u: Error parsing line: '%s'", filName.c_str(), linNum,
89 bool TSequenceFeatureObservations::saveAsSBAFiles(
95 std::map<TLandmarkID, std::map<TCameraPoseID, TPixelCoordf>> obs_by_point;
100 std::map<TCameraPoseID, TPixelCoordf>& m = obs_by_point[o.
id_feature];
105 ofstream f(pts_file.c_str());
106 if (!f.is_open())
return false;
108 f <<
"# X Y Z nframes frame0 x0 y0 frame1 x1 y1 ...\n";
109 for (
auto it = obs_by_point.begin(); it != obs_by_point.end(); ++it)
111 const std::map<TCameraPoseID, TPixelCoordf>& m = it->second;
112 f << pts[it->first].x <<
" " << pts[it->first].y <<
" "
113 << pts[it->first].z <<
" " << m.size() <<
" ";
115 f << itO.first <<
" " << itO.second.x <<
" " << itO.second.y <<
" ";
119 ofstream fc(cams_file.c_str());
120 if (!fc.is_open())
return false;
122 for (
const auto& pos : cams)
126 << p.
m_quat[3] <<
" " << p.
x() <<
" " << p.
y() <<
" " << p.z()
136 size_t TSequenceFeatureObservations::removeFewObservedFeatures(
137 size_t minNumObservations)
144 map<TLandmarkID, size_t> numViews;
145 for (
auto& it : *
this) numViews[it.id_feature]++;
150 if (numViews[(*
this)[idx].id_feature] < minNumObservations)
164 void TSequenceFeatureObservations::decimateCameraFrames(
165 const size_t decimate_ratio)
168 if (decimate_ratio == 1)
return;
171 set<TCameraPoseID> frameIDs;
173 frameIDs.insert(it->id_frame);
176 for (
auto it = frameIDs.begin(); it != frameIDs.end();)
181 for (
size_t d = 0; d < decimate_ratio - 1 && it != frameIDs.end(); d++)
187 newLst.reserve(
BASE::size() / decimate_ratio);
189 if (frameIDs.find(it->id_frame) != frameIDs.end())
190 newLst.push_back(*it);
198 void TSequenceFeatureObservations::compressIDs(
199 std::map<TCameraPoseID, TCameraPoseID>* old2new_camIDs,
200 std::map<TLandmarkID, TLandmarkID>* old2new_lmIDs)
203 std::map<TCameraPoseID, TCameraPoseID> camIDs;
204 std::map<TLandmarkID, TLandmarkID> lmIDs;
211 if (lmIDs.find(f_ID) == lmIDs.end())
216 lmIDs[f_ID] = nextID;
218 if (camIDs.find(c_ID) == camIDs.end())
223 camIDs[c_ID] = nextID;
230 for (
size_t i = 0; i < N; ++i)
232 newLst[i].id_feature = lmIDs[(*this)[i].id_feature];
233 newLst[i].id_frame = camIDs[(*this)[i].id_frame];
234 newLst[i].px = (*this)[i].px;
241 if (old2new_camIDs) old2new_camIDs->swap(camIDs);
242 if (old2new_lmIDs) old2new_lmIDs->swap(lmIDs);