add publishing PoseCentroid

ScazLab · Sep 12, 2019 · 5e25156 · 5e25156
1 parent 3a4c582
commit 5e25156
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 15 deletions.
diff --git a/aruco/include/aruco/marker.h b/aruco/include/aruco/marker.h
@@ -81,12 +81,12 @@ class  ARUCO_EXPORTS Marker: public std::vector<cv::Point2f>
      * @param setYPerpendicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis
      */
     void calculateExtrinsics(float markerSize, cv::Mat  CameraMatrix, cv::Mat Distorsion=cv::Mat(), bool setYPerpendicular=true)throw(cv::Exception);
-    
+
     /**Given the extrinsic camera parameters returns the GL_MODELVIEW matrix for opengl.
      * Setting this matrix, the reference coordinate system will be set in this marker
      */
     void glGetModelViewMatrix(  double modelview_matrix[16])throw(cv::Exception);
-    
+
     /**
      * Returns position vector and orientation quaternion for an Ogre scene node or entity.
      * 	Use:
@@ -97,8 +97,8 @@ class  ARUCO_EXPORTS Marker: public std::vector<cv::Point2f>
      * mySceneNode->setOrientation( ogreOrient  );
      * ...
      */
-    void OgreGetPoseParameters(  double position[3], double orientation[4] )throw(cv::Exception);    
-    
+    void OgreGetPoseParameters(  double position[3], double orientation[4] )throw(cv::Exception);
+
   /**Returns the centroid of the marker
       */
     cv::Point2f getCenter()const;
@@ -132,11 +132,11 @@ class  ARUCO_EXPORTS Marker: public std::vector<cv::Point2f>
 
         return str;
     }
-    
- 
+
+
 private:
   void rotateXAxis(cv::Mat &rotation);
- 
+
 };
 
 }

diff --git a/aruco/src/aruco/marker.cpp b/aruco/src/aruco/marker.cpp
@@ -272,12 +272,18 @@ namespace aruco {
 
     cv::Mat ImagePoints(4,2,CV_32FC1);
 
-    //Set image points from the marker
+    //Set image points from the marker and calculate the centroid on image
+    // Point cent(0,0);
     for (int c=0;c<4;c++)
     {
       ImagePoints.at<float>(c,0)=((*this)[c%4].x);
       ImagePoints.at<float>(c,1)=((*this)[c%4].y);
+
+      // cent.x += ImagePoints.at<float>(c,0);
+      // cent.y += ImagePoints.at<float>(c,1)
     }
+    // cent.x/=4.;
+    // cent.y/=4.;
 
     cv::Mat raux,taux;
     cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff,raux,taux);

diff --git a/aruco_msgs/CMakeLists.txt b/aruco_msgs/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   std_msgs)
 
-add_message_files( FILES Marker.msg MarkerArray.msg )
+add_message_files( FILES Marker.msg MarkerArray.msg PoseWithCentroid.msg)
 generate_messages( DEPENDENCIES std_msgs geometry_msgs )
 
 catkin_package(

diff --git a/aruco_msgs/msg/PoseWithCentroid.msg b/aruco_msgs/msg/PoseWithCentroid.msg
@@ -0,0 +1,3 @@
+Header header
+geometry_msgs/Pose pose
+geometry_msgs/Point image_centroid
diff --git a/aruco_ros/src/simple_single.cpp b/aruco_ros/src/simple_single.cpp
@@ -46,6 +46,8 @@ or implied, of Rafael Muñoz Salinas.
 #include <tf/transform_broadcaster.h>
 #include <tf/transform_listener.h>
 
+#include <aruco_msgs/PoseWithCentroid.h>
+
 using namespace aruco;
 
 class ArucoSimple
@@ -62,7 +64,8 @@ class ArucoSimple
   image_transport::Publisher image_pub;
   image_transport::Publisher debug_pub;
   ros::Publisher pose_pub;
-  ros::Publisher transform_pub; 
+  ros::Publisher pose_with_centroid_pub;
+  ros::Publisher transform_pub;
   ros::Publisher position_pub;
   std::string marker_frame;
   std::string camera_frame;
@@ -91,8 +94,8 @@ class ArucoSimple
     else if ( refinementMethod == "HARRIS" )
       mDetector.setCornerRefinementMethod(aruco::MarkerDetector::HARRIS);
     else if ( refinementMethod == "NONE" )
-      mDetector.setCornerRefinementMethod(aruco::MarkerDetector::NONE); 
-    else      
+      mDetector.setCornerRefinementMethod(aruco::MarkerDetector::NONE);
+    else
       mDetector.setCornerRefinementMethod(aruco::MarkerDetector::LINES);
 
     float min_size;
@@ -108,6 +111,7 @@ class ArucoSimple
     image_pub = it.advertise("result", 1);
     debug_pub = it.advertise("debug", 1);
     pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 100);
+    pose_with_centroid_pub = nh.advertise<aruco_msgs::PoseWithCentroid>("pose_with_centroid", 100);
     transform_pub = nh.advertise<geometry_msgs::TransformStamped>("transform", 100);
     position_pub = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
 
@@ -198,9 +202,9 @@ class ArucoSimple
                            cameraToReference);
             }
 
-            transform = 
-              static_cast<tf::Transform>(cameraToReference) 
-              * static_cast<tf::Transform>(rightToLeft) 
+            transform =
+              static_cast<tf::Transform>(cameraToReference)
+              * static_cast<tf::Transform>(rightToLeft)
               * transform;
 
             tf::StampedTransform stampedTransform(transform, curr_stamp,
@@ -216,6 +220,13 @@ class ArucoSimple
             tf::transformStampedTFToMsg(stampedTransform, transformMsg);
             transform_pub.publish(transformMsg);
 
+            aruco_msgs::PoseWithCentroid poseWithCentroidMsg;
+            poseWithCentroidMsg.header = transformMsg.header;
+            poseWithCentroidMsg.pose = poseMsg.pose;
+            poseWithCentroidMsg.image_centroid.x = markers[i].getCenter().x;
+            poseWithCentroidMsg.image_centroid.y = markers[i].getCenter().y;
+            pose_with_centroid_pub.publish(poseWithCentroidMsg);
+
             geometry_msgs::Vector3Stamped positionMsg;
             positionMsg.header = transformMsg.header;
             positionMsg.vector = transformMsg.transform.translation;