cleanup

src/conversion.cpp

@@ -4,36 +4,35 @@
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <opencv2/core/mat.hpp>
-#include <opencv2/core/quaternion.hpp>
 #include <tf2/convert.h>
 
 template<>
-void tf2::convert(const Eigen::Quaterniond& eq, geometry_msgs::msg::Quaternion& gq)
+void tf2::convert(const Eigen::Quaterniond& eigen_quat, geometry_msgs::msg::Quaternion& msg_quat)
 {
-    gq.w = eq.w();
-    gq.x = eq.x();
-    gq.y = eq.y();
-    gq.z = eq.z();
+    msg_quat.w = eigen_quat.w();
+    msg_quat.x = eigen_quat.x();
+    msg_quat.y = eigen_quat.y();
+    msg_quat.z = eigen_quat.z();
 }
 
 template<>
-void tf2::convert(const matd_t& mat, geometry_msgs::msg::Vector3& t)
+void tf2::convert(const matd_t& mat, geometry_msgs::msg::Vector3& msg_vec)
 {
     assert(mat.nrows == 3 || mat.ncols == 3);
 
-    t.x = mat.data[0];
-    t.y = mat.data[1];
-    t.z = mat.data[2];
+    msg_vec.x = mat.data[0];
+    msg_vec.y = mat.data[1];
+    msg_vec.z = mat.data[2];
 }
 
 template<>
-void tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t)
+void tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& msg_vec)
 {
     assert(vec.rows == 3 || vec.cols == 3);
 
-    t.x = vec.at<double>(0);
-    t.y = vec.at<double>(1);
-    t.z = vec.at<double>(2);
+    msg_vec.x = vec.at<double>(0);
+    msg_vec.y = vec.at<double>(1);
+    msg_vec.z = vec.at<double>(2);
 }
 
 template<>
@@ -52,19 +51,13 @@ template<>
 geometry_msgs::msg::Transform
 tf2::toMsg(const std::pair<cv::Mat_<double>, cv::Mat_<double>>& pose)
 {
-    const cv::Quat<double> cvq = cv::Quat<double>::createFromRvec(pose.second);
-
-    std::cout << "cv: " << cvq << std::endl;
-
     assert(pose.first.rows == 3 || pose.first.cols == 3);
     assert(pose.second.rows == 3 || pose.second.cols == 3);
 
     // convert compact rotation vector to angle-axis to quaternion
     const Eigen::Map<const Eigen::Vector3d> rvec(reinterpret_cast<double*>(pose.second.data));
     const Eigen::Quaterniond q({rvec.norm(), rvec.normalized()});
 
-    std::cout << "eigen (x,y,z,w): " << q.coeffs().transpose() << std::endl;
-
     geometry_msgs::msg::Transform t;
     tf2::convert(pose.first, t.translation);
     tf2::convert(q, t.rotation);