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

specify device using MXID or IP/USB name

Browse Source
This commit is contained in:
Borong Yuan
2023-07-15 20:58:00 +08:00
parent a2a75a28ba
commit ce81fe445a
2 changed files with 29 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -49,7 +49,7 @@ public:
public: public:
CameraDepthAI( CameraDepthAI(
const std::string & deviceSerial = "", const std::string & mxidOrName = "",
int resolution = 1, // 0=720p, 1=800p, 2=400p int resolution = 1, // 0=720p, 1=800p, 2=400p
float imageRate=0.0f, float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity()); const Transform & localTransform = Transform::getIdentity());
@@ -79,7 +79,7 @@ private:
StereoCameraModel stereoModel_; StereoCameraModel stereoModel_;
cv::Size targetSize_; cv::Size targetSize_;
Transform imuLocalTransform_; Transform imuLocalTransform_;
std::string deviceSerial_; std::string mxidOrName_;
bool outputDepth_; bool outputDepth_;
int depthConfidence_; int depthConfidence_;
int resolution_; int resolution_;

47
corelib/src/camera/CameraDepthAI.cpp
View File

@@ -46,14 +46,14 @@ bool CameraDepthAI::available()
} }
CameraDepthAI::CameraDepthAI( CameraDepthAI::CameraDepthAI(
const std::string & deviceSerial, const std::string & mxidOrName,
int resolution, int resolution,
float imageRate, float imageRate,
const Transform & localTransform) : const Transform & localTransform) :
Camera(imageRate, localTransform) Camera(imageRate, localTransform)
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
, ,
deviceSerial_(deviceSerial), mxidOrName_(mxidOrName),
outputDepth_(false), outputDepth_(false),
depthConfidence_(200), depthConfidence_(200),
resolution_(resolution), resolution_(resolution),
@@ -202,38 +202,45 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
std::vector<dai::DeviceInfo> devices = dai::Device::getAllAvailableDevices(); std::vector<dai::DeviceInfo> devices = dai::Device::getAllAvailableDevices();
if(devices.empty()) if(devices.empty())
{ {
UERROR("No DepthAI device found");
return false; return false;
} }
if(device_.get()) if(device_.get())
{
device_->close(); device_->close();
}
accBuffer_.clear(); accBuffer_.clear();
gyroBuffer_.clear(); gyroBuffer_.clear();
dai::DeviceInfo deviceToUse; bool deviceFound = false;
if(deviceSerial_.empty()) dai::DeviceInfo deviceToUse(mxidOrName_);
deviceToUse = devices[0]; if(mxidOrName_.empty())
for(size_t i=0; i<devices.size(); ++i)
{ {
UINFO("DepthAI device found: %s", devices[i].getMxId().c_str()); std::tie(deviceFound, deviceToUse) = dai::Device::getFirstAvailableDevice();
if(!deviceSerial_.empty() && deviceSerial_.compare(devices[i].getMxId()) == 0) }
else if(!deviceToUse.mxid.empty())
{ {
deviceToUse = devices[i]; std::tie(deviceFound, deviceToUse) = dai::Device::getDeviceByMxId(deviceToUse.mxid);
}
else
{
for(auto& device : devices)
{
if(deviceToUse.name.compare(device.name) == 0)
{
deviceFound = true;
deviceToUse = device;
}
} }
} }
if(deviceToUse.getMxId().empty()) if(!deviceFound)
{ {
UERROR("Could not find device with serial \"%s\", found devices:", deviceSerial_.c_str()); UERROR("Could not find DepthAI device with MXID or IP/USB name \"%s\", found devices:", mxidOrName_.c_str());
for(size_t i=0; i<devices.size(); ++i) for(auto& device : devices)
{ UERROR("%s", device.toString().c_str());
UERROR("DepthAI device found: %s", devices[i].getMxId().c_str());
}
return false; return false;
} }
deviceSerial_ = deviceToUse.getMxId();
// look for calibration files // look for calibration files
stereoModel_ = StereoCameraModel(); stereoModel_ = StereoCameraModel();
@@ -405,7 +412,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
double cy = new_camera_matrix.at<double>(1, 2); double cy = new_camera_matrix.at<double>(1, 2);
double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0; double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0;
UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline); UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_); stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
if(imuPublished_) if(imuPublished_)
{ {
@@ -517,7 +524,7 @@ bool CameraDepthAI::isCalibrated() const
std::string CameraDepthAI::getSerial() const std::string CameraDepthAI::getSerial() const
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
return deviceSerial_; return device_->getMxId();
#endif #endif
return ""; return "";
} }