49 void CObservation3DRangeScan::EXTERNALS_AS_TEXT(
bool value)
53 bool CObservation3DRangeScan::EXTERNALS_AS_TEXT()
59 #define COBS3DRANGE_USE_MEMPOOL 65 #ifdef COBS3DRANGE_USE_MEMPOOL 82 std::vector<float> pts_x, pts_y,
pts_z;
84 std::vector<uint16_t> idxs_x,
idxs_y;
98 return H == req.
H && W == req.
W;
166 CObservation3DRangeScan::CObservation3DRangeScan()
169 cameraParamsIntensity(),
170 relativePoseIntensityWRTDepth(
180 #ifdef COBS3DRANGE_USE_MEMPOOL 282 vector<char> validRange(N);
284 &validRange[0],
sizeof(validRange[0]) * N);
303 #ifdef COBS3DRANGE_USE_MEMPOOL 377 in >> do_have_labels;
438 const auto N = M.
cols();
440 auto& xs =
const_cast<std::vector<float>&
>(
points3D_x);
441 auto& ys =
const_cast<std::vector<float>&
>(
points3D_y);
442 auto& zs =
const_cast<std::vector<float>&
>(
points3D_z);
446 ::memcpy(&xs[0], &M(0, 0),
sizeof(
float) * N);
447 ::memcpy(&ys[0], &M(1, 0),
sizeof(
float) * N);
448 ::memcpy(&zs[0], &M(2, 0),
sizeof(
float) * N);
454 f >>
const_cast<std::vector<float>&
>(
points3D_x) >>
455 const_cast<std::vector<float>&
>(
points3D_y) >>
504 out_path = CImage::getImagesPathBase();
505 size_t N = CImage::getImagesPathBase().size() - 1;
506 if (CImage::getImagesPathBase()[N] !=
'/' &&
507 CImage::getImagesPathBase()[N] !=
'\\')
524 out_path = CImage::getImagesPathBase();
525 size_t N = CImage::getImagesPathBase().size() - 1;
526 if (CImage::getImagesPathBase()[N] !=
'/' &&
527 CImage::getImagesPathBase()[N] !=
'\\')
550 const string savedDir = CImage::getImagesPathBase();
551 CImage::setImagesPathBase(use_this_base_dir);
552 const string real_absolute_file_path =
554 CImage::setImagesPathBase(savedDir);
596 const string savedDir = CImage::getImagesPathBase();
597 CImage::setImagesPathBase(use_this_base_dir);
598 const string real_absolute_file_path =
600 CImage::setImagesPathBase(savedDir);
620 #define CALIB_DECIMAT 15 636 if (
x.size() < 4 + 4)
x.resize(4 + 4);
643 for (
size_t i = 0; i < 4; i++)
x[4 + i] = camPar.
dist[i];
652 for (
size_t i = 0; i < 4; i++) camPar.
dist[i] =
x[4 + i];
671 const size_t idx = nC *
r +
c;
682 const double x = P.
x / P.
z;
683 const double y = P.
y / P.
z;
687 const double r4 =
square(r2);
720 const double camera_offset)
731 TMyLevMar::TResultInfo info;
749 increments_x.
fill(1e-4);
759 1e-3, 1e-9, 1e-9,
false);
761 const double avr_px_err =
764 out_camParams.
ncols = nC;
765 out_camParams.
nrows = nR;
776 const unsigned int& r2,
const unsigned int& c1,
const unsigned int& c2)
781 ASSERT_((r1 < r2) && (c1 < c2));
782 ASSERT_((r2 < rows) && (c2 < cols));
818 for (
unsigned int i = r1; i < r2; i++)
819 for (
unsigned int j = c1; j < c2; j++)
838 #ifdef COBS3DRANGE_USE_MEMPOOL 901 #ifdef COBS3DRANGE_USE_MEMPOOL 930 static const double EPSILON = 1e-7;
983 const double real_FOV_left = atan2(cx, fx);
984 const double real_FOV_right = atan2(nCols - 1 - cx, fx);
987 const float FOV_equiv = 2. * std::max(real_FOV_left, real_FOV_right);
1011 const float tan_min = -tan(std::abs(sp.
angle_inf));
1012 const float tan_max = tan(std::abs(sp.
angle_sup));
1016 std::vector<float> vert_ang_tan(nRows);
1017 for (
size_t r = 0;
r < nRows;
r++)
1018 vert_ang_tan[
r] = static_cast<float>((cy -
r) / fy);
1028 double ang = -FOV_equiv * 0.5;
1029 const double A_ang = FOV_equiv / (nLaserRays - 1);
1038 for (
size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1041 const double tan_ang = tan(ang);
1044 static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1047 bool any_valid =
false;
1048 float closest_range = out_scan2d.
maxRange;
1050 for (
size_t r = 0;
r < nRows;
r++)
1056 const float this_point_tan = vert_ang_tan[
r] * D;
1057 if (this_point_tan > tan_min && this_point_tan < tan_max)
1069 i, closest_range * std::sqrt(1.0 + tan_ang * tan_ang));
1086 const std::vector<float>&xs = pc->getArrayX(), &ys = pc->getArrayY(),
1087 &zs = pc->getArrayZ();
1088 const size_t N = xs.size();
1090 const double A_ang = FOV_equiv / (nLaserRays - 1);
1091 const double ang0 = -FOV_equiv * 0.5;
1093 for (
size_t i = 0; i < N; i++)
1095 if (zs[i] < sp.
z_min || zs[i] > sp.
z_max)
continue;
1097 const double phi_wrt_origin = atan2(ys[i], xs[i]);
1099 int i_range = (phi_wrt_origin - ang0) / A_ang;
1100 if (i_range < 0 || i_range >=
int(nLaserRays))
continue;
1102 const float r_wrt_origin = ::hypotf(xs[i], ys[i]);
1103 if (out_scan2d.
scan[i_range] > r_wrt_origin)
1117 o <<
"Homogeneous matrix for the sensor's 3D pose, relative to robot " 1122 o <<
"maxRange = " <<
maxRange <<
" m" << endl;
1124 o <<
"Has 3D point cloud? ";
1127 o <<
"YES: " <<
points3D_x.size() <<
" points";
1132 o <<
" (embedded)." << endl;
1137 o <<
"Has raw range data? " << (
hasRangeImage ?
"YES" :
"NO");
1144 o <<
" (embedded)." << endl;
1154 o <<
" (embedded).\n";
1156 o <<
"Source channel: " 1170 o <<
" (embedded)." << endl;
1173 o << endl <<
"Has pixel labels? " << (
hasPixelLabels() ?
"YES" :
"NO");
1176 o <<
" Human readable labels:" << endl;
1177 for (
auto it =
pixelLabels->pixelLabelNames.begin();
1179 o <<
" label[" << it->first <<
"]: '" << it->second <<
"'" << endl;
1183 o <<
"Depth camera calibration parameters:" << endl;
1189 o << endl <<
"Intensity camera calibration parameters:" << endl;
1197 <<
"Pose of the intensity cam. wrt the depth cam:\n" 1211 out << BITFIELD_BYTES;
1213 this->internal_writeToStream(out);
1216 template <
unsigned int BYTES_REQUIRED_>
1223 pixelLabels.resize(nR, nC);
1227 in >> pixelLabelNames;
1229 template <
unsigned int BYTES_REQUIRED_>
1234 const auto nR =
static_cast<uint32_t>(pixelLabels.rows());
1235 const auto nC =
static_cast<uint32_t>(pixelLabels.cols());
1238 for (
uint32_t r = 0;
r < nR;
r++) out << pixelLabels.coeff(
r,
c);
1240 out << pixelLabelNames;
1256 in >> bitfield_bytes;
1261 switch (bitfield_bytes)
1280 throw std::runtime_error(
1281 "Unknown type of pixelLabel inner class while " 1300 z_min(-
std::numeric_limits<double>::max()),
1301 z_max(
std::numeric_limits<double>::max())
bool isSuitable(const CObservation3DRangeScan_Points_MemPoolParams &req) const
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
bool m_points3D_external_stored
If set to true, m_points3D_external_file is valid.
This class implements a config file-like interface over a memory-stored string list.
std::string m_rangeImage_external_file
rangeImage is in CImage::getImagesPathBase()+<this_file_name>
A compile-time fixed-size numeric matrix container.
std::string rangeImage_getExternalStorageFile() const
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto() ...
void writeToStream(mrpt::serialization::CArchive &out) const
bool points3D_isExternallyStored() const
bool isSuitable(const CObservation3DRangeScan_Ranges_MemPoolParams &req) const
void getZoneAsObs(CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2)
Extract a ROI of the 3D observation as a new one.
double x
X,Y,Z coordinates.
static bool EXTERNALS_AS_TEXT_value
size_t WH
Width*Height, that is, the number of 3D points.
TIntensityChannelID
Enum type for intensityImageChannel.
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
double fx() const
Get the value of the focal length x-value (in pixels).
void mempool_donate_range_matrix(CObservation3DRangeScan &obs)
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
std::vector< uint16_t > idxs_x
for each point, the corresponding (x,y) pixel coordinates
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void setScanRange(const size_t i, const float val)
static double recoverCameraCalibrationParameters(const CObservation3DRangeScan &in_obs, mrpt::img::TCamera &out_camParams, const double camera_offset=0.01)
A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a ran...
double DEG2RAD(const double x)
Degrees to radians.
void fill(const Scalar &val)
T3DPointsTo2DScanParams()
std::vector< uint16_t > points3D_idxs_x
If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z.
std::string points3D_getExternalStorageFile() const
void swap(CMatrixDynamic< T > &o)
Swap with another matrix very efficiently (just swaps a pointer and two integer values).
double oversampling_ratio
(Default=1.2=120%) How many more laser scans rays to create (read docs for CObservation3DRangeScan::c...
void dump_to_pool(const DATA_PARAMS ¶ms, POOLABLE_DATA *block)
Saves the passed data block (characterized by params) to the pool.
double angle_sup
(Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper & lower half-FOV angle (in radia...
This file implements several operations that operate element-wise on individual or pairs of container...
mrpt::math::CMatrixF rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)=0
void WriteBufferFixEndianness(const T *ptr, size_t ElementCount)
Writes a sequence of elemental datatypes, taking care of reordering their bytes from the running arch...
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
double fy() const
Get the value of the focal length y-value (in pixels).
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
static TPixelLabelInfoBase * readAndBuildFromStream(mrpt::serialization::CArchive &in)
void setRow(const Index row, const VECTOR &v)
A pair (x,y) of pixel coordinates (subpixel resolution).
bool isExternallyStored() const noexcept
See setExternalStorage().
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
void swap(CObservation &o)
Swap with another observation, ONLY the data defined here in the base class CObservation.
double z_min
(Default:-inf, +inf) [Only if use_origin_sensor_pose=true] Only obstacle points with Z coordinates wi...
POOLABLE_DATA * request_memory(const DATA_PARAMS ¶ms)
Request a block of data which fulfils the size requirements stated in params.
void unload() const noexcept
For external storage image objects only, this method unloads the image from memory (or does nothing i...
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
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
void push_back(const T &val)
void swap(CImage &o)
Efficiently swap of two images.
void rangeImage_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won't normally want to call this, it's only used from internal MRPT programs.
std::vector< float > points3D_z
const mrpt::math::CMatrixF * rangeMask_max
float maxRange
The maximum range allowed by the device, in meters (e.g.
static Ptr Create(Args &&... args)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream, std::istream, std::ostream, std::stringstream.
T square(const T x)
Inline function for the square of a number.
A generic system for versatile memory pooling.
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
#define ASSERT_(f)
Defines an assertion mechanism.
double focalLengthMeters
The focal length of the camera, in meters (can be used among 'intrinsicParams' to determine the pixel...
std::string rangeImage_getExternalStorageFileAbsolutePath() const
This base provides a set of functions for maths stuff.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
std::vector< float > points3D_y
double cy() const
Get the value of the principal point y-coordinate (in pixels).
CVectorDynamic< double > CVectorDouble
std::vector< uint16_t > points3D_idxs_y
bool rangeImage_isExternallyStored() const
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
void unload() override
Unload all images, for the case they being delayed-load images stored in external files (othewise...
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
void vec2cam(const CVectorDouble &x, TCamera &camPar)
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
std::string fileNameChangeExtension(const std::string &filename, const std::string &newExtension)
Replace the filename extension by another one.
Used in CObservation3DRangeScan::convertTo2DScan()
std::string m_points3D_external_file
3D points are in CImage::getImagesPathBase()+<this_file_name>
This namespace contains representation of robot actions and observations.
bool hasRangeImage
true means the field rangeImage contains valid data
Structure to hold the parameters of a pinhole camera model.
std::string getExternalStorageFile() const noexcept
Only if isExternallyStored() returns true.
void convertTo2DScan(mrpt::obs::CObservation2DRangeScan &out_scan2d, const T3DPointsTo2DScanParams &scanParams, const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Convert this 3D observation into an "equivalent 2D fake laser scan", with a configurable vertical FOV...
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
std::vector< float > pts_x
mrpt::img::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
const mrpt::math::CMatrixF * rangeMask_min
(Default: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
void resizeScanAndAssign(const size_t len, const float rangeVal, const bool rangeValidity, const int32_t rangeIntensity=0)
Resizes all data vectors to allocate a given number of scan rays and assign default values...
std::string points3D_getExternalStorageFileAbsolutePath() const
bool hasPoints3D
true means the field points3D contains valid data.
GLsizei const GLchar ** string
size_type rows() const
Number of rows in the matrix.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::vector< uint16_t > idxs_y
std::array< double, 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
const Scalar & coeff(int r, int c) const
fixed floating point 'f'
double cx() const
Get the value of the principal point x-coordinate (in pixels).
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
This class is a "CSerializable" wrapper for "CMatrixFloat".
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
GLdouble GLdouble GLdouble r
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>'s clear() method, but really forcing deallocating the memory...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
TPixelLabelInfoBase::Ptr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
void saveToConfigFile(const std::string §ion, mrpt::config::CConfigFileBase &cfg) const
Save as a config block:
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
#define ASSERT_ABOVE_(__A, __B)
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
void rangeImage_setSize(const int HEIGHT, const int WIDTH)
Similar to calling "rangeImage.setSize(H,W)" but this method provides memory pooling to speed-up the ...
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer...
void project3DPointsFromDepthImageInto(POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams, const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Project the RGB+D images into a 3D point cloud (with color if the target map supports it) and optiona...
void extract_patch(CImage &patch, const unsigned int col=0, const unsigned int row=0, const unsigned int width=1, const unsigned int height=1) const
Extract a patch from this image, saveing it into "patch" (its previous contents will be overwritten)...
mrpt::math::CMatrixF rangeImage
void cam2vec(const TCamera &camPar, CVectorDouble &x)
std::vector< float > pts_y
static CObservation3DRangeScan::TCached3DProjTables lut_3dproj
void points3D_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won't normally want to call this, it's only used from internal MRPT programs.
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
GLsizei const GLfloat * value
void cost_func(const CVectorDouble &par, const TLevMarData &d, CVectorDouble &err)
void resize(std::size_t N, bool zeroNewElements=false)
Saves data to a file and transparently compress the data using the given compression level...
void mempool_donate_xyz_buffers(CObservation3DRangeScan &obs)
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
mrpt::config::CConfigFileMemory CConfigFileMemory
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
~CObservation3DRangeScan() override
Destructor.
size_t getScanSize() const
Get the size of the scan pointcloud.
bool strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
unsigned __int32 uint32_t
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
TLevMarData(const CObservation3DRangeScan &obs_, const double z_offset_)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
GLenum const GLfloat * params
uint32_t ncols
Camera resolution.
Virtual interface to all pixel-label information structs.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
const CObservation3DRangeScan & obs
std::vector< float > pts_z
Grayscale or RGB visible channel of the camera sensor.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
bool use_origin_sensor_pose
(Default:false) If false, the conversion will be such that the 2D observation pose on the robot coinc...
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
static CGenericMemoryPool< DATA_PARAMS, POOLABLE_DATA > * getInstance(const size_t max_pool_entries=5)
Construct-on-first-use (~singleton) pattern: Return the unique instance of this class for a given tem...
float maxRange
The maximum range allowed by the device, in meters (e.g.
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
void setScanRangeValidity(const size_t i, const bool val)
bool hasConfidenceImage
true means the field confidenceImage contains valid data
bool do_range_filter(size_t r, size_t c, const float D) const
Returns true if the point (r,c) with depth D passes all filters.
void loadFromTextFile(std::istream &f)
Loads a vector/matrix from a text file, compatible with MATLAB text format.