Permalink
Browse files

New pointcloud map: CPointsMapXYZI; cleanup of pointclouds API

* Removal of map.copyFrom(): replaced by standard "= operator".
* Small refactoring/simplification of some methods in CPointsMap
* Add unit test for loadFromKittiVelodyneFile()
  • Loading branch information...
jlblancoc committed Nov 24, 2018
1 parent 0ed4003 commit 7dbb52e788eaf4632e9a6f39ac2c470519feb333
@@ -78,7 +78,9 @@ configuration files.
- This new module has been created to hold all serial devices &
networking classes, with minimal dependencies.
- \ref mrpt_maps_grp
- Added optional "channel" attribute to CReflectivityGrdMap2D and
- New map type: mrpt::maps::CPointsMapXYZI for pointclouds with an
intensity channel.
- Added optional "channel" attribute to CReflectivityGridMap2D and
CObservationReflectivity to support different colors of light.
- \ref mrpt_hwdrivers_grp
- COpenNI2Generic: is safer in multithreading apps.
@@ -136,7 +136,7 @@ void CDetectorDoorCrossing::process(
out_estimation.enoughtInformation = true;

out_estimation.informationGain = entropy.I - lastEntropy.I;
out_estimation.pointsMap.copyFrom(*auxMap.m_pointsMaps[0]);
out_estimation.pointsMap = *auxMap.m_pointsMaps[0];
}

// For next iterations:
@@ -50,3 +50,4 @@ if(BUILD_mrpt-maps)
endif()

endif()

