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

Added imu to odom bundle adjustment. Added IMUFilter classes. Changed Aruco parameter prefix to Marker. Zed: publishing IMU data.

Browse Source
This commit is contained in:
matlabbe
2019-05-07 18:57:53 -04:00
parent 4675240d6e
commit e6f471d88e
32 changed files with 1733 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/src/CMakeLists.txt
View File

@@ -88,6 +88,8 @@ SET(SRC_FILES
odometry/OdometryVINS.cpp
IMUThread.cpp
IMUFilter.cpp
imufilter/ComplementaryFilter.cpp
Stereo.cpp
StereoDense.cpp
@@ -470,7 +472,7 @@ IF(GTSAM_FOUND)
)
ENDIF()
SET(SRC_FILES
${SRC_FILES}
${SRC_FILES}
optimizer/gtsam/GravityFactor.cpp
)
IF(WIN32)
@@ -483,6 +485,13 @@ IF(GTSAM_FOUND)
)
ENDIF(GTSAM_FOUND)
IF(WITH_MADGWICK)
SET(SRC_FILES
${SRC_FILES}
imufilter/MadgwickFilter.cpp
)
ENDIF(WITH_MADGWICK)
####################################
# Generate resources files
####################################

88
corelib/src/IMUFilter.cpp Normal file
View File

