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

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

@@ -67,7 +67,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() = 0; virtual Odometry::Type getType() = 0;
virtual bool canProcessRawImages() const {return false;} virtual bool canProcessRawImages() const {return false;}
virtual bool canProcessIMU() const {return false;} virtual bool canProcessAsyncIMU() const {return false;}
//getters //getters
const Transform & getPose() const {return _pose;} const Transform & getPose() const {return _pose;}

2
corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

@@ -50,7 +50,6 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
const Signature & getMap() const {return *map_;} const Signature & getMap() const {return *map_;}
const Signature & getLastFrame() const {return *lastFrame_;} const Signature & getLastFrame() const {return *lastFrame_;}
virtual bool canProcessIMU() const;
virtual Odometry::Type getType() {return Odometry::kTypeF2M;} virtual Odometry::Type getType() {return Odometry::kTypeF2M;}
@@ -79,7 +78,6 @@ private:
Signature * lastFrame_; Signature * lastFrame_;
int lastFrameOldestNewId_; int lastFrameOldestNewId_;
std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_; std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
bool initGravity_;
std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>> std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
std::map<int, Transform> bundlePoses_; std::map<int, Transform> bundlePoses_;

2
corelib/include/rtabmap/core/odometry/OdometryMSCKF.h
View File

@@ -44,7 +44,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeMSCKF;} virtual Odometry::Type getType() {return Odometry::kTypeMSCKF;}
virtual bool canProcessRawImages() const {return true;} virtual bool canProcessRawImages() const {return true;}
virtual bool canProcessIMU() const {return true;} virtual bool canProcessAsyncIMU() const {return true;}
private: private:
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

2
corelib/include/rtabmap/core/odometry/OdometryOkvis.h
View File

@@ -46,7 +46,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeOkvis;} virtual Odometry::Type getType() {return Odometry::kTypeOkvis;}
virtual bool canProcessRawImages() const {return true;} virtual bool canProcessRawImages() const {return true;}
virtual bool canProcessIMU() const {return true;} virtual bool canProcessAsyncIMU() const {return true;}
private: private:
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

2
corelib/include/rtabmap/core/odometry/OdometryVINS.h
View File

