Skip to content
Permalink
Browse files

first attempt at a generic rotating scanner

  • Loading branch information...
jlblancoc committed Jul 22, 2019
1 parent f2c2637 commit 58f1170f6a096c4078a07ce02c6aa5448b77e256
@@ -77,6 +77,8 @@ class CPointsMapXYZI : public CPointsMap
* \return true on success */
bool loadFromKittiVelodyneFile(const std::string& filename);

bool saveToKittiVelodyneFile(const std::string& filename) const;

/** See CPointsMap::loadFromRangeScan() */
void loadFromRangeScan(
const mrpt::obs::CObservation2DRangeScan& rangeScan,
@@ -108,6 +110,10 @@ class CPointsMapXYZI : public CPointsMap
*/
bool saveXYZI_to_text_file(const std::string& file) const;

/** Loads from a text file, each line having "X Y Z I", I in [0,1].
* Returns false if any error occured, true elsewere. */
bool loadXYZI_from_text_file(const std::string& file);

/** Changes a given point from map. First index is 0.
* \exception Throws std::exception on index out of bound.
*/
@@ -0,0 +1,181 @@
/* +------------------------------------------------------------------------+
| Mobile Robot Programming Toolkit (MRPT) |
| https://www.mrpt.org/ |
| |
| Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
| See: https://www.mrpt.org/Authors - All rights reserved. |
| Released under BSD License. See: https://www.mrpt.org/License |
+------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/CSinCosLookUpTableFor2DScans.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/serialization/CSerializable.h>
#include <variant>
#include <vector>

namespace mrpt::obs
{
class CObservationVelodyneScan;
class CObservation2DRangeScan;
class CObservationPointCloud;

/** \addtogroup mrpt_obs_grp
* @{ */

/** A `CObservation`-derived class for raw range data from a 2D or 3D
* rotating scanner. This class is the preferred alternative to
* CObservationVelodyneScan and CObservation2DRangeScan in MRPT 2.x, since it
* exposes range data as an organized matrix, more convenient for feature
* detection directly on "range images".
* This class can also import data from KITTI dataset-like binary files
* containing unorganized (non "undistorted", i.e. without compensation for
* lidar motion) point clouds, which get organized into a 2D range image for
* easier filtering and postprocessing.
*
* Check out the main data fields in the list of members below.
*
* Note that this object has \b two timestamp fields:
* - The standard `CObservation::timestamp` field in the base class, which
* should contain the accurate satellite-based UTC timestamp if available,
* and
* - the field originalReceivedTimestamp, with the
* local computer-based timestamp based on the reception of the message in
* the computer.
*
* Both timestamps correspond to the firing of the <b>first</b> laser in
* the <b>first</b> CObservationRotatingScan::scan_packets packet.
*
* <div align=center> <img src="velodyne_axes.jpg"> </div>
*
* API for accurate reconstruction of point clouds from raw range images:
* - generatePointCloud()
* - generatePointCloudAlongSE3Trajectory()
*
* \note New in MRPT 2.0.0
* \sa CObservation, mrpt::hwdrivers::CVelodyneScanner
*/
class CObservationRotatingScan : public CObservation
{
DEFINE_SERIALIZABLE(CObservationRotatingScan)

public:
/** @name Scan range data
@{ */

/** Number of "Lidar rings" (e.g. 16 for a Velodyne VLP16, etc.). This
* should be constant for a given LiDAR scanner.
* All matrices in `imageLayer_*` have this number of rows.
*/
uint16_t rowCount{0};

/** Number of lidar "firings" for this scan. It is assumed
* that firings occur at a fixed rate. Consecutive scans ("scan"=instance of
* this class) may have different number of firings, and different start and
* end azimuth. All matrices defined below have this number of columns.
*/
uint16_t columnCount{0};

/** The NxM matrix of distances (ranges) for each direction
* (columns) and for each laser "ring" (rows). Matrix element are integers
* for efficiency of post-processing filters, etc. Zero means no return
* (i.e. invalid range). This member must be always provided, containing the
* ranges for the STRONGEST ray returns.
*/
mrpt::math::CMatrix_u16 rangeImage{0, 0};

/** Optionally, an intensity channel. Matrix with a 0x0 size if not
* provided. */
mrpt::math::CMatrix_u8 intensityImage{0, 0};

/** Optional additional layers, e.g. LAST return, etc. for lidars with
* multiple returns per firing. A descriptive name of what the alternative
* range means as std::map Key, e.g. `FIRST`, `SECOND`. */
std::map<std::string, mrpt::math::CMatrix_u16> rangeOtherLayers;

/** Real-world scale (in meters) of integer units in range images (e.g.
* 0.002 means 1 range unit is 2 millimeters) */
double rangeResolution;

/** Azimuth of the first and last columns in `ranges`, with respect to the
* *sensor* forward direction. */
double startAzimuth{-M_PI}, azimuthSpan{2 * M_PI};

/** Time(in seconds) that passed since `startAzimuth` to* `endAzimuth`. */
double sweepDuration{.0};

/** The driver should fill in this observation */
std::string lidarModel{"UNKNOWN_SCANNER"};

/** The maximum range allowed by the device, in meters (e.g. 100m).
* Stored
* here by the driver while capturing based on the sensor model. */
double minRange{1.0}, maxRange{130.0};

/** The SE(3) pose of the sensor on the robot/vehicle frame
* of reference */
mrpt::poses::CPose3D sensorPose;

// TODO: Calibration!!

/** The local computer-based timestamp based on the
* reception of the message in the computer. \sa
* has_satellite_timestamp, CObservation::timestamp in the
* base class, which should contain the accurate
* satellite-based UTC timestamp. */
mrpt::system::TTimeStamp originalReceivedTimestamp{INVALID_TIMESTAMP};

/** If true, CObservation::timestamp has been generated
* from accurate satellite clock. Otherwise, no GPS data
* is available and timestamps are based on the local
* computer clock. */
bool has_satellite_timestamp{false};

/** @} */

/** @name "Convert from" API
* @{ */

void fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o);
void fromScan2D(const mrpt::obs::CObservation2DRangeScan& o);
void fromPointCloud(const mrpt::obs::CObservationPointCloud& o);

/** Will convert from another observation if it's any of the supported
* source types (see fromVelodyne(), fromScan2D(), fromPointCloud()) and
* return true, or will return false otherwise if there is no known way to
* convert from the passed object. */
bool fromGeneric(const mrpt::obs::CObservation& o);

/** @} */

/** @name "Convert to" API
* @{ */

/** @} */

// See base class docs
mrpt::system::TTimeStamp getOriginalReceivedTimeStamp() const override;

// See base class docs
void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
{
out_sensorPose = sensorPose;
}
void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
{
sensorPose = newSensorPose;
}
void getDescriptionAsText(std::ostream& o) const override;

}; // End of class def.

