i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

LiDAR capture support in standalone library (#1264)

Browse Source 
* Working rtabmap_lidar-mapping example (live and pcap)

* finalizing merge, added some deprecated

* fixed build

* Working deskewing for Lidar + Camera/IMU (no camera pose correction yet) and Lidar + Odom Sensor in main UI.

* backward compatibility

* fixed some not used variable warnings, fixed qt build for lidar mapping example

* Refactored CameraMobile, added AREngine background support, fixed LidarVPL16 build error with PCL 1.8

* ARCoreJava: buffer last depth image in case its stamp i higher than pose stamp. CameraMobile: added pose buffer. SensorCaptureThread: to get pose, odomSensor should be explicitly set, but can be same as lidar or camera  inputs.

* Working external lidar on iOS

* util3d::commonFiltering()/adjustNormalsToViewPoint() added organized cloud support. MainWindow: updated odomSensor setup

* fixed winsock include order

* reverted camera tool

* disable imu filtering when odom sensor is used

* Updated package version

* fixed windows build

* fixing more windows build erros
This commit is contained in:
matlabbe
2024-04-14 19:06:04 -07:00
committed by GitHub
parent 6a6913c939
commit 700704bec9
131 changed files with 10585 additions and 7476 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
corelib/include/rtabmap/core/Camera.h
View File

@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -28,47 +28,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/CameraInfo.h"
#include <set>
#include <stack>
#include <list>
#include <vector>
class UDirectory;
class UTimer;
#include <rtabmap/core/SensorCapture.h>
#include <rtabmap/core/IMU.h>
namespace rtabmap
{
class IMUFilter;
/**
* Class Camera
*
*/
class RTABMAP_CORE_EXPORT Camera
class RTABMAP_CORE_EXPORT Camera : public SensorCapture
{
public:
virtual ~Camera();
SensorData takeImage(CameraInfo * info = 0);
SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);}
float getImageRate() const {return getFrameRate();}
void setImageRate(float imageRate) {setFrameRate(imageRate);}
void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter
bool isInterIMUPublishing() const {return publishInterIMU_;}
bool initFromFile(const std::string & calibrationPath);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
virtual bool isCalibrated() const = 0;
virtual std::string getSerial() const = 0;
virtual bool odomProvided() const { return false; }
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; }
//getters
float getImageRate() const {return _imageRate;}
const Transform & getLocalTransform() const {return _localTransform;}
//setters
void setImageRate(float imageRate) {_imageRate = imageRate;}
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
void resetTimer();
protected:
/**
* Constructor
@@ -78,19 +63,16 @@ protected:
*/
Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
/**
* returned rgb and depth images should be already rectified if calibration was loaded
*/
virtual SensorData captureImage(CameraInfo * info = 0) = 0;
virtual SensorData captureImage(SensorCaptureInfo * info = 0) = 0;
int getNextSeqID() {return ++_seq;}
void postInterIMU(const IMU & imu, double stamp);
private:
float _imageRate;
Transform _localTransform;
cv::Size _targetImageSize;
UTimer * _frameRateTimer;
int _seq;
virtual SensorData captureData(SensorCaptureInfo * info = 0) {return captureImage(info);}
private:
IMUFilter * imuFilter_;
bool publishInterIMU_;
};

63
corelib/include/rtabmap/core/CameraEvent.h
View File

