i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap into gtest

Browse Source
This commit is contained in:
matlabbe
2026-05-16 10:15:18 -07:00
parent 0405949ff4 27ca7ec058
commit 46cf7ec344
103 changed files with 3380 additions and 1652 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/DBReader.h
+3
View File
@@ -60,6 +60,7 @@ public:
int stopMapId = -1,
bool priorsIgnored = false,
bool imuIgnored = false,
bool intermediateNodesAreNormalNodes = false,
const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
DBReader(const std::list<std::string> & databasePaths,
float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
@@ -76,6 +77,7 @@ public:
int stopMapId = -1,
bool priorsIgnored = false,
bool imuIgnored = false,
bool intermediateNodesAreNormalNodes = false,
const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
virtual ~DBReader();
@@ -106,6 +108,7 @@ private:
int _stopId;
std::vector<unsigned int> _cameraIndices;
bool _intermediateNodesIgnored;
bool _intermediateNodesAreNormalNodes;
bool _landmarksIgnored;
bool _featuresIgnored;
bool _priorsIgnored;
corelib/include/rtabmap/core/Graph.h
+10 -7
View File
@@ -118,15 +118,18 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
float & rotational_max,
bool align2D = false);
void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
struct MaxGraphErrors
{
float linear=-1.0f; // absolute error (m) of the link with maximum linear error
float angular=-1.0f; // absolute error (rad) of the link with maximum angular error
float linearRatio=-1.0f; // Ratio = absolute error (m) / linear std (m), of the link with maximum linear error
float angularRatio=-1.0f; // Ratio = absolute error (rad) / angular std (rad), of the link with maximum angular error
Link linearLink; // link with maximum linear error
Link angularLink; // link with maximum angular error
};
MaxGraphErrors RTABMAP_CORE_EXPORT computeMaxGraphErrors(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
float & maxLinearErrorRatio,
float & maxAngularErrorRatio,
float & maxLinearError,
float & maxAngularError,
const Link ** maxLinearErrorLink = 0,
const Link ** maxAngularErrorLink = 0,
bool for3DoF = false);
std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
corelib/include/rtabmap/core/LaserScan.h
+6 -2
View File
@@ -64,7 +64,8 @@ public:
kXYZNormal=8, /**< 3D points with X, Y, Z and normal vectors. */
kXYZINormal=9, /**< 3D points with X, Y, Z, intensity and normal vectors. */
kXYZRGBNormal=10, /**< 3D points with X, Y, Z, RGB color and normal vectors. */
kXYZIT=11 /**< 3D points with X, Y, Z, intensity and time. */
kXYZIT=11, /**< 3D points with X, Y, Z, intensity and time. */
kXYZIRT=12 /**< 3D points with X, Y, Z, intensity, ring and time. */
};
/// @name Static Utility Functions
@@ -76,6 +77,7 @@ public:
static bool isScanHasRGB(const Format & format);
static bool isScanHasIntensity(const Format & format);
static bool isScanHasTime(const Format & format);
static bool isScanHasRing(const Format & format);
static float packRGB(unsigned char r, unsigned char g, unsigned char b);
static void unpackRGB(float rgb, unsigned char & r, unsigned char & g, unsigned char & b);
@@ -176,6 +178,7 @@ public:
bool hasRGB() const {return isScanHasRGB(format_);}
bool hasIntensity() const {return isScanHasIntensity(format_);}
bool hasTime() const {return isScanHasTime(format_);}
bool hasRing() const {return isScanHasRing(format_);}
bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
bool isOrganized() const {return data_.rows > 1;}
LaserScan clone() const;
@@ -187,7 +190,8 @@ public:
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;}
int getRingOffset() const {return format_==kXYZIRT?4:-1;}
int getTimeOffset() const {return format_==kXYZIT?4:(format_==kXYZIRT?5:-1);}
/**
* @brief Access a specific field value of a point.
corelib/include/rtabmap/core/Memory.h
+2 -1
View File
@@ -140,6 +140,7 @@ public:
float radius,
const std::map<int, Transform> & optimizedPoses,
int maxGraphDepth) const;
void convertToIntermediate(int locationId);
void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
void saveLocationData(int locationId);
void removeLink(int idA, int idB);
@@ -162,7 +163,7 @@ public:
float getSimilarityThreshold() const {return _similarityThreshold;}
std::map<int, int> getWeights() const;
int getLastSignatureId() const;
const Signature * getLastWorkingSignature() const;
const Signature * getLastWorkingSignature(bool ignoreIntermediateNodes) const;
std::map<int, Link> getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const;
int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
bool labelSignature(int id, const std::string & label);
corelib/include/rtabmap/core/Odometry.h
+2 -1
View File
@@ -57,7 +57,8 @@ public:
kTypeOpenVINS = 10,
kTypeFLOAM = 11,
kTypeOpen3D = 12,
kTypeCuVSLAM = 13
kTypeCuVSLAM = 13,
kTypeLIOSAM = 14
};
public:
corelib/include/rtabmap/core/Parameters.h
+19 -3
View File
@@ -218,7 +218,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Mem, ReduceGraph, bool, false, uFormat("Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100%% of the loop closures accepted are good, so it is highly recommended to enable \"%s\" at the same time.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2, "Ratio of locations after the last loop closure in WM that cannot be transferred.");
RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false, "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, uFormat("On merge, update to new id. When false, no copy. Keep this disable if %s=true.", kRtabmapCreateIntermediateNodes().c_str()));
RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location IDs, False=use input image IDs.");
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
@@ -249,7 +249,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Kp, MinDepth, float, 0, "Filter extracted keypoints by depth.");
RTABMAP_PARAM(Kp, MaxFeatures, int, 500, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
RTABMAP_PARAM(Kp, SSC, bool, false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, uFormat("Bad signature ratio. If %s=0, the ratio is computed from the average number of words per signature (less than Ratio x AverageWordsPerImage = bad).", kKpMaxFeatures().c_str()));
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
// OpenCV>2 without xFeatures2D module doesn't have BRIEF
@@ -379,6 +379,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with \"%s\", the disabled loop closure links will be removed.", kOptimizerRobust().c_str()));
RTABMAP_PARAM(RGBD, OptimizeMaxErrorRepairRadius, float, 0.0, uFormat("If two consecutive loop closures are rejected by %s on the same old loop closure link, we will remove that old link, and other old links under that radius if necessary, until optimization is accepted. When optimization is accepted, the old loop closure links are removed from the graph. This feature is useful to reject bad loop closures that were accepted previously. Set to 0 to disable this feature.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(RGBD, MaxLoopClosureDistance, float, 0.0, "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
RTABMAP_PARAM(RGBD, ForceOdom3DoF, bool, true, uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
RTABMAP_PARAM(RGBD, StartAtOrigin, bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
@@ -465,7 +466,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM 14=LIO-SAM");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
@@ -687,6 +688,21 @@ class RTABMAP_CORE_EXPORT Parameters
// Odometry cuVSLAM
RTABMAP_PARAM(OdomCuVSLAM, MulticamMode, int, 0, "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision.");
// Odometry LIO-SAM
RTABMAP_PARAM_STR(OdomLIOSAM, ConfigPath, "", "Path to LIO-SAM params.yaml config file. When set, sensor/IMU/feature parameters are loaded from the file and the individual parameters below are ignored.");
RTABMAP_PARAM(OdomLIOSAM, Sensor, int, 0, "LiDAR sensor: 0=Velodyne, 1=Ouster, 2=Livox");
RTABMAP_PARAM(OdomLIOSAM, NScan, int, 16, "Number of LiDAR channels (16, 32, 64, 128).");
RTABMAP_PARAM(OdomLIOSAM, HorizonScan, int, 1800, "Horizontal resolution (Velodyne:1800, Ouster:512/1024/2048).");
RTABMAP_PARAM(OdomLIOSAM, ImuAccNoise, float, 0.01, "IMU accelerometer white noise.");
RTABMAP_PARAM(OdomLIOSAM, ImuGyrNoise, float, 0.001, "IMU gyroscope white noise.");
RTABMAP_PARAM(OdomLIOSAM, ImuAccBiasN, float, 0.0002,"IMU accelerometer bias noise.");
RTABMAP_PARAM(OdomLIOSAM, ImuGyrBiasN, float, 0.00003,"IMU gyroscope bias noise.");
RTABMAP_PARAM(OdomLIOSAM, ImuGravity, float, 9.80511,"Gravity magnitude.");
RTABMAP_PARAM(OdomLIOSAM, EdgeThreshold,float, 1.0, "Edge feature curvature threshold.");
RTABMAP_PARAM(OdomLIOSAM, SurfThreshold,float, 0.1, "Surface feature curvature threshold.");
RTABMAP_PARAM(OdomLIOSAM, LinVar, float, 0.01, "Linear output variance.");
RTABMAP_PARAM(OdomLIOSAM, AngVar, float, 0.01, "Angular output variance.");
// Common registration parameters
RTABMAP_PARAM(Reg, RepeatOnce, bool, true, "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
RTABMAP_PARAM(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp");
corelib/include/rtabmap/core/Rtabmap.h
+10
View File
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Statistics.h"
#include "rtabmap/core/Link.h"
#include "rtabmap/core/ProgressState.h"
#include "rtabmap/core/Graph.h"
#include <opencv2/core/core.hpp>
#include <list>
@@ -264,6 +265,13 @@ private:
std::multimap<int, Link> * constraints = 0,
double * error = 0,
int * iterationsDone = 0) const;
std::list<std::pair<int, int> > repairGraph(
graph::MaxGraphErrors & maxGraphErrors,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
double & optimizationError,
int & optimizationIterations,
cv::Mat & optimizationCovariance);
void updateGoalIndex();
bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
@@ -320,6 +328,7 @@ private:
std::string _databasePath;
bool _optimizeFromGraphEnd;
float _optimizationMaxError;
float _optimizationMaxErrorRepairRadius;
bool _startNewMapOnLoopClosure;
bool _startNewMapOnGoodSignature;
float _goalReachedRadius; // meters
@@ -379,6 +388,7 @@ private:
std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures
std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
std::map<int, Transform> _markerPriors;
std::pair<int, int> _lastRejectedLoopClosureIds;
std::set<int> _nodesToRepublish;
corelib/include/rtabmap/core/RtabmapThread.h
-1
View File
@@ -106,7 +106,6 @@ private:
unsigned int _dataBufferMaxSize;
float _rate;
bool _createIntermediateNodes;
UTimer * _frameRateTimer;
double _previousStamp;
Rtabmap * _rtabmap;
corelib/include/rtabmap/core/SensorData.h
+8 -8
View File
@@ -495,6 +495,12 @@ public:
_userDataCompressed.empty() &&
_keypoints.size() == 0 &&
_descriptors.empty() &&
_groundCellsRaw.empty() &&
_groundCellsCompressed.empty() &&
_obstacleCellsRaw.empty() &&
_obstacleCellsCompressed.empty() &&
_emptyCellsRaw.empty() &&
_emptyCellsCompressed.empty() &&
imu_.empty());
}
@@ -750,12 +756,6 @@ public:
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewPoint);
/**
* @brief Clears raw occupancy grid data (keeps compressed)
*/
void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
/**
* @brief Returns raw ground cells
* @return Const reference to raw ground cells matrix
@@ -959,12 +959,12 @@ public:
* Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
* Raw data are kept is set.
*/
void clearCompressedData(bool images = true, bool scan = true, bool userData = true);
void clearCompressedData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
/**
* Clear raw rgb/depth (left/right) images, raw laser scan and raw user data.
* Compressed data are kept is set.
*/
void clearRawData(bool images = true, bool scan = true, bool userData = true);
void clearRawData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
/**
* @brief Checks if a 3D point is visible from any camera
corelib/include/rtabmap/core/Statistics.h
+7
View File
@@ -126,6 +126,13 @@ class RTABMAP_CORE_EXPORT Statistics
RTABMAP_STATS(Loop, Optimization_max_ang_error_ratio, );
RTABMAP_STATS(Loop, Optimization_error, );
RTABMAP_STATS(Loop, Optimization_iterations, );
RTABMAP_STATS(Loop, Optimization_max_error_from_id, );
RTABMAP_STATS(Loop, Optimization_max_error_to_id, );
RTABMAP_STATS(Loop, Optimization_max_ang_error_from_id, );
RTABMAP_STATS(Loop, Optimization_max_ang_error_to_id, );
RTABMAP_STATS(Loop, Optimization_max_error_removed_from_id, );
RTABMAP_STATS(Loop, Optimization_max_error_removed_to_id, );
RTABMAP_STATS(Loop, Optimization_max_error_removed_count, );
RTABMAP_STATS(Loop, Linear_variance,);
RTABMAP_STATS(Loop, Angular_variance,);
RTABMAP_STATS(Loop, Landmark_detected,);
corelib/include/rtabmap/core/impl/util3d.hpp
+84 -15
View File
@@ -41,11 +41,11 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
return LaserScan();
}
//determine the output type
int fieldStates[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity
int fieldStates[10] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity,time,ring
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
std::uint32_t fieldOffsets[8] = {0};
std::uint32_t fieldOffsets[10] = {0};
#else
pcl::uint32_t fieldOffsets[8] = {0};
pcl::uint32_t fieldOffsets[10] = {0};
#endif
for(unsigned int i=0; i<cloud.fields.size(); ++i)
{
@@ -102,6 +102,42 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
fieldStates[7] = 1;
fieldOffsets[7] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("time") == 0)
{
if(cloud.fields[i].datatype != pcl::PCLPointField::FLOAT32)
{
static bool warningShown = false;
if(!warningShown)
{
UWARN("The input scan cloud has an \"time\" field "
"but the datatype (%d) is not supported. Time will be ignored. "
"This message is only shown once.", cloud.fields[i].datatype);
warningShown = true;
}
continue;
}
fieldStates[8] = 1;
fieldOffsets[8] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("ring") == 0)
{
if(cloud.fields[i].datatype != pcl::PCLPointField::UINT16)
{
static bool warningShown = false;
if(!warningShown)
{
UWARN("The input scan cloud has an \"ring\" field "
"but the datatype (%d) is not supported. Ring will be ignored. "
"This message is only shown once.", cloud.fields[i].datatype);
warningShown = true;
}
continue;
}
fieldStates[9] = 1;
fieldOffsets[9] = cloud.fields[i].offset;
}
else
{
UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
@@ -117,6 +153,8 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
bool hasIntensity = fieldStates[7];
bool hasRGB = !hasIntensity&&fieldStates[6];
bool hasTime = hasIntensity&&fieldStates[8];
bool hasRing = hasIntensity&&fieldStates[9];
bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
LaserScan::Format format;
@@ -140,7 +178,18 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
}
else if(!hasNormals && hasIntensity)
{
format = LaserScan::kXYZI;
if(hasTime && hasRing)
{
format = LaserScan::kXYZIRT;
}
else if(hasTime)
{
format = LaserScan::kXYZIT;
}
else
{
format = LaserScan::kXYZI;
}
}
else if(!hasNormals && hasRGB)
{
@@ -183,6 +232,7 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
transformRot = transform.rotation();
}
int oi=0;
UASSERT(cloud.height == 1 || cloud.row_step != 0);
for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row)
{
const uint8_t* row_data = &cloud.data[row * cloud.row_step];
@@ -242,26 +292,45 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
{
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
if(format == LaserScan::kXYZIT)
{
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
ptr[4] = *(float*)(msg_data + fieldOffsets[8]);
}
else // kXYNormal
{
ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
}
valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
}
else if(laserScan.channels() == 6)
{
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
if(format == LaserScan::kXYINormal)
{
ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
}
else // XYZNormal
if(format == LaserScan::kXYZIRT)
{
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
ptr[4] = float(*(unsigned short*)(msg_data + fieldOffsets[9])); // Convert 16U to float
ptr[5] = *(float*)(msg_data + fieldOffsets[8]);
}
else // with normal
{
if(format == LaserScan::kXYINormal)
{
ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
}
else // XYZNormal
{
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
}
ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
}
ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]);
}
else if(laserScan.channels() == 7)
corelib/include/rtabmap/core/odometry/OdometryLIOSAM.h
+82
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.
*/
#ifndef ODOMETRYLIOSAM_H_
#define ODOMETRYLIOSAM_H_
#include <rtabmap/core/Odometry.h>
#ifdef RTABMAP_LIOSAM
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>
namespace lio_sam { class LioSamCore; }
#endif
namespace rtabmap {
class RTABMAP_CORE_EXPORT OdometryLIOSAM : public Odometry
{
public:
OdometryLIOSAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryLIOSAM();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeLIOSAM;}
virtual bool canProcessAsyncIMU() const {return true;}
private:
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
#ifdef RTABMAP_LIOSAM
bool init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform);
#endif
private:
#ifdef RTABMAP_LIOSAM
lio_sam::LioSamCore * lioSam_;
Transform lastPose_;
bool lost_;
float linVar_;
float angVar_;
ParametersMap parameters_;
Transform imuLocalTransform_; // base_link -> imu_link (cached for deferred init)
// Buffered IMU samples received before initialization
struct ImuSample {
double stamp;
Eigen::Vector3d acc;
Eigen::Vector3d gyro;
Eigen::Quaterniond orientation;
};
std::vector<ImuSample> imuBuffer_;
#endif
};
}
#endif /* ODOMETRYLIOSAM_H_ */
corelib/include/rtabmap/core/util3d.h
+35 -1
View File
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
#include <opencv2/core/core.hpp>
#include <rtabmap/core/ProgressState.h>
#include <cstdint>
#include <map>
#include <list>
@@ -48,6 +49,20 @@ namespace rtabmap
/**
* @brief This namespace contains 3D point cloud processing utilities.
*/
// 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
{
@@ -704,7 +719,7 @@ LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl:
LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
/**
* @ingroup LaserScanFromPointCloud
* @brief `pcl::PointXYZRGB` → (x, y, z, intensity) → LaserScan::kXYZI
* @brief `pcl::PointXYZI` → (x, y, z, intensity) → LaserScan::kXYZI
*/
LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
/**
@@ -713,6 +728,16 @@ LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl:
*/
LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
/**
* @ingroup LaserScanFromPointCloud
* @brief `rtabmap::PointXYZIRT` → (x, y, z, intensity, ring time) → LaserScan::kXYZIRT
*/
LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
/**
* @ingroup LaserScanFromPointCloud
* @brief `rtabmap::PointXYZIRT` → (x, y, z, intensity, ring time) → LaserScan::kXYZIRT
*/
LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
/**
* @ingroup LaserScanFromPointCloud
* @brief `pcl::PointXYZRGBNormal` → (x, y, z, rgb, nx, ny, nz) → LaserScan::kXYZRGBNormal
*/
@@ -1321,6 +1346,15 @@ LaserScan RTABMAP_CORE_EXPORT deskew(
} // 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_ */
corelib/src/CMakeLists.txt
+13 -4
View File
@@ -98,6 +98,7 @@ SET(SRC_FILES
odometry/OdometryORBSLAM3.cpp
odometry/OdometryLOAM.cpp
odometry/OdometryFLOAM.cpp
odometry/OdometryLIOSAM.cpp
odometry/OdometryMSCKF.cpp
odometry/OdometryVINSFusion.cpp
odometry/OdometryOpenVINS.cpp
@@ -604,6 +605,18 @@ IF(floam_FOUND)
)
ENDIF(floam_FOUND)
IF(lio_sam_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${lio_sam_INCLUDE_DIRS}
)
link_directories(${lio_sam_LIBRARY_DIRS})
SET(LIBRARIES
${LIBRARIES}
lio_sam_core
)
ENDIF(lio_sam_FOUND)
IF(ZED_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
@@ -791,10 +804,6 @@ IF(CUVSLAM_FOUND)
ENDIF(CUVSLAM_FOUND)
IF(GTSAM_FOUND)
SET(SRC_FILES
${SRC_FILES}
optimizer/gtsam/GravityFactor.cpp
)
SET(LIBRARIES
${LIBRARIES}
gtsam
corelib/src/DBReader.cpp
+5 -1
View File
@@ -57,6 +57,7 @@ DBReader::DBReader(const std::string & databasePath,
int stopMapId,
bool priorsIgnored,
bool imuIgnored,
bool intermediateNodesAreNormalNodes,
const std::vector<Transform> & cameraLocalTransformOverrides) :
Camera(frameRate),
_paths(uSplit(databasePath, ';')),
@@ -67,6 +68,7 @@ DBReader::DBReader(const std::string & databasePath,
_stopId(stopId),
_cameraIndices(cameraIndices),
_intermediateNodesIgnored(intermediateNodesIgnored),
_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_priorsIgnored(priorsIgnored),
@@ -99,6 +101,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
int stopMapId,
bool priorsIgnored,
bool imuIgnored,
bool intermediateNodesAreNormalNodes,
const std::vector<Transform> & cameraLocalTransformOverrides) :
Camera(frameRate),
_paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
_stopId(stopId),
_cameraIndices(cameraIndices),
_intermediateNodesIgnored(intermediateNodesIgnored),
_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_priorsIgnored(priorsIgnored),
@@ -749,7 +753,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
data.setStereoCameraModels(combinedStereoModels);
}
}
data.setId(seq);
data.setId(!_intermediateNodesAreNormalNodes && s->getWeight()==-1 ? -1 : seq);
data.setStamp(s->getStamp());
data.setGroundTruth(s->getGroundTruthPose());
if(!globalPose.isNull())
corelib/src/Graph.cpp
+12 -38
View File
@@ -927,21 +927,12 @@ Transform calcRMSE (
return t;
}
void computeMaxGraphErrors(
MaxGraphErrors computeMaxGraphErrors(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
float & maxLinearErrorRatio,
float & maxAngularErrorRatio,
float & maxLinearError,
float & maxAngularError,
const Link ** maxLinearErrorLink,
const Link ** maxAngularErrorLink,
bool force3DoF)
{
maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1;
maxLinearError = -1;
maxAngularError = -1;
MaxGraphErrors maxError;
UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -963,19 +954,7 @@ void computeMaxGraphErrors(
iter->second.to(),
t2.prettyPrint().c_str());
if(maxLinearErrorLink)
{
*maxLinearErrorLink = 0;
}
if(maxAngularErrorLink)
{
*maxAngularErrorLink = 0;
}
maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1;
maxLinearError = -1;
maxAngularError = -1;
return;
return MaxGraphErrors();
}
Transform t;
@@ -999,14 +978,11 @@ void computeMaxGraphErrors(
UASSERT(iter->second.transVariance(false)>0.0);
float stddevLinear = sqrt(iter->second.transVariance(false));
float linearErrorRatio = linearError/stddevLinear;
if(linearErrorRatio > maxLinearErrorRatio)
if(linearErrorRatio > maxError.linearRatio)
{
maxLinearError = linearError;
maxLinearErrorRatio = linearErrorRatio;
if(maxLinearErrorLink)
{
*maxLinearErrorLink = &iter->second;
}
maxError.linear = linearError;
maxError.linearRatio = linearErrorRatio;
maxError.linearLink = iter->second;
}
// For landmark links, don't compute angular error if it doesn't estimate orientation
@@ -1031,18 +1007,16 @@ void computeMaxGraphErrors(
UASSERT(iter->second.rotVariance(false)>0.0);
float stddevAngular = sqrt(iter->second.rotVariance(false));
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
if(angularErrorRatio > maxError.angularRatio)
{
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
if(maxAngularErrorLink)
{
*maxAngularErrorLink = &iter->second;
}
maxError.angular = angularError;
maxError.angularRatio = angularErrorRatio;
maxError.angularLink = iter->second;
}
}
}
}
return maxError;
}
std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)
corelib/src/LaserScan.cpp
+11 -3
View File
@@ -68,6 +68,9 @@ std::string LaserScan::formatName(const Format & format)
case kXYZIT:
name = "XYZIT";
break;
case kXYZIRT:
name = "XYZIRT";
break;
default:
name = "Unknown";
break;
@@ -96,6 +99,7 @@ int LaserScan::channels(const Format & format)
break;
case kXYZNormal:
case kXYINormal:
case kXYZIRT:
channels = 6;
break;
case kXYZINormal:
@@ -123,11 +127,15 @@ bool LaserScan::isScanHasRGB(const Format & format)
}
bool LaserScan::isScanHasIntensity(const Format & format)
{
return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT;
return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT || format==kXYZIRT;
}
bool LaserScan::isScanHasTime(const Format & format)
{
return format==kXYZIT;
return format==kXYZIT || format==kXYZIRT;
}
bool LaserScan::isScanHasRing(const Format & format)
{
return format==kXYZIRT;
}
float LaserScan::packRGB(unsigned char r, unsigned char g, unsigned char b)
@@ -420,7 +428,7 @@ void LaserScan::init(
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal || format == kXYZIRT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
}
}
corelib/src/Memory.cpp
+552 -319
View File
File diff suppressed because it is too large Load Diff
corelib/src/Odometry.cpp
+5 -1
View File
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/odometry/OdometryORBSLAM3.h"
#include "rtabmap/core/odometry/OdometryLOAM.h"
#include "rtabmap/core/odometry/OdometryFLOAM.h"
#include "rtabmap/core/odometry/OdometryLIOSAM.h"
#include "rtabmap/core/odometry/OdometryMSCKF.h"
#include "rtabmap/core/odometry/OdometryVINSFusion.h"
#include "rtabmap/core/odometry/OdometryOpenVINS.h"
@@ -101,6 +102,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
case Odometry::kTypeFLOAM:
odometry = new OdometryFLOAM(parameters);
break;
case Odometry::kTypeLIOSAM:
odometry = new OdometryLIOSAM(parameters);
break;
case Odometry::kTypeMSCKF:
odometry = new OdometryMSCKF(parameters);
break;
@@ -653,7 +657,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
UWARN("Could not find imu transform at %f", data.stamp());
}
}
else if(!guess.isNull()) {
else if(!guess.isNull() && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())) {
UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
}
corelib/src/Optimizer.cpp
+2 -1
View File
@@ -240,7 +240,8 @@ void Optimizer::getConnectedGraph(
{
UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
UASSERT(fromId>0);
UASSERT(uContains(posesIn, fromId));
UASSERT_MSG(uContains(posesIn, fromId), uFormat("poses=%ld (first=%d last=%d) fromId=%d",
posesIn.size(), posesIn.empty()?0:posesIn.begin()->first, posesIn.empty()?0:posesIn.rbegin()->first, fromId).c_str());
posesOut.clear();
linksOut.clear();
corelib/src/Parameters.cpp
+9 -3
View File
@@ -903,6 +903,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With LIO-SAM:";
#ifdef RTABMAP_LIOSAM
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With FOVIS:";
#ifdef RTABMAP_FOVIS
@@ -1227,13 +1233,13 @@ void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, Pa
std::vector<std::string> version = uListToVector(uSplit((*iter).second, '.'));
if(version.size() == 3)
{
if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
if(RTABMAP_VERSION_COMPARE(<, std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
{
if(configFilePath.find(".rtabmap") != std::string::npos)
{
UWARN("Version in the config file \"%s\" is more recent (\"%s\") than "
"current RTAB-Map version used (\"%s\"). The config file will be upgraded "
"to new version.",
"current RTAB-Map version used (\"%s\"). The config file will be downgraded "
"to current RTAB-Map version if saved.",
configFilePath.c_str(),
(*iter).second,
RTABMAP_VERSION);
corelib/src/Rtabmap.cpp
+714 -390
View File
File diff suppressed because it is too large Load Diff
corelib/src/RtabmapThread.cpp
+5 -9
View File
@@ -47,8 +47,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
_rate(Parameters::defaultRtabmapDetectionRate()),
_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
_frameRateTimer(new UTimer()),
_previousStamp(0.0),
_previousStamp(-1.0),
_rtabmap(rtabmap),
_paused(false),
lastPose_(Transform::getIdentity())
@@ -62,8 +61,6 @@ RtabmapThread::~RtabmapThread()
UEventsManager::removeHandler(this);
close(true);
delete _frameRateTimer;
}
void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent)
@@ -88,7 +85,7 @@ void RtabmapThread::clearBufferedData()
_newMapEvents.clear();
lastPose_.setIdentity();
covariance_ = cv::Mat();
_previousStamp = 0;
_previousStamp = -1;
}
_dataMutex.unlock();
@@ -500,9 +497,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
bool ignoreFrame = false;
if(_rate>0.0f)
{
if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate))
{
UDEBUG("Ignoring frame %f (previous stamp=%f, period=%f)",
odomEvent.data().stamp(), _previousStamp, 1.0/_rate);
ignoreFrame = true;
}
}
@@ -540,7 +538,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
else if(!ignoreFrame)
{
_frameRateTimer->start();
_previousStamp = odomEvent.data().stamp();
}
@@ -559,7 +556,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
// set negative id so rtabmap will detect it as an intermediate node
SensorData tmp = odomEvent.data();
tmp.setId(-1);
tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
}
else
corelib/src/SensorData.cpp
+18 -2
View File
@@ -1005,7 +1005,7 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
(_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
}
void SensorData::clearCompressedData(bool images, bool scan, bool userData)
void SensorData::clearCompressedData(bool images, bool scan, bool userData, bool occupancyGrid)
{
if(images)
{
@@ -1021,14 +1021,24 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
{
_userDataCompressed=cv::Mat();
}
if(occupancyGrid)
{
_groundCellsCompressed=cv::Mat();
_emptyCellsCompressed=cv::Mat();
_obstacleCellsCompressed=cv::Mat();
}
}
void SensorData::clearRawData(bool images, bool scan, bool userData)
void SensorData::clearRawData(bool images, bool scan, bool userData, bool occupancyGrid)
{
if(images)
{
_imageRaw=cv::Mat();
_depthOrRightRaw=cv::Mat();
_depthConfidenceRaw=cv::Mat();
#ifdef HAVE_OPENCV_CUDEV
_imageRawGpu = cv::cuda::GpuMat();
_depthOrRightRawGpu = cv::cuda::GpuMat();
#endif
}
if(scan)
{
@@ -1038,6 +1048,12 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
{
_userDataRaw=cv::Mat();
}
if(occupancyGrid)
{
_groundCellsRaw=cv::Mat();
_emptyCellsRaw=cv::Mat();
_obstacleCellsRaw=cv::Mat();
}
}
corelib/src/global_map/OccupancyGrid.cpp
+2 -2
View File
@@ -222,14 +222,14 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
if(!cache().empty())
{
UDEBUG("Updating from cache");
UDEBUG("Updating %ld poses from cache", newPoses.size());
for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
{
if(uContains(cache(), iter->first))
{
const LocalGrid & localGrid = cache().at(iter->first);
UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
//UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
//ground
cv::Mat ground;
corelib/src/odometry/OdometryLIOSAM.cpp
+478
View File
@@ -0,0 +1,478 @@
/*
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.
*/
#include "rtabmap/core/odometry/OdometryLIOSAM.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UFile.h"
#ifdef RTABMAP_LIOSAM
#include <LioSamCore.h>
#include <pcl/common/transforms.h>
#endif
namespace rtabmap {
static ParametersMap disableDeskewing(ParametersMap params) {
// LIO-SAM performs its own internal deskewing via imageProjection.
// The base-class deskew must be disabled so that the original per-point
// timestamps reach LIO-SAM intact.
params[Parameters::kOdomDeskewing()] = "false";
return params;
}
OdometryLIOSAM::OdometryLIOSAM(const ParametersMap & parameters) :
Odometry(disableDeskewing(parameters))
#ifdef RTABMAP_LIOSAM
,lioSam_(0)
,lastPose_(Transform::getIdentity())
,lost_(false)
,linVar_(Parameters::defaultOdomLIOSAMLinVar())
,angVar_(Parameters::defaultOdomLIOSAMAngVar())
,parameters_(parameters)
#endif
{
#ifdef RTABMAP_LIOSAM
Parameters::parse(parameters, Parameters::kOdomLIOSAMLinVar(), linVar_);
UASSERT(linVar_ > 0.0f);
Parameters::parse(parameters, Parameters::kOdomLIOSAMAngVar(), angVar_);
UASSERT(angVar_ > 0.0f);
#endif
}
OdometryLIOSAM::~OdometryLIOSAM()
{
#ifdef RTABMAP_LIOSAM
delete lioSam_;
#endif
}
void OdometryLIOSAM::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
#ifdef RTABMAP_LIOSAM
if(lioSam_)
{
lioSam_->reset();
}
lastPose_ = Transform::getIdentity();
lost_ = false;
imuLocalTransform_ = Transform();
imuBuffer_.clear();
#endif
}
#ifdef RTABMAP_LIOSAM
bool OdometryLIOSAM::init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform)
{
ParamServer config;
// Check if a config file path was provided
std::string configPath;
Parameters::parse(parameters_, Parameters::kOdomLIOSAMConfigPath(), configPath);
if(!configPath.empty())
{
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(!UFile::exists(configPath))
{
UERROR("LIO-SAM config file not found: %s", configPath.c_str());
return false;
}
UINFO("Loading LIO-SAM parameters from config file: %s", configPath.c_str());
config = loadParamsFromYaml(configPath);
}
else
{
UINFO("No LIO-SAM config file provided, using rtabmap parameters");
// Build ParamServer from individual rtabmap parameters
int sensorType = Parameters::defaultOdomLIOSAMSensor();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMSensor(), sensorType);
if(sensorType == 1)
config.sensor = SensorType::OUSTER;
else if(sensorType == 2)
config.sensor = SensorType::LIVOX;
else
config.sensor = SensorType::VELODYNE;
config.N_SCAN = Parameters::defaultOdomLIOSAMNScan();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMNScan(), config.N_SCAN);
config.Horizon_SCAN = Parameters::defaultOdomLIOSAMHorizonScan();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMHorizonScan(), config.Horizon_SCAN);
config.imuAccNoise = Parameters::defaultOdomLIOSAMImuAccNoise();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuAccNoise(), config.imuAccNoise);
config.imuGyrNoise = Parameters::defaultOdomLIOSAMImuGyrNoise();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGyrNoise(), config.imuGyrNoise);
config.imuAccBiasN = Parameters::defaultOdomLIOSAMImuAccBiasN();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuAccBiasN(), config.imuAccBiasN);
config.imuGyrBiasN = Parameters::defaultOdomLIOSAMImuGyrBiasN();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGyrBiasN(), config.imuGyrBiasN);
config.imuGravity = Parameters::defaultOdomLIOSAMImuGravity();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGravity(), config.imuGravity);
config.edgeThreshold = Parameters::defaultOdomLIOSAMEdgeThreshold();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMEdgeThreshold(), config.edgeThreshold);
config.surfThreshold = Parameters::defaultOdomLIOSAMSurfThreshold();
Parameters::parse(parameters_, Parameters::kOdomLIOSAMSurfThreshold(), config.surfThreshold);
// Set reasonable defaults for params not exposed via rtabmap
config.downsampleRate = 1;
config.lidarMinRange = 1.0f;
config.lidarMaxRange = 1000.0f;
config.imuRPYWeight = 0.01f;
config.odometrySurfLeafSize = 0.2f;
config.mappingCornerLeafSize = 0.2f;
config.mappingSurfLeafSize = 0.4f;
config.z_tollerance = FLT_MAX;
config.rotation_tollerance = FLT_MAX;
config.numberOfCores = 4;
config.mappingProcessInterval = 0.01;
config.surroundingkeyframeAddingDistThreshold = 1.0f;
config.surroundingkeyframeAddingAngleThreshold = 0.2f;
config.surroundingKeyframeDensity = 1.0f;
config.surroundingKeyframeSearchRadius = 50.0f;
config.loopClosureEnableFlag = false; // rtabmap handles loop closures
config.loopClosureFrequency = 1.0f;
config.surroundingKeyframeSize = 50;
config.historyKeyframeSearchRadius = 10.0f;
config.historyKeyframeSearchTimeDiff = 30.0f;
config.historyKeyframeSearchNum = 25;
config.historyKeyframeFitnessScore = 0.3f;
config.globalMapVisualizationSearchRadius = 1e3f;
config.globalMapVisualizationPoseDensity = 10.0f;
config.globalMapVisualizationLeafSize = 1.0f;
config.edgeFeatureMinValidNum = 10;
config.surfFeatureMinValidNum = 100;
config.savePCD = false;
config.useImuHeadingInitialization = false;
config.useGpsElevation = false;
config.gpsCovThreshold = 2.0f;
config.poseCovThreshold = 25.0f;
}
// Always override extrinsics from sensor local transforms when available.
// This ensures the IMU-to-lidar transform matches the actual sensor setup
// regardless of what the config file says.
// imuLocalTransform = T_base_imu (base_link -> imu_link)
// lidarLocalTransform = T_base_lidar (base_link -> lidar_link)
// LIO-SAM's imuConverter() expects T_lidar_imu:
// T_lidar_imu = T_base_lidar^{-1} * T_base_imu
if(!imuLocalTransform.isNull() && !lidarLocalTransform.isNull())
{
Transform T_lidar_imu = lidarLocalTransform.inverse() * imuLocalTransform;
Eigen::Matrix4d T = T_lidar_imu.toEigen4d();
Eigen::Matrix3d rot = T.block<3,3>(0,0);
Eigen::Vector3d trans = T.block<3,1>(0,3);
config.extRotV = {rot(0,0), rot(0,1), rot(0,2),
rot(1,0), rot(1,1), rot(1,2),
rot(2,0), rot(2,1), rot(2,2)};
config.extRPYV = config.extRotV;
config.extTransV = {trans(0), trans(1), trans(2)};
UINFO("LIO-SAM extrinsics (T_lidar_imu) computed from sensor local transforms: %s", T_lidar_imu.prettyPrint().c_str());
}
else if(config.extRotV.size() != 9 || config.extTransV.size() != 3)
{
// No valid extrinsics from sensor data or config file
UERROR("Cannot compute IMU-to-lidar extrinsics: IMU local transform %s, lidar local transform %s. "
"Both must be valid, or the config file must contain valid extrinsics.",
imuLocalTransform.isNull() ? "is null" : "is valid",
lidarLocalTransform.isNull() ? "is null" : "is valid");
return false;
}
else
{
UINFO("Using extrinsics from config file (sensor local transforms not available)");
}
// Set the global extrinsics used by imuConverter
extRot = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >(config.extRotV.data());
extRPY = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >(config.extRPYV.data());
extTrans = Eigen::Map<const Eigen::Matrix<double, 3, 1> >(config.extTransV.data());
extQRPY = Eigen::Quaterniond(extRPY).inverse();
lioSam_ = new lio_sam::LioSamCore(config);
// Replay buffered IMU samples
UINFO("Replaying %d buffered IMU samples into LIO-SAM", (int)imuBuffer_.size());
for(const ImuSample & s : imuBuffer_)
{
lioSam_->addImu(s.stamp, s.acc, s.gyro, s.orientation);
}
imuBuffer_.clear();
return true;
}
#endif
Transform OdometryLIOSAM::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
Transform t;
#ifdef RTABMAP_LIOSAM
UTimer timer;
UTimer timerTotal;
// Handle async IMU data (canProcessAsyncIMU() == true means
// the base class sends IMU-only data directly to computeTransform)
if(!data.imu().empty())
{
Eigen::Quaterniond qd(
data.imu().orientation()[3], // w
data.imu().orientation()[0], // x
data.imu().orientation()[1], // y
data.imu().orientation()[2]); // z
Eigen::Vector3d acc(
data.imu().linearAcceleration()[0],
data.imu().linearAcceleration()[1],
data.imu().linearAcceleration()[2]);
Eigen::Vector3d gyro(
data.imu().angularVelocity()[0],
data.imu().angularVelocity()[1],
data.imu().angularVelocity()[2]);
// Deferred initialization: need both IMU and lidar local transforms
// to compute T_lidar_imu extrinsics for LIO-SAM.
if(!lioSam_)
{
// Cache IMU local transform when first available
if(imuLocalTransform_.isNull() && !data.imu().localTransform().isNull())
{
imuLocalTransform_ = data.imu().localTransform();
}
// Try to initialize if we have both transforms
if(!imuLocalTransform_.isNull() && !data.laserScanRaw().isEmpty() &&
!data.laserScanRaw().localTransform().isNull())
{
if(!init(imuLocalTransform_, data.laserScanRaw().localTransform()))
{
UERROR("Failed to initialize LIO-SAM");
return t;
}
}
else
{
// Buffer IMU until we can initialize
ImuSample s;
s.stamp = data.stamp();
s.acc = acc;
s.gyro = gyro;
s.orientation = qd;
imuBuffer_.push_back(s);
if(data.laserScanRaw().isEmpty())
{
return t;
}
}
}
if(lioSam_)
{
lioSam_->addImu(data.stamp(), acc, gyro, qd);
}
// IMU-only: no pose to return
if(data.laserScanRaw().isEmpty())
{
return t;
}
}
if(!lioSam_)
{
// A scan arrived without IMU in the same message.
// Try to init if the IMU local transform was already cached.
if(!imuLocalTransform_.isNull() && !data.laserScanRaw().isEmpty() &&
!data.laserScanRaw().localTransform().isNull())
{
if(!init(imuLocalTransform_, data.laserScanRaw().localTransform()))
{
UERROR("Failed to initialize LIO-SAM");
return t;
}
}
else
{
UDEBUG("LIO-SAM not yet initialized, waiting for IMU (have=%s) and lidar (need scan) local transforms...",
imuLocalTransform_.isNull() ? "no" : "yes");
return t;
}
}
if(data.laserScanRaw().isEmpty())
{
UERROR("LIO-SAM requires laser scans and the current input is empty. Aborting odometry update...");
return t;
}
else if(data.laserScanRaw().is2d())
{
UERROR("LIO-SAM requires 3D laser scans. Aborting odometry update...");
return t;
}
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999;
if(!lost_)
{
const LaserScan & scan = data.laserScanRaw();
if(scan.format() != LaserScan::kXYZIRT)
{
UERROR("LIO-SAM requires a scan in format %s (got %s). "
"Populate the scan via util3d::laserScanFromPointCloud<PointXYZIRT>() "
"so that per-point ring and time fields are available.",
LaserScan::formatName(LaserScan::kXYZIRT).c_str(),
scan.formatName().c_str());
return t;
}
// Split the kXYZIRT scan into the three parallel buffers LIO-SAM expects.
const int numPoints = scan.size();
const int ringOffset = scan.getRingOffset();
const int timeOffset = scan.getTimeOffset();
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZI>);
laserCloudIn->reserve(numPoints);
std::vector<int> rings;
std::vector<float> times;
rings.reserve(numPoints);
times.reserve(numPoints);
for(int i=0; i<numPoints; ++i)
{
const int row = i / scan.data().cols;
const int col = i - row * scan.data().cols;
const float * ptr = scan.data().ptr<float>(row, col);
pcl::PointXYZI pt;
pt.x = ptr[0];
pt.y = ptr[1];
pt.z = ptr[2];
pt.intensity = ptr[3];
laserCloudIn->push_back(pt);
rings.push_back(static_cast<int>(ptr[ringOffset]));
times.push_back(ptr[timeOffset]);
}
UDEBUG("Scan split: %fs, points=%d", timer.ticks(), (int)laserCloudIn->size());
// Process scan. Retrieve the deskewed (motion-compensated) cloud
// produced by LIO-SAM's image projection stage so we can propagate
// it back into SensorData: otherwise downstream consumers such as
// loop closure registration would still see the raw pre-deskew scan.
Eigen::Affine3f poseOut;
Eigen::MatrixXd covOut;
pcl::PointCloud<pcl::PointXYZI>::Ptr deskewedCloud(new pcl::PointCloud<pcl::PointXYZI>);
bool ok = lioSam_->processScan(data.stamp(), laserCloudIn, rings, times, poseOut, covOut, deskewedCloud);
UDEBUG("LIO-SAM process: %fs", timer.ticks());
if(ok)
{
// Replace the raw scan on SensorData with LIO-SAM's deskewed
// cloud so downstream stages (loop closure registration in
// particular) use the motion-compensated points instead of
// the raw pre-deskew scan. The deskewed cloud is still in the
// lidar frame, so the existing localTransform/rangeMax apply.
if(deskewedCloud && !deskewedCloud->empty())
{
const LaserScan & rawScan = data.laserScanRaw();
LaserScan deskewedScan(
util3d::laserScanFromPointCloud(*deskewedCloud),
rawScan.maxPoints(),
rawScan.rangeMax(),
rawScan.localTransform());
data.setLaserScan(deskewedScan);
UDEBUG("Replaced raw scan with deskewed cloud (%d -> %d points)",
(int)laserCloudIn->size(), (int)deskewedCloud->size());
}
Transform pose = Transform::fromEigen3f(poseOut);
if(!pose.isNull())
{
covariance = cv::Mat::eye(6, 6, CV_64FC1);
covariance(cv::Range(0, 3), cv::Range(0, 3)) *= linVar_;
covariance(cv::Range(3, 6), cv::Range(3, 6)) *= angVar_;
t = lastPose_.inverse() * pose; // incremental
lastPose_ = pose;
const Transform & localTransform = data.laserScanRaw().localTransform();
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from laser frame to base frame
t = localTransform * t * localTransform.inverse();
}
if(info)
{
info->type = (int)kTypeLIOSAM;
if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
{
info->reg.covariance = covariance;
}
if(this->isInfoDataFilled())
{
pcl::PointCloud<pcl::PointXYZI>::Ptr localMap = lioSam_->getLocalMap();
if(localMap && !localMap->empty())
{
info->localScanMapSize = localMap->size();
info->localScanMap = LaserScan(util3d::laserScanFromPointCloud(*localMap), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
}
UDEBUG("Fill info data: %fs", timer.ticks());
}
}
}
else
{
lost_ = true;
UWARN("LIO-SAM failed to register the latest scan, odometry should be reset.");
}
}
else
{
UDEBUG("LIO-SAM processScan returned false (may be initializing)");
}
}
UINFO("LIO-SAM odom update time = %fs, lost=%s", timerTotal.elapsed(), lost_ ? "true" : "false");
#else
UERROR("RTAB-Map is not built with LIO-SAM support! Select another odometry approach.");
#endif
return t;
}
} // namespace rtabmap
corelib/src/odometry/OdometryMono.cpp
+6 -6
View File
@@ -239,7 +239,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
{
UDEBUG("");
bool newPtsAdded = false;
const Signature * newS = memory_->getLastWorkingSignature();
const Signature * newS = memory_->getLastWorkingSignature(false);
UDEBUG("newWords=%d", (int)newS->getWords().size());
nFeatures = (int)newS->getWords().size();
if((int)newS->getWords().size() > minInliers_)
@@ -646,7 +646,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
info->type = 1;
}
const Signature * refS = memory_->getLastWorkingSignature();
const Signature * refS = memory_->getLastWorkingSignature(false);
std::vector<cv::Point2f> refCorners(firstFrameGuessCorners_.size());
std::vector<cv::Point2f> refCornersGuess(firstFrameGuessCorners_.size());
@@ -804,10 +804,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
if(!refWords3.empty())
{
UDEBUG("Added %d/%d valid 3D features", (int)refWords3.size(), (int)localMap_.size());
keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), refWords3));
keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), refWords3));
}
keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), newModel));
keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), this->getPose()));
keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), newModel));
}
}
else
@@ -829,7 +829,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
// generate kpts
if(memory_->update(SensorData(data)))
{
const Signature * s = memory_->getLastWorkingSignature();
const Signature * s = memory_->getLastWorkingSignature(false);
const std::multimap<int, int> & words = s->getWords();
if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
{
corelib/src/optimizer/OptimizerGTSAM.cpp
+20 -13
View File
@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include "gtsam/GravityFactor.h"
#include <gtsam/navigation/AttitudeFactor.h>
#include <optimizer/gtsam/XYFactor.h>
#include <optimizer/gtsam/XYZFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
@@ -121,7 +121,7 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
params.relinearizeThreshold = threshold;
params.relinearizeSkip = skip;
params.evaluateNonlinearError = true;
isam2_ = new ISAM2(params);
isam2_ = new gtsam::ISAM2(params);
addedPoses_.clear();
lastAddedConstraints_.clear();
@@ -379,8 +379,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
int id1 = iter->second.from();
int id2 = iter->second.to();
UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str());
UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str());
UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d for constraint %d->%d (type=%d)", id1, id1, id2, iter->second.type()).c_str());
UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d for constraint %d->%d (type=%d)", id2, id1, id2, iter->second.type()).c_str());
UASSERT(!iter->second.transform().isNull());
if(id1 == id2)
@@ -392,7 +392,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
@@ -400,7 +400,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
@@ -431,7 +431,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
@@ -442,7 +442,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
@@ -471,10 +471,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end())
{
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma()));
graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
gtsam::Rot3 nRbMeas = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation();
gtsam::Unit3 nZ(0,0,1);
gtsam::Unit3 bGMeas = nRbMeas.unrotate(nZ);
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(2, gravitySigma());
#if GTSAM_VERSION_NUMERIC <= 40300
// Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here.
// Just replace "<=" above by "<" to use AttitudeFactor<Pose3> below.
graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas));
#else
graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));
#endif
lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
}
}
@@ -783,7 +790,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
float x,y,z,roll,pitch,yaw;
std::map<int, Transform> tmpPoses;
const Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
#if GTSAM_VERSION_NUMERIC >= 40200
for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter)
#else
corelib/src/optimizer/gtsam/GravityFactor.cpp
-91
View File
@@ -1,91 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* Author: Mathieu Labbe
* This file is a copy of AttitudeFactor.cpp of gtsam library but
* with attitudeError() function overridden to ignore yaw errors.
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
*/
/**
* @file GravityFactor.cpp
* @author Frank Dellaert
* @brief Implementation file for Attitude factor
* @date January 28, 2014
**/
#include "GravityFactor.h"
using namespace std;
namespace rtabmap {
//***************************************************************************
Vector GravityFactor::attitudeError(const Rot3& nRb,
OptionalJacobian<2, 3> H) const {
if (H) {
Matrix23 D_nRef_R;
Matrix22 D_e_nRef;
Vector3 r = nRb.xyz();
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
Vector e = nZ_.error(nRef, D_e_nRef);
(*H) = D_e_nRef * D_nRef_R;
//printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n",
// nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1),
// (*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2));
return e;
} else {
Vector3 r = nRb.xyz();
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
Vector e = nZ_.error(nRef);
//printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1));
return e;
}
}
//***************************************************************************
void Rot3GravityFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool Rot3GravityFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}
//***************************************************************************
void Pose3GravityFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool Pose3GravityFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}
//***************************************************************************
}/// namespace gtsam
corelib/src/optimizer/gtsam/GravityFactor.h
-271
View File
@@ -1,271 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* Author: Mathieu Labbe
* This file is a copy of AttitudeFactor.h of gtsam library but
* with attitudeError() function overridden to ignore yaw errors.
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
*/
/**
* @file Pose3GravityFactor.h
* @author Frank Dellaert
* @brief Header file for Attitude factor
* @date January 28, 2014
**/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#if GTSAM_VERSION_NUMERIC >= 40300 && defined(GTSAM_WITH_NOISE_MODEL_FACTOR_N)
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#endif
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
using namespace gtsam;
namespace rtabmap {
/**
* Base class for prior on gravity
* Example:
* - measurement is direction of gravity in navigation frame nG
* - reference is direction of z axis in body frame bF
* This factor will give zero error if nG is opposite direction of bF
* @addtogroup Navigation
*/
class GravityFactor {
protected:
const Unit3 nZ_, bRef_; ///< Position measurement in
public:
/** default constructor - only use for serialization */
GravityFactor() {
}
/**
* @brief Constructor
* @param nZ measured direction in navigation frame
* @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1])
*/
GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
nZ_(nZ), bRef_(bRef) {
}
/** vector of errors */
Vector attitudeError(const Rot3& p,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalJacobian<2,3> H = {}) const;
#else
OptionalJacobian<2,3> H = boost::none) const;
#endif
/** Serialization function */
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
/*ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));*/
}
#endif
};
/**
* Version of GravityFactor for Rot3
* @addtogroup Navigation
*/
class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
typedef NoiseModelFactor1<Rot3> Base;
public:
/// shorthand for a smart pointer to a factor
#if GTSAM_VERSION_NUMERIC >= 40300
typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
#else
typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
#endif
/// Typedef to this class
typedef Rot3GravityFactor This;
/** default constructor - only use for serialization */
Rot3GravityFactor() {
}
virtual ~Rot3GravityFactor() {
}
/**
* @brief Constructor
* @param key of the Rot3 variable that will be constrained
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
* @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
*/
Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
const Unit3& bRef = Unit3(0, 0, 1)) :
Base(model, key), GravityFactor(nZ, bRef) {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
#if GTSAM_VERSION_NUMERIC >= 40300
return std::static_pointer_cast<gtsam::NonlinearFactor>(
#else
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
#endif
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** vector of errors */
virtual Vector evaluateError(const Rot3& nRb, //
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<Matrix&> H = boost::none) const {
#endif
return attitudeError(nRb, H);
}
Unit3 nZ() const {
return nZ_;
}
Unit3 bRef() const {
return bRef_;
}
private:
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
/*ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("GravityFactor",
boost::serialization::base_object<GravityFactor>(*this));*/
}
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* Version of GravityFactor for Pose3
* @addtogroup Navigation
*/
class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
public GravityFactor {
typedef NoiseModelFactor1<Pose3> Base;
public:
/// shorthand for a smart pointer to a factor
#if GTSAM_VERSION_NUMERIC >= 40300
typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
#else
typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
#endif
/// Typedef to this class
typedef Pose3GravityFactor This;
/** default constructor - only use for serialization */
Pose3GravityFactor() {
}
virtual ~Pose3GravityFactor() {
}
/**
* @brief Constructor
* @param key of the Pose3 variable that will be constrained
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
* @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
*/
Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
const Unit3& bRef = Unit3(0, 0, 1)) :
Base(model, key), GravityFactor(nZ, bRef) {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
#if GTSAM_VERSION_NUMERIC >= 40300
return std::static_pointer_cast<gtsam::NonlinearFactor>(
#else
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
#endif
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** vector of errors */
virtual Vector evaluateError(const Pose3& nTb, //
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<Matrix&> H = boost::none) const {
#endif
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;
*H = Matrix::Zero(2,6);
H->block<2,3>(0,0) = H23;
}
return e;
}
Unit3 nZ() const {
return nZ_;
}
Unit3 bRef() const {
return bRef_;
}
private:
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
/*ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("GravityFactor",
boost::serialization::base_object<GravityFactor>(*this));*/
}
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} /// namespace gtsam
corelib/src/optimizer/gtsam/XYFactor.h
+2 -2
View File
@@ -43,7 +43,7 @@ public:
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const VALUE& p,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
gtsam::OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H = boost::none) const {
#endif
@@ -59,5 +59,5 @@ public:
};
} // namespace gtsamexamples
} // namespace rtabmap
corelib/src/optimizer/gtsam/XYZFactor.h
+3 -3
View File
@@ -43,7 +43,7 @@ public:
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose3& p,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
gtsam::OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H = boost::none) const {
#endif
@@ -55,7 +55,7 @@ public:
}
gtsam::Vector evaluateError(const gtsam::Point3& p,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
gtsam::OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H = boost::none) const {
#endif
@@ -63,5 +63,5 @@ public:
}
};
} // namespace gtsamexamples
} // namespace rtabmap
corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h
+6 -6
View File
@@ -31,9 +31,9 @@ namespace vertigo {
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const
gtsam::OptionalMatrixType H1 = OptionalNone,
gtsam::OptionalMatrixType H2 = OptionalNone,
gtsam::OptionalMatrixType H3 = OptionalNone) const
#else
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
@@ -71,9 +71,9 @@ namespace vertigo {
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const
gtsam::OptionalMatrixType H1 = OptionalNone,
gtsam::OptionalMatrixType H2 = OptionalNone,
gtsam::OptionalMatrixType H3 = OptionalNone) const
#else
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h
+3 -2
View File
@@ -13,6 +13,7 @@
// DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
#include "DerivedValue.h"
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace vertigo {
@@ -77,8 +78,8 @@ namespace vertigo {
/** between operation */
inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const {
gtsam::OptionalMatrixType H1=OptionalNone,
gtsam::OptionalMatrixType H2=OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h
+3 -2
View File
@@ -13,6 +13,7 @@
// DerivedValue.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
#include "DerivedValue.h"
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace vertigo {
@@ -77,8 +78,8 @@ namespace vertigo {
/** between operation */
inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const {
gtsam::OptionalMatrixType H1=OptionalNone,
gtsam::OptionalMatrixType H2=OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
corelib/src/python/PythonInterface.cpp
+2
View File
@@ -19,9 +19,11 @@ PythonInterface::PythonInterface()
guard_ = new pybind11::scoped_interpreter();
// Tell Python to look in this directory for DLLs
#ifdef _WIN32
std::string exe_dir = std::filesystem::current_path().string();
pybind11::module_ os = pybind11::module_::import("os");
os.attr("add_dll_directory")(exe_dir);
#endif
pybind11::module::import("threading");
release_ = new pybind11::gil_scoped_release();
corelib/src/util3d.cpp
+75 -7
View File
@@ -1699,6 +1699,55 @@ LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
LaserScan laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const Transform & transform, bool filterNaNs)
{
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
LaserScan laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
// Layout: [x, y, z, intensity, ring, time] (ring cast to float, values up to
// ~16M are exactly representable so all realistic laser line counts fit).
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
int oi = 0;
const int total = indices.get() ? (int)indices->size() : (int)cloud.size();
laserScan = cv::Mat(1, total, CV_32FC(6));
for(int i=0; i<total; ++i)
{
int index = indices.get() ? indices->at(i) : i;
const rtabmap::PointXYZIRT & src = cloud.at(index);
if(filterNaNs && !pcl::isFinite(src))
{
continue;
}
float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform)
{
pcl::PointXYZ pt(src.x, src.y, src.z);
pt = pcl::transformPoint(pt, transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
ptr[0] = src.x;
ptr[1] = src.y;
ptr[2] = src.z;
}
ptr[3] = src.intensity;
ptr[4] = static_cast<float>(src.ring);
ptr[5] = src.time;
}
if(oi == 0)
{
return LaserScan();
}
return LaserScan(laserScan(cv::Range::all(), cv::Range(0, oi)), 0, 0.0f, LaserScan::kXYZIRT);
}
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
cv::Mat laserScan;
@@ -2280,7 +2329,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
{
pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
}
else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT)
else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
{
pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
}
@@ -3738,9 +3787,11 @@ LaserScan deskew(
return LaserScan();
}
if(input.format() != LaserScan::kXYZIT)
if(!input.hasTime())
{
UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str());
UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".",
LaserScan::formatName(LaserScan::kXYZIT).c_str(),
LaserScan::formatName(LaserScan::kXYZIRT).c_str());
return LaserScan();
}
@@ -3796,7 +3847,14 @@ LaserScan deskew(
double stamp;
UTimer processingTime;
double scanTime = lastStamp - firstStamp;
cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense
// Preserve ring when input carries it (kXYZIRT): the geometric channel is
// still meaningful after deskewing. Per-point time is zeroed because all
// points share the same pose after correction.
const bool preserveRing = input.hasRing();
const int offsetRing = input.getRingOffset();
const LaserScan::Format outputFormat = preserveRing ? LaserScan::kXYZIRT : LaserScan::kXYZI;
const int outputChannels = preserveRing ? 6 : 4;
cv::Mat output(1, input.size(), CV_32FC(outputChannels));
int offsetIntensity = input.getIntensityOffset();
bool isLocalTransformIdentity = input.localTransform().isIdentity();
Transform localTransformInv = input.localTransform().inverse();
@@ -3835,7 +3893,12 @@ LaserScan deskew(
dataPtr[0] = pt.x;
dataPtr[1] = pt.y;
dataPtr[2] = pt.z;
dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
dataPtr[3] = inputPtr[offsetIntensity];
if(preserveRing)
{
dataPtr[4] = inputPtr[offsetRing];
dataPtr[5] = 0.0f;
}
}
}
}
@@ -3872,14 +3935,19 @@ LaserScan deskew(
dataPtr[0] = pt.x;
dataPtr[1] = pt.y;
dataPtr[2] = pt.z;
dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
dataPtr[3] = inputPtr[offsetIntensity];
if(preserveRing)
{
dataPtr[4] = inputPtr[offsetRing];
dataPtr[5] = 0.0f;
}
}
}
}
}
output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform());
return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform());
}