@@ -0,0 +1,88 @@
/*
Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/IMUFilter.h>
#include <rtabmap/utilite/ULogger.h>
#include "imufilter/ComplementaryFilter.h"
#ifdef RTABMAP_MADGWICK
#include "imufilter/MadgwickFilter.h"
#endif
namespace rtabmap {
IMUFilter * IMUFilter::create(const ParametersMap & parameters)
{
int type = Parameters::defaultKpDetectorStrategy();
Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
return create((IMUFilter::Type)type, parameters);
}
IMUFilter * IMUFilter::create(IMUFilter::Type type, const ParametersMap & parameters)
{
#ifndef RTABMAP_MADGWICK
if(type == IMUFilter::kMadgwick)
{
UWARN("Madgwick filter cannot be used as RTAB-Map is not built with the option enabled. Complementary filter is used instead.");
type = IMUFilter::kComplementaryFilter;
}
#endif
IMUFilter * filter = 0;
switch(type)
{
#ifdef RTABMAP_MADGWICK
case IMUFilter::kMadgwick:
filter = new MadgwickFilter(parameters);
break;
#endif
default:
filter = new ComplementaryFilter(parameters);
type = IMUFilter::kComplementaryFilter;
break;
}
return filter;
}
void IMUFilter::update(
double gx, double gy, double gz,
double ax, double ay, double az,
double stamp)
{
if(previousStamp_ == 0.0)
{
previousStamp_ = stamp;
}
double dt = stamp - previousStamp_;
updateImpl(gx, gy, gz, ax, ay, az, dt);
previousStamp_ = stamp;
}
}

36
corelib/src/MarkerDetector.cpp
View File

@@ -34,18 +34,18 @@ namespace rtabmap {
MarkerDetector::MarkerDetector(const ParametersMap & parameters)
{
#ifdef HAVE_OPENCV_ARUCO
markerLength_ = Parameters::defaultArucoMarkerLength();
maxDepthError_ = Parameters::defaultArucoMaxDepthError();
dictionaryId_ = Parameters::defaultArucoDictionary();
markerLength_ = Parameters::defaultMarkerLength();
maxDepthError_ = Parameters::defaultMarkerMaxDepthError();
dictionaryId_ = Parameters::defaultMarkerDictionary();
#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
detectorParams_ = cv::aruco::DetectorParameters::create();
#else
detectorParams_.reset(new cv::aruco::DetectorParameters());
#endif
#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
detectorParams_->cornerRefinementMethod = Parameters::defaultArucoCornerRefinementMethod();
detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod();
#else
detectorParams_->doCornerRefinement = Parameters::defaultArucoCornerRefinementMethod()!=0;
detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0;
#endif
parseParameters(parameters);
#endif
@@ -69,10 +69,10 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
detectorParams_->minDistanceToBorder = 3;
detectorParams_->minMarkerDistanceRate = 0.05;
#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
Parameters::parse(parameters, Parameters::kArucoCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
#else
int doCornerRefinement = detectorParams_->doCornerRefinement?1:0;
Parameters::parse(parameters, Parameters::kArucoCornerRefinementMethod(), doCornerRefinement);
Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), doCornerRefinement);
detectorParams_->doCornerRefinement = doCornerRefinement!=0;
#endif
detectorParams_->cornerRefinementWinSize = 5;
@@ -85,18 +85,18 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
detectorParams_->minOtsuStdDev = 5.0;
detectorParams_->errorCorrectionRate = 0.6;
Parameters::parse(parameters, Parameters::kArucoMarkerLength(), markerLength_);
Parameters::parse(parameters, Parameters::kArucoMaxDepthError(), maxDepthError_);
Parameters::parse(parameters, Parameters::kArucoDictionary(), dictionaryId_);
Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_);
Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_);
Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_);
#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
if(dictionaryId_ >= 17)
{
UERROR("Cannot set AprilTag dictionary. OpenCV version should be at least 3.4.2, "
"current version is %s. Setting %s to default (%d)",
CV_VERSION,
Parameters::kArucoDictionary().c_str(),
Parameters::defaultArucoDictionary());
dictionaryId_ = Parameters::defaultArucoDictionary();
Parameters::kMarkerDictionary().c_str(),
Parameters::defaultMarkerDictionary());
dictionaryId_ = Parameters::defaultMarkerDictionary();
}
#endif
#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
@@ -133,7 +133,7 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
{
if(depth.empty())
{
UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kArucoMarkerLength().c_str());
UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
return detections;
}
rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?model.imageWidth()/depth.cols:1);
@@ -171,9 +171,9 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
"the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f. "
"Parameter %s can be set to non-null to skip automatic "
"marker length estimation. Detections are ignored.",
fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kArucoMaxDepthError().c_str(),
fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(),
d1, d2, d3, d4,
Parameters::kArucoMarkerLength().c_str());
Parameters::kMarkerLength().c_str());
detections.clear();
return detections;
}
@@ -185,7 +185,7 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
"Parameter %s can be set to non-null to skip automatic "
"marker length estimation. Detections are ignored.",
d1,d2,d3,d4,
Parameters::kArucoMarkerLength().c_str());
Parameters::kMarkerLength().c_str());
detections.clear();
return detections;
}
@@ -217,7 +217,7 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
"Parameter %s can be set to non-null to skip automatic "
"marker length estimation. Detections are ignored.",
ids[i], scales[i], ids[0], scales[0],
Parameters::kArucoMarkerLength().c_str());
Parameters::kMarkerLength().c_str());
detections.clear();
return detections;
}

8
corelib/src/Memory.cpp
View File

@@ -108,8 +108,8 @@ Memory::Memory(const ParametersMap & parameters) :
_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
_detectMarkers(Parameters::defaultRGBDMarkerDetection()),
_markerLinVariance(Parameters::defaultArucoVarianceLinear()),
_markerAngVariance(Parameters::defaultArucoVarianceAngular()),
_markerLinVariance(Parameters::defaultMarkerVarianceLinear()),
_markerAngVariance(Parameters::defaultMarkerVarianceAngular()),
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
@@ -567,8 +567,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
Parameters::parse(params, Parameters::kRGBDMarkerDetection(), _detectMarkers);
Parameters::parse(params, Parameters::kArucoVarianceLinear(), _markerLinVariance);
Parameters::parse(params, Parameters::kArucoVarianceAngular(), _markerAngVariance);
Parameters::parse(params, Parameters::kMarkerVarianceLinear(), _markerLinVariance);
Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance);
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());

44
corelib/src/Odometry.cpp
View File

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/odometry/OdometryMSCKF.h"
#include "rtabmap/core/odometry/OdometryVINS.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/IMUFilter.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_mapping.h"
#include "rtabmap/core/util3d_filtering.h"
@@ -110,6 +111,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_holonomic(Parameters::defaultOdomHolonomic()),
guessFromMotion_(Parameters::defaultOdomGuessMotion()),
guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()),
_imuFilteringStrategy(Parameters::defaultOdomImuFilteringStrategy()),
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
_particleSize(Parameters::defaultOdomParticleSize()),
_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
@@ -127,7 +129,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_resetCurrentCount(0),
previousStamp_(0),
distanceTravelled_(0),
framesProcessed_(0)
framesProcessed_(0),
imuFilter_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
@@ -136,6 +139,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kOdomImuFilteringStrategy(), _imuFilteringStrategy);
Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
@@ -178,6 +182,11 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
{
initKalmanFilter();
}
if(_imuFilteringStrategy > 0)
{
imuFilter_ = IMUFilter::create((IMUFilter::Type)(_imuFilteringStrategy-1), parameters);
}
}
Odometry::~Odometry()
@@ -187,6 +196,7 @@ Odometry::~Odometry()
delete particleFilters_[i];
}
particleFilters_.clear();
delete imuFilter_;
}
void Odometry::reset(const Transform & initialPose)
@@ -241,6 +251,10 @@ void Odometry::reset(const Transform & initialPose)
{
_pose = initialPose;
}
if(imuFilter_)
{
imuFilter_->reset();
}
}
const Transform & Odometry::previousVelocityTransform() const
@@ -353,6 +367,28 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
// Update IMU orientation
if(!data.imu().empty() && imuFilter_ != 0)
{
imuFilter_->update(
data.imu().angularVelocity()[0],
data.imu().angularVelocity()[1],
data.imu().angularVelocity()[2],
data.imu().linearAcceleration()[0],
data.imu().linearAcceleration()[1],
data.imu().linearAcceleration()[2],
data.stamp());
double qx,qy,qz,qw;
imuFilter_->getOrientation(qx,qy,qz,qw);
data.setIMU(IMU(
cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
data.imu().angularVelocity(), data.imu().angularVelocityCovariance(),
data.imu().linearAcceleration(), data.imu().linearAccelerationCovariance(),
data.imu().localTransform()));
}
// KITTI datasets start with stamp=0
double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
@@ -370,6 +406,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
previousVelocities_.clear();
velocityGuess_.setNull();
}
if(!velocityGuess_.isNull())
{
if(guessFromMotion_)
@@ -466,6 +503,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
t = this->computeTransform(data, guess, info);
}
if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
{
return Transform(); // Return null on IMU-only updates
}
if(info)
{
info->timeEstimation = time.ticks();

17
corelib/src/Parameters.cpp
View File

@@ -180,6 +180,9 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
(icp && group.compare("Icp") == 0) ||
(vis && Parameters::isFeatureParameter(iter->first)) ||
group.compare("Reg") == 0 ||
group.compare("Optimizer") == 0 ||
group.compare("g2o") == 0 ||
group.compare("GTSAM") == 0 ||
(vis && group.compare("Vis") == 0) ||
iter->first.compare(kRtabmapPublishRAMUsage())==0)
{
@@ -234,6 +237,14 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.19.3
removedParameters_.insert(std::make_pair("Aruco/Dictionary", std::make_pair(true, Parameters::kMarkerDictionary())));
removedParameters_.insert(std::make_pair("Aruco/MarkerLength", std::make_pair(true, Parameters::kMarkerLength())));
removedParameters_.insert(std::make_pair("Aruco/MaxDepthError", std::make_pair(true, Parameters::kMarkerMaxDepthError())));
removedParameters_.insert(std::make_pair("Aruco/VarianceLinear", std::make_pair(true, Parameters::kMarkerVarianceLinear())));
removedParameters_.insert(std::make_pair("Aruco/VarianceAngular", std::make_pair(true, Parameters::kMarkerVarianceAngular())));
removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerCornerRefinementMethod())));
// 0.17.5
removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr", std::make_pair(true, Parameters::kGridGlobalOccupancyThr())));
@@ -591,6 +602,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With Madgwick:";
#ifdef RTABMAP_MADGWICK
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With TORO:";
#ifdef RTABMAP_TORO

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

@@ -126,6 +126,73 @@ CameraStereoZed::~CameraStereoZed()
#endif
}
#ifdef RTABMAP_ZED
static cv::Mat slMat2cvMat(sl::Mat& input) {
//convert MAT_TYPE to CV_TYPE
int cv_type = -1;
switch (input.getDataType()) {
case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
default: break;
}
// cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
//cv::Mat and sl::Mat will share the same memory pointer
return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
}
Transform zedPoseToTransform(const sl::Pose & pose)
{
return Transform(
pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
}
IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform)
{
sl::Orientation orientation = imuData.pose_data.getOrientation();
//Convert zed imu orientation from camera frame to world frame ENU!
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
Transform orientationT(0,0,0, orientation.ox, orientation.oy, orientation.oz, orientation.ow);
orientationT = opticalTransform * orientationT;
Eigen::Matrix4d opticalTransform4d = opticalTransform.toEigen4d();
Eigen::Vector4d accT = opticalTransform4d * Eigen::Vector4d(imuData.linear_acceleration.v[0], imuData.linear_acceleration.v[1], imuData.linear_acceleration.v[2], 1);
Eigen::Vector4d gyrT = opticalTransform4d * Eigen::Vector4d(imuData.angular_velocity.v[0], imuData.angular_velocity.v[1], imuData.angular_velocity.v[2], 1);
// FIXME covariance should be rotated too: see https://robotics.stackexchange.com/questions/2556/how-to-rotate-covariance
cv::Mat orientationCov = (cv::Mat_<double>(3,3)<<
imuData.pose_covariance[21], imuData.pose_covariance[22], imuData.pose_covariance[23],
imuData.pose_covariance[27], imuData.pose_covariance[28], imuData.pose_covariance[29],
imuData.pose_covariance[33], imuData.pose_covariance[34], imuData.pose_covariance[35]);
cv::Mat angCov = (cv::Mat_<double>(3,3)<<
imuData.angular_velocity_convariance.r[0], imuData.angular_velocity_convariance.r[1], imuData.angular_velocity_convariance.r[2],
imuData.angular_velocity_convariance.r[3], imuData.angular_velocity_convariance.r[4], imuData.angular_velocity_convariance.r[5],
imuData.angular_velocity_convariance.r[6], imuData.angular_velocity_convariance.r[7], imuData.angular_velocity_convariance.r[8]);
cv::Mat accCov = (cv::Mat_<double>(3,3)<<
imuData.linear_acceleration_convariance.r[0], imuData.linear_acceleration_convariance.r[1], imuData.linear_acceleration_convariance.r[2],
imuData.linear_acceleration_convariance.r[3], imuData.linear_acceleration_convariance.r[4], imuData.linear_acceleration_convariance.r[5],
imuData.linear_acceleration_convariance.r[6], imuData.linear_acceleration_convariance.r[7], imuData.linear_acceleration_convariance.r[8]);
Eigen::Quaternionf quat = orientationT.getQuaternionf();
return IMU(
cv::Vec4d(quat.x(), quat.y(), quat.z(), quat.w()),
orientationCov,
cv::Vec3d(gyrT[0], gyrT[1], gyrT[2]),
angCov,
cv::Vec3d(accT[0], accT[1], accT[2]),
accCov,
imuLocalTransform);
}
#endif
bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
@@ -217,6 +284,14 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
(int)res.height,
this->getLocalTransform().prettyPrint().c_str());
if(infos.camera_model == sl::MODEL_ZED_M)
{
imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse();
UINFO("IMU local transform: %s (imu2cam=%s))",
imuLocalTransform_.prettyPrint().c_str(),
zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
}
return true;
#else
UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
@@ -252,34 +327,6 @@ bool CameraStereoZed::odomProvided() const
return false;
#endif
}
#ifdef RTABMAP_ZED
static cv::Mat slMat2cvMat(sl::Mat& input) {
//convert MAT_TYPE to CV_TYPE
int cv_type = -1;
switch (input.getDataType()) {
case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
default: break;
}
// cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
//cv::Mat and sl::Mat will share the same memory pointer
return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
}
Transform zedPoseToTransform(const sl::Pose & pose)
{
return Transform(
pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
}
#endif
SensorData CameraStereoZed::captureImage(CameraInfo * info)
{
@@ -327,6 +374,14 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
}
sl::IMUData imudata;
res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE);
if(res == sl::SUCCESS && imudata.valid)
{
//ZED-Mini
data.setIMU(zedIMUtoIMU(imudata, imuLocalTransform_));
}
if (computeOdometry_ && info)
{
sl::Pose pose;

482
corelib/src/imufilter/ComplementaryFilter.cpp Normal file
View File

@@ -0,0 +1,482 @@
/*
@author Roberto G. Valenti <robertogl.valenti@gmail.com>
@section LICENSE
Copyright (c) 2015, City University of New York
CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
3. Neither the name of the City College of New York nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "ComplementaryFilter.h"
#include <rtabmap/utilite/ULogger.h>
#include <cstdio>
#include <cmath>
#include <iostream>
namespace rtabmap {
const double ComplementaryFilter::kGravity = 9.81;
const double ComplementaryFilter::gamma_ = 0.01;
// Bias estimation steady state thresholds
const double ComplementaryFilter::kAngularVelocityThreshold = 0.2;
const double ComplementaryFilter::kAccelerationThreshold = 0.1;
const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01;
ComplementaryFilter::ComplementaryFilter(const ParametersMap & parameters):
IMUFilter(parameters),
gain_acc_(Parameters::defaultImuFilterComplementaryGainAcc()),
bias_alpha_(Parameters::defaultImuFilterComplementaryBiasAlpha()),
do_bias_estimation_(Parameters::defaultImuFilterComplementaryDoBiasEstimation()),
do_adaptive_gain_(Parameters::defaultImuFilterComplementaryDoAdpativeGain()),
initialized_(false),
steady_state_(false),
q0_(1), q1_(0), q2_(0), q3_(0),
wx_prev_(0), wy_prev_(0), wz_prev_(0),
wx_bias_(0), wy_bias_(0), wz_bias_(0)
{
parseParameters(parameters);
}
void ComplementaryFilter::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kImuFilterComplementaryGainAcc(), gain_acc_);
Parameters::parse(parameters, Parameters::kImuFilterComplementaryBiasAlpha(), bias_alpha_);
Parameters::parse(parameters, Parameters::kImuFilterComplementaryDoBiasEstimation(), do_bias_estimation_);
Parameters::parse(parameters, Parameters::kImuFilterComplementaryDoAdpativeGain(), do_adaptive_gain_);
}
void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation)
{
do_bias_estimation_ = do_bias_estimation;
}
bool ComplementaryFilter::getDoBiasEstimation() const
{
return do_bias_estimation_;
}
void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain)
{
do_adaptive_gain_ = do_adaptive_gain;
}
bool ComplementaryFilter::getDoAdaptiveGain() const
{
return do_adaptive_gain_;
}
bool ComplementaryFilter::setGainAcc(double gain)
{
if (gain >= 0 && gain <= 1.0)
{
gain_acc_ = gain;
return true;
}
else
return false;
}
double ComplementaryFilter::getGainAcc() const
{
return gain_acc_;
}
bool ComplementaryFilter::getSteadyState() const
{
return steady_state_;
}
bool ComplementaryFilter::setBiasAlpha(double bias_alpha)
{
if (bias_alpha >= 0 && bias_alpha <= 1.0)
{
bias_alpha_ = bias_alpha;
return true;
}
else
return false;
}
double ComplementaryFilter::getBiasAlpha() const
{
return bias_alpha_;
}
void ComplementaryFilter::reset(
double qx, double qy, double qz, double qw)
{
// Set the state to inverse (state is fixed wrt body).
invertQuaternion(qw, qx, qy, qz, q0_, q1_, q2_, q3_);
wx_prev_=0;
wy_prev_=0;
wz_prev_=0;
wx_bias_=0;
wy_bias_=0;
wz_bias_=0;
initialized_ = false;
steady_state_= false;
}
double ComplementaryFilter::getAngularVelocityBiasX() const
{
return wx_bias_;
}
double ComplementaryFilter::getAngularVelocityBiasY() const
{
return wy_bias_;
}
double ComplementaryFilter::getAngularVelocityBiasZ() const
{
return wz_bias_;
}
void ComplementaryFilter::updateImpl(
double gx, double gy, double gz,
double ax, double ay, double az,
double dt)
{
if (!initialized_)
{
// First time - ignore prediction:
getMeasurement(ax, ay, az,
q0_, q1_, q2_, q3_);
initialized_ = true;
return;
}
if(dt <= 0.0)
{
UERROR("dt=%f <=0.0, orientation will not be updated!", dt);
return;
}
// Bias estimation.
if (do_bias_estimation_)
updateBiases(ax, ay, az, gx, gy, gz);
// Prediction.
double q0_pred, q1_pred, q2_pred, q3_pred;
getPrediction(gx, gy, gz, dt,
q0_pred, q1_pred, q2_pred, q3_pred);
// Correction (from acc):
// q_ = q_pred * [(1-gain) * qI + gain * dq_acc]
// where qI = identity quaternion
double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
getAccCorrection(ax, ay, az,
q0_pred, q1_pred, q2_pred, q3_pred,
dq0_acc, dq1_acc, dq2_acc, dq3_acc);
double gain;
if (do_adaptive_gain_)
{
gain = getAdaptiveGain(gain_acc_, ax, ay, az);
}
else
{
gain = gain_acc_;
}
scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
dq0_acc, dq1_acc, dq2_acc, dq3_acc,
q0_, q1_, q2_, q3_);
normalizeQuaternion(q0_, q1_, q2_, q3_);
}
bool ComplementaryFilter::checkState(double ax, double ay, double az,
double wx, double wy, double wz) const
{
double acc_magnitude = sqrt(ax*ax + ay*ay + az*az);
if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold)
return false;
if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold ||
fabs(wy - wy_prev_) > kDeltaAngularVelocityThreshold ||
fabs(wz - wz_prev_) > kDeltaAngularVelocityThreshold)
return false;
if (fabs(wx - wx_bias_) > kAngularVelocityThreshold ||
fabs(wy - wy_bias_) > kAngularVelocityThreshold ||
fabs(wz - wz_bias_) > kAngularVelocityThreshold)
return false;
return true;
}
void ComplementaryFilter::updateBiases(double ax, double ay, double az,
double wx, double wy, double wz)
{
steady_state_ = checkState(ax, ay, az, wx, wy, wz);
if (steady_state_)
{
wx_bias_ += bias_alpha_ * (wx - wx_bias_);
wy_bias_ += bias_alpha_ * (wy - wy_bias_);
wz_bias_ += bias_alpha_ * (wz - wz_bias_);
}
wx_prev_ = wx;
wy_prev_ = wy;
wz_prev_ = wz;
}
void ComplementaryFilter::getPrediction(
double wx, double wy, double wz, double dt,
double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const
{
double wx_unb = wx - wx_bias_;
double wy_unb = wy - wy_bias_;
double wz_unb = wz - wz_bias_;
q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_);
q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_);
q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_);
q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_);
normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred);
}
void ComplementaryFilter::getMeasurement(
double ax, double ay, double az,
double mx, double my, double mz,
double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
{
// q_acc is the quaternion obtained from the acceleration vector representing
// the orientation of the Global frame wrt the Local frame with arbitrary yaw
// (intermediary frame). q3_acc is defined as 0.
double q0_acc, q1_acc, q2_acc, q3_acc;
// Normalize acceleration vector.
normalizeVector(ax, ay, az);
if (az >=0)
{
q0_acc = sqrt((az + 1) * 0.5);
q1_acc = -ay/(2.0 * q0_acc);
q2_acc = ax/(2.0 * q0_acc);
q3_acc = 0;
}
else
{
double X = sqrt((1 - az) * 0.5);
q0_acc = -ay/(2.0 * X);
q1_acc = X;
q2_acc = 0;
q3_acc = ax/(2.0 * X);
}
// [lx, ly, lz] is the magnetic field reading, rotated into the intermediary
// frame by the inverse of q_acc.
// l = R(q_acc)^-1 m
double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx +
2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz;
double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc +
q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz;
// q_mag is the quaternion that rotates the Global frame (North West Up) into
// the intermediary frame. q1_mag and q2_mag are defined as 0.
double gamma = lx*lx + ly*ly;
double beta = sqrt(gamma + lx*sqrt(gamma));
double q0_mag = beta / (sqrt(2.0 * gamma));
double q3_mag = ly / (sqrt(2.0) * beta);
// The quaternion multiplication between q_acc and q_mag represents the
// quaternion, orientation of the Global frame wrt the local frame.
// q = q_acc times q_mag
quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc,
q0_mag, 0, 0, q3_mag,
q0_meas, q1_meas, q2_meas, q3_meas );
//q0_meas = q0_acc*q0_mag;
//q1_meas = q1_acc*q0_mag + q2_acc*q3_mag;
//q2_meas = q2_acc*q0_mag - q1_acc*q3_mag;
//q3_meas = q0_acc*q3_mag;
}
void ComplementaryFilter::getMeasurement(
double ax, double ay, double az,
double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas)
{
// q_acc is the quaternion obtained from the acceleration vector representing
// the orientation of the Global frame wrt the Local frame with arbitrary yaw
// (intermediary frame). q3_acc is defined as 0.
// Normalize acceleration vector.
normalizeVector(ax, ay, az);
if (az >=0)
{
q0_meas = sqrt((az + 1) * 0.5);
q1_meas = -ay/(2.0 * q0_meas);
q2_meas = ax/(2.0 * q0_meas);
q3_meas = 0;
}
else
{
double X = sqrt((1 - az) * 0.5);
q0_meas = -ay/(2.0 * X);
q1_meas = X;
q2_meas = 0;
q3_meas = ax/(2.0 * X);
}
}
void ComplementaryFilter::getAccCorrection(
double ax, double ay, double az,
double p0, double p1, double p2, double p3,
double& dq0, double& dq1, double& dq2, double& dq3)
{
// Normalize acceleration vector.
normalizeVector(ax, ay, az);
// Acceleration reading rotated into the world frame by the inverse predicted
// quaternion (predicted gravity):
double gx, gy, gz;
rotateVectorByQuaternion(ax, ay, az,
p0, -p1, -p2, -p3,
gx, gy, gz);
// Delta quaternion that rotates the predicted gravity into the real gravity:
dq0 = sqrt((gz + 1) * 0.5);
dq1 = -gy/(2.0 * dq0);
dq2 = gx/(2.0 * dq0);
dq3 = 0.0;
}
void ComplementaryFilter::getOrientation(
double& qx, double& qy, double& qz, double& qw) const
{
// Return the inverse of the state (state is fixed wrt body).
invertQuaternion(q0_, q1_, q2_, q3_, qw,qx,qy,qz);
}
double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, double az)
{
double a_mag = sqrt(ax*ax + ay*ay + az*az);
double error = fabs(a_mag - kGravity)/kGravity;
double factor;
double error1 = 0.1;
double error2 = 0.2;
double m = 1.0/(error1 - error2);
double b = 1.0 - m*error1;
if (error < error1)
factor = 1.0;
else if (error < error2)
factor = m*error + b;
else
factor = 0.0;
//printf("FACTOR: %f \n", factor);
return factor*alpha;
}
void normalizeVector(double& x, double& y, double& z)
{
double norm = sqrt(x*x + y*y + z*z);
x /= norm;
y /= norm;
z /= norm;
}
void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
{
double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm;
q1 /= norm;
q2 /= norm;
q3 /= norm;
}
void invertQuaternion(
double q0, double q1, double q2, double q3,
double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv)
{
// Assumes quaternion is normalized.
q0_inv = q0;
q1_inv = -q1;
q2_inv = -q2;
q3_inv = -q3;
}
void scaleQuaternion(
double gain,
double& dq0, double& dq1, double& dq2, double& dq3)
{
if (dq0 < 0.0)//0.9
{
// Slerp (Spherical linear interpolation):
double angle = acos(dq0);
double A = sin(angle*(1.0 - gain))/sin(angle);
double B = sin(angle * gain)/sin(angle);
dq0 = A + B * dq0;
dq1 = B * dq1;
dq2 = B * dq2;
dq3 = B * dq3;
}
else
{
// Lerp (Linear interpolation):
dq0 = (1.0 - gain) + gain * dq0;
dq1 = gain * dq1;
dq2 = gain * dq2;
dq3 = gain * dq3;
}
normalizeQuaternion(dq0, dq1, dq2, dq3);
}
void quaternionMultiplication(
double p0, double p1, double p2, double p3,
double q0, double q1, double q2, double q3,
double& r0, double& r1, double& r2, double& r3)
{
// r = p q
r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3;
r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2;
r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1;
r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0;
}
void rotateVectorByQuaternion(
double x, double y, double z,
double q0, double q1, double q2, double q3,
double& vx, double& vy, double& vz)
{
vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z;
vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z;
vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z;
}
} // namespace rtabmap

161
corelib/src/imufilter/ComplementaryFilter.h Normal file
View File

@@ -0,0 +1,161 @@
/*
@author Roberto G. Valenti <robertogl.valenti@gmail.com>
@section LICENSE
Copyright (c) 2015, City University of New York
CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
3. Neither the name of the City College of New York nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_
#define CORELIB_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_
#include <rtabmap/core/IMUFilter.h>
namespace rtabmap {
class ComplementaryFilter : public IMUFilter
{
public:
ComplementaryFilter(const ParametersMap & parameters = ParametersMap());
virtual ~ComplementaryFilter() {}
bool setGainAcc(double gain);
double getGainAcc() const;
bool setBiasAlpha(double bias_alpha);
double getBiasAlpha() const;
// When the filter is in the steady state, bias estimation will occur (if the
// parameter is enabled).
bool getSteadyState() const;
void setDoBiasEstimation(bool do_bias_estimation);
bool getDoBiasEstimation() const;
void setDoAdaptiveGain(bool do_adaptive_gain);
bool getDoAdaptiveGain() const;
double getAngularVelocityBiasX() const;
double getAngularVelocityBiasY() const;
double getAngularVelocityBiasZ() const;
virtual void parseParameters(const ParametersMap & parameters);
virtual IMUFilter::Type type() const {return IMUFilter::kComplementaryFilter;}
virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const;
virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0);
// Update from accelerometer and gyroscope data.
// [gx, gy, gz]: Angular veloctiy, in rad / s.
// [ax, ay, az]: Normalized gravity vector.
// dt: time delta, in seconds.
virtual void updateImpl(
double gx, double gy, double gz,
double ax, double ay, double az,
double dt);
private:
static const double kGravity;
static const double gamma_;
// Bias estimation steady state thresholds
static const double kAngularVelocityThreshold;
static const double kAccelerationThreshold;
static const double kDeltaAngularVelocityThreshold;
// Gain parameter for the complementary filter, belongs in [0, 1].
double gain_acc_;
// Bias estimation gain parameter, belongs in [0, 1].
double bias_alpha_;
// Parameter whether to do bias estimation or not.
bool do_bias_estimation_;
// Parameter whether to do adaptive gain or not.
bool do_adaptive_gain_;
bool initialized_;
bool steady_state_;
// The orientation as a Hamilton quaternion (q0 is the scalar). Represents
// the orientation of the fixed frame wrt the body frame.
double q0_, q1_, q2_, q3_;
// Bias in angular velocities;
double wx_prev_, wy_prev_, wz_prev_;
// Bias in angular velocities;
double wx_bias_, wy_bias_, wz_bias_;
void updateBiases(double ax, double ay, double az,
double wx, double wy, double wz);
bool checkState(double ax, double ay, double az,
double wx, double wy, double wz) const;
void getPrediction(
double wx, double wy, double wz, double dt,
double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const;
void getMeasurement(
double ax, double ay, double az,
double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas);
void getMeasurement(
double ax, double ay, double az,
double mx, double my, double mz,
double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas);
void getAccCorrection(
double ax, double ay, double az,
double p0, double p1, double p2, double p3,
double& dq0, double& dq1, double& dq2, double& dq3);
double getAdaptiveGain(double alpha, double ax, double ay, double az);
};
// Utility math functions:
void normalizeVector(double& x, double& y, double& z);
void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3);
void scaleQuaternion(double gain,
double& dq0, double& dq1, double& dq2, double& dq3);
void invertQuaternion(
double q0, double q1, double q2, double q3,
double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv);
void quaternionMultiplication(double p0, double p1, double p2, double p3,
double q0, double q1, double q2, double q3,
double& r0, double& r1, double& r2, double& r3);
void rotateVectorByQuaternion(double x, double y, double z,
double q0, double q1, double q2, double q3,
double& vx, double& vy, double& vz);
}
#endif /* CORELIB_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_ */