@@ -27,65 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include <rtabmap/utilite/UEvent.h>
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/CameraInfo.h"
namespace rtabmap
{
class CameraEvent :
public UEvent
{
public:
enum Code {
kCodeData,
kCodeNoMoreImages
};
public:
CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
UEvent(kCodeData),
data_(image, seq, stamp)
{
cameraInfo_.cameraName = cameraName;
}
CameraEvent() :
UEvent(kCodeNoMoreImages)
{
}
CameraEvent(const SensorData & data) :
UEvent(kCodeData),
data_(data)
{
}
CameraEvent(const SensorData & data, const std::string & cameraName) :
UEvent(kCodeData),
data_(data)
{
cameraInfo_.cameraName = cameraName;
}
CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) :
UEvent(kCodeData),
data_(data),
cameraInfo_(cameraInfo)
{
}
// Image or descriptors
const SensorData & data() const {return data_;}
const std::string & cameraName() const {return cameraInfo_.cameraName;}
const CameraInfo & info() const {return cameraInfo_;}
virtual ~CameraEvent() {}
virtual std::string getClassName() const {return std::string("CameraEvent");}
private:
SensorData data_;
CameraInfo cameraInfo_;
};
} // namespace rtabmap
#include "rtabmap/core/SensorEvent.h"

48
corelib/include/rtabmap/core/CameraInfo.h
View File

@@ -27,50 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include <string>
namespace rtabmap
{
class CameraInfo
{
public:
CameraInfo() :
cameraName(""),
id(0),
stamp(0.0),
timeCapture(0.0f),
timeDisparity(0.0f),
timeMirroring(0.0f),
timeStereoExposureCompensation(0.0f),
timeImageDecimation(0.0f),
timeHistogramEqualization(0.0f),
timeScanFromDepth(0.0f),
timeUndistortDepth(0.0f),
timeBilateralFiltering(0.0f),
timeTotal(0.0f),
odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
{
}
virtual ~CameraInfo() {}
std::string cameraName;
int id;
double stamp;
float timeCapture;
float timeDisparity;
float timeMirroring;
float timeStereoExposureCompensation;
float timeImageDecimation;
float timeHistogramEqualization;
float timeScanFromDepth;
float timeUndistortDepth;
float timeBilateralFiltering;
float timeTotal;
Transform odomPose;
cv::Mat odomCovariance;
std::vector<float> odomVelocity;
};
} // namespace rtabmap
#include "rtabmap/core/SensorCaptureInfo.h"

134
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -27,136 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsSender.h>
namespace clams
{
class DiscreteDepthDistortionModel;
}
namespace rtabmap
{
class Camera;
class CameraInfo;
class SensorData;
class StereoDense;
class IMUFilter;
class Feature2D;
/**
* Class CameraThread
*
*/
class RTABMAP_CORE_EXPORT CameraThread :
public UThread,
public UEventsSender
{
public:
// ownership transferred
CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap());
/**
* @param camera the camera to take images from
* @param odomSensor an odometry sensor to get a pose
* @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame
*/
CameraThread(Camera * camera,
Camera * odomSensor,
const Transform & extrinsics,
double poseTimeOffset = 0.0,
float poseScaleFactor = 1.0f,
bool odomAsGt = false,
const ParametersMap & parameters = ParametersMap());
CameraThread(Camera * camera,
bool odomAsGt,
const ParametersMap & parameters = ParametersMap());
virtual ~CameraThread();
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setImageDecimation(int decimation) {_imageDecimation = decimation;}
void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
void setImageRate(float imageRate);
void setDistortionModel(const std::string & path);
void enableBilateralFiltering(float sigmaS, float sigmaR);
void disableBilateralFiltering() {_bilateralFiltering = false;}
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
void disableIMUFiltering();
void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
void disableFeatureDetection();
// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
RTABMAP_DEPRECATED void setScanParameters(
bool fromDepth,
int downsampleStep, // decimation of the depth image in case the scan is from depth image
float rangeMin,
float rangeMax,
float voxelSize,
int normalsK,
int normalsRadius,
bool forceGroundNormalsUp);
void setScanParameters(
bool fromDepth,
int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
float rangeMin=0.0f,
float rangeMax=0.0f,
float voxelSize = 0.0f,
int normalsK = 0,
int normalsRadius = 0.0f,
float groundNormalsUp = 0.0f);
void postUpdate(SensorData * data, CameraInfo * info = 0) const;
//getters
bool isPaused() const {return !this->isRunning();}
bool isCapturing() const {return this->isRunning();}
bool odomProvided() const;
Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
Camera * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
private:
virtual void mainLoopBegin();
virtual void mainLoop();
virtual void mainLoopKill();
private:
Camera * _camera;
Camera * _odomSensor;
Transform _extrinsicsOdomToCamera;
bool _odomAsGt;
double _poseTimeOffset;
float _poseScaleFactor;
bool _mirroring;
bool _stereoExposureCompensation;
bool _colorOnly;
int _imageDecimation;
int _histogramMethod;
bool _stereoToDepth;
bool _scanFromDepth;
int _scanDownsampleStep;
float _scanRangeMin;
float _scanRangeMax;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
float _scanForceGroundNormalsUp;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
bool _bilateralFiltering;
float _bilateralSigmaS;
float _bilateralSigmaR;
IMUFilter * _imuFilter;
bool _imuBaseFrameConversion;
Feature2D * _featureDetector;
bool _depthAsMask;
};
} // namespace rtabmap
#include "rtabmap/core/SensorCaptureThread.h"

