Main MRPT website > C++ reference for MRPT 1.9.9
types.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/vision/types.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPose3DQuat.h>
17 #include <iostream>
18 #include <iomanip>
19 
20 using namespace std;
21 using namespace mrpt;
22 using namespace mrpt::vision;
23 using namespace mrpt::img;
24 using namespace mrpt::system;
25 
26 // ==================== TSequenceFeatureObservations ====================
27 /** Saves all entries to a text file, with each line having this format:
28  * #FRAME_ID #FEAT_ID #PIXEL_X #PIXEL_Y
29  * \sa loadFromTextFile */
31  const std::string& filName, bool skipFirstCommentLine) const
32 {
34 
35  ofstream f(filName.c_str());
36  if (!f.is_open())
37  THROW_EXCEPTION_FMT("Can't open file: %s", filName.c_str())
38 
39  if (!skipFirstCommentLine)
40  f << "% FRAME_ID FEAT_ID X Y \n"
41  "%-------------------------------------\n";
42 
43  for (BASE::const_iterator it = BASE::begin(); it != BASE::end(); ++it)
44  f << setw(7) << it->id_frame << setw(7) << it->id_feature << setw(13)
45  << it->px.x << setw(11) << it->px.y << endl;
46 
47  MRPT_END
48 }
49 
50 /** Load from a text file, in the format described in \a saveToTextFile */
52 {
54 
55  BASE::clear(); // Erase previous contents
56 
57  ifstream f(filName.c_str());
58  if (!f.is_open())
59  THROW_EXCEPTION_FMT("Can't open file: %s", filName.c_str())
60 
61  unsigned int linNum = 0;
62  while (!f.fail())
63  {
64  linNum++; // Line counter (for error reporting)
65  string lin;
66  std::getline(f, lin);
67  lin = trim(lin);
68  if (lin.empty() || lin[0] == '%') continue;
69 
70  // Read from each line as a stream:
71  std::istringstream s;
72  s.str(lin);
73 
74  TFeatureID featID;
75  TCameraPoseID camID;
76  TPixelCoordf px;
77  if (!(s >> camID >> featID >> px.x >> px.y))
79  format(
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 (std::map<TLandmarkID,
110  std::map<TCameraPoseID, TPixelCoordf>>::const_iterator it =
111  obs_by_point.begin();
112  it != obs_by_point.end(); ++it)
113  {
114  const std::map<TCameraPoseID, TPixelCoordf>& m = it->second;
115  f << pts[it->first].x << " " << pts[it->first].y << " "
116  << pts[it->first].z << " " << m.size() << " ";
118  m.begin();
119  itO != m.end(); ++itO)
120  f << itO->first << " " << itO->second.x << " " << itO->second.y
121  << " ";
122  f << endl;
123  }
124 
125  ofstream fc(cams_file.c_str());
126  if (!fc.is_open()) return false;
127 
128  for (size_t i = 0; i < cams.size(); i++)
129  {
130  const mrpt::poses::CPose3D& pos = cams[i];
131  const mrpt::poses::CPose3DQuat p(pos);
132  fc << p.m_quat[0] << " " << p.m_quat[1] << " " << p.m_quat[2] << " "
133  << p.m_quat[3] << " " << p.x() << " " << p.y() << " " << p.z()
134  << endl;
135  }
136 
137  return true;
138  MRPT_END
139 }
140 
141 /** Remove all those features that don't have a minimum number of observations
142  * from different camera frame IDs. */
143 size_t TSequenceFeatureObservations::removeFewObservedFeatures(
144  size_t minNumObservations)
145 {
146  MRPT_START
147 
148  size_t remCount = 0;
149 
150  // 1st pass: Count total views
151  map<TLandmarkID, size_t> numViews;
152  for (BASE::iterator it = BASE::begin(); it != BASE::end(); ++it)
153  numViews[it->id_feature]++;
154 
155  // 2nd pass: Remove selected ones:
156  for (size_t idx = 0; idx < BASE::size();)
157  {
158  if (numViews[(*this)[idx].id_feature] < minNumObservations)
159  {
160  BASE::erase(BASE::begin() + idx);
161  remCount++;
162  }
163  else
164  ++idx;
165  }
166  return remCount;
167  MRPT_END
168 }
169 
170 /** Remove one out of \a decimate_ratio camera frame IDs from the list.
171  * \sa After calling this you may want to call \a compressIDs */
172 void TSequenceFeatureObservations::decimateCameraFrames(
173  const size_t decimate_ratio)
174 {
175  ASSERT_ABOVEEQ_(decimate_ratio, 1);
176  if (decimate_ratio == 1) return; // =1 -> Delete no one!
177 
178  // 1) Make sorted list of frame IDs:
179  set<TCameraPoseID> frameIDs;
180  for (BASE::const_iterator it = BASE::begin(); it != BASE::end(); ++it)
181  frameIDs.insert(it->id_frame);
182 
183  // 2) Leave in "frameIDs" just the IDs that will survive:
184  for (set<TCameraPoseID>::iterator it = frameIDs.begin();
185  it != frameIDs.end();)
186  {
187  // Leave one:
188  ++it;
189  // Remove "decimate_ratio-1"
190  for (size_t d = 0; d < decimate_ratio - 1 && it != frameIDs.end(); d++)
191  it = mrpt::containers::erase_return_next(frameIDs, it);
192  }
193 
194  // 3) Make a new list of observations with only the desired data:
196  newLst.reserve(BASE::size() / decimate_ratio);
197  for (BASE::const_iterator it = BASE::begin(); it != BASE::end(); ++it)
198  if (frameIDs.find(it->id_frame) != frameIDs.end())
199  newLst.push_back(*it);
200 
201  // Finally, save content in "this":
202  this->swap(newLst);
203 }
204 
205 /** Rearrange frame and feature IDs such as they start at 0 and there are no
206  * gaps. */
207 void TSequenceFeatureObservations::compressIDs(
208  std::map<TCameraPoseID, TCameraPoseID>* old2new_camIDs,
209  std::map<TLandmarkID, TLandmarkID>* old2new_lmIDs)
210 {
211  // 1st pass: Make list of translation IDs.
212  std::map<TCameraPoseID, TCameraPoseID> camIDs;
213  std::map<TLandmarkID, TLandmarkID> lmIDs;
214 
215  for (BASE::const_iterator it = BASE::begin(); it != BASE::end(); ++it)
216  {
217  const TFeatureID f_ID = it->id_feature;
218  const TCameraPoseID c_ID = it->id_frame;
219 
220  if (lmIDs.find(f_ID) == lmIDs.end())
221  {
222  TLandmarkID nextID = lmIDs.size(); // *IMPORTANT* Separate in 2
223  // lines, otherwise lmIDs[] is
224  // called first (!?)
225  lmIDs[f_ID] = nextID;
226  }
227  if (camIDs.find(c_ID) == camIDs.end())
228  {
229  TCameraPoseID nextID = camIDs.size(); // *IMPORTANT* Separate in 2
230  // lines, otherwise camIDs[]
231  // is called first (!?)
232  camIDs[c_ID] = nextID;
233  }
234  }
235 
236  // 2nd: Create a new list with the translated IDs:
237  const size_t N = BASE::size();
239  for (size_t i = 0; i < N; ++i)
240  {
241  newLst[i].id_feature = lmIDs[(*this)[i].id_feature];
242  newLst[i].id_frame = camIDs[(*this)[i].id_frame];
243  newLst[i].px = (*this)[i].px;
244  }
245 
246  // And save that new list in "this":
247  this->swap(newLst);
248 
249  // Return translations, if requested:
250  if (old2new_camIDs) old2new_camIDs->swap(camIDs);
251  if (old2new_lmIDs) old2new_lmIDs->swap(lmIDs);
252 }
Scalar * iterator
Definition: eigen_plugins.h:26
TLandmarkID id_feature
A unique ID of this feature.
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
#define MRPT_START
Definition: exceptions.h:262
uint64_t TFeatureID
Definition of a feature ID.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
uint64_t TLandmarkID
Unique IDs for landmarks.
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
One feature observation entry, used within sequences with TSequenceFeatureObservations.
EIGEN_STRONG_INLINE void push_back(Scalar val)
Insert an element at the end of the container (for 1D vectors/arrays)
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:20
STL namespace.
GLdouble s
Definition: glext.h:3676
A complete sequence of observations of features from different camera frames (poses).
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...
GLuint GLuint end
Definition: glext.h:3528
static std::string & trim(std::string &s)
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:20
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::img::TPixelCoordf px
The pixel coordinates of the observed feature.
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
#define MRPT_END
Definition: exceptions.h:266
mrpt::aligned_std_vector< mrpt::poses::CPose3D > TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
TCameraPoseID id_frame
A unique ID of a "frame" (camera position) from where the feature was observed.
GLenum GLsizei GLenum format
Definition: glext.h:3531
GLsizeiptr size
Definition: glext.h:3923
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:43
GLfloat GLfloat p
Definition: glext.h:6305
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
const Scalar * const_iterator
Definition: eigen_plugins.h:27



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019