i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +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.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;
// Publish tf // Publish tf
/*if (publish_tf) { /*if (publish_tf) {
tf::Transform T_b_w_tf; tf::Transform T_b_w_tf;
tf::transformEigenToTF(T_b_w, T_b_w_tf); tf::transformEigenToTF(T_b_w, T_b_w_tf);
tf_pub.sendTransform(tf::StampedTransform( tf_pub.sendTransform(tf::StampedTransform(
T_b_w_tf, time, fixed_frame_id, child_frame_id)); T_b_w_tf, time, fixed_frame_id, child_frame_id));
}*/ }*/
// Publish the odometry // Publish the odometry
nav_msgs::Odometry odom_msg; nav_msgs::Odometry odom_msg;
@@ -725,20 +724,18 @@ public:
// Publish the 3D positions of the features that // Publish the 3D positions of the features that
// has been initialized. // has been initialized.
feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>()); feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
feature_msg_ptr->header.frame_id = fixed_frame_id; feature_msg_ptr->header.frame_id = fixed_frame_id;
feature_msg_ptr->height = 1; feature_msg_ptr->height = 1;
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 = feature_msg_ptr->points.push_back(pcl::PointXYZ(
msckf_vio::IMUState::T_imu_body.linear() * feature.position; feature.position(0), feature.position(1), feature.position(2)));
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->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