Method callbackReinitPose

src/ros-data-source.cpp:134–145

Getting re-initialization pose

// Getting re-initialization pose
void RosDataProvider::callbackReinitPose(
    const geometry_msgs::PoseStamped &reinitPose) {
  // Set reinitialization pose
  gtsam::Rot3 rotation(gtsam::Quaternion(
      reinitPose.pose.orientation.w, reinitPose.pose.orientation.x,
      reinitPose.pose.orientation.y, reinitPose.pose.orientation.z));

  gtsam::Point3 position(reinitPose.pose.position.x, reinitPose.pose.position.y,
                         reinitPose.pose.position.z);
  gtsam::Pose3 pose = gtsam::Pose3(rotation, position);
  reinit_packet_.setReinitPose(pose);
}

// Callback for stereo images and main spin
void RosDataProvider::callbackCamAndProcessStereo(