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

UI: Update preferences with the new imu filter parameters. Camera: added imu filtering option. Updated support for zedm, D435i and T265.

Browse Source
This commit is contained in:
matlabbe
2019-05-13 18:42:46 -04:00
parent cbca362cc4
commit f8a8e55e7e
15 changed files with 762 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -45,6 +45,7 @@ class Camera;
class CameraInfo;
class SensorData;
class StereoDense;
class IMUFilter;
/**
* Class CameraThread
@@ -68,6 +69,8 @@ public:
void setDistortionModel(const std::string & path);
void enableBilateralFiltering(float sigmaS, float sigmaR);
void disableBilateralFiltering() {_bilateralFiltering = false;}
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
void disableIMUFiltering();
void setScanParameters(
bool fromDepth,
@@ -122,6 +125,7 @@ private:
bool _bilateralFiltering;
float _bilateralSigmaS;
float _bilateralSigmaR;
IMUFilter * _imuFilter;
};
} // namespace rtabmap

3
corelib/include/rtabmap/core/Odometry.h
View File

@@ -38,7 +38,6 @@ namespace rtabmap {
class OdometryInfo;
class ParticleFilter;
class IMUFilter;
class RTABMAP_EXP Odometry
{
@@ -92,7 +91,6 @@ private:
bool _holonomic;
bool guessFromMotion_;
bool guessSmoothingDelay_;
int _imuFilteringStrategy;
int _filteringStrategy;
int _particleSize;
float _particleNoiseT;
@@ -117,7 +115,6 @@ private:
std::vector<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_;
IMUFilter * imuFilter_;
protected:
Odometry(const rtabmap::ParametersMap & parameters);

13
corelib/include/rtabmap/core/Parameters.h
View File

@@ -409,7 +409,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Odom, ImuFilteringStrategy, int, 0, "0=No filtering 1=Madgwick Filter 2=Complementary Filter. This is used to estimate the quaternion from acceleration and angular velocities of IMU before doing odometry updates. IMU data should be in ENU coordinates.");
RTABMAP_PARAM(Odom, FilteringStrategy, int, 0, "0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.");
RTABMAP_PARAM(Odom, ParticleSize, unsigned int, 400, "Number of particles of the filter.");
RTABMAP_PARAM(Odom, ParticleNoiseT, float, 0.002, "Noise (m) of translation components (x,y,z).");
@@ -732,18 +731,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(ImuFilter, ComplementaryDoBiasEstimation, bool, true, "Parameter whether to do bias estimation or not.");
RTABMAP_PARAM(ImuFilter, ComplementaryDoAdpativeGain, bool, true, "Parameter whether to do adaptive gain or not.");
//
double gain_acc_;
//
double bias_alpha_;
//
bool do_bias_estimation_;
//
bool do_adaptive_gain_;
public:
virtual ~Parameters();

4
corelib/include/rtabmap/core/camera/CameraRealSense2.h
View File

@@ -82,7 +82,7 @@ public:
void pose_callback(rs2::frame frame);
void frame_callback(rs2::frame frame);
void multiple_message_callback(rs2::frame frame);
bool getPoseAndIMU(
void getPoseAndIMU(
const double & stamp,
Transform & pose,
unsigned int & poseConfidence,
@@ -110,6 +110,8 @@ private:
std::map<double, cv::Vec3f> accBuffer_;
std::map<double, cv::Vec3f> gyroBuffer_;
std::map<double, std::pair<Transform, unsigned int> > poseBuffer_; // <stamp, <Pose, confidence: 1=lost, 2=low, 3=high> >
UMutex poseMutex_;
UMutex imuMutex_;
bool emitterEnabled_;
bool irDepth_;

49
corelib/src/CameraThread.cpp
View File

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/StereoDense.h"
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/IMUFilter.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include <opencv2/stitching/detail/exposure_compensate.hpp>
#include <rtabmap/utilite/UTimer.h>
@@ -65,7 +66,8 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_distortionModel(0),
_bilateralFiltering(false),
_bilateralSigmaS(10),
_bilateralSigmaR(0.1)
_bilateralSigmaR(0.1),
_imuFilter(0)
{
UASSERT(_camera != 0);
}
@@ -77,6 +79,7 @@ CameraThread::~CameraThread()
delete _camera;
delete _distortionModel;
delete _stereoDense;
delete _imuFilter;
}
void CameraThread::setImageRate(float imageRate)
@@ -115,6 +118,18 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
_bilateralSigmaR = sigmaR;
}
void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters)
{
delete _imuFilter;
_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
}
void CameraThread::disableIMUFiltering()
{
delete _imuFilter;
_imuFilter = 0;
}
void CameraThread::mainLoopBegin()
{
ULogger::registerCurrentThread("Camera");
@@ -386,6 +401,38 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
// filter the scan after registration
data.setLaserScan(util3d::commonFiltering(data.laserScanRaw(), _scanDownsampleStep, _scanRangeMin, _scanRangeMax, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp));
}
// IMU filtering
if(_imuFilter && !data.imu().empty())
{
_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()));
UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
data.imu().orientation()[0],
data.imu().orientation()[1],
data.imu().orientation()[2],
data.imu().orientation()[3],
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());
}
}
} // namespace rtabmap

39
corelib/src/Odometry.cpp
View File

@@ -37,7 +37,6 @@ 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"
@@ -111,7 +110,6 @@ 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()),
@@ -129,8 +127,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_resetCurrentCount(0),
previousStamp_(0),
distanceTravelled_(0),
framesProcessed_(0),
imuFilter_(0)
framesProcessed_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
@@ -139,7 +136,6 @@ 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);
@@ -182,11 +178,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
{
initKalmanFilter();
}
if(_imuFilteringStrategy > 0)
{
imuFilter_ = IMUFilter::create((IMUFilter::Type)(_imuFilteringStrategy-1), parameters);
}
}
Odometry::~Odometry()
@@ -196,7 +187,6 @@ Odometry::~Odometry()
delete particleFilters_[i];
}
particleFilters_.clear();
delete imuFilter_;
}
void Odometry::reset(const Transform & initialPose)
@@ -251,10 +241,6 @@ void Odometry::reset(const Transform & initialPose)
{
_pose = initialPose;
}
if(imuFilter_)
{
imuFilter_->reset();
}
}
const Transform & Odometry::previousVelocityTransform() const
@@ -367,28 +353,6 @@ 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();
@@ -406,7 +370,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
previousVelocities_.clear();
velocityGuess_.setNull();
}
if(!velocityGuess_.isNull())
{
if(guessFromMotion_)

385
corelib/src/camera/CameraRealSense2.cpp
View File

@@ -67,7 +67,7 @@ CameraRealSense2::CameraRealSense2(
emitterEnabled_(true),
irDepth_(false),
rectifyImages_(true),
odometryProvided_(true)
odometryProvided_(false)
#endif
{
UDEBUG("");
@@ -173,12 +173,15 @@ void CameraRealSense2::imu_callback(rs2::frame frame)
{
auto stream = frame.get_profile().stream_type();
cv::Vec3f crnt_reading = *reinterpret_cast<const cv::Vec3f*>(frame.get_data());
UDEBUG("%s callback! %f (%f %f %f)",
stream == RS2_STREAM_GYRO?"GYRO":"ACC",
frame.get_timestamp(),
crnt_reading[0],
crnt_reading[1],
crnt_reading[2]);
UScopeMutex sm(imuMutex_);
if(stream == RS2_STREAM_GYRO)
{
UDEBUG("GYRO callback! %f (%f %f %f)", frame.get_timestamp(),
crnt_reading[0],
crnt_reading[1],
crnt_reading[2]);
gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
if(gyroBuffer_.size() > 10)
{
@@ -187,18 +190,12 @@ void CameraRealSense2::imu_callback(rs2::frame frame)
}
else
{
UDEBUG("ACC callback! %f (%f %f %f)", frame.get_timestamp(),
crnt_reading[0],
crnt_reading[1],
crnt_reading[2]);
accBuffer_.insert(accBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
if(accBuffer_.size() > 10)
{
accBuffer_.erase(accBuffer_.begin());
}
}
}
Transform CameraRealSense2::realsense2PoseRotation_ = Transform(
@@ -220,6 +217,8 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
pose.rotation.w);
poseT = realsense2PoseRotation_ * poseT * realsense2PoseRotationInv_;
UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
UScopeMutex sm(poseMutex_);
poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
if(poseBuffer_.size() > 10)
{
@@ -242,14 +241,17 @@ void CameraRealSense2::multiple_message_callback(rs2::frame frame)
imu_callback(frame);
break;
case RS2_STREAM_POSE:
pose_callback(frame);
if(odometryProvided_)
{
pose_callback(frame);
}
break;
default:
frame_callback(frame);
}
}
bool CameraRealSense2::getPoseAndIMU(
void CameraRealSense2::getPoseAndIMU(
const double & stamp,
Transform & pose,
unsigned int & poseConfidence,
@@ -258,109 +260,166 @@ bool CameraRealSense2::getPoseAndIMU(
pose.setNull();
imu = IMU();
poseConfidence = 0;
if(poseBuffer_.empty() || accBuffer_.empty() || gyroBuffer_.empty())
if(accBuffer_.empty() || gyroBuffer_.empty())
{
return false;
return;
}
int maxWaitTime = 30;
// Interpolate pose
if(!poseBuffer_.empty())
{
std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterB = poseBuffer_.lower_bound(stamp);
std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterA = iterB;
if(iterA != poseBuffer_.begin())
poseMutex_.lock();
int waitTry = 0;
while(poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
{
iterA = --iterA;
poseMutex_.unlock();
++waitTry;
uSleep(1);
poseMutex_.lock();
}
if(iterB == poseBuffer_.end())
if(poseBuffer_.rbegin()->first < stamp)
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
pose = iterA->second.first;
poseConfidence = iterA->second.second;
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
pose = iterA->second.first.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second.first);
poseConfidence = iterA->second.second;
UWARN("Could not find poses to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, poseBuffer_.rbegin()->first);
}
else
{
UWARN("Could not find poses to interpolate at time %f", stamp);
return false;
std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterB = poseBuffer_.lower_bound(stamp);
std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterA = iterB;
if(iterA != poseBuffer_.begin())
{
iterA = --iterA;
}
if(iterB == poseBuffer_.end())
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
pose = iterA->second.first;
poseConfidence = iterA->second.second;
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
pose = iterA->second.first.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second.first);
poseConfidence = iterA->second.second;
}
else
{
UWARN("Could not find poses to interpolate at time %f", stamp);
}
}
poseMutex_.unlock();
}
// Interpolate acc
cv::Vec3d acc;
{
std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
if(iterA != accBuffer_.begin())
imuMutex_.lock();
int waitTry = 0;
while(accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
{
iterA = --iterA;
imuMutex_.unlock();
++waitTry;
uSleep(1);
imuMutex_.lock();
}
if(iterB == accBuffer_.end())
if(accBuffer_.rbegin()->first < stamp)
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
acc[0] = iterA->second[0];
acc[1] = iterA->second[1];
acc[2] = iterA->second[2];
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
UWARN("Could not find acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, accBuffer_.rbegin()->first);
imuMutex_.unlock();
return;
}
else
{
UWARN("Could not find acc data to interpolate at time %f", stamp);
return false;
std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
if(iterA != accBuffer_.begin())
{
iterA = --iterA;
}
if(iterB == accBuffer_.end())
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
acc[0] = iterA->second[0];
acc[1] = iterA->second[1];
acc[2] = iterA->second[2];
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
}
else
{
UWARN("Could not find acc data to interpolate at time %f", stamp);
imuMutex_.unlock();
return;
}
}
imuMutex_.unlock();
}
// Interpolate gyro
cv::Vec3d gyro;
{
std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
if(iterA != gyroBuffer_.begin())
imuMutex_.lock();
int waitTry = 0;
while(gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
{
iterA = --iterA;
imuMutex_.unlock();
++waitTry;
uSleep(1);
imuMutex_.lock();
}
if(iterB == gyroBuffer_.end())
if(gyroBuffer_.rbegin()->first < stamp)
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
gyro[0] = iterA->second[0];
gyro[1] = iterA->second[1];
gyro[2] = iterA->second[2];
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
UWARN("Could not find gyro data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, gyroBuffer_.rbegin()->first);
imuMutex_.unlock();
return;
}
else
{
UWARN("Could not find gyro data to interpolate at time %f", stamp);
return false;
std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
if(iterA != gyroBuffer_.begin())
{
iterA = --iterA;
}
if(iterB == gyroBuffer_.end())
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
gyro[0] = iterA->second[0];
gyro[1] = iterA->second[1];
gyro[2] = iterA->second[2];
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
}
else
{
UWARN("Could not find gyro data to interpolate at time %f", stamp);
imuMutex_.unlock();
return;
}
}
imuMutex_.unlock();
}
imu = IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_);
return true;
}
#endif
@@ -485,18 +544,25 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
auto profiles = sensors[i].get_stream_profiles();
bool added = false;
UINFO("profiles=%d", (int)profiles.size());
if(ULogger::level()>=ULogger::kInfo)
{
for (auto& profile : profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
UINFO("%s %d %d %d", rs2_format_to_string(
video_profile.format()),
video_profile.width(),
video_profile.height(),
video_profile.fps());
}
}
int pi = 0;
for (auto& profile : profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
UINFO("%s %d %d %d", rs2_format_to_string(
video_profile.format()),
video_profile.width(),
video_profile.height(),
video_profile.fps());
if(!stereo)
{
//D400 series:
if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
video_profile.width() == 640 &&
video_profile.height() == 480 &&
@@ -522,11 +588,18 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F)
{
//D435i:
//MOTION_XYZ32F 0 0 200
//MOTION_XYZ32F 0 0 400
//MOTION_XYZ32F 0 0 63
//MOTION_XYZ32F 0 0 250
profilesPerSensor[i].push_back(profile);
added = true;
}
}
else if(stereo)
{
//T265:
if(video_profile.format() == RS2_FORMAT_Y8 &&
video_profile.width() == 848 &&
video_profile.height() == 800 &&
@@ -551,11 +624,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
added = true;
}
//MOTION_XYZ32F 0 0 200
//MOTION_XYZ32F 0 0 62
//6DOF 0 0 200
else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
{
//MOTION_XYZ32F 0 0 200
//MOTION_XYZ32F 0 0 62
//6DOF 0 0 200
profilesPerSensor[0].push_back(profile);
added = true;
}
@@ -579,28 +652,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
*depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
std::function<void(rs2::frame)> imu_callback_function = [this](rs2::frame frame){imu_callback(frame);};
for (unsigned int i=0; i<sensors.size(); ++i)
if(profilesPerSensor.size() == 3 && !profilesPerSensor[2].empty() && !profilesPerSensor[0].empty())
{
if(profilesPerSensor[i].size())
{
UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
sensors[i].open(profilesPerSensor[i]);
if(sensors[i].is<rs2::depth_sensor>())
{
auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
depth_scale_meters_ = depth_sensor.get_depth_scale();
}
if(i == 2) // 2 is ACC/GYRO
{
sensors[i].start(imu_callback_function);
}
else
{
sensors[i].start(*syncer_);
}
}
rs2_extrinsics leftToIMU = profilesPerSensor[0][0].get_extrinsics_to(profilesPerSensor[2][0]);
Transform leftToIMUT(
leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
}
}
else
@@ -636,43 +697,73 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
// 3=ACC
// 4=POSE
UASSERT(profilesPerSensor[0].size() == 5);
rs2_extrinsics poseToLeft = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][1]);
rs2_extrinsics poseToIMU = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][2]);
Transform realsense2_pose_rotation(0, 0,-1,0,
-1, 0, 0,0,
0, 1, 0,0);
Transform poseToLeftT(
poseToLeft.rotation[0], poseToLeft.rotation[1], poseToLeft.rotation[2], poseToLeft.translation[0],
poseToLeft.rotation[3], poseToLeft.rotation[4], poseToLeft.rotation[5], poseToLeft.translation[1],
poseToLeft.rotation[6], poseToLeft.rotation[7], poseToLeft.rotation[8], poseToLeft.translation[2]);
poseToLeftT = realsense2PoseRotation_ * poseToLeftT;
UINFO("poseToLeft = %s", poseToLeftT.prettyPrint().c_str());
if(odometryProvided_)
{
rs2_extrinsics poseToLeft = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][1]);
rs2_extrinsics poseToIMU = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][2]);
Transform realsense2_pose_rotation(0, 0,-1,0,
-1, 0, 0,0,
0, 1, 0,0);
Transform poseToLeftT(
poseToLeft.rotation[0], poseToLeft.rotation[1], poseToLeft.rotation[2], poseToLeft.translation[0],
poseToLeft.rotation[3], poseToLeft.rotation[4], poseToLeft.rotation[5], poseToLeft.translation[1],
poseToLeft.rotation[6], poseToLeft.rotation[7], poseToLeft.rotation[8], poseToLeft.translation[2]);
poseToLeftT = realsense2PoseRotation_ * poseToLeftT;
UINFO("poseToLeft = %s", poseToLeftT.prettyPrint().c_str());
Transform poseToIMUT(
poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
poseToIMU.rotation[3], poseToIMU.rotation[4], poseToIMU.rotation[5], poseToIMU.translation[1],
poseToIMU.rotation[6], poseToIMU.rotation[7], poseToIMU.rotation[8], poseToIMU.translation[2]);
poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
Transform poseToIMUT(
poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
poseToIMU.rotation[3], poseToIMU.rotation[4], poseToIMU.rotation[5], poseToIMU.translation[1],
poseToIMU.rotation[6], poseToIMU.rotation[7], poseToIMU.rotation[8], poseToIMU.translation[2]);
poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
imuLocalTransform_ = poseToIMUT;
if(this->getLocalTransform().rotation().r13() == 1.0f &&
this->getLocalTransform().rotation().r21() == -1.0f &&
this->getLocalTransform().rotation().r32() == -1.0f)
{
UWARN("Detected optical rotation in local transform, removing it for convenience to match realsense2 poses.");
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse());
}
stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
imuLocalTransform_ = poseToIMUT;
}
else
{
// Set imu transform based on the left camera instead of pose
rs2_extrinsics leftToIMU = profilesPerSensor[0][1].get_extrinsics_to(profilesPerSensor[0][2]);
Transform leftToIMUT(
leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
stereoModel_.setLocalTransform(this->getLocalTransform());
}
if(rectifyImages_ && !stereoModel_.isValidForRectification())
{
UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
return false;
}
}
std::function<void(rs2::frame)> multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame);};
std::function<void(rs2::frame)> multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame);};
for (unsigned int i=0; i<sensors.size(); ++i)
for (unsigned int i=0; i<sensors.size(); ++i)
{
if(profilesPerSensor[i].size())
{
if(profilesPerSensor[i].size())
UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
sensors[i].open(profilesPerSensor[i]);
if(sensors[i].is<rs2::depth_sensor>())
{
UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
sensors[i].open(profilesPerSensor[i]);
sensors[i].start(multiple_message_callback_function);
auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
depth_scale_meters_ = depth_sensor.get_depth_scale();
}
sensors[i].start(multiple_message_callback_function);
}
}
@@ -844,26 +935,26 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
}
data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
IMU imu;
unsigned int confidence = 0;
getPoseAndIMU(frameset.get_timestamp(), info->odomPose, confidence, imu);
if(!info->odomPose.isNull())
{
info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
}
if(!imu.empty())
{
data.setIMU(imu);
}
}
else
{
UERROR("Not received depth and rgb");
}
IMU imu;
unsigned int confidence = 0;
getPoseAndIMU(frameset.get_timestamp(), info->odomPose, confidence, imu);
if(odometryProvided_ && !info->odomPose.isNull())
{
info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
}
if(!imu.empty())
{
data.setIMU(imu);
}
}
else
{

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

@@ -163,11 +163,10 @@ IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform
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);
static double deg2rad = 0.017453293;
Eigen::Vector4d accT = Eigen::Vector4d(imuData.linear_acceleration.v[0], imuData.linear_acceleration.v[1], imuData.linear_acceleration.v[2], 1);
Eigen::Vector4d gyrT = Eigen::Vector4d(imuData.angular_velocity.v[0]*deg2rad, imuData.angular_velocity.v[1]*deg2rad, imuData.angular_velocity.v[2]*deg2rad, 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],
@@ -182,6 +181,7 @@ IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform
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,

1
corelib/src/imufilter/MadgwickFilter.cpp
View File

@@ -312,7 +312,6 @@ void MadgwickFilter::updateImpl(
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;
}

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

@@ -195,7 +195,7 @@ Transform OdometryF2M::computeTransform(
info->type = 0;
}
if(!data.imu().empty())
if(sba_ && sba_->gravitySigma() > 0.0f && !data.imu().empty())
{
if(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0)
{
@@ -1332,14 +1332,16 @@ Transform OdometryF2M::getClosestIMU(const double & stamp, double & stampDiff) c
if(fabs(imuIterA->first - lastFrame_->getStamp()) <
fabs(imuIterB->first - lastFrame_->getStamp()))
{
imuT = imuIterA->second;
//imuT = imuIterA->second;
stampDiff = fabs(imuIterA->first - lastFrame_->getStamp());
}
else
{
imuT = imuIterB->second;
//imuT = imuIterB->second;
stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
}
//interpolate:
imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
}
return imuT;
}