4
corelib/include/rtabmap/core/DBReader.h
View File

@@ -86,10 +86,10 @@ public:
const DBDriver * driver() const {return _dbDriver;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
SensorData getNextData(CameraInfo * info = 0);
SensorData getNextData(SensorCaptureInfo * info = 0);
private:
std::list<std::string> _paths;

3
corelib/include/rtabmap/core/IMUFilter.h
View File

@@ -28,12 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <Eigen/Geometry>
namespace rtabmap {
class IMUFilter
class RTABMAP_CORE_EXPORT IMUFilter
{
public:
enum Type {

11
corelib/include/rtabmap/core/LaserScan.h
View File

@@ -47,7 +47,8 @@ public:
kXYZRGB=7,
kXYZNormal=8,
kXYZINormal=9,
kXYZRGBNormal=10};
kXYZRGBNormal=10,
kXYZIT=11};
static std::string formatName(const Format & format);
static int channels(const Format & format);
@@ -55,6 +56,7 @@ public:
static bool isScanHasNormals(const Format & format);
static bool isScanHasRGB(const Format & format);
static bool isScanHasIntensity(const Format & format);
static bool isScanHasTime(const Format & format);
static LaserScan backwardCompatibility(
const cv::Mat & oldScanFormat,
int maxPoints = 0,
@@ -121,22 +123,27 @@ public:
float angleMin() const {return angleMin_;}
float angleMax() const {return angleMax_;}
float angleIncrement() const {return angleIncrement_;}
void setLocalTransform(const Transform & t) {localTransform_ = t;}
Transform localTransform() const {return localTransform_;}
bool empty() const {return data_.empty();}
bool isEmpty() const {return data_.empty();}
int size() const {return data_.cols;}
int size() const {return data_.total();}
int dataType() const {return data_.type();}
bool is2d() const {return isScan2d(format_);}
bool hasNormals() const {return isScanHasNormals(format_);}
bool hasRGB() const {return isScanHasRGB(format_);}
bool hasIntensity() const {return isScanHasIntensity(format_);}
bool hasTime() const {return isScanHasTime(format_);}
bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
bool isOrganized() const {return data_.rows > 1;}
LaserScan clone() const;
LaserScan densify() const;
int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
int getTimeOffset() const {return hasTime()?4:-1;}
float & field(unsigned int pointIndex, unsigned int channelOffset);

57
corelib/include/rtabmap/core/Lidar.h Normal file
View File

@@ -0,0 +1,57 @@
/*
Copyright (c) 2010-2022, 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.
*/
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <rtabmap/core/SensorCapture.h>
namespace rtabmap
{
/**
* Class Lidar
*
*/
class RTABMAP_CORE_EXPORT Lidar : public SensorCapture
{
public:
virtual ~Lidar() {}
protected:
/**
* Constructor
*
* @param lidarRate the frame rate (Hz), 0 for fast as the lidar can
* @param localTransform the transform from base frame to lidar frame
*/
Lidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) :
SensorCapture(lidarRate, localTransform) {}
};
} // namespace rtabmap

1
corelib/include/rtabmap/core/Odometry.h
View File

@@ -111,6 +111,7 @@ private:
bool _alignWithGround;
bool _publishRAMUsage;
bool _imagesAlreadyRectified;
bool _deskewing;
Transform _pose;
int _resetCurrentCount;
double previousStamp_;

3
corelib/include/rtabmap/core/OdometryInfo.h
View File

@@ -50,6 +50,7 @@ public:
localBundleConstraints(0),
localBundleTime(0),
keyFrameAdded(false),
timeDeskewing(0.0f),
timeEstimation(0.0f),
timeParticleFiltering(0.0f),
stamp(0),
@@ -76,6 +77,7 @@ public:
output.localBundlePoses = localBundlePoses;
output.localBundleModels = localBundleModels;
output.keyFrameAdded = keyFrameAdded;
output.timeDeskewing = timeDeskewing;
output.timeEstimation = timeEstimation;
output.timeParticleFiltering = timeParticleFiltering;
output.stamp = stamp;
@@ -105,6 +107,7 @@ public:
std::map<int, Transform> localBundlePoses;
std::map<int, std::vector<CameraModel> > localBundleModels;
bool keyFrameAdded;
float timeDeskewing;
float timeEstimation;
float timeParticleFiltering;
double stamp;

3
corelib/include/rtabmap/core/Parameters.h
View File

@@ -463,6 +463,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ImageDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization.");
RTABMAP_PARAM(Odom, Deskewing, bool, true, "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).");
// Odometry Frame-to-Map
RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
@@ -752,6 +753,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.1, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(Icp, Force4DoF, bool, false, uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));
RTABMAP_PARAM(Icp, FiltersEnabled, int, 3, "Flag to enable filters: 1=\"from\" cloud only, 2=\"to\" cloud only, 3=both.");
#ifdef RTABMAP_POINTMATCHER
RTABMAP_PARAM(Icp, PointToPlane, bool, true, "Use point to plane ICP.");
#else
@@ -931,6 +933,7 @@ public:
static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);
static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
static void readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly = false);
static void writeINI(const std::string & configFile, const ParametersMap & parameters);
/**

1
corelib/include/rtabmap/core/RegistrationIcp.h
View File

@@ -69,6 +69,7 @@ private:
float _epsilon;
float _correspondenceRatio;
bool _force4DoF;
int _filtersEnabled;
bool _pointToPlane;
int _pointToPlaneK;
float _pointToPlaneRadius;

93
corelib/include/rtabmap/core/SensorCapture.h Normal file
View File

@@ -0,0 +1,93 @@
/*
Copyright (c) 2010-2022, Mathieu Labbe
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 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.
*/
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/core/SensorCaptureInfo.h>
#include "rtabmap/core/SensorData.h"
#include <set>
#include <stack>
#include <list>
#include <vector>
class UDirectory;
class UTimer;
namespace rtabmap
{
/**
* Class Camera
*
*/
class RTABMAP_CORE_EXPORT SensorCapture
{
public:
virtual ~SensorCapture();
SensorData takeData(SensorCaptureInfo * info = 0);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
virtual std::string getSerial() const = 0;
virtual bool odomProvided() const { return false; }
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; }
//getters
float getFrameRate() const {return _frameRate;}
const Transform & getLocalTransform() const {return _localTransform;}
//setters
void setFrameRate(float frameRate) {_frameRate = frameRate;}
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
void resetTimer();
protected:
/**
* Constructor
*
* @param frameRate the frame rate (Hz), 0 for fast as the sensor can
* @param localTransform the transform from base frame to sensor frame
*/
SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity());
/**
* returned rgb and depth images should be already rectified if calibration was loaded
*/
virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0;
int getNextSeqID() {return ++_seq;}
private:
float _frameRate;
Transform _localTransform;
UTimer * _frameRateTimer;
int _seq;
};
} // namespace rtabmap

