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

CMake: added WITH_OPENMP option (to be able to disable it). CameraOpenNI2: Added depth decimation parameter. CLAMS: can apply distortion model to smaller images.

Browse Source
This commit is contained in:
matlabbe
2021-09-24 17:31:16 -04:00
parent bccc5b13af
commit 8c56b5b1ce
8 changed files with 176 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+4 -2
View File
@@ -210,6 +210,7 @@ option(WITH_VINS "Include VINS-Fusion support" ON)
option(WITH_OPENVINS "Include OpenVINS support" ON)
option(WITH_MADGWICK "Include Madgwick IMU filtering support" ON)
option(WITH_FASTCV "Include FastCV support" ON)
option(WITH_OPENMP "Include OpenMP support" ON)
IF(MOBILE_BUILD)
option(PCL_OMP "With PCL OMP implementations" OFF)
ELSE()
@@ -254,7 +255,7 @@ endif()
# OpenMP ("-fopenmp" should be added for flann included in PCL)
# the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it
if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang"))
if(((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")) AND WITH_OPENMP)
find_package(OpenMP COMPONENTS C CXX)
endif()
if(OPENMP_FOUND)
@@ -263,9 +264,10 @@ if(OPENMP_FOUND)
set(CMAKE_INSTALL_OPENMP_LIBRARIES TRUE)
message (STATUS "Found OpenMP: ${OpenMP_CXX_LIBRARIES}")
if(PCL_OMP)
message (STATUS "Add PCL_OMP to definitions")
add_definitions(-DPCL_OMP)
endif(PCL_OMP)
else(OPENMP_FOUND)
elseif(WITH_OPENMP)
message (STATUS "Not found OpenMP")
endif()
corelib/include/rtabmap/core/camera/CameraOpenNI2.h
+2
View File
@@ -68,6 +68,7 @@ public:
bool setMirroring(bool enabled);
void setOpenNI2StampsAndIDsUsed(bool used);
void setIRDepthShift(int horizontal, int vertical);
void setDepthDecimation(int decimation);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
@@ -85,6 +86,7 @@ private:
StereoCameraModel _stereoModel;
int _depthHShift;
int _depthVShift;
int _depthDecimation;
#endif
};
corelib/src/Camera.cpp
+1 -1
View File
@@ -99,7 +99,7 @@ SensorData Camera::takeImage(CameraInfo * info)
}
UTimer timer;
SensorData data = this->captureImage(info);
SensorData data = this->captureImage(info);
double captureTime = timer.ticks();
if(warnFrameRateTooHigh)
{
corelib/src/CameraThread.cpp
+5 -3
View File
@@ -365,8 +365,10 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
if(_distortionModel && !data.depthRaw().empty())
{
UTimer timer;
if(_distortionModel->getWidth() == data.depthRaw().cols &&
_distortionModel->getHeight() == data.depthRaw().rows )
if(_distortionModel->getWidth() >= data.depthRaw().cols &&
_distortionModel->getHeight() >= data.depthRaw().rows &&
_distortionModel->getWidth() % data.depthRaw().cols == 0 &&
_distortionModel->getHeight() % data.depthRaw().rows == 0)
{
cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
_distortionModel->undistort(depth);
@@ -374,7 +376,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
else
{
UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
UERROR("Distortion model size is %dx%d but depth image is %dx%d!",
_distortionModel->getWidth(), _distortionModel->getHeight(),
data.depthRaw().cols, data.depthRaw().rows);
}
corelib/src/camera/CameraOpenNI2.cpp
+21 -2
View File
@@ -74,7 +74,8 @@ CameraOpenNI2::CameraOpenNI2(
_deviceId(deviceId),
_openNI2StampsAndIDsUsed(false),
_depthHShift(0),
_depthVShift(0)
_depthVShift(0),
_depthDecimation(1)
#endif
{
}
@@ -184,6 +185,14 @@ void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
#endif
}
void CameraOpenNI2::setDepthDecimation(int decimation)
{
#ifdef RTABMAP_OPENNI2
UASSERT(decimation >= 1);
_depthDecimation = decimation;
#endif
}
bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_OPENNI2
@@ -544,8 +553,18 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
{
depth = _stereoModel.left().rectifyImage(depth, 0);
depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
CameraModel depthModel = _stereoModel.left().scaled(1.0 / double(_depthDecimation));
depth = util2d::decimate(depth, _depthDecimation);
depth = util2d::registerDepth(depth, depthModel.K(), rgb.size()/_depthDecimation, _stereoModel.right().scaled(1.0/double(_depthDecimation)).K(), _stereoModel.stereoTransform());
}
else if (_depthDecimation > 1)
{
depth = util2d::decimate(depth, _depthDecimation);
}
}
else if (_depthDecimation > 1)
{
depth = util2d::decimate(depth, _depthDecimation);
}
}
else // IR
corelib/src/clams/discrete_depth_distortion_model.cpp
+10 -8
View File
@@ -275,19 +275,21 @@ namespace clams
void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
{
UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows);
UASSERT(width_ >= depth.cols && width_ % depth.cols == 0);
UASSERT(height_ >=depth.rows && height_ % depth.rows == 0);
UASSERT(height_ >= depth.rows && height_ % depth.rows == 0);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
int factor = width_ / depth.cols;
if(depth.type() == CV_32FC1)
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
for(int v = 0; v < depth.rows; ++v) {
for(int u = 0; u < depth.cols; ++u) {
float & z = depth.at<float>(v, u);
if(uIsNan(z) || z == 0.0f)
continue;
double zf = z;
frustum(v, u).interpolatedUndistort(&zf);
frustum(v * factor, u * factor).interpolatedUndistort(&zf);
z = zf;
}
}
@@ -295,13 +297,13 @@ namespace clams
else
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
for(int v = 0; v < depth.rows; ++v) {
for(int u = 0; u < depth.cols; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u);
if(uIsNan(z) || z == 0)
continue;
double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf);
frustum(v * factor, u * factor).interpolatedUndistort(&zf);
z = zf*1000;
}
}
guilib/src/PreferencesDialog.cpp
+5
View File
@@ -692,6 +692,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->openni2_stampsIdsUsed, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->openni2_hshift, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->openni2_vshift, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->openni2_depth_decimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_freenect2MinDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_freenect2MaxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1953,6 +1954,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->openni2_stampsIdsUsed->setChecked(false);
_ui->openni2_hshift->setValue(0);
_ui->openni2_vshift->setValue(0);
_ui->openni2_depth_decimation->setValue(1);
_ui->comboBox_freenect2Format->setCurrentIndex(1);
_ui->doubleSpinBox_freenect2MinDepth->setValue(0.3);
_ui->doubleSpinBox_freenect2MaxDepth->setValue(12.0);
@@ -2408,6 +2410,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->lineEdit_openni2OniPath->setText(settings.value("oniPath", _ui->lineEdit_openni2OniPath->text()).toString());
_ui->openni2_hshift->setValue(settings.value("hshift", _ui->openni2_hshift->value()).toInt());
_ui->openni2_vshift->setValue(settings.value("vshift", _ui->openni2_vshift->value()).toInt());
_ui->openni2_depth_decimation->setValue(settings.value("depthDecimation", _ui->openni2_depth_decimation->value()).toInt());
settings.endGroup(); // Openni2
settings.beginGroup("Freenect2");
@@ -2921,6 +2924,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("oniPath", _ui->lineEdit_openni2OniPath->text());
settings.setValue("hshift", _ui->openni2_hshift->value());
settings.setValue("vshift", _ui->openni2_vshift->value());
settings.setValue("depthDecimation", _ui->openni2_depth_decimation->value());
settings.endGroup(); // Openni2
settings.beginGroup("Freenect2");
@@ -6349,6 +6353,7 @@ Camera * PreferencesDialog::createCamera(
}
((CameraOpenNI2*)camera)->setIRDepthShift(_ui->openni2_hshift->value(), _ui->openni2_vshift->value());
((CameraOpenNI2*)camera)->setMirroring(_ui->openni2_mirroring->isChecked());
((CameraOpenNI2*)camera)->setDepthDecimation(_ui->openni2_depth_decimation->value());
}
}
}
guilib/src/ui/preferencesDialog.ui
+128 -105
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-573</y>
<width>686</width>
<height>3905</height>
<y>-466</y>
<width>675</width>
<height>3499</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -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,1">
@@ -3109,7 +3109,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>3</number>
<number>0</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -3346,28 +3346,35 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>OpenNI 2</string>
</property>
<layout class="QGridLayout" name="gridLayout_54" columnstretch="0,1,0">
<item row="7" column="1">
<widget class="QLabel" name="label_435">
<item row="1" column="0">
<widget class="QCheckBox" name="openni2_autoWhiteBalance">
<property name="text">
<string>IR-Depth horizontal shift.</string>
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_218">
<property name="text">
<string>Auto exposure.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QSpinBox" name="openni2_hshift">
<property name="suffix">
<string> pix</string>
</property>
<property name="maximum">
<number>65535</number>
<item row="0" column="1">
<widget class="QLineEdit" name="lineEdit_openni2OniPath">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="openni2_autoWhiteBalance">
<item row="2" column="0">
<widget class="QCheckBox" name="openni2_autoExposure">
<property name="text">
<string/>
</property>
@@ -3393,50 +3400,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="openni2_stampsIdsUsed">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_231">
<property name="text">
<string>Path to a *.ONI file.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_220">
<property name="text">
<string>Gain.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="lineEdit_openni2OniPath">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QToolButton" name="toolButton_openni2OniPath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_217">
<property name="text">
@@ -3447,6 +3410,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_436">
<property name="text">
<string>IR-Depth vertical shift.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_223">
<property name="text">
@@ -3467,46 +3440,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QSpinBox" name="openni2_gain">
<item row="8" column="0">
<widget class="QSpinBox" name="openni2_vshift">
<property name="suffix">
<string> pix</string>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item row="9" column="0">
<spacer name="verticalSpacer_33">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="openni2_autoExposure">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_218">
<property name="text">
<string>Auto exposure.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
<number>65535</number>
</property>
</widget>
</item>
@@ -3520,18 +3460,78 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_436">
<item row="4" column="0">
<widget class="QSpinBox" name="openni2_gain">
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_220">
<property name="text">
<string>IR-Depth vertical shift.</string>
<string>Gain.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QSpinBox" name="openni2_vshift">
<item row="0" column="2">
<widget class="QLabel" name="label_231">
<property name="text">
<string>Path to a *.ONI file.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QToolButton" name="toolButton_openni2OniPath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="openni2_stampsIdsUsed">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_435">
<property name="text">
<string>IR-Depth horizontal shift.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="10" column="0">
<spacer name="verticalSpacer_33">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
<item row="7" column="0">
<widget class="QSpinBox" name="openni2_hshift">
<property name="suffix">
<string> pix</string>
</property>
@@ -3540,6 +3540,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_641">
<property name="text">
<string>Depth decimation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QSpinBox" name="openni2_depth_decimation">
<property name="suffix">
<string/>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>65535</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>