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

CameraStereoZed: added odomForce3DoF option

Browse Source
This commit is contained in:
matlabbe
2019-01-28 17:05:15 -05:00
parent 0192cac18a
commit 76ef2c4b0e
4 changed files with 22 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraInfo.h
+1
View File
@@ -43,6 +43,7 @@ public:
timeCapture(0.0f), timeCapture(0.0f),
timeDisparity(0.0f), timeDisparity(0.0f),
timeMirroring(0.0f), timeMirroring(0.0f),
timeStereoExposureCompensation(0.0f),
timeImageDecimation(0.0f), timeImageDecimation(0.0f),
timeScanFromDepth(0.0f), timeScanFromDepth(0.0f),
timeUndistortDepth(0.0f), timeUndistortDepth(0.0f),
corelib/include/rtabmap/core/camera/CameraStereoZed.h
+5 -2
View File
@@ -57,7 +57,8 @@ public:
bool computeOdometry = false, bool computeOdometry = false,
float imageRate=0.0f, float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(), const Transform & localTransform = Transform::getIdentity(),
bool selfCalibration = true); bool selfCalibration = true,
bool odomForce3DoF = false);
CameraStereoZed( CameraStereoZed(
const std::string & svoFilePath, const std::string & svoFilePath,
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
@@ -66,7 +67,8 @@ public:
bool computeOdometry = false, bool computeOdometry = false,
float imageRate=0.0f, float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(), const Transform & localTransform = Transform::getIdentity(),
bool selfCalibration = true); bool selfCalibration = true,
bool odomForce3DoF = false);
virtual ~CameraStereoZed(); virtual ~CameraStereoZed();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -91,6 +93,7 @@ private:
int confidenceThr_; int confidenceThr_;
bool computeOdometry_; bool computeOdometry_;
bool lost_; bool lost_;
bool force3DoF_;
#endif #endif
}; };
corelib/src/camera/CameraStereoZed.cpp
+12 -5
View File
@@ -55,7 +55,8 @@ CameraStereoZed::CameraStereoZed(
bool computeOdometry, bool computeOdometry,
float imageRate, float imageRate,
const Transform & localTransform, const Transform & localTransform,
bool selfCalibration) : bool selfCalibration,
bool odomForce3DoF) :
Camera(imageRate, localTransform) Camera(imageRate, localTransform)
#ifdef RTABMAP_ZED #ifdef RTABMAP_ZED
, ,
@@ -69,7 +70,8 @@ CameraStereoZed::CameraStereoZed(
sensingMode_(sensingMode), sensingMode_(sensingMode),
confidenceThr_(confidenceThr), confidenceThr_(confidenceThr),
computeOdometry_(computeOdometry), computeOdometry_(computeOdometry),
lost_(true) lost_(true),
force3DoF_(odomForce3DoF)
#endif #endif
{ {
UDEBUG(""); UDEBUG("");
@@ -89,7 +91,8 @@ CameraStereoZed::CameraStereoZed(
bool computeOdometry, bool computeOdometry,
float imageRate, float imageRate,
const Transform & localTransform, const Transform & localTransform,
bool selfCalibration) : bool selfCalibration,
bool odomForce3DoF) :
Camera(imageRate, localTransform) Camera(imageRate, localTransform)
#ifdef RTABMAP_ZED #ifdef RTABMAP_ZED
, ,
@@ -103,7 +106,8 @@ CameraStereoZed::CameraStereoZed(
sensingMode_(sensingMode), sensingMode_(sensingMode),
confidenceThr_(confidenceThr), confidenceThr_(confidenceThr),
computeOdometry_(computeOdometry), computeOdometry_(computeOdometry),
lost_(true) lost_(true),
force3DoF_(odomForce3DoF)
#endif #endif
{ {
UDEBUG(""); UDEBUG("");
@@ -339,7 +343,10 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
//transform x->forward, y->left, z->up //transform x->forward, y->left, z->up
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse(); info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
if(force3DoF_)
{
info->odomPose = info->odomPose.to3DoF();
}
if (lost_) if (lost_)
{ {
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
guilib/src/PreferencesDialog.cpp
+4 -2
View File
@@ -5233,7 +5233,8 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
_ui->checkbox_stereoZed_odom->isChecked(), _ui->checkbox_stereoZed_odom->isChecked(),
this->getGeneralInputRate(), this->getGeneralInputRate(),
this->getSourceLocalTransform(), this->getSourceLocalTransform(),
_ui->checkbox_stereoZed_selfCalibration->isChecked()); _ui->checkbox_stereoZed_selfCalibration->isChecked(),
_ui->loopClosure_bowForce2D->isChecked());
} }
else else
{ {
@@ -5246,7 +5247,8 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
_ui->checkbox_stereoZed_odom->isChecked(), _ui->checkbox_stereoZed_odom->isChecked(),
this->getGeneralInputRate(), this->getGeneralInputRate(),
this->getSourceLocalTransform(), this->getSourceLocalTransform(),
_ui->checkbox_stereoZed_selfCalibration->isChecked()); _ui->checkbox_stereoZed_selfCalibration->isChecked(),
_ui->loopClosure_bowForce2D->isChecked());
} }
} }
else if(driver == kSrcUsbDevice) else if(driver == kSrcUsbDevice)