364
corelib/src/imufilter/MadgwickFilter.cpp Normal file
View File

@@ -0,0 +1,364 @@
/*
* Copyright (C) 2010, CCNY Robotics Lab
* Ivan Dryanovski <ivan.dryanovski@gmail.com>
*
* http://robotics.ccny.cuny.edu
*
* Based on implementation of Madgwick's IMU and AHRS algorithms.
* http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
*
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "MadgwickFilter.h"
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Reciprocal_of_the_square_root
static inline float invSqrt(float x)
{
float xhalf = 0.5f * x;
union
{
float x;
int i;
} u;
u.x = x;
u.i = 0x5f3759df - (u.i >> 1);
/* The next line can be repeated any number of times to increase accuracy */
u.x = u.x * (1.5f - xhalf * u.x * u.x);
return u.x;
}
template<typename T>
static inline void normalizeVectorOpt(T& vx, T& vy, T& vz)
{
T recipNorm = invSqrt (vx * vx + vy * vy + vz * vz);
vx *= recipNorm;
vy *= recipNorm;
vz *= recipNorm;
}
template<typename T>
static inline void normalizeQuaternion(T& q0, T& q1, T& q2, T& q3)
{
T recipNorm = invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
static inline void rotateAndScaleVector(
float q0, float q1, float q2, float q3,
float _2dx, float _2dy, float _2dz,
float& rx, float& ry, float& rz) {
// result is half as long as input
rx = _2dx * (0.5f - q2 * q2 - q3 * q3)
+ _2dy * (q0 * q3 + q1 * q2)
+ _2dz * (q1 * q3 - q0 * q2);
ry = _2dx * (q1 * q2 - q0 * q3)
+ _2dy * (0.5f - q1 * q1 - q3 * q3)
+ _2dz * (q0 * q1 + q2 * q3);
rz = _2dx * (q0 * q2 + q1 * q3)
+ _2dy * (q2 * q3 - q0 * q1)
+ _2dz * (0.5f - q1 * q1 - q2 * q2);
}
static inline void orientationChangeFromGyro(
float q0, float q1, float q2, float q3,
float gx, float gy, float gz,
float& qDot1, float& qDot2, float& qDot3, float& qDot4)
{
// Rate of change of quaternion from gyroscope
// See EQ 12
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
}
static inline void addGradientDescentStep(
float q0, float q1, float q2, float q3,
float _2dx, float _2dy, float _2dz,
float mx, float my, float mz,
float& s0, float& s1, float& s2, float& s3)
{
float f0, f1, f2;
// Gradient decent algorithm corrective step
// EQ 15, 21
rotateAndScaleVector(q0,q1,q2,q3, _2dx, _2dy, _2dz, f0, f1, f2);
f0 -= mx;
f1 -= my;
f2 -= mz;
// EQ 22, 34
// Jt * f
s0 += (_2dy * q3 - _2dz * q2) * f0
+ (-_2dx * q3 + _2dz * q1) * f1
+ (_2dx * q2 - _2dy * q1) * f2;
s1 += (_2dy * q2 + _2dz * q3) * f0
+ (_2dx * q2 - 2.0f * _2dy * q1 + _2dz * q0) * f1
+ (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2;
s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0
+ (_2dx * q1 + _2dz * q3) * f1
+ (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2;
s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0
+ (-_2dx * q0 - 2.0f * _2dy * q3 + _2dz * q2) * f1
+ (_2dx * q1 + _2dy * q2) * f2;
}
template<typename T>
static inline void crossProduct(
T ax, T ay, T az,
T bx, T by, T bz,
T& rx, T& ry, T& rz) {
rx = ay*bz - az*by;
ry = az*bx - ax*bz;
rz = ax*by - ay*bx;
}
template<typename T>
static inline T normalizeVector(T& vx, T& vy, T& vz) {
T norm = sqrt(vx*vx + vy*vy + vz*vz);
T inv = 1.0 / norm;
vx *= inv;
vy *= inv;
vz *= inv;
return norm;
}
static inline bool computeOrientation(
Eigen::Vector3f A,
Eigen::Vector3f E,
Eigen::Quaternionf& orientation) {
float Hx, Hy, Hz;
float Mx, My, Mz;
float normH;
// A: pointing up
float Ax = A[0], Ay = A[1], Az = A[2];
// E: pointing down/north
float Ex = E[0], Ey = E[1], Ez = E[2];
// H: vector horizontal, pointing east
// H = E x A
crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz);
// normalize H
normH = normalizeVector(Hx, Hy, Hz);
if (normH < 1E-7) {
// device is close to free fall (or in space?), or close to
// magnetic north pole.
// mag in T => Threshold 1E-7, typical values are > 1E-5.
return false;
}
// normalize A
normalizeVector(Ax, Ay, Az);
// M: vector horizontal, pointing north
// M = A x H
crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz);
// Create matrix for basis transformation
Eigen::Matrix3f R;
//case WorldFrame::ENU:
// vector space world W:
// Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up
// vector space local L:
// Basis: H, M , A
// W(1,0,0) => L(H)
// W(0,1,0) => L(M)
// W(0,0,1) => L(A)
// R: Transform Matrix local => world equals basis of L, because basis of W is I
R(0,0) = Hx; R(0,1) = Mx; R(0,2) = Ax;
R(1,0) = Hy; R(1,1) = My; R(1,2) = Ay;
R(2,0) = Hz; R(2,1) = Mz; R(2,2) = Az;
// Matrix.getRotation assumes vector rotation, but we're using
// coordinate systems. Thus negate rotation angle (inverse).
Eigen::Quaternionf q(R);
orientation = q.inverse();
return true;
}
static inline bool computeOrientation(
Eigen::Vector3f A,
Eigen::Quaternionf& orientation) {
// This implementation could be optimized regarding speed.
// magnetic Field E must not be parallel to A,
// choose an arbitrary orthogonal vector
Eigen::Vector3f E;
if (fabs(A[0]) > 0.1 || fabs(A[1]) > 0.1) {
E[0] = A[1];
E[1] = A[0];
E[2] = 0.0;
} else if (fabs(A[2]) > 0.1) {
E[0] = 0.0;
E[1] = A[2];
E[2] = A[1];
} else {
// free fall
return false;
}
return computeOrientation(A, E, orientation);
}
MadgwickFilter::MadgwickFilter(const ParametersMap & parameters) :
IMUFilter(parameters),
q0(1.0), q1(0.0), q2(0.0), q3(0.0),
w_bx_(0.0), w_by_(0.0), w_bz_(0.0),
initialized_(false),
gain_ (Parameters::defaultImuFilterMadgwickGain()),
zeta_ (Parameters::defaultImuFilterMadgwickZeta())
{
parseParameters(parameters);
}
void MadgwickFilter::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kImuFilterMadgwickGain(), gain_);
Parameters::parse(parameters, Parameters::kImuFilterMadgwickZeta(), zeta_);
}
/**
* Gain of the filter. Higher values lead to faster convergence but
* more noise. Lower values lead to slower convergence but smoother signal. [0.0, 1.0]
*/
void MadgwickFilter::setAlgorithmGain(double gain)
{
gain_ = gain;
}
/**
* Gyro drift gain (approx. rad/s). [-1.0, 1.0]
*/
void MadgwickFilter::setDriftBiasGain(double zeta)
{
zeta_ = zeta;
}
void MadgwickFilter::getOrientation(double & qx, double & qy, double & qz, double & qw) const
{
qx = this->q1;
qy = this->q2;
qz = this->q3;
qw = this->q0;
// perform precise normalization of the output, using 1/sqrt()
// instead of the fast invSqrt() approximation. Without this,
// TF2 complains that the quaternion is not normalized.
double recipNorm = 1 / sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
qx *= recipNorm;
qy *= recipNorm;
qz *= recipNorm;
qw *= recipNorm;
}
void MadgwickFilter::reset(double qx, double qy, double qz, double qw)
{
this->q0 = qw;
this->q1 = qx;
this->q2 = qy;
this->q3 = qz;
w_bx_ = 0;
w_by_ = 0;
w_bz_ = 0;
initialized_ = false;
}
void MadgwickFilter::updateImpl(
double gx, double gy, double gz,
double ax, double ay, double az,
double dt)
{
if(!initialized_)
{
Eigen::Quaternionf orientation;
Eigen::Vector3f A;
A[0] = ax;
A[1] = ay;
A[2] = az;
computeOrientation(A,orientation);
reset(orientation.x(), orientation.y(), orientation.z(), orientation.w());
printf("%f %f %f -> %f %f %f %f\n", A[0], A[1], A[2], orientation.x(), orientation.y(), orientation.z(), orientation.w());
initialized_ = true;
return;
}
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
// Rate of change of quaternion from gyroscope
orientationChangeFromGyro (q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
// Normalise accelerometer measurement
normalizeVectorOpt(ax, ay, az);
// Gradient decent algorithm corrective step
s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
//case WorldFrame::ENU:
// Gravity: [0, 0, 1]
addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
normalizeQuaternion(s0, s1, s2, s3);
// Apply feedback step
qDot1 -= gain_ * s0;
qDot2 -= gain_ * s1;
qDot3 -= gain_ * s2;
qDot4 -= gain_ * s3;
}
// Integrate rate of change of quaternion to yield quaternion
if(dt <= 0.0)
{
UERROR("dt=%f <=0.0, orientation will not be updated!", dt);
return;
}
q0 += qDot1 * dt;
q1 += qDot2 * dt;
q2 += qDot3 * dt;
q3 += qDot4 * dt;
//printf("%fs %f %f %f %f\n", dt, q0, q1, q2, q3);
// Normalise quaternion
normalizeQuaternion (q0, q1, q2, q3);
}
}

