35 "mrpt::maps::CColouredPointsMap,colourPointsMap",
38 CColouredPointsMap::TMapDefinition::TMapDefinition() =
default;
39 void CColouredPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
42 insertionOpts.loadFromConfigFile(
43 source, sectionNamePrefix +
string(
"_insertOpts"));
44 likelihoodOpts.loadFromConfigFile(
45 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
46 colourOpts.loadFromConfigFile(
47 source, sectionNamePrefix +
string(
"_colorOpts"));
50 void CColouredPointsMap::TMapDefinition::dumpToTextStream_map_specific(
51 std::ostream&
out)
const 53 this->insertionOpts.dumpToTextStream(
out);
54 this->likelihoodOpts.dumpToTextStream(
out);
55 this->colourOpts.dumpToTextStream(
out);
76 m_y.reserve(newLength);
77 m_z.reserve(newLength);
78 m_color_R.reserve(newLength);
79 m_color_G.reserve(newLength);
80 m_color_B.reserve(newLength);
88 m_x.resize(newLength, 0);
89 m_y.resize(newLength, 0);
90 m_z.resize(newLength, 0);
91 m_color_R.resize(newLength, 1);
92 m_color_G.resize(newLength, 1);
93 m_color_B.resize(newLength, 1);
100 void CColouredPointsMap::setSize(
size_t newLength)
102 m_x.assign(newLength, 0);
103 m_y.assign(newLength, 0);
104 m_z.assign(newLength, 0);
105 m_color_R.assign(newLength, 1);
106 m_color_G.assign(newLength, 1);
107 m_color_B.assign(newLength, 1);
111 void CColouredPointsMap::impl_copyFrom(
const CPointsMap& obj)
114 CPointsMap::base_copyFrom(obj);
120 m_color_G = pCol->m_color_G;
121 m_color_B = pCol->m_color_B;
125 uint8_t CColouredPointsMap::serializeGetVersion()
const {
return 9; }
128 uint32_t n = m_x.size();
135 out.WriteBufferFixEndianness(&m_x[0], n);
136 out.WriteBufferFixEndianness(&m_y[0], n);
137 out.WriteBufferFixEndianness(&m_z[0], n);
139 out << m_color_R << m_color_G << m_color_B;
141 out << genericMapParams;
142 insertionOptions.writeToStream(
144 likelihoodOptions.writeToStream(
out);
147 void CColouredPointsMap::serializeFrom(
169 in >> m_color_R >> m_color_G >> m_color_B;
172 in >> genericMapParams;
175 bool disableSaveAs3DObject;
176 in >> disableSaveAs3DObject;
177 genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
179 insertionOptions.readFromStream(in);
180 likelihoodOptions.readFromStream(in);
224 std::vector<uint32_t> dummy_pointWeight(n);
226 &dummy_pointWeight[0], n);
231 std::vector<uint32_t> dummy_pointWeight(n);
240 in >> insertionOptions.minDistBetweenLaserPoints >>
241 insertionOptions.addToExistingPointsMap >>
242 insertionOptions.also_interpolate >>
243 insertionOptions.disableDeletion >>
244 insertionOptions.fuseWithExisting >>
245 insertionOptions.isPlanarMap;
249 bool old_matchStaticPointsOnly;
250 in >> old_matchStaticPointsOnly;
253 in >> insertionOptions.maxDistForInterpolatePoints;
255 bool disableSaveAs3DObject;
256 in >> disableSaveAs3DObject;
257 genericMapParams.enableSaveAs3DObject =
258 !disableSaveAs3DObject;
264 in >> insertionOptions.horizontalTolerance;
269 in >> m_color_R >> m_color_G >> m_color_B;
276 std::vector<float> dummy_dist;
282 m_color_R.assign(m_x.size(), 1.0f);
283 m_color_G.assign(m_x.size(), 1.0f);
284 m_color_B.assign(m_x.size(), 1.0f);
289 likelihoodOptions.readFromStream(in);
292 in >> insertionOptions.insertInvalidPoints;
303 void CColouredPointsMap::internal_clear()
320 void CColouredPointsMap::setPointRGB(
321 size_t index,
float x,
float y,
float z,
float R,
float G,
float B)
327 this->m_color_R[index] =
R;
328 this->m_color_G[index] =
G;
329 this->m_color_B[index] = B;
336 void CColouredPointsMap::setPointColor(
size_t index,
float R,
float G,
float B)
339 this->m_color_R[index] =
R;
340 this->m_color_G[index] =
G;
341 this->m_color_B[index] = B;
345 void CColouredPointsMap::insertPointFast(
float x,
float y,
float z)
350 m_color_R.push_back(1);
351 m_color_G.push_back(1);
352 m_color_B.push_back(1);
357 void CColouredPointsMap::insertPointRGB(
358 float x,
float y,
float z,
float R,
float G,
float B)
363 m_color_R.push_back(
R);
364 m_color_G.push_back(
G);
365 m_color_B.push_back(B);
373 void CColouredPointsMap::getAs3DObject(
378 if (!genericMapParams.enableSaveAs3DObject)
return;
381 std::make_shared<opengl::CPointCloudColoured>();
383 obj->loadFromPointsMap(
this);
384 obj->setColor(1, 1, 1, 1.0);
386 obj->setPointSize(this->renderOptions.point_size);
394 CColouredPointsMap::TColourOptions::TColourOptions()
398 void CColouredPointsMap::TColourOptions::loadFromConfigFile(
401 scheme = source.
read_enum(section,
"scheme", scheme);
407 void CColouredPointsMap::TColourOptions::dumpToTextStream(
408 std::ostream&
out)
const 410 out <<
"\n----------- [CColouredPointsMap::TColourOptions] ------------ " 413 out <<
"scheme = " << scheme << endl;
414 out <<
"z_min = " << z_min << endl;
415 out <<
"z_max = " << z_max << endl;
416 out <<
"d_max = " << d_max << endl;
419 void CColouredPointsMap::getPointRGB(
420 size_t index,
float& x,
float& y,
float& z,
float&
R,
float&
G,
428 R = m_color_R[index];
429 G = m_color_G[index];
430 B = m_color_B[index];
435 void CColouredPointsMap::getPointColor(
436 size_t index,
float&
R,
float&
G,
float& B)
const 440 R = m_color_R[index];
441 G = m_color_G[index];
442 B = m_color_B[index];
450 bool accept_points_behind)
454 const double x = P.
x / P.
z;
455 const double y = P.
y / P.
z;
459 const double r4 =
square(r2);
460 const double r6 = r2 * r4;
465 2 *
params.dist[2] * x * y +
470 2 *
params.dist[3] * x * y +
477 bool CColouredPointsMap::colourFromObservation(
487 cameraPoseW = robotPose + cameraPoseR;
494 std::vector<TPixelCoordf>
496 std::vector<TPixelCoordf>::iterator
498 std::vector<size_t> p_idx;
499 std::vector<float> p_dist;
500 std::vector<unsigned int> p_proj;
503 kdTreeNClosestPoint2DIdx(
504 cameraPoseW.
x(), cameraPoseW.
y(),
509 for (
size_t k = 0; k < p_idx.size(); k++)
511 float d = sqrt(p_dist[k]);
512 size_t idx = p_idx[k];
513 if (d < colorScheme.d_max)
519 projectedPoints.push_back(px);
525 unsigned int chR, chG, chB;
539 unsigned int n_proj = 0;
540 const float factor = 1.0f / 255;
544 for (itProPoints = projectedPoints.begin(), k = 0;
545 itProPoints != projectedPoints.end(); ++itProPoints, ++k)
548 if (itProPoints->x >= 0 && itProPoints->x < imgW &&
549 itProPoints->y > 0 && itProPoints->y < imgH)
551 unsigned int ii = p_idx[p_proj[k]];
552 uint8_t* p = obs.
image(
553 (
unsigned int)itProPoints->x, (
unsigned int)itProPoints->y);
555 m_color_R[ii] = p[chR] * factor;
556 m_color_G[ii] = p[chG] * factor;
557 m_color_B[ii] = p[chB] * factor;
567 void CColouredPointsMap::resetPointsMinDist(
float defValue)
573 bool CColouredPointsMap::save3D_and_colour_to_text_file(
574 const std::string& file)
const 577 if (!f)
return false;
579 for (
size_t i = 0; i < m_x.size(); i++)
581 f,
"%f %f %f %d %d %d\n", m_x[i], m_y[i], m_z[i],
582 (uint8_t)(255 * m_color_R[i]), (uint8_t)(255 * m_color_G[i]),
583 (uint8_t)(255 * m_color_B[i]));
594 void CColouredPointsMap::PLY_import_set_vertex_count(
const size_t N)
604 void CColouredPointsMap::PLY_import_set_vertex(
609 idx, pt.
x, pt.
y, pt.
z, pt_color->
R, pt_color->
G, pt_color->
B);
611 this->setPoint(idx, pt.
x, pt.
y, pt.
z);
619 void CColouredPointsMap::PLY_export_get_vertex(
629 pt_color.
R = m_color_R[idx];
630 pt_color.
G = m_color_G[idx];
631 pt_color.
B = m_color_B[idx];
637 void CColouredPointsMap::addFrom_classSpecific(
638 const CPointsMap& anotherMap,
const size_t nPreviousPoints)
640 const size_t nOther = anotherMap.
size();
643 const auto* anotheMap_col =
648 for (
size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
650 m_color_R[j] = anotheMap_col->
m_color_R[i];
651 m_color_G[j] = anotheMap_col->m_color_G[i];
652 m_color_B[j] = anotheMap_col->m_color_B[i];
677 lric.
fVars.resize(4);
692 const float rel_z = gz - lric.
HM(2, 3);
695 float& pR = lric.
fVars[0];
696 float& pG = lric.
fVars[1];
697 float& pB = lric.
fVars[2];
698 float Az_1_color = lric.
fVars[3];
704 case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
705 case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
714 CColouredPointsMap::cmFromHeightRelativeToSensorGray)
724 case CColouredPointsMap::cmFromIntensityImage:
741 float& pR = lric.
fVars[0];
742 float& pG = lric.
fVars[1];
743 float& pB = lric.
fVars[2];
768 lric.
fVars.resize(9);
769 float& cx = lric.
fVars[5];
770 float& cy = lric.
fVars[6];
771 float& fx = lric.
fVars[7];
772 float& fy = lric.
fVars[8];
779 lric.
uVars.resize(4);
780 unsigned int& imgW = lric.
uVars[0];
781 unsigned int& imgH = lric.
uVars[1];
782 unsigned int& img_idx_x = lric.
uVars[2];
783 unsigned int& img_idx_y = lric.
uVars[3];
789 lric.
bVars.resize(3);
790 uint8_t& hasValidIntensityImage = lric.
bVars[0];
791 uint8_t& hasColorIntensityImg = lric.
bVars[1];
792 uint8_t& simple_3d_to_color_relation = lric.
bVars[2];
797 lric.
fVars[4] = 1.0f / 255;
799 hasValidIntensityImage =
false;
810 hasValidIntensityImage =
true;
816 hasColorIntensityImg =
825 simple_3d_to_color_relation =
847 float& pR = lric.
fVars[0];
848 float& pG = lric.
fVars[1];
849 float& pB = lric.
fVars[2];
850 float& Az_1_color = lric.
fVars[3];
851 float& K_8u = lric.
fVars[4];
852 float& cx = lric.
fVars[5];
853 float& cy = lric.
fVars[6];
854 float& fx = lric.
fVars[7];
855 float& fy = lric.
fVars[8];
857 unsigned int& imgW = lric.
uVars[0];
858 unsigned int& imgH = lric.
uVars[1];
859 unsigned int& img_idx_x = lric.
uVars[2];
860 unsigned int& img_idx_y = lric.
uVars[3];
862 const uint8_t& hasValidIntensityImage = lric.
bVars[0];
863 const uint8_t& hasColorIntensityImg = lric.
bVars[1];
864 const uint8_t& simple_3d_to_color_relation = lric.
bVars[2];
867 const float rel_z = gz - lric.
HM(2, 3);
873 case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
874 case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
883 CColouredPointsMap::cmFromHeightRelativeToSensorGray)
893 case CColouredPointsMap::cmFromIntensityImage:
896 bool hasValidColor =
false;
897 if (simple_3d_to_color_relation)
899 hasValidColor =
true;
913 img_idx_x = cx + fx * pt.
x / pt.
z;
914 img_idx_y = cy + fy * pt.
y / pt.
z;
916 hasValidColor = img_idx_x < imgW &&
924 if (hasValidColor && hasColorIntensityImg)
926 const auto* c = ii.
ptr<uint8_t>(img_idx_x, img_idx_y);
931 else if (hasValidColor && hasValidIntensityImage)
933 const auto c = ii.at<uint8_t>(img_idx_x, img_idx_y);
934 pR = pG = pB = c * K_8u;
954 float& pR = lric.
fVars[0];
955 float& pG = lric.
fVars[1];
956 float& pB = lric.
fVars[2];
973 unsigned int& imgW = lric.
uVars[0];
974 unsigned int& img_idx_x = lric.
uVars[2];
975 unsigned int& img_idx_y = lric.
uVars[3];
977 const uint8_t& hasValidIntensityImage = lric.
bVars[0];
978 const uint8_t& simple_3d_to_color_relation = lric.
bVars[2];
982 if (simple_3d_to_color_relation && hasValidIntensityImage)
984 if (++img_idx_x >= imgW)
994 void CColouredPointsMap::loadFromRangeScan(
1002 void CColouredPointsMap::loadFromRangeScan(
static void internal_loadFromRangeScan3D_prepareOneRange(CColouredPointsMap &me, const float gx, const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
#define THROW_EXCEPTION(msg)
double fx() const
Get the value of the focal length x-value (in pixels).
const mrpt::obs::CObservation3DRangeScan & rangeScan
int void fclose(FILE *f)
An OS-independent version of fclose.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
double fy() const
Get the value of the focal length y-value (in pixels).
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
size_t getHeight() const override
Returns the height of the image in pixels.
mrpt::vision::TStereoCalibParams params
mrpt::aligned_std_vector< float > m_color_R
The color data.
std::string getChannelsOrder() const
As of mrpt 2.0.0, this returns either "GRAY" or "BGR".
A pair (x,y) of pixel coordinates (subpixel resolution).
static void internal_loadFromRangeScan2D_prepareOneRange(CColouredPointsMap &me, const float gx, const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
TColourOptions colorScheme
The options employed when inserting laser scans in the map.
void reserve(size_t newLength) override
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
ENUMTYPE read_enum(const std::string §ion, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts
#define ASSERT_(f)
Defines an assertion mechanism.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
mrpt::aligned_std_vector< float > m_color_B
size_t getWidth() const override
Returns the width of the image in pixels.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
mrpt::img::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
void jet2rgb(const float color_index, float &r, float &g, float &b)
Computes the RGB color components (range [0,1]) for the corresponding color index in the range [0...
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
float scan_x
In internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points b...
static void internal_loadFromRangeScan2D_init(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
This namespace contains representation of robot actions and observations.
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Parameters for the Brown-Conrady camera lens distortion model.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
mrpt::img::TCamera cameraParams
Intrinsic and distortion parameters of the camera.
double x() const
Common members of all points & poses classes.
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
mrpt::maps::CColouredPointsMap::TColourOptions colourOpts
#define ASSERT_NOT_EQUAL_(__A, __B)
static void internal_loadFromRangeScan3D_postPushBack(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
A map of 2D/3D points with individual colours (RGB).
double norm() const
Returns the euclidean norm of vector: .
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
mrpt::aligned_std_vector< float > m_color_G
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
return_t square(const num_t x)
Inline function for the square of a number.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
double cx() const
Get the value of the principal point x-coordinate (in pixels).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
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.
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>'s clear() method, but really forcing deallocating the memory...
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
mrpt::vision::TStereoCalibResults out
bool hasIntensityImage
true means the field intensityImage contains valid data
An RGBA color - floats in the range [0,1].
std::vector< uint8_t > bVars
const T * ptr(unsigned int col, unsigned int row, unsigned int channel=0) const
Returns a pointer to a given pixel, without checking for boundaries.
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
static void aux_projectPoint_with_distortion(const mrpt::math::TPoint3D &P, const TCamera ¶ms, TPixelCoordf &pixel, bool accept_points_behind)
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
static void internal_loadFromRangeScan2D_postPushBack(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
static void internal_loadFromRangeScan3D_postOneRange(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
static void internal_loadFromRangeScan3D_init(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
std::vector< unsigned int > uVars