LaserScan.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 CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
29 #define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
30 
31 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
32 
33 #include <rtabmap/core/Transform.h>
34 
35 namespace rtabmap {
36 
37 class RTABMAP_CORE_EXPORT LaserScan
38 {
39 public:
40  enum Format{kUnknown=0,
41  kXY=1,
42  kXYI=2,
43  kXYNormal=3,
44  kXYINormal=4,
45  kXYZ=5,
46  kXYZI=6,
47  kXYZRGB=7,
48  kXYZNormal=8,
49  kXYZINormal=9,
50  kXYZRGBNormal=10,
51  kXYZIT=11};
52 
53  static std::string formatName(const Format & format);
54  static int channels(const Format & format);
55  static bool isScan2d(const Format & format);
56  static bool isScanHasNormals(const Format & format);
57  static bool isScanHasRGB(const Format & format);
58  static bool isScanHasIntensity(const Format & format);
59  static bool isScanHasTime(const Format & format);
60  static LaserScan backwardCompatibility(
61  const cv::Mat & oldScanFormat,
62  int maxPoints = 0,
63  int maxRange = 0,
64  const Transform & localTransform = Transform::getIdentity());
65  static LaserScan backwardCompatibility(
66  const cv::Mat & oldScanFormat,
67  float minRange,
68  float maxRange,
69  float angleMin,
70  float angleMax,
71  float angleInc,
72  const Transform & localTransform = Transform::getIdentity());
73 
74 public:
75  LaserScan();
76  LaserScan(const LaserScan & data,
77  int maxPoints,
78  float maxRange,
79  const Transform & localTransform = Transform::getIdentity());
80  // Use version without \"format\" argument.
82  int maxPoints,
83  float maxRange,
84  Format format,
85  const Transform & localTransform = Transform::getIdentity());
86  LaserScan(const cv::Mat & data,
87  int maxPoints,
88  float maxRange,
89  Format format,
90  const Transform & localTransform = Transform::getIdentity());
91  // Use version without \"format\" argument.
93  Format format,
94  float minRange,
95  float maxRange,
96  float angleMin,
97  float angleMax,
98  float angleIncrement,
99  const Transform & localTransform = Transform::getIdentity());
100  LaserScan(const LaserScan & data,
101  float minRange,
102  float maxRange,
103  float angleMin,
104  float angleMax,
105  float angleIncrement,
106  const Transform & localTransform = Transform::getIdentity());
107  LaserScan(const cv::Mat & data,
108  Format format,
109  float minRange,
110  float maxRange,
111  float angleMin,
112  float angleMax,
113  float angleIncrement,
114  const Transform & localTransform = Transform::getIdentity());
115 
116  const cv::Mat & data() const {return data_;}
117  Format format() const {return format_;}
118  std::string formatName() const {return formatName(format_);}
119  int channels() const {return data_.channels();}
120  int maxPoints() const {return maxPoints_;}
121  float rangeMin() const {return rangeMin_;}
122  float rangeMax() const {return rangeMax_;}
123  float angleMin() const {return angleMin_;}
124  float angleMax() const {return angleMax_;}
125  float angleIncrement() const {return angleIncrement_;}
126  void setLocalTransform(const Transform & t) {localTransform_ = t;}
127  Transform localTransform() const {return localTransform_;}
128 
129  bool empty() const {return data_.empty();}
130  bool isEmpty() const {return data_.empty();}
131  int size() const {return data_.total();}
132  int dataType() const {return data_.type();}
133  bool is2d() const {return isScan2d(format_);}
134  bool hasNormals() const {return isScanHasNormals(format_);}
135  bool hasRGB() const {return isScanHasRGB(format_);}
136  bool hasIntensity() const {return isScanHasIntensity(format_);}
137  bool hasTime() const {return isScanHasTime(format_);}
138  bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
139  bool isOrganized() const {return data_.rows > 1;}
140  LaserScan clone() const;
141  LaserScan densify() const;
142 
143  int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
144  int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
145  int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
146  int getTimeOffset() const {return hasTime()?4:-1;}
147 
148  float & field(unsigned int pointIndex, unsigned int channelOffset);
149 
150  void clear() {data_ = cv::Mat();}
151 
155  LaserScan & operator+=(const LaserScan &);
159  LaserScan operator+(const LaserScan &);
160 
161 private:
162  void init(const cv::Mat & data,
163  Format format,
164  float minRange,
165  float maxRange,
166  float angleMin,
167  float angleMax,
168  float angleIncrement,
169  int maxPoints,
170  const Transform & localTransform = Transform::getIdentity());
171 
172 private:
173  cv::Mat data_;
176  float rangeMin_;
177  float rangeMax_;
178  float angleMin_;
179  float angleMax_;
182 };
183 
184 }
185 
186 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ */
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::LaserScan::hasTime
bool hasTime() const
Definition: LaserScan.h:137
rtabmap::LaserScan::getTimeOffset
int getTimeOffset() const
Definition: LaserScan.h:146
rtabmap::LaserScan::Format
Format
Definition: LaserScan.h:40
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:120
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:122
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:135
rtabmap::LaserScan::angleIncrement_
float angleIncrement_
Definition: LaserScan.h:180
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::LaserScan::angleMax_
float angleMax_
Definition: LaserScan.h:179
rtabmap::RTABMAP_DEPRECATED
RTABMAP_DEPRECATED(typedef SensorData Image, "rtabmap::Image class is renamed to rtabmap::SensorData, use the last one instead.")
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:145
Transform.h
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:134
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:131
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:117
rtabmap::LaserScan::format_
Format format_
Definition: LaserScan.h:174
rtabmap::LaserScan::setLocalTransform
void setLocalTransform(const Transform &t)
Definition: LaserScan.h:126
glm::detail::operator+
GLM_FUNC_DECL tmat2x2< T, P > operator+(tmat2x2< T, P > const &m, T const &s)
rtabmap::LaserScan::rangeMin_
float rangeMin_
Definition: LaserScan.h:176
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:133
rtabmap::LaserScan::rangeMax_
float rangeMax_
Definition: LaserScan.h:177
rtabmap::LaserScan::maxPoints_
int maxPoints_
Definition: LaserScan.h:175
rtabmap::LaserScan::angleMin_
float angleMin_
Definition: LaserScan.h:178
rtabmap::LaserScan::getRGBOffset
int getRGBOffset() const
Definition: LaserScan.h:144
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:124
rtabmap::LaserScan::getIntensityOffset
int getIntensityOffset() const
Definition: LaserScan.h:143
rtabmap_netvlad.init
def init(descriptorDim)
Definition: rtabmap_netvlad.py:30
rtabmap::LaserScan::clear
void clear()
Definition: LaserScan.h:150
rtabmap::Transform
Definition: Transform.h:41
rtabmap::LaserScan::data_
cv::Mat data_
Definition: LaserScan.h:173
rtabmap::LaserScan::isOrganized
bool isOrganized() const
Definition: LaserScan.h:139
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:125
rtabmap::LaserScan::localTransform_
Transform localTransform_
Definition: LaserScan.h:181
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:136
rtabmap::LaserScan::channels
int channels() const
Definition: LaserScan.h:119
rtabmap::LaserScan::dataType
int dataType() const
Definition: LaserScan.h:132
rtabmap::LaserScan::formatName
std::string formatName() const
Definition: LaserScan.h:118
t
Point2 t(10, 10)
Format
Format
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::LaserScan::isCompressed
bool isCompressed() const
Definition: LaserScan.h:138
operator+=
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 & operator+=(bfloat16 &a, const bfloat16 &b)
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:123
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:121


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