rtabmap/archive/82c0771fdfbb6de49665292f5f7a29c8620ee042.tar.gz

/*

Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke

All rights reserved.


Redistribution and use in source and binary forms, with or without

modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright

      notice, this list of conditions and the following disclaimer.

    * Redistributions in binary form must reproduce the above copyright

      notice, this list of conditions and the following disclaimer in the

      documentation and/or other materials provided with the distribution.

    * Neither the name of the Universite de Sherbrooke nor the

      names of its contributors may be used to endorse or promote products

      derived from this software without specific prior written permission.


THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND

ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY

DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/


#ifndef UTIL3D_H_

#define UTIL3D_H_


#include <functional>

#include "rtabmap/core/rtabmap_core_export.h"


#include <pcl/point_cloud.h>

#include <pcl/point_types.h>

#include <pcl/pcl_base.h>

#include <pcl/TextureMesh.h>

#include <rtabmap/core/Transform.h>

#include <rtabmap/core/SensorData.h>

#include <rtabmap/core/Parameters.h>

#include <opencv2/core/core.hpp>

#include <rtabmap/core/ProgressState.h>

#include <cstdint>

#include <map>

#include <list>


namespace rtabmap

{


// Point type carrying xyz + intensity + ring (laser line index) + time

// (per-point acquisition offset, seconds from the scan start). Matches the

// layout expected by LIO-SAM's Velodyne feature extractor so it can be fed

// directly via util3d::laserScanFromPointCloud().


struct EIGEN_ALIGN16 PointXYZIRT

{

  PCL_ADD_POINT4D;

  float intensity;

  std::uint16_t ring;

  float time;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};


namespace util3d

{


cv::Mat RTABMAP_CORE_EXPORT rgbFromCloud(

    const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,

    bool bgrOrder = true);


cv::Mat RTABMAP_CORE_EXPORT depthFromCloud(

    const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,

    bool depth16U = true);


void RTABMAP_CORE_EXPORT rgbdFromCloud(

    const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,

    cv::Mat & rgb,

    cv::Mat & depth,

    bool bgrOrder = true,

    bool depth16U = true);


pcl::PointXYZ RTABMAP_CORE_EXPORT projectDepthTo3D(

    const cv::Mat & depthImage,

    float x, float y,

    float cx, float cy,

    float fx, float fy,

    bool smoothing,

    float depthErrorRatio = 0.02f);


Eigen::Vector3f RTABMAP_CORE_EXPORT projectDepthTo3DRay(

    const cv::Size & imageSize,

    float x, float y,

    float cx, float cy,

    float fx, float fy);


RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(

    const cv::Mat & imageDepth,

    float cx, float cy,

    float fx, float fy,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0);

pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(

    const cv::Mat & imageDepth,

    const CameraModel & model,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0);

pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(

    const cv::Mat & imageDepth,

    const cv::Mat & imageDepthConfidence,

    const CameraModel & model,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    unsigned char confidenceThr = 0,

    std::vector<int> * validIndices = 0);


RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(

    const cv::Mat & imageRgb,

    const cv::Mat & imageDepth,

    float cx, float cy,

    float fx, float fy,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0);


pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(

    const cv::Mat & imageRgb,

    const cv::Mat & imageDepth,

    const CameraModel & model,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0);

pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(

  const cv::Mat & imageRgb,

  const cv::Mat & imageDepth,

  const cv::Mat & imageDepthConfidence,

  const CameraModel & model,

  int decimation = 1,

  float maxDepth = 0.0f,

  float minDepth = 0.0f,

  unsigned char confidenceThr = 0, // 0=low, 100=high

  std::vector<int> * validIndices = 0);


pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDisparity(

    const cv::Mat & imageDisparity,

    const StereoCameraModel & model,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0);


pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDisparityRGB(

    const cv::Mat & imageRgb,

    const cv::Mat & imageDisparity,

    const StereoCameraModel & model,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0);


pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages(

    const cv::Mat & imageLeft,

    const cv::Mat & imageRight,

    const StereoCameraModel & model,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0,

    const ParametersMap & parameters = ParametersMap());


std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RTABMAP_CORE_EXPORT cloudsFromSensorData(

    const SensorData & sensorData,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<pcl::IndicesPtr> * validIndices = 0,

    const ParametersMap & stereoParameters = ParametersMap(),

    const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo

    unsigned char confidenceThr = 0);                            // ignored for stereo


pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(

    const SensorData & sensorData,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0,

    const ParametersMap & stereoParameters = ParametersMap(),

    const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo

    unsigned char confidenceThr = 0);                                // ignored for stereo


std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(

    const SensorData & sensorData,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<pcl::IndicesPtr > * validIndices = 0,

    const ParametersMap & stereoParameters = ParametersMap(),

    const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo

    unsigned char confidenceThr = 0);                            // ignored for stereo


pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(

    const SensorData & sensorData,

    int decimation = 1,

    float maxDepth = 0.0f,

    float minDepth = 0.0f,

    std::vector<int> * validIndices = 0,

    const ParametersMap & stereoParameters = ParametersMap(),

    const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo

    unsigned char confidenceThr = 0);                            // ignored for stereo


pcl::PointCloud<pcl::PointXYZ> RTABMAP_CORE_EXPORT laserScanFromDepthImage(

          const cv::Mat & depthImage,

          float fx,

          float fy,

          float cx,

          float cy,

          float maxDepth = 0,

          float minDepth = 0,

          const Transform & localTransform = Transform::getIdentity());

pcl::PointCloud<pcl::PointXYZ> RTABMAP_CORE_EXPORT laserScanFromDepthImages(

    const cv::Mat & depthImages,

    const std::vector<CameraModel> & cameraModels,

    float maxDepth,

    float minDepth);


LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);


template<typename PointCloud2T>

LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs = true, bool is2D = false, const Transform & transform = Transform());

LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);

LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);

pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform());

pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform());

pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform = Transform());

pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);

pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);

pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);


pcl::PointXYZ RTABMAP_CORE_EXPORT laserScanToPoint(const LaserScan & laserScan, int index);

pcl::PointNormal RTABMAP_CORE_EXPORT laserScanToPointNormal(const LaserScan & laserScan, int index);

pcl::PointXYZRGB RTABMAP_CORE_EXPORT laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);

pcl::PointXYZI RTABMAP_CORE_EXPORT laserScanToPointI(const LaserScan & laserScan, int index, float intensity);

pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b);

pcl::PointXYZINormal RTABMAP_CORE_EXPORT laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity);


void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max);


void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max);


cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(

    const cv::Point2f & pt,

    float disparity,

    const StereoCameraModel & model);


cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(

    const cv::Point2f & pt,

    const cv::Mat & disparity,

    const StereoCameraModel & model);


cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(

    const cv::Size & imageSize,

    const cv::Mat & cameraMatrixK,

    const cv::Mat & laserScan,

    const rtabmap::Transform & cameraTransform);


cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(

    const cv::Size & imageSize,

    const cv::Mat & cameraMatrixK,

    const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan,

    const rtabmap::Transform & cameraTransform);


cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(

    const cv::Size & imageSize,

    const cv::Mat & cameraMatrixK,

    const pcl::PCLPointCloud2::Ptr laserScan,

    const rtabmap::Transform & cameraTransform);


void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(

    cv::Mat & depthRegistered,

    bool verticalDirection,

    bool fillToBorder);


cv::Mat RTABMAP_CORE_EXPORT filterFloor(

  const cv::Mat & depth,

  const std::vector<CameraModel> & cameraModels,

  float threshold,

  cv::Mat * depthBelow = 0);


std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT projectCloudToCameras (

    const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,

    const std::map<int, Transform> & cameraPoses,

    const std::map<int, std::vector<CameraModel> > & cameraModels,

    float maxDistance = 0.0f,

    float maxAngle = 0.0f,

    float maxDepthError = 0.0f,

    const std::vector<float> & roiRatios = std::vector<float>(),

    const cv::Mat & projMask = cv::Mat(),

    bool distanceToCamPolicy = false,

    const ProgressState * state = 0);

std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT projectCloudToCameras (

    const pcl::PointCloud<pcl::PointXYZINormal> & cloud,

    const std::map<int, Transform> & cameraPoses,

    const std::map<int, std::vector<CameraModel> > & cameraModels,

    float maxDistance = 0.0f,

    float maxAngle = 0.0f,

    float maxDepthError = 0.0f,

    const std::vector<float> & roiRatios = std::vector<float>(),

    const cv::Mat & projMask = cv::Mat(),

    bool distanceToCamPolicy = false,

    const ProgressState * state = 0);


bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f & pt);


pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT concatenateClouds(

    const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);

pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT concatenateClouds(

    const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);


pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(

    const std::vector<pcl::IndicesPtr> & indices);


pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(

    const pcl::IndicesPtr & indicesA,

    const pcl::IndicesPtr & indicesB);


void RTABMAP_CORE_EXPORT savePCDWords(

    const std::string & fileName,

    const std::multimap<int, pcl::PointXYZ> & words,

    const Transform & transform = Transform::getIdentity());


void RTABMAP_CORE_EXPORT savePCDWords(

    const std::string & fileName,

    const std::multimap<int, cv::Point3f> & words,

    const Transform & transform = Transform::getIdentity());


cv::Mat RTABMAP_CORE_EXPORT loadBINScan(const std::string & fileName);

pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string & fileName);

RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string & fileName, int dim);


LaserScan RTABMAP_CORE_EXPORT loadScan(const std::string & path);


RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadCloud(

    const std::string & path,

    const Transform & transform = Transform::getIdentity(),

    int downsampleStep = 1,

    float voxelSize = 0.0f);


LaserScan RTABMAP_CORE_EXPORT deskew(

    const LaserScan & input,

    double inputStamp,

    const rtabmap::Transform & velocity);


LaserScan RTABMAP_CORE_EXPORT deskew(

    const LaserScan & input,

    double inputStamp,

    const std::function<rtabmap::Transform(double stamp)> & motion,

    bool slerp = false);


} // namespace util3d

} // namespace rtabmap


POINT_CLOUD_REGISTER_POINT_STRUCT(rtabmap::PointXYZIRT,

  (float, x, x)

  (float, y, y)

  (float, z, z)

  (float, intensity, intensity)

  (std::uint16_t, ring, ring)

  (float, time, time)

)


#include "rtabmap/core/impl/util3d.hpp"


#endif /* UTIL3D_H_ */

