Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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>
17 #include <iostream>
18 #include <iomanip>
19 
20 using namespace std;
21 using namespace mrpt;
22 using namespace mrpt::utils;
23 using namespace mrpt::vision;
24 using namespace mrpt::system;
25 
26 // ==================== TSequenceFeatureObservations ====================
27 /** Saves all entries to a text file, with each line having this format: #FRAME_ID #FEAT_ID #PIXEL_X #PIXEL_Y
28  * \sa loadFromTextFile */
29 void TSequenceFeatureObservations::saveToTextFile(const std::string &filName, bool skipFirstCommentLine ) const
30 {
32 
33  ofstream f(filName.c_str());
34  if (!f.is_open()) THROW_EXCEPTION_FMT("Can't open file: %s",filName.c_str())
35 
36  if (!skipFirstCommentLine)
37  f << "% FRAME_ID FEAT_ID X Y \n"
38  "%-------------------------------------\n";
39 
40  for (BASE::const_iterator it=BASE::begin();it!=BASE::end();++it)
41  f << setw(7) << it->id_frame << setw(7) << it->id_feature << setw(13) << it->px.x << setw(11) << it->px.y << endl;
42 
43  MRPT_END
44 }
45 
46 /** Load from a text file, in the format described in \a saveToTextFile */
48 {
50 
51  BASE::clear(); // Erase previous contents
52 
53  ifstream f(filName.c_str());
54  if (!f.is_open()) THROW_EXCEPTION_FMT("Can't open file: %s",filName.c_str())
55 
56  unsigned int linNum = 0;
57  while (!f.fail())
58  {
59  linNum++; // Line counter (for error reporting)
60  string lin;
61  std::getline(f,lin);
62  lin=trim(lin);
63  if (lin.empty() || lin[0]=='%') continue;
64 
65  // Read from each line as a stream:
66  std::istringstream s;
67  s.str(lin);
68 
69  TFeatureID featID;
70  TCameraPoseID camID;
71  TPixelCoordf px;
72  if (!(s >> camID >> featID >> px.x >> px.y))
73  THROW_EXCEPTION(format("%s:%u: Error parsing line: '%s'",filName.c_str(),linNum,lin.c_str()))
74 
75  BASE::push_back( TFeatureObservation(featID,camID,px) );
76  }
77 
78  MRPT_END
79 }
80 
81 bool TSequenceFeatureObservations::saveAsSBAFiles(
82  const TLandmarkLocationsVec &pts,
83  const std::string &pts_file,
84  const TFramePosesVec &cams,
85  const std::string &cams_file) const
86 {
88 
89  std::map<TLandmarkID, std::map<TCameraPoseID, TPixelCoordf> > obs_by_point;
90 
91  for (size_t i=0;i<BASE::size();i++)
92  {
93  const TFeatureObservation & o = (*this)[i];
94  std::map<TCameraPoseID, TPixelCoordf> & m = obs_by_point[ o.id_feature ];
95  m[o.id_frame]= o.px;
96  }
97 
98  // # X Y Z nframes frame0 x0 y0 frame1 x1 y1 ...
99  ofstream f(pts_file.c_str());
100  if (!f.is_open())
101  return false;
102 
103  f << "# X Y Z nframes frame0 x0 y0 frame1 x1 y1 ...\n";
104  for (std::map<TLandmarkID, std::map<TCameraPoseID, TPixelCoordf> >::const_iterator it=obs_by_point.begin();it!=obs_by_point.end();++it)
105  {
106  const std::map<TCameraPoseID, TPixelCoordf> & m = it->second;
107  f << pts[it->first].x << " "
108  << pts[it->first].y << " "
109  << pts[it->first].z << " "
110  << m.size() << " ";
111  for (std::map<TCameraPoseID, TPixelCoordf>::const_iterator itO=m.begin();itO!=m.end();++itO)
112  f << itO->first << " " << itO->second.x << " "<< itO->second.y << " ";
113  f << endl;
114  }
115 
116  ofstream fc(cams_file.c_str());
117  if (!fc.is_open())
118  return false;
119 
120  for (size_t i=0;i<cams.size();i++)
121  {
122  const mrpt::poses::CPose3D &pos = cams[i];
123  const mrpt::poses::CPose3DQuat p(pos);
124  fc << p.m_quat[0] << " "
125  << p.m_quat[1] << " "
126  << p.m_quat[2] << " "
127  << p.m_quat[3] << " "
128  << p.x() << " "
129  << p.y() << " "
130  << p.z() << endl;
131  }
132 
133  return true;
134  MRPT_END
135 }
136 
137 
138 /** Remove all those features that don't have a minimum number of observations from different camera frame IDs. */
139 size_t TSequenceFeatureObservations::removeFewObservedFeatures(size_t minNumObservations )
140 {
141  MRPT_START
142 
143  size_t remCount = 0;
144 
145  // 1st pass: Count total views
146  map<TLandmarkID,size_t> numViews;
147  for (BASE::iterator it=BASE::begin();it!=BASE::end(); ++it )
148  numViews[it->id_feature]++;
149 
150  // 2nd pass: Remove selected ones:
151  for (size_t idx=0; idx<BASE::size(); )
152  {
153  if (numViews[(*this)[idx].id_feature]<minNumObservations)
154  {
155  BASE::erase( BASE::begin()+idx );
156  remCount++;
157  }
158  else ++idx;
159  }
160  return remCount;
161  MRPT_END
162 }
163 
164 /** Remove one out of \a decimate_ratio camera frame IDs from the list.
165  * \sa After calling this you may want to call \a compressIDs */
166 void TSequenceFeatureObservations::decimateCameraFrames(const size_t decimate_ratio)
167 {
168  ASSERT_ABOVEEQ_(decimate_ratio,1)
169  if (decimate_ratio==1)
170  return; // =1 -> Delete no one!
171 
172  // 1) Make sorted list of frame IDs:
173  set<TCameraPoseID> frameIDs;
174  for (BASE::const_iterator it=BASE::begin();it!=BASE::end();++it)
175  frameIDs.insert(it->id_frame);
176 
177  // 2) Leave in "frameIDs" just the IDs that will survive:
178  for (set<TCameraPoseID>::iterator it=frameIDs.begin();it!=frameIDs.end(); )
179  {
180  // Leave one:
181  ++it;
182  // Remove "decimate_ratio-1"
183  for (size_t d=0;d<decimate_ratio-1 && it!=frameIDs.end();d++)
184  it=mrpt::utils::erase_return_next(frameIDs,it);
185  }
186 
187  // 3) Make a new list of observations with only the desired data:
189  newLst.reserve(BASE::size() / decimate_ratio);
190  for (BASE::const_iterator it=BASE::begin();it!=BASE::end();++it)
191  if (frameIDs.find(it->id_frame)!=frameIDs.end())
192  newLst.push_back(*it);
193 
194  // Finally, save content in "this":
195  this->swap(newLst);
196 }
197 
198 /** Rearrange frame and feature IDs such as they start at 0 and there are no gaps. */
199 void TSequenceFeatureObservations::compressIDs(
200  std::map<TCameraPoseID,TCameraPoseID> *old2new_camIDs,
201  std::map<TLandmarkID,TLandmarkID> *old2new_lmIDs )
202 {
203  // 1st pass: Make list of translation IDs.
204  std::map<TCameraPoseID,TCameraPoseID> camIDs;
205  std::map<TLandmarkID,TLandmarkID> lmIDs;
206 
207  for (BASE::const_iterator it=BASE::begin();it!=BASE::end();++it)
208  {
209  const TFeatureID f_ID = it->id_feature;
210  const TCameraPoseID c_ID = it->id_frame;
211 
212  if (lmIDs.find(f_ID)==lmIDs.end())
213  {
214  TLandmarkID nextID = lmIDs.size(); // *IMPORTANT* Separate in 2 lines, otherwise lmIDs[] is called first (!?)
215  lmIDs[f_ID]=nextID;
216  }
217  if (camIDs.find(c_ID)==camIDs.end())
218  {
219  TCameraPoseID nextID = camIDs.size(); // *IMPORTANT* Separate in 2 lines, otherwise camIDs[] is called first (!?)
220  camIDs[c_ID]=nextID;
221  }
222  }
223 
224  // 2nd: Create a new list with the translated IDs:
225  const size_t N = BASE::size();
227  for (size_t i=0;i<N;++i)
228  {
229  newLst[i].id_feature = lmIDs [ (*this)[i].id_feature ];
230  newLst[i].id_frame = camIDs[ (*this)[i].id_frame ];
231  newLst[i].px = (*this)[i].px;
232  }
233 
234  // And save that new list in "this":
235  this->swap(newLst);
236 
237  // Return translations, if requested:
238  if (old2new_camIDs) old2new_camIDs->swap(camIDs);
239  if (old2new_lmIDs) old2new_lmIDs->swap(lmIDs);
240 
241 }
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
TLandmarkID id_feature
A unique ID of this feature.
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...
#define THROW_EXCEPTION(msg)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Scalar * iterator
Definition: eigen_plugins.h:23
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
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)
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::utils::TPixelCoordf px
The pixel coordinates of the observed feature.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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...
mrpt::aligned_containers< mrpt::poses::CPose3D >::vector_t TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
GLdouble s
Definition: glew.h:1295
GLuint GLuint end
Definition: glew.h:1167
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define MRPT_END
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glew.h:1168
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
GLfloat GLfloat p
Definition: glew.h:10113
uint64_t TFeatureID
Definition of a feature ID.
GLsizeiptr size
Definition: glew.h:1586
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
#define MRPT_START
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
uint64_t TLandmarkID
Unique IDs for landmarks.
#define ASSERT_ABOVEEQ_(__A, __B)
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
const GLdouble * m
Definition: glew.h:5094
std::string BASE_IMPEXP trim(const std::string &str)
Removes leading and trailing spaces.
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:48
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018