util.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef UTIL_H_
29 #define UTIL_H_
30 
31 
32 #include <android/log.h>
36 #include <tango-gl/util.h>
37 #include <pcl/point_cloud.h>
38 #include <pcl/point_types.h>
39 #include <pcl/Vertices.h>
40 #include <pcl/pcl_base.h>
41 
42 namespace rtabmap {
43 
44 class LogHandler : public UEventsHandler
45 {
46 public:
48  {
52 
54  }
55 protected:
56  virtual bool handleEvent(UEvent * event)
57  {
58  if(event->getClassName().compare("ULogEvent") == 0)
59  {
60  ULogEvent * logEvent = (ULogEvent*)event;
61  if(logEvent->getCode() == ULogger::kDebug)
62  {
63  LOGD("%s", logEvent->getMsg().c_str());
64  }
65  else if(logEvent->getCode() == ULogger::kInfo)
66  {
67  LOGI("%s", logEvent->getMsg().c_str());
68  }
69  else if(logEvent->getCode() == ULogger::kWarning)
70  {
71  LOGW("%s", logEvent->getMsg().c_str());
72  }
73  else if(logEvent->getCode() >= ULogger::kError)
74  {
75  LOGE("%s", logEvent->getMsg().c_str());
76  }
77  else if(logEvent->getCode() >= ULogger::kFatal)
78  {
79  LOGF("%s", logEvent->getMsg().c_str());
80  }
81 
82  }
83  return false;
84  }
85 };
86 
88  1.0f, 0.0f, 0.0f, 0.0f,
89  0.0f, -1.0f, 0.0f, 0.0f,
90  0.0f, 0.0f, -1.0f, 0.0f);
91 
93  1.0f, 0.0f, 0.0f, 0.0f,
94  0.0f, 0.0f, 1.0f, 0.0f,
95  0.0f, -1.0f, 0.0f, 0.0f);
96 
98  0.0f, 1.0f, 0.0f, 0.0f,
99  -1.0f, 0.0f, 0.0f, 0.0f,
100  0.0f, 0.0f, 1.0f, 0.0f);
101 
103  0.0f, -1.0f, 0.0f, 0.0f,
104  0.0f, 0.0f, 1.0f, 0.0f,
105  -1.0f, 0.0f, 0.0f, 0.0f);
106 
108  0.0f, -1.0f, 0.0f, 0.0f,
109  1.0f, 0.0f, 0.0f, 0.0f,
110  0.0f, 0.0f, 1.0f, 0.0f);
111 
113  0.0f, -1.0f, 0.0f, 0.0f,
114  0.0f, 0.0f, 1.0f, 0.0f,
115  -1.0f, 0.0f, 0.0f, 0.0f);
116 
118  0.0f, 0.0f,-1.0f, 0.0f,
119  -1.0f, 0.0f, 0.0f, 0.0f,
120  0.0f, 1.0f, 0.0f, 0.0f);
121 
123 {
124  glm::mat4 mat(1.0f);
125  // gl is column wise
126  mat[0][0] = transform(0,0);
127  mat[1][0] = transform(0,1);
128  mat[2][0] = transform(0,2);
129  mat[0][1] = transform(1,0);
130  mat[1][1] = transform(1,1);
131  mat[2][1] = transform(1,2);
132  mat[0][2] = transform(2,0);
133  mat[1][2] = transform(2,1);
134  mat[2][2] = transform(2,2);
135 
136  mat[3][0] = transform(0,3);
137  mat[3][1] = transform(1,3);
138  mat[3][2] = transform(2,3);
139  return mat;
140 }
141 
143 {
144  rtabmap::Transform transform;
145  // gl is column wise
146  transform(0,0) = mat[0][0];
147  transform(0,1) = mat[1][0];
148  transform(0,2) = mat[2][0];
149  transform(1,0) = mat[0][1];
150  transform(1,1) = mat[1][1];
151  transform(1,2) = mat[2][1];
152  transform(2,0) = mat[0][2];
153  transform(2,1) = mat[1][2];
154  transform(2,2) = mat[2][2];
155 
156  transform(0,3) = mat[3][0];
157  transform(1,3) = mat[3][1];
158  transform(2,3) = mat[3][2];
159 
160  return transform;
161 }
162 
163 class Mesh
164 {
165 public:
166  Mesh() :
167  cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
168  normals(new pcl::PointCloud<pcl::Normal>),
169  indices(new std::vector<int>),
170  visible(true)
171  {
172  gains[0] = gains[1] = gains[2] = 1.0f;
173  }
174 
175  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
176  pcl::PointCloud<pcl::Normal>::Ptr normals;
177  pcl::IndicesPtr indices;
178  std::vector<pcl::Vertices> polygons;
179  std::vector<pcl::Vertices> polygonsLowRes;
180  rtabmap::Transform pose; // in rtabmap coordinates
181  bool visible;
183  double gains[3]; // RGB gains
184 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
185  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords;
186 #else
187  std::vector<Eigen::Vector2f> texCoords;
188 #endif
189  cv::Mat texture;
190 };
191 
192 typedef enum {
195 
198 
201 
204 
208 
209 inline int NormalizedColorCameraRotation(int camera_rotation) {
210  int camera_n = 0;
211  switch (camera_rotation) {
212  case 90:
213  camera_n = 1;
214  break;
215  case 180:
216  camera_n = 2;
217  break;
218  case 270:
219  camera_n = 3;
220  break;
221  default:
222  camera_n = 0;
223  break;
224  }
225  return camera_n;
226 }
227 
228 // Get the Android rotation integer value from color camera to display.
229 // This function is used to compute the orientation difference to handle
230 // the portrait and landscape mode for color camera display.
231 //
232 // @param display: the device display orientation.
233 // @param color_camera: integer value of color camera oreintation, values
234 // available are 0, 90, 180, 270. Followed by Android camera orientation
235 // standard:
236 // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
238  ScreenRotation display_rotation, int color_camera_rotation) {
239  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
240 
241  int ret = static_cast<int>(display_rotation) - color_camera_n;
242  if (ret < 0) {
243  ret += 4;
244  }
245  return static_cast<ScreenRotation>(ret % 4);
246 }
247 
248 // Get the Android rotation integer value from color camera to display.
249 // This function is used to compute the orientation difference to handle
250 // the portrait and landscape mode for color camera display.
251 //
252 // @param display: integer value of display orientation, values available
253 // are 0, 1, 2 ,3. Followed by Android display orientation standard:
254 // https://developer.android.com/reference/android/view/Display.html#getRotation()
255 // @param color_camera: integer value of color camera orientation, values
256 // available are 0, 90, 180, 270. Followed by Android camera orientation
257 // standard:
258 // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
260  int display_rotation, int color_camera_rotation) {
261  ScreenRotation r =
262  static_cast<ScreenRotation>(display_rotation);
264  r, color_camera_rotation);
265 }
266 
267 }
268 
269 #endif /* UTIL_H_ */
int getCode() const
Definition: UEvent.h:74
static void setPrintThreadId(bool printThreadId)
Definition: ULogger.h:309
glm::mat4 glmFromTransform(const rtabmap::Transform &transform)
Definition: util.h:122
static const rtabmap::Transform tango_device_T_rtabmap_world(0.0f,-1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f)
rtabmap::CameraModel cameraModel
Definition: util.h:182
std::vector< Eigen::Vector2f > texCoords
Definition: util.h:187
0 degree rotation (natural orientation)
Definition: util.h:197
pcl::IndicesPtr indices
Definition: util.h:177
rtabmap::Transform glmToTransform(const glm::mat4 &mat)
Definition: util.h:142
#define LOGW(...)
static const rtabmap::Transform rtabmap_world_T_tango_world(0.0f, 1.0f, 0.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f)
Definition: UEvent.h:57
static const rtabmap::Transform optical_T_opengl(1.0f, 0.0f, 0.0f, 0.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f, 0.0f,-1.0f, 0.0f)
f
pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud
Definition: util.h:175
bool visible
Definition: util.h:181
ScreenRotation GetAndroidRotationFromColorCameraToDisplay(ScreenRotation display_rotation, int color_camera_rotation)
Definition: util.h:237
int NormalizedColorCameraRotation(int camera_rotation)
Definition: util.h:209
std::vector< pcl::Vertices > polygonsLowRes
Definition: util.h:179
#define LOGE(...)
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
std::vector< pcl::Vertices > polygons
Definition: util.h:178
static const rtabmap::Transform opengl_world_T_tango_world(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,-1.0f, 0.0f, 0.0f)
Not apply any rotation.
Definition: util.h:194
static const rtabmap::Transform opengl_world_T_rtabmap_world(0.0f,-1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f)
#define LOGD(...)
#define true
Definition: ConvertUTF.c:57
virtual std::string getClassName() const =0
pcl::PointCloud< pcl::Normal >::Ptr normals
Definition: util.h:176
static void setEventLevel(ULogger::Level eventSentLevel)
Definition: ULogger.h:348
270 degree rotation.
Definition: util.h:206
cv::Mat texture
Definition: util.h:189
180 degree rotation.
Definition: util.h:203
static const rtabmap::Transform tango_world_T_rtabmap_world(0.0f,-1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f)
#define LOGI(...)
void registerToEventsManager()
#define LOGF(...)
static const rtabmap::Transform rtabmap_world_T_opengl_world(0.0f, 0.0f,-1.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f)
90 degree rotation.
Definition: util.h:200
ULogger class and convenient macros.
virtual bool handleEvent(UEvent *event)
Definition: util.h:56
ScreenRotation
Definition: util.h:192
const std::string & getMsg() const
Definition: ULogger.h:137
rtabmap::Transform pose
Definition: util.h:180


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:37:06