From 2aa139b57948b4dea9c263f81927a909e4d6b3a7 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sun, 8 Oct 2023 11:36:00 +0800 Subject: [PATCH] Update odometry info of OpenVINS (#1142) * add msckf features visualization * fill localMap with active tracked features --- corelib/src/odometry/OdometryOpenVINS.cpp | 68 ++++++++++++++++------- 1 file changed, 47 insertions(+), 21 deletions(-) diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 0d8d4f7ed6..eaa8fc1ff1 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -333,7 +333,15 @@ Transform OdometryOpenVINS::computeTransform( 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(); + info->type = this->getType(); + info->localMapSize = feat_posinG.size(); + info->features = features_SLAM.size() + good_features_MSCKF.size(); info->reg.covariance = cv::Mat(6,6,CV_64FC1); std::vector> statevars; statevars.emplace_back(state->_imu->pose()->p()); @@ -347,33 +355,51 @@ Transform OdometryOpenVINS::computeTransform( } } - Transform fixT = this->getPose() * previousPoseInv_; - Transform camLocalTransformInv; - if(!data.rightRaw().empty()) - camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse(); - else - camLocalTransformInv = data.cameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse(); - - for (auto &it_per_id : vioManager_->get_features_SLAM()) + if(this->isInfoDataFilled()) { - cv::Point3f pt3d(it_per_id[0], it_per_id[1], it_per_id[2]); - pt3d = util3d::transformPoint(pt3d, fixT); - info->localMap.emplace_hint(info->localMap.end(), info->localMap.size(), pt3d); + 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) + { + 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->imagesAlreadyRectified()) { - cv::Point2f pt; - pt3d = util3d::transformPoint(pt3d, camLocalTransformInv); - 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 : 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); + } } } - info->features = info->newCorners.size(); - info->localMapSize = info->localMap.size(); } previousPoseInv_ = p.inverse();