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

Refactored how IMU is used in odometry (if guess is not set, use imu orientation for guess). Changed canProcessIMU() to canProcessAsynIMU() to make it more clear for odometry approaches able to process IMU between image frames (VIO approaches). ZedOC: fixed device closed if imu is not detected.

Browse Source
This commit is contained in:
matlabbe
2021-03-01 19:27:24 -05:00
parent f7bc47572b
commit ea4cc7cb6c
11 changed files with 171 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
corelib/src/Odometry.cpp
View File

@@ -284,6 +284,34 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
// cache imu data
if(!data.imu().empty() && !this->canProcessAsyncIMU())
{
if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
// orientation includes roll and pitch but not yaw in local transform
Transform imuT = Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse();
if( this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
this->framesProcessed() == 0)
{
Eigen::Quaterniond imuQuat = imuT.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());
this->reset(newFramePose);
}
imus_.insert(std::make_pair(data.stamp(), imuT));
if(imus_.size() > 1000)
{
imus_.erase(imus_.begin());
}
}
}
if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
{
if(data.stereoCameraModel().isValidForRectification())
@@ -386,21 +414,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
// cache imu data
if(!data.imu().empty())
{
if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
// orientation includes roll and pitch but not yaw in local transform
imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
if(imus_.size() > 1000)
{
imus_.erase(imus_.begin());
}
}
}
// 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();
@@ -447,7 +460,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
guess = guessIn;
}
else if(!data.imu().empty() && !imus_.empty())
else if(!imus_.empty())
{
// replace orientation guess with IMU (if available)
imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
@@ -458,6 +471,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
if(_force3DoF)
{
guess = guess.to3DoF();
}
}
}
@@ -523,7 +540,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
}
else
else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty()))
{
t = this->computeTransform(data, guess, info);
}

53
corelib/src/camera/CameraStereoZedOC.cpp
View File

