MRPT  1.9.9
types.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPose3DQuat.h>
16 #include <mrpt/vision/types.h>
17 #include <fstream>
18 #include <iomanip>
19 #include <iostream>
20 
21 using namespace std;
22 using namespace mrpt;
23 using namespace mrpt::vision;
24 using namespace mrpt::img;
25 using namespace mrpt::system;
26 
27 // ==================== TSequenceFeatureObservations ====================
28 /** Saves all entries to a text file, with each line having this format:
29  * #FRAME_ID #FEAT_ID #PIXEL_X #PIXEL_Y
30  * \sa loadFromTextFile */
31 void TSequenceFeatureObservations::saveToTextFile(
32  const std::string& filName, bool skipFirstCommentLine) const
33 {
35 
36  ofstream f(filName.c_str());
37  if (!f.is_open())
38  THROW_EXCEPTION_FMT("Can't open file: %s", filName.c_str());
39 
40  if (!skipFirstCommentLine)
41  f << "% FRAME_ID FEAT_ID X Y \n"
42  "%-------------------------------------\n";
43 
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;
47 
48  MRPT_END
49 }
50 
51 /** Load from a text file, in the format described in \a saveToTextFile */
52 void TSequenceFeatureObservations::loadFromTextFile(const std::string& filName)
53 {
55 
56  BASE::clear(); // Erase previous contents
57 
58  ifstream f(filName.c_str());
59  if (!f.is_open())
60  THROW_EXCEPTION_FMT("Can't open file: %s", filName.c_str());
61 
62  unsigned int linNum = 0;
63  while (!f.fail())
64  {
65  linNum++; // Line counter (for error reporting)
66  string lin;
67  std::getline(f, lin);
68  lin = trim(lin);
69  if (lin.empty() || lin[0] == '%') continue;
70 
71  // Read from each line as a stream:
72  std::istringstream s;
73  s.str(lin);
74 
75  TFeatureID featID;
76  TCameraPoseID camID;
77  TPixelCoordf px;
78  if (!(s >> camID >> featID >> px.x >> px.y))
80  "%s:%u: Error parsing line: '%s'", filName.c_str(), linNum,
81  lin.c_str()));
82 
83  BASE::push_back(TFeatureObservation(featID, camID, px));
84  }
85 
86  MRPT_END
87 }
88 
89 bool TSequenceFeatureObservations::saveAsSBAFiles(
90  const TLandmarkLocationsVec& pts, const std::string& pts_file,
91  const TFramePosesVec& cams, const std::string& cams_file) const
92 {
94 
95  std::map<TLandmarkID, std::map<TCameraPoseID, TPixelCoordf>> obs_by_point;
96 
97  for (size_t i = 0; i < BASE::size(); i++)
98  {
99  const TFeatureObservation& o = (*this)[i];
100  std::map<TCameraPoseID, TPixelCoordf>& m = obs_by_point[o.id_feature];
101  m[o.id_frame] = o.px;
102  }
103 
104  // # X Y Z nframes frame0 x0 y0 frame1 x1 y1 ...
105  ofstream f(pts_file.c_str());
106  if (!f.is_open()) return false;
107 
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)
110  {
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() << " ";
114  for (auto itO : m)
115  f << itO.first << " " << itO.second.x << " " << itO.second.y << " ";
116  f << endl;
117  }
118 
119  ofstream fc(cams_file.c_str());
120  if (!fc.is_open()) return false;
121 
122  for (const auto& pos : cams)
123  {
124  const mrpt::poses::CPose3DQuat p(pos);
125  fc << p.m_quat[0] << " " << p.m_quat[1] << " " << p.m_quat[2] << " "
126  << p.m_quat[3] << " " << p.x() << " " << p.y() << " " << p.z()
127  << endl;
128  }
129 
130  return true;
131  MRPT_END
132 }
133 
134 /** Remove all those features that don't have a minimum number of observations
135  * from different camera frame IDs. */
136 size_t TSequenceFeatureObservations::removeFewObservedFeatures(
137  size_t minNumObservations)
138 {
139  MRPT_START
140 
141  size_t remCount = 0;
142 
143  // 1st pass: Count total views
144  map<TLandmarkID, size_t> numViews;
145  for (auto& it : *this) numViews[it.id_feature]++;
146 
147  // 2nd pass: Remove selected ones:
148  for (size_t idx = 0; idx < BASE::size();)
149  {
150  if (numViews[(*this)[idx].id_feature] < minNumObservations)
151  {
152  BASE::erase(BASE::begin() + idx);
153  remCount++;
154  }
155  else
156  ++idx;
157  }
158  return remCount;
159  MRPT_END
160 }
161 
162 /** Remove one out of \a decimate_ratio camera frame IDs from the list.
163  * \sa After calling this you may want to call \a compressIDs */
164 void TSequenceFeatureObservations::decimateCameraFrames(
165  const size_t decimate_ratio)
166 {
167  ASSERT_ABOVEEQ_(decimate_ratio, 1);
168  if (decimate_ratio == 1) return; // =1 -> Delete no one!
169 
170  // 1) Make sorted list of frame IDs:
171  set<TCameraPoseID> frameIDs;
172  for (auto it = BASE::begin(); it != BASE::end(); ++it)
173  frameIDs.insert(it->id_frame);
174 
175  // 2) Leave in "frameIDs" just the IDs that will survive:
176  for (auto it = frameIDs.begin(); it != frameIDs.end();)
177  {
178  // Leave one:
179  ++it;
180  // Remove "decimate_ratio-1"
181  for (size_t d = 0; d < decimate_ratio - 1 && it != frameIDs.end(); d++)
182  it = mrpt::containers::erase_return_next(frameIDs, it);
183  }
184 
185  // 3) Make a new list of observations with only the desired data:
187  newLst.reserve(BASE::size() / decimate_ratio);
188  for (auto it = BASE::begin(); it != BASE::end(); ++it)
189  if (frameIDs.find(it->id_frame) != frameIDs.end())
190  newLst.push_back(*it);
191 
192  // Finally, save content in "this":
193  this->swap(newLst);
194 }
195 
196 /** Rearrange frame and feature IDs such as they start at 0 and there are no
197  * gaps. */
198 void TSequenceFeatureObservations::compressIDs(
199  std::map<TCameraPoseID, TCameraPoseID>* old2new_camIDs,
200  std::map<TLandmarkID, TLandmarkID>* old2new_lmIDs)
201 {
202  // 1st pass: Make list of translation IDs.
203  std::map<TCameraPoseID, TCameraPoseID> camIDs;
204  std::map<TLandmarkID, TLandmarkID> lmIDs;
205 
206  for (auto it = BASE::begin(); it != BASE::end(); ++it)
207  {
208  const TFeatureID f_ID = it->id_feature;
209  const TCameraPoseID c_ID = it->id_frame;
210 
211  if (lmIDs.find(f_ID) == lmIDs.end())
212  {
213  TLandmarkID nextID = lmIDs.size(); // *IMPORTANT* Separate in 2
214  // lines, otherwise lmIDs[] is
215  // called first (!?)
216  lmIDs[f_ID] = nextID;
217  }
218  if (camIDs.find(c_ID) == camIDs.end())
219  {
220  TCameraPoseID nextID = camIDs.size(); // *IMPORTANT* Separate in 2
221  // lines, otherwise camIDs[]
222  // is called first (!?)
223  camIDs[c_ID] = nextID;
224  }
225  }
226 
227  // 2nd: Create a new list with the translated IDs:
228  const size_t N = BASE::size();
230  for (size_t i = 0; i < N; ++i)
231  {
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;
235  }
236 
237  // And save that new list in "this":
238  this->swap(newLst);
239 
240  // Return translations, if requested:
241  if (old2new_camIDs) old2new_camIDs->swap(camIDs);
242  if (old2new_lmIDs) old2new_lmIDs->swap(lmIDs);
243 }
TLandmarkID id_feature
A unique ID of this feature.
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
#define MRPT_START
Definition: exceptions.h:241
uint64_t TFeatureID
Definition of a feature ID.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
uint64_t TLandmarkID
Unique IDs for landmarks.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
size_t size(const MATRIXLIKE &m, const int dim)
One feature observation entry, used within sequences with TSequenceFeatureObservations.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
A complete sequence of observations of features from different camera frames (poses).
std::vector< mrpt::poses::CPose3D > TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
std::list< T >::iterator erase_return_next(std::list< T > &cont, typename std::list< T >::iterator &it)
Calls the standard "erase" method of a STL container, but also returns an iterator to the next elemen...
static std::string & trim(std::string &s)
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
mrpt::img::TPixelCoordf px
The pixel coordinates of the observed feature.
const_iterator end() const
Definition: ts_hash_map.h:246
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const_iterator begin() const
Definition: ts_hash_map.h:240
#define MRPT_END
Definition: exceptions.h:245
TCameraPoseID id_frame
A unique ID of a "frame" (camera position) from where the feature was observed.
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:54
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020