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

Added parameter "Rtabmap/RectifyOnlyFeatures". Added support of fisheye distortion model.

Browse Source
This commit is contained in:
matlabbe
2018-09-24 14:36:54 -04:00
parent f903ffb927
commit c341648a44
14 changed files with 425 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -327,6 +327,7 @@ private:
bool _savedMaximized;
QStringList _waypoints;
int _waypointsIndex;
std::vector<CameraModel> _rectCameraModels;
QMap<int, Signature> _cachedSignatures;
long _cachedMemoryUsage;

109
guilib/src/CalibrationDialog.cpp
View File

@@ -706,7 +706,19 @@ void CalibrationDialog::calibrate()
K = cv::Mat::eye(3,3,CV_64FC1);
UINFO("calibrate!");
//Find intrinsic and extrinsic camera parameters
double rms = cv::calibrateCamera(objectPoints,
double rms = 0.0;
bool fishEye = ui_->checkBox_fisheye->isChecked();
if(fishEye)
{
rms = cv::fisheye::calibrate(objectPoints,
imagePoints_[id],
imageSize_[id],
K,
D,
rvecs,
tvecs);
}
rms = cv::calibrateCamera(objectPoints,
imagePoints_[id],
imageSize_[id],
K,
@@ -724,7 +736,14 @@ void CalibrationDialog::calibrate()
for( i = 0; i < (int)objectPoints.size(); ++i )
{
cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
if(fishEye)
{
cv::fisheye::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
}
else
{
cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
}
err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);
int n = (int)objectPoints[i].size();
@@ -741,6 +760,16 @@ void CalibrationDialog::calibrate()
P.at<double>(2,3) = 1;
K.copyTo(P.colRange(0,3).rowRange(0,3));
if(fishEye)
{
// Convert to unified distortion model (k1,k2,p1,p2,k3,k4)
cv::Mat newD = cv::Mat::zeros(1,6,CV_64FC1);
newD.at<double>(0,0) = D.at<double>(0,0);
newD.at<double>(0,1) = D.at<double>(0,1);
newD.at<double>(0,4) = D.at<double>(0,2);
newD.at<double>(0,5) = D.at<double>(0,3);
}
std::cout << "K = " << K << std::endl;
std::cout << "D = " << D << std::endl;
std::cout << "width = " << imageSize_[id].width << std::endl;
@@ -875,28 +904,68 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
objectPoints[0].push_back(cv::Point3f(float(j*squareSize), float(i*squareSize), 0));
objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
double rms = 0.0;
bool fishEye = left.D_raw().cols == 6;
// calibrate extrinsic
if(fishEye)
{
cv::Mat D_left(1,4,CV_64FC1);
D_left.at<double>(0,0) = left.D_raw().at<double>(0,0);
D_left.at<double>(0,1) = left.D_raw().at<double>(0,1);
D_left.at<double>(0,2) = left.D_raw().at<double>(0,4);
D_left.at<double>(0,3) = left.D_raw().at<double>(0,5);
cv::Mat D_right(1,4,CV_64FC1);
UASSERT(right.D_raw().cols == 6);
D_right.at<double>(0,0) = right.D_raw().at<double>(0,0);
D_right.at<double>(0,1) = right.D_raw().at<double>(0,1);
D_right.at<double>(0,2) = right.D_raw().at<double>(0,4);
D_right.at<double>(0,3) = right.D_raw().at<double>(0,5);
#if CV_MAJOR_VERSION < 3
double rms = cv::stereoCalibrate(
objectPoints,
stereoImagePoints_[0],
stereoImagePoints_[1],
left.K_raw(), left.D_raw(),
right.K_raw(), right.D_raw(),
imageSize, R, T, E, F,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
cv::CALIB_FIX_INTRINSIC);
rms = cv::fisheye::stereoCalibrate(
objectPoints,
stereoImagePoints_[0],
stereoImagePoints_[1],
left.K_raw(), D_left,
right.K_raw(), D_right,
imageSize, R, T,
cv::CALIB_FIX_INTRINSIC,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
#else
double rms = cv::stereoCalibrate(
objectPoints,
stereoImagePoints_[0],
stereoImagePoints_[1],
left.K_raw(), left.D_raw(),
right.K_raw(), right.D_raw(),
imageSize, R, T, E, F,
cv::CALIB_FIX_INTRINSIC,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
rms = cv::fisheye::stereoCalibrate(
objectPoints,
stereoImagePoints_[0],
stereoImagePoints_[1],
left.K_raw(), D_left,
right.K_raw(), D_right,
imageSize, R, T,
cv::CALIB_FIX_INTRINSIC,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
#endif
}
else
{
#if CV_MAJOR_VERSION < 3
rms = cv::stereoCalibrate(
objectPoints,
stereoImagePoints_[0],
stereoImagePoints_[1],
left.K_raw(), left.D_raw(),
right.K_raw(), right.D_raw(),
imageSize, R, T, E, F,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
cv::CALIB_FIX_INTRINSIC);
#else
rms = cv::stereoCalibrate(
objectPoints,
stereoImagePoints_[0],
stereoImagePoints_[1],
left.K_raw(), left.D_raw(),
right.K_raw(), right.D_raw(),
imageSize, R, T, E, F,
cv::CALIB_FIX_INTRINSIC,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
#endif
}
UINFO("stereo calibration... done with RMS error=%f", rms);
std::cout << "R = " << R << std::endl;

54
guilib/src/MainWindow.cpp
View File

@@ -2781,6 +2781,57 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
cv::Mat image, depth;
SensorData data = iter->sensorData();
data.uncompressData(&image, &depth, 0);
ParametersMap allParameters = _preferencesDialog->getAllParameters();
bool rectifyOnlyFeatures = Parameters::defaultRtabmapRectifyOnlyFeatures();
bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
Parameters::parse(allParameters, Parameters::kRtabmapRectifyOnlyFeatures(), rectifyOnlyFeatures);
Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
if(rectifyOnlyFeatures && !imagesAlreadyRectified)
{
if(data.cameraModels().size())
{
UTimer time;
// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
cv::Mat rectifiedImages = data.imageRaw().clone();
bool initRectMaps = _rectCameraModels.empty();
if(initRectMaps)
{
_rectCameraModels.resize(data.cameraModels().size());
}
for(unsigned int i=0; i<data.cameraModels().size(); ++i)
{
if(data.cameraModels()[i].isValidForRectification())
{
if(initRectMaps)
{
_rectCameraModels[i] = data.cameraModels()[i];
if(!_rectCameraModels[i].isRectificationMapInitialized())
{
UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
_rectCameraModels[i].initRectificationMap();
UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
}
}
UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
_rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
}
else
{
UWARN("Camera %d of data %d is not valid for rectification (%dx%d).",
i, data.id(),
data.cameraModels()[i].imageWidth(),
data.cameraModels()[i].imageHeight());
}
}
UINFO("Time rectification: %fs", time.ticks());
data.setImageRaw(rectifiedImages);
image = rectifiedImages;
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
@@ -2792,7 +2843,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
_preferencesDialog->getCloudMaxDepth(0),
_preferencesDialog->getCloudMinDepth(0),
indices.get(),
_preferencesDialog->getAllParameters(),
allParameters,
_preferencesDialog->getCloudRoiRatios(0));
// view point
@@ -6293,6 +6344,7 @@ void MainWindow::clearTheCache()
_octomap = new OctoMap(_preferencesDialog->getAllParameters());
#endif
_occupancyGrid->clear();
_rectCameraModels.clear();
}
void MainWindow::openHelp()

1
guilib/src/PreferencesDialog.cpp
View File

@@ -703,6 +703,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
_ui->general_checkBox_startNewMapOnGoodSignature->setObjectName(Parameters::kRtabmapStartNewMapOnGoodSignature().c_str());
_ui->general_checkBox_imagesAlreadyRectified->setObjectName(Parameters::kRtabmapImagesAlreadyRectified().c_str());
_ui->general_checkBox_rectifyOnlyFeatures->setObjectName(Parameters::kRtabmapRectifyOnlyFeatures().c_str());
_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
connect(_ui->toolButton_workingDirectory, SIGNAL(clicked()), this, SLOT(changeWorkingDirectory()));

63
guilib/src/ui/calibrationDialog.ui
View File

@@ -17,16 +17,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -154,21 +145,12 @@
<rect>
<x>0</x>
<y>0</y>
<width>413</width>
<width>379</width>
<height>816</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -299,7 +281,14 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout_6">
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1,1">
<layout class="QGridLayout" name="gridLayout" columnstretch="0,0,0">
<item row="5" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Skew</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_19">
<property name="text">
@@ -435,13 +424,6 @@
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Skew</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QProgressBar" name="progressBar_skew">
<property name="value">
@@ -462,7 +444,7 @@
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0">
<layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0,0">
<item>
<widget class="QPushButton" name="pushButton_calibrate">
<property name="text">
@@ -470,6 +452,13 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_fisheye">
<property name="text">
<string>Fish eye</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_unlock">
<property name="text">
@@ -485,6 +474,13 @@
<string>Camera intrinsic parameters</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<item row="8" column="2">
<widget class="QLineEdit" name="lineEdit_P_2">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_fx">
<property name="text">
@@ -699,13 +695,6 @@
</property>
</widget>
</item>
<item row="8" column="2">
<widget class="QLineEdit" name="lineEdit_P_2">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_baseline_name">
<property name="text">

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

@@ -95,7 +95,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>681</width>
<width>678</width>
<height>2943</height>
</rect>
</property>
@@ -103,16 +103,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -126,7 +117,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>16</number>
<number>7</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -5093,16 +5084,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Directory of images (optional settings)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_93">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -6290,6 +6272,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<layout class="QVBoxLayout" name="verticalLayout_15">
<item>
<layout class="QGridLayout" name="gridLayout_43" columnstretch="0,1">
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
<property name="suffix">
<string> Hz</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="general_checkBox_SLAM_mode">
<property name="text">
@@ -6383,19 +6378,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
<property name="suffix">
<string> Hz</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_165">
<property name="text">
@@ -6455,6 +6437,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_498">
<property name="text">
<string>Rectify only features. If images are not already rectified (see parameter above) and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="general_checkBox_rectifyOnlyFeatures">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -15276,16 +15281,7 @@ Lower the ratio -&gt; higher the precision.</string>
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -15365,16 +15361,7 @@ Lower the ratio -&gt; higher the precision.</string>
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -15486,16 +15473,7 @@ Lower the ratio -&gt; higher the precision.</string>
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>