i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

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

@@ -672,18 +672,17 @@ public:
T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose();
T_i_w.translation() = imu_state.position;
Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w *
msckf_vio::IMUState::T_imu_body.inverse();
Eigen::Isometry3d T_b_w = T_i_w * msckf_vio::IMUState::T_imu_body.inverse();
Eigen::Vector3d body_velocity =
msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity;
// Publish tf
/*if (publish_tf) {
tf::Transform T_b_w_tf;
tf::transformEigenToTF(T_b_w, T_b_w_tf);
tf_pub.sendTransform(tf::StampedTransform(
T_b_w_tf, time, fixed_frame_id, child_frame_id));
}*/
tf::Transform T_b_w_tf;
tf::transformEigenToTF(T_b_w, T_b_w_tf);
tf_pub.sendTransform(tf::StampedTransform(
T_b_w_tf, time, fixed_frame_id, child_frame_id));
}*/
// Publish the odometry
nav_msgs::Odometry odom_msg;
@@ -725,20 +724,18 @@ public:
// Publish the 3D positions of the features that
// has been initialized.
feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
feature_msg_ptr->header.frame_id = fixed_frame_id;
feature_msg_ptr->height = 1;
for (const auto& item : map_server) {
const auto& feature = item.second;
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_position(0), feature_position(1), feature_position(2)));
}
}
feature_msg_ptr->width = feature_msg_ptr->points.size();
feature_msg_ptr->header.frame_id = fixed_frame_id;
feature_msg_ptr->height = 1;
for (const auto& item : map_server) {
const auto& feature = item.second;
if (feature.is_initialized) {
feature_msg_ptr->points.push_back(pcl::PointXYZ(
feature.position(0), feature.position(1), feature.position(2)));
}
}
feature_msg_ptr->width = feature_msg_ptr->points.size();
//feature_pub.publish(feature_msg_ptr);
// feature_pub.publish(feature_msg_ptr);
return odom_msg;
}
@@ -755,7 +752,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
imageProcessor_(0),
msckf_(0),
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()),
initGravity_(false)
#endif