| 199 | } |
| 200 | |
| 201 | void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) |
| 202 | { |
| 203 | //ROS_INFO("vio_callback!"); |
| 204 | Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z); |
| 205 | Quaterniond vio_q; |
| 206 | vio_q.w() = pose_msg->pose.pose.orientation.w; |
| 207 | vio_q.x() = pose_msg->pose.pose.orientation.x; |
| 208 | vio_q.y() = pose_msg->pose.pose.orientation.y; |
| 209 | vio_q.z() = pose_msg->pose.pose.orientation.z; |
| 210 | |
| 211 | vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio; |
| 212 | vio_q = posegraph.w_r_vio * vio_q; |
| 213 | |
| 214 | vio_t = posegraph.r_drift * vio_t + posegraph.t_drift; |
| 215 | vio_q = posegraph.r_drift * vio_q; |
| 216 | |
| 217 | Vector3d vio_t_cam; |
| 218 | Quaterniond vio_q_cam; |
| 219 | vio_t_cam = vio_t + vio_q * tic; |
| 220 | vio_q_cam = vio_q * qic; |
| 221 | |
| 222 | if (!VISUALIZE_IMU_FORWARD) |
| 223 | { |
| 224 | cameraposevisual.reset(); |
| 225 | cameraposevisual.add_pose(vio_t_cam, vio_q_cam); |
| 226 | cameraposevisual.publish_by(pub_camera_pose_visual, pose_msg->header); |
| 227 | } |
| 228 | |
| 229 | odometry_buf.push(vio_t_cam); |
| 230 | if (odometry_buf.size() > 10) |
| 231 | { |
| 232 | odometry_buf.pop(); |
| 233 | } |
| 234 | |
| 235 | visualization_msgs::Marker key_odometrys; |
| 236 | key_odometrys.header = pose_msg->header; |
| 237 | key_odometrys.header.frame_id = "world"; |
| 238 | key_odometrys.ns = "key_odometrys"; |
| 239 | key_odometrys.type = visualization_msgs::Marker::SPHERE_LIST; |
| 240 | key_odometrys.action = visualization_msgs::Marker::ADD; |
| 241 | key_odometrys.pose.orientation.w = 1.0; |
| 242 | key_odometrys.lifetime = ros::Duration(); |
| 243 | |
| 244 | //static int key_odometrys_id = 0; |
| 245 | key_odometrys.id = 0; //key_odometrys_id++; |
| 246 | key_odometrys.scale.x = 0.1; |
| 247 | key_odometrys.scale.y = 0.1; |
| 248 | key_odometrys.scale.z = 0.1; |
| 249 | key_odometrys.color.r = 1.0; |
| 250 | key_odometrys.color.a = 1.0; |
| 251 | |
| 252 | for (unsigned int i = 0; i < odometry_buf.size(); i++) |
| 253 | { |
| 254 | geometry_msgs::Point pose_marker; |
| 255 | Vector3d vio_t; |
| 256 | vio_t = odometry_buf.front(); |
| 257 | odometry_buf.pop(); |
| 258 | pose_marker.x = vio_t.x(); |