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

change for initial frame update

Browse Source
This commit is contained in:
Borong Yuan
2023-07-24 21:10:55 +08:00
parent f8b761d79b
commit 5d6875ed98
1 changed files with 18 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/src/odometry/OdometryMSCKF.cpp
View File

@@ -672,8 +672,7 @@ public:
T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose(); T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose();
T_i_w.translation() = imu_state.position; T_i_w.translation() = imu_state.position;
Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w * Eigen::Isometry3d T_b_w = T_i_w * msckf_vio::IMUState::T_imu_body.inverse();
msckf_vio::IMUState::T_imu_body.inverse();
Eigen::Vector3d body_velocity = Eigen::Vector3d body_velocity =
msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity; msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity;
@@ -730,15 +729,13 @@ public:
for (const auto& item : map_server) { for (const auto& item : map_server) {
const auto& feature = item.second; const auto& feature = item.second;
if (feature.is_initialized) { if (feature.is_initialized) {
Eigen::Vector3d feature_position =
msckf_vio::IMUState::T_imu_body.linear() * feature.position;
feature_msg_ptr->points.push_back(pcl::PointXYZ( feature_msg_ptr->points.push_back(pcl::PointXYZ(
feature_position(0), feature_position(1), feature_position(2))); feature.position(0), feature.position(1), feature.position(2)));
} }
} }
feature_msg_ptr->width = feature_msg_ptr->points.size(); feature_msg_ptr->width = feature_msg_ptr->points.size();
//feature_pub.publish(feature_msg_ptr); // feature_pub.publish(feature_msg_ptr);
return odom_msg; return odom_msg;
} }
@@ -755,7 +752,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
imageProcessor_(0), imageProcessor_(0),
msckf_(0), msckf_(0),
parameters_(parameters), parameters_(parameters),
fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0), fixPoseRotation_(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0),
previousPose_(Transform::getIdentity()), previousPose_(Transform::getIdentity()),
initGravity_(false) initGravity_(false)
#endif #endif