82
corelib/src/imufilter/MadgwickFilter.h Normal file
View File

@@ -0,0 +1,82 @@
/*
* Copyright (C) 2010, CCNY Robotics Lab
* Ivan Dryanovski <ivan.dryanovski@gmail.com>
*
* http://robotics.ccny.cuny.edu
*
* Based on implementation of Madgwick's IMU and AHRS algorithms.
* http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
*
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_
#define CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_
#include <rtabmap/core/IMUFilter.h>
#include <cmath>
namespace rtabmap {
class MadgwickFilter : public IMUFilter
{
public:
MadgwickFilter(const ParametersMap & parameters = ParametersMap());
virtual ~MadgwickFilter(){}
private:
// **** state variables
double q0, q1, q2, q3; // quaternion
float w_bx_, w_by_, w_bz_; //
bool initialized_;
// **** paramaters
double gain_; // algorithm gain
double zeta_; // gyro drift bias gain
public:
/**
* Gain of the filter. Higher values lead to faster convergence but
* more noise. Lower values lead to slower convergence but smoother signal. [0.0, 1.0]
*/
void setAlgorithmGain(double gain);
/**
* Gyro drift gain (approx. rad/s). [-1.0, 1.0]
*/
void setDriftBiasGain(double zeta);
virtual void parseParameters(const ParametersMap & parameters);
virtual IMUFilter::Type type() const {return IMUFilter::kMadgwick;}
virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const;
virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0);
private:
// Update from accelerometer and gyroscope data.
// [gx, gy, gz]: Angular veloctiy, in rad / s.
// [ax, ay, az]: Normalized gravity vector.
// dt: time delta, in seconds.
void updateImpl(
double gx, double gy, double gz,
double ax, double ay, double az,
double dt);
};
}
#endif /* CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_ */