/** @} */

} // namespace mrpt::obs

namespace mrpt::typemeta
{ // Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(CObservationRotatingScan, ::mrpt::obs)
}
@@ -11,6 +11,7 @@

#include <mrpt/core/bits_mem.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileInputStream.h>
#include <mrpt/maps/CPointsMapXYZI.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
@@ -252,6 +253,40 @@ bool CPointsMapXYZI::saveXYZI_to_text_file(const std::string& file) const
return true;
}

bool CPointsMapXYZI::loadXYZI_from_text_file(const std::string& file)
{
MRPT_START

// Clear current map:
mark_as_modified();
this->clear();

std::ifstream f;
f.open(file);
if (!f.is_open()) return false;

while (!f.eof())
{
std::string line;
std::getline(f, line);

std::stringstream ss(line);

float x, y, z, i;
if (!(ss >> x >> y >> z >> i))
{
break;
}

insertPointFast(x, y, z);
m_intensity.push_back(i);
}

return true;

MRPT_END
}

/*---------------------------------------------------------------
addFrom_classSpecific
---------------------------------------------------------------*/
@@ -481,3 +516,25 @@ bool CPointsMapXYZI::loadFromKittiVelodyneFile(const std::string& filename)
return false;
}
}

bool CPointsMapXYZI::saveToKittiVelodyneFile(const std::string& filename) const
{
try
{
mrpt::io::CFileGZOutputStream f(filename);

for (size_t i = 0; i < m_x.size(); i++)
{
const float xyzi[4] = {m_x[i], m_y[i], m_z[i], m_intensity[i]};
const auto toWrite = sizeof(float) * 4;
std::size_t nWr = f.Write(&xyzi, toWrite);
ASSERT_EQUAL_(nWr, toWrite);
}
return true;
}
catch (const std::exception& e)
{
std::cerr << "[saveToKittiVelodyneFile] " << e.what() << std::endl;
return false;
}
}
@@ -10,6 +10,7 @@
#define MRPT_NO_WARN_BIG_HDR // Yes, we really want to include all classes.
#include <mrpt/maps.h>
#include <mrpt/obs/CObservationPointCloud.h>
#include <mrpt/obs/CObservationRotatingScan.h>

#include <CTraitsTest.h>
#include <gtest/gtest.h>
@@ -43,6 +44,7 @@ TEST_CLASS_MOVE_COPY_CTORS(COctoMap);
TEST_CLASS_MOVE_COPY_CTORS(CColouredOctoMap);
TEST_CLASS_MOVE_COPY_CTORS(CObservationPointCloud);
TEST_CLASS_MOVE_COPY_CTORS(CSinCosLookUpTableFor2DScans);
TEST_CLASS_MOVE_COPY_CTORS(CObservationRotatingScan);

// Create a set of classes, then serialize and deserialize to test possible
// bugs:
@@ -64,7 +66,8 @@ TEST(SerializeTestMaps, WriteReadToMem)
CLASS_ID(CPointsMapXYZI),
CLASS_ID(COctoMap),
CLASS_ID(CColouredOctoMap),
CLASS_ID(CObservationPointCloud)};
CLASS_ID(CObservationPointCloud),
CLASS_ID(CObservationRotatingScan)};

for (auto& lstClasse : lstClasses)
{

0 comments on commit 58f1170

Please sign in to comment.
You can’t perform that action at this time.