Skip to content

Commit

Permalink
start outputting data after static initialization (#1171)
Browse files Browse the repository at this point in the history 
* start outputting data after static initialization

* restore pixel noise to 1
  • Loading branch information
@borongyuan
borongyuan authored Nov 28, 2023
1 parent 7ab1ee0 commit 07c24e9
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 76 deletions.
4 changes: 2 additions & 2 deletions corelib/include/rtabmap/core/Parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
141 changes: 69 additions & 72 deletions corelib/src/odometry/OdometryOpenVINS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -385,98 +385,95 @@ Transform OdometryOpenVINS::computeTransform(
}
vioManager_->feed_measurement_camera(message);

if(vioManager_->initialized())
std::shared_ptr<ov_msckf::State> 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<ov_msckf::State> 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<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();

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<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();
Eigen::Matrix<double, 6, 6> 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<double, 6, 6> 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();
}
}
}
Expand Down
4 changes: 2 additions & 2 deletions guilib/src/ui/preferencesDialog.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19580,7 +19580,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<double>0.100000000000000</double>
</property>
<property name="value">
<double>2.000000000000000</double>
<double>1.000000000000000</double>
</property>
</widget>
</item>
Expand Down Expand Up @@ -19638,7 +19638,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<double>0.100000000000000</double>
</property>
<property name="value">
<double>2.000000000000000</double>
<double>1.000000000000000</double>
</property>
</widget>
</item>
Expand Down

0 comments on commit 07c24e9

Please sign in to comment.