Skip to content

Commit 258095e

Browse files
matlabbematlabbe
authored
DBReader: added fine-grained camera selection (#1457)
1 parent 35c3ca2 commit 258095e

File tree

5 files changed

+171
-51
lines changed

5 files changed

+171
-51
lines changed

‎corelib/include/rtabmap/core/DBReader.h

+3-3
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera {
5151
bool ignoreGoalDelay = false,
5252
bool goalsIgnored = false,
5353
int startId = 0,
54-
int cameraIndex = -1,
54+
const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
5555
int stopId = 0,
5656
bool intermediateNodesIgnored = false,
5757
bool landmarksIgnored = false,
@@ -65,7 +65,7 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera {
6565
bool ignoreGoalDelay = false,
6666
bool goalsIgnored = false,
6767
int startId = 0,
68-
int cameraIndex = -1,
68+
const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
6969
int stopId = 0,
7070
bool intermediateNodesIgnored = false,
7171
bool landmarksIgnored = false,
@@ -99,7 +99,7 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera {
9999
bool _goalsIgnored;
100100
int _startId;
101101
int _stopId;
102-
int _cameraIndex;
102+
std::vector<unsigned int> _cameraIndices;
103103
bool _intermediateNodesIgnored;
104104
bool _landmarksIgnored;
105105
bool _featuresIgnored;

‎corelib/src/DBReader.cpp

+98-22
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ DBReader::DBReader(const std::string & databasePath,
4848
bool ignoreGoalDelay,
4949
bool goalsIgnored,
5050
int startId,
51-
int cameraIndex,
51+
const std::vector<unsigned int> & cameraIndices,
5252
int stopId,
5353
bool intermediateNodesIgnored,
5454
bool landmarksIgnored,
@@ -63,7 +63,7 @@ DBReader::DBReader(const std::string & databasePath,
6363
_goalsIgnored(goalsIgnored),
6464
_startId(startId),
6565
_stopId(stopId),
66-
_cameraIndex(cameraIndex),
66+
_cameraIndices(cameraIndices),
6767
_intermediateNodesIgnored(intermediateNodesIgnored),
6868
_landmarksIgnored(landmarksIgnored),
6969
_featuresIgnored(featuresIgnored),
@@ -94,7 +94,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
9494
bool ignoreGoalDelay,
9595
bool goalsIgnored,
9696
int startId,
97-
int cameraIndex,
97+
const std::vector<unsigned int> & cameraIndices,
9898
int stopId,
9999
bool intermediateNodesIgnored,
100100
bool landmarksIgnored,
@@ -109,7 +109,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
109109
_goalsIgnored(goalsIgnored),
110110
_startId(startId),
111111
_stopId(stopId),
112-
_cameraIndex(cameraIndex),
112+
_cameraIndices(cameraIndices),
113113
_intermediateNodesIgnored(intermediateNodesIgnored),
114114
_landmarksIgnored(landmarksIgnored),
115115
_featuresIgnored(featuresIgnored),
@@ -557,34 +557,77 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
557557
}
558558

559559
data.uncompressData();
560-
if(data.cameraModels().size() > 1 &&
561-
_cameraIndex >= 0)
560+
std::map<int, int> cameraOldNewIndices;
561+
std::vector<CameraModel> dbModels = data.cameraModels();
562+
if(dbModels.empty() && !data.stereoCameraModels().empty())
562563
{
563-
if(_cameraIndex < (int)data.cameraModels().size())
564+
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
564565
{
565-
// select one camera
566-
int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
567-
cv::Mat image;
566+
dbModels.push_back(data.stereoCameraModels()[i].left());
567+
}
568+
}
569+
if(dbModels.size() > 1 &&
570+
!_cameraIndices.empty())
571+
{
572+
cv::Mat combinedImages;
573+
cv::Mat combinedDepthImages;
574+
std::vector<CameraModel> combinedModels;
575+
std::vector<StereoCameraModel> combinedStereoModels;
576+
for(size_t i=0; i<_cameraIndices.size(); ++i)
577+
{
578+
UASSERT_MSG(_cameraIndices[i] < dbModels.size(), uFormat("DBReader: camera index %ld is not valid (should be between 0 and %ld)",
579+
_cameraIndices[i], dbModels.size()-1).c_str());
580+
581+
int addedCameras = std::max(combinedModels.size(), combinedStereoModels.size());
582+
583+
int subImageWidth = data.imageRaw().cols/dbModels.size();
568584
UASSERT(!data.imageRaw().empty() &&
569-
data.imageRaw().cols % data.cameraModels().size() == 0 &&
570-
_cameraIndex*subImageWidth < data.imageRaw().cols);
571-
image= cv::Mat(data.imageRaw(),
572-
cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone();
585+
data.imageRaw().cols % dbModels.size() == 0 &&
586+
(int)_cameraIndices[i]*subImageWidth < data.imageRaw().cols);
587+
if(combinedImages.empty())
588+
{
589+
// initialize with first camera
590+
combinedImages = cv::Mat(data.imageRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.imageRaw().type());
591+
}
592+
593+
cv::Mat fromROI = cv::Mat(data.imageRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.imageRaw().rows));
594+
cv::Mat toROI = cv::Mat(combinedImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedImages.rows));
595+
fromROI.copyTo(toROI);
573596

574597
cv::Mat depth;
575598
if(!data.depthOrRightRaw().empty())
576599
{
577-
UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
578-
subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
579-
_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
580-
depth = cv::Mat(data.depthOrRightRaw(),
581-
cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone();
600+
subImageWidth = data.depthOrRightRaw().cols/dbModels.size();
601+
UASSERT(data.depthOrRightRaw().cols % dbModels.size() == 0 &&
602+
subImageWidth == data.depthOrRightRaw().cols/(int)dbModels.size() &&
603+
(int)_cameraIndices[i]*subImageWidth < data.depthOrRightRaw().cols);
604+
if(combinedDepthImages.empty())
605+
{
606+
// initialize with first camera
607+
combinedDepthImages = cv::Mat(data.depthOrRightRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthOrRightRaw().type());
608+
}
609+
fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows));
610+
toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows));
611+
fromROI.copyTo(toROI);
612+
}
613+
614+
if(!data.cameraModels().empty())
615+
{
616+
combinedModels.push_back(data.cameraModels()[_cameraIndices[i]]);
617+
}
618+
else
619+
{
620+
combinedStereoModels.push_back(data.stereoCameraModels()[_cameraIndices[i]]);
582621
}
583-
data.setRGBDImage(image, depth, data.cameraModels().at(_cameraIndex));
622+
cameraOldNewIndices.insert(std::make_pair(_cameraIndices[i], i));
623+
}
624+
if(!combinedModels.empty())
625+
{
626+
data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
584627
}
585628
else
586629
{
587-
UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
630+
data.setStereoImage(combinedImages, combinedDepthImages, combinedStereoModels);
588631
}
589632
}
590633
data.setId(seq);
@@ -623,7 +666,40 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
623666
(keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
624667
(descriptors.empty() || (int)keypoints.size() == descriptors.rows))
625668
{
626-
data.setFeatures(keypoints, keypoints3D, descriptors);
669+
if(!cameraOldNewIndices.empty())
670+
{
671+
cv::Mat newDescriptors;
672+
std::vector<cv::KeyPoint> newKeypoints;
673+
std::vector<cv::Point3f> newKeypoints3D;
674+
UASSERT(!dbModels.empty() && dbModels[0].imageWidth()>0);
675+
int subImageWidth = dbModels[0].imageWidth();
676+
for(size_t i = 0; i<keypoints.size(); ++i)
677+
{
678+
int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
679+
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
680+
uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
681+
cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
682+
if(cameraOldNewIndices.find(cameraIndex) != cameraOldNewIndices.end())
683+
{
684+
int newCameraIndex = cameraOldNewIndices.at(cameraIndex);
685+
newKeypoints.push_back(keypoints[i]);
686+
newKeypoints.back().pt.x += (newCameraIndex-cameraIndex)*subImageWidth;
687+
if(!keypoints3D.empty())
688+
{
689+
newKeypoints3D.push_back(keypoints3D.at(i));
690+
}
691+
if(!descriptors.empty())
692+
{
693+
newDescriptors.push_back(descriptors.row(i));
694+
}
695+
}
696+
}
697+
data.setFeatures(newKeypoints, newKeypoints3D, newDescriptors);
698+
}
699+
else
700+
{
701+
data.setFeatures(keypoints, keypoints3D, descriptors);
702+
}
627703
}
628704
else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
629705
{

‎guilib/src/PreferencesDialog.cpp

+43-6
Original file line numberDiff line numberDiff line change
@@ -742,7 +742,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
742742
connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
743743
connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
744744
connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
745-
connect(_ui->source_spinBox_database_cameraIndex, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
745+
connect(_ui->source_lineEdit_databaseCameraIndex, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
746746
connect(_ui->source_checkBox_stereoToDepthDB, SIGNAL(toggled(bool)), _ui->checkbox_stereo_depthGenerated, SLOT(setChecked(bool)));
747747
connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(toggled(bool)), _ui->source_checkBox_stereoToDepthDB, SLOT(setChecked(bool)));
748748

@@ -2136,7 +2136,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
21362136
_ui->source_checkBox_ignorePriors->setChecked(false);
21372137
_ui->source_spinBox_databaseStartId->setValue(0);
21382138
_ui->source_spinBox_databaseStopId->setValue(0);
2139-
_ui->source_spinBox_database_cameraIndex->setValue(-1);
2139+
_ui->source_lineEdit_databaseCameraIndex->setText("");
21402140
_ui->source_checkBox_useDbStamps->setChecked(true);
21412141

21422142
#ifdef _WIN32
@@ -2867,7 +2867,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
28672867
_ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool());
28682868
_ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt());
28692869
_ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt());
2870-
_ui->source_spinBox_database_cameraIndex->setValue(settings.value("cameraIndex", _ui->source_spinBox_database_cameraIndex->value()).toInt());
2870+
_ui->source_lineEdit_databaseCameraIndex->setText(settings.value("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text()).toString());
28712871
_ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool());
28722872
settings.endGroup(); // Database
28732873

@@ -3467,7 +3467,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
34673467
settings.setValue("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked());
34683468
settings.setValue("startId", _ui->source_spinBox_databaseStartId->value());
34693469
settings.setValue("stopId", _ui->source_spinBox_databaseStopId->value());
3470-
settings.setValue("cameraIndex", _ui->source_spinBox_database_cameraIndex->value());
3470+
settings.setValue("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text());
34713471
settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked());
34723472
settings.endGroup(); // Database
34733473

@@ -4389,7 +4389,7 @@ void PreferencesDialog::selectSourceDatabase()
43894389
_ui->source_database_lineEdit_path->setText(paths.size()==1?paths.front():paths.join(";"));
43904390
_ui->source_spinBox_databaseStartId->setValue(0);
43914391
_ui->source_spinBox_databaseStopId->setValue(0);
4392-
_ui->source_spinBox_database_cameraIndex->setValue(-1);
4392+
_ui->source_lineEdit_databaseCameraIndex->setText("");
43934393
}
43944394
}
43954395

