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

CameraRealSense2: updated how imu are published in inter mode and fixed some local transforms. GUI-VINS: features are shown in Odometry view.

Browse Source
This commit is contained in:
matlabbe
2019-10-13 14:48:11 -04:00
parent 9cb1e4bbc5
commit 1e298dcfa2
9 changed files with 124 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -413,7 +413,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg"); RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg");
// Odometry // Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO"); RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
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, 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, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");

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

@@ -90,7 +90,8 @@ public:
const double & stamp, const double & stamp,
Transform & pose, Transform & pose,
unsigned int & poseConfidence, unsigned int & poseConfidence,
IMU & imu) const; IMU & imu,
int maxWaitTimeMs = 35) const;
#endif #endif
protected: protected:

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

@@ -55,7 +55,7 @@ private:
MsckfVioNoROS * msckf_; MsckfVioNoROS * msckf_;
IMU lastImu_; IMU lastImu_;
ParametersMap parameters_; ParametersMap parameters_;
Transform flipXY_; Transform fixPoseRotation_;
Transform previousPose_; Transform previousPose_;
bool initGravity_; bool initGravity_;
#endif #endif

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

@@ -216,45 +216,45 @@ void CameraRealSense2::imu_callback(rs2::frame frame)
UScopeMutex sm(imuMutex_); UScopeMutex sm(imuMutex_);
if(stream == RS2_STREAM_GYRO) if(stream == RS2_STREAM_GYRO)
{ {
gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(hostStartStamp_ == 0?UTimer::now():frame.get_timestamp(), crnt_reading)); gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
if(gyroBuffer_.size() > 100) if(gyroBuffer_.size() > 1000)
{ {
gyroBuffer_.erase(gyroBuffer_.begin()); gyroBuffer_.erase(gyroBuffer_.begin());
} }
} }
else else
{ {
accBuffer_.insert(accBuffer_.end(), std::make_pair(hostStartStamp_ == 0?UTimer::now():frame.get_timestamp(), crnt_reading)); accBuffer_.insert(accBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
if(accBuffer_.size() > 100) if(accBuffer_.size() > 1000)
{ {
accBuffer_.erase(accBuffer_.begin()); accBuffer_.erase(accBuffer_.begin());
} }
} }
} }
// See https://github.com/IntelRealSense/realsense-ros/blob/2a45f09003c98a5bdf39ee89df032bdb9c9bcd2d/realsense2_camera/src/base_realsense_node.cpp#L1397-L1404
Transform CameraRealSense2::realsense2PoseRotation_ = Transform( Transform CameraRealSense2::realsense2PoseRotation_ = Transform(
0, 0,-1,0, 0, 0,-1,0,
-1, 0, 0,0, -1, 0, 0,0,
0, 1, 0,0); 0, 1, 0,0);
Transform CameraRealSense2::realsense2PoseRotationInv_ = realsense2PoseRotation_.inverse();
void CameraRealSense2::pose_callback(rs2::frame frame) void CameraRealSense2::pose_callback(rs2::frame frame)
{ {
rs2_pose pose = frame.as<rs2::pose_frame>().get_pose_data(); rs2_pose pose = frame.as<rs2::pose_frame>().get_pose_data();
// See https://github.com/IntelRealSense/realsense-ros/blob/2a45f09003c98a5bdf39ee89df032bdb9c9bcd2d/realsense2_camera/src/base_realsense_node.cpp#L1397-L1404
Transform poseT = Transform( Transform poseT = Transform(
pose.translation.x, -pose.translation.z,
-pose.translation.x,
pose.translation.y, pose.translation.y,
pose.translation.z, -pose.rotation.z,
pose.rotation.x, -pose.rotation.x,
pose.rotation.y, pose.rotation.y,
pose.rotation.z,
pose.rotation.w); 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); UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
UScopeMutex sm(poseMutex_); UScopeMutex sm(poseMutex_);
poseBuffer_.insert(poseBuffer_.end(), std::make_pair(hostStartStamp_ == 0?UTimer::now():frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence))); poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
if(poseBuffer_.size() > 100) if(poseBuffer_.size() > 100)
{ {
poseBuffer_.erase(poseBuffer_.begin()); poseBuffer_.erase(poseBuffer_.begin());
@@ -268,7 +268,7 @@ void CameraRealSense2::frame_callback(rs2::frame frame)
} }
void CameraRealSense2::multiple_message_callback(rs2::frame frame) void CameraRealSense2::multiple_message_callback(rs2::frame frame)
{ {
if(dev_[1]==0 && frame.get_timestamp() < UTimer::now()+1000000000) if(frame.get_timestamp() < UTimer::now()+1000000000)
{ {
// 1) In dual setup, use host time // 1) In dual setup, use host time
// 2) ISSUE: my D435i reports timestamps for images 50 years in the future, // 2) ISSUE: my D435i reports timestamps for images 50 years in the future,
@@ -307,7 +307,8 @@ void CameraRealSense2::getPoseAndIMU(
const double & stamp, const double & stamp,
Transform & pose, Transform & pose,
unsigned int & poseConfidence, unsigned int & poseConfidence,
IMU & imu) const IMU & imu,
int maxWaitTimeMs) const
{ {
pose.setNull(); pose.setNull();
imu = IMU(); imu = IMU();
@@ -317,14 +318,12 @@ void CameraRealSense2::getPoseAndIMU(
return; return;
} }
int maxWaitTime = 35;
// Interpolate pose // Interpolate pose
if(!poseBuffer_.empty()) if(!poseBuffer_.empty())
{ {
poseMutex_.lock(); poseMutex_.lock();
int waitTry = 0; int waitTry = 0;
while(poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime) while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
{ {
poseMutex_.unlock(); poseMutex_.unlock();
++waitTry; ++waitTry;
@@ -333,7 +332,10 @@ void CameraRealSense2::getPoseAndIMU(
} }
if(poseBuffer_.rbegin()->first < stamp) if(poseBuffer_.rbegin()->first < stamp)
{ {
UWARN("Could not find poses to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, poseBuffer_.rbegin()->first); if(maxWaitTimeMs > 0)
{
UWARN("Could not find poses to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
}
} }
else else
{ {
@@ -370,7 +372,7 @@ void CameraRealSense2::getPoseAndIMU(
{ {
imuMutex_.lock(); imuMutex_.lock();
int waitTry = 0; int waitTry = 0;
while(accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime) while(maxWaitTimeMs > 0 && accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
{ {
imuMutex_.unlock(); imuMutex_.unlock();
++waitTry; ++waitTry;
@@ -379,7 +381,10 @@ void CameraRealSense2::getPoseAndIMU(
} }
if(accBuffer_.rbegin()->first < stamp) if(accBuffer_.rbegin()->first < stamp)
{ {
UWARN("Could not find acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, accBuffer_.rbegin()->first); if(maxWaitTimeMs>0)
{
UWARN("Could not find acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, accBuffer_.rbegin()->first);
}
imuMutex_.unlock(); imuMutex_.unlock();
return; return;
} }
@@ -423,7 +428,7 @@ void CameraRealSense2::getPoseAndIMU(
{ {
imuMutex_.lock(); imuMutex_.lock();
int waitTry = 0; int waitTry = 0;
while(gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime) while(maxWaitTimeMs>0 && gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
{ {
imuMutex_.unlock(); imuMutex_.unlock();
++waitTry; ++waitTry;
@@ -432,7 +437,10 @@ void CameraRealSense2::getPoseAndIMU(
} }
if(gyroBuffer_.rbegin()->first < stamp) if(gyroBuffer_.rbegin()->first < stamp)
{ {
UWARN("Could not find gyro data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, gyroBuffer_.rbegin()->first); if(maxWaitTimeMs>0)
{
UWARN("Could not find gyro data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, gyroBuffer_.rbegin()->first);
}
imuMutex_.unlock(); imuMutex_.unlock();
return; return;
} }
@@ -740,19 +748,19 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
profilesPerSensor[i].push_back(profile); profilesPerSensor[i].push_back(profile);
auto intrinsic = video_profile.get_intrinsics(); auto intrinsic = video_profile.get_intrinsics();
if(pi==0) if(pi==0)
{
// RIGHT FISHEYE
depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
depthStreamProfile = profile;
*depthIntrinsics_ = intrinsic;
}
else
{ {
// LEFT FISHEYE // LEFT FISHEYE
rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0)); rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
rgbStreamProfile = profile; rgbStreamProfile = profile;
*rgbIntrinsics_ = intrinsic; *rgbIntrinsics_ = intrinsic;
} }
else
{
// RIGHT FISHEYE
depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
depthStreamProfile = profile;
*depthIntrinsics_ = intrinsic;
}
added = true; added = true;
} }
else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF) else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
@@ -797,13 +805,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
if(dualMode_) if(dualMode_)
{ {
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
UINFO("Set base to pose");
this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse()); this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse());
UINFO("poseToLeftIR = %s", dualExtrinsics_.prettyPrint().c_str()); UINFO("poseToLeftIR = %s", dualExtrinsics_.prettyPrint().c_str());
if(ir_) Transform baseToCam = this->getLocalTransform()*dualExtrinsics_*opticalTransform;
{ if(!ir_)
this->setLocalTransform(this->getLocalTransform()*dualExtrinsics_*opticalTransform);
}
else
{ {
Transform leftIRToRGB( Transform leftIRToRGB(
depthToRGBExtrinsics_->rotation[0], depthToRGBExtrinsics_->rotation[1], depthToRGBExtrinsics_->rotation[2], depthToRGBExtrinsics_->translation[0], depthToRGBExtrinsics_->rotation[0], depthToRGBExtrinsics_->rotation[1], depthToRGBExtrinsics_->rotation[2], depthToRGBExtrinsics_->translation[0],
@@ -811,11 +817,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
depthToRGBExtrinsics_->rotation[6], depthToRGBExtrinsics_->rotation[7], depthToRGBExtrinsics_->rotation[8], depthToRGBExtrinsics_->translation[2]); depthToRGBExtrinsics_->rotation[6], depthToRGBExtrinsics_->rotation[7], depthToRGBExtrinsics_->rotation[8], depthToRGBExtrinsics_->translation[2]);
leftIRToRGB = leftIRToRGB.inverse(); leftIRToRGB = leftIRToRGB.inverse();
UINFO("leftIRToRGB = %s", leftIRToRGB.prettyPrint().c_str()); UINFO("leftIRToRGB = %s", leftIRToRGB.prettyPrint().c_str());
this->setLocalTransform(this->getLocalTransform()*dualExtrinsics_*opticalTransform*leftIRToRGB); baseToCam *= leftIRToRGB;
} }
UASSERT(profilesPerSensor.size()>=2); UASSERT(profilesPerSensor.size()>=2);
UASSERT(profilesPerSensor.back().size() == 3); UASSERT(profilesPerSensor.back().size() == 3);
rs2_extrinsics poseToIMU = profilesPerSensor.back()[2].get_extrinsics_to(profilesPerSensor.back()[0]); rs2_extrinsics poseToIMU = profilesPerSensor.back()[0].get_extrinsics_to(profilesPerSensor.back()[2]);
Transform poseToIMUT( Transform poseToIMUT(
poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0], poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
@@ -824,9 +830,9 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
poseToIMUT = realsense2PoseRotation_ * poseToIMUT; poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str()); UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
UINFO("PoseToCam = %s", this->getLocalTransform().prettyPrint().c_str()); UINFO("BaseToCam = %s", baseToCam.prettyPrint().c_str());
model_.setLocalTransform(this->getLocalTransform()); model_.setLocalTransform(baseToCam);
imuLocalTransform_ = poseToIMUT; imuLocalTransform_ = this->getLocalTransform() * poseToIMUT;
} }
if(ir_ && !irDepth_ && profilesPerSensor.size() >= 2 && profilesPerSensor[1].size() >= 2) if(ir_ && !irDepth_ && profilesPerSensor.size() >= 2 && profilesPerSensor[1].size() >= 2)
@@ -900,19 +906,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
} }
// Get extrinsics with pose as the base frame: // Get extrinsics with pose as the base frame:
// 0=Right fisheye // 0=Left fisheye
// 1=Left fisheye // 1=Right fisheye
// 2=GYRO // 2=GYRO
// 3=ACC // 3=ACC
// 4=POSE // 4=POSE
UASSERT(profilesPerSensor[0].size() == 5); UASSERT(profilesPerSensor[0].size() == 5);
if(odometryProvided_) if(odometryProvided_)
{ {
rs2_extrinsics poseToLeft = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][1]); rs2_extrinsics poseToLeft = profilesPerSensor[0][0].get_extrinsics_to(profilesPerSensor[0][4]);
rs2_extrinsics poseToIMU = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][2]); rs2_extrinsics poseToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][4]);
Transform realsense2_pose_rotation(0, 0,-1,0,
-1, 0, 0,0,
0, 1, 0,0);
Transform poseToLeftT( Transform poseToLeftT(
poseToLeft.rotation[0], poseToLeft.rotation[1], poseToLeft.rotation[2], poseToLeft.translation[0], 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[3], poseToLeft.rotation[4], poseToLeft.rotation[5], poseToLeft.translation[1],
@@ -927,17 +930,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
poseToIMUT = realsense2PoseRotation_ * poseToIMUT; poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str()); UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
UINFO("Removing optical rotation to match realsense2 poses."); UINFO("Set base to pose");
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); this->setLocalTransform(this->getLocalTransform()*poseToLeftT.inverse());
this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse());
stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT); stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
imuLocalTransform_ = poseToIMUT; imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;
} }
else else
{ {
// Set imu transform based on the left camera instead of pose // Set imu transform based on the left camera instead of pose
rs2_extrinsics leftToIMU = profilesPerSensor[0][1].get_extrinsics_to(profilesPerSensor[0][2]); rs2_extrinsics leftToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][0]);
Transform leftToIMUT( Transform leftToIMUT(
leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0], 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[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
@@ -1091,7 +1093,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
{ {
double stamp; double stamp;
// See ISSUE in multiple_message_callback() // See ISSUE in multiple_message_callback()
if(frameset.get_timestamp() > UTimer::now()+1000000000 || hostStartStamp_ == 0) if(frameset.get_timestamp() >= UTimer::now()+1000000000)
{ {
stamp = UTimer::now(); stamp = UTimer::now();
} }
@@ -1218,20 +1220,25 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
IMU imu; IMU imu;
unsigned int confidence = 0; unsigned int confidence = 0;
double imuStamp = hostStartStamp_==0?stamp:frameset.get_timestamp()> UTimer::now()+1000000000?stamp*1000.0:frameset.get_timestamp(); double imuStamp = frameset.get_timestamp()> UTimer::now()+1000000000?stamp*1000.0:frameset.get_timestamp();
getPoseAndIMU(imuStamp, info->odomPose, confidence, imu); getPoseAndIMU(imuStamp, info->odomPose, confidence, imu);
if(odometryProvided_ && !info->odomPose.isNull()) if(odometryProvided_ && !info->odomPose.isNull())
{ {
// Transform in base frame (local transform should contain base to pose transform)
info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse();
info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001; info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence); info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence); info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
} }
if(!imu.empty()) if(!imu.empty() && !publishInterIMU_)
{ {
data.setIMU(imu); data.setIMU(imu);
}
if(publishInterIMU_ && lastImuStamp_ > 0.0) else if(publishInterIMU_ && !gyroBuffer_.empty())
{
if(lastImuStamp_ > 0.0)
{ {
UASSERT(imuStamp > lastImuStamp_); UASSERT(imuStamp > lastImuStamp_);
imuMutex_.lock(); imuMutex_.lock();
@@ -1241,8 +1248,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
{ {
++iterA; ++iterA;
} }
if(iterB != gyroBuffer_.end())
{
++iterB;
}
if(iterA != iterB) if(iterA != iterB)
{ {
int pub = 0;
for(;iterA != iterB;++iterA) for(;iterA != iterB;++iterA)
{ {
Transform tmp; Transform tmp;
@@ -1251,8 +1263,14 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
if(!imuTmp.empty()) if(!imuTmp.empty())
{ {
UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0)); UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
pub++;
}
else
{
break;
} }
} }
UDEBUG("inter imu published=%d, %f -> %f", pub, lastImuStamp_, imuStamp);
} }
imuMutex_.unlock(); imuMutex_.unlock();
} }

46
corelib/src/odometry/OdometryMSCKF.cpp
View File

@@ -111,7 +111,10 @@ public:
cam1_intrinsics[2] = rectified?model.right().cx():model.right().K_raw().at<double>(0,2); cam1_intrinsics[2] = rectified?model.right().cx():model.right().K_raw().at<double>(0,2);
cam1_intrinsics[3] = rectified?model.right().cy():model.right().K_raw().at<double>(1,2); cam1_intrinsics[3] = rectified?model.right().cy():model.right().K_raw().at<double>(1,2);
UINFO("Local transform=%s", model.localTransform().prettyPrint().c_str());
UINFO("imuLocalTransform=%s", imuLocalTransform.prettyPrint().c_str());
Transform imuCam = model.localTransform().inverse() * imuLocalTransform; Transform imuCam = model.localTransform().inverse() * imuLocalTransform;
UINFO("imuCam=%s", imuCam.prettyPrint().c_str());
cv::Mat T_imu_cam0 = imuCam.dataMatrix(); cv::Mat T_imu_cam0 = imuCam.dataMatrix();
cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3))); cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
cv::Vec3d t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3)); cv::Vec3d t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3));
@@ -130,7 +133,7 @@ public:
{ {
cam0cam1 = model.stereoTransform(); cam0cam1 = model.stereoTransform();
} }
UINFO("cam0cam1=%s", cam0cam1.prettyPrint().c_str());
UASSERT(!cam0cam1.isNull()); UASSERT(!cam0cam1.isNull());
Transform imuCam1 = cam0cam1 * imuCam; Transform imuCam1 = cam0cam1 * imuCam;
cv::Mat T_imu_cam1 = imuCam1.dataMatrix(); cv::Mat T_imu_cam1 = imuCam1.dataMatrix();
@@ -423,7 +426,10 @@ public:
state_server.state_cov(i, i) = extrinsic_translation_cov; state_server.state_cov(i, i) = extrinsic_translation_cov;
// Transformation offsets between the frames involved. // Transformation offsets between the frames involved.
UINFO("Local transform=%s", model.localTransform().prettyPrint().c_str());
UINFO("imuLocalTransform=%s", imuLocalTransform.prettyPrint().c_str());
Transform imuCam = model.localTransform().inverse() * imuLocalTransform; Transform imuCam = model.localTransform().inverse() * imuLocalTransform;
UINFO("imuCam=%s", imuCam.prettyPrint().c_str());
Eigen::Isometry3d T_imu_cam0(imuCam.toEigen4d()); Eigen::Isometry3d T_imu_cam0(imuCam.toEigen4d());
Eigen::Isometry3d T_cam0_imu = T_imu_cam0.inverse(); Eigen::Isometry3d T_cam0_imu = T_imu_cam0.inverse();
@@ -441,8 +447,9 @@ public:
{ {
cam0cam1 = model.stereoTransform(); cam0cam1 = model.stereoTransform();
} }
UINFO("cam0cam1=%s", cam0cam1.prettyPrint().c_str());
msckf_vio::CAMState::T_cam0_cam1 = cam0cam1.toEigen3d().matrix(); msckf_vio::CAMState::T_cam0_cam1 = cam0cam1.toEigen3d().matrix();
msckf_vio::IMUState::T_imu_body = Transform::getIdentity().toEigen3d().matrix(); msckf_vio::IMUState::T_imu_body = imuLocalTransform.toEigen3d().matrix();
// Maximum number of camera states to be stored // Maximum number of camera states to be stored
Parameters::parse(parameters_, Parameters::kOdomMSCKFMaxCamStateSize(), max_cam_state_size); //30 Parameters::parse(parameters_, Parameters::kOdomMSCKFMaxCamStateSize(), max_cam_state_size); //30
@@ -748,7 +755,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
imageProcessor_(0), imageProcessor_(0),
msckf_(0), msckf_(0),
parameters_(parameters), parameters_(parameters),
flipXY_(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0), fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0),
previousPose_(Transform::getIdentity()), previousPose_(Transform::getIdentity()),
initGravity_(false) initGravity_(false)
#endif #endif
@@ -889,19 +896,21 @@ Transform OdometryMSCKF::computeTransform(
pcl::PointCloud<pcl::PointXYZ>::Ptr localMap; pcl::PointCloud<pcl::PointXYZ>::Ptr localMap;
nav_msgs::Odometry odom = msckf_->featureCallback2(measurements, localMap); nav_msgs::Odometry odom = msckf_->featureCallback2(measurements, localMap);
Transform p = Transform( if( odom.pose.pose.orientation.x != 0.0f ||
odom.pose.pose.position.x, odom.pose.pose.orientation.y != 0.0f ||
odom.pose.pose.position.y, odom.pose.pose.orientation.z != 0.0f ||
odom.pose.pose.position.z, odom.pose.pose.orientation.w != 0.0f)
odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w);
if(!p.isNull())
{ {
// pose in rtabmap/ros coordinates Transform p = Transform(
p = flipXY_*p*lastImu_.localTransform(); odom.pose.pose.position.x,
odom.pose.pose.position.y,
odom.pose.pose.position.z,
odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w);
p = fixPoseRotation_*p;
if(this->getPose().rotation().isIdentity()) if(this->getPose().rotation().isIdentity())
{ {
@@ -956,10 +965,10 @@ Transform OdometryMSCKF::computeTransform(
{ {
if(localMap.get() && localMap->size()) if(localMap.get() && localMap->size())
{ {
Eigen::Affine3f flip = (this->getPose()*previousPoseInv*flipXY_).toEigen3f(); Eigen::Affine3f fixRot = fixPoseRotation_.toEigen3f();
for(unsigned int i=0; i<localMap->size(); ++i) for(unsigned int i=0; i<localMap->size(); ++i)
{ {
pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), flip); pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), fixRot);
info->localMap.insert(std::make_pair(i, cv::Point3f(pt.x, pt.y, pt.z))); info->localMap.insert(std::make_pair(i, cv::Point3f(pt.x, pt.y, pt.z)));
} }
} }
@@ -980,9 +989,8 @@ Transform OdometryMSCKF::computeTransform(
} }
} }
} }
UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
} }
UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
} }
} }

