46 void CObservation3DRangeScan::EXTERNALS_AS_TEXT(
bool value)
50 bool CObservation3DRangeScan::EXTERNALS_AS_TEXT()
57 #define COBS3DRANGE_USE_MEMPOOL 63 #ifdef COBS3DRANGE_USE_MEMPOOL 80 std::vector<float> pts_x, pts_y,
pts_z;
82 std::vector<uint16_t> idxs_x,
idxs_y;
97 return H == req.
H && W == req.
W;
168 CObservation3DRangeScan::CObservation3DRangeScan()
169 : m_points3D_external_stored(false),
170 m_rangeImage_external_stored(false),
172 hasRangeImage(false),
173 range_is_depth(true),
174 hasIntensityImage(false),
175 intensityImageChannel(CH_VISIBLE),
176 hasConfidenceImage(false),
179 cameraParamsIntensity(),
180 relativePoseIntensityWRTDepth(
194 #ifdef COBS3DRANGE_USE_MEMPOOL 309 vector<char> validRange(N);
311 &validRange[0],
sizeof(validRange[0]) * N);
330 #ifdef COBS3DRANGE_USE_MEMPOOL 404 in >> do_have_labels;
463 M.loadFromTextFile(fil);
466 M.extractRow(0,
const_cast<std::vector<float>&
>(
points3D_x));
467 M.extractRow(1,
const_cast<std::vector<float>&
>(
points3D_y));
468 M.extractRow(2,
const_cast<std::vector<float>&
>(
points3D_z));
473 f >>
const_cast<std::vector<float>&
>(
points3D_x) >>
474 const_cast<std::vector<float>&
>(
points3D_y) >>
522 out_path = CImage::getImagesPathBase();
523 size_t N = CImage::getImagesPathBase().size() - 1;
524 if (CImage::getImagesPathBase()[N] !=
'/' &&
525 CImage::getImagesPathBase()[N] !=
'\\')
542 out_path = CImage::getImagesPathBase();
543 size_t N = CImage::getImagesPathBase().size() - 1;
544 if (CImage::getImagesPathBase()[N] !=
'/' &&
545 CImage::getImagesPathBase()[N] !=
'\\')
568 const string savedDir = CImage::getImagesPathBase();
569 CImage::setImagesPathBase(use_this_base_dir);
570 const string real_absolute_file_path =
572 CImage::setImagesPathBase(savedDir);
613 const string savedDir = CImage::getImagesPathBase();
614 CImage::setImagesPathBase(use_this_base_dir);
615 const string real_absolute_file_path =
617 CImage::setImagesPathBase(savedDir);
636 #define CALIB_DECIMAT 15 649 : obs(obs_), z_offset(z_offset_)
656 if (
x.size() < 4 + 4)
x.resize(4 + 4);
663 for (
size_t i = 0; i < 4; i++)
x[4 + i] = camPar.
dist[i];
672 for (
size_t i = 0; i < 4; i++) camPar.
dist[i] =
x[4 + i];
682 const size_t nC = obs.
rangeImage.getColCount();
683 const size_t nR = obs.
rangeImage.getRowCount();
691 const size_t idx = nC *
r +
c;
702 const double x = P.
x / P.
z;
703 const double y = P.
y / P.
z;
707 const double r4 =
square(r2);
724 err.push_back(
c - pixel.
x);
725 err.push_back(
r - pixel.
y);
742 const double camera_offset)
753 TMyLevMar::TResultInfo info;
755 const size_t nR = obs.
rangeImage.getRowCount();
756 const size_t nC = obs.
rangeImage.getColCount();
771 increments_x.assign(1e-4);
779 mrpt::utils::LVL_INFO,
781 1e-3, 1e-9, 1e-9,
false);
783 const double avr_px_err =
786 out_camParams.
ncols = nC;
787 out_camParams.
nrows = nR;
798 const unsigned int& r2,
const unsigned int& c1,
const unsigned int& c2)
803 ASSERT_((r1 < r2) && (c1 < c2))
804 ASSERT_((r2 < rows) && (c2 < cols))
841 for (
unsigned int i = r1; i < r2; i++)
842 for (
unsigned int j = c1; j < c2; j++)
861 #ifdef COBS3DRANGE_USE_MEMPOOL 924 #ifdef COBS3DRANGE_USE_MEMPOOL 953 static const double EPSILON = 1e-7;
1006 const double real_FOV_left = atan2(cx, fx);
1007 const double real_FOV_right = atan2(nCols - 1 - cx, fx);
1010 const float FOV_equiv = 2. * std::max(real_FOV_left, real_FOV_right);
1017 const size_t nLaserRays =
1035 const float tan_min = -tan(std::abs(sp.
angle_inf));
1036 const float tan_max = tan(std::abs(sp.
angle_sup));
1040 std::vector<float> vert_ang_tan(nRows);
1041 for (
size_t r = 0;
r < nRows;
r++)
1042 vert_ang_tan[
r] = static_cast<float>((cy -
r) / fy);
1052 double ang = -FOV_equiv * 0.5;
1053 const double A_ang = FOV_equiv / (nLaserRays - 1);
1062 for (
size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1065 const double tan_ang = tan(ang);
1068 static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1071 bool any_valid =
false;
1072 float closest_range = out_scan2d.
maxRange;
1074 for (
size_t r = 0;
r < nRows;
r++)
1080 const float this_point_tan = vert_ang_tan[
r] * D;
1081 if (this_point_tan > tan_min && this_point_tan < tan_max)
1093 i, closest_range * std::sqrt(1.0 + tan_ang * tan_ang));
1108 mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
1111 const std::vector<float> &xs = pc->getArrayX(), &ys = pc->getArrayY(),
1112 &zs = pc->getArrayZ();
1113 const size_t N = xs.size();
1115 const double A_ang = FOV_equiv / (nLaserRays - 1);
1116 const double ang0 = -FOV_equiv * 0.5;
1118 for (
size_t i = 0; i < N; i++)
1120 if (zs[i] < sp.
z_min || zs[i] > sp.
z_max)
continue;
1122 const double phi_wrt_origin = atan2(ys[i], xs[i]);
1124 int i_range = (phi_wrt_origin - ang0) / A_ang;
1125 if (i_range < 0 || i_range >=
int(nLaserRays))
continue;
1127 const float r_wrt_origin = ::hypotf(xs[i], ys[i]);
1128 if (out_scan2d.
scan[i_range] > r_wrt_origin)
1142 o <<
"Homogeneous matrix for the sensor's 3D pose, relative to robot " 1146 o <<
"maxRange = " <<
maxRange <<
" m" << endl;
1148 o <<
"Has 3D point cloud? ";
1151 o <<
"YES: " <<
points3D_x.size() <<
" points";
1156 o <<
" (embedded)." << endl;
1161 o <<
"Has raw range data? " << (
hasRangeImage ?
"YES" :
"NO");
1168 o <<
" (embedded)." << endl;
1178 o <<
" (embedded).\n";
1180 o <<
"Source channel: " 1194 o <<
" (embedded)." << endl;
1197 o << endl <<
"Has pixel labels? " << (
hasPixelLabels() ?
"YES" :
"NO");
1200 o <<
" Human readable labels:" << endl;
1204 o <<
" label[" << it->first <<
"]: '" << it->second <<
"'" << endl;
1208 o <<
"Depth camera calibration parameters:" << endl;
1214 o << endl <<
"Intensity camera calibration parameters:" << endl;
1222 <<
"Pose of the intensity cam. wrt the depth cam:\n" 1255 in >> bitfield_bytes;
1260 switch (bitfield_bytes)
1279 throw std::runtime_error(
1280 "Unknown type of pixelLabel inner class while " 1299 z_min(-
std::numeric_limits<double>::max()),
1300 z_max(
std::numeric_limits<double>::max()),
1301 oversampling_ratio(1.2),
1302 use_origin_sensor_pose(false)
#define ASSERT_EQUAL_(__A, __B)
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.
A pair (x,y) of pixel coordinates (subpixel resolution).
virtual void internal_writeToStream(mrpt::utils::CStream &out) const =0
std::string m_rangeImage_external_file
rangeImage is in CImage::getImagesPathBase()+<this_file_name>
std::string rangeImage_getExternalStorageFile() const
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto() ...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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.
static bool EXTERNALS_AS_TEXT_value
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
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 ...
#define ASSERT_ABOVE_(__A, __B)
double focalLengthMeters
The focal length of the camera, in meters (can be used among 'intrinsicParams' to determine the pixel...
void mempool_donate_range_matrix(CObservation3DRangeScan &obs)
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
std::vector< uint16_t > idxs_x
for each point, the corresponding (x,y) pixel coordinates
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void setScanRange(const size_t i, const float val)
T3DPointsTo2DScanParams()
void unload() const noexcept
For external storage image objects only, this method unloads the image from memory (or does nothing i...
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
double cy() const
Get the value of the principal point y-coordinate (in pixels).
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...
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
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.
const mrpt::math::CMatrix * rangeMask_min
(Default: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
Look-up-table struct for project3DPointsFromDepthImageInto()
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
const Scalar * const_iterator
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.
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
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 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.
T square(const T x)
Inline function for the square of a number.
std::vector< float > points3D_z
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
float maxRange
The maximum range allowed by the device, in meters (e.g.
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
const uint8_t BITFIELD_BYTES
Minimum number of bytes required to hold MAX_NUM_DIFFERENT_LABELS bits.
A generic system for versatile memory pooling.
double fx() const
Get the value of the focal length x-value (in pixels).
This class implements a config file-like interface over a memory-stored string list.
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
void WriteBufferFixEndianness(const T *ptr, size_t ElementCount)
Writes a sequence of elemental datatypes, taking care of reordering their bytes from the running arch...
double fy() const
Get the value of the focal length y-value (in pixels).
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
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
std::vector< uint16_t > points3D_idxs_y
bool rangeImage_isExternallyStored() const
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
A helper class that can convert an enum value into its textual representation, and viceversa...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
virtual 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)
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.
uint32_t ncols
Camera resolution.
bool hasRangeImage
true means the field rangeImage contains valid data
double x
X,Y,Z coordinates.
mrpt::system::CGenericMemoryPool< CObservation3DRangeScan_Ranges_MemPoolParams, CObservation3DRangeScan_Ranges_MemPoolData > TMyRangesMemPool
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
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
void swap(CImage &o)
Very efficient swap of two images (just swap the internal pointers)
double cx() const
Get the value of the principal point x-coordinate (in pixels).
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
virtual void internal_readFromStream(mrpt::utils::CStream &in)=0
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
std::vector< uint16_t > idxs_y
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
void writeToStream(mrpt::utils::CStream &out) const
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>'s clear() method, but really forcing deallocating the memory...
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 ~CObservation3DRangeScan()
Destructor.
#define CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level...
GLdouble GLdouble GLdouble r
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
static TPixelLabelInfoBase * readAndBuildFromStream(mrpt::utils::CStream &in)
mrpt::math::CArrayDouble< 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
const mrpt::math::CMatrix * rangeMask_max
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...
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
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)...
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
mrpt::system::CGenericMemoryPool< CObservation3DRangeScan_Points_MemPoolParams, CObservation3DRangeScan_Points_MemPoolData > TMyPointsMemPool
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...
static double recoverCameraCalibrationParameters(const CObservation3DRangeScan &in_obs, mrpt::utils::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...
A matrix of dynamic size.
mrpt::math::CMatrix rangeImage
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 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)
mrpt::utils::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
GLsizei const GLfloat * value
void cost_func(const CVectorDouble &par, const TLevMarData &d, CVectorDouble &err)
std::string getExternalStorageFile() const noexcept
Only if isExternallyStored() returns true.
bool isExternallyStored() const noexcept
See setExternalStorage().
void mempool_donate_xyz_buffers(CObservation3DRangeScan &obs)
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
std::shared_ptr< CPointCloud > Ptr
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
This class is a "CSerializable" wrapper for "CMatrixFloat".
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_)
GLenum const GLfloat * params
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.
fixed floating point 'f'
Structure to hold the parameters of a pinhole camera model.
virtual void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
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...
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
void saveToConfigFile(const std::string §ion, mrpt::utils::CConfigFileBase &cfg) const
Save as a config block:
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.