@@ -307,7 +307,7 @@ bool initCalibration(std::string calibration_file, cv::Size image_size, rtabmap:
// (Linux only) Safety check A: Wrong "." or "," reading in file conf.
#ifndef _WIN32
if (right_cam_k1 == 0 && left_cam_k1 == 0 && left_cam_k2 == 0 && right_cam_k2 == 0) {
std::cout << "ZED File invalid" << std::endl;
UERROR("ZED File invalid");
std::string cmd = "rm " + calibration_file;
int res = system(cmd.c_str());
@@ -596,15 +596,14 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
zed_ = new sl_oc::video::VideoCapture(params);
if( !zed_->initializeVideo(usbDevice_) )
{
std::cerr << "Cannot open camera video capture" << std::endl;
std::cerr << "See verbosity level for more details." << std::endl;
UERROR("Cannot open camera video capture. Set log level <= info for more details.");
delete zed_;
zed_ = 0;
return false;
}
int sn = zed_->getSerialNumber();
std::cout << "Connected to camera sn: " << sn << std::endl;
UINFO("Connected to camera sn: %d", sn);
// <---- Create Video Capture
// ----> Retrieve calibration file from Stereolabs server
@@ -614,12 +613,12 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
// Download camera calibration file
if( !downloadCalibrationFile(serial_number, calibration_file) )
{
std::cerr << "Could not load calibration file from Stereolabs servers" << std::endl;
UERROR("Could not load calibration file from Stereolabs servers");
delete zed_;
zed_ = 0;
return false;
}
std::cout << "Calibration file found. Loading..." << std::endl;
UINFO("Calibration file found. Loading...");
// ----> Frame size
int w,h;
@@ -629,8 +628,11 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
// ----> Initialize calibration
if(initCalibration(calibration_file, cv::Size(w/2,h), stereoModel_, this->getLocalTransform()))
{
std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl;
std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl;
if(ULogger::level() <= ULogger::kInfo)
{
std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl;
std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl;
}
stereoModel_.initRectificationMap();
}
@@ -638,37 +640,42 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
sensors_ = new sl_oc::sensors::SensorCapture((sl_oc::VERBOSITY)params.verbose);
if( !sensors_->initializeSensors(serial_number) ) // Note: we use the serial number acquired by the VideoCapture object
{
std::cerr << "Cannot open sensors capture" << std::endl;
std::cerr << "Try to enable verbose to get more info" << std::endl;
UERROR("Cannot open sensors capture. Set log level <= info for more details.");
delete sensors_;
sensors_ = 0;
}
else
{
std::cout << "Sensors Capture connected to camera sn: " << sensors_->getSerialNumber() << std::endl;
UINFO("Sensors Capture connected to camera sn: %d", sensors_->getSerialNumber());
UINFO("Wait max 5 sec to see if the camera has imu...");
// Check is IMU data is available
const sl_oc::sensors::data::Imu& imu = sensors_->getLastIMUData();
if(imu.valid != sl_oc::sensors::data::Imu::NEW_VAL)
UTimer timer;
while(timer.elapsed() < 5 &&
sensors_->getLastIMUData().valid != sl_oc::sensors::data::Imu::NEW_VAL)
{
UINFO("CameraStereoZEDOC: Camera doesn't have IMU sensor");
delete sensors_;
sensors_ = 0;
// wait 5 sec to see if we can get an imu stream...
uSleep(100);
}
if(timer.elapsed() > 5)
{
UINFO("Camera doesn't have IMU sensor");
}
else
{
UINFO("Camera has IMU");
// Start the sensor capture thread. Note: since sensor data can be retrieved at 400Hz and video data frequency is
// minor (max 100Hz), we use a separated thread for sensors.
// Transform based on ZED2: x->down, y->right, z->backward
Transform imuLocalTransform_ = this->getLocalTransform() * Transform(0,1,0,0, 1,0,0,0, 0,0,-1,0);
std::cout << imuLocalTransform_ << std::endl;
//std::cout << imuLocalTransform_ << std::endl;
imuThread_ = new ZedOCThread(sensors_, imuLocalTransform_);
// <---- Create Sensors Capture
// ----> Enable video/sensors synchronization
if(!zed_->enableSensorSync(sensors_))
{
UWARN("CameraStereoZEDOC: Failed to start synchronization");
UWARN("Failed to enable image/imu synchronization");
}
// <---- Enable video/sensors synchronization
@@ -712,7 +719,8 @@ SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
{
UTimer timer;
bool imuReceived = true;
bool imuReceived = imuThread_!=0;
bool warned = false;
do
{
const sl_oc::video::Frame frame = zed_->getLastFrame();
@@ -729,6 +737,11 @@ SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
{
imuThread_->getIMU(stamp, imu, 10);
imuReceived = !imu.empty();
if(!imuReceived && !warned && timer.elapsed() > 1.0)
{
UWARN("Waiting for synchronized imu (this can take several seconds when camera has been just started)...");
warned = true;
}
}
if(imuReceived)

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

@@ -73,7 +73,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
map_(new Signature(-1)),
lastFrame_(new Signature(1)),
lastFrameOldestNewId_(0),
initGravity_(false),
bundleSeq_(0),
sba_(0)
{
@@ -181,27 +180,19 @@ OdometryF2M::~OdometryF2M()
void OdometryF2M::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
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;
}
initGravity_ = false;
}
bool OdometryF2M::canProcessIMU() const
{
return sba_ && sba_->gravitySigma() > 0.0f;
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;
}
// return not null transform if odometry is correctly computed
@@ -220,33 +211,9 @@ Transform OdometryF2M::computeTransform(
}
Transform imuT;
if(sba_ && sba_->gravitySigma() > 0.0f && !data.imu().empty())
if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
{
if(imus().empty())
{
UERROR("IMU received doesn't have orientation set, it is ignored. If you are using RTAB-Map standalone, enable IMU filtering in Preferences->Source panel. On ROS, use \"imu_filter_madgwick\" or \"imu_complementary_filter\" packages to compute the orientation.");
}
else
{
imuT = Transform::getTransform(imus(), data.stamp());
if(this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f)
{
if(!imuT.isNull())
{
Eigen::Quaterniond imuQuat = imuT.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;
}
imuT = Transform::getTransform(imus(), data.stamp());
}
RegistrationInfo regInfo;