12
corelib/src/odometry/OdometryVINS.cpp
View File

@@ -355,6 +355,8 @@ Transform OdometryVINS::computeTransform(
Vector3d acc(dx, dy, dz); Vector3d acc(dx, dy, dz);
Vector3d gyr(rx, ry, rz); Vector3d gyr(rx, ry, rz);
UDEBUG("IMU update stamp=%f", data.stamp());
if(vinsEstimator_ != 0) if(vinsEstimator_ != 0)
{ {
vinsEstimator_->inputIMU(t, acc, gyr); vinsEstimator_->inputIMU(t, acc, gyr);
@@ -450,6 +452,7 @@ Transform OdometryVINS::computeTransform(
if(info) if(info)
{ {
info->type = this->getType();
info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1); info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001; info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
@@ -473,7 +476,16 @@ Transform OdometryVINS::computeTransform(
p.z = w_pts_i(2); p.z = w_pts_i(2);
p = util3d::transformPoint(p, fixT); p = util3d::transformPoint(p, fixT);
info->localMap.insert(std::make_pair(it_per_id.feature_id, p)); info->localMap.insert(std::make_pair(it_per_id.feature_id, p));
if(this->imagesAlreadyRectified())
{
cv::Point2f pt;
data.stereoCameraModel().left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y);
info->reg.inliersIDs.push_back(info->newCorners.size());
info->newCorners.push_back(pt);
}
} }
info->features = info->newCorners.size();
info->localMapSize = info->localMap.size(); info->localMapSize = info->localMap.size();
} }
UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str()); UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());

2
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -372,7 +372,7 @@ private:
LoopClosureViewer * _loopClosureViewer; LoopClosureViewer * _loopClosureViewer;
QString _graphSavingFileName; QString _graphSavingFileName;
bool _exportPosesFrame; int _exportPosesFrame;
QMap<int, QString> _exportPosesFileName; QMap<int, QString> _exportPosesFileName;
bool _autoScreenCaptureOdomSync; bool _autoScreenCaptureOdomSync;
bool _autoScreenCaptureRAM; bool _autoScreenCaptureRAM;

6
guilib/src/MainWindow.cpp
View File

@@ -1341,7 +1341,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
else if(odom.info().type == (int)Odometry::kTypeF2F || else if(odom.info().type == (int)Odometry::kTypeF2F ||
odom.info().type == (int)Odometry::kTypeViso2 || odom.info().type == (int)Odometry::kTypeViso2 ||
odom.info().type == (int)Odometry::kTypeFovis || odom.info().type == (int)Odometry::kTypeFovis ||
odom.info().type == (int)Odometry::kTypeMSCKF) odom.info().type == (int)Odometry::kTypeMSCKF ||
odom.info().type == (int)Odometry::kTypeVINS)
{ {
std::vector<cv::KeyPoint> kpts; std::vector<cv::KeyPoint> kpts;
cv::KeyPoint::convert(odom.info().newCorners, kpts, 7); cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
@@ -1389,7 +1390,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if( odom.info().type == (int)Odometry::kTypeF2M || if( odom.info().type == (int)Odometry::kTypeF2M ||
odom.info().type == (int)Odometry::kTypeORBSLAM2 || odom.info().type == (int)Odometry::kTypeORBSLAM2 ||
odom.info().type == (int)Odometry::kTypeMSCKF) odom.info().type == (int)Odometry::kTypeMSCKF ||
odom.info().type == (int)Odometry::kTypeVINS)
{ {
if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown()) if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
{ {

6
guilib/src/ui/preferencesDialog.ui
View File

@@ -7,7 +7,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>976</width> <width>976</width>
<height>900</height> <height>896</height>
</rect> </rect>
</property> </property>
<property name="sizePolicy"> <property name="sizePolicy">
@@ -63,7 +63,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-571</y> <y>-1371</y>
<width>680</width> <width>680</width>
<height>3083</height> <height>3083</height>
</rect> </rect>
@@ -6131,7 +6131,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="14" column="1"> <item row="14" column="1">
<widget class="QSpinBox" name="spinBox_cameraImages_max_imu_rate"> <widget class="QSpinBox" name="spinBox_cameraImages_max_imu_rate">
<property name="toolTip"> <property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string> <string>EuRoC: 200 Hz -&gt; 250 Hz</string>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>99999999</number> <number>99999999</number>