Update odometry info of OpenVINS (#1142)

* add msckf features visualization * fill localMap with active tracked features
introlab · Oct 8, 2023 · 2aa139b · 2aa139b
1 parent 67c6a15
commit 2aa139b
Showing 1 changed file with 47 additions and 21 deletions.
diff --git 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<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();