28
corelib/src/imufilter/README.txt Normal file
View File

@@ -0,0 +1,28 @@
IMU filters taken from ROS imu_tools stack: https://github.com/ccny-ros-pkg/imu_tools, please look below for licensing. To avoid GPL license, MadgwickFilter can be disabled on compilation with "cmake -DWITH_MADGWICK=OFF ..".
===================================
Overview
-----------------------------------
IMU-related filters:
* `MadgwickFilter`: a filter which fuses angular velocities,
accelerations, and (optionally) magnetic readings from a generic IMU
device into an orientation. Based on the work of [1].
* `ComplementaryFilter`: a filter which fuses angular velocities,
accelerations, and (optionally) magnetic readings from a generic IMU
device into an orientation quaternion using a novel approach based on a complementary fusion. Based on the work of [2].
License
-----------------------------------
* `MadgwickFilter`: currently licensed as GPL, following the original implementation
* `ComplementaryFilter`: BSD
References
-----------------------------------
[1] http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
[2] http://www.mdpi.com/1424-8220/15/8/19302

145
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -72,6 +72,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
map_(new Signature(-1)),
lastFrame_(new Signature(1)),
lastFrameOldestNewId_(0),
initGravity_(false),
bundleSeq_(0),
sba_(0)
{
@@ -150,6 +151,7 @@ OdometryF2M::~OdometryF2M()
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
imus_.clear();
delete sba_;
delete regPipeline_;
UDEBUG("");
@@ -158,26 +160,33 @@ OdometryF2M::~OdometryF2M()
void OdometryF2M::reset(const Transform & initialPose)
{
UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
Odometry::reset(initialPose);
*lastFrame_ = Signature(1);
*map_ = Signature(-1);
scansBuffer_.clear();
bundleWordReferences_.clear();
bundlePoses_.clear();
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
bundleSeq_ = 0;
lastFrameOldestNewId_ = 0;
if(!initGravity_)
{
UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
Odometry::reset(initialPose);
*lastFrame_ = Signature(1);
*map_ = Signature(-1);
scansBuffer_.clear();
bundleWordReferences_.clear();
bundlePoses_.clear();
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
bundleSeq_ = 0;
lastFrameOldestNewId_ = 0;
imus_.clear();
}
initGravity_ = false;
}
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
SensorData & data,
const Transform & guess,
const Transform & guessIn,
OdometryInfo * info)
{
Transform guess = guessIn;
UTimer timer;
Transform output;
@@ -186,6 +195,39 @@ Transform OdometryF2M::computeTransform(
info->type = 0;
}
if(!data.imu().empty())
{
if(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0)
{
UERROR("IMU received doesn't have orientation set, it is ignored.");
}
else
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
//UWARN("%fs %s", data.stamp(), orientation.prettyPrint().c_str());
imus_.insert(std::make_pair(data.stamp(), orientation*data.imu().localTransform().inverse()));
if(imus_.size() > 1000)
{
imus_.erase(imus_.begin());
}
if(this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f)
{
Eigen::Quaterniond imuQuat = imus_.rbegin()->second.getQuaterniond();
Transform previous = this->getPose();
Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
initGravity_ = true;
this->reset(newFramePose);
}
}
if(data.imageRaw().empty() && data.laserScanRaw().isEmpty())
{
return output;
}
}
RegistrationInfo regInfo;
int nFeatures = 0;
@@ -285,6 +327,21 @@ Transform OdometryF2M::computeTransform(
UDEBUG("Registration time = %fs", regInfo.totalTime);
if(!transform.isNull())
{
Transform imuT;
if(!imus_.empty())
{
double stampDiff = 0.0;
imuT = getClosestIMU(lastFrame_->getStamp(), stampDiff);
if(stampDiff < 0.05)
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT)));
}
else
{
UWARN("IMUs are set, but we could not find one matching the current frame stamp %f (stampDiff=%f > 0.05)", lastFrame_->getStamp(), stampDiff);
}
}
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
@@ -311,6 +368,7 @@ Transform OdometryF2M::computeTransform(
bundlePoses = bundlePoses_;
bundleLinks = bundleLinks_;
bundleModels = bundleModels_;
bundleLinks.insert(bundleIMUOrientations_.begin(), bundleIMUOrientations_.end());
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
@@ -320,6 +378,11 @@ Transform OdometryF2M::computeTransform(
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
if(!imuT.isNull())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT)));
}
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
@@ -445,7 +508,9 @@ Transform OdometryF2M::computeTransform(
else
{
transform = bundlePoses.rbegin()->second;
bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
UASSERT(iter != bundleLinks.end());
iter->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
@@ -534,8 +599,14 @@ Transform OdometryF2M::computeTransform(
if(bundleAdjustment_>0)
{
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
UASSERT(graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false) != bundleLinks.end());
bundleLinks_.insert(*bundleLinks.find(bundlePoses_.rbegin()->first));
std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
UASSERT(iter != bundleLinks.end());
bundleLinks_.insert(*iter);
iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
if(iter != bundleLinks.end())
{
bundleIMUOrientations_.insert(*iter);
}
uInsert(bundlePoses_, bundlePoses);
UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
@@ -818,6 +889,7 @@ Transform OdometryF2M::computeTransform(
UASSERT(bundlePoses_.erase(iter->first) == 1);
bundleLinks_.erase(iter->first);
bundleModels_.erase(iter->first);
bundleIMUOrientations_.erase(iter->first);
bundlePoseReferences_.erase(iter++);
}
}
@@ -1012,6 +1084,7 @@ Transform OdometryF2M::computeTransform(
bool frameValid = false;
Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired())
{
int ptsWithDepth = 0;
@@ -1115,6 +1188,11 @@ Transform OdometryF2M::computeTransform(
}
bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
if(!imus_.empty())
{
bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, newFramePose)));
}
}
map_->setWords(words);
@@ -1229,4 +1307,41 @@ Transform OdometryF2M::computeTransform(
return output;
}
Transform OdometryF2M::getClosestIMU(const double & stamp, double & stampDiff) const
{
UASSERT(!imus_.empty());
std::map<double, Transform>::const_iterator imuIterB = imus_.lower_bound(stamp);
std::map<double, Transform>::const_iterator imuIterA = imuIterB;
if(imuIterA != imus_.begin())
{
imuIterA = --imuIterA;
}
if(imuIterB == imus_.end())
{
imuIterB = --imuIterB;
}
Transform imuT;
stampDiff = 0.0;
if(imuIterB->first == lastFrame_->getStamp() || imuIterA == imuIterB)
{
imuT = imuIterB->second;
stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
}
else if(imuIterA != imuIterB)
{
if(fabs(imuIterA->first - lastFrame_->getStamp()) <
fabs(imuIterB->first - lastFrame_->getStamp()))
{
imuT = imuIterA->second;
stampDiff = fabs(imuIterA->first - lastFrame_->getStamp());
}
else
{
imuT = imuIterB->second;
stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
}
}
return imuT;
}
} // namespace rtabmap