82
corelib/include/rtabmap/core/SensorCaptureInfo.h Normal file
View File

@@ -0,0 +1,82 @@
/*
Copyright (c) 2010-2016, 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.
*/
#pragma once
#include "rtabmap/core/Transform.h"
#include <string>
namespace rtabmap
{
class SensorCaptureInfo
{
public:
SensorCaptureInfo() :
cameraName(""),
id(0),
stamp(0.0),
timeCapture(0.0f),
timeDeskewing(0.0f),
timeDisparity(0.0f),
timeMirroring(0.0f),
timeStereoExposureCompensation(0.0f),
timeImageDecimation(0.0f),
timeHistogramEqualization(0.0f),
timeScanFromDepth(0.0f),
timeUndistortDepth(0.0f),
timeBilateralFiltering(0.0f),
timeTotal(0.0f),
odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
{
}
virtual ~SensorCaptureInfo() {}
std::string cameraName;
int id;
double stamp;
float timeCapture;
float timeDeskewing;
float timeDisparity;
float timeMirroring;
float timeStereoExposureCompensation;
float timeImageDecimation;
float timeHistogramEqualization;
float timeScanFromDepth;
float timeUndistortDepth;
float timeBilateralFiltering;
float timeTotal;
Transform odomPose;
cv::Mat odomCovariance;
std::vector<float> odomVelocity;
};
//backward compatibility
RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo;
} // namespace rtabmap

