i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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-08 11:36:00 +08: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

68
corelib/src/odometry/OdometryOpenVINS.cpp
View File

@@ -333,7 +333,15 @@ Transform OdometryOpenVINS::computeTransform(
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->localMapSize = feat_posinG.size();
info->features = features_SLAM.size() + good_features_MSCKF.size();
info->reg.covariance = cv::Mat(6,6,CV_64FC1);
std::vector<std::shared_ptr<ov_type::Type>> 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();