3
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -1417,6 +1417,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity());
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && !iterModel->second.localTransform().isNull());
priorEdge->setCameraInvLocalTransform(iterModel->second.localTransform().inverse().toEigen3d().linear());
priorEdge->setMeasurement(m);
priorEdge->setInformation(information);
priorEdge->vertices()[0] = v1;

19
corelib/src/optimizer/g2o/edge_sbacam_gravity.h
View File

@@ -48,6 +48,11 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6,
virtual bool read(std::istream& is) {return false;} // not implemented
virtual bool write(std::ostream& os) const {return false;} // not implemented
void setCameraInvLocalTransform(const Eigen::Matrix3d & t)
{
cameraInvLocalTransform_ = t;
}
// return the error estimate as a 3-vector
void computeError(){
const g2o::VertexCam* v1 = static_cast<const g2o::VertexCam*>(_vertices[0]);
@@ -57,7 +62,8 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6,
Eigen::Vector3d ea;
Eigen::Matrix3d t = v1->estimate().rotation().toRotationMatrix();
// Transform pose from camera frame to world frame
Eigen::Matrix3d t = v1->estimate().rotation().toRotationMatrix() * cameraInvLocalTransform_;
ea[0] = atan2(t (2, 1), t (2, 2));
ea[1] = asin(-t (2, 0));
ea[2] = atan2(t (1, 0), t (0, 0));
@@ -69,10 +75,10 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6,
Eigen::Vector3d estimate = rot * -direction;
_error = estimate - measurement;
//printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(),
// measurement[0], measurement[1], measurement[2],
// estimate[0], estimate[1], estimate[2],
// _error[0], _error[1], _error[2]);
/*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(),
measurement[0], measurement[1], measurement[2],
estimate[0], estimate[1], estimate[2],
_error[0], _error[1], _error[2]);*/
}
// 6 values:
@@ -82,6 +88,9 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6,
_measurement.head<3>() = m.head<3>().normalized();
_measurement.tail<3>() = m.tail<3>().normalized();
}
private:
Eigen::Matrix3d cameraInvLocalTransform_;
};
}