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

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

Browse Source
This commit is contained in:
matlabbe
2025-03-09 15:25:05 -07:00
parent cb3637b6db 6eebca83b2
commit 8fa82faf94
45 changed files with 2012 additions and 1354 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/docker.yml
+19 -9
View File
@@ -7,6 +7,16 @@ on:
jobs:
docker_deps:
# Disabling ###-deps step from CI because it is too flaky (seg faults, arm64 build timeout...)
# Only way I was able to build all images is to do it from a ubuntu 20.04 computer with:
# $ sudo add-apt-repository ppa:canonical-server/server-backports
# $ sudo apt-get update
# $ sudo apt-get upgrade qemu-user-static
# $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes
# More info: https://github.com/introlab/rtabmap/issues/1454
if: false
runs-on: ubuntu-latest
strategy:
@@ -49,21 +59,21 @@ jobs:
uses: actions/checkout@v2
-
name: Set up QEMU
uses: docker/setup-qemu-action@v1
uses: docker/setup-qemu-action@v3
with:
platforms: all
-
name: Set up Docker Buildx
uses: docker/setup-buildx-action@v1
uses: docker/setup-buildx-action@v3
-
name: Login to DockerHub
uses: docker/login-action@v1
uses: docker/login-action@v3
with:
username: ${{ secrets.DOCKERHUB_USERNAME }}
password: ${{ secrets.DOCKERHUB_TOKEN }}
-
name: Build and push
uses: docker/build-push-action@v2
uses: docker/build-push-action@v6
with:
context: .
push: true
@@ -74,7 +84,7 @@ jobs:
cache-to: type=inline
docker:
needs: docker_deps
#needs: docker_deps
runs-on: ubuntu-latest
strategy:
@@ -172,21 +182,21 @@ jobs:
uses: actions/checkout@v2
-
name: Set up QEMU
uses: docker/setup-qemu-action@v1
uses: docker/setup-qemu-action@v3
with:
platforms: all
-
name: Set up Docker Buildx
uses: docker/setup-buildx-action@v1
uses: docker/setup-buildx-action@v3
-
name: Login to DockerHub
uses: docker/login-action@v1
uses: docker/login-action@v3
with:
username: ${{ secrets.DOCKERHUB_USERNAME }}
password: ${{ secrets.DOCKERHUB_TOKEN }}
-
name: Build and push
uses: docker/build-push-action@v2
uses: docker/build-push-action@v6
with:
context: .
push: true
CMakeLists.txt
+2 -2
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 21)
SET(RTABMAP_PATCH_VERSION 10)
SET(RTABMAP_PATCH_VERSION 11)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
@@ -208,7 +208,7 @@ option(WITH_MYNTEYE "Include mynteye-s support" ON)
option(WITH_DEPTHAI "Include depthai-core support" OFF)
option(WITH_XVSDK "Include XVisio SDK support" OFF)
option(WITH_OCTOMAP "Include OctoMap support" ON)
option(WITH_GRIDMAP "Include GridMap support" ON)
option(WITH_GRIDMAP "Include GridMap support" OFF)
option(WITH_CPUTSDF "Include CPUTSDF support" OFF)
option(WITH_OPENCHISEL "Include open_chisel support" OFF)
option(WITH_ALICE_VISION "Include AliceVision support" OFF)
README.md
+2 -2
View File
@@ -64,8 +64,8 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
<td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
</tr>
<tr>
<td>Iron</td>
<td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
<td>Jazzy</td>
<td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
</tr>
<tr>
<td>Rolling</td>
app/ios/RTABMapApp.xcodeproj/project.pbxproj
+4
View File
@@ -157,6 +157,7 @@
/* End PBXBuildFile section */
/* Begin PBXFileReference section */
44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */ = {isa = PBXFileReference; lastKnownFileType = text.plist.entitlements; name = RTABMapApp.entitlements; path = RTABMapApp/RTABMapApp.entitlements; sourceTree = "<group>"; };
4E0D83822621F52C00C879AC /* Settings.bundle */ = {isa = PBXFileReference; lastKnownFileType = "wrapper.plug-in"; path = Settings.bundle; sourceTree = "<group>"; };
4E1E9CF22D617D08000CB881 /* Measure.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = Measure.h; path = ../android/jni/Measure.h; sourceTree = "<group>"; };
4E1E9CF32D618079000CB881 /* quad_color.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = quad_color.h; path = ../android/jni/quad_color.h; sourceTree = "<group>"; };
@@ -483,6 +484,7 @@
4EE01634259BDCC7008CCE65 /* RTABMapApp */ = {
isa = PBXGroup;
children = (
44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */,
4E20B24F266AB95600316EE6 /* Images.xcassets */,
4EFD0BB0259D503200575D88 /* NativeWrapper */,
4EFD0B62259D501E00575D88 /* tango-gl */,
@@ -1002,6 +1004,7 @@
ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
CLANG_CXX_LIBRARY = "libc++";
CLANG_USE_OPTIMIZATION_PROFILE = NO;
CODE_SIGN_ENTITLEMENTS = RTABMapApp/RTABMapApp.entitlements;
CODE_SIGN_IDENTITY = "Apple Development";
CODE_SIGN_STYLE = Automatic;
CURRENT_PROJECT_VERSION = 1;
@@ -1059,6 +1062,7 @@
ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
CLANG_CXX_LIBRARY = "libc++";
CLANG_USE_OPTIMIZATION_PROFILE = NO;
CODE_SIGN_ENTITLEMENTS = RTABMapApp/RTABMapApp.entitlements;
CODE_SIGN_IDENTITY = "Apple Development";
CODE_SIGN_STYLE = Automatic;
CURRENT_PROJECT_VERSION = 1;
app/ios/RTABMapApp/RTABMapApp.entitlements
+8
View File
@@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
<plist version="1.0">
<dict>
<key>com.apple.developer.kernel.increased-memory-limit</key>
<true/>
</dict>
</plist>
corelib/include/rtabmap/core/DBReader.h
+3 -3
View File
@@ -51,7 +51,7 @@ public:
bool ignoreGoalDelay = false,
bool goalsIgnored = false,
int startId = 0,
int cameraIndex = -1,
const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
int stopId = 0,
bool intermediateNodesIgnored = false,
bool landmarksIgnored = false,
@@ -65,7 +65,7 @@ public:
bool ignoreGoalDelay = false,
bool goalsIgnored = false,
int startId = 0,
int cameraIndex = -1,
const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
int stopId = 0,
bool intermediateNodesIgnored = false,
bool landmarksIgnored = false,
@@ -99,7 +99,7 @@ private:
bool _goalsIgnored;
int _startId;
int _stopId;
int _cameraIndex;
std::vector<unsigned int> _cameraIndices;
bool _intermediateNodesIgnored;
bool _landmarksIgnored;
bool _featuresIgnored;
corelib/include/rtabmap/core/OdometryInfo.h
+6
View File
@@ -49,6 +49,8 @@ public:
localBundleOutliers(0),
localBundleConstraints(0),
localBundleTime(0),
localBundleAvgInlierDistance(0.0f),
localBundleMaxKeyFramesForInlier(0),
keyFrameAdded(false),
timeDeskewing(0.0f),
timeEstimation(0.0f),
@@ -76,6 +78,8 @@ public:
output.localBundleTime = localBundleTime;
output.localBundlePoses = localBundlePoses;
output.localBundleModels = localBundleModels;
output.localBundleAvgInlierDistance = localBundleAvgInlierDistance;
output.localBundleMaxKeyFramesForInlier = localBundleMaxKeyFramesForInlier;
output.keyFrameAdded = keyFrameAdded;
output.timeDeskewing = timeDeskewing;
output.timeEstimation = timeEstimation;
@@ -106,6 +110,8 @@ public:
float localBundleTime;
std::map<int, Transform> localBundlePoses;
std::map<int, std::vector<CameraModel> > localBundleModels;
float localBundleAvgInlierDistance;
int localBundleMaxKeyFramesForInlier;
bool keyFrameAdded;
float timeDeskewing;
float timeEstimation;
corelib/include/rtabmap/core/Parameters.h
+5
View File
@@ -477,6 +477,8 @@ class RTABMAP_CORE_EXPORT Parameters
// Odometry Frame-to-Map
RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
RTABMAP_PARAM(OdomF2M, MaxNewFeatures, int, 0, "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
RTABMAP_PARAM(OdomF2M, InitDepthFactor, float, 0.05, "[Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx.");
RTABMAP_PARAM(OdomF2M, FloorThreshold, float, 0.0, "[Visual] Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled.");
RTABMAP_PARAM(OdomF2M, ScanMaxSize, int, 2000, "[Geometry] Maximum local scan map size.");
RTABMAP_PARAM(OdomF2M, ScanSubtractRadius, float, 0.05, "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
@@ -488,6 +490,9 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#endif
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMinMotion, float, 0.0, "To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame.");
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxKeyFramesPerFeature, int, 0, "Maximum keyframes per feature for bundle adjustment. 0 means not limit.");
RTABMAP_PARAM(OdomF2M, BundleUpdateFeatureMapOnAllFrames, bool, false, uFormat("Update 3D local feature map on every frame with bundle adjustment. Recommended if %s=false and %s=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).", kVisDepthAsMask().c_str(), kMemUseOdomFeatures().c_str()));
// Odometry Mono
RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step.");
corelib/include/rtabmap/core/SensorData.h
+3 -3
View File
@@ -207,8 +207,8 @@ public:
void setStereoCameraModels(const std::vector<StereoCameraModel> & stereoCameraModels) {_stereoCameraModels = stereoCameraModels;}
//for convenience
cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
cv::Mat depthRaw() const {return !(_depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3) ? _depthOrRightRaw : cv::Mat();}
cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3 ? _depthOrRightRaw : cv::Mat();}
// Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
RTABMAP_DEPRECATED void setImageRaw(const cv::Mat & image);
@@ -330,7 +330,7 @@ private:
LaserScan _laserScanCompressed; // compressed data
cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthOrRightRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
cv::Mat _depthOrRightRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 or CV_8UC3
LaserScan _laserScanRaw;
std::vector<CameraModel> _cameraModels;
corelib/include/rtabmap/core/camera/CameraDepthAI.h
+12 -11
View File
@@ -50,21 +50,20 @@ public:
public:
CameraDepthAI(
const std::string & mxidOrName = "",
int resolution = 1, // 0=720p, 1=800p, 2=400p
float imageRate=0.0f,
int imageWidth = 1280, // 640 or 1280
float imageRate = 0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraDepthAI();
void setOutputMode(int outputMode = 0);
void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
void setExtendedDisparity(bool extendedDisparity);
void setSubpixelMode(bool enabled, int fractionalBits = 3);
void setCompanding(bool enabled, int width=96);
void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f, bool enabled=true);
void setExtendedDisparity(bool extendedDisparity = false, bool enableCompanding = false);
void setSubpixelMode(bool enabled = false, int fractionalBits = 3);
void setDisparityWidthAndFilter(int disparityWidth = 96, int medianFilter = 5);
void setRectification(bool useSpecTranslation = false, float alphaScaling = 0.0f, bool enabled = true);
void setIMU(bool imuPublished, bool publishInterIMU);
void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f);
void setDetectFeatures(int detectFeatures = 0);
void setBlobPath(const std::string & blobPath);
void setDetectFeatures(int detectFeatures = 0, const std::string & blobPath = "");
void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
void setSuperPointDetector(float threshold = 0.01f, bool nms = true, int nmsRadius = 4);
@@ -84,10 +83,12 @@ private:
int outputMode_;
int confThreshold_;
int lrcThreshold_;
int resolution_;
int imageWidth_;
bool extendedDisparity_;
bool enableCompanding_;
int subpixelFractionalBits_;
int compandingWidth_;
int disparityWidth_;
int medianFilter_;
bool useSpecTranslation_;
float alphaScaling_;
bool imagesRectified_;
@@ -103,7 +104,7 @@ private:
bool nms_;
int nmsRadius_;
std::string blobPath_;
std::shared_ptr<dai::Device> device_;
std::unique_ptr<dai::Device> device_;
std::shared_ptr<dai::DataOutputQueue> cameraQueue_;
std::map<double, cv::Vec3f> accBuffer_;
std::map<double, cv::Vec3f> gyroBuffer_;
corelib/include/rtabmap/core/camera/CameraStereoImages.h
+3
View File
@@ -56,6 +56,8 @@ public:
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoImages();
void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;}
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
@@ -69,6 +71,7 @@ protected:
private:
CameraImages * camera2_;
StereoCameraModel stereoModel_;
bool rightGrayScale_;
};
corelib/include/rtabmap/core/camera/CameraStereoVideo.h
+2
View File
@@ -70,6 +70,7 @@ public:
void setResolution(int width, int height) {_width=width, _height=height;}
void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; }
void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;}
protected:
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
@@ -88,6 +89,7 @@ private:
int _width;
int _height;
std::string _fourcc;
bool rightGrayScale_;
};
} // namespace rtabmap
corelib/include/rtabmap/core/camera/CameraStereoZed.h
+2
View File
@@ -79,6 +79,7 @@ public:
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
void postInterIMUPublic(const IMU & imu, double stamp);
void setRightGrayScale(bool enabled = true);
protected:
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
@@ -100,6 +101,7 @@ private:
bool computeOdometry_;
bool lost_;
bool force3DoF_;
bool rightGrayScale_;
ZedIMUThread * imuPublishingThread_;
#endif
};
corelib/include/rtabmap/core/camera/CameraStereoZedOC.h
+3
View File
@@ -62,6 +62,8 @@ public:
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
void setRightGrayScale(bool enabled = true);
protected:
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
@@ -74,6 +76,7 @@ private:
int usbDevice_;
int resolution_;
uint64_t lastStamp_;
bool rightGrayScale_;
#endif
};
corelib/include/rtabmap/core/odometry/OdometryF2M.h
+5
View File
@@ -62,6 +62,8 @@ private:
float keyFrameThr_;
int visKeyFrameThr_;
int maxNewFeatures_;
float initDepthFactor_;
float floorThreshold_;
float scanKeyFrameThr_;
int scanMaximumMapSize_;
float scanSubtractRadius_;
@@ -69,6 +71,9 @@ private:
float scanMapMaxRange_;
int bundleAdjustment_;
int bundleMaxFrames_;
float bundleMinMotion_;
int bundleMaxKeyFramesPerFeature_;
bool bundleUpdateFeatureMapOnAllFrames_;
float validDepthRatio_;
int pointToPlaneK_;
float pointToPlaneRadius_;
corelib/src/DBReader.cpp
+98 -22
View File
@@ -48,7 +48,7 @@ DBReader::DBReader(const std::string & databasePath,
bool ignoreGoalDelay,
bool goalsIgnored,
int startId,
int cameraIndex,
const std::vector<unsigned int> & cameraIndices,
int stopId,
bool intermediateNodesIgnored,
bool landmarksIgnored,
@@ -63,7 +63,7 @@ DBReader::DBReader(const std::string & databasePath,
_goalsIgnored(goalsIgnored),
_startId(startId),
_stopId(stopId),
_cameraIndex(cameraIndex),
_cameraIndices(cameraIndices),
_intermediateNodesIgnored(intermediateNodesIgnored),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
@@ -94,7 +94,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
bool ignoreGoalDelay,
bool goalsIgnored,
int startId,
int cameraIndex,
const std::vector<unsigned int> & cameraIndices,
int stopId,
bool intermediateNodesIgnored,
bool landmarksIgnored,
@@ -109,7 +109,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
_goalsIgnored(goalsIgnored),
_startId(startId),
_stopId(stopId),
_cameraIndex(cameraIndex),
_cameraIndices(cameraIndices),
_intermediateNodesIgnored(intermediateNodesIgnored),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
@@ -557,34 +557,77 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
}
data.uncompressData();
if(data.cameraModels().size() > 1 &&
_cameraIndex >= 0)
std::map<int, int> cameraOldNewIndices;
std::vector<CameraModel> dbModels = data.cameraModels();
if(dbModels.empty() && !data.stereoCameraModels().empty())
{
if(_cameraIndex < (int)data.cameraModels().size())
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
// select one camera
int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
cv::Mat image;
dbModels.push_back(data.stereoCameraModels()[i].left());
}
}
if(dbModels.size() > 1 &&
!_cameraIndices.empty())
{
cv::Mat combinedImages;
cv::Mat combinedDepthImages;
std::vector<CameraModel> combinedModels;
std::vector<StereoCameraModel> combinedStereoModels;
for(size_t i=0; i<_cameraIndices.size(); ++i)
{
UASSERT_MSG(_cameraIndices[i] < dbModels.size(), uFormat("DBReader: camera index %ld is not valid (should be between 0 and %ld)",
_cameraIndices[i], dbModels.size()-1).c_str());
int addedCameras = std::max(combinedModels.size(), combinedStereoModels.size());
int subImageWidth = data.imageRaw().cols/dbModels.size();
UASSERT(!data.imageRaw().empty() &&
data.imageRaw().cols % data.cameraModels().size() == 0 &&
_cameraIndex*subImageWidth < data.imageRaw().cols);
image= cv::Mat(data.imageRaw(),
cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone();
data.imageRaw().cols % dbModels.size() == 0 &&
(int)_cameraIndices[i]*subImageWidth < data.imageRaw().cols);
if(combinedImages.empty())
{
// initialize with first camera
combinedImages = cv::Mat(data.imageRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.imageRaw().type());
}
cv::Mat fromROI = cv::Mat(data.imageRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.imageRaw().rows));
cv::Mat toROI = cv::Mat(combinedImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedImages.rows));
fromROI.copyTo(toROI);
cv::Mat depth;
if(!data.depthOrRightRaw().empty())
{
UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
depth = cv::Mat(data.depthOrRightRaw(),
cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone();
subImageWidth = data.depthOrRightRaw().cols/dbModels.size();
UASSERT(data.depthOrRightRaw().cols % dbModels.size() == 0 &&
subImageWidth == data.depthOrRightRaw().cols/(int)dbModels.size() &&
(int)_cameraIndices[i]*subImageWidth < data.depthOrRightRaw().cols);
if(combinedDepthImages.empty())
{
// initialize with first camera
combinedDepthImages = cv::Mat(data.depthOrRightRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthOrRightRaw().type());
}
fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows));
toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows));
fromROI.copyTo(toROI);
}
data.setRGBDImage(image, depth, data.cameraModels().at(_cameraIndex));
if(!data.cameraModels().empty())
{
combinedModels.push_back(data.cameraModels()[_cameraIndices[i]]);
}
else
{
combinedStereoModels.push_back(data.stereoCameraModels()[_cameraIndices[i]]);
}
cameraOldNewIndices.insert(std::make_pair(_cameraIndices[i], i));
}
if(!combinedModels.empty())
{
data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
}
else
{
UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
data.setStereoImage(combinedImages, combinedDepthImages, combinedStereoModels);
}
}
data.setId(seq);
@@ -623,7 +666,40 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
(keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
(descriptors.empty() || (int)keypoints.size() == descriptors.rows))
{
data.setFeatures(keypoints, keypoints3D, descriptors);
if(!cameraOldNewIndices.empty())
{
cv::Mat newDescriptors;
std::vector<cv::KeyPoint> newKeypoints;
std::vector<cv::Point3f> newKeypoints3D;
UASSERT(!dbModels.empty() && dbModels[0].imageWidth()>0);
int subImageWidth = dbModels[0].imageWidth();
for(size_t i = 0; i<keypoints.size(); ++i)
{
int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
if(cameraOldNewIndices.find(cameraIndex) != cameraOldNewIndices.end())
{
int newCameraIndex = cameraOldNewIndices.at(cameraIndex);
newKeypoints.push_back(keypoints[i]);
newKeypoints.back().pt.x += (newCameraIndex-cameraIndex)*subImageWidth;
if(!keypoints3D.empty())
{
newKeypoints3D.push_back(keypoints3D.at(i));
}
if(!descriptors.empty())
{
newDescriptors.push_back(descriptors.row(i));
}
}
}
data.setFeatures(newKeypoints, newKeypoints3D, newDescriptors);
}
else
{
data.setFeatures(keypoints, keypoints3D, descriptors);
}
}
else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
{
corelib/src/Features2d.cpp
+13 -2
View File
@@ -908,25 +908,36 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
if(d_imageLeft.empty()) {
d_imageLeft = cv::cuda::GpuMat(imageLeft);
}
// convert to grayscale
// convert to grayscale if not already
if(d_imageLeft.channels() > 1) {
cv::cuda::GpuMat tmp;
cv::cuda::cvtColor(d_imageLeft, tmp, cv::COLOR_BGR2GRAY);
d_imageLeft = tmp;
}
d_imageRight = data.depthOrRightRawGpu();
if(d_imageRight.empty()) {
d_imageRight = cv::cuda::GpuMat(imageRight);
}
// convert to grayscale if not already
if(d_imageRight.channels() > 1) {
cv::cuda::GpuMat tmp;
cv::cuda::cvtColor(d_imageRight, tmp, cv::COLOR_BGR2GRAY);
d_imageRight = tmp;
}
}
else
#endif
{
// convert to grayscale (right image should be already grayscale)
// convert to grayscale
if(imageLeft.channels() > 1)
{
cv::cvtColor(data.imageRaw(), imageLeft, cv::COLOR_BGR2GRAY);
}
if(imageRight.channels() > 1)
{
cv::cvtColor(data.rightRaw(), imageRight, cv::COLOR_BGR2GRAY);
}
}
std::vector<cv::Point2f> leftCorners;
corelib/src/Memory.cpp
+55 -18
View File
@@ -4519,17 +4519,55 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
UDEBUG("");
SensorData data = inputData;
bool isIntermediateNode = data.id() < 0;
// uncompress data if needed
if(!isIntermediateNode)
{
// We need raw images if we need to extract features and/or do tag detection
bool needRawImages = _feature2D->getMaxFeatures() >= 0 &&
(!_useOdometryFeatures ||
data.keypoints().empty() ||
(int)data.keypoints().size() != data.descriptors().rows ||
data.descriptors().empty() ||
_detectMarkers ||
_rotateImagesUpsideUp ||
_imagePostDecimation > 1 ||
(_createOccupancyGrid && _localMapMaker->isGridFromDepth()));
// Note: we could avoid uncompressing scan if we don't do any filtering
// and if we don't use it for local occupancy grid
bool needRawScan = true;
if( (needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty()) ||
(needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
(needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
{
cv::Mat left, right;
LaserScan laserScan;
UDEBUG("Uncompressing data...");
data.uncompressData(
needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty() ? &left : 0,
needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty() ? &right : 0,
needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty() ? &laserScan : 0);
UDEBUG("Uncompressing data...done!");
}
}
UASSERT(data.imageRaw().empty() ||
data.imageRaw().type() == CV_8UC1 ||
data.imageRaw().type() == CV_8UC3);
UASSERT_MSG(data.depthOrRightRaw().empty() ||
( ( data.depthOrRightRaw().type() == CV_16UC1 ||
data.depthOrRightRaw().type() == CV_32FC1 ||
data.depthOrRightRaw().type() == CV_8UC1)
data.depthOrRightRaw().type() == CV_8UC1 ||
data.depthOrRightRaw().type() == CV_8UC3)
&&
( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
( (data.imageRaw().empty() && !(data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3)) ||
(data.depthOrRightRaw().rows <= data.imageRaw().rows && data.depthOrRightRaw().cols <= data.imageRaw().cols))),
uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d(stereo)]). "
uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d-%d(stereo)]). "
"For stereo, left and right images should be same size. "
"For RGB-D, depth can be X times smaller than RGB (where X is an integer).",
data.imageRaw().cols,
@@ -4540,7 +4578,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
data.depthOrRightRaw().cols,
data.depthOrRightRaw().rows,
data.depthOrRightRaw().type(),
CV_16UC1, CV_32FC1, CV_8UC1).c_str());
CV_16UC1, CV_32FC1, CV_8UC1, CV_8UC3).c_str());
if(!data.depthOrRightRaw().empty() &&
data.cameraModels().empty() &&
@@ -4559,7 +4597,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
float t;
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
bool isIntermediateNode = data.id() < 0;
int id = data.id();
if(_generateIds)
{
@@ -5754,8 +5791,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
}
// Filter the laser scan?
LaserScan laserScan = data.laserScanRaw();
// Filter the laser scan?
if(!isIntermediateNode && laserScan.size())
{
if(laserScan.rangeMax() == 0.0f)
@@ -5902,12 +5939,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
!stereoCameraModels.empty()?
SensorData(
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform()):
LaserScan(compressedScan,
LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
@@ -5915,20 +5952,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
laserScan.angleMax(),
laserScan.angleIncrement(),
laserScan.localTransform()),
compressedImage,
compressedDepth,
compressedImage.empty()?data.imageCompressed():compressedImage,
compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
stereoCameraModels,
id,
0,
compressedUserData):
SensorData(
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform()):
LaserScan(compressedScan,
LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
@@ -5936,8 +5973,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
laserScan.angleMax(),
laserScan.angleIncrement(),
laserScan.localTransform()),
compressedImage,
compressedDepth,
compressedImage.empty()?data.imageCompressed():compressedImage,
compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
cameraModels,
id,
0,
@@ -5986,12 +6023,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
!stereoCameraModels.empty()?
SensorData(
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform()):
LaserScan(compressedScan,
LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
@@ -6007,12 +6044,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
compressedUserData):
SensorData(
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform()):
LaserScan(compressedScan,
LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
corelib/src/Odometry.cpp
+9
View File
@@ -337,6 +337,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
if((data.imageRaw().empty() && !data.imageCompressed().empty()) ||
(data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
(data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
{
UDEBUG("Received compressed data, uncompressing...");
data.uncompressData();
UDEBUG("Received compressed data, uncompressing...done!");
}
if(!data.imageRaw().empty())
{
UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld",
corelib/src/OdometryThread.cpp
+5 -1
View File
@@ -155,7 +155,11 @@ void OdometryThread::addData(const SensorData & data)
bool notify = true;
_dataMutex.lock();
{
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || data.imu().empty())
if( !data.imageRaw().empty() ||
!data.imageCompressed().empty() ||
!data.laserScanRaw().isEmpty() ||
!data.laserScanCompressed().empty() ||
data.imu().empty())
{
_dataBuffer.push_back(data);
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
corelib/src/SensorCaptureThread.cpp
+1 -1
View File
@@ -530,7 +530,7 @@ void SensorCaptureThread::mainLoop()
info.odomPose.setNull();
}
if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
if(!data.imageCompressed().empty() || !data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
{
postUpdate(&data, &info);
info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial();
corelib/src/SensorData.cpp
+2 -1
View File
@@ -365,7 +365,8 @@ void SensorData::setStereoImage(
}
else if(!right.empty())
{
UASSERT(right.type() == CV_8UC1); // Mono
UASSERT(right.type() == CV_8UC1 || // Mono
right.type() == CV_8UC3); // RGB
_depthOrRightRaw = right;
if(clearData)
{
corelib/src/Stereo.cpp
+6
View File
@@ -79,6 +79,8 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const
{
UASSERT(leftImage.type() == CV_8UC1);
UASSERT(rightImage.type() == CV_8UC1);
std::vector<cv::Point2f> rightCorners;
UDEBUG("util2d::calcStereoCorrespondences() begin");
rightCorners = util2d::calcStereoCorrespondences(
@@ -145,6 +147,8 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const
{
UASSERT(leftImage.type() == CV_8UC1);
UASSERT(rightImage.type() == CV_8UC1);
std::vector<cv::Point2f> rightCorners;
std::vector<float> err;
#ifdef HAVE_OPENCV_CUDAOPTFLOW
@@ -184,6 +188,8 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
{
std::vector<cv::Point2f> rightCorners;
#ifdef HAVE_OPENCV_CUDAOPTFLOW
UASSERT(leftImage.type() == CV_8UC1);
UASSERT(rightImage.type() == CV_8UC1);
UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device begin");
cv::cuda::GpuMat d_leftImage(leftImage);
cv::cuda::GpuMat d_rightImage(rightImage);
corelib/src/camera/CameraDepthAI.cpp
+293 -347
View File
@@ -47,7 +47,7 @@ bool CameraDepthAI::available()
CameraDepthAI::CameraDepthAI(
const std::string & mxidOrName,
int resolution,
int imageWidth,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform)
@@ -57,12 +57,15 @@ CameraDepthAI::CameraDepthAI(
outputMode_(0),
confThreshold_(200),
lrcThreshold_(5),
resolution_(resolution),
imageWidth_(imageWidth),
extendedDisparity_(false),
subpixelFractionalBits_(0),
compandingWidth_(0),
enableCompanding_(false),
subpixelFractionalBits_(3),
disparityWidth_(1),
medianFilter_(5),
useSpecTranslation_(false),
alphaScaling_(0.0),
imagesRectified_(true),
imuPublished_(true),
publishInterIMU_(false),
dotIntensity_(0.0),
@@ -70,14 +73,16 @@ CameraDepthAI::CameraDepthAI(
detectFeatures_(0),
useHarrisDetector_(false),
minDistance_(7.0),
numTargetFeatures_(1000),
numTargetFeatures_(320),
threshold_(0.01),
nms_(true),
nmsRadius_(4)
#endif
{
#ifdef RTABMAP_DEPTHAI
UASSERT(resolution_>=(int)dai::MonoCameraProperties::SensorResolution::THE_720_P && resolution_<=(int)dai::MonoCameraProperties::SensorResolution::THE_1200_P);
UASSERT(imageWidth_ == 640 || imageWidth_ == 1280);
if(this->getImageRate() <= 0)
this->setImageRate(30);
#endif
}
@@ -85,9 +90,7 @@ CameraDepthAI::~CameraDepthAI()
{
#ifdef RTABMAP_DEPTHAI
if(device_.get())
{
device_->close();
}
#endif
}
@@ -110,22 +113,15 @@ void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold)
#endif
}
void CameraDepthAI::setExtendedDisparity(bool extendedDisparity)
void CameraDepthAI::setExtendedDisparity(bool extendedDisparity, bool enableCompanding)
{
#ifdef RTABMAP_DEPTHAI
extendedDisparity_ = extendedDisparity;
if(extendedDisparity_)
enableCompanding_ = enableCompanding;
if(extendedDisparity_ && enableCompanding_)
{
if(subpixelFractionalBits_>0)
{
UWARN("Extended disparity has been enabled while subpixel being also enabled, disabling subpixel...");
subpixelFractionalBits_ = 0;
}
if(compandingWidth_>0)
{
UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding...");
compandingWidth_ = 0;
}
UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding...");
enableCompanding_ = false;
}
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
@@ -137,25 +133,22 @@ void CameraDepthAI::setSubpixelMode(bool enabled, int fractionalBits)
#ifdef RTABMAP_DEPTHAI
UASSERT(fractionalBits>=3 && fractionalBits<=5);
subpixelFractionalBits_ = enabled?fractionalBits:0;
if(subpixelFractionalBits_ != 0 && extendedDisparity_)
{
UWARN("Subpixel has been enabled while extended disparity being also enabled, disabling extended disparity...");
extendedDisparity_ = false;
}
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setCompanding(bool enabled, int width)
void CameraDepthAI::setDisparityWidthAndFilter(int disparityWidth, int medianFilter)
{
#ifdef RTABMAP_DEPTHAI
UASSERT(width == 64 || width == 96);
compandingWidth_ = enabled?width:0;
if(compandingWidth_ != 0 && extendedDisparity_)
UASSERT(disparityWidth == 64 || disparityWidth == 96);
disparityWidth_ = disparityWidth;
medianFilter_ = medianFilter;
int maxDisp = (extendedDisparity_?2:1) * std::pow(2,subpixelFractionalBits_) * (disparityWidth_-1);
if(medianFilter_ && maxDisp > 1024)
{
UWARN("Companding has been enabled while extended disparity being also enabled, disabling extended disparity...");
extendedDisparity_ = false;
UWARN("Maximum disparity value '%d' exceeds the maximum supported '1024' by median filter, disabling median filter...", maxDisp);
medianFilter_ = 0;
}
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
@@ -193,19 +186,26 @@ void CameraDepthAI::setIrIntensity(float dotIntensity, float floodIntensity)
#endif
}
void CameraDepthAI::setDetectFeatures(int detectFeatures)
void CameraDepthAI::setDetectFeatures(int detectFeatures, const std::string & blobPath)
{
#ifdef RTABMAP_DEPTHAI
detectFeatures_ = detectFeatures;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setBlobPath(const std::string & blobPath)
{
#ifdef RTABMAP_DEPTHAI
blobPath_ = blobPath;
if(detectFeatures_ && outputMode_==2)
{
UWARN("On-device feature detectors cannot be enabled on color camera input, disabling on-device feature detector...");
detectFeatures_ = 0;
}
if(detectFeatures_>=2 && blobPath_.empty())
{
UWARN("Missing MyriadX blob file, disabling on-device feature detector");
detectFeatures_ = 0;
}
if(detectFeatures_>=2 && this->getImageRate()>15)
{
UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!");
this->setImageRate(15);
}
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
@@ -245,9 +245,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
return false;
}
if(device_.get())
device_->close();
accBuffer_.clear();
gyroBuffer_.clear();
@@ -268,259 +265,22 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
return false;
}
// look for calibration files
stereoModel_ = StereoCameraModel();
targetSize_ = cv::Size(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
dai::Pipeline p;
auto monoLeft = p.create<dai::node::MonoCamera>();
auto monoRight = p.create<dai::node::MonoCamera>();
std::shared_ptr<dai::node::StereoDepth> stereo;
if(imagesRectified_)
stereo = p.create<dai::node::StereoDepth>();
std::shared_ptr<dai::node::Camera> colorCam;
if(outputMode_==2)
{
colorCam = p.create<dai::node::Camera>();
if(!imagesRectified_)
colorCam->setMeshSource(dai::CameraProperties::WarpMeshSource::NONE);
if(detectFeatures_)
{
UWARN("On-device feature detectors cannot be enabled on color camera input!");
detectFeatures_ = 0;
}
}
std::shared_ptr<dai::node::IMU> imu;
if(imuPublished_)
imu = p.create<dai::node::IMU>();
std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
std::shared_ptr<dai::node::ImageManip> manip;
std::shared_ptr<dai::node::NeuralNetwork> neuralNetwork;
if(detectFeatures_ == 1)
{
gfttDetector = p.create<dai::node::FeatureTracker>();
}
else if(detectFeatures_ >= 2)
{
if(!blobPath_.empty())
{
manip = p.create<dai::node::ImageManip>();
neuralNetwork = p.create<dai::node::NeuralNetwork>();
}
else
{
UWARN("Missing MyriadX blob file!");
detectFeatures_ = 0;
}
}
auto sync = p.create<dai::node::Sync>();
auto xoutCamera = p.create<dai::node::XLinkOut>();
std::shared_ptr<dai::node::XLinkOut> xoutIMU;
if(imuPublished_)
xoutIMU = p.create<dai::node::XLinkOut>();
// XLinkOut
xoutCamera->setStreamName("camera");
if(imuPublished_)
xoutIMU->setStreamName("imu");
monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
monoLeft->setCamera("left");
monoRight->setCamera("right");
if(detectFeatures_ >= 2)
{
if(this->getImageRate() <= 0 || this->getImageRate() > 15)
{
UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!");
monoLeft->setFps(15);
monoRight->setFps(15);
}
}
else if(this->getImageRate() > 0)
{
monoLeft->setFps(this->getImageRate());
monoRight->setFps(this->getImageRate());
}
// StereoDepth
if(stereo.get())
{
if(outputMode_ == 2)
stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
else
stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
stereo->setExtendedDisparity(extendedDisparity_);
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
stereo->enableDistortionCorrection(true);
stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
if(alphaScaling_ > -1.0f)
stereo->setAlphaScaling(alphaScaling_);
stereo->initialConfig.setConfidenceThreshold(confThreshold_);
stereo->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
if(lrcThreshold_>=0)
stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
auto config = stereo->initialConfig.get();
config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
config.censusTransform.kernelMask = 0X2AA00AA805540155;
config.postProcessing.brightnessFilter.maxBrightness = 255;
stereo->initialConfig.set(config);
// Link plugins CAM -> STEREO -> XLINK
monoLeft->out.link(stereo->left);
monoRight->out.link(stereo->right);
}
if(outputMode_ == 2)
{
colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);
colorCam->setSize(targetSize_.width, targetSize_.height);
if(this->getImageRate() > 0)
colorCam->setFps(this->getImageRate());
if(alphaScaling_ > -1.0f)
colorCam->setCalibrationAlpha(alphaScaling_);
}
this->setImageRate(0);
// Using VideoEncoder on PoE devices, Subpixel is not supported
if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
{
auto leftOrColorEnc = p.create<dai::node::VideoEncoder>();
auto depthOrRightEnc = p.create<dai::node::VideoEncoder>();
leftOrColorEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
if(outputMode_ < 2)
{
if(imagesRectified_) {
stereo->rectifiedLeft.link(leftOrColorEnc->input);
}
else {
monoLeft->out.link(leftOrColorEnc->input);
}
leftOrColorEnc->bitstream.link(sync->inputs["left"]);
}
else
{
colorCam->video.link(leftOrColorEnc->input);
leftOrColorEnc->bitstream.link(sync->inputs["color"]);
}
if(imagesRectified_ && outputMode_)
{
depthOrRightEnc->setQuality(100);
stereo->disparity.link(depthOrRightEnc->input);
depthOrRightEnc->bitstream.link(sync->inputs["depth"]);
}
else
{
if(imagesRectified_) {
stereo->rectifiedRight.link(depthOrRightEnc->input);
}
else {
monoRight->out.link(depthOrRightEnc->input);
}
depthOrRightEnc->bitstream.link(sync->inputs["right"]);
}
}
else
{
if(stereo.get()) {
stereo->setSubpixel(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5);
if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
stereo->setSubpixelFractionalBits(subpixelFractionalBits_);
auto config = stereo->initialConfig.get();
config.costMatching.enableCompanding = compandingWidth_>0;
if(compandingWidth_>0)
config.costMatching.disparityWidth = compandingWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
stereo->initialConfig.set(config);
}
if(outputMode_ < 2)
{
if(imagesRectified_)
stereo->rectifiedLeft.link(sync->inputs["left"]);
else
monoLeft->out.link(sync->inputs["left"]);
}
else
{
monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
colorCam->video.link(sync->inputs["color"]);
}
if(imagesRectified_) {
if(outputMode_)
stereo->depth.link(sync->inputs["depth"]);
else
stereo->rectifiedRight.link(sync->inputs["right"]);
}
else {
monoRight->out.link(sync->inputs["right"]);
}
}
sync->setSyncThreshold(std::chrono::milliseconds(int(500 / monoLeft->getFps())));
sync->out.link(xoutCamera->input);
if(imuPublished_)
{
// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
imu->setBatchReportThreshold(1);
// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
// if lower or equal to batchReportThreshold then the sending is always blocking on device
// useful to reduce device's CPU load and number of lost packets, if CPU load is high on device side due to multiple nodes
imu->setMaxBatchReports(10);
// Link plugins IMU -> XLINK
imu->out.link(xoutIMU->input);
}
if(detectFeatures_ == 1)
{
gfttDetector->setHardwareResources(1, 2);
gfttDetector->initialConfig.setCornerDetector(
useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI);
gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_);
gfttDetector->initialConfig.setMotionEstimator(false);
auto cfg = gfttDetector->initialConfig.get();
cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
gfttDetector->initialConfig.set(cfg);
if(stereo.get())
stereo->rectifiedLeft.link(gfttDetector->inputImage);
else
monoLeft->out.link(gfttDetector->inputImage);
gfttDetector->outputFeatures.link(sync->inputs["feat"]);
}
else if(detectFeatures_ >= 2)
{
manip->setKeepAspectRatio(false);
manip->setMaxOutputFrameSize(320 * 200);
manip->initialConfig.setResize(320, 200);
neuralNetwork->setBlobPath(blobPath_);
neuralNetwork->setNumInferenceThreads(2);
neuralNetwork->setNumNCEPerInferenceThread(1);
neuralNetwork->input.setBlocking(false);
if(stereo.get())
stereo->rectifiedLeft.link(manip->inputImage);
else
monoLeft->out.link(manip->inputImage);
manip->out.link(neuralNetwork->input);
neuralNetwork->out.link(sync->inputs["feat"]);
}
device_.reset(new dai::Device(p, deviceToUse));
UINFO("Device serial: %s", device_->getMxId().c_str());
UINFO("Available camera sensors: ");
device_ = std::make_unique<dai::Device>(deviceToUse);
auto deviceName = device_->getDeviceName();
auto imuType = device_->getConnectedIMU();
UINFO("Device Name: %s, Device Serial: %s", deviceName.c_str(), device_->getMxId().c_str());
UINFO("Available Camera Sensors: ");
for(auto& sensor : device_->getCameraSensorNames()) {
UINFO("Socket: CAM_%c - %s", 'A'+(unsigned char)sensor.first, sensor.second.c_str());
}
UINFO("IMU Type: %s", imuType.c_str());
UINFO("Loading eeprom calibration data");
dai::CalibrationHandler calibHandler = device_->readCalibration();
auto calibHandler = device_->readCalibration();
auto boardName = calibHandler.getEepromData().boardName;
stereoModel_ = StereoCameraModel();
targetSize_ = cv::Size(imageWidth_, imageWidth_/640*((outputMode_==2&&boardName!="BC2087")?360:400));
if(!calibrationFolder.empty() && !cameraName.empty() && imagesRectified_)
{
@@ -591,10 +351,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
std::cout << "Expected K with rectification_alpha=0: " << stereoModel_.left().K()*(double(targetSize_.width)/double(stereoModel_.left().imageWidth())) << std::endl;
}
device_->flashCalibration2(calibHandler);
UINFO("Closing device...");
device_->close();
UINFO("Restarting pipeline...");
device_.reset(new dai::Device(p, deviceToUse));
}
catch(const std::runtime_error & e) {
UERROR("Failed flashing calibration: %s", e.what());
@@ -609,10 +365,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
calibHandler = device_->readCalibration();
}
auto eeprom = calibHandler.getEepromData();
UINFO("Product name: %s, board name: %s", eeprom.productName.c_str(), eeprom.boardName.c_str());
auto cameraId = outputMode_<2?dai::CameraBoardSocket::CAM_B:dai::CameraBoardSocket::CAM_A;
auto cameraId = outputMode_==2?dai::CameraBoardSocket::CAM_A:dai::CameraBoardSocket::CAM_B;
cv::Mat cameraMatrix, distCoeffs, newCameraMatrix;
std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(cameraId, targetSize_.width, targetSize_.height);
@@ -636,15 +389,15 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
double cy = newCameraMatrix.at<double>(1, 2);
UINFO("fx=%f fy=%f cx=%f cy=%f (target size = %dx%d)", fx, fy, cx, cy, targetSize_.width, targetSize_.height);
if(outputMode_ == 2) {
stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, 0, this->getLocalTransform(), targetSize_);
stereoModel_ = StereoCameraModel(deviceName, fx, fy, cx, cy, 0, this->getLocalTransform(), targetSize_);
}
else {
double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0;
UINFO("baseline=%f", baseline);
stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, outputMode_==0?baseline:0, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
stereoModel_ = StereoCameraModel(deviceName, fx, fy, cx, cy, outputMode_==0?baseline:0, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
}
if(imuPublished_)
if(imuPublished_ || imuType.empty())
{
// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
@@ -653,44 +406,266 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
// matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
// matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
// matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
if(eeprom.boardName == "OAK-D" ||
eeprom.boardName == "BW1098OBC")
if(deviceName == "OAK-D")
{
imuLocalTransform_ = Transform(
0, -1, 0, 0.0525,
1, 0, 0, 0.013662,
0, 0, 1, 0);
}
else if(eeprom.boardName == "DM9098")
else if(boardName == "BC2087") // OAK-D LR
{
imuLocalTransform_ = Transform(
1, 0, 0, 0.021425,
0, 1, 0, 0.009925,
0, 0, 1, 0);
}
else if(boardName == "DM2080") // OAK-D SR
{
imuLocalTransform_ = Transform(
-1, 0, 0, 0,
0, -1, 0, -0.0024,
0, 0, 1, 0);
}
else if(boardName == "DM9098") // OAK-D S2, OAK-D W, OAK-D Pro, OAK-D Pro W
{
imuLocalTransform_ = Transform(
0, 1, 0, 0.037945,
1, 0, 0, 0.00079,
0, 0, -1, 0);
}
else if(eeprom.boardName == "NG2094")
else if(boardName == "NG2094") // OAK-D Pro W Dev
{
imuLocalTransform_ = Transform(
0, 1, 0, 0.0374,
1, 0, 0, 0.00176,
0, 0, -1, 0);
}
else if(eeprom.boardName == "NG9097")
else if(boardName == "NG9097") // OAK-D S2 PoE, OAK-D W PoE, OAK-D Pro PoE, OAK-D Pro W PoE
{
imuLocalTransform_ = Transform(
0, 1, 0, 0.04,
1, 0, 0, 0.020265,
0, 0, -1, 0);
if(imuType == "BMI270")
{
imuLocalTransform_ = Transform(
0, 1, 0, 0.04,
1, 0, 0, 0.020265,
0, 0, -1, 0);
}
else // BNO085/086
{
imuLocalTransform_ = Transform(
0, -1, 0, 0.04,
-1, 0, 0, 0.020265,
0, 0, -1, 0);
}
}
else
{
UWARN("Unknown boardName (%s)! Disabling IMU!", eeprom.boardName.c_str());
UWARN("Unsupported boardName (%s)! Disabling IMU!", boardName.c_str());
imuPublished_ = false;
}
}
else
{
UINFO("IMU disabled");
imuPublished_ = false;
}
dai::Pipeline pipeline;
auto sync = pipeline.create<dai::node::Sync>();
sync->setSyncThreshold(std::chrono::milliseconds(int(500 / this->getImageRate())));
if(outputMode_ == 2)
{
auto rgbCamera = pipeline.create<dai::node::Camera>();
rgbCamera->setCamera("color");
if(boardName == "BC2087")
rgbCamera->setSize(1920, 1200);
else if(boardName == "NG2094")
rgbCamera->setSize(1280, 720);
else
rgbCamera->setSize(1920, 1080);
rgbCamera->setSize(1920, 1200);
rgbCamera->setVideoSize(targetSize_.width, targetSize_.height);
rgbCamera->setPreviewSize(targetSize_.width, targetSize_.height);
rgbCamera->setFps(this->getImageRate());
rgbCamera->setMeshSource(imagesRectified_?dai::CameraProperties::WarpMeshSource::CALIBRATION:dai::CameraProperties::WarpMeshSource::NONE);
if(imagesRectified_ && alphaScaling_>-1.0f)
rgbCamera->setCalibrationAlpha(alphaScaling_);
rgbCamera->properties.ispScale.horizNumerator = rgbCamera->properties.ispScale.vertNumerator = imageWidth_/640;
rgbCamera->properties.ispScale.horizDenominator = rgbCamera->properties.ispScale.vertDenominator = boardName=="NG2094"?2:3;
auto rgbEncoder = pipeline.create<dai::node::VideoEncoder>();
rgbEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
rgbCamera->video.link(rgbEncoder->input);
rgbEncoder->bitstream.link(sync->inputs["rgb"]);
}
auto stereoDepth = pipeline.create<dai::node::StereoDepth>();
if(outputMode_ == 2)
stereoDepth->setDepthAlign(dai::CameraBoardSocket::CAM_A);
else
stereoDepth->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
{
stereoDepth->setSubpixel(true);
stereoDepth->setSubpixelFractionalBits(subpixelFractionalBits_);
}
stereoDepth->setExtendedDisparity(extendedDisparity_);
stereoDepth->enableDistortionCorrection(true);
stereoDepth->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
stereoDepth->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
if(alphaScaling_ > -1.0f)
stereoDepth->setAlphaScaling(alphaScaling_);
stereoDepth->initialConfig.setConfidenceThreshold(confThreshold_);
stereoDepth->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
if(lrcThreshold_>=0)
stereoDepth->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
stereoDepth->initialConfig.setMedianFilter(dai::MedianFilter(medianFilter_));
auto config = stereoDepth->initialConfig.get();
config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
config.censusTransform.kernelMask = 0X5092A28C5152428;
config.costMatching.disparityWidth = disparityWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
config.costMatching.enableCompanding = enableCompanding_;
config.costMatching.linearEquationParameters.alpha = 2;
config.costMatching.linearEquationParameters.beta = 4;
config.costAggregation.horizontalPenaltyCostP1 = 100;
config.costAggregation.horizontalPenaltyCostP2 = 500;
config.costAggregation.verticalPenaltyCostP1 = 100;
config.costAggregation.verticalPenaltyCostP2 = 500;
config.postProcessing.brightnessFilter.maxBrightness = 255;
stereoDepth->initialConfig.set(config);
stereoDepth->depth.link(sync->inputs["depth"]);
if(outputMode_ < 2)
{
auto leftEncoder = pipeline.create<dai::node::VideoEncoder>();
leftEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
if(imagesRectified_)
stereoDepth->rectifiedLeft.link(leftEncoder->input);
else
stereoDepth->syncedLeft.link(leftEncoder->input);
leftEncoder->bitstream.link(sync->inputs["left"]);
}
if(!outputMode_)
{
auto rightEncoder = pipeline.create<dai::node::VideoEncoder>();
rightEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
if(imagesRectified_)
stereoDepth->rectifiedRight.link(rightEncoder->input);
else
stereoDepth->syncedRight.link(rightEncoder->input);
rightEncoder->bitstream.link(sync->inputs["right"]);
}
if(boardName == "BC2087")
{
auto leftCamera = pipeline.create<dai::node::ColorCamera>();
leftCamera->setCamera("left");
leftCamera->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1200_P);
leftCamera->setIspScale(imageWidth_/640, 3);
leftCamera->setFps(this->getImageRate());
auto rightCamera = pipeline.create<dai::node::ColorCamera>();
rightCamera->setCamera("right");
rightCamera->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1200_P);
rightCamera->setIspScale(imageWidth_/640, 3);
rightCamera->setFps(this->getImageRate());
leftCamera->isp.link(stereoDepth->left);
rightCamera->isp.link(stereoDepth->right);
}
else
{
auto leftCamera = pipeline.create<dai::node::MonoCamera>();
leftCamera->setCamera("left");
leftCamera->setResolution(imageWidth_==640?dai::MonoCameraProperties::SensorResolution::THE_400_P:dai::MonoCameraProperties::SensorResolution::THE_800_P);
leftCamera->setFps(this->getImageRate());
auto rightCamera = pipeline.create<dai::node::MonoCamera>();
rightCamera->setCamera("right");
rightCamera->setResolution(imageWidth_==640?dai::MonoCameraProperties::SensorResolution::THE_400_P:dai::MonoCameraProperties::SensorResolution::THE_800_P);
rightCamera->setFps(this->getImageRate());
leftCamera->out.link(stereoDepth->left);
rightCamera->out.link(stereoDepth->right);
}
if(detectFeatures_ == 1)
{
auto gfttDetector = pipeline.create<dai::node::FeatureTracker>();
gfttDetector->setHardwareResources(2, 2);
gfttDetector->initialConfig.setCornerDetector(
useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI);
gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_);
gfttDetector->initialConfig.setMotionEstimator(false);
auto cfg = gfttDetector->initialConfig.get();
cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
gfttDetector->initialConfig.set(cfg);
if(imagesRectified_)
stereoDepth->rectifiedLeft.link(gfttDetector->inputImage);
else
stereoDepth->syncedLeft.link(gfttDetector->inputImage);
gfttDetector->outputFeatures.link(sync->inputs["feat"]);
}
else if(detectFeatures_ >= 2)
{
auto imageManip = pipeline.create<dai::node::ImageManip>();
imageManip->setKeepAspectRatio(false);
imageManip->setMaxOutputFrameSize(320 * 200);
imageManip->initialConfig.setResize(320, 200);
auto neuralNetwork = pipeline.create<dai::node::NeuralNetwork>();
neuralNetwork->setBlobPath(blobPath_);
neuralNetwork->setNumInferenceThreads(2);
neuralNetwork->setNumNCEPerInferenceThread(1);
neuralNetwork->input.setBlocking(false);
if(imagesRectified_)
stereoDepth->rectifiedLeft.link(imageManip->inputImage);
else
stereoDepth->syncedLeft.link(imageManip->inputImage);
imageManip->out.link(neuralNetwork->input);
neuralNetwork->out.link(sync->inputs["feat"]);
}
auto xoutCamera = pipeline.create<dai::node::XLinkOut>();
xoutCamera->setStreamName("camera");
sync->out.link(xoutCamera->input);
if(imuPublished_)
{
auto imu = pipeline.create<dai::node::IMU>();
if(imuType == "BMI270")
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
else // BNO085/086
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER, dai::IMUSensor::GYROSCOPE_UNCALIBRATED}, 200);
imu->setBatchReportThreshold(boardName=="NG9097"?4:1);
imu->setMaxBatchReports(10);
auto xoutIMU = pipeline.create<dai::node::XLinkOut>();
xoutIMU->setStreamName("imu");
imu->out.link(xoutIMU->input);
}
device_->startPipeline(pipeline);
if(!device_->getIrDrivers().empty())
{
UINFO("Setting IR intensity");
device_->setIrLaserDotProjectorIntensity(dotIntensity_);
device_->setIrFloodLightIntensity(floodIntensity_);
}
else if(dotIntensity_ > 0 || floodIntensity_ > 0)
{
UWARN("No IR drivers were detected! IR intensity cannot be set.");
}
cameraQueue_ = device_->getOutputQueue("camera", 8, false);
@@ -726,17 +701,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
});
}
if(!device_->getIrDrivers().empty())
{
UINFO("Setting IR intensity");
device_->setIrLaserDotProjectorIntensity(dotIntensity_);
device_->setIrFloodLightIntensity(floodIntensity_);
}
else if(dotIntensity_ > 0 || floodIntensity_ > 0)
{
UWARN("No IR drivers were detected! IR intensity cannot be set.");
}
uSleep(2000); // avoid bad frames on start
return true;
@@ -769,32 +733,14 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
#ifdef RTABMAP_DEPTHAI
auto messageGroup = cameraQueue_->get<dai::MessageGroup>();
auto rectifLeftOrColor = messageGroup->get<dai::ImgFrame>(outputMode_<2?"left":"color");
auto rectifRightOrDepth = messageGroup->get<dai::ImgFrame>(imagesRectified_ && outputMode_?"depth":"right");
auto rgbOrLeft = messageGroup->get<dai::ImgFrame>(outputMode_==2?"rgb":"left");
auto depthOrRight = messageGroup->get<dai::ImgFrame>(outputMode_?"depth":"right");
cv::Mat leftOrColor, depthOrRight;
if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
{
leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
if(imagesRectified_ && outputMode_)
{
cv::Mat disp;
depthOrRight.convertTo(disp, CV_16UC1);
cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight);
}
}
double stamp = std::chrono::duration<double>(depthOrRight->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
if(outputMode_)
data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_ANYCOLOR), depthOrRight->getCvFrame(), stereoModel_.left(), this->getNextSeqID(), stamp);
else
{
leftOrColor = rectifLeftOrColor->getCvFrame();
depthOrRight = rectifRightOrDepth->getCvFrame();
}
double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
if(imagesRectified_ && outputMode_)
data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
else
data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_GRAYSCALE), cv::imdecode(depthOrRight->getData(), cv::IMREAD_GRAYSCALE), stereoModel_, this->getNextSeqID(), stamp);
if(imuPublished_ && !publishInterIMU_)
{
corelib/src/camera/CameraStereoImages.cpp
+5 -3
View File
@@ -44,7 +44,8 @@ CameraStereoImages::CameraStereoImages(
float imageRate,
const Transform & localTransform) :
CameraImages(pathLeftImages, imageRate, localTransform),
camera2_(new CameraImages(pathRightImages))
camera2_(new CameraImages(pathRightImages)),
rightGrayScale_(true)
{
this->setImagesRectified(rectifyImages);
}
@@ -55,7 +56,8 @@ CameraStereoImages::CameraStereoImages(
float imageRate,
const Transform & localTransform) :
CameraImages("", imageRate, localTransform),
camera2_(0)
camera2_(0),
rightGrayScale_(true)
{
std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
if(paths.size() >= 1)
@@ -179,7 +181,7 @@ SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info)
// Rectification
cv::Mat leftImage = left.imageRaw();
cv::Mat rightImage = right.imageRaw();
if(rightImage.type() != CV_8UC1)
if(rightImage.type() != CV_8UC1 && rightGrayScale_)
{
cv::Mat tmp;
cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
corelib/src/camera/CameraStereoVideo.cpp
+9 -5
View File
@@ -56,7 +56,8 @@ CameraStereoVideo::CameraStereoVideo(
usbDevice_(0),
usbDevice2_(-1),
_width(0),
_height(0)
_height(0),
rightGrayScale_(true)
{
}
@@ -74,7 +75,8 @@ CameraStereoVideo::CameraStereoVideo(
usbDevice_(0),
usbDevice2_(-1),
_width(0),
_height(0)
_height(0),
rightGrayScale_(true)
{
}
@@ -89,7 +91,8 @@ CameraStereoVideo::CameraStereoVideo(
usbDevice_(device),
usbDevice2_(-1),
_width(0),
_height(0)
_height(0),
rightGrayScale_(true)
{
}
@@ -105,7 +108,8 @@ CameraStereoVideo::CameraStereoVideo(
usbDevice_(deviceLeft),
usbDevice2_(deviceRight),
_width(0),
_height(0)
_height(0),
rightGrayScale_(true)
{
}
@@ -379,7 +383,7 @@ SensorData CameraStereoVideo::captureImage(SensorCaptureInfo * info)
// Rectification
bool rightCvt = false;
if(rightImage.type() != CV_8UC1)
if(rightImage.type() != CV_8UC1 && rightGrayScale_)
{
cv::Mat tmp;
cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
corelib/src/camera/CameraStereoZed.cpp
+11 -1
View File
@@ -783,7 +783,10 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
#endif
cv::Mat rgbaRight = slMat2cvMat(tmp);
cv::Mat right;
cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
if(rightGrayScale_)
cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
else
cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2BGR);
#if ZED_SDK_MAJOR_VERSION < 3
data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
#else
@@ -891,4 +894,11 @@ void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp)
postInterIMU(imu, stamp);
}
void CameraStereoZed::setRightGrayScale(bool enabled)
{
#ifdef RTABMAP_ZED
rightGrayScale_ = enabled;
#endif
}
} // namespace rtabmap
corelib/src/camera/CameraStereoZedOC.cpp
+11 -1
View File
@@ -755,7 +755,10 @@ SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info)
// ----> Extract left and right images from side-by-side
left = frameBGR(cv::Rect(0, 0, frameBGR.cols / 2, frameBGR.rows));
cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
if(rightGrayScale_)
cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
else
right = frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows));
// <---- Extract left and right images from side-by-side
if(stereoModel_.isValidForRectification())
@@ -792,4 +795,11 @@ SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info)
return data;
}
void CameraStereoZedOC::setRightGrayScale(bool enabled)
{
#ifdef RTABMAP_ZEDOC
rightGrayScale_ = enabled;
#endif
}
} // namespace rtabmap
corelib/src/odometry/OdometryF2M.cpp
+167 -29
View File
@@ -60,6 +60,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
initDepthFactor_(Parameters::defaultOdomF2MInitDepthFactor()),
floorThreshold_(Parameters::defaultOdomF2MFloorThreshold()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
@@ -67,6 +69,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
scanMapMaxRange_(Parameters::defaultOdomF2MScanRange()),
bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
bundleMinMotion_(Parameters::defaultOdomF2MBundleAdjustmentMinMotion()),
bundleMaxKeyFramesPerFeature_(Parameters::defaultOdomF2MBundleAdjustmentMaxKeyFramesPerFeature()),
bundleUpdateFeatureMapOnAllFrames_(Parameters::defaultOdomF2MBundleUpdateFeatureMapOnAllFrames()),
validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()),
pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()),
@@ -81,6 +86,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
Parameters::parse(parameters, Parameters::kOdomF2MInitDepthFactor(), initDepthFactor_);
Parameters::parse(parameters, Parameters::kOdomF2MFloorThreshold(), floorThreshold_);
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
@@ -91,6 +98,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomF2MScanRange(), scanMapMaxRange_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMinMotion(), bundleMinMotion_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature(), bundleMaxKeyFramesPerFeature_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleUpdateFeatureMapOnAllFrames(), bundleUpdateFeatureMapOnAllFrames_);
Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_);
@@ -120,6 +130,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
UASSERT(visKeyFrameThr_>=0);
UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
UASSERT(maxNewFeatures_ >= 0);
UASSERT(initDepthFactor_>0.0f);
int corType = Parameters::defaultVisCorType();
Parameters::parse(parameters, Parameters::kVisCorType(), corType);
@@ -280,6 +291,7 @@ Transform OdometryF2M::computeTransform(
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, std::vector<CameraModel> > bundleModels;
float bundleAvgInlierDistance = 0.0f;
for(int guessIteration=0;
guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
@@ -386,6 +398,7 @@ Transform OdometryF2M::computeTransform(
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, FeatureBA> > wordReferences;
size_t maxKeyFramesForInlier = 0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
int wordId =regInfo.inliersIDs[i];
@@ -398,6 +411,10 @@ Transform OdometryF2M::computeTransform(
// all other references
std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
if(info && refIter->second.size() > maxKeyFramesForInlier)
{
maxKeyFramesForInlier = refIter->second.size();
}
std::map<int, FeatureBA> references;
int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
@@ -472,6 +489,7 @@ Transform OdometryF2M::computeTransform(
{
info->localBundlePoses = bundlePoses;
info->localBundleModels = bundleModels;
info->localBundleMaxKeyFramesForInlier = maxKeyFramesForInlier;
}
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
@@ -534,13 +552,52 @@ Transform OdometryF2M::computeTransform(
regInfo.covariance.at<double>(4,4) *= 0.1;
if(regInfo.covariance.at<double>(5,5)>thrAng)
regInfo.covariance.at<double>(5,5) *= 0.1;
// Estimate how much the new frame moved from previous frame in term of pixels
if(bundleMinMotion_ > 0.0f)
{
UASSERT(!bundlePoses_.empty());
int count = 0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
std::map<int, std::map<int, FeatureBA> >::iterator wter = wordReferences.find(regInfo.inliersIDs[i]);
if(wter != wordReferences.end())
{
std::map<int, FeatureBA>::iterator fter = wter->second.find(bundlePoses_.rbegin()->first);
if(fter != wter->second.end())
{
const FeatureBA & f1 = fter->second; // previous key-frame
const FeatureBA & f2 = wter->second.find(lastFrame_->id())->second; // current key-frame
float dx = f1.kpt.pt.x - f2.kpt.pt.x;
float dy = f1.kpt.pt.y - f2.kpt.pt.y;
bundleAvgInlierDistance += sqrt(dx*dx + dy*dy);
++count;
}
}
}
if(count)
{
bundleAvgInlierDistance /= count;
}
UDEBUG("Average pixel distance between %d inliers: %f", count, bundleAvgInlierDistance);
if(info)
{
info->localBundleAvgInlierDistance = bundleAvgInlierDistance;
}
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
}
else
{
regInfo.rejectedMsg = "Last bundle pose is null?!";
transform.setNull();
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
}
else
{
UWARN("Local bundle adjustment failed! transform is not refined.");
regInfo.rejectedMsg = "Local bundle adjustment failed!";
transform.setNull();
}
}
}
@@ -594,19 +651,73 @@ Transform OdometryF2M::computeTransform(
std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();
// update last frame features without depth (if bundle adjustment was done)
// Do this before adding bundle frames to keep mono observations without depth
bool lastFrameWords3Updated = false;
std::vector<cv::Point3f> lastFrameWords3;
if( regPipeline_->isImageRequired() &&
!visDepthAsMask &&
bundleAdjustment_>0 &&
!lastFrame_->getWords().empty() &&
lastFrame_->getWords().size() == lastFrame_->getWords3().size() &&
!points3DMap.empty())
{
lastFrameWords3 = lastFrame_->getWords3();
Transform newFramePoseInv = newFramePose.inverse();
for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin();
iter!=lastFrame_->getWords().end();
++iter)
{
cv::Point3f & pt = lastFrameWords3.at(iter->second);
if(!util3d::isFinite(pt))
{
std::map<int, cv::Point3f>::iterator mapIter = points3DMap.find(iter->first);
if(mapIter != points3DMap.end())
{
// in base frame
pt = util3d::transformPoint(mapIter->second, newFramePoseInv);
lastFrameWords3Updated = true;
}
}
}
}
if( regPipeline_->isImageRequired() &&
bundleAdjustment_>0 &&
bundleUpdateFeatureMapOnAllFrames_ &&
!points3DMap.empty())
{
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
UASSERT(mapWords.count(iter->first) == 1);
mapPoints[mapWords.find(iter->first)->second] = iter->second;
}
modified = true;
}
bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
(keyFrameThr_ == 0.0f ||
visKeyFrameThr_ == 0 ||
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
regInfo.inliers <= visKeyFrameThr_);
regInfo.inliers <= visKeyFrameThr_) &&
(bundleAdjustment_==0 || bundleAvgInlierDistance >= bundleMinMotion_);
bool addGeometricKeyFrame = regPipeline_->isScanRequired() &&
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
addKeyFrame = addVisualKeyFrame || addGeometricKeyFrame;
UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d (avg dist=%f, min=%f) features=%d mp=%d",
keyFrameThr_,
visKeyFrameThr_,
regInfo.matches,
regInfo.inliers,
bundleAvgInlierDistance,
bundleMinMotion_,
(int)lastFrame_->sensorData().keypoints().size(),
(int)mapPoints.size());
UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, visKeyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
if(addKeyFrame)
{
//Visual
@@ -615,6 +726,7 @@ Transform OdometryF2M::computeTransform(
UTimer tmpTimer;
UDEBUG("Update local map");
modified = bundleAdjustment_>0; // We always add new references even if we don't add/remove points
// update local map
UASSERT(mapWords.size() == mapPoints.size());
@@ -639,12 +751,14 @@ Transform OdometryF2M::computeTransform(
bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
if(!bundleUpdateFeatureMapOnAllFrames_)
{
UASSERT(mapWords.count(iter->first) == 1);
//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z);
mapPoints[mapWords.find(iter->first)->second] = iter->second;
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
UASSERT(mapWords.count(iter->first) == 1);
mapPoints[mapWords.find(iter->first)->second] = iter->second;
}
}
}
@@ -733,7 +847,16 @@ Transform OdometryF2M::computeTransform(
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
std::map<int, rtabmap::FeatureBA> & keyframes = bundleWordReferences_.find(iter->first)->second;
if(bundleMaxKeyFramesPerFeature_ != 0 && (int)keyframes.size() > bundleMaxKeyFramesPerFeature_)
{
// To keep number of keyframes looking at same feature bounded
int frameId = keyframes.rbegin()->first;
UASSERT(bundlePoseReferences_.find(frameId) != bundlePoseReferences_.end());
bundlePoseReferences_.at(frameId) -= 1;
keyframes.erase(frameId);
}
keyframes.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
}
}
}
@@ -750,6 +873,28 @@ Transform OdometryF2M::computeTransform(
if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
{
int cameraIndex = iter->second.second.second.second.second;
cv::Point3f pt = iter->second.second.second.first;
if(!util3d::isFinite(pt))
{
// get the ray instead
float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
float y = iter->second.second.first.pt.y;
Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
lastFrameModels[cameraIndex].imageSize(),
x,
y,
lastFrameModels[cameraIndex].cx(),
lastFrameModels[cameraIndex].cy(),
lastFrameModels[cameraIndex].fx(),
lastFrameModels[cameraIndex].fy());
float scaleInf = initDepthFactor_ * lastFrameModels[cameraIndex].fx();
pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
}
if(floorThreshold_ != 0.0f && pt.z < floorThreshold_)
{
continue;
}
if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->second.first) == 1)
@@ -778,23 +923,6 @@ Transform OdometryF2M::computeTransform(
mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
mapWordsKpts.push_back(iter->second.second.first);
cv::Point3f pt = iter->second.second.second.first;
if(!util3d::isFinite(pt))
{
// get the ray instead
float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
float y = iter->second.second.first.pt.y;
Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
lastFrameModels[cameraIndex].imageSize(),
x,
y,
lastFrameModels[cameraIndex].cx(),
lastFrameModels[cameraIndex].cy(),
lastFrameModels[cameraIndex].fx(),
lastFrameModels[cameraIndex].fy());
float scaleInf = (0.05 * lastFrameModels[cameraIndex].fx()) / 0.01;
pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
}
mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
mapDescriptors.push_back(iter->second.second.second.second.first);
if(lastFrameOldestNewId_ > iter->second.first)
@@ -803,6 +931,10 @@ Transform OdometryF2M::computeTransform(
}
++added;
}
else
{
break;
}
}
UDEBUG("");
@@ -1125,6 +1257,12 @@ Transform OdometryF2M::computeTransform(
map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
}
if(lastFrameWords3Updated)
{
// update output with refined 3d points from bundle adjustment
data.setFeatures(lastFrame_->getWordsKpts(), lastFrameWords3, lastFrame_->getWordsDescriptors());
}
}
if(info)
corelib/src/odometry/OdometryMono.cpp
+9 -5
View File
@@ -205,17 +205,21 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
int nFeatures = 0;
// convert to grayscale
if(data.imageRaw().channels() > 1)
if(data.imageRaw().channels() > 1 || data.rightRaw().channels() > 1)
{
cv::Mat newFrame;
cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
cv::Mat newFrame = data.imageRaw();
cv::Mat newFrameRight = data.rightRaw();
if(data.imageRaw().channels() > 1)
cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
if(data.rightRaw().channels() > 1)
cv::cvtColor(data.rightRaw(), newFrameRight, cv::COLOR_BGR2GRAY);
if(!data.stereoCameraModels().empty())
{
data.setStereoImage(newFrame, data.rightRaw(), data.stereoCameraModels());
data.setStereoImage(newFrame, newFrameRight, data.stereoCameraModels());
}
else
{
data.setRGBDImage(newFrame, data.depthRaw(), data.cameraModels());
data.setRGBDImage(newFrame, newFrameRight, data.cameraModels());
}
}
corelib/src/odometry/OdometryORBSLAM3.cpp
+7 -2
View File
@@ -448,11 +448,16 @@ Transform OdometryORBSLAM3::computeTransform(
{
localTransform = data.stereoCameraModels()[0].localTransform();
cv::Mat leftMono = data.imageRaw();
if(data.imageRaw().channels() == 3) {
if(data.imageRaw().channels() == 3) {
leftMono = cv::Mat();
cv::cvtColor(data.imageRaw(), leftMono, CV_BGR2GRAY);
}
Tcw = orbslam_->TrackStereo(leftMono, data.rightRaw(), data.stamp(), orbslamImus_);
cv::Mat rightMono = data.rightRaw();
if(data.rightRaw().channels() == 3) {
rightMono = cv::Mat();
cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY);
}
Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_);
orbslamImus_.clear();
}
else
corelib/src/stereo/StereoBM.cpp
+14 -3
View File
@@ -82,7 +82,8 @@ cv::Mat StereoBM::computeDisparity(
{
UASSERT(!leftImage.empty() && !rightImage.empty());
UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
UASSERT(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3);
UASSERT(rightImage.type() == CV_8UC1 || rightImage.type() == CV_8UC3);
cv::Mat leftMono;
if(leftImage.channels() == 3)
@@ -94,6 +95,16 @@ cv::Mat StereoBM::computeDisparity(
leftMono = leftImage;
}
cv::Mat rightMono;
if(rightImage.channels() == 3)
{
cv::cvtColor(rightImage, rightMono, CV_BGR2GRAY);
}
else
{
rightMono = rightImage;
}
cv::Mat disparity;
#if CV_MAJOR_VERSION < 3
cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
@@ -106,7 +117,7 @@ cv::Mat StereoBM::computeDisparity(
stereo.state->textureThreshold = textureThreshold_;
stereo.state->speckleWindowSize = speckleWindowSize_;
stereo.state->speckleRange = speckleRange_;
stereo(leftMono, rightImage, disparity, CV_16SC1);
stereo(leftMono, rightMono, disparity, CV_16SC1);
#else
cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
stereo->setBlockSize(blockSize_);
@@ -119,7 +130,7 @@ cv::Mat StereoBM::computeDisparity(
stereo->setSpeckleWindowSize(speckleWindowSize_);
stereo->setSpeckleRange(speckleRange_);
stereo->setDisp12MaxDiff(disp12MaxDiff_);
stereo->compute(leftMono, rightImage, disparity);
stereo->compute(leftMono, rightMono, disparity);
#endif
if(minDisparity_>0)
corelib/src/stereo/StereoSGBM.cpp
+14 -3
View File
@@ -71,7 +71,8 @@ cv::Mat StereoSGBM::computeDisparity(
{
UASSERT(!leftImage.empty() && !rightImage.empty());
UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
UASSERT(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3);
UASSERT(rightImage.type() == CV_8UC1 || rightImage.type() == CV_8UC3);
cv::Mat leftMono;
if(leftImage.channels() == 3)
@@ -83,6 +84,16 @@ cv::Mat StereoSGBM::computeDisparity(
leftMono = leftImage;
}
cv::Mat rightMono;
if(rightImage.channels() == 3)
{
cv::cvtColor(rightImage, rightMono, CV_BGR2GRAY);
}
else
{
rightMono = rightImage;
}
cv::Mat disparity;
#if CV_MAJOR_VERSION < 3
cv::StereoSGBM stereo(
@@ -97,7 +108,7 @@ cv::Mat StereoSGBM::computeDisparity(
speckleWindowSize_,
speckleRange_,
mode_==1);
stereo(leftMono, rightImage, disparity);
stereo(leftMono, rightMono, disparity);
#else
cv::Ptr<cv::StereoSGBM> stereo = cv::StereoSGBM::create(
minDisparity_,
@@ -111,7 +122,7 @@ cv::Mat StereoSGBM::computeDisparity(
speckleWindowSize_,
speckleRange_,
mode_);
stereo->compute(leftMono, rightImage, disparity);
stereo->compute(leftMono, rightMono, disparity);
#endif
if(minDisparity_>0)
corelib/src/util3d.cpp
+24 -4
View File
@@ -822,14 +822,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
const ParametersMap & parameters)
{
UASSERT(!imageLeft.empty() && !imageRight.empty());
UASSERT(imageRight.type() == CV_8UC1);
UASSERT(imageRight.type() == CV_8UC1 || imageRight.type() == CV_8UC3);
UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
UASSERT(imageLeft.rows == imageRight.rows &&
imageLeft.cols == imageRight.cols);
UASSERT(decimation >= 1.0f);
cv::Mat leftColor = imageLeft;
cv::Mat rightMono = imageRight;
cv::Mat rightColor = imageRight;
cv::Mat leftMono;
if(leftColor.channels() == 3)
@@ -841,6 +841,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
leftMono = leftColor;
}
cv::Mat rightMono;
if(rightColor.channels() == 3)
{
cv::cvtColor(rightColor, rightMono, CV_BGR2GRAY);
}
else
{
rightMono = rightColor;
}
return cloudFromDisparityRGB(
leftColor,
util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
@@ -954,7 +964,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
{
//stereo
UASSERT(sensorData.rightRaw().type() == CV_8UC1);
UASSERT(sensorData.rightRaw().type() == CV_8UC1 || sensorData.rightRaw().type() == CV_8UC3);
cv::Mat leftMono;
if(sensorData.imageRaw().channels() == 3)
@@ -966,6 +976,16 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
leftMono = sensorData.imageRaw();
}
cv::Mat rightMono;
if(sensorData.rightRaw().channels() == 3)
{
cv::cvtColor(sensorData.rightRaw(), rightMono, CV_BGR2GRAY);
}
else
{
rightMono = sensorData.rightRaw();
}
UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
@@ -979,7 +999,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
if(sensorData.stereoCameraModels()[i].isValidForProjection())
{
cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
StereoCameraModel model = sensorData.stereoCameraModels()[i];
if( roiRatios.size() == 4 &&
((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
corelib/src/util3d_mapping.cpp
+6 -14
View File
@@ -703,10 +703,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
if(end!=start)
{
if(localScans.size() > 1 || map.at<signed char>(end.y, end.x) != 0)
{
rayTrace(start, end, map, true); // trace free space
}
rayTrace(start, end, map, true); // trace free space
}
}
// ray tracing for no hits
@@ -730,14 +727,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
if(end!=start)
{
if(localScans.size() > 1 || map.at<signed char>(end.y, end.x) != 0)
{
rayTrace(start, end, map, true); // trace free space
if(map.at<signed char>(end.y, end.x) == -1)
{
map.at<signed char>(end.y, end.x) = 0; // empty
}
}
rayTrace(start, end, map, true); // trace free space
}
}
}
@@ -823,8 +813,6 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
{
UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
cv::Point2i ptA, ptB;
ptA = start;
@@ -832,6 +820,10 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
// clip end point
ptB.x = std::min(std::max(ptB.x, 0), grid.cols-1);
ptB.y = std::min(std::max(ptB.y, 0), grid.rows-1);
bool swapped = false;
if(slope<-1.0f || slope>1.0f)
{
docker/jammy/Dockerfile
-17
View File
@@ -2,23 +2,6 @@
FROM introlab3it/rtabmap:jammy-deps
# June 19 2023: moved opengv here so that jammy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI.
#commit Aug 6 2020
RUN apt-get update && apt install -y wget && \
apt-get clean && rm -rf /var/lib/apt/lists/
RUN git clone https://github.com/laurentkneip/opengv.git && \
cd opengv && \
git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \
wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \
git apply opengv_disable_march_native.patch && \
mkdir build && \
cd build && \
cmake -DCMAKE_BUILD_TYPE=Release .. && \
make -j$(nproc) && \
make install && \
cd && \
rm -r opengv
# Will be used to read/store databases on host
RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
docker/jammy/deps/Dockerfile
+19 -10
View File
@@ -24,18 +24,11 @@ RUN apt-get update && \
apt-get install -y git libtbb-dev ros-humble-ros-base ros-dev-tools && \
apt-get clean && rm -rf /var/lib/apt/lists/
RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt-get update && \
RUN apt-get update && \
apt upgrade -y && \
apt-get install -y libfreenect-dev ros-humble-rtabmap-ros && \
apt-get remove -y ros-humble-rtabmap* && \
apt-get clean && rm -rf /var/lib/apt/lists/; fi
# current issue that ros-humble-rtabmap-ros is not available on arm64
RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then apt-get update && \
apt upgrade -y && \
apt-get install -y libfreenect-dev ros-humble-rtabmap-launch && \
apt-get remove -y ros-humble-rtabmap* && \
apt-get clean && rm -rf /var/lib/apt/lists/; fi
apt-get remove -y ros-humble-rtabmap* ros-humble-grid-map-core && \
apt-get clean && rm -rf /var/lib/apt/lists/
WORKDIR /root/
@@ -86,6 +79,22 @@ RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \
cd ../.. && \
rm -rf opencv opencv_contrib
#commit Aug 6 2020
RUN apt-get update && apt install -y wget && \
apt-get clean && rm -rf /var/lib/apt/lists/
RUN git clone https://github.com/laurentkneip/opengv.git && \
cd opengv && \
git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \
wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \
git apply opengv_disable_march_native.patch && \
mkdir build && \
cd build && \
cmake -DCMAKE_BUILD_TYPE=Release .. && \
make -j$(nproc) && \
make install && \
cd && \
rm -r opengv
RUN rm /bin/sh && ln -s /bin/bash /bin/sh
COPY ./docker/jammy/deps/ros_entrypoint.sh /ros_entrypoint.sh
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -278,6 +278,7 @@ public:
bool isSourceFeatureDetection() const;
bool isSourceStereoDepthGenerated() const;
bool isSourceStereoExposureCompensation() const;
bool isRightGrayScale() const;
bool isSourceScanFromDepth() const;
bool isSourceScanDeskewing() const;
int getSourceScanDownsampleStep() const;
guilib/src/CameraViewer.cpp
+61 -25
View File
@@ -132,9 +132,23 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
{
processingImages_ = true;
QString sizes;
imageView_->setVisible(!data.imageRaw().empty() || !data.imageRaw().empty());
cv::Mat left;
cv::Mat depthOrRight;
LaserScan scan;
if( !data.imageRaw().empty() || !data.imageCompressed().empty() ||
!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty() ||
!data.laserScanRaw().empty() || !data.laserScanCompressed().empty())
{
data.uncompressDataConst(
!data.imageRaw().empty() || !data.imageCompressed().empty()?&left:0,
!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty()?&depthOrRight:0,
!data.laserScanRaw().empty() || !data.laserScanCompressed().empty()?&scan:0);
}
imageView_->setVisible(!left.empty() || !left.empty());
std::map<int, MarkerInfo> detections;
if(!data.imageRaw().empty())
if(!left.empty())
{
std::vector<CameraModel> models;
if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
@@ -152,36 +166,58 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
if(!models.empty() && models[0].isValidForProjection())
{
cv::Mat imageWithDetections;
detections = markerDetector_->detect(data.imageRaw(), models, data.depthRaw(), std::map<int, float>(), &imageWithDetections);
detections = markerDetector_->detect(left, models, depthOrRight, std::map<int, float>(), &imageWithDetections);
imageView_->setImage(uCvMat2QImage(imageWithDetections));
}
else
{
imageView_->setImage(uCvMat2QImage(data.imageRaw()));
imageView_->setImage(uCvMat2QImage(left));
}
sizes.append(QString("Color=%1x%2").arg(data.imageRaw().cols).arg(data.imageRaw().rows));
sizes.append(QString("Color=%1x%2").arg(left.cols).arg(left.rows));
}
if(!data.depthOrRightRaw().empty())
if(!depthOrRight.empty())
{
imageView_->setImageDepth(data.depthOrRightRaw());
sizes.append(QString(" Depth=%1x%2").arg(data.depthOrRightRaw().cols).arg(data.depthOrRightRaw().rows));
imageView_->setImageDepth(depthOrRight);
sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows));
}
imageSizeLabel_->setText(sizes);
if(!data.depthOrRightRaw().empty() &&
if(!depthOrRight.empty() &&
((data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()) || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
{
if(showCloudCheckbox_->isChecked())
{
if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
if(!left.empty() && !depthOrRight.empty())
{
showCloudCheckbox_->setEnabled(true);
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
if(data.imageRaw().empty())
{
if(!data.stereoCameraModels().empty())
{
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.stereoCameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
}
else
{
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
}
}
else
{
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
}
}
else if(!data.depthOrRightRaw().empty())
else if(!depthOrRight.empty())
{
showCloudCheckbox_->setEnabled(true);
cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
if(data.depthOrRightRaw().empty())
{
cloudView_->addCloud("cloud", util3d::cloudFromSensorData(SensorData(cv::Mat(), depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
}
else
{
cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
}
}
// Add landmarks to 3D Map view
@@ -208,37 +244,37 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
}
}
if(!data.laserScanRaw().isEmpty())
if(!scan.isEmpty())
{
showScanCheckbox_->setEnabled(true);
if(showScanCheckbox_->isChecked())
{
if(data.laserScanRaw().hasNormals())
if(scan.hasNormals())
{
if(data.laserScanRaw().hasIntensity())
if(scan.hasIntensity())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
else if(data.laserScanRaw().hasRGB())
else if(scan.hasRGB())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
else
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
}
else if(data.laserScanRaw().hasIntensity())
else if(scan.hasIntensity())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
else if(data.laserScanRaw().hasRGB())
else if(scan.hasRGB())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
else
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
}
}
guilib/src/DatabaseViewer.cpp
+15 -6
View File
@@ -4850,7 +4850,7 @@ void DatabaseViewer::update(int value,
{
keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
}
view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1||data.depthOrRightRaw().type() == CV_8UC3?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
}
Transform odomPose, g;
@@ -5088,7 +5088,7 @@ void DatabaseViewer::update(int value,
}
//stereo
if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
if(!data.depthOrRightRaw().empty() && (data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3))
{
this->updateStereo(&data);
}
@@ -5782,7 +5782,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
ui_->dockWidget_stereoView->isVisible() &&
!data->imageRaw().empty() &&
!data->depthOrRightRaw().empty() &&
data->depthOrRightRaw().type() == CV_8UC1 &&
(data->depthOrRightRaw().type() == CV_8UC1 || data->depthOrRightRaw().type() == CV_8UC3) &&
data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras
data->stereoCameraModels()[0].isValidForProjection())
{
@@ -5795,6 +5795,15 @@ void DatabaseViewer::updateStereo(const SensorData * data)
{
leftMono = data->imageRaw();
}
cv::Mat rightMono;
if(data->rightRaw().channels() == 3)
{
cv::cvtColor(data->rightRaw(), rightMono, CV_BGR2GRAY);
}
else
{
rightMono = data->rightRaw();
}
UTimer timer;
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
@@ -5826,7 +5835,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
rightCorners = stereo->computeCorrespondences(
leftMono,
data->rightRaw(),
rightMono,
leftCorners,
status);
delete stereo;
@@ -6435,7 +6444,7 @@ void DatabaseViewer::updateConstraintView(
}
dataFrom.uncompressData();
UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type()==CV_8UC3 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
if(signatureTo.id()>0)
{
@@ -6447,7 +6456,7 @@ void DatabaseViewer::updateConstraintView(
}
dataTo.uncompressData();
UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type()==CV_8UC3 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
// get odom pose
Transform pose = Transform::getIdentity();
guilib/src/MainWindow.cpp
+4
View File
@@ -659,6 +659,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false);
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false);
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false);
_ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", false);
_ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", false);
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false);
_ui->statsToolBox->updateStat("Odometry/Interval/ms", false);
_ui->statsToolBox->updateStat("Odometry/Speed/kph", false);
@@ -1829,6 +1831,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleAvgInlierDistance, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleMaxKeyFramesForInlier, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures());
guilib/src/PreferencesDialog.cpp
+97 -36
View File
@@ -742,7 +742,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_database_cameraIndex, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_lineEdit_databaseCameraIndex, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_stereoToDepthDB, SIGNAL(toggled(bool)), _ui->checkbox_stereo_depthGenerated, SLOT(setChecked(bool)));
connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(toggled(bool)), _ui->source_checkBox_stereoToDepthDB, SLOT(setChecked(bool)));
@@ -858,13 +858,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->spinBox_stereoMyntEye_contrast, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_stereoMyntEye_irControl, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_depthai_image_width, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_depthai_output_mode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_depthai_conf_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_depthai_lrc_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_depthai_extended_disparity, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_depthai_disparity_companding, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_depthai_subpixel_fractional_bits, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_depthai_disparity_companding, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_depthai_disparity_width, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_depthai_median_filter, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_depthai_use_spec_translation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_depthai_alpha_scaling, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_depthai_imu_published, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -880,6 +882,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->checkbox_source_feature_detection, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_stereo_exposureCompensation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_stereo_rightGrayScale, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple()));
connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath()));
@@ -1403,13 +1406,18 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
//Odometry Frame to Map
_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
_ui->doubleSpinBox_odom_f2m_floorThreshold->setObjectName(Parameters::kOdomF2MFloorThreshold().c_str());
_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str());
_ui->doubleSpinBox_odom_f2m_scanAngle->setObjectName(Parameters::kOdomF2MScanSubtractAngle().c_str());
_ui->doubleSpinBox_odom_f2m_scanRange->setObjectName(Parameters::kOdomF2MScanRange().c_str());
_ui->odom_f2m_validDepthRatio->setObjectName(Parameters::kOdomF2MValidDepthRatio().c_str());
_ui->odom_f2m_initDepthFactor->setObjectName(Parameters::kOdomF2MInitDepthFactor().c_str());
_ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str());
_ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str());
_ui->odom_f2m_bundleMinMotion->setObjectName(Parameters::kOdomF2MBundleAdjustmentMinMotion().c_str());
_ui->odom_f2m_bundleMaxKeyFramesPerFeature->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature().c_str());
_ui->odom_f2m_bundleUpdateFeatureMapOnAllFrames->setObjectName(Parameters::kOdomF2MBundleUpdateFeatureMapOnAllFrames().c_str());
//Odometry Frame To Frame
_ui->comboBox_odomf2f_corType->setObjectName(Parameters::kVisCorType().c_str());
@@ -2134,7 +2142,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->source_checkBox_ignorePriors->setChecked(false);
_ui->source_spinBox_databaseStartId->setValue(0);
_ui->source_spinBox_databaseStopId->setValue(0);
_ui->source_spinBox_database_cameraIndex->setValue(-1);
_ui->source_lineEdit_databaseCameraIndex->setText("");
_ui->source_checkBox_useDbStamps->setChecked(true);
#ifdef _WIN32
@@ -2172,6 +2180,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkbox_source_feature_detection->setChecked(false);
_ui->checkbox_stereo_depthGenerated->setChecked(false);
_ui->checkBox_stereo_exposureCompensation->setChecked(false);
_ui->checkBox_stereo_rightGrayScale->setChecked(true);
_ui->openni2_autoWhiteBalance->setChecked(true);
_ui->openni2_autoExposure->setChecked(true);
_ui->openni2_exposure->setValue(0);
@@ -2249,12 +2258,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->spinBox_stereoMyntEye_brightness->setValue(120);
_ui->spinBox_stereoMyntEye_contrast->setValue(116);
_ui->spinBox_stereoMyntEye_irControl->setValue(0);
_ui->comboBox_depthai_resolution->setCurrentIndex(1);
_ui->comboBox_depthai_output_mode->setCurrentIndex(0);
_ui->comboBox_depthai_image_width->setCurrentIndex(1);
_ui->comboBox_depthai_output_mode->setCurrentIndex(1);
_ui->spinBox_depthai_conf_threshold->setValue(200);
_ui->checkBox_depthai_extended_disparity->setChecked(false);
_ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(2);
_ui->comboBox_depthai_disparity_companding->setCurrentIndex(1);
_ui->checkBox_depthai_disparity_companding->setChecked(false);
_ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(1);
_ui->comboBox_depthai_disparity_width->setCurrentIndex(1);
_ui->comboBox_depthai_median_filter->setCurrentIndex(2);
_ui->spinBox_depthai_lrc_threshold->setValue(5);
_ui->checkBox_depthai_use_spec_translation->setChecked(false);
_ui->doubleSpinBox_depthai_alpha_scaling->setValue(-1);
@@ -2646,6 +2657,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->comboBox_cameraStereo->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraStereo->currentIndex()).toInt());
_ui->checkbox_stereo_depthGenerated->setChecked(settings.value("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked()).toBool());
_ui->checkBox_stereo_exposureCompensation->setChecked(settings.value("exposureCompensation", _ui->checkBox_stereo_exposureCompensation->isChecked()).toBool());
_ui->checkBox_stereo_rightGrayScale->setChecked(settings.value("rightGrayScale", _ui->checkBox_stereo_rightGrayScale->isChecked()).toBool());
settings.endGroup(); // stereo
settings.beginGroup("rgb");
@@ -2761,13 +2773,15 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.endGroup(); // MyntEye
settings.beginGroup("DepthAI");
_ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt());
_ui->comboBox_depthai_image_width->setCurrentIndex(settings.value("image_width", _ui->comboBox_depthai_image_width->currentIndex()).toInt());
_ui->comboBox_depthai_output_mode->setCurrentIndex(settings.value("output_mode", _ui->comboBox_depthai_output_mode->currentIndex()).toInt());
_ui->spinBox_depthai_conf_threshold->setValue(settings.value("conf_threshold", _ui->spinBox_depthai_conf_threshold->value()).toInt());
_ui->spinBox_depthai_lrc_threshold->setValue(settings.value("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value()).toInt());
_ui->checkBox_depthai_extended_disparity->setChecked(settings.value("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked()).toBool());
_ui->checkBox_depthai_disparity_companding->setChecked(settings.value("disparity_companding", _ui->checkBox_depthai_disparity_companding->isChecked()).toBool());
_ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(settings.value("subpixel_fractional_bits", _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()).toInt());
_ui->comboBox_depthai_disparity_companding->setCurrentIndex(settings.value("companding", _ui->comboBox_depthai_disparity_companding->currentIndex()).toInt());
_ui->comboBox_depthai_disparity_width->setCurrentIndex(settings.value("disparity_width", _ui->comboBox_depthai_disparity_width->currentIndex()).toInt());
_ui->comboBox_depthai_median_filter->setCurrentIndex(settings.value("median_filter", _ui->comboBox_depthai_median_filter->currentIndex()).toInt());
_ui->checkBox_depthai_use_spec_translation->setChecked(settings.value("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()).toBool());
_ui->doubleSpinBox_depthai_alpha_scaling->setValue(settings.value("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()).toDouble());
_ui->checkBox_depthai_imu_published->setChecked(settings.value("imu_published", _ui->checkBox_depthai_imu_published->isChecked()).toBool());
@@ -2865,7 +2879,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool());
_ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt());
_ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt());
_ui->source_spinBox_database_cameraIndex->setValue(settings.value("cameraIndex", _ui->source_spinBox_database_cameraIndex->value()).toInt());
_ui->source_lineEdit_databaseCameraIndex->setText(settings.value("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text()).toString());
_ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool());
settings.endGroup(); // Database
@@ -3248,6 +3262,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("driver", _ui->comboBox_cameraStereo->currentIndex());
settings.setValue("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked());
settings.setValue("exposureCompensation", _ui->checkBox_stereo_exposureCompensation->isChecked());
settings.setValue("rightGrayScale", _ui->checkBox_stereo_rightGrayScale->isChecked());
settings.endGroup(); // stereo
settings.beginGroup("rgb");
@@ -3363,20 +3378,22 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.endGroup(); // MyntEye
settings.beginGroup("DepthAI");
settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex());
settings.setValue("output_mode", _ui->comboBox_depthai_output_mode->currentIndex());
settings.setValue("conf_threshold", _ui->spinBox_depthai_conf_threshold->value());
settings.setValue("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value());
settings.setValue("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked());
settings.setValue("image_width", _ui->comboBox_depthai_image_width->currentIndex());
settings.setValue("output_mode", _ui->comboBox_depthai_output_mode->currentIndex());
settings.setValue("conf_threshold", _ui->spinBox_depthai_conf_threshold->value());
settings.setValue("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value());
settings.setValue("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked());
settings.setValue("disparity_companding", _ui->checkBox_depthai_disparity_companding->isChecked());
settings.setValue("subpixel_fractional_bits", _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex());
settings.setValue("companding", _ui->comboBox_depthai_disparity_companding->currentIndex());
settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked());
settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value());
settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked());
settings.setValue("dot_intensity", _ui->doubleSpinBox_depthai_dot_intensity->value());
settings.setValue("flood_intensity", _ui->doubleSpinBox_depthai_flood_intensity->value());
settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex());
settings.setValue("blob_path", _ui->lineEdit_depthai_blob_path->text());
settings.setValue("disparity_width", _ui->comboBox_depthai_disparity_width->currentIndex());
settings.setValue("median_filter", _ui->comboBox_depthai_median_filter->currentIndex());
settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked());
settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value());
settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked());
settings.setValue("dot_intensity", _ui->doubleSpinBox_depthai_dot_intensity->value());
settings.setValue("flood_intensity", _ui->doubleSpinBox_depthai_flood_intensity->value());
settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex());
settings.setValue("blob_path", _ui->lineEdit_depthai_blob_path->text());
settings.endGroup(); // DepthAI
settings.beginGroup("Images");
@@ -3465,7 +3482,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked());
settings.setValue("startId", _ui->source_spinBox_databaseStartId->value());
settings.setValue("stopId", _ui->source_spinBox_databaseStopId->value());
settings.setValue("cameraIndex", _ui->source_spinBox_database_cameraIndex->value());
settings.setValue("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text());
settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked());
settings.endGroup(); // Database
@@ -4285,8 +4302,8 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant)
else if(src == kSrcStereoDepthAI) // OAK-D-Pro (variant==2), OAK-D (variant==1), OAK-D Lite (variant==0)
{
_ui->checkBox_depthai_imu_published->setChecked(variant >= 1);
_ui->comboBox_depthai_resolution->setCurrentIndex(variant >= 1?1:3);
_ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:0);
_ui->comboBox_depthai_image_width->setCurrentIndex(1);
_ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:1);
_ui->doubleSpinBox_depthai_dot_intensity->setValue(variant==2?1:0);
}
}
@@ -4387,7 +4404,7 @@ void PreferencesDialog::selectSourceDatabase()
_ui->source_database_lineEdit_path->setText(paths.size()==1?paths.front():paths.join(";"));
_ui->source_spinBox_databaseStartId->setValue(0);
_ui->source_spinBox_databaseStopId->setValue(0);
_ui->source_spinBox_database_cameraIndex->setValue(-1);
_ui->source_lineEdit_databaseCameraIndex->setText("");
}
}
@@ -6426,6 +6443,10 @@ bool PreferencesDialog::isSourceStereoExposureCompensation() const
{
return _ui->checkBox_stereo_exposureCompensation->isChecked();
}
bool PreferencesDialog::isRightGrayScale() const
{
return _ui->checkBox_stereo_rightGrayScale->isChecked();
}
bool PreferencesDialog::isSourceScanFromDepth() const
{
return _ui->checkBox_source_scanFromDepth->isChecked();
@@ -6748,8 +6769,8 @@ Camera * PreferencesDialog::createCamera(
_ui->checkBox_cameraImages_timestamps->isChecked(),
_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
((CameraStereoImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
((CameraStereoImages*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if (driver == kSrcStereoUsb)
{
@@ -6775,6 +6796,7 @@ Camera * PreferencesDialog::createCamera(
{
((CameraStereoVideo*)camera)->setFOURCC(_ui->lineEdit_stereousbcam_fourcc->text().toStdString());
}
((CameraStereoVideo*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if(driver == kSrcStereoVideo)
{
@@ -6797,6 +6819,7 @@ Camera * PreferencesDialog::createCamera(
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
((CameraStereoVideo*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if (driver == kSrcStereoTara)
@@ -6846,6 +6869,7 @@ Camera * PreferencesDialog::createCamera(
_ui->checkbox_publishInterIMU->isChecked(),
_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?
IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0);
((CameraStereoZed*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if (driver == kSrcStereoZedOC)
{
@@ -6855,25 +6879,25 @@ Camera * PreferencesDialog::createCamera(
_ui->comboBox_stereoZedOC_resolution->currentIndex(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraStereoZedOC*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if (driver == kSrcStereoDepthAI)
{
UDEBUG("DepthAI");
camera = new CameraDepthAI(
device.toStdString().c_str(),
_ui->comboBox_depthai_resolution->currentIndex(),
_ui->comboBox_depthai_image_width->currentIndex()?1280:640,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraDepthAI*)camera)->setOutputMode(_ui->comboBox_depthai_output_mode->currentIndex());
((CameraDepthAI*)camera)->setDepthProfile(_ui->spinBox_depthai_conf_threshold->value(), _ui->spinBox_depthai_lrc_threshold->value());
((CameraDepthAI*)camera)->setExtendedDisparity(_ui->checkBox_depthai_extended_disparity->isChecked());
((CameraDepthAI*)camera)->setExtendedDisparity(_ui->checkBox_depthai_extended_disparity->isChecked(), _ui->checkBox_depthai_disparity_companding->isChecked());
((CameraDepthAI*)camera)->setSubpixelMode(_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()!=0, _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==2?4:_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==3?5:3);
((CameraDepthAI*)camera)->setCompanding(_ui->comboBox_depthai_disparity_companding->currentIndex()!=0, _ui->comboBox_depthai_disparity_companding->currentIndex()==1?64:96);
((CameraDepthAI*)camera)->setDisparityWidthAndFilter(_ui->comboBox_depthai_disparity_width->currentIndex()==0?64:96, _ui->comboBox_depthai_median_filter->currentIndex()==2?5:_ui->comboBox_depthai_median_filter->currentIndex()==3?7:3);
((CameraDepthAI*)camera)->setRectification(_ui->checkBox_depthai_use_spec_translation->isChecked(), _ui->doubleSpinBox_depthai_alpha_scaling->value(), !useRawImages);
((CameraDepthAI*)camera)->setIMU(_ui->checkBox_depthai_imu_published->isChecked(), _ui->checkbox_publishInterIMU->isChecked());
((CameraDepthAI*)camera)->setIrIntensity(_ui->doubleSpinBox_depthai_dot_intensity->value(), _ui->doubleSpinBox_depthai_flood_intensity->value());
((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex());
((CameraDepthAI*)camera)->setBlobPath(_ui->lineEdit_depthai_blob_path->text().toStdString());
((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex(), _ui->lineEdit_depthai_blob_path->text().toStdString());
if(_ui->comboBox_depthai_detect_features->currentIndex() == 1)
{
((CameraDepthAI*)camera)->setGFTTDetector(_ui->checkBox_GFTT_useHarrisDetector->isChecked(), _ui->doubleSpinBox_GFTT_minDistance->value(), _ui->reextract_maxFeatures->value());
@@ -6951,13 +6975,50 @@ Camera * PreferencesDialog::createCamera(
}
else if(driver == kSrcDatabase)
{
std::vector<unsigned int> cameraIndices;
if(!_ui->source_lineEdit_databaseCameraIndex->text().isEmpty())
{
// read the first node to know how many cameras we have in the database
std::shared_ptr<DBReader> reader(
new DBReader(
_ui->source_database_lineEdit_path->text().toStdString(),
0,
false,
false,
false,
_ui->source_spinBox_databaseStartId->value()));
if(reader->init())
{
SensorData data = reader->takeImage();
unsigned int numCamerasInDb = data.cameraModels().size()>0?data.cameraModels().size():data.stereoCameraModels().size();
if(numCamerasInDb>1)
{
QStringList indicesStr = _ui->source_lineEdit_databaseCameraIndex->text().split(' ');
for(QStringList::iterator iter=indicesStr.begin(); iter!=indicesStr.end(); ++iter)
{
cameraIndices.push_back(iter->toInt());
if(cameraIndices.back() > numCamerasInDb)
{
QMessageBox::warning(this,
tr("Creating Database Reader"),
tr("Camera index %1 is not valid, it should be between 0 and %2. Remove or update camera indices under Source->Database panel (currently set to \"%3\").")
.arg(cameraIndices.back()).arg(numCamerasInDb).arg(_ui->source_lineEdit_databaseCameraIndex->text()),
QMessageBox::Ok);
cameraIndices.clear();
return 0;
}
}
}
}
}
camera = new DBReader(_ui->source_database_lineEdit_path->text().toStdString(),
_ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
_ui->source_checkBox_ignoreOdometry->isChecked(),
_ui->source_checkBox_ignoreGoalDelay->isChecked(),
_ui->source_checkBox_ignoreGoals->isChecked(),
_ui->source_spinBox_databaseStartId->value(),
_ui->source_spinBox_database_cameraIndex->value(),
cameraIndices,
_ui->source_spinBox_databaseStopId->value(),
!_ui->general_checkBox_createIntermediateNodes->isChecked(),
_ui->source_checkBox_ignoreLandmarks->isChecked(),
guilib/src/ui/preferencesDialog.ui
+956 -761
View File
File diff suppressed because it is too large Load Diff
package.xml
+2 -2
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap</name>
<version>0.21.10</version>
<version>0.21.11</version>
<description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
@@ -22,7 +22,7 @@
<!-- <depend>libproj-dev</depend> needed due to error in vtk6 (kinetic)-->
<depend>libsqlite3-dev</depend>
<depend>liboctomap-dev</depend>
<depend>grid_map_core</depend>
<!-- <depend>grid_map_core</depend> --> <!-- till this PR is released https://github.com/ANYbotics/grid_map/pull/499 -->
<depend>qt_gui_cpp</depend> <!-- libqt4-dev or libqt5-dev -->
<depend>zlib</depend>
tools/Reprocess/main.cpp
+19 -5
View File
@@ -81,7 +81,8 @@ void showUsage()
" -stop_s # Last map session to process.\n"
" -a Append mode: if Mem/IncrementalMemory is true, RTAB-Map is initialized with the first input database,\n"
" then next databases are reprocessed on top of the first one.\n"
" -cam # Camera index to stream. Ignored if a database doesn't contain multi-camera data.\n"
" -cam # Camera index to stream. Ignored if a database doesn't contain multi-camera data. Can also be multiple \n"
" indices split by spaces in a string like \"0 2\" to stream cameras 0 and 2 only.\n"
" -nolandmark Don't republish landmarks contained in input database.\n"
" -nopriors Don't republish priors contained in input database.\n"
" -pub_loops Republish loop closures contained in input database.\n"
@@ -225,6 +226,15 @@ int main(int argc, char * argv[])
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kError);
// override help from Parameters to show the whole thing
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "--help") == 0)
{
showUsage();
}
}
ParametersMap customParameters = Parameters::parseArguments(argc, argv);
if(argc < 3)
@@ -245,7 +255,7 @@ int main(int argc, char * argv[])
int startMapId = 0;
int stopMapId = -1;
bool appendMode = false;
int cameraIndex = -1;
std::vector<unsigned int> cameraIndices;
int framesToSkip = 0;
bool ignoreLandmarks = false;
bool ignorePriors = false;
@@ -361,8 +371,12 @@ int main(int argc, char * argv[])
++i;
if(i < argc - 2)
{
cameraIndex = atoi(argv[i]);
printf("Camera index = %d.\n", cameraIndex);
std::list<std::string> indicesStr = uSplit(argv[i], ' ');
for(std::list<std::string>::iterator iter=indicesStr.begin(); iter!=indicesStr.end(); ++iter)
{
cameraIndices.push_back(uStr2Int(*iter));
printf("Camera index = %d.\n", cameraIndices.back());
}
}
else
{
@@ -768,7 +782,7 @@ int main(int argc, char * argv[])
false,
false,
startId,
cameraIndex,
cameraIndices,
stopId,
!intermediateNodes,
ignoreLandmarks,