@@ -30,12 +30,12 @@ MRPT_WARNING(
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/maps/CWeightedPointsMap.h>
#include <mrpt/maps/CPointsMapXYZI.h>
#include <mrpt/maps/COctoMap.h>
#include <mrpt/maps/CColouredOctoMap.h>

//#include <mrpt/maps/PCL_adapters.h> // NOTE: This file must be included from
// the user
// code only if he has already #include'd PCL headers.
// the user code only if he has already #include'd PCL headers.

#include <mrpt/opengl/CAngularObservationMesh.h>
#include <mrpt/opengl/CPlanarLaserScan.h>
@@ -31,6 +31,18 @@ class CColouredPointsMap : public CPointsMap
DEFINE_SERIALIZABLE(CColouredPointsMap)

public:
CColouredPointsMap() = default;

CColouredPointsMap(const CPointsMap& o) : CColouredPointsMap()
{
CPointsMap::operator=(o);
}
CColouredPointsMap operator=(const CPointsMap& o)
{
CPointsMap::operator=(o);
return *this;
}

// --------------------------------------------
/** @name Pure virtual interfaces to be implemented by any class derived
from CPointsMap
@@ -40,23 +52,10 @@ class CColouredPointsMap : public CPointsMap
void resize(size_t newLength) override; // See base class docs
void setSize(size_t newLength) override; // See base class docs

/** Changes the coordinates of the given point (0-based index), *without*
* checking for out-of-bounds and *without* calling mark_as_modified() \sa
* setPoint */
void setPointFast(size_t index, float x, float y, float z) override
{
m_x[index] = x;
m_y[index] = y;
m_z[index] = z;
}

/** The virtual method for \a insertPoint() *without* calling
* mark_as_modified() */
void insertPointFast(float x, float y, float z = 0) override;

/** Virtual assignment operator, to be implemented in derived classes */
void copyFrom(const CPointsMap& obj) override;

/** Get all the data fields for one point as a vector: [X Y Z R G B]
* Unlike getPointAllFields(), this method does not check for index out of
* bounds
@@ -101,8 +100,7 @@ class CColouredPointsMap : public CPointsMap
const mrpt::poses::CPose3D* robotPose = nullptr) override;

protected:
/** Auxiliary method called from within \a addFrom() automatically, to
* finish the copying of class-specific data */
void impl_copyFrom(const CPointsMap& obj) override;
void addFrom_classSpecific(
const CPointsMap& anotherMap, const size_t nPreviousPoints) override;

@@ -125,48 +123,14 @@ class CColouredPointsMap : public CPointsMap
/** Changes a given point from map. First index is 0.
* \exception Throws std::exception on index out of bound.
*/
void setPoint(
void setPointRGB(
size_t index, float x, float y, float z, float R, float G,
float B) override;

// The following overloads must be repeated here (from CPointsMap) due to
// the shadowing of the above "setPoint()"
/// \overload
inline void setPoint(size_t index, float x, float y, float z)
{
ASSERT_BELOW_(index, this->size());
setPointFast(index, x, y, z);
mark_as_modified();
}
/// \overload
inline void setPoint(size_t index, mrpt::math::TPoint3Df& p)
{
setPoint(index, p.x, p.y, p.z);
}
/// \overload
inline void setPoint(size_t index, float x, float y)
{
setPoint(index, x, y, 0);
}

/** Adds a new point given its coordinates and color (colors range is [0,1])
*/
void insertPoint(
void insertPointRGB(
float x, float y, float z, float R, float G, float B) override;
// The following overloads must be repeated here (from CPointsMap) due to
// the shadowing of the above "insertPoint()"
/// \overload
template <typename POINT_T>
inline void insertPoint(const POINT_T& p)
{
insertPoint(p[0], p[1], p[2]);
}
/// \overload
inline void insertPoint(float x, float y, float z)
{
insertPointFast(x, y, z);
mark_as_modified();
}

/** Changes just the color of a given point from the map. First index is 0.
* \exception Throws std::exception on index out of bound.
@@ -183,13 +147,10 @@ class CColouredPointsMap : public CPointsMap

/** Retrieves a point and its color (colors range is [0,1])
*/
void getPoint(
void getPointRGB(
size_t index, float& x, float& y, float& z, float& R, float& G,
float& B) const override;

/** Retrieves a point */
unsigned long getPoint(size_t index, float& x, float& y, float& z) const;

/** Retrieves a point color (colors range is [0,1]) */
void getPointColor(size_t index, float& R, float& G, float& B) const;

@@ -401,14 +362,14 @@ class PointCloudAdapter<mrpt::maps::CColouredPointsMap>
inline void getPointXYZ_RGBf(
const size_t idx, T& x, T& y, T& z, float& r, float& g, float& b) const
{
m_obj.getPoint(idx, x, y, z, r, g, b);
m_obj.getPointRGB(idx, x, y, z, r, g, b);
}
/** Set XYZ_RGBf coordinates of i'th point */
inline void setPointXYZ_RGBf(
const size_t idx, const coords_t x, const coords_t y, const coords_t z,
const float r, const float g, const float b)
{
m_obj.setPoint(idx, x, y, z, r, g, b);
m_obj.setPointRGB(idx, x, y, z, r, g, b);
}

/** Get XYZ_RGBu8 coordinates of i'th point */
@@ -418,7 +379,7 @@ class PointCloudAdapter<mrpt::maps::CColouredPointsMap>
uint8_t& b) const
{
float Rf, Gf, Bf;
m_obj.getPoint(idx, x, y, z, Rf, Gf, Bf);
m_obj.getPointRGB(idx, x, y, z, Rf, Gf, Bf);
r = Rf * 255;
g = Gf * 255;
b = Bf * 255;
@@ -428,7 +389,7 @@ class PointCloudAdapter<mrpt::maps::CColouredPointsMap>
const size_t idx, const coords_t x, const coords_t y, const coords_t z,
const uint8_t r, const uint8_t g, const uint8_t b)
{
m_obj.setPoint(idx, x, y, z, r / 255.f, g / 255.f, b / 255.f);
m_obj.setPointRGB(idx, x, y, z, r / 255.f, g / 255.f, b / 255.f);
}

/** Get RGBf color of i'th point */
@@ -116,6 +116,13 @@ class CPointsMap : public CMetricMap,
/** Virtual destructor. */
~CPointsMap() override;

CPointsMap& operator=(const CPointsMap& o)
{
this->impl_copyFrom(o);
return *this;
}
CPointsMap(const CPointsMap& o) : CPointsMap() { *this = o; }

// --------------------------------------------
/** @name Pure virtual interfaces to be implemented by any class derived
from CPointsMap
@@ -145,18 +152,19 @@ class CPointsMap : public CMetricMap,
virtual void setSize(size_t newLength) = 0;

/** Changes the coordinates of the given point (0-based index), *without*
* checking for out-of-bounds and *without* calling mark_as_modified() \sa
* setPoint */
virtual void setPointFast(size_t index, float x, float y, float z) = 0;
* checking for out-of-bounds and *without* calling mark_as_modified().
* Also, color, intensity, or other data is left unchanged. \sa setPoint */
inline void setPointFast(size_t index, float x, float y, float z)
{
m_x[index] = x;
m_y[index] = y;
m_z[index] = z;
}

/** The virtual method for \a insertPoint() *without* calling
* mark_as_modified() */
virtual void insertPointFast(float x, float y, float z = 0) = 0;

/** Virtual assignment operator, copies as much common data (XYZ, color,...)
* as possible from the source map into this one. */
virtual void copyFrom(const CPointsMap& obj) = 0;

/** Get all the data fields for one point as a vector: depending on the
* implementation class this can be [X Y Z] or [X Y Z R G B], etc...
* Unlike getPointAllFields(), this method does not check for index out of
@@ -176,6 +184,10 @@ class CPointsMap : public CMetricMap,
const size_t index, const std::vector<float>& point_data) = 0;

protected:
/** Virtual assignment operator, copies as much common data (XYZ, color,...)
* as possible from the source map into this one. */
virtual void impl_copyFrom(const CPointsMap& obj) = 0;

/** Auxiliary method called from within \a addFrom() automatically, to
* finish the copying of class-specific data */
virtual void addFrom_classSpecific(
@@ -407,27 +419,25 @@ class CPointsMap : public CMetricMap,
*/
inline size_t size() const { return m_x.size(); }
/** Access to a given point from map, as a 2D point. First index is 0.
* \return The return value is the weight of the point (the times it has
* been fused), or 1 if weights are not used.
* \exception Throws std::exception on index out of bound.
* \sa setPoint, getPointFast
*/
unsigned long getPoint(size_t index, float& x, float& y, float& z) const;
void getPoint(size_t index, float& x, float& y, float& z) const;
/// \overload
unsigned long getPoint(size_t index, float& x, float& y) const;
void getPoint(size_t index, float& x, float& y) const;
/// \overload
unsigned long getPoint(size_t index, double& x, double& y, double& z) const;
void getPoint(size_t index, double& x, double& y, double& z) const;
/// \overload
unsigned long getPoint(size_t index, double& x, double& y) const;
void getPoint(size_t index, double& x, double& y) const;
/// \overload
inline unsigned long getPoint(size_t index, mrpt::math::TPoint2D& p) const
inline void getPoint(size_t index, mrpt::math::TPoint2D& p) const
{
return getPoint(index, p.x, p.y);
getPoint(index, p.x, p.y);
}
/// \overload
inline unsigned long getPoint(size_t index, mrpt::math::TPoint3D& p) const
inline void getPoint(size_t index, mrpt::math::TPoint3D& p) const
{
return getPoint(index, p.x, p.y, p.z);
getPoint(index, p.x, p.y, p.z);
}

/** Access to a given point from map, and its colors, if the map defines
@@ -436,12 +446,12 @@ class CPointsMap : public CMetricMap,
* been fused)
* \exception Throws std::exception on index out of bound.
*/
virtual void getPoint(
virtual void getPointRGB(
size_t index, float& x, float& y, float& z, float& R, float& G,
float& B) const
{
getPoint(index, x, y, z);
R = G = B = 1;
R = G = B = 1.f;
}

/** Just like \a getPoint() but without checking out-of-bound index and
@@ -481,7 +491,7 @@ class CPointsMap : public CMetricMap,
setPoint(index, x, y, 0);
}
/// overload (RGB data is ignored in classes without color information)
virtual void setPoint(
virtual void setPointRGB(
size_t index, float x, float y, float z, float R, float G, float B)
{
MRPT_UNUSED_PARAM(R);
@@ -621,7 +631,7 @@ class CPointsMap : public CMetricMap,
insertPoint(p.x, p.y, p.z);
}
/// overload (RGB data is ignored in classes without color information)
virtual void insertPoint(
virtual void insertPointRGB(
float x, float y, float z, float R, float G, float B)
{
MRPT_UNUSED_PARAM(R);
Oops, something went wrong.

0 comments on commit 7dbb52e

Please sign in to comment.