37 CColouredPointsMap::TMapDefinition::TMapDefinition() =
default;
38 void CColouredPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
41 insertionOpts.loadFromConfigFile(
42 source, sectionNamePrefix +
string(
"_insertOpts"));
43 likelihoodOpts.loadFromConfigFile(
44 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
45 colourOpts.loadFromConfigFile(
46 source, sectionNamePrefix +
string(
"_colorOpts"));
49 void CColouredPointsMap::TMapDefinition::dumpToTextStream_map_specific(
50 std::ostream& out)
const 52 this->insertionOpts.dumpToTextStream(out);
53 this->likelihoodOpts.dumpToTextStream(out);
54 this->colourOpts.dumpToTextStream(out);
73 #include <pcl/io/pcd_io.h> 74 #include <pcl/point_types.h> 78 void CColouredPointsMap::reserve(
size_t newLength)
80 m_x.reserve(newLength);
81 m_y.reserve(newLength);
82 m_z.reserve(newLength);
83 m_color_R.reserve(newLength);
84 m_color_G.reserve(newLength);
85 m_color_B.reserve(newLength);
91 void CColouredPointsMap::resize(
size_t newLength)
93 m_x.resize(newLength, 0);
94 m_y.resize(newLength, 0);
95 m_z.resize(newLength, 0);
96 m_color_R.resize(newLength, 1);
97 m_color_G.resize(newLength, 1);
98 m_color_B.resize(newLength, 1);
105 void CColouredPointsMap::setSize(
size_t newLength)
107 m_x.assign(newLength, 0);
108 m_y.assign(newLength, 0);
109 m_z.assign(newLength, 0);
110 m_color_R.assign(newLength, 1);
111 m_color_G.assign(newLength, 1);
112 m_color_B.assign(newLength, 1);
119 CPointsMap::base_copyFrom(
obj);
125 m_color_G = pCol->m_color_G;
126 m_color_B = pCol->m_color_B;
130 uint8_t CColouredPointsMap::serializeGetVersion()
const {
return 9; }
144 out << m_color_R << m_color_G << m_color_B;
146 out << genericMapParams;
147 insertionOptions.writeToStream(
149 likelihoodOptions.writeToStream(out);
152 void CColouredPointsMap::serializeFrom(
170 in.ReadBufferFixEndianness(&m_x[0],
n);
171 in.ReadBufferFixEndianness(&m_y[0],
n);
172 in.ReadBufferFixEndianness(&m_z[0],
n);
174 in >> m_color_R >> m_color_G >> m_color_B;
177 in >> genericMapParams;
180 bool disableSaveAs3DObject;
181 in >> disableSaveAs3DObject;
182 genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
184 insertionOptions.readFromStream(
in);
185 likelihoodOptions.readFromStream(
in);
208 in.ReadBufferFixEndianness(&m_x[0],
n);
209 in.ReadBufferFixEndianness(&m_y[0],
n);
210 in.ReadBufferFixEndianness(&m_z[0],
n);
229 std::vector<uint32_t> dummy_pointWeight(
n);
230 in.ReadBufferFixEndianness(
231 &dummy_pointWeight[0],
n);
236 std::vector<uint32_t> dummy_pointWeight(
n);
237 in.ReadBufferFixEndianness(&dummy_pointWeight[0],
n);
245 in >> insertionOptions.minDistBetweenLaserPoints >>
246 insertionOptions.addToExistingPointsMap >>
247 insertionOptions.also_interpolate >>
248 insertionOptions.disableDeletion >>
249 insertionOptions.fuseWithExisting >>
250 insertionOptions.isPlanarMap;
254 bool old_matchStaticPointsOnly;
255 in >> old_matchStaticPointsOnly;
258 in >> insertionOptions.maxDistForInterpolatePoints;
260 bool disableSaveAs3DObject;
261 in >> disableSaveAs3DObject;
262 genericMapParams.enableSaveAs3DObject =
263 !disableSaveAs3DObject;
269 in >> insertionOptions.horizontalTolerance;
274 in >> m_color_R >> m_color_G >> m_color_B;
281 std::vector<float> dummy_dist;
287 m_color_R.assign(m_x.size(), 1.0f);
288 m_color_G.assign(m_x.size(), 1.0f);
289 m_color_B.assign(m_x.size(), 1.0f);
294 likelihoodOptions.readFromStream(
in);
297 in >> insertionOptions.insertInvalidPoints;
308 void CColouredPointsMap::internal_clear()
325 void CColouredPointsMap::setPointRGB(
326 size_t index,
float x,
float y,
float z,
float R,
float G,
float B)
332 this->m_color_R[
index] =
R;
333 this->m_color_G[
index] =
G;
334 this->m_color_B[
index] = B;
341 void CColouredPointsMap::setPointColor(
size_t index,
float R,
float G,
float B)
344 this->m_color_R[
index] =
R;
345 this->m_color_G[
index] =
G;
346 this->m_color_B[
index] = B;
350 void CColouredPointsMap::insertPointFast(
float x,
float y,
float z)
355 m_color_R.push_back(1);
356 m_color_G.push_back(1);
357 m_color_B.push_back(1);
362 void CColouredPointsMap::insertPointRGB(
363 float x,
float y,
float z,
float R,
float G,
float B)
368 m_color_R.push_back(
R);
369 m_color_G.push_back(
G);
370 m_color_B.push_back(B);
378 void CColouredPointsMap::getAs3DObject(
383 if (!genericMapParams.enableSaveAs3DObject)
return;
386 std::make_shared<opengl::CPointCloudColoured>();
388 obj->loadFromPointsMap(
this);
389 obj->setColor(1, 1, 1, 1.0);
391 obj->setPointSize(this->renderOptions.point_size);
399 CColouredPointsMap::TColourOptions::TColourOptions()
403 void CColouredPointsMap::TColourOptions::loadFromConfigFile(
406 scheme =
source.read_enum(section,
"scheme", scheme);
412 void CColouredPointsMap::TColourOptions::dumpToTextStream(
413 std::ostream& out)
const 415 out <<
"\n----------- [CColouredPointsMap::TColourOptions] ------------ " 418 out <<
"scheme = " << scheme << endl;
419 out <<
"z_min = " << z_min << endl;
420 out <<
"z_max = " << z_max << endl;
421 out <<
"d_max = " << d_max << endl;
424 void CColouredPointsMap::getPointRGB(
425 size_t index,
float&
x,
float&
y,
float&
z,
float&
R,
float&
G,
435 B = m_color_B[
index];
440 void CColouredPointsMap::getPointColor(
441 size_t index,
float&
R,
float&
G,
float& B)
const 447 B = m_color_B[
index];
455 bool accept_points_behind)
459 const double x = P.
x / P.
z;
460 const double y = P.
y / P.
z;
464 const double r4 =
square(r2);
465 const double r6 = r2 * r4;
482 bool CColouredPointsMap::colourFromObservation(
492 cameraPoseW = robotPose + cameraPoseR;
499 std::vector<TPixelCoordf>
501 std::vector<TPixelCoordf>::iterator
503 std::vector<size_t> p_idx;
504 std::vector<float> p_dist;
505 std::vector<unsigned int> p_proj;
508 kdTreeNClosestPoint2DIdx(
509 cameraPoseW.
x(), cameraPoseW.
y(),
514 for (
size_t k = 0; k < p_idx.size(); k++)
516 float d = sqrt(p_dist[k]);
517 size_t idx = p_idx[k];
518 if (d < colorScheme.d_max)
524 projectedPoints.push_back(px);
530 unsigned int chR, chG, chB;
544 unsigned int n_proj = 0;
545 const float factor = 1.0f / 255;
549 for (itProPoints = projectedPoints.begin(), k = 0;
550 itProPoints != projectedPoints.end(); ++itProPoints, ++k)
553 if (itProPoints->x >= 0 && itProPoints->x < imgW &&
554 itProPoints->y > 0 && itProPoints->y < imgH)
556 unsigned int ii = p_idx[p_proj[k]];
558 (
unsigned int)itProPoints->x, (
unsigned int)itProPoints->y);
560 m_color_R[ii] =
p[chR] * factor;
561 m_color_G[ii] =
p[chG] * factor;
562 m_color_B[ii] =
p[chB] * factor;
572 void CColouredPointsMap::resetPointsMinDist(
float defValue)
578 bool CColouredPointsMap::save3D_and_colour_to_text_file(
582 if (!f)
return false;
584 for (
unsigned int i = 0; i < m_x.size(); i++)
586 f,
"%f %f %f %d %d %d\n", m_x[i], m_y[i], m_z[i],
588 (
uint8_t)(255 * m_color_B[i]));
601 void CColouredPointsMap::PLY_import_set_vertex_count(
const size_t N)
611 void CColouredPointsMap::PLY_import_set_vertex(
616 idx, pt.
x, pt.
y, pt.
z, pt_color->
R, pt_color->
G, pt_color->
B);
618 this->setPoint(idx, pt.
x, pt.
y, pt.
z);
626 void CColouredPointsMap::PLY_export_get_vertex(
636 pt_color.
R = m_color_R[idx];
637 pt_color.
G = m_color_G[idx];
638 pt_color.
B = m_color_B[idx];
644 void CColouredPointsMap::addFrom_classSpecific(
645 const CPointsMap& anotherMap,
const size_t nPreviousPoints)
647 const size_t nOther = anotherMap.
size();
650 const auto* anotheMap_col =
655 for (
size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
657 m_color_R[j] = anotheMap_col->
m_color_R[i];
658 m_color_G[j] = anotheMap_col->m_color_G[i];
659 m_color_B[j] = anotheMap_col->m_color_B[i];
666 bool CColouredPointsMap::savePCDFile(
667 const std::string& filename,
bool save_as_binary)
const 670 pcl::PointCloud<pcl::PointXYZRGB> cloud;
672 const size_t nThis = this->
size();
677 cloud.is_dense =
false;
678 cloud.points.resize(cloud.width * cloud.height);
680 const float f = 255.f;
687 for (
size_t i = 0; i < nThis; ++i)
689 cloud.points[i].x = m_x[i];
690 cloud.points[i].y = m_y[i];
691 cloud.points[i].z = m_z[i];
693 aux_val.rgb[0] =
static_cast<uint8_t>(this->m_color_B[i] * f);
694 aux_val.rgb[1] =
static_cast<uint8_t>(this->m_color_G[i] * f);
695 aux_val.rgb[2] =
static_cast<uint8_t>(this->m_color_R[i] * f);
697 cloud.points[i].rgb = aux_val.f;
700 return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
705 THROW_EXCEPTION(
"Operation not available: MRPT was built without PCL");
729 lric.
fVars.resize(4);
744 const float rel_z = gz - lric.
HM(2, 3);
747 float& pR = lric.
fVars[0];
748 float& pG = lric.
fVars[1];
749 float& pB = lric.
fVars[2];
750 const float& Az_1_color = lric.
fVars[3];
756 case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
757 case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
766 CColouredPointsMap::cmFromHeightRelativeToSensorGray)
776 case CColouredPointsMap::cmFromIntensityImage:
793 float& pR = lric.
fVars[0];
794 float& pG = lric.
fVars[1];
795 float& pB = lric.
fVars[2];
820 lric.
fVars.resize(9);
821 float& cx = lric.
fVars[5];
822 float& cy = lric.
fVars[6];
823 float& fx = lric.
fVars[7];
824 float& fy = lric.
fVars[8];
831 lric.
uVars.resize(4);
832 unsigned int& imgW = lric.
uVars[0];
833 unsigned int& imgH = lric.
uVars[1];
834 unsigned int& img_idx_x = lric.
uVars[2];
835 unsigned int& img_idx_y = lric.
uVars[3];
841 lric.
bVars.resize(3);
849 lric.
fVars[4] = 1.0f / 255;
851 hasValidIntensityImage =
false;
862 hasValidIntensityImage =
true;
868 hasColorIntensityImg =
877 simple_3d_to_color_relation =
899 float& pR = lric.
fVars[0];
900 float& pG = lric.
fVars[1];
901 float& pB = lric.
fVars[2];
902 float& Az_1_color = lric.
fVars[3];
903 float& K_8u = lric.
fVars[4];
904 float& cx = lric.
fVars[5];
905 float& cy = lric.
fVars[6];
906 float& fx = lric.
fVars[7];
907 float& fy = lric.
fVars[8];
909 unsigned int& imgW = lric.
uVars[0];
910 unsigned int& imgH = lric.
uVars[1];
911 unsigned int& img_idx_x = lric.
uVars[2];
912 unsigned int& img_idx_y = lric.
uVars[3];
916 const uint8_t& simple_3d_to_color_relation = lric.
bVars[2];
919 const float rel_z = gz - lric.
HM(2, 3);
925 case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
926 case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
935 CColouredPointsMap::cmFromHeightRelativeToSensorGray)
945 case CColouredPointsMap::cmFromIntensityImage:
948 bool hasValidColor =
false;
949 if (simple_3d_to_color_relation)
951 hasValidColor =
true;
965 img_idx_x = cx + fx * pt.
x / pt.
z;
966 img_idx_y = cy + fy * pt.
y / pt.
z;
968 hasValidColor = img_idx_x < imgW &&
976 if (hasValidColor && hasColorIntensityImg)
978 const auto*
c = ii.
ptr<
uint8_t>(img_idx_x, img_idx_y);
983 else if (hasValidColor && hasValidIntensityImage)
985 const auto c = ii.at<
uint8_t>(img_idx_x, img_idx_y);
986 pR = pG = pB =
c * K_8u;
1006 float& pR = lric.
fVars[0];
1007 float& pG = lric.
fVars[1];
1008 float& pB = lric.
fVars[2];
1025 unsigned int& imgW = lric.
uVars[0];
1026 unsigned int& img_idx_x = lric.
uVars[2];
1027 unsigned int& img_idx_y = lric.
uVars[3];
1030 const uint8_t& simple_3d_to_color_relation = lric.
bVars[2];
1034 if (simple_3d_to_color_relation && hasValidIntensityImage)
1036 if (++img_idx_x >= imgW)
1046 void CColouredPointsMap::loadFromRangeScan(
1054 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.
double x
X,Y,Z coordinates.
#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.
GLdouble GLdouble GLdouble GLdouble q
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).
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.
size_t getHeight() const override
Returns the height of the image in pixels.
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).
GLsizei GLsizei GLuint * obj
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.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
T square(const T x)
Inline function for the square of a number.
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).
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Lightweight 3D point (float version).
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.
Structure to hold the parameters of a pinhole camera model.
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
GLsizei const GLchar ** string
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.
#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).
bool hasIntensityImage
true means the field intensityImage contains valid data
A RGB 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...
GLsizei GLsizei GLchar * source
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.
unsigned __int32 uint32_t
size_t size() const
Returns the number of stored points in the map.
GLenum const GLfloat * params
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