@@ -6953,13 +6953,50 @@ Camera * PreferencesDialog::createCamera(
69536953
}
69546954
else if(driver == kSrcDatabase)
69556955
{
6956+
std::vector<unsigned int> cameraIndices;
6957+
if(!_ui->source_lineEdit_databaseCameraIndex->text().isEmpty())
6958+
{
6959+
// read the first node to know how many cameras we have in the database
6960+
std::shared_ptr<DBReader> reader(
6961+
new DBReader(
6962+
_ui->source_database_lineEdit_path->text().toStdString(),
6963+
0,
6964+
false,
6965+
false,
6966+
false,
6967+
_ui->source_spinBox_databaseStartId->value()));
6968+
if(reader->init())
6969+
{
6970+
SensorData data = reader->takeImage();
6971+
unsigned int numCamerasInDb = data.cameraModels().size()>0?data.cameraModels().size():data.stereoCameraModels().size();
6972+
if(numCamerasInDb>1)
6973+
{
6974+
QStringList indicesStr = _ui->source_lineEdit_databaseCameraIndex->text().split(' ');
6975+
for(QStringList::iterator iter=indicesStr.begin(); iter!=indicesStr.end(); ++iter)
6976+
{
6977+
cameraIndices.push_back(iter->toInt());
6978+
if(cameraIndices.back() > numCamerasInDb)
6979+
{
6980+
QMessageBox::warning(this,
6981+
tr("Creating Database Reader"),
6982+
tr("Camera index %1 is not valid, it should be between 0 and %2. Remove or update camera indices under Source->Database panel (currently set to \"%3\").")
6983+
.arg(cameraIndices.back()).arg(numCamerasInDb).arg(_ui->source_lineEdit_databaseCameraIndex->text()),
6984+
QMessageBox::Ok);
6985+
cameraIndices.clear();
6986+
return 0;
6987+
}
6988+
}
6989+
}
6990+
}
6991+
}
6992+
69566993
camera = new DBReader(_ui->source_database_lineEdit_path->text().toStdString(),
69576994
_ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
69586995
_ui->source_checkBox_ignoreOdometry->isChecked(),
69596996
_ui->source_checkBox_ignoreGoalDelay->isChecked(),
69606997
_ui->source_checkBox_ignoreGoals->isChecked(),
69616998
_ui->source_spinBox_databaseStartId->value(),
6962-
_ui->source_spinBox_database_cameraIndex->value(),
6999+
cameraIndices,
69637000
_ui->source_spinBox_databaseStopId->value(),
69647001
!_ui->general_checkBox_createIntermediateNodes->isChecked(),
69657002
_ui->source_checkBox_ignoreLandmarks->isChecked(),

‎guilib/src/ui/preferencesDialog.ui

+8-15
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@
6363
<property name="geometry">
6464
<rect>
6565
<x>0</x>
66-
<y>-520</y>
66+
<y>-594</y>
6767
<width>713</width>
6868
<height>4653</height>
6969
</rect>
@@ -95,7 +95,7 @@
9595
<enum>QFrame::Raised</enum>
9696
</property>
9797
<property name="currentIndex">
98-
<number>15</number>
98+
<number>5</number>
9999
</property>
100100
<widget class="QWidget" name="page_22">
101101
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3424,7 +3424,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
34243424
<item>
34253425
<widget class="QStackedWidget" name="stackedWidget_src">
34263426
<property name="currentIndex">
3427-
<number>2</number>
3427+
<number>3</number>
34283428
</property>
34293429
<widget class="QWidget" name="page_41">
34303430
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -6777,7 +6777,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
67776777
<property name="checked">
67786778
<bool>false</bool>
67796779
</property>
6780-
<layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0,0">
6780+
<layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1,0">
67816781
<item row="8" column="0">
67826782
<widget class="QSpinBox" name="source_spinBox_databaseStartId">
67836783
<property name="minimum">
@@ -6930,7 +6930,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
69306930
<item row="10" column="1">
69316931
<widget class="QLabel" name="label_315">
69326932
<property name="text">
6933-
<string>Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed.</string>
6933+
<string>Camera index. If the database contains multi-camera data, you can choose which camera to use. Leave empty to use all cameras. Can also be multiple indices split by spaces in a string like &quot;0 2&quot; to stream cameras 0 and 2 only.</string>
69346934
</property>
69356935
<property name="wordWrap">
69366936
<bool>true</bool>
@@ -6981,16 +6981,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
69816981
</property>
69826982
</widget>
69836983
</item>
6984-
<item row="10" column="0">
6985-
<widget class="QSpinBox" name="source_spinBox_database_cameraIndex">
6986-
<property name="minimum">
6987-
<number>-1</number>
6988-
</property>
6989-
<property name="maximum">
6990-
<number>9999</number>
6991-
</property>
6992-
</widget>
6993-
</item>
69946984
<item row="8" column="1">
69956985
<widget class="QLabel" name="label_58">
69966986
<property name="text">
@@ -7041,6 +7031,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
70417031
</property>
70427032
</widget>
70437033
</item>
7034+
<item row="10" column="0">
7035+
<widget class="QLineEdit" name="source_lineEdit_databaseCameraIndex"/>
7036+
</item>
70447037
</layout>
70457038
</widget>
70467039
</item>

0 commit comments

Comments
 (0)