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

CameraK4A: use mkv framerate if camera rate is -1. Preferences/Source: fixed imu filter parameters not reset on restore defaults

Browse Source
This commit is contained in:
matlabbe
2019-07-11 18:13:20 -04:00
parent 0973f48b2c
commit 89f1e809e6
4 changed files with 116 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/camera/CameraK4A.h
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraModel.h" #include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/Camera.h" #include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h" #include "rtabmap/core/Version.h"
#include "rtabmap/utilite/UTimer.h"
namespace rtabmap namespace rtabmap
{ {
@@ -72,6 +73,8 @@ private:
int deviceId_; int deviceId_;
std::string fileName_; std::string fileName_;
bool ir_; bool ir_;
double previousStamp_;
UTimer timer_;
#endif #endif
}; };

42
corelib/src/camera/CameraK4A.cpp
View File

@@ -57,7 +57,8 @@ CameraK4A::CameraK4A(
,playbackHandle_(NULL), ,playbackHandle_(NULL),
transformationHandle_(NULL), transformationHandle_(NULL),
deviceId_(deviceId), deviceId_(deviceId),
ir_(false) ir_(false),
previousStamp_(0.0)
#endif #endif
{ {
UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are."); UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
@@ -73,7 +74,8 @@ CameraK4A::CameraK4A(
transformationHandle_(NULL), transformationHandle_(NULL),
deviceId_(-1), deviceId_(-1),
fileName_(fileName), fileName_(fileName),
ir_(false) ir_(false),
previousStamp_(0.0)
#endif #endif
{ {
} }
@@ -388,6 +390,42 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
{ {
data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp); data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
data.setIMU(imu); data.setIMU(imu);
// Frame rate
if (this->getImageRate() < 0.0f)
{
if (stamp == 0)
{
UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
}
else if (previousStamp_ > 0)
{
float ratio = -this->getImageRate();
int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
if (sleepTime > 10000)
{
UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
sleepTime / 1000, previousStamp_, stamp);
sleepTime = 10000;
}
if (sleepTime > 2)
{
uSleep(sleepTime - 2);
}
// Add precision at the cost of a small overhead
while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
{
//
}
double slept = timer_.getElapsedTime();
timer_.start();
UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
}
previousStamp_ = stamp;
}
break; break;
} }
} }

14
guilib/src/PreferencesDialog.cpp
View File

