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

Added stereo multi-camera support (#884)

Browse Source 
* Integrated OpenGV

* Fixed build without opengv

* Cmake: moved OpenGV dependency status under solvers group

* Added multi-stereocamera models support

* Fixed OpenGV 0 sample error when one of the camera doesn't have features. Fixed g2o BA id offset with multi-camera.

* Fixed multicam 3d points generated from stereo correspondences

* db: Fixed multi stereo models not loaded correctly

* gui: fixed stereo rectification option, RegVis: fixed projection error with old databases (image size not set in calibration)

* OdomF2M: Fixed map.at error when bundle adjustment is not used

* depthai: added imu firmware update option for convenience

* Fixed various refactor errors

* Moved "large number stereo correspondences rejected" warning outside computeCorrespondences function for multicam

* Added error log if ba correspondences are computed with empty signatures

* fixed compilation errors with latest opencv

Co-authored-by: mathieu86 <mathieu@robust.ai>
This commit is contained in:
matlabbe
2022-07-20 15:20:14 -04:00
committed by GitHub
parent 71a28bb570
commit 5943a8b065
64 changed files with 2205 additions and 1010 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -167,7 +167,7 @@ public:
void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
bool getCalibration(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
bool getLaserScanInfo(int signatureId, LaserScan & info) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -272,7 +272,7 @@ protected:
virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const = 0;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;

2
corelib/include/rtabmap/core/DBDriverSqlite3.h
View File

@@ -137,7 +137,7 @@ protected:
virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
virtual void getLastNodeIdsQuery(std::set<int> & ids) const;

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

@@ -206,7 +206,7 @@ public:
std::vector<GlobalDescriptor> & globalDescriptors) const;
void getNodeCalibration(int nodeId,
std::vector<CameraModel> & models,
StereoCameraModel & stereoModel) const;
std::vector<StereoCameraModel> & stereoModels) const;
std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
bool memoryChanged() const {return _memoryChanged;}
bool isIncremental() const {return _incrementalMemory;}
@@ -348,7 +348,7 @@ private:
bool _allNodesInWM;
GPS _gpsOrigin;
std::vector<CameraModel> _rectCameraModels;
StereoCameraModel _rectStereoCameraModel;
std::vector<StereoCameraModel> _rectStereoCameraModels;
std::vector<double> _odomMaxInf;
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...

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

@@ -122,7 +122,7 @@ private:
std::vector<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_;
StereoCameraModel stereoModel_;
std::vector<StereoCameraModel> stereoModels_;
std::vector<CameraModel> models_;
std::map<double, Transform> imus_;

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

@@ -103,7 +103,7 @@ public:
int localBundleConstraints;
float localBundleTime;
std::map<int, Transform> localBundlePoses;
std::map<int, CameraModel> localBundleModels;
std::map<int, std::vector<CameraModel> > localBundleModels;
bool keyFrameAdded;
float timeEstimation;
float timeParticleFiltering;

12
corelib/include/rtabmap/core/Optimizer.h
View File

@@ -41,14 +41,18 @@ namespace rtabmap {
class FeatureBA
{
public:
FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat()):
FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat(), int cameraIndexIn = 0):
kpt(kptIn),
depth(depthIn),
descriptor(descriptorIn)
{}
descriptor(descriptorIn),
cameraIndex(cameraIndexIn)
{
//UDEBUG("kpt=(%f,%f) depth=%f, camIndex=%d", kpt.pt.x, kpt.pt.y, depth, cameraIndex);
}
cv::KeyPoint kpt;
float depth;
cv::Mat descriptor;
int cameraIndex;
};
////////////////////////////////////////////
@@ -134,7 +138,7 @@ public:
int rootId, // if negative, all other poses are fixed
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
std::set<int> * outliers = 0);

31
corelib/include/rtabmap/core/SensorData.h
View File