216
corelib/include/rtabmap/core/SensorCaptureThread.h Normal file
View File

@@ -0,0 +1,216 @@
/*
Copyright (c) 2010-2016, 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.
*/
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsSender.h>
namespace clams
{
class DiscreteDepthDistortionModel;
}
namespace rtabmap
{
class Camera;
class Lidar;
class SensorCapture;
class SensorCaptureInfo;
class SensorData;
class StereoDense;
class IMUFilter;
class Feature2D;
/**
* Class CameraThread
*
*/
class RTABMAP_CORE_EXPORT SensorCaptureThread :
public UThread,
public UEventsSender
{
public:
// ownership transferred
SensorCaptureThread(
Camera * camera,
const ParametersMap & parameters = ParametersMap());
/**
* @param camera the camera to take images from
* @param odomSensor an odometry sensor to get a pose (can be again the camera)
* @param odomAsGt set odometry sensor pose as ground truth instead of odometry
* @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame (without optical rotation)
*/
SensorCaptureThread(
Camera * camera,
SensorCapture * odomSensor,
const Transform & extrinsics,
double poseTimeOffset = 0.0,
float poseScaleFactor = 1.0f,
double poseWaitTime = 0.1,
const ParametersMap & parameters = ParametersMap());
/**
* @param lidar the lidar to take scans from
*/
SensorCaptureThread(
Lidar * lidar,
const ParametersMap & parameters = ParametersMap());
/**
* @param lidar the lidar to take scans from
* @param camera the camera to take images from. If the camera is providing a pose, it can be used for deskewing
*/
SensorCaptureThread(
Lidar * lidar,
Camera * camera,
const ParametersMap & parameters = ParametersMap());
/**
* @param lidar the lidar to take scans from
* @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the lidar)
*/
SensorCaptureThread(
Lidar * lidar,
SensorCapture * odomSensor,
double poseTimeOffset = 0.0,
float poseScaleFactor = 1.0f,
double poseWaitTime = 0.1,
const ParametersMap & parameters = ParametersMap());
/**
* @param lidar the lidar to take scans from
* @param camera the camera to take images from
* @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the camera or lidar)
* @param extrinsics the static transform between odometry frame to camera frame (without optical rotation)
*/
SensorCaptureThread(
Lidar * lidar,
Camera * camera,
SensorCapture * odomSensor,
const Transform & extrinsics,
double poseTimeOffset = 0.0,
float poseScaleFactor = 1.0f,
double poseWaitTime = 0.1,
const ParametersMap & parameters = ParametersMap());
virtual ~SensorCaptureThread();
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setImageDecimation(int decimation) {_imageDecimation = decimation;}
void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
void setFrameRate(float frameRate);
RTABMAP_DEPRECATED void setImageRate(float frameRate) {setFrameRate(frameRate);}
void setDistortionModel(const std::string & path);
void setOdomAsGroundTruth(bool enabled) {_odomAsGt = enabled;}
void enableBilateralFiltering(float sigmaS, float sigmaR);
void disableBilateralFiltering() {_bilateralFiltering = false;}
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
void disableIMUFiltering();
void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
void disableFeatureDetection();
// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
RTABMAP_DEPRECATED void setScanParameters(
bool fromDepth,
int downsampleStep, // decimation of the depth image in case the scan is from depth image
float rangeMin,
float rangeMax,
float voxelSize,
int normalsK,
float normalsRadius,
bool forceGroundNormalsUp,
bool deskewing);
void setScanParameters(
bool fromDepth,
int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
float rangeMin=0.0f,
float rangeMax=0.0f,
float voxelSize = 0.0f,
int normalsK = 0,
float normalsRadius = 0.0f,
float groundNormalsUp = 0.0f,
bool deskewing = false);
void postUpdate(SensorData * data, SensorCaptureInfo * info = 0) const;
//getters
bool isPaused() const {return !this->isRunning();}
bool isCapturing() const {return this->isRunning();}
bool odomProvided() const;
Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
SensorCapture * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
Lidar * lidar() {return _lidar;} // return null if not set, valid until CameraThread is deleted
private:
virtual void mainLoopBegin();
virtual void mainLoop();
virtual void mainLoopKill();
private:
Camera * _camera;
SensorCapture * _odomSensor;
Lidar * _lidar;
Transform _extrinsicsOdomToCamera;
bool _odomAsGt;
double _poseTimeOffset;
float _poseScaleFactor;
double _poseWaitTime;
bool _mirroring;
bool _stereoExposureCompensation;
bool _colorOnly;
int _imageDecimation;
int _histogramMethod;
bool _stereoToDepth;
bool _scanDeskewing;
bool _scanFromDepth;
int _scanDownsampleStep;
float _scanRangeMin;
float _scanRangeMax;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
float _scanForceGroundNormalsUp;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
bool _bilateralFiltering;
float _bilateralSigmaS;
float _bilateralSigmaR;
IMUFilter * _imuFilter;
bool _imuBaseFrameConversion;
Feature2D * _featureDetector;
bool _depthAsMask;
};
//backward compatibility
RTABMAP_DEPRECATED typedef SensorCaptureThread CameraThread;
} // namespace rtabmap

