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

Odom sensor option (#726)

Browse Source 
* Added Odom Sensor option

* Odom Sensor: Added pose time offset parameter

* Gui: fixed odom cloud not shown when rgb/depth doesn't have same size. Odom sensor: fixed rawImages set on non-stereo camera (because stereoRectify wrongly set to all cameras)

* Calibration: fixed rgb and depth suffixes not correctly set. OpenNI2: horizontal and vertical shifts can be also set for default calib.

* Fixed a warning

* OdomSensor: added option to use odom sensor output as ground truth (for comparison between odom sensor and rtabmap odom). Added scale factor option.

* FindG2O.cmake: updated path suffixes to find EXTERNAL csparse

* Pose3GravityFactor: fixed dllimport error on windows

* UI: Statistics panel not updated if not visible and keep stats in cache is unchecked. Added description to some timing debug logs in processStatistics().

* Removed vtkOutputWindow on Windows

* Odom Sensor: added zed sdk support

* bump version 0.20.11
This commit is contained in:
matlabbe
2021-05-23 17:21:12 -04:00
committed by GitHub
parent fe896260c5
commit 21dbeed4b7
25 changed files with 1532 additions and 728 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
guilib/include/rtabmap/gui/CalibrationDialog.h
View File

@@ -57,6 +57,9 @@ public:
const rtabmap::StereoCameraModel & getStereoCameraModel() const {return stereoModel_;}
bool isProcessing() const {return processingData_;}
int getStereoPairs() const {return (int)stereoImagePoints_[0].size();}
int boardWidth() const;
int boardHeight() const;
double squareSize() const;
void saveSettings(QSettings & settings, const QString & group = "") const;
void loadSettings(QSettings & settings, const QString & group = "");

7
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -165,6 +165,8 @@ public:
int getOdomRegistrationApproach() const;
double getOdomF2MGravitySigma() const;
bool isOdomDisabled() const;
bool isOdomSensorAsGt() const;
double getOdomSensorScaleFactor() const;
bool isGroundTruthAligned() const;
bool isGraphsShown() const;
@@ -249,6 +251,7 @@ public:
PreferencesDialog::Src getSourceDriver() const;
QString getSourceDriverStr() const;
QString getSourceDevice() const;
PreferencesDialog::Src getOdomSourceDriver() const;
bool isSourceDatabaseStampsUsed() const;
bool isSourceDatabaseStereoToDepth() const;
@@ -277,6 +280,7 @@ public:
QString getIMUPath() const;
int getIMURate() const;
Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null
Camera * createOdomSensor(Transform & extrinsics, double & timeOffset); // return camera should be deleted if not null
int getIgnoredDCComponents() const;
@@ -314,6 +318,7 @@ public Q_SLOTS:
void selectSourceDriver(Src src, int variant = 0);
void calibrate();
void calibrateSimple();
void calibrateOdomSensorExtrinsics();
private Q_SLOTS:
void closeDialog ( QAbstractButton * button );
@@ -353,6 +358,7 @@ private Q_SLOTS:
void visualizeDistortionModel();
void selectSourceDatabase();
void selectCalibrationPath();
void selectOdomSensorCalibrationPath();
void selectSourceImagesStamps();
void selectSourceRGBDImagesPathRGB();
void selectSourceRGBDImagesPathDepth();
@@ -409,6 +415,7 @@ private:
void addParameters(const QGroupBox * box);
QList<QGroupBox*> getGroupBoxes();
void readSettingsBegin();
Camera * createCamera(Src driver, const QString & device, const QString & calibrationPath, bool useRawImages, bool useColor, bool odomOnly); // return camera should be deleted if not null
protected:
PANEL_FLAGS _obsoletePanels;

22
guilib/src/CalibrationDialog.cpp
View File

@@ -213,6 +213,19 @@ void CalibrationDialog::setStereoMode(bool stereo, const QString & leftSuffix, c
ui_->label_stereoBaseline->setVisible(stereo_);
}
int CalibrationDialog::boardWidth() const
{
return ui_->spinBox_boardWidth->value();
}
int CalibrationDialog::boardHeight() const
{
return ui_->spinBox_boardHeight->value();
}
double CalibrationDialog::squareSize() const
{
return ui_->doubleSpinBox_squareSize->value();
}
void CalibrationDialog::setBoardWidth(int width)
{
if(width != ui_->spinBox_boardWidth->value())
@@ -314,11 +327,14 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
{
UDEBUG("Processing images");
processingData_ = true;
cameraName_ = "0000";
if(!cameraName.isEmpty())
if(cameraName_.isEmpty())
{
cameraName_ = cameraName;
}
else if(cameraName.isEmpty())
{
cameraName_ = "0000";
}
if(ui_->label_serial->text().compare(cameraName_)!=0)
{
@@ -875,6 +891,8 @@ void CalibrationDialog::calibrate()
cv::Mat P = stereoModel_.right().P().clone();
P.at<double>(0,3) = -P.at<double>(0,0)*ui_->doubleSpinBox_stereoBaseline->value();
double scale = ui_->doubleSpinBox_stereoBaseline->value() / stereoModel_.baseline();
UWARN("Scale %f (setting square size from %f to %f)", scale, ui_->doubleSpinBox_squareSize->value(), ui_->doubleSpinBox_squareSize->value()*scale);
ui_->doubleSpinBox_squareSize->setValue(ui_->doubleSpinBox_squareSize->value()*scale);
stereoModel_ = StereoCameraModel(
stereoModel_.name(),
stereoModel_.left().imageSize(),stereoModel_.left().K_raw(), stereoModel_.left().D_raw(), stereoModel_.left().R(), stereoModel_.left().P(),

3
guilib/src/CloudViewer.cpp
View File

@@ -141,9 +141,6 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
{
UDEBUG("");
this->setMinimumSize(200, 200);
#if VTK_MAJOR_VERSION >= 8
vtkObject::GlobalWarningDisplayOff();
#endif
int argc = 0;
UASSERT(style!=0);

393
guilib/src/MainWindow.cpp
View File

@@ -902,7 +902,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
else
{
Q_EMIT cameraInfoReceived(cameraEvent->info());
if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
if (_odomThread == 0 && (_camera->odomProvided()) && _preferencesDialog->isRGBDMode())
{
OdometryInfo odomInfo;
odomInfo.reg.covariance = cameraEvent->info().odomCovariance;
@@ -965,15 +965,18 @@ void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
{
_firstStamp = info.stamp;
}
_ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
_ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
Q_EMIT(cameraInfoProcessed());
}
@@ -1053,7 +1056,6 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(!imagesAlreadyRectified)
{
rectifiedData = odom.data();
UTimer time;
if(data->cameraModels().size())
{
// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
@@ -1123,7 +1125,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
UWARN("Stereo camera model of data %d is not valid for rectification.", data->id());
}
}
UINFO("Time rectification: %fs", time.ticks());
UDEBUG("Time rectification: %fs", time.ticks());
data = &rectifiedData;
}
}
@@ -1138,8 +1140,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(!pose.isNull())
{
// 3d cloud
if(data->depthOrRightRaw().cols == data->imageRaw().cols &&
data->depthOrRightRaw().rows == data->imageRaw().rows &&
if(!data->imageRaw().empty() &&
!data->depthOrRightRaw().empty() &&
(data->cameraModels().size() || data->stereoCameraModel().isValidForProjection()) &&
_preferencesDialog->isCloudsShown(1))
@@ -1491,6 +1492,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_cloudViewer->removeLine("odom_imu_acc");
}
}
UDEBUG("Time 3D Rendering: %fs", time.ticks());
}
if(!odom.pose().isNull())
@@ -1540,6 +1542,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
}
#endif
_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
UDEBUG("Time Update Pose: %fs", time.ticks());
}
_cloudViewer->update();
@@ -1549,6 +1552,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
{
_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
_ui->graphicsView_graphView->update();
UDEBUG("Time Update graphview: %fs", time.ticks());
}
}
@@ -1682,6 +1686,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
}
_ui->imageView_odometry->update();
UDEBUG("Time update imageview: %fs", time.ticks());
}
if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
@@ -1690,145 +1696,149 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
}
//Process info
_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralDistribution, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().timeParticleFiltering>0.0f)
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
if(odom.info().gravityRollError>0.0f || odom.info().gravityPitchError > 0.0f)
{
_ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().gravityRollError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().gravityPitchError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().localBundleTime > 0.0f)
{
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures());
Transform odomT;
float dist=0.0f, x,y,z, roll,pitch,yaw;
if(!odom.info().transform.isNull())
{
odomT = odom.info().transform;
odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().transform.getNorm();
_ui->statsToolBox->updateStat("Odometry/T/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Tx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Ty/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Tz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Troll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Tpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Tyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
if(!odom.info().transformFiltered.isNull())
{
odomT = odom.info().transformFiltered;
odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().transformFiltered.getNorm();
_ui->statsToolBox->updateStat("Odometry/TF/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
if(odom.info().interval > 0)
{
_ui->statsToolBox->updateStat("Odometry/Interval/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().interval*1000.f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Speed/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Speed/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
if(!odom.info().guess.isNull())
_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralDistribution, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().timeParticleFiltering>0.0f)
{
odom.info().guess.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().guess.getNorm();
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
}
if(!odom.info().transformGroundTruth.isNull())
{
if(!odomT.isNull())
if(odom.info().gravityRollError>0.0f || odom.info().gravityPitchError > 0.0f)
{
rtabmap::Transform diff = odom.info().transformGroundTruth.inverse()*odomT;
_ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getNorm(), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TG_error_ang/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getAngle()*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().gravityRollError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().gravityPitchError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().localBundleTime > 0.0f)
{
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures());
Transform odomT;
float dist=0.0f, x,y,z, roll,pitch,yaw;
if(!odom.info().transform.isNull())
{
odomT = odom.info().transform;
odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().transform.getNorm();
_ui->statsToolBox->updateStat("Odometry/T/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Tx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Ty/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Tz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Troll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Tpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Tyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().transformGroundTruth.getNorm();
_ui->statsToolBox->updateStat("Odometry/TG/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
if(!odom.info().transformFiltered.isNull())
{
odomT = odom.info().transformFiltered;
odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().transformFiltered.getNorm();
_ui->statsToolBox->updateStat("Odometry/TF/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TFyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
if(odom.info().interval > 0)
{
_ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedG/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedG/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Interval/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().interval*1000.f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Speed/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Speed/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
if(!odom.info().guess.isNull())
{
odom.info().guess.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().guess.getNorm();
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
}
}
}
//cumulative pose
if(!odom.pose().isNull())
{
odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/Px/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Py/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Pz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Proll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Ppitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Pyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
if(!data->groundTruth().isNull())
{
data->groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/PGx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
if(!odom.info().transformGroundTruth.isNull())
{
if(!odomT.isNull())
{
rtabmap::Transform diff = odom.info().transformGroundTruth.inverse()*odomT;
_ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getNorm(), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TG_error_ang/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getAngle()*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
if(odom.info().distanceTravelled > 0)
{
_ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().distanceTravelled, _preferencesDialog->isCacheSavedInFigures());
}
odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().transformGroundTruth.getNorm();
_ui->statsToolBox->updateStat("Odometry/TG/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().interval > 0)
{
_ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedG/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedG/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
}
}
_ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), time.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures());
//cumulative pose
if(!odom.pose().isNull())
{
odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/Px/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Py/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Pz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Proll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Ppitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Pyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
if(!data->groundTruth().isNull())
{
data->groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/PGx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/PGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
}
if(odom.info().distanceTravelled > 0)
{
_ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().distanceTravelled, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), time.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures());
UDEBUG("Time updating Stats toolbox: %fs", time.ticks());
}
_processingOdometry = false;
Q_EMIT(odometryProcessed());
@@ -2011,8 +2021,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
" Gray = Not Quantized to Vocabulary");
}
UDEBUG("time= %d ms", time.restart());
int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
int landmarkId = static_cast<int>(uValue(stat.data(), Statistics::kLoopLandmark_detected(), 0.0f));
@@ -2094,6 +2102,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
_cachedLocalizationsCount[matchId] += 1.0f;
}
UDEBUG("time= %d ms (update detection ui)", time.restart());
//update image views
if(!signature.sensorData().imageRaw().empty() || signature.getWords().size())
@@ -2125,7 +2134,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
qimageLoopThread.join();
QImage img = qimageThread.getQImage();
QImage lcImg = qimageLoopThread.getQImage();
UDEBUG("time= %d ms", time.restart());
UDEBUG("time= %d ms (convert image to qt)", time.restart());
if(!img.isNull())
{
@@ -2174,7 +2183,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
}
UDEBUG("time= %d ms", time.restart());
UDEBUG("time= %d ms (update detection imageviews)", time.restart());
// do it after scaling
std::multimap<int, cv::KeyPoint> wordsA;
@@ -2189,7 +2198,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
this->drawKeypoints(wordsA, wordsB);
UDEBUG("time= %d ms", time.restart());
UDEBUG("time= %d ms (draw keypoints)", time.restart());
// loop closure view
if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
@@ -2205,10 +2214,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UTimer loopTimer;
_loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
_ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
_ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
}
}
UDEBUG("time= %d ms", time.restart());
UDEBUG("time= %d ms (update loop closure viewer)", time.restart());
}
}
@@ -2230,20 +2242,24 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
_rawLikelihoodCurve->setData(QMap<int, float>(stat.rawLikelihood()), QMap<int, int>(stat.weights()));
}
UDEBUG("time= %d ms (update likelihood and posterior)", time.restart());
// Update statistics tool box
const std::map<std::string, float> & statistics = stat.data();
std::string odomStr = "Odometry/";
for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
//ULOGGER_DEBUG("Updating stat \"%s\"", (*iter).first.c_str());
if((*iter).first.size()<odomStr.size() || (*iter).first.substr(0, odomStr.size()).compare(odomStr)!=0)
const std::map<std::string, float> & statistics = stat.data();
std::string odomStr = "Odometry/";
for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
{
_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), (*iter).second, _preferencesDialog->isCacheSavedInFigures());
//ULOGGER_DEBUG("Updating stat \"%s\"", (*iter).first.c_str());
if((*iter).first.size()<odomStr.size() || (*iter).first.substr(0, odomStr.size()).compare(odomStr)!=0)
{
_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), (*iter).second, _preferencesDialog->isCacheSavedInFigures());
}
}
}
UDEBUG("time= %d ms", time.restart());
UDEBUG("time= %d ms (update stats toolbox)", time.restart());
//======================
// RGB-D Mapping stuff
@@ -2285,7 +2301,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
std::map<int, Transform> poses = stat.poses();
UDEBUG("time= %d ms", time.restart());
UDEBUG("time= %d ms (update gt-gps stuff)", time.restart());
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
if(_preferencesDialog->isFramesShown())
@@ -2373,11 +2389,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_odometryReceived = false;
UDEBUG("time= %d ms", time.restart());
UDEBUG("time= %d ms (update map cloud)", time.restart());
for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
_ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
{
_ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
}
}
}
_odometryCorrection = stat.mapCorrection();
@@ -2415,8 +2434,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
}
UDEBUG("time= %d ms (update graph view)", time.restart());
}
UDEBUG("");
_cachedSignatures.remove(0); // remove tmp negative ids
@@ -2430,7 +2449,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_cachedMemoryUsage += s.sensorData().getMemoryUsed();
}
UDEBUG("");
UDEBUG("time= %d ms (update cache)", time.restart());
}
else if(!stat.extended() && stat.loopClosureId()>0)
{
@@ -2443,7 +2462,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
float elapsedTime = static_cast<float>(totalTime.elapsed());
UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
_ui->statsToolBox->updateStat("GUI/Refresh stats/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), elapsedTime, _preferencesDialog->isCacheSavedInFigures());
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
_ui->statsToolBox->updateStat("GUI/Refresh stats/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), elapsedTime, _preferencesDialog->isCacheSavedInFigures());
}
if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
{
this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
@@ -2455,11 +2477,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_cachedMemoryUsage = 0;
_cachedWordsCount.clear();
}
_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
#ifdef RTABMAP_OCTOMAP
_ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
#endif
}
if(_state != kMonitoring && _state != kDetecting)
{
_ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
@@ -4435,12 +4460,14 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
_ui->graphicsView_graphView->setGlobalPath(event.getPoses());
}
_ui->statsToolBox->updateStat("Planning/From/", float(event.getPoses().size()?event.getPoses().begin()->first:0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Planning/Time/ms", float(event.getPlanningTime()*1000.0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Planning/Goal/", float(event.getGoal()), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Planning/Poses/", float(event.getPoses().size()), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Planning/Length/m", float(graph::computePathLength(event.getPoses())), _preferencesDialog->isCacheSavedInFigures());
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
_ui->statsToolBox->updateStat("Planning/From/", float(event.getPoses().size()?event.getPoses().begin()->first:0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Planning/Time/ms", float(event.getPlanningTime()*1000.0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Planning/Goal/", float(event.getGoal()), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Planning/Poses/", float(event.getPoses().size()), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Planning/Length/m", float(graph::computePathLength(event.getPoses())), _preferencesDialog->isCacheSavedInFigures());
}
if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
{
// use MessageBox
@@ -5436,8 +5463,28 @@ void MainWindow::startDetection()
Q_EMIT stateChanged(kInitialized);
return;
}
double poseTimeOffset = 0.0;
Transform extrinsics;
Camera * odomSensor = 0;
if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef &&
_preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() &&
!(_preferencesDialog->getOdomSourceDriver() == PreferencesDialog::kSrcStereoRealSense2 &&
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2))
{
UINFO("Create Odom Sensor %d (camera = %d)",
_preferencesDialog->getOdomSourceDriver(),
_preferencesDialog->getSourceDriver());
odomSensor = _preferencesDialog->createOdomSensor(extrinsics, poseTimeOffset);
}
_camera = new CameraThread(camera, parameters);
if(odomSensor)
{
_camera = new CameraThread(camera, odomSensor, extrinsics, poseTimeOffset, _preferencesDialog->getOdomSensorScaleFactor(), _preferencesDialog->isOdomSensorAsGt(), parameters);
}
else
{
_camera = new CameraThread(camera, _preferencesDialog->getOdomSensorScaleFactor(), _preferencesDialog->isOdomSensorAsGt(), parameters);
}
_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
_camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
@@ -5504,7 +5551,7 @@ void MainWindow::startDetection()
_imuThread = 0;
}
if(!camera->odomProvided() && !_preferencesDialog->isOdomDisabled())
if((!_camera->odomProvided() || _preferencesDialog->isOdomSensorAsGt()) && !_preferencesDialog->isOdomDisabled())
{
ParametersMap odomParameters = parameters;
if(_preferencesDialog->getOdomRegistrationApproach() < 3)

490
guilib/src/PreferencesDialog.cpp
View File

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QProgressDialog>
#include <QScrollBar>
#include <QStatusBar>
#include <QFormLayout>
#include <QDesktopServices>
#include <QtGui/QCloseEvent>
@@ -358,6 +359,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
{
_ui->comboBox_cameraRGBD->setItemData(kSrcRealSense2 - kSrcRGBD, 0, Qt::UserRole - 1);
_ui->comboBox_cameraStereo->setItemData(kSrcStereoRealSense2 - kSrcStereo, 0, Qt::UserRole - 1);
_ui->comboBox_odom_sensor->setItemData(1, 0, Qt::UserRole - 1);
}
if(!CameraStereoDC1394::available())
{
@@ -370,6 +372,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
if (!CameraStereoZed::available())
{
_ui->comboBox_cameraStereo->setItemData(kSrcStereoZed - kSrcStereo, 0, Qt::UserRole - 1);
_ui->comboBox_odom_sensor->setItemData(2, 0, Qt::UserRole - 1);
}
if (!CameraStereoTara::available())
{
@@ -708,8 +711,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->spinBox_rs2_height_depth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_rs2_rate_depth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_rs2_globalTimeStync, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_rs2_dualMode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_rs2_dualModeExtrinsics, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_rs2_jsonFile, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_rs2_jsonFile, SIGNAL(clicked()), this, SLOT(selectSourceRealsense2JsonPath()));
@@ -771,12 +772,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->comboBox_stereoZed_sensingMode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_stereoZed_confidenceThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_stereoZed_texturenessConfidenceThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_stereoZed_odom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_zedSvoPath, SIGNAL(clicked()), this, SLOT(selectSourceSvoPath()));
connect(_ui->lineEdit_zedSvoPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_stereoRealSense2_odom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_stereoMyntEye_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_stereoMyntEye_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_stereoMyntEye_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -813,6 +811,16 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_bilateral_sigmaS, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_bilateral_sigmaR, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_odom_sensor_extrinsics, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_odom_sensor, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_odom_sensor_path_calibration, SIGNAL(clicked()), this, SLOT(selectOdomSensorCalibrationPath()));
connect(_ui->pushButton_odom_sensor_calibrate, SIGNAL(clicked()), this, SLOT(calibrateOdomSensorExtrinsics()));
connect(_ui->lineEdit_odom_sensor_path_calibration, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_odomSourceDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_odom_sensor_time_offset, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_odom_sensor_scale_factor, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_odom_sensor_use_as_gt, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_imuFilter, SLOT(setCurrentIndex(int)));
_ui->stackedWidget_imuFilter->setCurrentIndex(_ui->comboBox_imuFilter_strategy->currentIndex());
@@ -1959,8 +1967,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->spinBox_rs2_height_depth->setValue(480);
_ui->spinBox_rs2_rate_depth->setValue(30);
_ui->checkbox_rs2_globalTimeStync->setChecked(true);
_ui->checkbox_rs2_dualMode->setChecked(false);
_ui->lineEdit_rs2_dualModeExtrinsics->setText("0.009 0.021 0.027 0 -0.018 0.005");
_ui->lineEdit_rs2_jsonFile->clear();
_ui->lineEdit_openniOniPath->clear();
_ui->lineEdit_openni2OniPath->clear();
@@ -1997,10 +2003,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(0);
_ui->spinBox_stereoZed_confidenceThr->setValue(100);
_ui->spinBox_stereoZed_texturenessConfidenceThr->setValue(90);
_ui->checkbox_stereoZed_odom->setChecked(false);
_ui->lineEdit_zedSvoPath->clear();
_ui->comboBox_stereoZedOC_resolution->setCurrentIndex(3);
_ui->checkbox_stereoRealSense2_odom->setChecked(false);
_ui->checkbox_stereoMyntEye_rectify->setChecked(false);
_ui->checkbox_stereoMyntEye_depth->setChecked(false);
_ui->checkbox_stereoMyntEye_autoExposure->setChecked(true);
@@ -2028,6 +2032,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->lineEdit_cameraImages_imu_transform->setText("0 0 1 0 -1 0 1 0 0");
_ui->spinBox_cameraImages_max_imu_rate->setValue(0);
_ui->comboBox_odom_sensor->setCurrentIndex(0);
_ui->lineEdit_odom_sensor_extrinsics->setText("0.006977 -0.042236 0.004599 -0.012436 -0.002432 -0.004827");
_ui->lineEdit_odom_sensor_path_calibration->setText("");
_ui->lineEdit_odomSourceDevice->setText("");
_ui->doubleSpinBox_odom_sensor_time_offset->setValue(0.0);
_ui->doubleSpinBox_odom_sensor_scale_factor->setValue(1);
_ui->checkBox_odom_sensor_use_as_gt->setChecked(false);
_ui->comboBox_imuFilter_strategy->setCurrentIndex(2);
_ui->doubleSpinBox_imuFilterMadgwickGain->setValue(Parameters::defaultImuFilterMadgwickGain());
_ui->doubleSpinBox_imuFilterMadgwickZeta->setValue(Parameters::defaultImuFilterMadgwickZeta());
@@ -2425,8 +2437,6 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->spinBox_rs2_height_depth->setValue(settings.value("height_depth", _ui->spinBox_rs2_height_depth->value()).toInt());
_ui->spinBox_rs2_rate_depth->setValue(settings.value("rate_depth", _ui->spinBox_rs2_rate_depth->value()).toInt());
_ui->checkbox_rs2_globalTimeStync->setChecked(settings.value("global_time_sync", _ui->checkbox_rs2_globalTimeStync->isChecked()).toBool());
_ui->checkbox_rs2_dualMode->setChecked(settings.value("dual_mode", _ui->checkbox_rs2_dualMode->isChecked()).toBool());
_ui->lineEdit_rs2_dualModeExtrinsics->setText(settings.value("dual_mode_extrinsics", _ui->lineEdit_rs2_dualModeExtrinsics->text()).toString());
_ui->lineEdit_rs2_jsonFile->setText(settings.value("json_preset", _ui->lineEdit_rs2_jsonFile->text()).toString());
settings.endGroup(); // RealSense
@@ -2461,14 +2471,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(settings.value("sensing_mode", _ui->comboBox_stereoZed_sensingMode->currentIndex()).toInt());
_ui->spinBox_stereoZed_confidenceThr->setValue(settings.value("confidence_thr", _ui->spinBox_stereoZed_confidenceThr->value()).toInt());
_ui->spinBox_stereoZed_texturenessConfidenceThr->setValue(settings.value("textureness_confidence_thr", _ui->spinBox_stereoZed_texturenessConfidenceThr->value()).toInt());
_ui->checkbox_stereoZed_odom->setChecked(settings.value("odom", _ui->checkbox_stereoZed_odom->isChecked()).toBool());
_ui->lineEdit_zedSvoPath->setText(settings.value("svo_path", _ui->lineEdit_zedSvoPath->text()).toString());
settings.endGroup(); // StereoZed
settings.beginGroup("StereoRealSense2");
_ui->checkbox_stereoRealSense2_odom->setChecked(settings.value("odom", _ui->checkbox_stereoRealSense2_odom->isChecked()).toBool());
settings.endGroup(); // StereoRealSense2
settings.beginGroup("MyntEye");
_ui->checkbox_stereoMyntEye_rectify->setChecked(settings.value("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked()).toBool());
_ui->checkbox_stereoMyntEye_depth->setChecked(settings.value("depth", _ui->checkbox_stereoMyntEye_depth->isChecked()).toBool());
@@ -2509,6 +2514,16 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->spinBox_cameraImages_max_imu_rate->setValue(settings.value("imu_rate", _ui->spinBox_cameraImages_max_imu_rate->value()).toInt());
settings.endGroup(); // images
settings.beginGroup("OdomSensor");
_ui->comboBox_odom_sensor->setCurrentIndex(settings.value("odom_sensor", _ui->comboBox_odom_sensor->currentIndex()).toInt());
_ui->lineEdit_odom_sensor_extrinsics->setText(settings.value("odom_sensor_extrinsics", _ui->lineEdit_odom_sensor_extrinsics->text()).toString());
_ui->lineEdit_odom_sensor_path_calibration->setText(settings.value("odom_sensor_calibration_path", _ui->lineEdit_odom_sensor_path_calibration->text()).toString());
_ui->lineEdit_odomSourceDevice->setText(settings.value("odom_sensor_device", _ui->lineEdit_odomSourceDevice->text()).toString());
_ui->doubleSpinBox_odom_sensor_time_offset->setValue(settings.value("odom_sensor_offset_time", _ui->doubleSpinBox_odom_sensor_time_offset->value()).toDouble());
_ui->doubleSpinBox_odom_sensor_scale_factor->setValue(settings.value("odom_sensor_scale_factor", _ui->doubleSpinBox_odom_sensor_scale_factor->value()).toDouble());
_ui->checkBox_odom_sensor_use_as_gt->setChecked(settings.value("odom_sensor_odom_as_gt", _ui->checkBox_odom_sensor_use_as_gt->isChecked()).toBool());
settings.endGroup(); // OdomSensor
settings.beginGroup("UsbCam");
_ui->spinBox_usbcam_streamWidth->setValue(settings.value("width", _ui->spinBox_usbcam_streamWidth->value()).toInt());
_ui->spinBox_usbcam_streamHeight->setValue(settings.value("height", _ui->spinBox_usbcam_streamHeight->value()).toInt());
@@ -2934,8 +2949,6 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("height_depth", _ui->spinBox_rs2_height_depth->value());
settings.setValue("rate_depth", _ui->spinBox_rs2_rate_depth->value());
settings.setValue("global_time_sync", _ui->checkbox_rs2_globalTimeStync->isChecked());
settings.setValue("dual_mode", _ui->checkbox_rs2_dualMode->isChecked());
settings.setValue("dual_mode_extrinsics", _ui->lineEdit_rs2_dualModeExtrinsics->text());
settings.setValue("json_preset", _ui->lineEdit_rs2_jsonFile->text());
settings.endGroup(); // RealSense2
@@ -2970,14 +2983,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("sensing_mode", _ui->comboBox_stereoZed_sensingMode->currentIndex());
settings.setValue("confidence_thr", _ui->spinBox_stereoZed_confidenceThr->value());
settings.setValue("textureness_confidence_thr", _ui->spinBox_stereoZed_texturenessConfidenceThr->value());
settings.setValue("odom", _ui->checkbox_stereoZed_odom->isChecked());
settings.setValue("svo_path", _ui->lineEdit_zedSvoPath->text());
settings.endGroup(); // StereoZed
settings.beginGroup("StereoRealSense2");
settings.setValue("odom", _ui->checkbox_stereoRealSense2_odom->isChecked());
settings.endGroup(); // StereoRealSense2
settings.beginGroup("MyntEye");
settings.setValue("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked());
settings.setValue("depth", _ui->checkbox_stereoMyntEye_depth->isChecked());
@@ -3016,6 +3024,16 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("imu_rate", _ui->spinBox_cameraImages_max_imu_rate->value());
settings.endGroup(); // images
settings.beginGroup("OdomSensor");
settings.setValue("odom_sensor", _ui->comboBox_odom_sensor->currentIndex());
settings.setValue("odom_sensor_extrinsics", _ui->lineEdit_odom_sensor_extrinsics->text());
settings.setValue("odom_sensor_calibration_path", _ui->lineEdit_odom_sensor_path_calibration->text());
settings.setValue("odom_sensor_device", _ui->lineEdit_odomSourceDevice->text());
settings.setValue("odom_sensor_offset_time", _ui->doubleSpinBox_odom_sensor_time_offset->value());
settings.setValue("odom_sensor_scale_factor", _ui->doubleSpinBox_odom_sensor_scale_factor->value());
settings.setValue("odom_sensor_odom_as_gt", _ui->checkBox_odom_sensor_use_as_gt->isChecked());
settings.endGroup(); // OdomSensor
settings.beginGroup("UsbCam");
settings.setValue("width", _ui->spinBox_usbcam_streamWidth->value());
settings.setValue("height", _ui->spinBox_usbcam_streamHeight->value());
@@ -3994,6 +4012,24 @@ void PreferencesDialog::selectCalibrationPath()
}
}
void PreferencesDialog::selectOdomSensorCalibrationPath()
{
QString dir = _ui->lineEdit_odom_sensor_path_calibration->text();
if(dir.isEmpty())
{
dir = getWorkingDirectory()+"/camera_info";
}
else if(!dir.contains('.'))
{
dir = getWorkingDirectory()+"/camera_info/"+dir;
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Calibration file (*.yaml)"));
if(path.size())
{
_ui->lineEdit_odom_sensor_path_calibration->setText(path);
}
}
void PreferencesDialog::selectSourceImagesStamps()
{
QString dir = _ui->lineEdit_cameraImages_timestamps->text();
@@ -5084,7 +5120,6 @@ void PreferencesDialog::updateSourceGrpVisibility()
_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo ||
_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo ||
_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoUsb - kSrcStereo ||
_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo ||
_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo ||
_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo ||
_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoDepthAI - kSrcStereo));
@@ -5092,7 +5127,6 @@ void PreferencesDialog::updateSourceGrpVisibility()
_ui->groupBox_cameraStereoVideo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo - kSrcStereo);
_ui->groupBox_cameraStereoUsb->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoUsb - kSrcStereo);
_ui->groupBox_cameraStereoZed->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo);
_ui->groupBox_cameraStereoRealSense2->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo);
_ui->groupBox_cameraMyntEye->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo);
_ui->groupBox_cameraStereoZedOC->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo);
_ui->groupBox_cameraDepthAI->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoDepthAI - kSrcStereo);
@@ -5226,6 +5260,14 @@ bool PreferencesDialog::isOdomDisabled() const
{
return _ui->checkbox_odomDisabled->isChecked();
}
bool PreferencesDialog::isOdomSensorAsGt() const
{
return _ui->checkBox_odom_sensor_use_as_gt->isChecked();
}
double PreferencesDialog::getOdomSensorScaleFactor() const
{
return _ui->doubleSpinBox_odom_sensor_scale_factor->value();
}
int PreferencesDialog::getOdomRegistrationApproach() const
{
return _ui->odom_registration->currentIndex();
@@ -5635,6 +5677,25 @@ QString PreferencesDialog::getSourceDevice() const
return _ui->lineEdit_sourceDevice->text();
}
PreferencesDialog::Src PreferencesDialog::getOdomSourceDriver() const
{
if(_ui->comboBox_odom_sensor->currentIndex() == 1)
{
//RealSense2
return kSrcStereoRealSense2;
}
else if(_ui->comboBox_odom_sensor->currentIndex() == 2)
{
//Zed SDK
return kSrcStereoZed;
}
else if(_ui->comboBox_odom_sensor->currentIndex() != 0)
{
UERROR("Not implemented!");
}
return kSrcUndef;
}
Transform PreferencesDialog::getSourceLocalTransform() const
{
Transform t = Transform::fromString(_ui->lineEdit_sourceLocalTransform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
@@ -5756,8 +5817,34 @@ double PreferencesDialog::getSourceScanForceGroundNormalsUp() const
Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
{
return createCamera(
this->getSourceDriver(),
_ui->lineEdit_sourceDevice->text(),
_ui->lineEdit_calibrationFile->text(),
(this->getSourceDriver()>=kSrcStereo &&
this->getSourceDriver()<kSrcRGB &&
!_ui->checkBox_stereo_rectify->isChecked()) ||
useRawImages,
useColor,
false);
}
Camera * PreferencesDialog::createCamera(
Src driver,
const QString & device,
const QString & calibrationPath,
bool useRawImages,
bool useColor,
bool odomOnly)
{
if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed))
{
QMessageBox::warning(this, tr("Odometry Sensor"),
tr("Driver \%1 cannot support odometry only mode.").arg(driver), QMessageBox::Ok);
return 0;
}
UDEBUG("");
Src driver = this->getSourceDriver();
Camera * camera = 0;
if(driver == PreferencesDialog::kSrcOpenNI_PCL)
{
@@ -5771,7 +5858,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else
{
camera = new CameraOpenni(
_ui->lineEdit_openniOniPath->text().isEmpty()?this->getSourceDevice().toStdString():_ui->lineEdit_openniOniPath->text().toStdString(),
_ui->lineEdit_openniOniPath->text().isEmpty()?device.toStdString():_ui->lineEdit_openniOniPath->text().toStdString(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -5779,7 +5866,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else if(driver == PreferencesDialog::kSrcOpenNI2)
{
camera = new CameraOpenNI2(
_ui->lineEdit_openni2OniPath->text().isEmpty()?this->getSourceDevice().toStdString():_ui->lineEdit_openni2OniPath->text().toStdString(),
_ui->lineEdit_openni2OniPath->text().isEmpty()?device.toStdString():_ui->lineEdit_openni2OniPath->text().toStdString(),
useColor?CameraOpenNI2::kTypeColorDepth:CameraOpenNI2::kTypeIRDepth,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
@@ -5787,7 +5874,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else if(driver == PreferencesDialog::kSrcFreenect)
{
camera = new CameraFreenect(
this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
device.isEmpty()?0:atoi(device.toStdString().c_str()),
useColor?CameraFreenect::kTypeColorDepth:CameraFreenect::kTypeIRDepth,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
@@ -5813,7 +5900,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else if(driver == kSrcFreenect2)
{
camera = new CameraFreenect2(
this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
device.isEmpty()?0:atoi(device.toStdString().c_str()),
useRawImages?CameraFreenect2::kTypeColorIR:(CameraFreenect2::Type)_ui->comboBox_freenect2Format->currentIndex(),
this->getGeneralInputRate(),
this->getSourceLocalTransform(),
@@ -5827,7 +5914,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else if (driver == kSrcK4W2)
{
camera = new CameraK4W2(
this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
device.isEmpty() ? 0 : atoi(device.toStdString().c_str()),
(CameraK4W2::Type)_ui->comboBox_k4w2Format->currentIndex(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
@@ -5845,7 +5932,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else
{
camera = new CameraK4A(
this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
device.isEmpty() ? 0 : atoi(device.toStdString().c_str()),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -5867,7 +5954,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else
{
camera = new CameraRealSense(
this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
device.isEmpty() ? 0 : atoi(device.toStdString().c_str()),
_ui->comboBox_realsensePresetRGB->currentIndex(),
_ui->comboBox_realsensePresetDepth->currentIndex(),
_ui->checkbox_realsenseOdom->isChecked(),
@@ -5889,14 +5976,14 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else
{
camera = new CameraRealSense2(
this->getSourceDevice().toStdString(),
device.toStdString(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraRealSense2*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
if(driver == kSrcStereoRealSense2)
{
((CameraRealSense2*)camera)->setImagesRectified(_ui->checkBox_stereo_rectify->isChecked() && !useRawImages);
((CameraRealSense2*)camera)->setOdomProvided(_ui->checkbox_stereoRealSense2_odom->isChecked());
((CameraRealSense2*)camera)->setImagesRectified(!useRawImages);
((CameraRealSense2*)camera)->setOdomProvided(_ui->comboBox_odom_sensor->currentIndex() == 1 || odomOnly, odomOnly);
}
else
{
@@ -5905,7 +5992,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
((CameraRealSense2*)camera)->setResolution(_ui->spinBox_rs2_width->value(), _ui->spinBox_rs2_height->value(), _ui->spinBox_rs2_rate->value());
((CameraRealSense2*)camera)->setDepthResolution(_ui->spinBox_rs2_width_depth->value(), _ui->spinBox_rs2_height_depth->value(), _ui->spinBox_rs2_rate_depth->value());
((CameraRealSense2*)camera)->setGlobalTimeSync(_ui->checkbox_rs2_globalTimeStync->isChecked());
((CameraRealSense2*)camera)->setDualMode(_ui->checkbox_rs2_dualMode->isChecked(), Transform::fromString(_ui->lineEdit_rs2_dualModeExtrinsics->text().toStdString()));
((CameraRealSense2*)camera)->setDualMode(_ui->comboBox_odom_sensor->currentIndex()==1, Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().toStdString()));
((CameraRealSense2*)camera)->setJsonConfig(_ui->lineEdit_rs2_jsonFile->text().toStdString());
}
}
@@ -5922,7 +6009,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else
{
camera = new CameraMyntEye(
this->getSourceDevice().toStdString(),
device.toStdString(),
_ui->checkbox_stereoMyntEye_rectify->isChecked(),
_ui->checkbox_stereoMyntEye_depth->isChecked(),
this->getGeneralInputRate(),
@@ -5994,7 +6081,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
camera = new CameraStereoImages(
_ui->lineEdit_cameraStereoImages_path_left->text().append(QDir::separator()).toStdString(),
_ui->lineEdit_cameraStereoImages_path_right->text().append(QDir::separator()).toStdString(),
_ui->checkBox_stereo_rectify->isChecked() && !useRawImages,
!useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraStereoImages*)camera)->setStartIndex(_ui->spinBox_cameraStereoImages_startIndex->value());
@@ -6019,17 +6106,17 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
if(_ui->spinBox_stereo_right_device->value()>=0)
{
camera = new CameraStereoVideo(
this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
device.isEmpty() ? 0 : atoi(device.toStdString().c_str()),
_ui->spinBox_stereo_right_device->value(),
_ui->checkBox_stereo_rectify->isChecked() && !useRawImages,
!useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
else
{
camera = new CameraStereoVideo(
this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
_ui->checkBox_stereo_rectify->isChecked() && !useRawImages,
device.isEmpty() ? 0 : atoi(device.toStdString().c_str()),
!useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6043,7 +6130,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
camera = new CameraStereoVideo(
_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
_ui->lineEdit_cameraStereoVideo_path_2->text().toStdString(),
_ui->checkBox_stereo_rectify->isChecked() && !useRawImages,
!useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6052,7 +6139,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
// side-by-side video
camera = new CameraStereoVideo(
_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
_ui->checkBox_stereo_rectify->isChecked() && !useRawImages,
!useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6062,8 +6149,8 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
{
camera = new CameraStereoTara(
this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
_ui->checkBox_stereo_rectify->isChecked() && !useRawImages,
device.isEmpty()?0:atoi(device.toStdString().c_str()),
!useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
@@ -6078,7 +6165,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
_ui->comboBox_stereoZed_quality->currentIndex(),
_ui->comboBox_stereoZed_sensingMode->currentIndex(),
_ui->spinBox_stereoZed_confidenceThr->value(),
_ui->checkbox_stereoZed_odom->isChecked(),
_ui->comboBox_odom_sensor->currentIndex() == 2,
this->getGeneralInputRate(),
this->getSourceLocalTransform(),
_ui->checkbox_stereoZed_selfCalibration->isChecked(),
@@ -6088,12 +6175,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else
{
camera = new CameraStereoZed(
this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
device.isEmpty()?0:atoi(device.toStdString().c_str()),
_ui->comboBox_stereoZed_resolution->currentIndex(),
_ui->comboBox_stereoZed_quality->currentIndex(),
// depth should be enabled for zed vo to work
_ui->comboBox_stereoZed_quality->currentIndex()==0&&odomOnly?1:_ui->comboBox_stereoZed_quality->currentIndex(),
_ui->comboBox_stereoZed_sensingMode->currentIndex(),
_ui->spinBox_stereoZed_confidenceThr->value(),
_ui->checkbox_stereoZed_odom->isChecked(),
_ui->comboBox_odom_sensor->currentIndex() == 2 || odomOnly,
this->getGeneralInputRate(),
this->getSourceLocalTransform(),
_ui->checkbox_stereoZed_selfCalibration->isChecked(),
@@ -6106,7 +6194,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
{
UDEBUG("ZEDOC");
camera = new CameraStereoZedOC(
this->getSourceDevice().isEmpty()?-1:atoi(this->getSourceDevice().toStdString().c_str()),
device.isEmpty()?-1:atoi(device.toStdString().c_str()),
_ui->comboBox_stereoZedOC_resolution->currentIndex(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
@@ -6115,7 +6203,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
{
UDEBUG("DepthAI");
camera = new CameraDepthAI(
this->getSourceDevice().toStdString().c_str(),
device.toStdString().c_str(),
_ui->comboBox_depthai_resolution->currentIndex(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
@@ -6124,8 +6212,8 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
else if(driver == kSrcUsbDevice)
{
camera = new CameraVideo(
this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
_ui->checkBox_rgb_rectify->isChecked() && !useRawImages,
device.isEmpty()?0:atoi(device.toStdString().c_str()),
!useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraVideo*)camera)->setResolution(_ui->spinBox_usbcam_streamWidth->value(), _ui->spinBox_usbcam_streamHeight->value());
@@ -6134,7 +6222,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
{
camera = new CameraVideo(
_ui->source_video_lineEdit_path->text().toStdString(),
_ui->checkBox_rgb_rectify->isChecked() && !useRawImages,
!useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6147,7 +6235,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
((CameraImages*)camera)->setStartIndex(_ui->source_images_spinBox_startPos->value());
((CameraImages*)camera)->setMaxFrames(_ui->source_images_spinBox_maxFrames->value());
((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgb_rectify->isChecked() && !useRawImages);
((CameraImages*)camera)->setImagesRectified(!useRawImages);
((CameraImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
((CameraImages*)camera)->setOdometryPath(
@@ -6191,16 +6279,17 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
{
UDEBUG("Init");
QString dir = this->getCameraInfoDir();
QString calibrationFile = _ui->lineEdit_calibrationFile->text();
QString calibrationFile = calibrationPath;
if(!(driver >= kSrcRGB && driver <= kSrcVideo))
{
calibrationFile.remove("_left.yaml").remove("_right.yaml").remove("_pose.yaml").remove("_rgb.yaml").remove("_depth.yaml");
}
QString name = QFileInfo(calibrationFile.remove(".yaml")).baseName();
if(!_ui->lineEdit_calibrationFile->text().isEmpty())
if(!calibrationPath.isEmpty())
{
QDir d = QFileInfo(_ui->lineEdit_calibrationFile->text()).dir();
if(!_ui->lineEdit_calibrationFile->text().remove(QFileInfo(_ui->lineEdit_calibrationFile->text()).baseName()).isEmpty())
QDir d = QFileInfo(calibrationPath).dir();
QString tmp = calibrationPath;
if(!tmp.remove(QFileInfo(calibrationPath).baseName()).isEmpty())
{
dir = d.absolutePath();
}
@@ -6240,6 +6329,30 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
return camera;
}
Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & timeOffset)
{
Src driver = getOdomSourceDriver();
if(driver != kSrcUndef)
{
if(driver == getSourceDriver() &&
_ui->lineEdit_odomSourceDevice->text().compare(_ui->lineEdit_sourceDevice->text()) == 0)
{
QMessageBox::warning(this, tr("Odometry Sensor"),
tr("Cannot create an odometry sensor with same driver than camera AND with same "
"device. Change device ID of the odometry or camera sensor. To use odometry option "
"from a single camera, look at the parameters of that driver to enable it, "
"then disable odometry sensor. "), QMessageBox::Ok);
return 0;
}
extrinsics = Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
timeOffset = _ui->doubleSpinBox_odom_sensor_time_offset->value()/1000.0;
return createCamera(driver, _ui->lineEdit_odomSourceDevice->text(), _ui->lineEdit_odom_sensor_path_calibration->text(), false, true, true);
}
return 0;
}
bool PreferencesDialog::isStatisticsPublished() const
{
return _ui->groupBox_publishing->isChecked();
@@ -6619,7 +6732,7 @@ void PreferencesDialog::calibrate()
button = QMessageBox::question(this, tr("Calibration"),
tr("We will now calibrate the IR camera. Hide the IR projector with a Post-It and "
"make sure you have enough ambient IR light (e.g., external IR source or sunlight!) to see the "
"checkboard with the IR camera. Do you want to continue?"),
"checkerboard with the IR camera. Do you want to continue?"),
QMessageBox::Yes | QMessageBox::No | QMessageBox::Ignore, QMessageBox::Yes);
if(button == QMessageBox::Yes || button == QMessageBox::Ignore)
{
@@ -6646,10 +6759,10 @@ void PreferencesDialog::calibrate()
button = QMessageBox::question(this, tr("Calibration"),
tr("We will now calibrate the extrinsics. Important: Make sure "
"the cameras and the checkboard don't move and that both "
"cameras can see the checkboard. We will repeat this "
"the cameras and the checkerboard don't move and that both "
"cameras can see the checkerboard. We will repeat this "
"multiple times. Each time, you will have to move the camera (or "
"checkboard) for a different point of view. Do you want to "
"checkerboard) for a different point of view. Do you want to "
"continue?"),
QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
@@ -6657,7 +6770,7 @@ void PreferencesDialog::calibrate()
int totalSamples = 0;
if(button == QMessageBox::Yes)
{
totalSamples = QInputDialog::getInt(this, tr("Calibration"), tr("Samples: "), 3, 1, 99, 1, &ok);
totalSamples = QInputDialog::getInt(this, tr("Calibration"), tr("Samples: "), 1, 1, 99, 1, &ok);
}
if(ok)
@@ -6705,7 +6818,7 @@ void PreferencesDialog::calibrate()
if(count < totalSamples)
{
button = QMessageBox::question(this, tr("Calibration"),
tr("A stereo pair has been taken (total=%1/%2). Move the checkboard or "
tr("A stereo pair has been taken (total=%1/%2). Move the checkerboard or "
"camera to another position. Press \"Yes\" when you are ready "
"for the next capture.").arg(count).arg(totalSamples),
QMessageBox::Yes | QMessageBox::Abort, QMessageBox::Yes);
@@ -6714,7 +6827,7 @@ void PreferencesDialog::calibrate()
else
{
button = QMessageBox::question(this, tr("Calibration"),
tr("Could not detect the checkboard on both images or "
tr("Could not detect the checkerboard on both images or "
"the point of view didn't change enough. Try again?"),
QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
}
@@ -6758,6 +6871,7 @@ void PreferencesDialog::calibrate()
bool freenect2 = driver == kSrcFreenect2;
bool fisheye = driver == kSrcStereoRealSense2;
_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB && driver != kSrcRealSense, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
_calibrationDialog->setCameraName("");
_calibrationDialog->setSwitchedImages(freenect2);
_calibrationDialog->setFisheyeImages(fisheye);
_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
@@ -6784,5 +6898,245 @@ void PreferencesDialog::calibrateSimple()
}
}
void PreferencesDialog::calibrateOdomSensorExtrinsics()
{
if(this->getSourceType() == kSrcDatabase)
{
QMessageBox::warning(this,
tr("Calibration"),
tr("Cannot calibrate database source!"));
return;
}
if(!this->getCameraInfoDir().isEmpty())
{
QDir dir(this->getCameraInfoDir());
if (!dir.exists())
{
UINFO("Creating camera_info directory: \"%s\"", this->getCameraInfoDir().toStdString().c_str());
if(!dir.mkpath(this->getCameraInfoDir()))
{
UWARN("Could create camera_info directory: \"%s\"", this->getCameraInfoDir().toStdString().c_str());
}
}
}
Src odomDriver;
if(_ui->comboBox_odom_sensor->currentIndex() == 0)
{
QMessageBox::warning(this,
tr("Calibration"),
tr("No odometry sensor selected!"));
return;
}
else if(_ui->comboBox_odom_sensor->currentIndex() == 1)
{
odomDriver = kSrcStereoRealSense2;
}
else if(_ui->comboBox_odom_sensor->currentIndex() == 2)
{
odomDriver = kSrcStereoZed;
}
else
{
UERROR("Odom sensor %d not implemented", _ui->comboBox_odom_sensor->currentIndex());
return;
}
// 3 steps calibration: RGB -> IR -> Extrinsic
QMessageBox::StandardButton button = QMessageBox::question(this, tr("Calibration"),
tr("We will calibrate the extrinsics. Important: Make sure "
"the cameras and the checkerboard don't move and that both "
"cameras can see the checkerboard. We will repeat this "
"multiple times. Each time, you will have to move the camera (or "
"checkerboard) for a different point of view. Do you want to "
"continue?"),
QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
if(button == QMessageBox::Yes)
{
_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
bool ok = false;
int totalSamples = 0;
if(button == QMessageBox::Yes)
{
QDialog dialog(this);
QFormLayout form(&dialog);
// Add the lineEdits with their respective labels
QSpinBox * samples = new QSpinBox(&dialog);
samples->setMinimum(1);
samples->setMaximum(99);
samples->setValue(1);
QSpinBox * boardWidth = new QSpinBox(&dialog);
boardWidth->setMinimum(2);
boardWidth->setMaximum(99);
boardWidth->setValue(_calibrationDialog->boardWidth());
QSpinBox * boardHeight = new QSpinBox(&dialog);
boardHeight->setMinimum(2);
boardHeight->setMaximum(99);
boardHeight->setValue(_calibrationDialog->boardHeight());
QDoubleSpinBox * squareSize = new QDoubleSpinBox(&dialog);
squareSize->setDecimals(4);
squareSize->setMinimum(0.0001);
squareSize->setMaximum(9);
squareSize->setValue(_calibrationDialog->squareSize());
squareSize->setSuffix(" m");
form.addRow("Samples: ", samples);
form.addRow("Checkerboard Width: ", boardWidth);
form.addRow("Checkerboard Height: ", boardHeight);
form.addRow("Checkerboard Square Size: ", squareSize);
// Add some standard buttons (Cancel/Ok) at the bottom of the dialog
QDialogButtonBox buttonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel,
Qt::Horizontal, &dialog);
form.addRow(&buttonBox);
QObject::connect(&buttonBox, SIGNAL(accepted()), &dialog, SLOT(accept()));
QObject::connect(&buttonBox, SIGNAL(rejected()), &dialog, SLOT(reject()));
// Show the dialog as modal
if (dialog.exec() == QDialog::Accepted) {
_calibrationDialog->setBoardWidth(boardWidth->value());
_calibrationDialog->setBoardHeight(boardHeight->value());
_calibrationDialog->setSquareSize(squareSize->value());
totalSamples = samples->value();
ok = true;
}
}
if(ok)
{
int count = 0;
_calibrationDialog->setStereoMode(true, "camera", "odom_sensor"); // this forces restart
_calibrationDialog->setCameraName("");
_calibrationDialog->setModal(true);
_calibrationDialog->setProgressVisibility(false);
_calibrationDialog->show();
CameraModel cameraModel;
CameraModel odomSensorModel;
std::string serial;
for(;count < totalSamples && button == QMessageBox::Yes; )
{
// Step 3: Extrinsics
Camera * camera = this->createCamera(
odomDriver,
_ui->lineEdit_odomSourceDevice->text(),
_ui->lineEdit_odom_sensor_path_calibration->text(),
false, true, false); // Odom sensor
if(!camera)
{
return;
}
else if(!camera->isCalibrated())
{
QMessageBox::warning(_calibrationDialog, tr("Calibration"),
tr("Odom sensor is not calibrated. Camera and odometry sensor should be individually calibrated (intrinsics) before calibrating the extrinsics between them. Aborting..."), QMessageBox::Ok);
delete camera;
return;
}
SensorData odomSensorData = camera->takeImage();
if(odomSensorData.cameraModels().size() == 1) {
odomSensorModel = odomSensorData.cameraModels()[0];
}
else {
odomSensorModel = odomSensorData.stereoCameraModel().left();
}
delete camera;
int currentIndex = _ui->comboBox_odom_sensor->currentIndex();
_ui->comboBox_odom_sensor->setCurrentIndex(0);
camera = this->createCamera(false, true); // Camera
_ui->comboBox_odom_sensor->setCurrentIndex(currentIndex);
if(!camera)
{
return;
}
else if(!camera->isCalibrated())
{
QMessageBox::warning(_calibrationDialog, tr("Calibration"),
tr("Odom sensor is not calibrated. Camera and odometry sensor should be individually calibrated (intrinsics) before calibrating the extrinsics between them. Aborting..."), QMessageBox::Ok);
delete camera;
return;
}
SensorData camData = camera->takeImage();
serial = camera->getSerial();
if(camData.cameraModels().size() == 1) {
cameraModel = camData.cameraModels()[0];
}
else {
cameraModel = camData.stereoCameraModel().left();
}
delete camera;
if(!odomSensorData.imageRaw().empty() && !camData.imageRaw().empty())
{
int pair = _calibrationDialog->getStereoPairs();
_calibrationDialog->processImages(camData.imageRaw(), odomSensorData.imageRaw(), serial.c_str());
if(_calibrationDialog->getStereoPairs() - pair > 0)
{
++count;
if(count < totalSamples)
{
button = QMessageBox::question(_calibrationDialog, tr("Calibration"),
tr("A stereo pair has been taken (total=%1/%2). Move the checkerboard or "
"camera to another position. Press \"Yes\" when you are ready "
"for the next capture.").arg(count).arg(totalSamples),
QMessageBox::Yes | QMessageBox::Abort, QMessageBox::Yes);
}
}
else
{
button = QMessageBox::question(_calibrationDialog, tr("Calibration"),
tr("Could not detect the checkerboard on both images or "
"the point of view didn't change enough. Try again?"),
QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
}
}
else
{
button = QMessageBox::question(_calibrationDialog, tr("Calibration"),
tr("Failed to start the camera. Try again?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
}
}
if(count == totalSamples && button == QMessageBox::Yes)
{
// Assume cameras are already rectified!
cameraModel = CameraModel("camera", cameraModel.imageSize(), cameraModel.K(), cv::Mat::zeros(1,5,CV_64FC1), cv::Mat(), cv::Mat(), cameraModel.localTransform());
odomSensorModel = CameraModel("odom_sensor", odomSensorModel.imageSize(), odomSensorModel.K(), cv::Mat::zeros(1,5,CV_64FC1), cv::Mat(), cv::Mat(), odomSensorModel.localTransform());
StereoCameraModel stereoModel = _calibrationDialog->stereoCalibration(cameraModel, odomSensorModel, true);
stereoModel.setName(stereoModel.name(), "camera", "odom_sensor");
if(stereoModel.stereoTransform().isNull())
{
QMessageBox::warning(_calibrationDialog, tr("Calibration"),
tr("Extrinsic calibration has failed! Look on the terminal "
"for possible error messages."), QMessageBox::Ok);
std::cout << "stereoModel: " << stereoModel << std::endl;
}
else
{
UINFO("Odom sensor local transform (pose to left cam): %s", odomSensorModel.localTransform().prettyPrint().c_str());
UINFO("Extrinsics (odom left cam to camera left cam): %s", stereoModel.stereoTransform().prettyPrint().c_str());
Transform t = odomSensorModel.localTransform() * stereoModel.stereoTransform();
UINFO("Odom sensor frame to camera frame: %s", t.prettyPrint().c_str());
float x,y,z,roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
_ui->lineEdit_odom_sensor_extrinsics->setText(QString("%1 %2 %3 %4 %5 %6")
.arg(x).arg(y).arg(z)
.arg(roll).arg(pitch).arg(yaw));
QMessageBox::information(_calibrationDialog, tr("Calibration"),
tr("Calibration is completed! Extrinsics have been computed: %1. "
"You can close the calibration dialog.").arg(t.prettyPrint().c_str()), QMessageBox::Ok);
}
}
}
}
}
}

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

File diff suppressed because it is too large Load Diff