rtabmap::CameraModel
Represents a pinhole camera model containing intrinsic and extrinsic parameters, used for projection,...
Definition CameraModel.h:53

rtabmap::LaserScan
Represents 2D or 3D laser scan data with support for multiple point data formats.
Definition LaserScan.h:46

rtabmap::ProgressState
Definition ProgressState.h:36

rtabmap::SensorData
Container class for all sensor data captured at a specific time.
Definition SensorData.h:97

rtabmap::StereoCameraModel
A class representing a calibrated stereo camera system.
Definition StereoCameraModel.h:54

rtabmap::Transform
Represents a 3D rigid body transformation (rotation + translation).
Definition Transform.h:53

rtabmap::Transform::getIdentity
static Transform getIdentity()
Returns identity transform.

rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
PointXYZ → LaserScan::kXY

rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Convert pcl::PCLPointCloud2 to rtabmap::LaserScan with all supported fields (see rtabmap::LaserScan::...
Definition util3d.hpp:37

rtabmap::util3d::laserScanToPointI
pcl::PointXYZI RTABMAP_CORE_EXPORT laserScanToPointI(const LaserScan &laserScan, int index, float intensity)
The point at index of the scan, as PointXYZI.

rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Convert rtabmap::LaserScan to pcl::PCLPointCloud2 with all supported fields (see rtabmap::LaserScan::...

rtabmap::util3d::laserScanToPointNormal
pcl::PointNormal RTABMAP_CORE_EXPORT laserScanToPointNormal(const LaserScan &laserScan, int index)
The point at index of the scan, as PointNormal.

rtabmap::util3d::laserScanToPointRGBNormal
pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT laserScanToPointRGBNormal(const LaserScan &laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
The point at index of the scan, as PointXYZRGBNormal.

rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
LaserScan → PointXYZI (x, y, z, intensity); intensity is used if the scan has none.

rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
LaserScan → PointXYZRGBNormal (x, y, z, rgb, nx, ny, nz); missing color and normals are filled as abo...

rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
LaserScan → PointXYZINormal (x, y, z, intensity, nx, ny, nz); missing intensity and normals are fille...

rtabmap::util3d::laserScanToPointINormal
pcl::PointXYZINormal RTABMAP_CORE_EXPORT laserScanToPointINormal(const LaserScan &laserScan, int index, float intensity)
The point at index of the scan, as PointXYZINormal.

rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
LaserScan → PointXYZ (x, y, z); any other field of the scan is dropped.

rtabmap::util3d::laserScanToPointRGB
pcl::PointXYZRGB RTABMAP_CORE_EXPORT laserScanToPointRGB(const LaserScan &laserScan, int index, unsigned char r=100, unsigned char g=100, unsigned char b=100)
The point at index of the scan, as PointXYZRGB.

rtabmap::util3d::laserScanToPoint
pcl::PointXYZ RTABMAP_CORE_EXPORT laserScanToPoint(const LaserScan &laserScan, int index)
The point at index of the scan, as PointXYZ.

rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
LaserScan → PointXYZRGB (x, y, z, rgb); r, g and b are used if the scan has no color.

rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
LaserScan → PointNormal (x, y, z, nx, ny, nz); normals are zeroed if the scan has none.

rtabmap::util3d::laserScanFromDepthImages
pcl::PointCloud< pcl::PointXYZ > RTABMAP_CORE_EXPORT laserScanFromDepthImages(const cv::Mat &depthImages, const std::vector< CameraModel > &cameraModels, float maxDepth, float minDepth)
Converts multiple depth images (e.g., from a stereo or multi-camera setup) into a single laser scan (...

rtabmap::util3d::cloudFromDisparityRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromDisparityRGB(const cv::Mat &imageRgb, const cv::Mat &imageDisparity, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Converts a disparity image and an RGB image to a 3D point cloud with color.

rtabmap::util3d::cloudsRGBFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >(), unsigned char confidenceThr=0)
Generates a point cloud with RGB color data from sensor data.

rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >(), unsigned char confidenceThr=0)
Generates a point cloud of type pcl::PointXYZRGB from sensor data.

rtabmap::util3d::rgbFromCloud
cv::Mat RTABMAP_CORE_EXPORT rgbFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, bool bgrOrder=true)
Converts a PCL point cloud with RGBA information to an OpenCV RGB or BGR image.

rtabmap::util3d::laserScanFromDepthImage
pcl::PointCloud< pcl::PointXYZ > RTABMAP_CORE_EXPORT laserScanFromDepthImage(const cv::Mat &depthImage, float fx, float fy, float cx, float cy, float maxDepth=0, float minDepth=0, const Transform &localTransform=Transform::getIdentity())
Converts the middle row of a depth image into a laser scan (point cloud) using camera intrinsics and ...

rtabmap::util3d::cloudFromStereoImages
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages(const cv::Mat &imageLeft, const cv::Mat &imageRight, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &parameters=ParametersMap())
Converts a pair of stereo images (left and right) into a 3D point cloud with RGB color information.

rtabmap::util3d::projectDisparityTo3D
cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
Projects a 2D point from the left image and its disparity into 3D space.

rtabmap::util3d::cloudFromDepth
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Converts a depth image to a 3D point cloud.

rtabmap::util3d::projectDepthTo3D
pcl::PointXYZ RTABMAP_CORE_EXPORT projectDepthTo3D(const cv::Mat &depthImage, float x, float y, float cx, float cy, float fx, float fy, bool smoothing, float depthErrorRatio=0.02f)
Projects a single depth pixel into 3D space.

rtabmap::util3d::savePCDWords
void RTABMAP_CORE_EXPORT savePCDWords(const std::string &fileName, const std::multimap< int, pcl::PointXYZ > &words, const Transform &transform=Transform::getIdentity())
Saves 3D word points to a PCD file, applying a transform to each point.

rtabmap::util3d::rgbdFromCloud
void RTABMAP_CORE_EXPORT rgbdFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, cv::Mat &rgb, cv::Mat &depth, bool bgrOrder=true, bool depth16U=true)
Converts a PCL point cloud (with RGBA colors) into aligned RGB and depth OpenCV images.

rtabmap::util3d::loadBINScan
cv::Mat RTABMAP_CORE_EXPORT loadBINScan(const std::string &fileName)
Loads a KITTI-style Velodyne binary scan file into an OpenCV matrix.

rtabmap::util3d::loadBINCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string &fileName)
Loads a KITTI-style Velodyne binary scan and converts it to a PCL point cloud.

