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

start outputting data after static initialization (#1171)

Browse Source 
* start outputting data after static initialization

* restore pixel noise to 1
This commit is contained in:
Borong Yuan
2023-11-29 05:20:45 +08:00
committed by GitHub
parent 7ab1ee0a0b
commit 07c24e95ba
3 changed files with 90 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/Parameters.h
View File

@@ -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, 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, 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, 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, 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"); RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier, double, 1.0, "Chi2 multiplier for SLAM features");
// Odometry Open3D // Odometry Open3D

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

@@ -385,98 +385,95 @@ Transform OdometryOpenVINS::computeTransform(
} }
vioManager_->feed_measurement_camera(message); 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(); p = p * imuLocalTransformInv_;
Transform p((float)state->_imu->pos()(0),
(float)state->_imu->pos()(1), if(this->getPose().rotation().isIdentity())
(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_; 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();
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();
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)
{
Eigen::Matrix<double, 6, 6> covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose();
cv::eigen2cv(covariance, info->reg.covariance);
}
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)
{
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())
{
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);
}
}
}
}
previousPoseInv_ = p.inverse();
} }
if(previousPoseInv_.isIdentity())
previousPoseInv_ = p.inverse();
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();
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)
{
Eigen::Matrix<double, 6, 6> covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose();
cv::eigen2cv(covariance, info->reg.covariance);
}
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)
{
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())
{
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);
}
}
}
}
previousPoseInv_ = p.inverse();
} }
} }
} }

4
guilib/src/ui/preferencesDialog.ui
View File

@@ -19580,7 +19580,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<double>0.100000000000000</double> <double>0.100000000000000</double>
</property> </property>
<property name="value"> <property name="value">
<double>2.000000000000000</double> <double>1.000000000000000</double>
</property> </property>
</widget> </widget>
</item> </item>
@@ -19638,7 +19638,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<double>0.100000000000000</double> <double>0.100000000000000</double>
</property> </property>
<property name="value"> <property name="value">
<double>2.000000000000000</double> <double>1.000000000000000</double>
</property> </property>
</widget> </widget>
</item> </item>