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

Camera class: localTransform parameter should not have optical rotation anymore (it is added afterwards in the constructor). OdomSensor: the extrinsics should not contain optical rotation anymore. These 2 changes make it more convenient to set transfomation parameters in the UI. MainWindow::createCamera() now has odomSensor argument for convenience (for inherited classes to set both camera and odom sensor in the same function).

Browse Source
This commit is contained in:
matlabbe
2021-05-26 13:40:46 -04:00
parent 21dbeed4b7
commit ada3e75005
34 changed files with 96 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -305,7 +305,11 @@ protected:
const QString & newDatabasePathOutput() const { return _newDatabasePathOutput; }
virtual ParametersMap getCustomParameters() {return ParametersMap();}
virtual Camera* createCamera();
virtual Camera * createCamera(
Camera ** odomSensor,
Transform & odomSensorExtrinsics,
double odomSensorTimeOffset,
float odomSensorScaleFactor);
private:
Ui_mainWindow * _ui;

3
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -166,7 +166,6 @@ public:
double getOdomF2MGravitySigma() const;
bool isOdomDisabled() const;
bool isOdomSensorAsGt() const;
double getOdomSensorScaleFactor() const;
bool isGroundTruthAligned() const;
bool isGraphsShown() const;
@@ -280,7 +279,7 @@ public:
QString getIMUPath() const;
int getIMURate() const;
Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null
Camera * createOdomSensor(Transform & extrinsics, double & timeOffset); // return camera should be deleted if not null
Camera * createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor); // return camera should be deleted if not null
int getIgnoredDCComponents() const;

47
guilib/src/MainWindow.cpp
View File