94
corelib/include/rtabmap/core/SensorEvent.h Normal file
View File

@@ -0,0 +1,94 @@
/*
Copyright (c) 2010-2016, 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.
*/
#pragma once
#include <rtabmap/core/SensorCaptureInfo.h>
#include <rtabmap/utilite/UEvent.h>
#include "rtabmap/core/SensorData.h"
namespace rtabmap
{
class SensorEvent :
public UEvent
{
public:
enum Code {
kCodeData,
kCodeNoMoreImages
};
public:
SensorEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
UEvent(kCodeData),
data_(image, seq, stamp)
{
sensorCaptureInfo_.cameraName = cameraName;
}
SensorEvent() :
UEvent(kCodeNoMoreImages)
{
}
SensorEvent(const SensorData & data) :
UEvent(kCodeData),
data_(data)
{
}
SensorEvent(const SensorData & data, const std::string & cameraName) :
UEvent(kCodeData),
data_(data)
{
sensorCaptureInfo_.cameraName = cameraName;
}
SensorEvent(const SensorData & data, const SensorCaptureInfo & sensorCaptureInfo) :
UEvent(kCodeData),
data_(data),
sensorCaptureInfo_(sensorCaptureInfo)
{
}
// Image or descriptors
const SensorData & data() const {return data_;}
const std::string & cameraName() const {return sensorCaptureInfo_.cameraName;}
const SensorCaptureInfo & info() const {return sensorCaptureInfo_;}
virtual ~SensorEvent() {}
virtual std::string getClassName() const {return std::string("SensorEvent");}
private:
SensorData data_;
SensorCaptureInfo sensorCaptureInfo_;
};
//backward compatibility
RTABMAP_DEPRECATED typedef SensorEvent CameraEvent;
} // namespace rtabmap