rtabmap::util3d::projectDepthTo3DRay
Eigen::Vector3f RTABMAP_CORE_EXPORT projectDepthTo3DRay(const cv::Size &imageSize, float x, float y, float cx, float cy, float fx, float fy)
Projects pixel coordinates to a normalized 3D ray in camera coordinates.

rtabmap::util3d::projectCloudToCamera
cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
Register a point cloud (laser scan) to the camera's frame of reference and return a registered depth ...

rtabmap::util3d::concatenateClouds
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT concatenateClouds(const std::list< pcl::PointCloud< pcl::PointXYZ >::Ptr > &clouds)
Concatenates a list of PointXYZ point clouds into a single point cloud.

rtabmap::util3d::loadCloud
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT loadCloud(const std::string &path, const Transform &transform=Transform::getIdentity(), int downsampleStep=1, float voxelSize=0.0f)
Loads and optionally transforms/downsamples/voxelizes a point cloud.

rtabmap::util3d::cloudFromDepthRGB
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(const cv::Mat &imageRgb, const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Creates a point cloud from an RGB image and a depth image.

rtabmap::util3d::fillProjectedCloudHoles
void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(cv::Mat &depthRegistered, bool verticalDirection, bool fillToBorder)
Fills holes (missing depth values) in a depth image by interpolating between non-zero values.

rtabmap::util3d::projectCloudToCameras
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > > RTABMAP_CORE_EXPORT projectCloudToCameras(const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance=0.0f, float maxAngle=0.0f, float maxDepthError=0.0f, const std::vector< float > &roiRatios=std::vector< float >(), const cv::Mat &projMask=cv::Mat(), bool distanceToCamPolicy=false, const ProgressState *state=0)
Projects a 3D point cloud to the best camera (NodeID -> CameraIndex) for each point based on a policy...

rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >(), unsigned char confidenceThr=0)
Generates a point cloud from sensor data.