@@ -5353,9 +5353,27 @@ void MainWindow::editDatabase()
}
}
Camera * MainWindow::createCamera()
Camera * MainWindow::createCamera(
Camera ** odomSensor,
Transform & odomSensorExtrinsics,
double odomSensorTimeOffset,
float odomSensorScaleFactor)
{
return _preferencesDialog->createCamera();
Camera * camera = _preferencesDialog->createCamera();
if(camera &&
_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef &&
_preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() &&
!(_preferencesDialog->getOdomSourceDriver() == PreferencesDialog::kSrcStereoRealSense2 &&
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2))
{
UINFO("Create Odom Sensor %d (camera = %d)",
_preferencesDialog->getOdomSourceDriver(),
_preferencesDialog->getSourceDriver());
*odomSensor = _preferencesDialog->createOdomSensor(odomSensorExtrinsics, odomSensorTimeOffset, odomSensorScaleFactor);
}
return camera;
}
void MainWindow::startDetection()
@@ -5457,33 +5475,24 @@ void MainWindow::startDetection()
}
Camera * camera = this->createCamera();
double poseTimeOffset = 0.0;
float scaleFactor = 0.0f;
Transform extrinsics;
Camera * odomSensor = 0;
Camera * camera = this->createCamera(&odomSensor, extrinsics, poseTimeOffset, scaleFactor);
if(!camera)
{
Q_EMIT stateChanged(kInitialized);
return;
}
double poseTimeOffset = 0.0;
Transform extrinsics;
Camera * odomSensor = 0;
if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef &&
_preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() &&
!(_preferencesDialog->getOdomSourceDriver() == PreferencesDialog::kSrcStereoRealSense2 &&
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2))
{
UINFO("Create Odom Sensor %d (camera = %d)",
_preferencesDialog->getOdomSourceDriver(),
_preferencesDialog->getSourceDriver());
odomSensor = _preferencesDialog->createOdomSensor(extrinsics, poseTimeOffset);
}
if(odomSensor)
{
_camera = new CameraThread(camera, odomSensor, extrinsics, poseTimeOffset, _preferencesDialog->getOdomSensorScaleFactor(), _preferencesDialog->isOdomSensorAsGt(), parameters);
_camera = new CameraThread(camera, odomSensor, extrinsics, poseTimeOffset, scaleFactor, _preferencesDialog->isOdomSensorAsGt(), parameters);
}
else
{
_camera = new CameraThread(camera, _preferencesDialog->getOdomSensorScaleFactor(), _preferencesDialog->isOdomSensorAsGt(), parameters);
_camera = new CameraThread(camera, _preferencesDialog->isOdomSensorAsGt(), parameters);
}
_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
@@ -5584,7 +5593,7 @@ void MainWindow::startDetection()
odomStrategy != Odometry::kTypeOpenVINS)
{
QMessageBox::warning(this, tr("Source IMU Path"),
tr("IMU path is set but odometry chosen doesn't support IMU, ignoring IMU..."), QMessageBox::Ok);
tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok);
}
else
{

21
guilib/src/PreferencesDialog.cpp
View File

@@ -1885,7 +1885,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->lineEdit_calibrationFile->clear();
_ui->comboBox_sourceType->setCurrentIndex(kSrcRGBD);
_ui->lineEdit_sourceDevice->setText("");
_ui->lineEdit_sourceLocalTransform->setText("0 0 1 -1 0 0 0 -1 0");
_ui->lineEdit_sourceLocalTransform->setText("0 0 0 0 0 0");
_ui->source_comboBox_image_type->setCurrentIndex(kSrcUsbDevice-kSrcUsbDevice);
_ui->source_images_spinBox_startPos->setValue(0);
@@ -2033,7 +2033,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->spinBox_cameraImages_max_imu_rate->setValue(0);
_ui->comboBox_odom_sensor->setCurrentIndex(0);
_ui->lineEdit_odom_sensor_extrinsics->setText("0.006977 -0.042236 0.004599 -0.012436 -0.002432 -0.004827");
_ui->lineEdit_odom_sensor_extrinsics->setText("-0.000622602 0.0303752 0.031389 -0.00272485 0.00749254 0.0");
_ui->lineEdit_odom_sensor_path_calibration->setText("");
_ui->lineEdit_odomSourceDevice->setText("");
_ui->doubleSpinBox_odom_sensor_time_offset->setValue(0.0);
@@ -2358,6 +2358,12 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->lineEdit_sourceDevice->setText(settings.value("device",_ui->lineEdit_sourceDevice->text()).toString());
_ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString());
_ui->spinBox_source_imageDecimation->setValue(settings.value("imageDecimation",_ui->spinBox_source_imageDecimation->value()).toInt());
// Backward compatibility
if(_ui->lineEdit_sourceLocalTransform->text().compare("0 0 1 -1 0 0 0 -1 0") == 0)
{
UWARN("From 0.20.11, the local transform of the camera should not contain optical rotation (read=\"%s\"). Resetting to default Identity for convenience.", _ui->lineEdit_sourceLocalTransform->text().toStdString().c_str());
_ui->lineEdit_sourceLocalTransform->setText("0 0 0 0 0 0");
}
settings.beginGroup("rgbd");
_ui->comboBox_cameraRGBD->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraRGBD->currentIndex()).toInt());
@@ -5264,10 +5270,6 @@ bool PreferencesDialog::isOdomSensorAsGt() const
{
return _ui->checkBox_odom_sensor_use_as_gt->isChecked();
}
double PreferencesDialog::getOdomSensorScaleFactor() const
{
return _ui->doubleSpinBox_odom_sensor_scale_factor->value();
}
int PreferencesDialog::getOdomRegistrationApproach() const
{
return _ui->odom_registration->currentIndex();
@@ -6329,7 +6331,7 @@ Camera * PreferencesDialog::createCamera(
return camera;
}
Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & timeOffset)
Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor)
{
Src driver = getOdomSourceDriver();
if(driver != kSrcUndef)
@@ -6347,6 +6349,7 @@ Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & ti
extrinsics = Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
timeOffset = _ui->doubleSpinBox_odom_sensor_time_offset->value()/1000.0;
scaleFactor = (float)_ui->doubleSpinBox_odom_sensor_scale_factor->value();
return createCamera(driver, _ui->lineEdit_odomSourceDevice->text(), _ui->lineEdit_odom_sensor_path_calibration->text(), false, true, true);
}
@@ -6505,7 +6508,7 @@ void PreferencesDialog::testOdometry()
this->getOdomStrategy() != Odometry::kTypeOpenVINS)
{
QMessageBox::warning(this, tr("Source IMU Path"),
tr("IMU path is set but odometry chosen doesn't support IMU, ignoring IMU..."), QMessageBox::Ok);
tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok);
}
else
{
@@ -7121,7 +7124,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics()
UINFO("Odom sensor local transform (pose to left cam): %s", odomSensorModel.localTransform().prettyPrint().c_str());
UINFO("Extrinsics (odom left cam to camera left cam): %s", stereoModel.stereoTransform().prettyPrint().c_str());
Transform t = odomSensorModel.localTransform() * stereoModel.stereoTransform();
Transform t = odomSensorModel.localTransform() * stereoModel.stereoTransform() * CameraModel::opticalRotation().inverse();
UINFO("Odom sensor frame to camera frame: %s", t.prettyPrint().c_str());
float x,y,z,roll,pitch,yaw;

10
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-2147</y>
<y>0</y>
<width>686</width>
<height>3905</height>
</rect>
@@ -2955,10 +2955,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="3" column="0">
<widget class="QLineEdit" name="lineEdit_sourceLocalTransform">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /gray_camera = 0 0 1 -1 0 0 0 -1 0&lt;br/&gt;KITTI: /base_link to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 0&lt;br/&gt;KITTI: /base_footprint to /gray_camera = 0 0 1 0 -1 0 0 0 0 -1 0 1.67&lt;br/&gt;KITTI: /base_footprint to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 1.67&lt;/p&gt;&lt;p&gt;EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = -0.0257742 0.00375623 0.999661 0.00981073 -0.999557 -0.0149672 -0.0257155 0.064677 0.0148655 -0.999881 0.00414038 -0.0216401&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /gray_camera = 0 0 0 0 0 0&lt;br/&gt;KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0&lt;br/&gt;KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0&lt;br/&gt;KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0&lt;/p&gt;&lt;p&gt;EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="text">
<string>0 0 1 -1 0 0 0 -1 0</string>
<string>0 0 0 0 0 0</string>
</property>
</widget>
</item>
@@ -6948,7 +6948,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="3" column="1">
<widget class="QLabel" name="label_634">
<property name="text">
<string>Extrinsics between odometry sensor's left lens and camera's left lens. Default extrinsics match the 3D printed bracket &lt;a href=&quot; https://www.intelrealsense.com/depth-and-tracking-combined-get-started/&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;here&lt;/span&gt;&lt;/a&gt; for T265+D400 setup. (&lt;a href=&quot;https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;stl&lt;/span&gt;&lt;/a&gt;). Not used if camera and odometry sensors are the same sensor.</string>
<string>Extrinsics between pose frame and camera's left lens (without optical rotation). Default extrinsics match the 3D printed bracket &lt;a href=&quot; https://www.intelrealsense.com/depth-and-tracking-combined-get-started/&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;here&lt;/span&gt;&lt;/a&gt; for T265+D400 setup. (&lt;a href=&quot;https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;stl&lt;/span&gt;&lt;/a&gt;). Not used if camera and odometry sensors are the same sensor.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7068,7 +7068,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="3" column="0">
<widget class="QLineEdit" name="lineEdit_odom_sensor_extrinsics">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="text">
<string>0.009 0.021 0.027 0.000 -0.018 0.005</string>