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

Added realsense2 playback support (bag) (#1636)

Browse Source
This commit is contained in:
matlabbe
2026-01-17 18:07:32 -08:00
committed by GitHub
parent 93ca4ed8d7
commit 7db14533f1
3 changed files with 224 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/camera/CameraRealSense2.h
+1
View File
@@ -146,6 +146,7 @@ private:
Transform dualExtrinsics_;
std::string jsonConfig_;
bool closing_;
bool playback_;
static Transform realsense2PoseRotation_;
static Transform realsense2PoseRotationInv_;
corelib/src/camera/CameraRealSense2.cpp
+98 -56
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UDirectory.h>
#include <opencv2/imgproc/types_c.h>
#ifdef RTABMAP_REALSENSE2
@@ -78,7 +79,8 @@ CameraRealSense2::CameraRealSense2(
cameraDepthFps_(30),
globalTimeSync_(true),
dualMode_(false),
closing_(false)
closing_(false),
playback_(false)
#endif
{
UDEBUG("");
@@ -130,6 +132,7 @@ void CameraRealSense2::close()
}
closing_ = false;
playback_ = false;
}
void CameraRealSense2::imu_callback(rs2::frame frame)
@@ -492,64 +495,76 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
clockSyncWarningShown_ = false;
imuGlobalSyncWarningShown_ = false;
rs2::device_list list = ctx_.query_devices();
if (0 == list.size())
if(uStrContains(deviceId_, ".bag"))
{
UERROR("No RealSense2 devices were found!");
return false;
// playback (bag recorded by realsense-viewer)
dev_.resize(1);
dev_[0] = ctx_.load_device(uReplaceChar(deviceId_, '~', UDirectory::homeDir()));
playback_ = true;
UINFO("Device ID is a bag (\"%s\"), using playback mode", deviceId_.c_str());
}
bool found=false;
try
else
{
for (rs2::device dev : list)
rs2::device_list list = ctx_.query_devices();
if (0 == list.size())
{
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
UERROR("No RealSense2 devices were found!");
return false;
}
uint16_t pid;
std::stringstream ss;
ss << std::hex << pid_str;
ss >> pid;
UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
if(dualMode_ && pid == 0x0B37)
bool found=false;
try
{
for (rs2::device dev : list)
{
// Dual setup: device[0] = D400, device[1] = T265
// T265
dev_.resize(2);
dev_[1] = dev;
}
else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
{
if(dev_.empty())
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
uint16_t pid;
std::stringstream ss;
ss << std::hex << pid_str;
ss >> pid;
UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
if(dualMode_ && pid == 0x0B37)
{
dev_.resize(1);
// Dual setup: device[0] = D400, device[1] = T265
// T265
dev_.resize(2);
dev_[1] = dev;
}
else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
{
if(dev_.empty())
{
dev_.resize(1);
}
dev_[0] = dev;
found=true;
}
dev_[0] = dev;
found=true;
}
}
}
catch(const rs2::error & error)
{
UWARN("%s. Is the camera already used with another app?", error.what());
catch(const rs2::error & error)
{
UWARN("%s. Is the camera already used with another app?", error.what());
}
if (!found)
{
if(dualMode_ && dev_.size()==2)
{
UERROR("Dual setup is enabled, but a D400 camera is not detected!");
dev_.clear();
}
else
{
UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
}
return false;
}
}
if (!found)
{
if(dualMode_ && dev_.size()==2)
{
UERROR("Dual setup is enabled, but a D400 camera is not detected!");
dev_.clear();
}
else
{
UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
}
return false;
}
else if(dualMode_ && dev_.size()!=2)
if(dualMode_ && dev_.size()!=2)
{
UERROR("Dual setup is enabled, but a T265 camera is not detected!");
dev_.clear();
@@ -634,7 +649,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
sensors[1] = elem;
if(sensors[1].supports(rs2_option::RS2_OPTION_EMITTER_ENABLED))
{
sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
if(!sensors[1].is_option_read_only(rs2_option::RS2_OPTION_EMITTER_ENABLED))
{
sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
}
else if(!emitterEnabled_)
{
UWARN("rs2_option::RS2_OPTION_EMITTER_ENABLED option is read-only, cannot disable IR emitter.");
}
}
}
else if ("Coded-Light Depth Sensor" == module_name)
@@ -732,7 +754,8 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
video_profile.width() == cameraWidth_ &&
video_profile.height() == cameraHeight_ &&
video_profile.fps() == cameraFps_) ||
(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 &&
((strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 ||
(playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0)) &&
video_profile.width() == cameraDepthWidth_ &&
video_profile.height() == cameraDepthHeight_ &&
video_profile.fps() == cameraDepthFps_))
@@ -741,7 +764,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
// rgb or ir left
if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) ||
(ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
(ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
{
if(!profilesPerSensor[i].empty())
{
@@ -880,8 +903,17 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
if (!added)
{
UERROR("Given stream configuration is not supported by the device! "
"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
if(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 ||
(playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0))
{
UERROR("Given stream configuration is not supported by the device! "
"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraDepthWidth_, cameraDepthHeight_, cameraDepthFps_);
}
else
{
UERROR("Given stream configuration is not supported by the device! "
"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
}
UERROR("Available configurations:");
for (auto& profile : profiles)
{
@@ -1087,9 +1119,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
{
float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
if(!sensors[i].is_option_read_only(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
{
sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
}
else if(value != 1)
{
UWARN("rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED option is read-only, cannot enable it.");
}
}
sensors[i].open(profilesPerSensor[i]);
if(sensors[i].is<rs2::depth_sensor>())
@@ -1538,7 +1577,10 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
}
catch(const std::exception& ex)
{
UERROR("An error has occurred during frame callback: %s", ex.what());
if(!playback_)
{
UERROR("An error has occurred during frame callback: %s", ex.what());
}
}
#else
UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
guilib/src/ui/preferencesDialog.ui
+125 -125
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-603</y>
<y>-747</y>
<width>684</width>
<height>5110</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>5</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3590,7 +3590,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_rgbd">
<property name="currentIndex">
<number>12</number>
<number>9</number>
</property>
<widget class="QWidget" name="page_32">
<layout class="QVBoxLayout" name="verticalLayout_63">
@@ -4578,8 +4578,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>RealSense2</string>
</property>
<layout class="QGridLayout" name="gridLayout_100" columnstretch="0,0,1">
<item row="0" column="1">
<widget class="QCheckBox" name="checkbox_rs2_emitter">
<item row="10" column="0">
<widget class="QToolButton" name="toolButton_rs2_jsonFile">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QCheckBox" name="checkbox_rs2_globalTimeStync">
<property name="text">
<string/>
</property>
@@ -4588,10 +4595,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_468">
<item row="10" column="1">
<widget class="QLineEdit" name="lineEdit_rs2_jsonFile"/>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_562">
<property name="text">
<string>IR emitter enabled</string>
<string>Use depth image in IR mode (instead of right image).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -4601,13 +4611,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QCheckBox" name="checkbox_rs2_irMode">
<item row="8" column="2">
<widget class="QLabel" name="label_613">
<property name="text">
<string/>
<string>(L515 or playback) Depth stream rate.</string>
</property>
<property name="checked">
<bool>false</bool>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
@@ -4624,45 +4637,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QCheckBox" name="checkbox_rs2_irDepth">
<item row="10" column="2">
<widget class="QLabel" name="label_571">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_562">
<property name="text">
<string>Use depth image in IR mode (instead of right image).</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;D400 Series Visual Presets. See this &lt;a href=&quot;https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;page&lt;/span&gt;&lt;/a&gt;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QSpinBox" name="spinBox_rs2_width">
<property name="suffix">
<string> pix</string>
</property>
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_549">
<property name="text">
<string>RGB/IR stream width. Set 1280 for L515.</string>
</property>
<property name="wordWrap">
<property name="openExternalLinks">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
@@ -4670,16 +4653,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QSpinBox" name="spinBox_rs2_height">
<property name="suffix">
<string> pix</string>
</property>
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QLabel" name="label_550">
<property name="text">
@@ -4716,8 +4689,67 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QSpinBox" name="spinBox_rs2_width_depth">
<item row="3" column="2">
<widget class="QLabel" name="label_549">
<property name="text">
<string>RGB/IR stream width. Set 1280 for L515.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QSpinBox" name="spinBox_rs2_width">
<property name="suffix">
<string> pix</string>
</property>
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_468">
<property name="text">
<string>IR emitter enabled</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QCheckBox" name="checkbox_rs2_irMode">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="7" column="2">
<widget class="QLabel" name="label_612">
<property name="text">
<string>(L515 or playback) Depth stream height.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QSpinBox" name="spinBox_rs2_height">
<property name="suffix">
<string> pix</string>
</property>
@@ -4729,7 +4761,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="6" column="2">
<widget class="QLabel" name="label_611">
<property name="text">
<string>(L515) Depth stream width.</string>
<string>(L515 or playback) Depth stream width.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -4739,54 +4771,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QSpinBox" name="spinBox_rs2_height_depth">
<property name="suffix">
<string> pix</string>
</property>
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="7" column="2">
<widget class="QLabel" name="label_612">
<property name="text">
<string>(L515) Depth stream height.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QSpinBox" name="spinBox_rs2_rate_depth">
<property name="suffix">
<string> Hz</string>
</property>
<property name="maximum">
<number>999</number>
</property>
</widget>
</item>
<item row="8" column="2">
<widget class="QLabel" name="label_613">
<property name="text">
<string>(L515) Depth stream rate.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QCheckBox" name="checkbox_rs2_globalTimeStync">
<item row="0" column="1">
<widget class="QCheckBox" name="checkbox_rs2_emitter">
<property name="text">
<string/>
</property>
@@ -4808,29 +4794,43 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QToolButton" name="toolButton_rs2_jsonFile">
<property name="text">
<string>...</string>
<item row="6" column="1">
<widget class="QSpinBox" name="spinBox_rs2_width_depth">
<property name="suffix">
<string> pix</string>
</property>
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLineEdit" name="lineEdit_rs2_jsonFile"/>
<item row="8" column="1">
<widget class="QSpinBox" name="spinBox_rs2_rate_depth">
<property name="suffix">
<string> Hz</string>
</property>
<property name="maximum">
<number>999</number>
</property>
</widget>
</item>
<item row="10" column="2">
<widget class="QLabel" name="label_571">
<item row="7" column="1">
<widget class="QSpinBox" name="spinBox_rs2_height_depth">
<property name="suffix">
<string> pix</string>
</property>
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QCheckBox" name="checkbox_rs2_irDepth">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;D400 Series Visual Presets. See this &lt;a href=&quot;https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;page&lt;/span&gt;&lt;/a&gt;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string/>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="openExternalLinks">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>