mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Added CameraStereoImages class to read stereo images from a directory. Added a particle filter to smooth odometry trajectory. Added parameter RGBD/OptimizeEpsilon to limit TORO iterations when error improvement is small. Added Rtabmap/CreateIntermediateNodes parameter: this can be used to keep all odometry poses 'between' nodes used for loop closure detection. Added PnP approach to loop closure constraint estimation. Fixed decimation of stereo images when image size is odd.
This commit is contained in:
@@ -107,6 +107,7 @@ public:
|
||||
|
||||
virtual bool init();
|
||||
std::string getPath() const {return _path;}
|
||||
unsigned int imagesCount() const;
|
||||
|
||||
protected:
|
||||
virtual cv::Mat captureImage();
|
||||
|
||||
@@ -110,11 +110,11 @@ public:
|
||||
}
|
||||
virtual ~StereoCameraModel() {}
|
||||
|
||||
bool isValid() const {return left_.isValid() && right_.isValid() && !R_.empty() && !T_.empty() && !E_.empty() && !F_.empty();}
|
||||
bool isValid() const {return left_.isValid() && right_.isValid();}
|
||||
const std::string & name() const {return name_;}
|
||||
|
||||
bool load(const std::string & directory, const std::string & cameraName);
|
||||
bool save(const std::string & directory, const std::string & cameraName);
|
||||
bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
|
||||
bool save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
|
||||
|
||||
double baseline() const {return -right_.Tx()/right_.fx();}
|
||||
|
||||
|
||||
@@ -86,7 +86,7 @@ class RTABMAP_EXP CameraRGBD
|
||||
{
|
||||
public:
|
||||
virtual ~CameraRGBD();
|
||||
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
|
||||
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
|
||||
|
||||
virtual bool init(const std::string & calibrationFolder = ".") = 0;
|
||||
virtual bool isCalibrated() const = 0;
|
||||
@@ -116,7 +116,7 @@ protected:
|
||||
/**
|
||||
* returned rgb and depth images should be already rectified
|
||||
*/
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) = 0;
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) = 0;
|
||||
|
||||
private:
|
||||
float _imageRate;
|
||||
@@ -152,7 +152,7 @@ public:
|
||||
virtual std::string getSerial() const;
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
|
||||
|
||||
private:
|
||||
pcl::Grabber* interface_;
|
||||
@@ -186,7 +186,7 @@ public:
|
||||
virtual std::string getSerial() const {return "";} // unknown with OpenCV
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
|
||||
|
||||
private:
|
||||
bool _asus;
|
||||
@@ -222,7 +222,7 @@ public:
|
||||
bool setMirroring(bool enabled);
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
|
||||
|
||||
private:
|
||||
openni::Device * _device;
|
||||
@@ -257,7 +257,7 @@ public:
|
||||
virtual std::string getSerial() const;
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
|
||||
|
||||
private:
|
||||
int deviceId_;
|
||||
@@ -295,7 +295,7 @@ public:
|
||||
virtual std::string getSerial() const;
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
|
||||
|
||||
private:
|
||||
int deviceId_;
|
||||
@@ -328,7 +328,7 @@ public:
|
||||
virtual std::string getSerial() const;
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy);
|
||||
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
|
||||
|
||||
private:
|
||||
DC1394Device *device_;
|
||||
@@ -353,11 +353,46 @@ public:
|
||||
virtual std::string getSerial() const;
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy);
|
||||
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
|
||||
|
||||
private:
|
||||
FlyCapture2::Camera * camera_;
|
||||
void * triclopsCtx_; // TriclopsContext
|
||||
};
|
||||
|
||||
/////////////////////////
|
||||
// CameraStereoImages
|
||||
/////////////////////////
|
||||
class CameraImages;
|
||||
class RTABMAP_EXP CameraStereoImages :
|
||||
public CameraRGBD
|
||||
{
|
||||
public:
|
||||
static bool available();
|
||||
|
||||
public:
|
||||
CameraStereoImages(
|
||||
const std::string & path,
|
||||
const std::string & cameraName = "stereo_images", // calibration file name
|
||||
const std::string & timestampsPath = "", // "times.txt"
|
||||
float imageRate=0.0f,
|
||||
const Transform & localTransform = Transform::getIdentity());
|
||||
virtual ~CameraStereoImages();
|
||||
|
||||
virtual bool init(const std::string & calibrationFolder = ".");
|
||||
virtual bool isCalibrated() const;
|
||||
virtual std::string getSerial() const;
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
|
||||
|
||||
private:
|
||||
CameraImages * camera_;
|
||||
CameraImages * camera2_;
|
||||
std::string cameraName_;
|
||||
std::string timestampsPath_;
|
||||
std::list<double> stamps_;
|
||||
StereoCameraModel stereoModel_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -70,6 +70,7 @@ public:
|
||||
int iterations() const {return iterations_;}
|
||||
bool isSlam2d() const {return slam2d_;}
|
||||
bool isCovarianceIgnored() const {return covarianceIgnored_;}
|
||||
double epsilon() const {return epsilon_;}
|
||||
|
||||
virtual std::map<int, Transform> optimize(
|
||||
int rootId,
|
||||
@@ -80,13 +81,18 @@ public:
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
|
||||
protected:
|
||||
Optimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false);
|
||||
Optimizer(
|
||||
int iterations = Parameters::defaultRGBDOptimizeIterations(),
|
||||
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
|
||||
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
|
||||
double epsilon = Parameters::defaultRGBDOptimizeEpsilon());
|
||||
Optimizer(const ParametersMap & parameters);
|
||||
|
||||
private:
|
||||
int iterations_;
|
||||
bool slam2d_;
|
||||
bool covarianceIgnored_;
|
||||
double epsilon_;
|
||||
};
|
||||
|
||||
class RTABMAP_EXP TOROOptimizer : public Optimizer
|
||||
|
||||
@@ -76,6 +76,27 @@ public:
|
||||
transVariance_ = transVariance;
|
||||
}
|
||||
|
||||
Link merge(const Link & link) const
|
||||
{
|
||||
UASSERT(to_ == link.from());
|
||||
UASSERT(type_ == link.type());
|
||||
UASSERT(!transform_.isNull());
|
||||
UASSERT(!link.transform().isNull());
|
||||
UASSERT(rotVariance_ > 0 && link.rotVariance() > 0 && transVariance_ > 0 && link.transVariance() > 0);
|
||||
return Link(
|
||||
from_,
|
||||
link.to(),
|
||||
type_,
|
||||
transform_ * link.transform(),
|
||||
1.0f/(1.0f/rotVariance_ + 1.0f/link.rotVariance()),
|
||||
1.0f/(1.0f/transVariance_ + 1.0f/link.transVariance()));
|
||||
}
|
||||
|
||||
Link inverse() const
|
||||
{
|
||||
return Link(to_, from_, type_, transform_.inverse(), rotVariance_, transVariance_);
|
||||
}
|
||||
|
||||
private:
|
||||
int from_;
|
||||
int to_;
|
||||
|
||||
@@ -91,6 +91,7 @@ public:
|
||||
int maxCheckedInDatabase = -1,
|
||||
bool incrementMarginOnLoop = false,
|
||||
bool ignoreLoopIds = false,
|
||||
bool ignoreBadSignatures = false,
|
||||
double * dbAccessTime = 0) const;
|
||||
std::map<int, float> getNeighborsIdRadius(
|
||||
int signatureId,
|
||||
@@ -264,6 +265,9 @@ private:
|
||||
bool _bowForce2D;
|
||||
bool _bowEpipolarGeometry;
|
||||
float _bowEpipolarGeometryVar;
|
||||
bool _bowPnPEstimation;
|
||||
double _bowPnPReprojError;
|
||||
int _bowPnPFlags;
|
||||
float _icpMaxTranslation;
|
||||
float _icpMaxRotation;
|
||||
int _icpDecimation;
|
||||
|
||||
@@ -42,11 +42,12 @@ namespace rtabmap {
|
||||
|
||||
class Feature2D;
|
||||
class OdometryInfo;
|
||||
class ParticleFilter;
|
||||
|
||||
class RTABMAP_EXP Odometry
|
||||
{
|
||||
public:
|
||||
virtual ~Odometry() {}
|
||||
virtual ~Odometry();
|
||||
Transform process(const SensorData & data, OdometryInfo * info = 0);
|
||||
virtual void reset(const Transform & initialPose = Transform::getIdentity());
|
||||
|
||||
@@ -75,12 +76,21 @@ private:
|
||||
float _maxDepth;
|
||||
int _resetCountdown;
|
||||
bool _force2D;
|
||||
bool _particleFiltering;
|
||||
int _particleSize;
|
||||
float _particleNoiseT;
|
||||
float _particleLambdaT;
|
||||
float _particleNoiseR;
|
||||
float _particleLambdaR;
|
||||
bool _fillInfoData;
|
||||
bool _pnpEstimation;
|
||||
double _pnpReprojError;
|
||||
int _pnpFlags;
|
||||
Transform _pose;
|
||||
int _resetCurrentCount;
|
||||
double previousStamp_;
|
||||
|
||||
std::vector<ParticleFilter *> filters_;
|
||||
|
||||
protected:
|
||||
Odometry(const rtabmap::ParametersMap & parameters);
|
||||
|
||||
@@ -40,7 +40,9 @@ public:
|
||||
variance(-1),
|
||||
features(-1),
|
||||
localMapSize(-1),
|
||||
time(-1),
|
||||
timeEstimation(-1),
|
||||
stamp(0),
|
||||
interval(0),
|
||||
type(-1)
|
||||
{}
|
||||
bool lost;
|
||||
@@ -49,7 +51,12 @@ public:
|
||||
float variance;
|
||||
int features;
|
||||
int localMapSize;
|
||||
float time;
|
||||
float timeEstimation;
|
||||
float timeParticleFiltering;
|
||||
double stamp;
|
||||
double interval;
|
||||
Transform transform;
|
||||
Transform transformFiltered;
|
||||
|
||||
int type; // 0=BOW, 1=Optical Flow, 2=ICP
|
||||
|
||||
|
||||
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
#include <rtabmap/utilite/UEventsHandler.h>
|
||||
#include <list>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
@@ -40,7 +41,7 @@ class Odometry;
|
||||
class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
|
||||
public:
|
||||
// take ownership of Odometry
|
||||
OdometryThread(Odometry * odometry);
|
||||
OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1);
|
||||
virtual ~OdometryThread();
|
||||
|
||||
protected:
|
||||
@@ -54,13 +55,14 @@ private:
|
||||
//============================================================
|
||||
void mainLoop();
|
||||
void addData(const SensorData & data);
|
||||
void getData(SensorData & data);
|
||||
bool getData(SensorData & data);
|
||||
|
||||
private:
|
||||
USemaphore _dataAdded;
|
||||
UMutex _dataMutex;
|
||||
SensorData _dataBuffer;
|
||||
std::list<SensorData> _dataBuffer;
|
||||
Odometry * _odometry;
|
||||
unsigned int _dataBufferMaxSize;
|
||||
bool _resetOdometry;
|
||||
};
|
||||
|
||||
|
||||
@@ -169,7 +169,8 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(Rtabmap, TimeThr, float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity).");
|
||||
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0, "Maximum signatures in the Working Memory (ms) (0 means infinity).");
|
||||
RTABMAP_PARAM(Rtabmap, DetectionRate, float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
|
||||
RTABMAP_PARAM(Rtabmap, ImageBufferSize, int, 1, "Data buffer size (0 min inf).");
|
||||
RTABMAP_PARAM(Rtabmap, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
|
||||
RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes, bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0.");
|
||||
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory(), "Working directory.");
|
||||
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
|
||||
RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
|
||||
@@ -307,6 +308,7 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations.");
|
||||
RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
|
||||
RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
|
||||
RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.001, "Stop optimizing when the error improvement is less than this value.");
|
||||
|
||||
// Odometry
|
||||
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");
|
||||
@@ -314,16 +316,23 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(Odom, MaxFeatures, int, 400, "0 no limits.");
|
||||
RTABMAP_PARAM(Odom, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
|
||||
RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
|
||||
RTABMAP_PARAM(Odom, Iterations, int, 30, "Maximum iterations to compute the transform from visual words.");
|
||||
RTABMAP_PARAM(Odom, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
|
||||
RTABMAP_PARAM(Odom, RefineIterations, int, 5, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
|
||||
RTABMAP_PARAM(Odom, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit).");
|
||||
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
|
||||
RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
|
||||
RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
|
||||
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
|
||||
RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
|
||||
RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences.");
|
||||
RTABMAP_PARAM(Odom, PnPReprojError, double, 8.0, "PnP reprojection error.");
|
||||
RTABMAP_PARAM(Odom, PnPFlags, int, 0, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
|
||||
RTABMAP_PARAM(Odom, PnPReprojError, double, 5.0, "PnP reprojection error.");
|
||||
RTABMAP_PARAM(Odom, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
|
||||
RTABMAP_PARAM(Odom, ParticleFiltering, bool, false, "Particle filtering to smooth the odometry trajectory.");
|
||||
RTABMAP_PARAM(Odom, ParticleSize, unsigned int, 400, "Number of particles of the filter.");
|
||||
RTABMAP_PARAM(Odom, ParticleNoiseT, float, 0.002, "Noise (m) of translation components (x,y,z).");
|
||||
RTABMAP_PARAM(Odom, ParticleLambdaT, float, 100, "Lambda of translation components (x,y,z).");
|
||||
RTABMAP_PARAM(Odom, ParticleNoiseR, float, 0.002, "Noise (rad) of rotational components (roll,pitch,yaw).");
|
||||
RTABMAP_PARAM(Odom, ParticleLambdaR, float, 100, "Lambda of rotational components (roll,pitch,yaw).");
|
||||
|
||||
// Odometry Bag-of-words
|
||||
RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
|
||||
@@ -359,6 +368,9 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
|
||||
RTABMAP_PARAM(LccBow, EpipolarGeometry, bool, false, "Use epipolar geometry to compute the loop closure transform.");
|
||||
RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
|
||||
RTABMAP_PARAM(LccBow, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences.");
|
||||
RTABMAP_PARAM(LccBow, PnPReprojError, double, 5.0, "PnP reprojection error.");
|
||||
RTABMAP_PARAM(LccBow, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
|
||||
RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
|
||||
RTABMAP_PARAM(LccReextract, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
|
||||
RTABMAP_PARAM(LccReextract, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");
|
||||
@@ -371,13 +383,13 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set.");
|
||||
RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
|
||||
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "Max iterations.");
|
||||
RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, "Ratio of matching correspondences to accept the transform.");
|
||||
RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.0, "Ratio of matching correspondences to accept the transform.");
|
||||
RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "Use point to plane ICP.");
|
||||
RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane.");
|
||||
|
||||
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences.");
|
||||
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "Max iterations.");
|
||||
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, "Ratio of matching correspondences to accept the transform.");
|
||||
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.0, "Ratio of matching correspondences to accept the transform.");
|
||||
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.025, "Voxel size to be used for ICP computation.");
|
||||
|
||||
// Stereo disparity
|
||||
|
||||
@@ -106,9 +106,11 @@ public:
|
||||
bool setUserData(int id, const std::vector<unsigned char> & data);
|
||||
void generateDOTGraph(const std::string & path, int id=0, int margin=5);
|
||||
void generateTOROGraph(const std::string & path, bool optimized, bool global);
|
||||
void exportPoses(const std::string & path, bool optimized, bool global);
|
||||
void resetMemory();
|
||||
void dumpPrediction() const;
|
||||
void dumpData() const;
|
||||
void dumpPoses(const std::string & path, const std::map<int, Transform> & poses) const;
|
||||
void parseParameters(const ParametersMap & parameters);
|
||||
void setWorkingDirectory(std::string path);
|
||||
void rejectLoopClosure(int oldId, int newId);
|
||||
|
||||
@@ -67,6 +67,8 @@ public:
|
||||
kCmdGenerateDOTLocalGraph, // params: path, id, margin
|
||||
kCmdGenerateTOROGraphLocal, // params: path, optimized
|
||||
kCmdGenerateTOROGraphGlobal, // params: path, optimized
|
||||
kCmdExportPosesGlobal,
|
||||
kCmdExportPosesLocal,
|
||||
kCmdCleanDataBuffer,
|
||||
kCmdPublish3DMapLocal, // params: optimized
|
||||
kCmdPublish3DMapGlobal, // params: optimized
|
||||
|
||||
@@ -64,6 +64,8 @@ public:
|
||||
kStateGeneratingDOTLocalGraph,
|
||||
kStateGeneratingTOROGraphLocal,
|
||||
kStateGeneratingTOROGraphGlobal,
|
||||
kStateExportingPosesLocal,
|
||||
kStateExportingPosesGlobal,
|
||||
kStateCleanDataBuffer,
|
||||
kStatePublishingMapLocal,
|
||||
kStatePublishingMapGlobal,
|
||||
@@ -81,7 +83,8 @@ public:
|
||||
|
||||
void clearBufferedData();
|
||||
void setDetectorRate(float rate);
|
||||
void setBufferSize(int bufferSize);
|
||||
void setDataBufferSize(unsigned int bufferSize);
|
||||
void createIntermediateNodes(bool enabled);
|
||||
|
||||
protected:
|
||||
virtual void handleEvent(UEvent * anEvent);
|
||||
@@ -91,9 +94,8 @@ private:
|
||||
virtual void mainLoopKill();
|
||||
void process();
|
||||
void addData(const SensorData & data);
|
||||
void getData(SensorData & data);
|
||||
bool getData(SensorData & data);
|
||||
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
|
||||
void setDataBufferSize(int size);
|
||||
void publishMap(bool optimized, bool full) const;
|
||||
void publishGraph(bool optimized, bool full) const;
|
||||
|
||||
@@ -105,8 +107,9 @@ private:
|
||||
std::list<SensorData> _dataBuffer;
|
||||
UMutex _dataMutex;
|
||||
USemaphore _dataAdded;
|
||||
int _dataBufferMaxSize;
|
||||
unsigned int _dataBufferMaxSize;
|
||||
float _rate;
|
||||
bool _createIntermediateNodes;
|
||||
UTimer * _frameRateTimer;
|
||||
|
||||
Rtabmap * _rtabmap;
|
||||
|
||||
Reference in New Issue
Block a user