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 
34 #include <tango-gl/util.h>
35 #include <pcl/point_cloud.h>
36 #include <pcl/point_types.h>
37 #include <pcl/Vertices.h>
38 #include <pcl/pcl_base.h>
39 
40 namespace rtabmap {
41 
42 class LogHandler : public UEventsHandler
43 {
44 public:
46  {
47 #ifdef DISABLE_LOG
50 #else
53 #endif
55 
57  }
58 protected:
59  virtual bool handleEvent(UEvent * event)
60  {
61  if(event->getClassName().compare("ULogEvent") == 0)
62  {
63  ULogEvent * logEvent = (ULogEvent*)event;
64  if(logEvent->getCode() == ULogger::kDebug)
65  {
66  LOGD("%s", logEvent->getMsg().c_str());
67  }
68  else if(logEvent->getCode() == ULogger::kInfo)
69  {
70  LOGI("%s", logEvent->getMsg().c_str());
71  }
72  else if(logEvent->getCode() == ULogger::kWarning)
73  {
74  LOGW("%s", logEvent->getMsg().c_str());
75  }
76  else if(logEvent->getCode() >= ULogger::kError)
77  {
78  LOGE("%s", logEvent->getMsg().c_str());
79  }
80  else if(logEvent->getCode() >= ULogger::kFatal)
81  {
82  LOGF("%s", logEvent->getMsg().c_str());
83  }
84  }
85  return false;
86  }
87 };
88 
90  1.0f, 0.0f, 0.0f, 0.0f,
91  0.0f, -1.0f, 0.0f, 0.0f,
92  0.0f, 0.0f, -1.0f, 0.0f);
93 
95  1.0f, 0.0f, 0.0f, 0.0f,
96  0.0f, 0.0f, 1.0f, 0.0f,
97  0.0f, -1.0f, 0.0f, 0.0f);
98 
100  0.0f, 1.0f, 0.0f, 0.0f,
101  -1.0f, 0.0f, 0.0f, 0.0f,
102  0.0f, 0.0f, 1.0f, 0.0f);
103 
105  0.0f, -1.0f, 0.0f, 0.0f,
106  0.0f, 0.0f, 1.0f, 0.0f,
107  -1.0f, 0.0f, 0.0f, 0.0f);
108 
110  0.0f, -1.0f, 0.0f, 0.0f,
111  1.0f, 0.0f, 0.0f, 0.0f,
112  0.0f, 0.0f, 1.0f, 0.0f);
113 
115  0.0f, -1.0f, 0.0f, 0.0f,
116  0.0f, 0.0f, 1.0f, 0.0f,
117  -1.0f, 0.0f, 0.0f, 0.0f);
118 
120  0.0f, 0.0f,-1.0f, 0.0f,
121  -1.0f, 0.0f, 0.0f, 0.0f,
122  0.0f, 1.0f, 0.0f, 0.0f);
123 
125 {
126  glm::mat4 mat(1.0f);
127  // gl is column wise
128  mat[0][0] = transform(0,0);
129  mat[1][0] = transform(0,1);
130  mat[2][0] = transform(0,2);
131  mat[0][1] = transform(1,0);
132  mat[1][1] = transform(1,1);
133  mat[2][1] = transform(1,2);
134  mat[0][2] = transform(2,0);
135  mat[1][2] = transform(2,1);
136  mat[2][2] = transform(2,2);
137 
138  mat[3][0] = transform(0,3);
139  mat[3][1] = transform(1,3);
140  mat[3][2] = transform(2,3);
141  return mat;
142 }
143 
145 {
147  // gl is column wise
148  transform(0,0) = mat[0][0];
149  transform(0,1) = mat[1][0];
150  transform(0,2) = mat[2][0];
151  transform(1,0) = mat[0][1];
152  transform(1,1) = mat[1][1];
153  transform(1,2) = mat[2][1];
154  transform(2,0) = mat[0][2];
155  transform(2,1) = mat[1][2];
156  transform(2,2) = mat[2][2];
157 
158  transform(0,3) = mat[3][0];
159  transform(1,3) = mat[3][1];
160  transform(2,3) = mat[3][2];
161 
162  return transform;
163 }
164 
165 class Mesh
166 {
167 public:
168  Mesh() :
169  cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
170  normals(new pcl::PointCloud<pcl::Normal>),
171  indices(new std::vector<int>),
172  visible(true)
173  {
174  gains[0] = gains[1] = gains[2] = 1.0f;
175  }
176 
177  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
178  pcl::PointCloud<pcl::Normal>::Ptr normals;
179  pcl::IndicesPtr indices;
180  std::vector<pcl::Vertices> polygons;
181  std::vector<pcl::Vertices> polygonsLowRes;
182  rtabmap::Transform pose; // in rtabmap coordinates
183  bool visible;
185  double gains[3]; // RGB gains
186 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
187  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords;
188 #else
189  std::vector<Eigen::Vector2f> texCoords;
190 #endif
191  cv::Mat texture;
192 };
193 
194 typedef enum {
197 
200 
203 
206 
210 
211 inline int NormalizedColorCameraRotation(int camera_rotation) {
212  int camera_n = 0;
213  switch (camera_rotation) {
214  case 90:
215  camera_n = 1;
216  break;
217  case 180:
218  camera_n = 2;
219  break;
220  case 270:
221  camera_n = 3;
222  break;
223  default:
224  camera_n = 0;
225  break;
226  }
227  return camera_n;
228 }
229 
230 // Get the Android rotation integer value from color camera to display.
231 // This function is used to compute the orientation difference to handle
232 // the portrait and landscape mode for color camera display.
233 //
234 // @param display: the device display orientation.
235 // @param color_camera: integer value of color camera oreintation, values
236 // available are 0, 90, 180, 270. Followed by Android camera orientation
237 // standard:
238 // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
240  ScreenRotation display_rotation, int color_camera_rotation) {
241  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
242 
243  int ret = static_cast<int>(display_rotation) - color_camera_n;
244  if (ret < 0) {
245  ret += 4;
246  }
247  return static_cast<ScreenRotation>(ret % 4);
248 }
249 
250 // Get the Android rotation integer value from color camera to display.
251 // This function is used to compute the orientation difference to handle
252 // the portrait and landscape mode for color camera display.
253 //
254 // @param display: integer value of display orientation, values available
255 // are 0, 1, 2 ,3. Followed by Android display orientation standard:
256 // https://developer.android.com/reference/android/view/Display.html#getRotation()
257 // @param color_camera: integer value of color camera orientation, values
258 // available are 0, 90, 180, 270. Followed by Android camera orientation
259 // standard:
260 // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
262  int display_rotation, int color_camera_rotation) {
263  ScreenRotation r =
264  static_cast<ScreenRotation>(display_rotation);
266  r, color_camera_rotation);
267 }
268 
269 }
270 
271 #endif /* UTIL_H_ */
UEventsHandler
Definition: UEventsHandler.h:128
int
int
pcl
Definition: CameraOpenni.h:47
rtabmap::NormalizedColorCameraRotation
int NormalizedColorCameraRotation(int camera_rotation)
Definition: util.h:211
rtabmap::GetAndroidRotationFromColorCameraToDisplay
ScreenRotation GetAndroidRotationFromColorCameraToDisplay(ScreenRotation display_rotation, int color_camera_rotation)
Definition: util.h:239
ULogger::kError
@ kError
Definition: ULogger.h:252
ret
int ret
UEventsHandler::registerToEventsManager
void registerToEventsManager()
Definition: UEventsHandler.cpp:29
rtabmap::Mesh::polygonsLowRes
std::vector< pcl::Vertices > polygonsLowRes
Definition: util.h:181
rtabmap::tango_world_T_rtabmap_world
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)
rtabmap::CameraModel
Definition: CameraModel.h:38
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::rtabmap_world_T_tango_world
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)
rtabmap::Mesh::normals
pcl::PointCloud< pcl::Normal >::Ptr normals
Definition: util.h:178
rtabmap::ROTATION_IGNORED
@ ROTATION_IGNORED
Not apply any rotation.
Definition: util.h:196
ULogger::kDebug
@ kDebug
Definition: ULogger.h:252
util.h
rtabmap::LogHandler::handleEvent
virtual bool handleEvent(UEvent *event)
Definition: util.h:59
UEvent::getCode
int getCode() const
Definition: UEvent.h:74
true
#define true
Definition: ConvertUTF.c:57
rtabmap::Mesh::visible
bool visible
Definition: util.h:183
rtabmap::Mesh::pose
rtabmap::Transform pose
Definition: util.h:182
rtabmap::ROTATION_270
@ ROTATION_270
270 degree rotation.
Definition: util.h:208
glm::detail::tmat4x4
Definition: type_mat.hpp:47
rtabmap::LogHandler
Definition: util.h:42
LOGF
#define LOGF(...)
Definition: tango-gl/include/tango-gl/util.h:62
LOGE
#define LOGE(...)
Definition: tango-gl/include/tango-gl/util.h:61
rtabmap::Mesh
Definition: util.h:165
rtabmap::tango_device_T_rtabmap_world
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)
UEvent
Definition: UEvent.h:57
rtabmap::Mesh::cameraModel
rtabmap::CameraModel cameraModel
Definition: util.h:184
rtabmap::rtabmap_world_T_opengl_world
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)
ULogger::setEventLevel
static void setEventLevel(ULogger::Level eventSentLevel)
Definition: ULogger.h:348
UEvent::getClassName
virtual std::string getClassName() const =0
rtabmap::Mesh::texture
cv::Mat texture
Definition: util.h:191
rtabmap::opengl_world_T_rtabmap_world
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)
rtabmap::optical_T_opengl
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)
rtabmap::LogHandler::LogHandler
LogHandler()
Definition: util.h:45
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
ULogger::kInfo
@ kInfo
Definition: ULogger.h:252
rtabmap::ROTATION_90
@ ROTATION_90
90 degree rotation.
Definition: util.h:202
ULogEvent
Definition: ULogger.h:121
rtabmap::ROTATION_180
@ ROTATION_180
180 degree rotation.
Definition: util.h:205
rtabmap::Mesh::polygons
std::vector< pcl::Vertices > polygons
Definition: util.h:180
LOGI
#define LOGI(...)
Definition: tango-gl/include/tango-gl/util.h:53
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::Mesh::Mesh
Mesh()
Definition: util.h:168
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
LOGW
#define LOGW(...)
Definition: tango-gl/include/tango-gl/util.h:54
UEventsHandler.h
LOGD
#define LOGD(...)
Definition: tango-gl/include/tango-gl/util.h:52
rtabmap::ScreenRotation
ScreenRotation
Definition: util.h:194
std
rtabmap::Mesh::indices
pcl::IndicesPtr indices
Definition: util.h:179
rtabmap::Mesh::cloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud
Definition: util.h:177
ULogger::kFatal
@ kFatal
Definition: ULogger.h:252
CameraModel.h
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
ULogger::setPrintThreadId
static void setPrintThreadId(bool printThreadId)
Definition: ULogger.h:309
rtabmap::ROTATION_0
@ ROTATION_0
0 degree rotation (natural orientation)
Definition: util.h:199
rtabmap::glmFromTransform
glm::mat4 glmFromTransform(const rtabmap::Transform &transform)
Definition: util.h:124
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::glmToTransform
rtabmap::Transform glmToTransform(const glm::mat4 &mat)
Definition: util.h:144
rtabmap::Mesh::gains
double gains[3]
Definition: util.h:185
rtabmap::Mesh::texCoords
std::vector< Eigen::Vector2f > texCoords
Definition: util.h:189
ULogEvent::getMsg
const std::string & getMsg() const
Definition: ULogger.h:137
rtabmap::opengl_world_T_tango_world
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)
mat
else mat


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:23