@@ -699,6 +699,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->lineEdit_openniOniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_openniOniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_openni2OniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_openni2OniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_k4a_mkv, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_k4a_mkv, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_useMKVStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_source_distortionModel, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_source_distortionModel, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->groupBox_bilateral, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->groupBox_bilateral, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_bilateral_sigmaS, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_bilateral_sigmaS, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1788,6 +1789,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->lineEdit_openni2OniPath->clear(); _ui->lineEdit_openni2OniPath->clear();
_ui->checkbox_k4a_irDepth->setChecked(false); _ui->checkbox_k4a_irDepth->setChecked(false);
_ui->lineEdit_k4a_mkv->clear(); _ui->lineEdit_k4a_mkv->clear();
_ui->source_checkBox_useMKVStamps->setChecked(true);
_ui->lineEdit_cameraRGBDImages_path_rgb->setText(""); _ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
_ui->lineEdit_cameraRGBDImages_path_depth->setText(""); _ui->lineEdit_cameraRGBDImages_path_depth->setText("");
_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(1.0); _ui->doubleSpinBox_cameraRGBDImages_scale->setValue(1.0);
@@ -1831,6 +1833,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->lineEdit_cameraImages_imu_transform->setText("0 0 1 0 -1 0 1 0 0"); _ui->lineEdit_cameraImages_imu_transform->setText("0 0 1 0 -1 0 1 0 0");
_ui->spinBox_cameraImages_max_imu_rate->setValue(0); _ui->spinBox_cameraImages_max_imu_rate->setValue(0);
_ui->comboBox_imuFilter_strategy->setCurrentIndex(0);
_ui->doubleSpinBox_imuFilterMadgwickGain->setValue(Parameters::defaultImuFilterMadgwickGain());
_ui->doubleSpinBox_imuFilterMadgwickZeta->setValue(Parameters::defaultImuFilterMadgwickZeta());
_ui->doubleSpinBox_imuFilterComplementaryGainAcc->setValue(Parameters::defaultImuFilterComplementaryGainAcc());
_ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->setValue(Parameters::defaultImuFilterComplementaryBiasAlpha());
_ui->checkBox_imuFilterComplementaryDoAdaptiveGain->setChecked(Parameters::defaultImuFilterComplementaryDoAdpativeGain());
_ui->checkBox_imuFilterComplementaryDoBiasEstimation->setChecked(Parameters::defaultImuFilterComplementaryDoBiasEstimation());
_ui->checkBox_source_scanFromDepth->setChecked(false); _ui->checkBox_source_scanFromDepth->setChecked(false);
_ui->spinBox_source_scanDownsampleStep->setValue(1); _ui->spinBox_source_scanDownsampleStep->setValue(1);
_ui->doubleSpinBox_source_scanRangeMin->setValue(0); _ui->doubleSpinBox_source_scanRangeMin->setValue(0);
@@ -2185,6 +2195,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.beginGroup("K4A"); settings.beginGroup("K4A");
_ui->checkbox_k4a_irDepth->setChecked(settings.value("ir", _ui->checkbox_k4a_irDepth->isChecked()).toBool()); _ui->checkbox_k4a_irDepth->setChecked(settings.value("ir", _ui->checkbox_k4a_irDepth->isChecked()).toBool());
_ui->lineEdit_k4a_mkv->setText(settings.value("mkvPath", _ui->lineEdit_k4a_mkv->text()).toString()); _ui->lineEdit_k4a_mkv->setText(settings.value("mkvPath", _ui->lineEdit_k4a_mkv->text()).toString());
_ui->source_checkBox_useMKVStamps->setChecked(settings.value("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()).toBool());
settings.endGroup(); // K4A settings.endGroup(); // K4A
settings.beginGroup("RealSense"); settings.beginGroup("RealSense");
@@ -2640,6 +2651,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.beginGroup("K4A"); settings.beginGroup("K4A");
settings.setValue("ir", _ui->checkbox_k4a_irDepth->isChecked()); settings.setValue("ir", _ui->checkbox_k4a_irDepth->isChecked());
settings.setValue("mkvPath", _ui->lineEdit_k4a_mkv->text()); settings.setValue("mkvPath", _ui->lineEdit_k4a_mkv->text());
settings.setValue("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked());
settings.endGroup(); // K4A settings.endGroup(); // K4A
settings.beginGroup("RealSense"); settings.beginGroup("RealSense");
@@ -5325,7 +5337,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
// playback // playback
camera = new CameraK4A( camera = new CameraK4A(
_ui->lineEdit_k4a_mkv->text().toStdString().c_str(), _ui->lineEdit_k4a_mkv->text().toStdString().c_str(),
this->getGeneralInputRate(), _ui->source_checkBox_useMKVStamps->isChecked() ? -1 : this->getGeneralInputRate(),
this->getSourceLocalTransform()); this->getSourceLocalTransform());
} }
else else

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

@@ -4207,17 +4207,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Kinect for Azure</string> <string>Kinect for Azure</string>
</property> </property>
<layout class="QGridLayout" name="gridLayout_113"> <layout class="QGridLayout" name="gridLayout_113">
<item row="0" column="0"> <item row="0" column="1" colspan="2">
<widget class="QCheckBox" name="checkbox_k4a_irDepth">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_558"> <widget class="QLabel" name="label_558">
<property name="text"> <property name="text">
<string>Use IR for RGB image </string> <string>Use IR for RGB image </string>
@@ -4227,34 +4217,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="0"> <item row="5" column="0">
<widget class="QToolButton" name="toolButton_k4a_mkv">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLineEdit" name="lineEdit_k4a_mkv">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_556">
<property name="text">
<string>Path to a *.MKV file.</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">
<spacer name="verticalSpacer_83"> <spacer name="verticalSpacer_83">
<property name="orientation"> <property name="orientation">
<enum>Qt::Vertical</enum> <enum>Qt::Vertical</enum>
@@ -4267,6 +4230,63 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</spacer> </spacer>
</item> </item>
<item row="1" column="0">
<widget class="QToolButton" name="toolButton_k4a_mkv">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="1" column="3">
<widget class="QLabel" name="label_556">
<property name="text">
<string>Path to a *.MKV file.</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="0">
<widget class="QCheckBox" name="checkbox_k4a_irDepth">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="1" column="1" colspan="2">
<widget class="QLineEdit" name="lineEdit_k4a_mkv">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_557">
<property name="text">
<string>Use MKV file stamps as input rate.</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="source_checkBox_useMKVStamps">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout> </layout>
</widget> </widget>
</item> </item>
@@ -4278,7 +4298,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="sizeHint" stdset="0"> <property name="sizeHint" stdset="0">
<size> <size>
<width>20</width> <width>20</width>
<height>168</height> <height>0</height>
</size> </size>
</property> </property>
</spacer> </spacer>