i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

1
corelib/include/rtabmap/core/CameraInfo.h
View File

@@ -43,6 +43,7 @@ public:
timeCapture(0.0f),
timeDisparity(0.0f),
timeMirroring(0.0f),
timeStereoExposureCompensation(0.0f),
timeImageDecimation(0.0f),
timeScanFromDepth(0.0f),
timeUndistortDepth(0.0f),

7
corelib/include/rtabmap/core/camera/CameraStereoZed.h
View File

@@ -57,7 +57,8 @@ public:
bool computeOdometry = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
bool selfCalibration = true);
bool selfCalibration = true,
bool odomForce3DoF = false);
CameraStereoZed(
const std::string & svoFilePath,
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
@@ -66,7 +67,8 @@ public:
bool computeOdometry = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
bool selfCalibration = true);
bool selfCalibration = true,
bool odomForce3DoF = false);
virtual ~CameraStereoZed();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -91,6 +93,7 @@ private:
int confidenceThr_;
bool computeOdometry_;
bool lost_;
bool force3DoF_;
#endif
};

17
corelib/src/camera/CameraStereoZed.cpp
View File

@@ -55,7 +55,8 @@ CameraStereoZed::CameraStereoZed(
bool computeOdometry,
float imageRate,
const Transform & localTransform,
bool selfCalibration) :
bool selfCalibration,
bool odomForce3DoF) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_ZED
,
@@ -69,7 +70,8 @@ CameraStereoZed::CameraStereoZed(
sensingMode_(sensingMode),
confidenceThr_(confidenceThr),
computeOdometry_(computeOdometry),
lost_(true)
lost_(true),
force3DoF_(odomForce3DoF)
#endif
{
UDEBUG("");
@@ -89,7 +91,8 @@ CameraStereoZed::CameraStereoZed(
bool computeOdometry,
float imageRate,
const Transform & localTransform,
bool selfCalibration) :
bool selfCalibration,
bool odomForce3DoF) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_ZED
,
@@ -103,7 +106,8 @@ CameraStereoZed::CameraStereoZed(
sensingMode_(sensingMode),
confidenceThr_(confidenceThr),
computeOdometry_(computeOdometry),
lost_(true)
lost_(true),
force3DoF_(odomForce3DoF)
#endif
{
UDEBUG("");
@@ -339,7 +343,10 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
//transform x->forward, y->left, z->up
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
if(force3DoF_)
{
info->odomPose = info->odomPose.to3DoF();
}
if (lost_)
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose