From 1e298dcfa25e4b0a10565af1c9e9c49aac06399f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 13 Oct 2019 14:48:11 -0400 Subject: [PATCH] CameraRealSense2: updated how imu are published in inter mode and fixed some local transforms. GUI-VINS: features are shown in Odometry view. --- corelib/include/rtabmap/core/Parameters.h | 2 +- .../rtabmap/core/camera/CameraRealSense2.h | 3 +- .../rtabmap/core/odometry/OdometryMSCKF.h | 2 +- corelib/src/camera/CameraRealSense2.cpp | 128 ++++++++++-------- corelib/src/odometry/OdometryMSCKF.cpp | 46 ++++--- corelib/src/odometry/OdometryVINS.cpp | 12 ++ guilib/include/rtabmap/gui/MainWindow.h | 2 +- guilib/src/MainWindow.cpp | 6 +- guilib/src/ui/preferencesDialog.ui | 6 +- 9 files changed, 124 insertions(+), 83 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index cddfc75a..617ea74b 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -413,7 +413,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg"); // 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, 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)."); diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense2.h b/corelib/include/rtabmap/core/camera/CameraRealSense2.h index 1aa489dd..14ae7a82 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h @@ -90,7 +90,8 @@ public: const double & stamp, Transform & pose, unsigned int & poseConfidence, - IMU & imu) const; + IMU & imu, + int maxWaitTimeMs = 35) const; #endif protected: diff --git a/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h b/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h index 94106cf5..14cc08da 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h +++ b/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h @@ -55,7 +55,7 @@ private: MsckfVioNoROS * msckf_; IMU lastImu_; ParametersMap parameters_; - Transform flipXY_; + Transform fixPoseRotation_; Transform previousPose_; bool initGravity_; #endif diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index a2df52dc..cacf03b7 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -216,45 +216,45 @@ void CameraRealSense2::imu_callback(rs2::frame frame) UScopeMutex sm(imuMutex_); if(stream == RS2_STREAM_GYRO) { - gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(hostStartStamp_ == 0?UTimer::now():frame.get_timestamp(), crnt_reading)); - if(gyroBuffer_.size() > 100) + gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading)); + if(gyroBuffer_.size() > 1000) { gyroBuffer_.erase(gyroBuffer_.begin()); } } else { - accBuffer_.insert(accBuffer_.end(), std::make_pair(hostStartStamp_ == 0?UTimer::now():frame.get_timestamp(), crnt_reading)); - if(accBuffer_.size() > 100) + accBuffer_.insert(accBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading)); + if(accBuffer_.size() > 1000) { 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( 0, 0,-1,0, -1, 0, 0,0, 0, 1, 0,0); -Transform CameraRealSense2::realsense2PoseRotationInv_ = realsense2PoseRotation_.inverse(); void CameraRealSense2::pose_callback(rs2::frame frame) { rs2_pose pose = frame.as().get_pose_data(); + // See https://github.com/IntelRealSense/realsense-ros/blob/2a45f09003c98a5bdf39ee89df032bdb9c9bcd2d/realsense2_camera/src/base_realsense_node.cpp#L1397-L1404 Transform poseT = Transform( - pose.translation.x, + -pose.translation.z, + -pose.translation.x, pose.translation.y, - pose.translation.z, - pose.rotation.x, + -pose.rotation.z, + -pose.rotation.x, pose.rotation.y, - pose.rotation.z, 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(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) { poseBuffer_.erase(poseBuffer_.begin()); @@ -268,7 +268,7 @@ void CameraRealSense2::frame_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 // 2) ISSUE: my D435i reports timestamps for images 50 years in the future, @@ -307,7 +307,8 @@ void CameraRealSense2::getPoseAndIMU( const double & stamp, Transform & pose, unsigned int & poseConfidence, - IMU & imu) const + IMU & imu, + int maxWaitTimeMs) const { pose.setNull(); imu = IMU(); @@ -317,14 +318,12 @@ void CameraRealSense2::getPoseAndIMU( return; } - int maxWaitTime = 35; - // Interpolate pose if(!poseBuffer_.empty()) { poseMutex_.lock(); int waitTry = 0; - while(poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime) + while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs) { poseMutex_.unlock(); ++waitTry; @@ -333,7 +332,10 @@ void CameraRealSense2::getPoseAndIMU( } 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 { @@ -370,7 +372,7 @@ void CameraRealSense2::getPoseAndIMU( { imuMutex_.lock(); int waitTry = 0; - while(accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime) + while(maxWaitTimeMs > 0 && accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs) { imuMutex_.unlock(); ++waitTry; @@ -379,7 +381,10 @@ void CameraRealSense2::getPoseAndIMU( } 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(); return; } @@ -423,7 +428,7 @@ void CameraRealSense2::getPoseAndIMU( { imuMutex_.lock(); int waitTry = 0; - while(gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime) + while(maxWaitTimeMs>0 && gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs) { imuMutex_.unlock(); ++waitTry; @@ -432,7 +437,10 @@ void CameraRealSense2::getPoseAndIMU( } 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(); return; } @@ -740,19 +748,19 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st profilesPerSensor[i].push_back(profile); auto intrinsic = video_profile.get_intrinsics(); if(pi==0) - { - // RIGHT FISHEYE - depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0)); - depthStreamProfile = profile; - *depthIntrinsics_ = intrinsic; - } - else { // LEFT FISHEYE rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0)); rgbStreamProfile = profile; *rgbIntrinsics_ = intrinsic; } + else + { + // RIGHT FISHEYE + depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0)); + depthStreamProfile = profile; + *depthIntrinsics_ = intrinsic; + } added = true; } 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_) { 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()); UINFO("poseToLeftIR = %s", dualExtrinsics_.prettyPrint().c_str()); - if(ir_) - { - this->setLocalTransform(this->getLocalTransform()*dualExtrinsics_*opticalTransform); - } - else + Transform baseToCam = this->getLocalTransform()*dualExtrinsics_*opticalTransform; + if(!ir_) { Transform leftIRToRGB( 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]); leftIRToRGB = leftIRToRGB.inverse(); UINFO("leftIRToRGB = %s", leftIRToRGB.prettyPrint().c_str()); - this->setLocalTransform(this->getLocalTransform()*dualExtrinsics_*opticalTransform*leftIRToRGB); + baseToCam *= leftIRToRGB; } UASSERT(profilesPerSensor.size()>=2); 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( 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; UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str()); - UINFO("PoseToCam = %s", this->getLocalTransform().prettyPrint().c_str()); - model_.setLocalTransform(this->getLocalTransform()); - imuLocalTransform_ = poseToIMUT; + UINFO("BaseToCam = %s", baseToCam.prettyPrint().c_str()); + model_.setLocalTransform(baseToCam); + imuLocalTransform_ = this->getLocalTransform() * poseToIMUT; } 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: - // 0=Right fisheye - // 1=Left fisheye + // 0=Left fisheye + // 1=Right fisheye // 2=GYRO // 3=ACC // 4=POSE UASSERT(profilesPerSensor[0].size() == 5); 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); + rs2_extrinsics poseToLeft = profilesPerSensor[0][0].get_extrinsics_to(profilesPerSensor[0][4]); + rs2_extrinsics poseToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][4]); 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], @@ -927,17 +930,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st poseToIMUT = realsense2PoseRotation_ * poseToIMUT; UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str()); - UINFO("Removing optical rotation to match realsense2 poses."); - Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); - this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse()); + UINFO("Set base to pose"); + this->setLocalTransform(this->getLocalTransform()*poseToLeftT.inverse()); stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT); - imuLocalTransform_ = poseToIMUT; + imuLocalTransform_ = this->getLocalTransform()* 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]); + rs2_extrinsics leftToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][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], @@ -1091,7 +1093,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) { double stamp; // 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(); } @@ -1218,20 +1220,25 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) IMU imu; 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); 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.rowRange(0,3) *= pow(10, 3-(int)confidence); info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence); } - if(!imu.empty()) + if(!imu.empty() && !publishInterIMU_) { data.setIMU(imu); - - if(publishInterIMU_ && lastImuStamp_ > 0.0) + } + else if(publishInterIMU_ && !gyroBuffer_.empty()) + { + if(lastImuStamp_ > 0.0) { UASSERT(imuStamp > lastImuStamp_); imuMutex_.lock(); @@ -1241,8 +1248,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) { ++iterA; } + if(iterB != gyroBuffer_.end()) + { + ++iterB; + } if(iterA != iterB) { + int pub = 0; for(;iterA != iterB;++iterA) { Transform tmp; @@ -1251,8 +1263,14 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) if(!imuTmp.empty()) { UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0)); + pub++; + } + else + { + break; } } + UDEBUG("inter imu published=%d, %f -> %f", pub, lastImuStamp_, imuStamp); } imuMutex_.unlock(); } diff --git a/corelib/src/odometry/OdometryMSCKF.cpp b/corelib/src/odometry/OdometryMSCKF.cpp index 8a6c8def..c4aaca3c 100644 --- a/corelib/src/odometry/OdometryMSCKF.cpp +++ b/corelib/src/odometry/OdometryMSCKF.cpp @@ -111,7 +111,10 @@ public: cam1_intrinsics[2] = rectified?model.right().cx():model.right().K_raw().at(0,2); cam1_intrinsics[3] = rectified?model.right().cy():model.right().K_raw().at(1,2); + UINFO("Local transform=%s", model.localTransform().prettyPrint().c_str()); + UINFO("imuLocalTransform=%s", imuLocalTransform.prettyPrint().c_str()); Transform imuCam = model.localTransform().inverse() * imuLocalTransform; + UINFO("imuCam=%s", imuCam.prettyPrint().c_str()); cv::Mat T_imu_cam0 = imuCam.dataMatrix(); 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)); @@ -130,7 +133,7 @@ public: { cam0cam1 = model.stereoTransform(); } - + UINFO("cam0cam1=%s", cam0cam1.prettyPrint().c_str()); UASSERT(!cam0cam1.isNull()); Transform imuCam1 = cam0cam1 * imuCam; cv::Mat T_imu_cam1 = imuCam1.dataMatrix(); @@ -423,7 +426,10 @@ public: state_server.state_cov(i, i) = extrinsic_translation_cov; // 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; + UINFO("imuCam=%s", imuCam.prettyPrint().c_str()); Eigen::Isometry3d T_imu_cam0(imuCam.toEigen4d()); Eigen::Isometry3d T_cam0_imu = T_imu_cam0.inverse(); @@ -441,8 +447,9 @@ public: { cam0cam1 = model.stereoTransform(); } + UINFO("cam0cam1=%s", cam0cam1.prettyPrint().c_str()); 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 Parameters::parse(parameters_, Parameters::kOdomMSCKFMaxCamStateSize(), max_cam_state_size); //30 @@ -748,7 +755,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) : imageProcessor_(0), msckf_(0), 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()), initGravity_(false) #endif @@ -889,19 +896,21 @@ Transform OdometryMSCKF::computeTransform( pcl::PointCloud::Ptr localMap; nav_msgs::Odometry odom = msckf_->featureCallback2(measurements, localMap); - Transform p = Transform( - 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); - - if(!p.isNull()) + if( odom.pose.pose.orientation.x != 0.0f || + odom.pose.pose.orientation.y != 0.0f || + odom.pose.pose.orientation.z != 0.0f || + odom.pose.pose.orientation.w != 0.0f) { - // pose in rtabmap/ros coordinates - p = flipXY_*p*lastImu_.localTransform(); + Transform p = Transform( + 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()) { @@ -956,10 +965,10 @@ Transform OdometryMSCKF::computeTransform( { if(localMap.get() && localMap->size()) { - Eigen::Affine3f flip = (this->getPose()*previousPoseInv*flipXY_).toEigen3f(); + Eigen::Affine3f fixRot = fixPoseRotation_.toEigen3f(); for(unsigned int i=0; isize(); ++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))); } } @@ -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()); } } diff --git a/corelib/src/odometry/OdometryVINS.cpp b/corelib/src/odometry/OdometryVINS.cpp index d9395e2c..72a6baf9 100644 --- a/corelib/src/odometry/OdometryVINS.cpp +++ b/corelib/src/odometry/OdometryVINS.cpp @@ -355,6 +355,8 @@ Transform OdometryVINS::computeTransform( Vector3d acc(dx, dy, dz); Vector3d gyr(rx, ry, rz); + UDEBUG("IMU update stamp=%f", data.stamp()); + if(vinsEstimator_ != 0) { vinsEstimator_->inputIMU(t, acc, gyr); @@ -450,6 +452,7 @@ Transform OdometryVINS::computeTransform( if(info) { + info->type = this->getType(); info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1); info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001; @@ -473,7 +476,16 @@ Transform OdometryVINS::computeTransform( p.z = w_pts_i(2); p = util3d::transformPoint(p, fixT); 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(); } UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str()); diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 28c055ac..030b3e65 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -372,7 +372,7 @@ private: LoopClosureViewer * _loopClosureViewer; QString _graphSavingFileName; - bool _exportPosesFrame; + int _exportPosesFrame; QMap _exportPosesFileName; bool _autoScreenCaptureOdomSync; bool _autoScreenCaptureRAM; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 85c7ae8b..f945e07d 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1341,7 +1341,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI else if(odom.info().type == (int)Odometry::kTypeF2F || odom.info().type == (int)Odometry::kTypeViso2 || 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 kpts; 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 || 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()) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 95d4c121..faf9f4c1 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -7,7 +7,7 @@ 0 0 976 - 900 + 896 @@ -63,7 +63,7 @@ 0 - -571 + -1371 680 3083 @@ -6131,7 +6131,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - <html><head/><body><p>KITTI: 130 000 points</p></body></html> + EuRoC: 200 Hz -> 250 Hz 99999999