@@ -43,7 +43,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeVINS;} virtual Odometry::Type getType() {return Odometry::kTypeVINS;}
virtual bool canProcessRawImages() const {return true;} virtual bool canProcessRawImages() const {return true;}
virtual bool canProcessIMU() const {return true;} virtual bool canProcessAsyncIMU() const {return true;}
private: private:
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

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()); 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(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
{ {
if(data.stereoCameraModel().isValidForRectification()) 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 // KITTI datasets start with stamp=0
double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.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(); 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; guess = guessIn;
} }
else if(!data.imu().empty() && !imus_.empty()) else if(!imus_.empty())
{ {
// replace orientation guess with IMU (if available) // replace orientation guess with IMU (if available)
imuCurrentTransform = Transform::getTransform(imus_, data.stamp()); 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.r11(), orientation.r12(), orientation.r13(), guess.x(),
orientation.r21(), orientation.r22(), orientation.r23(), guess.y(), orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
orientation.r31(), orientation.r32(), orientation.r33(), guess.z()); 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); 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. // (Linux only) Safety check A: Wrong "." or "," reading in file conf.
#ifndef _WIN32 #ifndef _WIN32
if (right_cam_k1 == 0 && left_cam_k1 == 0 && left_cam_k2 == 0 && right_cam_k2 == 0) { 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; std::string cmd = "rm " + calibration_file;
int res = system(cmd.c_str()); 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); zed_ = new sl_oc::video::VideoCapture(params);
if( !zed_->initializeVideo(usbDevice_) ) if( !zed_->initializeVideo(usbDevice_) )
{ {
std::cerr << "Cannot open camera video capture" << std::endl; UERROR("Cannot open camera video capture. Set log level <= info for more details.");
std::cerr << "See verbosity level for more details." << std::endl;
delete zed_; delete zed_;
zed_ = 0; zed_ = 0;
return false; return false;
} }
int sn = zed_->getSerialNumber(); int sn = zed_->getSerialNumber();
std::cout << "Connected to camera sn: " << sn << std::endl; UINFO("Connected to camera sn: %d", sn);
// <---- Create Video Capture // <---- Create Video Capture
// ----> Retrieve calibration file from Stereolabs server // ----> Retrieve calibration file from Stereolabs server
@@ -614,12 +613,12 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
// Download camera calibration file // Download camera calibration file
if( !downloadCalibrationFile(serial_number, 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_; delete zed_;
zed_ = 0; zed_ = 0;
return false; return false;
} }
std::cout << "Calibration file found. Loading..." << std::endl; UINFO("Calibration file found. Loading...");
// ----> Frame size // ----> Frame size
int w,h; int w,h;
@@ -629,8 +628,11 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
// ----> Initialize calibration // ----> Initialize calibration
if(initCalibration(calibration_file, cv::Size(w/2,h), stereoModel_, this->getLocalTransform())) if(initCalibration(calibration_file, cv::Size(w/2,h), stereoModel_, this->getLocalTransform()))
{ {
std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl; if(ULogger::level() <= ULogger::kInfo)
std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl; {
std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl;
std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl;
}
stereoModel_.initRectificationMap(); 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); 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 if( !sensors_->initializeSensors(serial_number) ) // Note: we use the serial number acquired by the VideoCapture object
{ {
std::cerr << "Cannot open sensors capture" << std::endl; UERROR("Cannot open sensors capture. Set log level <= info for more details.");
std::cerr << "Try to enable verbose to get more info" << std::endl;
delete sensors_; delete sensors_;
sensors_ = 0; sensors_ = 0;
} }
else 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 // Check is IMU data is available
const sl_oc::sensors::data::Imu& imu = sensors_->getLastIMUData(); UTimer timer;
if(imu.valid != sl_oc::sensors::data::Imu::NEW_VAL) while(timer.elapsed() < 5 &&
sensors_->getLastIMUData().valid != sl_oc::sensors::data::Imu::NEW_VAL)
{ {
UINFO("CameraStereoZEDOC: Camera doesn't have IMU sensor"); // wait 5 sec to see if we can get an imu stream...
delete sensors_; uSleep(100);
sensors_ = 0; }
if(timer.elapsed() > 5)
{
UINFO("Camera doesn't have IMU sensor");
} }
else else
{ {
UINFO("Camera has IMU");
// Start the sensor capture thread. Note: since sensor data can be retrieved at 400Hz and video data frequency is // 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. // minor (max 100Hz), we use a separated thread for sensors.
// Transform based on ZED2: x->down, y->right, z->backward // 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); 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_); imuThread_ = new ZedOCThread(sensors_, imuLocalTransform_);
// <---- Create Sensors Capture // <---- Create Sensors Capture
// ----> Enable video/sensors synchronization // ----> Enable video/sensors synchronization
if(!zed_->enableSensorSync(sensors_)) if(!zed_->enableSensorSync(sensors_))
{ {
UWARN("CameraStereoZEDOC: Failed to start synchronization"); UWARN("Failed to enable image/imu synchronization");
} }
// <---- Enable video/sensors synchronization // <---- Enable video/sensors synchronization
@@ -712,7 +719,8 @@ SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
{ {
UTimer timer; UTimer timer;
bool imuReceived = true; bool imuReceived = imuThread_!=0;
bool warned = false;
do do
{ {
const sl_oc::video::Frame frame = zed_->getLastFrame(); const sl_oc::video::Frame frame = zed_->getLastFrame();
@@ -729,6 +737,11 @@ SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
{ {
imuThread_->getIMU(stamp, imu, 10); imuThread_->getIMU(stamp, imu, 10);
imuReceived = !imu.empty(); 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) if(imuReceived)

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

@@ -73,7 +73,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
map_(new Signature(-1)), map_(new Signature(-1)),
lastFrame_(new Signature(1)), lastFrame_(new Signature(1)),
lastFrameOldestNewId_(0), lastFrameOldestNewId_(0),
initGravity_(false),
bundleSeq_(0), bundleSeq_(0),
sba_(0) sba_(0)
{ {
@@ -181,27 +180,19 @@ OdometryF2M::~OdometryF2M()
void OdometryF2M::reset(const Transform & initialPose) void OdometryF2M::reset(const Transform & initialPose)
{ {
Odometry::reset(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 UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
{ Odometry::reset(initialPose);
return sba_ && sba_->gravitySigma() > 0.0f; *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 // return not null transform if odometry is correctly computed
@@ -220,33 +211,9 @@ Transform OdometryF2M::computeTransform(
} }
Transform imuT; Transform imuT;
if(sba_ && sba_->gravitySigma() > 0.0f && !data.imu().empty()) if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
{ {
if(imus().empty()) imuT = Transform::getTransform(imus(), data.stamp());
{
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;
}
} }
RegistrationInfo regInfo; RegistrationInfo regInfo;

2
guilib/src/PreferencesDialog.cpp
View File

@@ -6022,7 +6022,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
{ {
UDEBUG("ZEDOC"); UDEBUG("ZEDOC");
camera = new CameraStereoZedOC( camera = new CameraStereoZedOC(
this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()), this->getSourceDevice().isEmpty()?-1:atoi(this->getSourceDevice().toStdString().c_str()),
_ui->comboBox_stereoZedOC_resolution->currentIndex(), _ui->comboBox_stereoZedOC_resolution->currentIndex(),
this->getGeneralInputRate(), this->getGeneralInputRate(),
this->getSourceLocalTransform()); this->getSourceLocalTransform());

12
tools/CameraRGBD/main.cpp
View File

@@ -213,7 +213,7 @@ int main(int argc, char * argv[])
UERROR("Not built with Freenect support..."); UERROR("Not built with Freenect support...");
exit(-1); exit(-1);
} }
camera = new rtabmap::CameraFreenect(uStr2Int(deviceId)); camera = new rtabmap::CameraFreenect(deviceId.empty()?0:uStr2Int(deviceId));
} }
else if(driver == 3) else if(driver == 3)
{ {
@@ -240,7 +240,7 @@ int main(int argc, char * argv[])
UERROR("Not built with Freenect2 support..."); UERROR("Not built with Freenect2 support...");
exit(-1); exit(-1);
} }
camera = new rtabmap::CameraFreenect2(uStr2Int(deviceId), rtabmap::CameraFreenect2::kTypeColor2DepthSD); camera = new rtabmap::CameraFreenect2(deviceId.empty()?0:uStr2Int(deviceId), rtabmap::CameraFreenect2::kTypeColor2DepthSD);
} }
} }
else if(driver == 6) else if(driver == 6)
@@ -268,7 +268,7 @@ int main(int argc, char * argv[])
UERROR("Not built with ZED sdk support..."); UERROR("Not built with ZED sdk support...");
exit(-1); exit(-1);
} }
camera = new rtabmap::CameraStereoZed(uStr2Int(deviceId)); camera = new rtabmap::CameraStereoZed(deviceId.empty()?0:uStr2Int(deviceId));
} }
else if (driver == 9) else if (driver == 9)
{ {
@@ -277,7 +277,7 @@ int main(int argc, char * argv[])
UERROR("Not built with RealSense support..."); UERROR("Not built with RealSense support...");
exit(-1); exit(-1);
} }
camera = new rtabmap::CameraRealSense(uStr2Int(deviceId)); camera = new rtabmap::CameraRealSense(deviceId.empty()?0:uStr2Int(deviceId));
} }
else if (driver == 10) else if (driver == 10)
{ {
@@ -286,7 +286,7 @@ int main(int argc, char * argv[])
UERROR("Not built with Kinect for Windows 2 SDK support..."); UERROR("Not built with Kinect for Windows 2 SDK support...");
exit(-1); exit(-1);
} }
camera = new rtabmap::CameraK4W2(uStr2Int(deviceId)); camera = new rtabmap::CameraK4W2(deviceId.empty()?0:uStr2Int(deviceId));
} }
else if (driver == 11) else if (driver == 11)
{ {
@@ -322,7 +322,7 @@ int main(int argc, char * argv[])
UERROR("Not built with Zed Open Capture support..."); UERROR("Not built with Zed Open Capture support...");
exit(-1); exit(-1);
} }
camera = new rtabmap::CameraStereoZedOC(uStr2Int(deviceId)); camera = new rtabmap::CameraStereoZedOC(deviceId.empty()?-1:uStr2Int(deviceId));
} }
else else
{ {

163
tools/EurocDataset/main.cpp
View File

@@ -330,59 +330,57 @@ int main(int argc, char * argv[])
Odometry * odom = Odometry::create(odomParameters); Odometry * odom = Odometry::create(odomParameters);
std::ifstream imu_file; std::ifstream imu_file;
if(odom->canProcessIMU())
{
// open the IMU file
std::string line;
imu_file.open(pathImu.c_str());
if (!imu_file.good()) {
UERROR("no imu file found at %s",pathImu.c_str());
return -1;
}
int number_of_lines = 0;
while (std::getline(imu_file, line))
++number_of_lines;
printf("No. IMU measurements: %d\n", number_of_lines-1);
if (number_of_lines - 1 <= 0) {
UERROR("no imu messages present in %s", pathImu.c_str());
return -1;
}
// set reading position to second line
imu_file.clear();
imu_file.seekg(0, std::ios::beg);
std::getline(imu_file, line);
if(odomStrategy == Odometry::kTypeMSCKF) // open the IMU file
{ std::string line;
if(seq.compare("MH_01_easy") == 0) imu_file.open(pathImu.c_str());
{ if (!imu_file.good()) {
printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n"); UERROR("no imu file found at %s",pathImu.c_str());
((CameraStereoImages*)cameraThread.camera())->setStartIndex(440); return -1;
}
else if(seq.compare("MH_02_easy") == 0)
{
printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(525);
}
else if(seq.compare("MH_03_medium") == 0)
{
printf("MH_03_medium detected with MSCFK odometry, ignoring first moving 210 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(210);
}
else if(seq.compare("MH_04_difficult") == 0)
{
printf("MH_04_difficult detected with MSCFK odometry, ignoring first moving 250 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(250);
}
else if(seq.compare("MH_05_difficult") == 0)
{
printf("MH_05_difficult detected with MSCFK odometry, ignoring first moving 310 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(310);
}
}
cameraThread.enableIMUFiltering(imuFilter, parameters);
} }
int number_of_lines = 0;
while (std::getline(imu_file, line))
++number_of_lines;
printf("No. IMU measurements: %d\n", number_of_lines-1);
if (number_of_lines - 1 <= 0) {
UERROR("no imu messages present in %s", pathImu.c_str());
return -1;
}
// set reading position to second line
imu_file.clear();
imu_file.seekg(0, std::ios::beg);
std::getline(imu_file, line);
if(odomStrategy == Odometry::kTypeMSCKF)
{
if(seq.compare("MH_01_easy") == 0)
{
printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(440);
}
else if(seq.compare("MH_02_easy") == 0)
{
printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(525);
}
else if(seq.compare("MH_03_medium") == 0)
{
printf("MH_03_medium detected with MSCFK odometry, ignoring first moving 210 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(210);
}
else if(seq.compare("MH_04_difficult") == 0)
{
printf("MH_04_difficult detected with MSCFK odometry, ignoring first moving 250 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(250);
}
else if(seq.compare("MH_05_difficult") == 0)
{
printf("MH_05_difficult detected with MSCFK odometry, ignoring first moving 310 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(310);
}
}
cameraThread.enableIMUFiltering(imuFilter, parameters);
Rtabmap rtabmap; Rtabmap rtabmap;
rtabmap.init(parameters, databasePath); rtabmap.init(parameters, databasePath);
@@ -404,46 +402,43 @@ int main(int argc, char * argv[])
while(data.isValid() && g_forever) while(data.isValid() && g_forever)
{ {
UDEBUG(""); UDEBUG("");
if(odom->canProcessIMU()) // get all IMU measurements till then
{ double t_imu = start;
// get all IMU measurements till then do {
double t_imu = start; std::string line;
do { if (!std::getline(imu_file, line)) {
std::string line; std::cout << std::endl << "Finished parsing IMU." << std::endl << std::flush;
if (!std::getline(imu_file, line)) { break;
std::cout << std::endl << "Finished parsing IMU." << std::endl << std::flush; }
break;
}
std::stringstream stream(line); std::stringstream stream(line);
std::string s; std::string s;
std::getline(stream, s, ',');
std::string nanoseconds = s.substr(s.size() - 9, 9);
std::string seconds = s.substr(0, s.size() - 9);
cv::Vec3d gyr;
for (int j = 0; j < 3; ++j) {
std::getline(stream, s, ','); std::getline(stream, s, ',');
std::string nanoseconds = s.substr(s.size() - 9, 9); gyr[j] = uStr2Double(s);
std::string seconds = s.substr(0, s.size() - 9); }
cv::Vec3d gyr; cv::Vec3d acc;
for (int j = 0; j < 3; ++j) { for (int j = 0; j < 3; ++j) {
std::getline(stream, s, ','); std::getline(stream, s, ',');
gyr[j] = uStr2Double(s); acc[j] = uStr2Double(s);
} }
cv::Vec3d acc; t_imu = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
for (int j = 0; j < 3; ++j) {
std::getline(stream, s, ',');
acc[j] = uStr2Double(s);
}
t_imu = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9; if (t_imu - start + 1 > 0) {
if (t_imu - start + 1 > 0) { SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);
cameraThread.postUpdate(&dataImu);
odom->process(dataImu);
}
SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu); } while (t_imu <= data.stamp());
cameraThread.postUpdate(&dataImu);
odom->process(dataImu);
}
} while (t_imu <= data.stamp());
}
cameraThread.postUpdate(&data, &cameraInfo); cameraThread.postUpdate(&data, &cameraInfo);