i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-13 23:10:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update odometry info of OpenVINS (#1142)

Browse Source 
* add msckf features visualization

* fill localMap with active tracked features
This commit is contained in:
Borong Yuan
2023-10-07 20:36:00 -07:00
committed by GitHub
parent 67c6a15b93
commit 2aa139b579
1 changed files with 47 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/odometry/OdometryOpenVINS.cpp
+39 -13
View File
@@ -333,7 +333,15 @@ Transform OdometryOpenVINS::computeTransform(
if(info) if(info)
{ {
double timestamp;
std::unordered_map<size_t, Eigen::Vector3d> 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->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); info->reg.covariance = cv::Mat(6,6,CV_64FC1);
std::vector<std::shared_ptr<ov_type::Type>> statevars; std::vector<std::shared_ptr<ov_type::Type>> statevars;
statevars.emplace_back(state->_imu->pose()->p()); statevars.emplace_back(state->_imu->pose()->p());
@@ -347,23 +355,29 @@ Transform OdometryOpenVINS::computeTransform(
} }
} }
Transform fixT = this->getPose() * previousPoseInv_; if(this->isInfoDataFilled())
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())
{ {
cv::Point3f pt3d(it_per_id[0], it_per_id[1], it_per_id[2]); 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); pt3d = util3d::transformPoint(pt3d, fixT);
info->localMap.emplace_hint(info->localMap.end(), info->localMap.size(), pt3d); info->localMap.emplace(feature.first, pt3d);
}
if(this->imagesAlreadyRectified()) if(this->imagesAlreadyRectified())
{ {
for(auto &feature : features_SLAM)
{
cv::Point3f pt3d(feature[0], feature[1], feature[2]);
pt3d = util3d::transformPoint(pt3d, camT);
cv::Point2f pt; cv::Point2f pt;
pt3d = util3d::transformPoint(pt3d, camLocalTransformInv);
if(!data.rightRaw().empty()) if(!data.rightRaw().empty())
data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
else else
@@ -371,9 +385,21 @@ Transform OdometryOpenVINS::computeTransform(
info->reg.inliersIDs.emplace_back(info->newCorners.size()); info->reg.inliersIDs.emplace_back(info->newCorners.size());
info->newCorners.emplace_back(pt); 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(); previousPoseInv_ = p.inverse();