@@ -126,6 +126,25 @@ public:
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Multi-cameras stereo constructor
SensorData(
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<StereoCameraModel> & cameraModels,
int id = 0,
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Multi-cameras stereo constructor + laser scan
SensorData(
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<StereoCameraModel> & cameraModels,
int id = 0,
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// IMU constructor
SensorData(
const IMU & imu,
@@ -143,8 +162,8 @@ public:
_depthOrRightCompressed.empty() &&
_laserScanRaw.isEmpty() &&
_laserScanCompressed.isEmpty() &&
_cameraModels.size() == 0 &&
!_stereoCameraModel.isValidForProjection() &&
_cameraModels.empty() &&
_stereoCameraModels.empty() &&
_userDataRaw.empty() &&
_userDataCompressed.empty() &&
_keypoints.size() == 0 &&
@@ -173,6 +192,7 @@ public:
void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const std::vector<CameraModel> & models, bool clearPreviousData = true);
void setStereoImage(const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & stereoCameraModel, bool clearPreviousData = true);
void setStereoImage(const cv::Mat & left, const cv::Mat & right, const std::vector<StereoCameraModel> & stereoCameraModels, bool clearPreviousData = true);
/**
* Set laser scan data. Detect automatically if raw or compressed.
@@ -183,7 +203,8 @@ public:
void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModels.clear(); _stereoCameraModels.push_back(stereoCameraModel);}
void setStereoCameraModels(const std::vector<StereoCameraModel> & stereoCameraModels) {_stereoCameraModels = stereoCameraModels;}
//for convenience
cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
@@ -213,7 +234,7 @@ public:
cv::Mat * emptyCellsRaw = 0) const;
const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
const std::vector<StereoCameraModel> & stereoCameraModels() const {return _stereoCameraModels;}
/**
* Set user data. Detect automatically if raw or compressed. If raw, the data is
@@ -302,7 +323,7 @@ private:
LaserScan _laserScanRaw;
std::vector<CameraModel> _cameraModels;
StereoCameraModel _stereoCameraModel;
std::vector<StereoCameraModel> _stereoCameraModels;
// user data
cv::Mat _userDataCompressed; // compressed data

2
corelib/include/rtabmap/core/Transform.h
View File

@@ -143,6 +143,8 @@ public:
static Transform fromEigen3d(const Eigen::Affine3d & matrix);
static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
static Transform fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix);
static Transform fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix);
static Transform opengl_T_rtabmap() {return Transform(
0.0f, -1.0f, 0.0f, 0.0f,

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

@@ -58,6 +58,7 @@ public:
virtual ~CameraDepthAI();
void setOutputDepth(bool enabled, int confidence = 200);
void setIMUFirmwareUpdate(bool enabled);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
@@ -74,6 +75,7 @@ private:
bool outputDepth_;
int depthConfidence_;
int resolution_;
bool imuFirmwareUpdate_;
std::shared_ptr<dai::Device> device_;
std::shared_ptr<dai::DataOutputQueue> leftQueue_;
std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;

2
corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

@@ -83,7 +83,7 @@ private:
std::map<int, Transform> bundlePoses_;
std::multimap<int, Link> bundleLinks_;
std::multimap<int, Link> bundleIMUOrientations_;
std::map<int, CameraModel> bundleModels_;
std::map<int, std::vector<CameraModel> > bundleModels_;
std::map<int, int> bundlePoseReferences_;
int bundleSeq_;
Optimizer * sba_;

2
corelib/include/rtabmap/core/odometry/OdometryMono.h
View File

@@ -73,7 +73,7 @@ private:
std::map<int, std::map<int, cv::Point3f> > keyFrameWords3D_;
std::map<int, Transform> keyFramePoses_;
std::multimap<int, Link> keyFrameLinks_;
std::map<int, CameraModel> keyFrameModels_;
std::map<int, std::vector<CameraModel> > keyFrameModels_;
float maxVariance_;
float keyFrameThr_;
};

2
corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h
View File

@@ -55,7 +55,7 @@ public:
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
const std::map<int, std::vector<CameraModel> > & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0);

2
corelib/include/rtabmap/core/optimizer/OptimizerG2O.h
View File

@@ -62,7 +62,7 @@ public:
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0);

15
corelib/include/rtabmap/core/util3d_motion_estimation.h
View File

@@ -53,6 +53,21 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
cv::Mat * covariance = 0, // mean reproj error if words3B is not set
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
Transform RTABMAP_EXP estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const std::vector<CameraModel> & cameraModels,
int minInliers = 10,
int iterations = 100,
double reprojError = 5.,
int flagsPnP = 0,
int pnpRefineIterations = 1,
const Transform & guess = Transform::getIdentity(),
const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
cv::Mat * covariance = 0, // mean reproj error if words3B is not set
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
Transform RTABMAP_EXP estimateMotion3DTo3D(
const std::map<int, cv::Point3f> & words3A,

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

@@ -93,6 +93,9 @@ pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP transformPointCloud(
cv::Point3f RTABMAP_EXP transformPoint(
const cv::Point3f & pt,
const Transform & transform);
cv::Point3d RTABMAP_EXP transformPoint(
const cv::Point3d & pt,
const Transform & transform);
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform);