From 3f332799abda9742677867c0e95ec2da79ef4ef4 Mon Sep 17 00:00:00 2001 From: Hydran00 Date: Tue, 9 Jul 2024 10:08:28 +0200 Subject: [PATCH] Marker id is now in the request field --- README.md | 20 +-- .../config/config.yaml | 2 +- .../include/image_processor.hpp | 69 +++++----- .../src/image_processor.cpp | 59 +++++--- .../src/pose_service.cpp | 130 ++++++++++-------- calibration_srv/srv/Calibration.srv | 2 +- 6 files changed, 159 insertions(+), 123 deletions(-) diff --git a/README.md b/README.md index b300108..fdc662d 100644 --- a/README.md +++ b/README.md @@ -31,31 +31,31 @@ ros2 launch aruco_camera_pose_estimator_cpp aruco_camera_pose_estimator.launch.p ``` In another sourced terminal call the service ``` -ros2 service call /calibration/get_camera_pose calibration_srv/srv/Calibration "req: - data: ''" +ros2 service call /calibration/get_camera_pose calibration_srv/srv/Calibration "marker_id: " ``` +You will get a PoseStamped message as response corresponding to the camera pose with respect to the world frame. ### Maths behind this implementation -The OpenCV Aruco Library let us compute $M_{c}^{a} \in \mathcal{R}^{4x4}$ that express the rototranslation of the Aruco with respect to the camera frame. +The OpenCV Aruco Library let us compute $M_{a}^{c} \in \mathcal{R}^{4x4}$ that express the rototranslation of the Aruco with respect to the camera frame. -Since we want $M_{w}^{c}$ (the camera with respect to the world frame), we can compute this matrix using homogeneous matrices multiplication as: +Since we want $M_{c}^{w}$ (the camera with respect to the world frame), we can compute this matrix using homogeneous matrices multiplication as: -$M_{w}^{c} = M_{w}^{a} M_{a}^{c}$ +$M_{c}^{w} = M_{a}^{w} M_{c}^{a}$ -where $M_{a}^{c} \in \mathcal{R}^{4x4}$ is the transpose of $M_{c}^{a}$ and $M_{w}^{a} \in \mathcal{R}^{4x4}$ is the rototranslation of the aruco marker with respect to the world frame. +where $M_{c}^{a} \in \mathcal{R}^{4x4}$ is the transpose of $M_{a}^{c}$ and $M_{a}^{w} \in \mathcal{R}^{4x4}$ is the rototranslation of the aruco marker with respect to the world frame. -**Note that if you just need the camera pose with respect to the aruco marker you can neglect $M_{w}^{a}$, i.e. setting it to equal to the identity matrix.** +**Note that if you just need the camera pose with respect to the aruco marker you can neglect $M_{a}^{w}$, i.e. setting it to equal to the identity matrix.** ### Parameters #### ROS2 Parameters - `input_topic_name`: ROS2 input topic name of the camera image stream - `output_service_name`: ROS2 output service name of the computed camera pose - - `aruco_marker_id`: id of the aruco in the dictionary. - `n_observation`: the number of measurements used for the computation of the mean rototranslation (`timeout` should be set accordingly) + - `frame_id`: the name of the world frame in which the camera pose will be expressed - `timeout_ms`: the amount of time in milliseconds before the data collection is considered failed. Higher `n_observation` means more time to wait for the image stream. When the timeout is reached, the service response is sent having `error` in the `frame_id` field. - `show_img`: wether to show the axis of the aruco when one is found. - `aruco_size`: the aruco edge size in centimeters - - `aruco_XYZ_offset_from_baseframe`: XYZ displacement of the aruco with respect to the world frame. This corresponds to the entries (0,3), (1,3), (2,3) of $M_{w}^{a}$ - `aruco_rot_offset_from_baseframe`: orientation displacement of the aruco with respect to the world frame. It is specified in (w,x,y,z) quaternion forms. This, after the conversion to the rotation matrix, corresponds to the 3x3 matrix starting from (0,0) in $M_{w}^{a}$ + - `aruco_XYZ_offset_from_baseframe`: XYZ displacement of the aruco with respect to the world frame. This corresponds to the entries (0,3), (1,3), (2,3) of $M_{a}^{w}$ + `aruco_rot_offset_from_baseframe`: orientation displacement of the aruco with respect to the world frame. It is specified in (w,x,y,z) quaternion forms. This, after the conversion to the rotation matrix, corresponds to the 3x3 matrix starting from (0,0) in $M_{a}^{w}$ #### Camera calibration parameters Camera intrinsic and distortion coefficients can be set editing `calibration_params.yaml` under `config`. diff --git a/aruco_camera_pose_estimator_cpp/config/config.yaml b/aruco_camera_pose_estimator_cpp/config/config.yaml index 691d8db..a90a9be 100644 --- a/aruco_camera_pose_estimator_cpp/config/config.yaml +++ b/aruco_camera_pose_estimator_cpp/config/config.yaml @@ -2,7 +2,7 @@ pose_service: ros__parameters: input_topic_name: "/camera/camera/color/image_raw" output_service_name: "/calibration/get_camera_pose" - aruco_marker_id: 9 + frame_id: "base_link" n_observation: 10 # number of measurements for computing the average timeout_ms: 2000 show_img: false diff --git a/aruco_camera_pose_estimator_cpp/include/image_processor.hpp b/aruco_camera_pose_estimator_cpp/include/image_processor.hpp index 6a26047..167f2fb 100644 --- a/aruco_camera_pose_estimator_cpp/include/image_processor.hpp +++ b/aruco_camera_pose_estimator_cpp/include/image_processor.hpp @@ -10,42 +10,43 @@ #include #include -class ImageProcessor : public rclcpp::Node { - public: - ImageProcessor(const std::string &topic_name, uint32_t N, - Eigen::Vector3d &mean_tvec, Eigen::Quaterniond &mean_quat, - bool &mean_computed, bool show_img, uint16_t aruco_marker_id, - double arucobase_size, double cx, double cy, double fx, - double fy, double k1, double k2, double k3, double p1, - double p2); - std::string topic_name_; - uint32_t N_; - Eigen::Vector3d &mean_tvec_; - Eigen::Quaterniond &mean_quat_; - bool &mean_computed_; - int idx_; - rclcpp::Subscription::SharedPtr sub_; - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); +class ImageProcessor : public rclcpp::Node +{ +public: + ImageProcessor(const std::string &topic_name, uint32_t N, + Eigen::Vector3d &mean_tvec, Eigen::Quaterniond &mean_quat, + bool &mean_computed, bool show_img, + double arucobase_size, double cx, double cy, double fx, + double fy, double k1, double k2, double k3, double p1, + double p2); + std::string topic_name_; + uint32_t N_; + Eigen::Vector3d &mean_tvec_; + Eigen::Quaterniond &mean_quat_; + bool &mean_computed_; + int idx_; + rclcpp::Subscription::SharedPtr sub_; + uint16_t aruco_marker_id_; + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); - private: - std::vector rvec_cam_list_; - std::vector tvec_cam_list_; - cv::Ptr dictionary_; - cv::Ptr detector_params_; - cv::Mat image_; - bool show_img_; - uint16_t aruco_marker_id_; - double arucobase_size_; - double cx_, cy_, fx_, fy_; - double k1_, k2_, k3_, p1_, p2_; - cv::Mat intrinsic_camera_ = cv::Mat_(3, 3); - cv::Mat distortion_camera_ = cv::Mat_(1, 5); +private: + std::vector rvec_cam_list_; + std::vector tvec_cam_list_; + cv::Ptr dictionary_; + cv::Ptr detector_params_; + cv::Mat image_; + bool show_img_; + double arucobase_size_; + double cx_, cy_, fx_, fy_; + double k1_, k2_, k3_, p1_, p2_; + cv::Mat intrinsic_camera_ = cv::Mat_(3, 3); + cv::Mat distortion_camera_ = cv::Mat_(1, 5); - bool camera_pose_estimation(cv::Mat &frame, cv::Vec3d &tvec_camera, - cv::Vec3d &rvec_camera); + bool camera_pose_estimation(cv::Mat &frame, cv::Vec3d &tvec_camera, + cv::Vec3d &rvec_camera); - Eigen::Quaterniond quaternion_avg( - const std::vector &quat_list); + Eigen::Quaterniond quaternion_avg( + const std::vector &quat_list); }; -#endif // IMAGE_PROCESSOR_HPP +#endif // IMAGE_PROCESSOR_HPP diff --git a/aruco_camera_pose_estimator_cpp/src/image_processor.cpp b/aruco_camera_pose_estimator_cpp/src/image_processor.cpp index f65fd01..edfbaf9 100644 --- a/aruco_camera_pose_estimator_cpp/src/image_processor.cpp +++ b/aruco_camera_pose_estimator_cpp/src/image_processor.cpp @@ -13,7 +13,7 @@ using namespace std::chrono_literals; ImageProcessor::ImageProcessor( const std::string &topic_name, const uint32_t N, Eigen::Vector3d &mean_tvec, Eigen::Quaterniond &mean_quat, bool &mean_computed, bool show_img, - uint16_t aruco_marker_id, double arucobase_size, double cx, double cy, + double arucobase_size, double cx, double cy, double fx, double fy, double k1, double k2, double k3, double p1, double p2) : Node("image_processor"), topic_name_(topic_name), @@ -22,7 +22,6 @@ ImageProcessor::ImageProcessor( mean_quat_(mean_quat), mean_computed_(mean_computed), show_img_(show_img), - aruco_marker_id_(aruco_marker_id), arucobase_size_(arucobase_size), cx_(cx), cy_(cy), @@ -32,7 +31,9 @@ ImageProcessor::ImageProcessor( k2_(k2), k3_(k3), p1_(p1), - p2_(p2) { + p2_(p2) +{ + aruco_marker_id_ = 0; idx_ = 0; dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); detector_params_ = cv::aruco::DetectorParameters::create(); @@ -46,16 +47,21 @@ ImageProcessor::ImageProcessor( bool ImageProcessor::camera_pose_estimation(cv::Mat &frame, cv::Vec3d &tvec_camera, - cv::Vec3d &rvec_camera) { + cv::Vec3d &rvec_camera) +{ cv::Mat gray; cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); std::vector ids; std::vector> corners; cv::aruco::detectMarkers(gray, dictionary_, corners, ids, detector_params_); - if (!ids.empty()) { - for (size_t i = 0; i < ids.size(); ++i) { - if (ids[i] == aruco_marker_id_) { + // RCLCPP_INFO(this->get_logger(), "Looking for aruco with id %d", aruco_marker_id_); + if (!ids.empty()) + { + for (size_t i = 0; i < ids.size(); ++i) + { + if (ids[i] == aruco_marker_id_) + { // RCLCPP_INFO(this->get_logger(), "Detected Aruco base marker"); std::vector rvec, tvec; cv::aruco::estimatePoseSingleMarkers(corners, arucobase_size_, @@ -64,7 +70,8 @@ bool ImageProcessor::camera_pose_estimation(cv::Mat &frame, tvec_camera = tvec[0]; rvec_camera = rvec[0]; // Draw axis for the aruco marker - if (show_img_) { + if (show_img_) + { cv::aruco::drawAxis(frame, intrinsic_camera_, distortion_camera_, rvec[0], tvec[0], 0.1); cv::imshow("Aruco Marker", frame); @@ -78,9 +85,11 @@ bool ImageProcessor::camera_pose_estimation(cv::Mat &frame, } Eigen::Quaterniond ImageProcessor::quaternion_avg( - const std::vector &quat_list) { + const std::vector &quat_list) +{ Eigen::Matrix4d M = Eigen::Matrix4d::Zero(); - for (const auto &q : quat_list) { + for (const auto &q : quat_list) + { Eigen::Vector4d q_vec(q.w(), q.x(), q.y(), q.z()); M += q_vec * q_vec.transpose(); } @@ -92,18 +101,24 @@ Eigen::Quaterniond ImageProcessor::quaternion_avg( } void ImageProcessor::image_callback( - const sensor_msgs::msg::Image::SharedPtr msg) { + const sensor_msgs::msg::Image::SharedPtr msg) +{ + // RCLCPP_INFO(this->get_logger(), "Received image"); cv_bridge::CvImagePtr cv_ptr; - try { + try + { cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); - } catch (cv_bridge::Exception &e) { + } + catch (cv_bridge::Exception &e) + { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; } cv::Vec3d tvec_camera, rvec_camera; bool success = camera_pose_estimation(cv_ptr->image, tvec_camera, rvec_camera); - if (success) { + if (success) + { tvec_cam_list_[idx_] = tvec_camera; rvec_cam_list_[idx_] = rvec_camera; idx_++; @@ -112,10 +127,12 @@ void ImageProcessor::image_callback( cv_ptr.reset(); // Check if we've reached N samples - if (idx_ == static_cast(N_)) { + if (idx_ == static_cast(N_)) + { idx_ = 0; Eigen::Vector3d mean_tvec = Eigen::Vector3d::Zero(); - for (const auto &tvec : tvec_cam_list_) { + for (const auto &tvec : tvec_cam_list_) + { mean_tvec += Eigen::Vector3d(tvec[0], tvec[1], tvec[2]); } mean_tvec /= tvec_cam_list_.size(); @@ -123,7 +140,8 @@ void ImageProcessor::image_callback( std::vector quat_list(tvec_cam_list_.size()); cv::Mat R_cv; Eigen::Matrix3d R; - for (size_t i = 0; i < tvec_cam_list_.size(); ++i) { + for (size_t i = 0; i < tvec_cam_list_.size(); ++i) + { cv::Rodrigues(rvec_cam_list_[i], R_cv); cv::cv2eigen(R_cv, R); quat_list[i] = Eigen::Quaterniond(R).normalized(); @@ -131,8 +149,11 @@ void ImageProcessor::image_callback( mean_quat_ = quaternion_avg(quat_list); // Lock mean_computed with mutex (if multithreading is involved) mean_computed_ = true; - } else { - if (idx_ >= static_cast(N_)) { + } + else + { + if (idx_ >= static_cast(N_)) + { idx_ = 0; } } diff --git a/aruco_camera_pose_estimator_cpp/src/pose_service.cpp b/aruco_camera_pose_estimator_cpp/src/pose_service.cpp index 978a38a..d0b4300 100755 --- a/aruco_camera_pose_estimator_cpp/src/pose_service.cpp +++ b/aruco_camera_pose_estimator_cpp/src/pose_service.cpp @@ -5,7 +5,8 @@ using std::chrono::milliseconds; PoseService::PoseService() : Node("pose_service"), mean_tvec_(Eigen::Vector3d::Zero()), - mean_quat_(Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)) { + mean_quat_(Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)) +{ this->declare_parameter("input_topic_name", "/camera/camera/color/image_raw"); this->declare_parameter("output_service_name", "/calibration/get_camera_pose"); @@ -20,8 +21,7 @@ PoseService::PoseService() // Aruco parameters this->declare_parameter("show_img", true); - this->declare_parameter("aruco_marker_id", 0); - this->declare_parameter("aruco_size", 0.10); // in meters + this->declare_parameter("aruco_size", 0.10); // in meters // Aruco offsets from base frame this->declare_parameter("aruco_XYZ_offset_from_baseframe", @@ -34,6 +34,8 @@ PoseService::PoseService() aruco_rot_offset_from_baseframe_ = this->get_parameter("aruco_rot_offset_from_baseframe").as_double_array(); + this->declare_parameter("frame_id", "world"); + // camera intrinsics this->declare_parameter("cx", 958.620910644531); this->declare_parameter("cy", 564.277893066406); @@ -53,7 +55,6 @@ PoseService::PoseService() this->get_parameter("n_observation").as_int(), mean_tvec_, mean_quat_, mean_computed_, this->get_parameter("show_img").as_bool(), - this->get_parameter("aruco_marker_id").as_int(), this->get_parameter("aruco_size").as_double(), this->get_parameter("cx").as_double(), this->get_parameter("cy").as_double(), @@ -71,7 +72,7 @@ PoseService::PoseService() RCLCPP_INFO(this->get_logger(), "Parameters: input_topic_name: %s, output_service_name: %s, " "n_observation: %ld, " - "timeout_ms: %ld, show_img: %d, aruco_marker_id: %ld, " + "timeout_ms: %ld, show_img: %d, " "aruco_size: %f, cx: %f, cy: %f, fx: %f, fy: %f, " "k1: %f, k2: %f, k3: %f, p1: %f, p2: %f", this->get_parameter("input_topic_name").as_string().c_str(), @@ -79,7 +80,6 @@ PoseService::PoseService() this->get_parameter("n_observation").as_int(), this->get_parameter("timeout_ms").as_int(), this->get_parameter("show_img").as_bool(), - this->get_parameter("aruco_marker_id").as_int(), this->get_parameter("aruco_size").as_double(), this->get_parameter("cx").as_double(), this->get_parameter("cy").as_double(), @@ -100,81 +100,48 @@ PoseService::PoseService() aruco_rot_offset_from_baseframe_[2], aruco_rot_offset_from_baseframe_[3]); } -void PoseService::get_camera_pose(const Eigen::Vector3d &tvec, - const Eigen::Quaterniond &quat, - geometry_msgs::msg::PoseStamped &pose_msg) { - // rTc = rTa * cTa -> rTc = rTa * cTa where r is robot frame, a is aruco - // frame, c is camera frame - Eigen::Matrix3d R = quat.toRotationMatrix(); - - auto tvec_cTa = -R.transpose() * tvec; - Eigen::Quaterniond q(R.transpose()); - - // create aTc 4x4 matrix - Eigen::Matrix4d T_aruco_to_camera = Eigen::Matrix4d::Identity(); - T_aruco_to_camera.block<3, 3>(0, 0) = R.transpose(); - T_aruco_to_camera.block<3, 1>(0, 3) = tvec_cTa; - - // create rTa 4x4 matrix - Eigen::Matrix4d T_robot_to_aruco = Eigen::Matrix4d::Identity(); - T_robot_to_aruco.block<3, 3>(0, 0) = - Eigen::Quaterniond(aruco_rot_offset_from_baseframe_[0], - aruco_rot_offset_from_baseframe_[1], - aruco_rot_offset_from_baseframe_[2], - aruco_rot_offset_from_baseframe_[3]) - .normalized() - .toRotationMatrix(); - - T_robot_to_aruco.block<3, 1>(0, 3) = Eigen::Vector3d( - aruco_XYZ_offset_from_baseframe_[0], aruco_XYZ_offset_from_baseframe_[1], - aruco_XYZ_offset_from_baseframe_[2]); - - // create rTc 4x4 matrix - Eigen::Matrix4d T_robot_to_camera = T_robot_to_aruco * T_aruco_to_camera; - - pose_msg.pose.position.x = T_robot_to_camera(0, 3); - pose_msg.pose.position.y = T_robot_to_camera(1, 3); - pose_msg.pose.position.z = T_robot_to_camera(2, 3); - - Eigen::Quaterniond q_rTc(T_robot_to_camera.block<3, 3>(0, 0)); - pose_msg.pose.orientation.w = q_rTc.w(); - pose_msg.pose.orientation.x = q_rTc.x(); - pose_msg.pose.orientation.y = q_rTc.y(); - pose_msg.pose.orientation.z = q_rTc.z(); -} - void PoseService::get_camera_pose_service_callback( const std::shared_ptr request, - std::shared_ptr response) { + std::shared_ptr response) +{ (void)request; RCLCPP_INFO(this->get_logger(), "Request received"); mean_computed_ = false; // Unsubscribe if already subscribed - if (image_processor_node_->sub_) { + if (image_processor_node_->sub_) + { image_processor_node_->sub_.reset(); } + RCLCPP_INFO(this->get_logger(), "Received request to find Aruco with id %d", + request->marker_id); // subscribe to the camera topic image_processor_node_->sub_ = image_processor_node_->create_subscription( this->get_parameter("input_topic_name").as_string(), 1, std::bind(&ImageProcessor::image_callback, image_processor_node_, std::placeholders::_1)); + image_processor_node_->aruco_marker_id_ = request->marker_id; auto start_time = this->get_clock()->now(); while (rclcpp::ok() && !mean_computed_ && - (this->get_clock()->now() - start_time) < milliseconds(timeout_)) { + (this->get_clock()->now() - start_time) < milliseconds(timeout_)) + { node_executor_.spin_some(); } - if (!mean_computed_) { - if (image_processor_node_->idx_ == 0) { + if (!mean_computed_) + { + if (image_processor_node_->idx_ == 0) + { RCLCPP_ERROR(this->get_logger(), "No Aruco found, possible reasons: topic name wrong, " "Aruco id wrong or Aruco not in the field of view!"); - } else { + } + else + { RCLCPP_ERROR(this->get_logger(), "Timeout reached before collecting %ld observations: just %d " "measurements collected, try improving the timeout time!", @@ -187,16 +154,63 @@ void PoseService::get_camera_pose_service_callback( // unsubscribe from the camera topic image_processor_node_->sub_.reset(); // node_executor_.remove_node(image_processor_node_); - RCLCPP_INFO(this->get_logger(), "Mean computed correctly"); + RCLCPP_INFO(this->get_logger(), "Mean pose computed correctly after %d " + "observations", + image_processor_node_->idx_); get_camera_pose(mean_tvec_, mean_quat_, response->camera_pose); - response->camera_pose.header.frame_id = "base_link"; + response->camera_pose.header.frame_id = this->get_parameter("frame_id").as_string(); response->camera_pose.header.stamp = this->get_clock()->now(); mean_tvec_ = Eigen::Vector3d::Zero(); mean_quat_ = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); } -int main(int argc, char **argv) { +void PoseService::get_camera_pose(const Eigen::Vector3d &tvec, + const Eigen::Quaterniond &quat, + geometry_msgs::msg::PoseStamped &pose_msg) +{ + // rTc = rTa * cTa -> rTc = rTa * cTa where r is robot frame, a is aruco + // frame, c is camera frame + Eigen::Matrix3d R = quat.toRotationMatrix(); + + auto tvec_cTa = -R.transpose() * tvec; + Eigen::Quaterniond q(R.transpose()); + + // create aTc 4x4 matrix + Eigen::Matrix4d T_aruco_to_camera = Eigen::Matrix4d::Identity(); + T_aruco_to_camera.block<3, 3>(0, 0) = R.transpose(); + T_aruco_to_camera.block<3, 1>(0, 3) = tvec_cTa; + + // create rTa 4x4 matrix + Eigen::Matrix4d T_robot_to_aruco = Eigen::Matrix4d::Identity(); + T_robot_to_aruco.block<3, 3>(0, 0) = + Eigen::Quaterniond(aruco_rot_offset_from_baseframe_[0], + aruco_rot_offset_from_baseframe_[1], + aruco_rot_offset_from_baseframe_[2], + aruco_rot_offset_from_baseframe_[3]) + .normalized() + .toRotationMatrix(); + + T_robot_to_aruco.block<3, 1>(0, 3) = Eigen::Vector3d( + aruco_XYZ_offset_from_baseframe_[0], aruco_XYZ_offset_from_baseframe_[1], + aruco_XYZ_offset_from_baseframe_[2]); + + // create rTc 4x4 matrix + Eigen::Matrix4d T_robot_to_camera = T_robot_to_aruco * T_aruco_to_camera; + + pose_msg.pose.position.x = T_robot_to_camera(0, 3); + pose_msg.pose.position.y = T_robot_to_camera(1, 3); + pose_msg.pose.position.z = T_robot_to_camera(2, 3); + + Eigen::Quaterniond q_rTc(T_robot_to_camera.block<3, 3>(0, 0)); + pose_msg.pose.orientation.w = q_rTc.w(); + pose_msg.pose.orientation.x = q_rTc.x(); + pose_msg.pose.orientation.y = q_rTc.y(); + pose_msg.pose.orientation.z = q_rTc.z(); +} + +int main(int argc, char **argv) +{ rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::executors::MultiThreadedExecutor executor; diff --git a/calibration_srv/srv/Calibration.srv b/calibration_srv/srv/Calibration.srv index 085697b..c150aa1 100755 --- a/calibration_srv/srv/Calibration.srv +++ b/calibration_srv/srv/Calibration.srv @@ -1,3 +1,3 @@ -std_msgs/String req +uint16 marker_id --- geometry_msgs/PoseStamped camera_pose