diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 046ff7ee60..0f8ed51b91 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -641,9 +641,9 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk, double, 0.001, "[m/s^3/sqrt(Hz)] (accel bias diffusion)"); RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity, double, 0.001, "[rad/s/sqrt(Hz)] (gyro \"white noise\")"); RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk, double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)"); - RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx, double, 2.0, "Pixel noise for MSCKF features"); + RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx, double, 1.0, "Pixel noise for MSCKF features"); RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier, double, 1.0, "Chi2 multiplier for MSCKF features"); - RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 2.0, "Pixel noise for SLAM features"); + RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 1.0, "Pixel noise for SLAM features"); RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier, double, 1.0, "Chi2 multiplier for SLAM features"); // Odometry Open3D diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index ffde226cfa..da81f7ee7f 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -385,98 +385,95 @@ Transform OdometryOpenVINS::computeTransform( } vioManager_->feed_measurement_camera(message); - if(vioManager_->initialized()) + std::shared_ptr state = vioManager_->get_state(); + Transform p((float)state->_imu->pos()(0), + (float)state->_imu->pos()(1), + (float)state->_imu->pos()(2), + (float)state->_imu->quat()(0), + (float)state->_imu->quat()(1), + (float)state->_imu->quat()(2), + (float)state->_imu->quat()(3)); + if(!p.isNull() && !p.isIdentity()) { - std::shared_ptr state = vioManager_->get_state(); - Transform p((float)state->_imu->pos()(0), - (float)state->_imu->pos()(1), - (float)state->_imu->pos()(2), - (float)state->_imu->quat()(0), - (float)state->_imu->quat()(1), - (float)state->_imu->quat()(2), - (float)state->_imu->quat()(3)); - if(!p.isNull()) + p = p * imuLocalTransformInv_; + + if(this->getPose().rotation().isIdentity()) { - p = p * imuLocalTransformInv_; + initGravity_ = true; + this->reset(this->getPose() * p.rotation()); + } - if(this->getPose().rotation().isIdentity()) - { - initGravity_ = true; - this->reset(this->getPose() * p.rotation()); - } + if(previousPoseInv_.isIdentity()) + previousPoseInv_ = p.inverse(); - if(previousPoseInv_.isIdentity()) - previousPoseInv_ = p.inverse(); + t = previousPoseInv_ * p; - t = previousPoseInv_ * p; + if(info) + { + double timestamp; + std::unordered_map feat_posinG, feat_tracks_uvd; + vioManager_->get_active_tracks(timestamp, feat_posinG, feat_tracks_uvd); + auto features_SLAM = vioManager_->get_features_SLAM(); + auto good_features_MSCKF = vioManager_->get_good_features_MSCKF(); - if(info) + info->type = this->getType(); + info->localMapSize = feat_posinG.size(); + info->features = features_SLAM.size() + good_features_MSCKF.size(); + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1); + if(covFilled) { - double timestamp; - std::unordered_map feat_posinG, feat_tracks_uvd; - vioManager_->get_active_tracks(timestamp, feat_posinG, feat_tracks_uvd); - auto features_SLAM = vioManager_->get_features_SLAM(); - auto good_features_MSCKF = vioManager_->get_good_features_MSCKF(); + Eigen::Matrix covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose(); + cv::eigen2cv(covariance, info->reg.covariance); + } - info->type = this->getType(); - info->localMapSize = feat_posinG.size(); - info->features = features_SLAM.size() + good_features_MSCKF.size(); - info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1); - if(covFilled) + if(this->isInfoDataFilled()) + { + Transform fixT = this->getPose() * previousPoseInv_; + Transform camT; + if(!data.rightRaw().empty()) + camT = data.stereoCameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT; + else + camT = data.cameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT; + + for(auto &feature : feat_posinG) { - Eigen::Matrix covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose(); - cv::eigen2cv(covariance, info->reg.covariance); + cv::Point3f pt3d(feature.second[0], feature.second[1], feature.second[2]); + pt3d = util3d::transformPoint(pt3d, fixT); + info->localMap.emplace(feature.first, pt3d); } - if(this->isInfoDataFilled()) + if(this->imagesAlreadyRectified()) { - Transform fixT = this->getPose() * previousPoseInv_; - Transform camT; - if(!data.rightRaw().empty()) - camT = data.stereoCameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT; - else - camT = data.cameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT; - - for(auto &feature : feat_posinG) + for(auto &feature : features_SLAM) { - cv::Point3f pt3d(feature.second[0], feature.second[1], feature.second[2]); - pt3d = util3d::transformPoint(pt3d, fixT); - info->localMap.emplace(feature.first, pt3d); + cv::Point3f pt3d(feature[0], feature[1], feature[2]); + pt3d = util3d::transformPoint(pt3d, camT); + cv::Point2f pt; + if(!data.rightRaw().empty()) + data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + else + data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + info->reg.inliersIDs.emplace_back(info->newCorners.size()); + info->newCorners.emplace_back(pt); } - if(this->imagesAlreadyRectified()) + for(auto &feature : good_features_MSCKF) { - for(auto &feature : features_SLAM) - { - cv::Point3f pt3d(feature[0], feature[1], feature[2]); - pt3d = util3d::transformPoint(pt3d, camT); - cv::Point2f pt; - if(!data.rightRaw().empty()) - data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); - else - data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); - info->reg.inliersIDs.emplace_back(info->newCorners.size()); - info->newCorners.emplace_back(pt); - } - - for(auto &feature : good_features_MSCKF) - { - cv::Point3f pt3d(feature[0], feature[1], feature[2]); - pt3d = util3d::transformPoint(pt3d, camT); - cv::Point2f pt; - if(!data.rightRaw().empty()) - data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); - else - data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); - info->reg.matchesIDs.emplace_back(info->newCorners.size()); - info->newCorners.emplace_back(pt); - } + cv::Point3f pt3d(feature[0], feature[1], feature[2]); + pt3d = util3d::transformPoint(pt3d, camT); + cv::Point2f pt; + if(!data.rightRaw().empty()) + data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + else + data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + info->reg.matchesIDs.emplace_back(info->newCorners.size()); + info->newCorners.emplace_back(pt); } } } - - previousPoseInv_ = p.inverse(); } + + previousPoseInv_ = p.inverse(); } } } diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 643be37779..283f69af30 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -19580,7 +19580,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.100000000000000 - 2.000000000000000 + 1.000000000000000 @@ -19638,7 +19638,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.100000000000000 - 2.000000000000000 + 1.000000000000000