rtabmap::util3d::depthFromCloud
cv::Mat RTABMAP_CORE_EXPORT depthFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, bool depth16U=true)
Generates a depth image from a PCL organized point cloud.

rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Checks if all coordinates of a 3D point are finite.

rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Computes the minimum and maximum 3D points from a laser scan matrix.

rtabmap::util3d::cloudsFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr > RTABMAP_CORE_EXPORT cloudsFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >(), unsigned char confidenceThr=0)
Generates a set of point clouds from sensor data.

rtabmap::util3d::filterFloor
cv::Mat RTABMAP_CORE_EXPORT filterFloor(const cv::Mat &depth, const std::vector< CameraModel > &cameraModels, float threshold, cv::Mat *depthBelow=0)
Filters out points below a certain threshold in a depth image based on camera models.

rtabmap::util3d::deskew
LaserScan RTABMAP_CORE_EXPORT deskew(const LaserScan &input, double inputStamp, const rtabmap::Transform &velocity)
Lidar deskewing.

rtabmap::util3d::cloudFromDisparity
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromDisparity(const cv::Mat &imageDisparity, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Converts a disparity image to a 3D point cloud.

rtabmap::util3d::loadScan
LaserScan RTABMAP_CORE_EXPORT loadScan(const std::string &path)
Loads a 3D scan from a file (.pcd, .ply, or .bin format).

rtabmap::util3d::concatenate
pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(const std::vector< pcl::IndicesPtr > &indices)
Concatenates multiple sets of indices into a single index vector.

rtabmap
Definition BayesFilter.h:42

rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Parameter keys mapped to their values, as used by every configurable class (see Parameters).
Definition Parameters.h:44

rtabmap::PointXYZIRT
This namespace contains 3D point cloud processing utilities.
Definition util3d.h:59