2
corelib/include/rtabmap/core/camera/CameraDepthAI.h
View File

@@ -73,7 +73,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_DEPTHAI

2
corelib/include/rtabmap/core/camera/CameraFreenect.h
View File

@@ -61,7 +61,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_FREENECT

2
corelib/include/rtabmap/core/camera/CameraFreenect2.h
View File

@@ -77,7 +77,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_FREENECT2

2
corelib/include/rtabmap/core/camera/CameraImages.h
View File

@@ -118,7 +118,7 @@ public:
}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
bool readPoses(

2
corelib/include/rtabmap/core/camera/CameraK4A.h
View File

@@ -63,7 +63,7 @@ public:
void setPreferences(int rgb_resolution, int framerate, int depth_resolution);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
void close();

2
corelib/include/rtabmap/core/camera/CameraK4W2.h
View File

@@ -72,7 +72,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
void close();

2
corelib/include/rtabmap/core/camera/CameraMyntEye.h
View File

@@ -67,7 +67,7 @@ protected:
/**
* returned rgb and depth images should be already rectified if calibration was loaded
*/
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_MYNTEYE

2
corelib/include/rtabmap/core/camera/CameraOpenNI2.h
View File

@@ -69,7 +69,7 @@ public:
void setDepthDecimation(int decimation);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_OPENNI2

2
corelib/include/rtabmap/core/camera/CameraOpenNICV.h
View File

@@ -51,7 +51,7 @@ public:
virtual std::string getSerial() const {return "";} // unknown with OpenCV
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
bool _asus;

2
corelib/include/rtabmap/core/camera/CameraOpenni.h
View File

@@ -85,7 +85,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
pcl::Grabber* interface_;

2
corelib/include/rtabmap/core/camera/CameraRGBDImages.h
View File

@@ -53,7 +53,7 @@ public:
virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
CameraImages cameraDepth_;

2
corelib/include/rtabmap/core/camera/CameraRealSense.h
View File

@@ -72,7 +72,7 @@ public:
virtual bool odomProvided() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_REALSENSE

7
corelib/include/rtabmap/core/camera/CameraRealSense2.h
View File

@@ -68,7 +68,7 @@ public:
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const;
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
// parameters are set during initialization
// D400 series
@@ -77,7 +77,7 @@ public:
void setResolution(int width, int height, int fps = 30);
void setDepthResolution(int width, int height, int fps = 30);
void setGlobalTimeSync(bool enabled);
void publishInterIMU(bool enabled);
/**
* Dual mode (D400+T265 or L500+T265)
* @param enabled enable dual mode
@@ -105,7 +105,7 @@ private:
#endif
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_REALSENSE2
@@ -142,7 +142,6 @@ private:
int cameraDepthHeight_;
int cameraDepthFps_;
bool globalTimeSync_;
bool publishInterIMU_;
bool dualMode_;
Transform dualExtrinsics_;
std::string jsonConfig_;

2
corelib/include/rtabmap/core/camera/CameraStereoDC1394.h
View File

@@ -51,7 +51,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_DC1394

2
corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h
View File

@@ -53,7 +53,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_FLYCAPTURE2

2
corelib/include/rtabmap/core/camera/CameraStereoImages.h
View File

@@ -64,7 +64,7 @@ public:
virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
CameraImages * camera2_;

2
corelib/include/rtabmap/core/camera/CameraStereoTara.h
View File

@@ -60,7 +60,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
cv::VideoCapture capture_;

2
corelib/include/rtabmap/core/camera/CameraStereoVideo.h
View File

@@ -71,7 +71,7 @@ public:
void setResolution(int width, int height) {_width=width, _height=height;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
cv::VideoCapture capture_;

7
corelib/include/rtabmap/core/camera/CameraStereoZed.h
View File

@@ -76,12 +76,12 @@ public:
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const;
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
void publishInterIMU(bool enabled);
void postInterIMUPublic(const IMU & imu, double stamp);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_ZED
@@ -100,7 +100,6 @@ private:
bool computeOdometry_;
bool lost_;
bool force3DoF_;
bool publishInterIMU_;
ZedIMUThread * imuPublishingThread_;
#endif
};

2
corelib/include/rtabmap/core/camera/CameraStereoZedOC.h
View File

@@ -63,7 +63,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_ZEDOC

2
corelib/include/rtabmap/core/camera/CameraVideo.h
View File

@@ -64,7 +64,7 @@ public:
void setResolution(int width, int height) {_width=width, _height=height;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
// File type

94
corelib/include/rtabmap/core/lidar/LidarVLP16.h Normal file
View File

@@ -0,0 +1,94 @@
/*
Copyright (c) 2010-2022, Mathieu Labbe
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 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 CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_
// Should be first on windows to avoid "WinSock.h has already been included" error
#include <pcl/io/vlp_grabber.h>
#include <rtabmap/core/Lidar.h>
#include <rtabmap/utilite/USemaphore.h>
namespace rtabmap {
struct PointXYZIT {
float x;
float y;
float z;
float i;
float t;
};
class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber {
public:
LidarVLP16(
const std::string& pcapFile,
bool organized = false,
bool stampLast = true,
float frameRate = 0.0f,
Transform localTransform = Transform::getIdentity());
LidarVLP16(
const boost::asio::ip::address& ipAddress,
const std::uint16_t port = 2368,
bool organized = false,
bool useHostTime = true,
bool stampLast = true,
float frameRate = 0.0f,
Transform localTransform = Transform::getIdentity());
virtual ~LidarVLP16();
SensorData takeScan(SensorCaptureInfo * info = 0) {return takeData(info);}
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") override;
virtual std::string getSerial() const override {return getName();}
void setOrganized(bool enable);
private:
void buildTimings(bool dualMode);
virtual void toPointClouds (HDLDataPacket *dataPacket) override;
protected:
virtual SensorData captureData(SensorCaptureInfo * info = 0) override;
private:
// timing offset lookup table
std::vector< std::vector<float> > timingOffsets_;
bool timingOffsetsDualMode_;
double startSweepTime_;
double startSweepTimeHost_;
bool organized_;
bool useHostTime_;
bool stampLast_;
SensorData lastScan_;
std::vector<std::vector<PointXYZIT> > accumulatedScans_;
USemaphore scanReady_;
UMutex lastScanMutex_;
};
} /* namespace rtabmap */
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_ */

13
corelib/include/rtabmap/core/util3d.h
View File

@@ -455,6 +455,19 @@ RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadC
int downsampleStep = 1,
float voxelSize = 0.0f);
/**
* @brief Lidar deskewing
* @param input lidar, format should have time channel
* @param input stamp of the lidar
* @param velocity in base frame
* @param velocity stamp at which it has been computed
* @return lidar deskewed
*/
LaserScan RTABMAP_CORE_EXPORT deskew(
const LaserScan & input,
double inputStamp,
const rtabmap::Transform & velocity);
} // namespace util3d
} // namespace rtabmap