mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-08 12:30:20 +08:00
Merge branch 'borongyuan-alphascaling'
This commit is contained in:
matlabbe
@@ -56,6 +56,7 @@ public:
|
||||
virtual ~CameraDepthAI();
|
||||
|
||||
void setOutputDepth(bool enabled, int confidence = 200);
|
||||
void setAlphaScaling(float alphaScaling = 0.0f);
|
||||
void setIMUFirmwareUpdate(bool enabled);
|
||||
void setIMUPublished(bool published);
|
||||
void publishInterIMU(bool enabled);
|
||||
@@ -77,6 +78,7 @@ private:
|
||||
bool outputDepth_;
|
||||
int depthConfidence_;
|
||||
int resolution_;
|
||||
float alphaScaling_;
|
||||
bool imuFirmwareUpdate_;
|
||||
bool imuPublished_;
|
||||
bool publishInterIMU_;
|
||||
|
||||
@@ -56,6 +56,7 @@ CameraDepthAI::CameraDepthAI(
|
||||
outputDepth_(false),
|
||||
depthConfidence_(200),
|
||||
resolution_(resolution),
|
||||
alphaScaling_(0.0),
|
||||
imuFirmwareUpdate_(false),
|
||||
imuPublished_(true),
|
||||
publishInterIMU_(false),
|
||||
@@ -91,6 +92,15 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
|
||||
#endif
|
||||
}
|
||||
|
||||
void CameraDepthAI::setAlphaScaling(float alphaScaling)
|
||||
{
|
||||
#ifdef RTABMAP_DEPTHAI
|
||||
alphaScaling_ = alphaScaling;
|
||||
#else
|
||||
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
|
||||
#endif
|
||||
}
|
||||
|
||||
void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
|
||||
{
|
||||
#ifdef RTABMAP_DEPTHAI
|
||||
@@ -218,6 +228,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
||||
stereo->setSubpixelFractionalBits(4);
|
||||
stereo->setExtendedDisparity(false);
|
||||
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
|
||||
if(alphaScaling_>-1.0f)
|
||||
stereo->setAlphaScaling(alphaScaling_);
|
||||
stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
|
||||
stereo->initialConfig.setLeftRightCheck(true);
|
||||
stereo->initialConfig.setLeftRightCheckThreshold(5);
|
||||
@@ -258,11 +270,30 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
||||
|
||||
UINFO("Loading eeprom calibration data");
|
||||
dai::CalibrationHandler calibHandler = device_->readCalibration();
|
||||
|
||||
cv::Mat cameraMatrix, distCoeffs, new_camera_matrix;
|
||||
|
||||
std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, dai::Size2f(targetSize.width, targetSize.height));
|
||||
double fx = matrix[0][0];
|
||||
double fy = matrix[1][1];
|
||||
double cx = matrix[0][2];
|
||||
double cy = matrix[1][2];
|
||||
cameraMatrix = (cv::Mat_<double>(3,3) <<
|
||||
matrix[0][0], matrix[0][1], matrix[0][2],
|
||||
matrix[1][0], matrix[1][1], matrix[1][2],
|
||||
matrix[2][0], matrix[2][1], matrix[2][2]);
|
||||
|
||||
std::vector<float> coeffs = calibHandler.getDistortionCoefficients(dai::CameraBoardSocket::LEFT);
|
||||
if(calibHandler.getDistortionModel(dai::CameraBoardSocket::LEFT) == dai::CameraModel::Perspective)
|
||||
distCoeffs = (cv::Mat_<double>(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]);
|
||||
|
||||
if(alphaScaling_>-1.0f) {
|
||||
new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize, alphaScaling_);
|
||||
}
|
||||
else {
|
||||
new_camera_matrix = cameraMatrix;
|
||||
}
|
||||
|
||||
double fx = new_camera_matrix.at<double>(0, 0);
|
||||
double fy = new_camera_matrix.at<double>(1, 1);
|
||||
double cx = new_camera_matrix.at<double>(0, 2);
|
||||
double cy = new_camera_matrix.at<double>(1, 2);
|
||||
double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT, false)/100.0;
|
||||
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);
|
||||
|
||||
@@ -2532,7 +2532,8 @@ cv::Point3f projectDisparityTo3D(
|
||||
c = model.right().cx() - model.left().cx();
|
||||
}
|
||||
float W = model.baseline()/(disparity + c);
|
||||
return cv::Point3f((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W);
|
||||
return cv::Point3f((pt.x - model.left().cx())*W,
|
||||
(pt.y - model.left().cy())*model.left().fx()/model.left().fy()*W, model.left().fx()*W);
|
||||
}
|
||||
float bad_point = std::numeric_limits<float>::quiet_NaN ();
|
||||
return cv::Point3f(bad_point, bad_point, bad_point);
|
||||
|
||||
@@ -805,6 +805,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
|
||||
connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
|
||||
connect(_ui->checkBox_depthai_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
|
||||
connect(_ui->spinBox_depthai_confidence, SIGNAL(valueChanged(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()));
|
||||
connect(_ui->checkBox_depthai_imu_firmware_update, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
|
||||
connect(_ui->doubleSpinBox_depthai_laser_dot_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
|
||||
@@ -2061,6 +2062,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
|
||||
_ui->comboBox_depthai_resolution->setCurrentIndex(1);
|
||||
_ui->checkBox_depthai_depth->setChecked(false);
|
||||
_ui->spinBox_depthai_confidence->setValue(200);
|
||||
_ui->doubleSpinBox_depthai_alpha_scaling->setValue(0.0);
|
||||
_ui->checkBox_depthai_imu_published->setChecked(true);
|
||||
_ui->checkBox_depthai_imu_firmware_update->setChecked(false);
|
||||
_ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(0.0);
|
||||
@@ -2547,6 +2549,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
|
||||
_ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt());
|
||||
_ui->checkBox_depthai_depth->setChecked(settings.value("depth", _ui->checkBox_depthai_depth->isChecked()).toBool());
|
||||
_ui->spinBox_depthai_confidence->setValue(settings.value("confidence", _ui->spinBox_depthai_confidence->value()).toInt());
|
||||
_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());
|
||||
_ui->checkBox_depthai_imu_firmware_update->setChecked(settings.value("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()).toBool());
|
||||
_ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(settings.value("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()).toDouble());
|
||||
@@ -3076,6 +3079,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
|
||||
settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex());
|
||||
settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked());
|
||||
settings.setValue("confidence", _ui->spinBox_depthai_confidence->value());
|
||||
settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value());
|
||||
settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked());
|
||||
settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked());
|
||||
settings.setValue("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value());
|
||||
@@ -6325,6 +6329,7 @@ Camera * PreferencesDialog::createCamera(
|
||||
this->getGeneralInputRate(),
|
||||
this->getSourceLocalTransform());
|
||||
((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value());
|
||||
((CameraDepthAI*)camera)->setAlphaScaling(_ui->doubleSpinBox_depthai_alpha_scaling->value());
|
||||
((CameraDepthAI*)camera)->setIMUFirmwareUpdate(_ui->checkBox_depthai_imu_firmware_update->isChecked());
|
||||
((CameraDepthAI*)camera)->setIMUPublished(_ui->checkBox_depthai_imu_published->isChecked());
|
||||
((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
|
||||
|
||||
+130
-101
@@ -63,7 +63,7 @@
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>-611</y>
|
||||
<y>-606</y>
|
||||
<width>756</width>
|
||||
<height>3657</height>
|
||||
</rect>
|
||||
@@ -95,7 +95,7 @@
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<property name="currentIndex">
|
||||
<number>12</number>
|
||||
<number>5</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="page_22">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
|
||||
@@ -4971,7 +4971,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
<item>
|
||||
<widget class="QStackedWidget" name="stackedWidget_stereo">
|
||||
<property name="currentIndex">
|
||||
<number>4</number>
|
||||
<number>10</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="page_49">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_91"/>
|
||||
@@ -5809,52 +5809,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
<string>DepthAI</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_120" columnstretch="0,1">
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="label_626">
|
||||
<property name="text">
|
||||
<string>Resolution.</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="QLabel" name="label_625">
|
||||
<property name="text">
|
||||
<string>Output depth.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="label_627">
|
||||
<property name="text">
|
||||
<string>Depth confidence.</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="0">
|
||||
<widget class="QCheckBox" name="checkBox_depthai_depth">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="0">
|
||||
<widget class="QComboBox" name="comboBox_depthai_resolution">
|
||||
<property name="currentIndex">
|
||||
@@ -5890,6 +5844,127 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="0">
|
||||
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_alpha_scaling">
|
||||
<property name="minimum">
|
||||
<double>-1.000000000000000</double>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<double>1.000000000000000</double>
|
||||
</property>
|
||||
<property name="singleStep">
|
||||
<double>1.000000000000000</double>
|
||||
</property>
|
||||
<property name="value">
|
||||
<double>0.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="label_627">
|
||||
<property name="text">
|
||||
<string>Depth confidence.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="6" column="0">
|
||||
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_laser_dot_brightness">
|
||||
<property name="suffix">
|
||||
<string> mA</string>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<double>1200.000000000000000</double>
|
||||
</property>
|
||||
<property name="value">
|
||||
<double>0.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="0">
|
||||
<widget class="QCheckBox" name="checkBox_depthai_imu_published">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="7" column="1">
|
||||
<widget class="QLabel" name="label_677">
|
||||
<property name="text">
|
||||
<string>Floodlight brightness.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="6" column="1">
|
||||
<widget class="QLabel" name="label_676">
|
||||
<property name="text">
|
||||
<string>Laser dot brightness.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="5" column="1">
|
||||
<widget class="QLabel" name="label_646">
|
||||
<property name="text">
|
||||
<string>IMU firmware update</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="label_626">
|
||||
<property name="text">
|
||||
<string>Resolution.</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="0">
|
||||
<widget class="QCheckBox" name="checkBox_depthai_depth">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="1">
|
||||
<widget class="QLabel" name="label_678">
|
||||
<property name="text">
|
||||
<string>Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1 (when all the source image pixels are retained in the undistorted image). On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha < 0.0 helps removing invalid areas. If alpha is set to -1, old rectification policy is used.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QSpinBox" name="spinBox_depthai_confidence">
|
||||
<property name="minimum">
|
||||
@@ -5904,26 +5979,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="1">
|
||||
<widget class="QLabel" name="label_646">
|
||||
<property name="text">
|
||||
<string>IMU firmware update</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="0">
|
||||
<widget class="QCheckBox" name="checkBox_depthai_imu_firmware_update">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="1">
|
||||
<widget class="QLabel" name="label_650">
|
||||
<property name="text">
|
||||
<string>IMU published</string>
|
||||
@@ -5936,30 +5991,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="0">
|
||||
<widget class="QCheckBox" name="checkBox_depthai_imu_published">
|
||||
<item row="5" column="0">
|
||||
<widget class="QCheckBox" name="checkBox_depthai_imu_firmware_update">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="5" column="0">
|
||||
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_laser_dot_brightness">
|
||||
<property name="suffix">
|
||||
<string> mA</string>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<double>1200.000000000000000</double>
|
||||
</property>
|
||||
<property name="value">
|
||||
<double>0.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="5" column="1">
|
||||
<widget class="QLabel" name="label_676">
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="label_625">
|
||||
<property name="text">
|
||||
<string>Laser dot brightness.</string>
|
||||
<string>Output depth.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
@@ -5969,7 +6011,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="6" column="0">
|
||||
<item row="7" column="0">
|
||||
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_floodlight_brightness">
|
||||
<property name="suffix">
|
||||
<string> mA</string>
|
||||
@@ -5982,19 +6024,6 @@ 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="QLabel" name="label_677">
|
||||
<property name="text">
|
||||
<string>Floodlight brightness.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
|
||||
Reference in New Issue
Block a user