improvement and debug

.gitignore

@@ -8,4 +8,10 @@
 *bin
 *~
 src/ar_track_alvar
-*.user
+*.user
+*.swp
+GPATH
+GRTAGS
+GTAGS
+GSYMS
+

ar_track_alvar/include/ar_track_alvar/Marker.h

@@ -100,6 +100,8 @@ namespace alvar {
     void SetMarkerSize(double _edge_length = 0, int _res = 0, double _margin = 0);
     /** \brief Get edge length (to support different size markers */
     double GetMarkerEdgeLength() const { return edge_length; }
+    /** \brief Method for Get the marker resolution  */
+    int GetMarkerRes(void);
     /** \brief Destructor  */
     ~Marker();
     /** \brief Default constructor 

ar_track_alvar/launch/pr2_bundle.launch

@@ -1,4 +1,5 @@
 <launch>
+	<arg name="inverse_tf" default="false" />
 	<arg name="marker_size" default="4.4" />
 	<arg name="max_new_marker_error" default="0.08" />
 	<arg name="max_track_error" default="0.0" />
@@ -10,5 +11,5 @@
     <arg name="med_filt_size" default="10" />
 	<arg name="bundle_files" default="$(find ar_track_alvar)/bundles/truthTableLeg.xml $(find ar_track_alvar)/bundles/table_8_9_10.xml" />
 
-	<node name="ar_track_alvar" pkg="ar_track_alvar" type="findMarkerBundles" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame) $(arg med_filt_size) $(arg bundle_files)" />
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="findMarkerBundles" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame) $(arg med_filt_size) $(arg inverse_tf) $(arg bundle_files)" />
 </launch>

ar_track_alvar/launch/pr2_bundle_no_kinect.launch

@@ -1,4 +1,5 @@
 <launch>
+	<arg name="inverse_tf" default="false" />
 	<arg name="marker_size" default="4.4" />
 	<arg name="max_new_marker_error" default="0.08" />
 	<arg name="max_track_error" default="0.2" />
@@ -9,5 +10,5 @@
 	<arg name="output_frame" default="/torso_lift_link" />
 	<arg name="bundle_files" default="$(find ar_track_alvar)/bundles/truthTableLeg.xml $(find ar_track_alvar)/bundles/table_8_9_10.xml" />
 
-	<node name="ar_track_alvar" pkg="ar_track_alvar" type="findMarkerBundlesNoKinect" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame) $(arg bundle_files)" />
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="findMarkerBundlesNoKinect" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame) $(arg inverse_tf) $(arg bundle_files)" />
 </launch>

ar_track_alvar/launch/pr2_indiv.launch

@@ -1,4 +1,5 @@
 <launch>
+	<arg name="inverse_tf" default="false" />
 	<arg name="marker_size" default="4.4" />
 	<arg name="max_new_marker_error" default="0.08" />
 	<arg name="max_track_error" default="0.2" />
@@ -12,6 +13,7 @@
 		<param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
 		<param name="max_track_error"       type="double" value="$(arg max_track_error)" />
 		<param name="output_frame"          type="string" value="$(arg output_frame)" />
+		<param name="inverse_tf"            type="bool"   value="$(arg inverse_tf)" />
 
 		<remap from="camera_image"  to="$(arg cam_image_topic)" />
 		<remap from="camera_info"   to="$(arg cam_info_topic)" />

ar_track_alvar/launch/pr2_indiv_no_kinect.launch

@@ -1,4 +1,5 @@
 <launch>
+	<arg name="inverse_tf" default="false" />
 	<arg name="marker_size" default="4.4" />
 	<arg name="max_new_marker_error" default="0.08" />
 	<arg name="max_track_error" default="0.2" />
@@ -11,6 +12,7 @@
 		<param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
 		<param name="max_track_error"       type="double" value="$(arg max_track_error)" />
 		<param name="output_frame"          type="string" value="$(arg output_frame)" />
+		<param name="inverse_tf"            type="bool"   value="$(arg inverse_tf)" />
 
 		<remap from="camera_image"  to="$(arg cam_image_topic)" />
 		<remap from="camera_info"   to="$(arg cam_info_topic)" />

ar_track_alvar/nodes/FindMarkerBundles.cpp

@@ -112,6 +112,8 @@ std::string cam_info_topic;
 std::string output_frame;
 int n_bundles = 0;   
 
+bool inverse_tf = false;
+
 //Debugging utility function
 void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad)
 {
@@ -527,8 +529,13 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_
   out << id;
   std::string id_string = out.str();
   markerFrame += id_string;
-  tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
-  tf_broadcaster->sendTransform(camToMarker);
+  if (inverse_tf) {
+    tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame.c_str(), output_frame);
+    tf_broadcaster->sendTransform(markerToCam);
+  } else {
+    tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
+    tf_broadcaster->sendTransform(camToMarker);
+  }
 
   //Create the rviz visualization message
   tf::poseTFToMsg (markerPose, rvizMarker->pose);
@@ -771,10 +778,10 @@ int main(int argc, char *argv[])
   ros::init (argc, argv, "marker_detect");
   ros::NodeHandle n;
 
-  if(argc < 9){
+  if(argc < 10){
     std::cout << std::endl;
     cout << "Not enough arguments provided." << endl;
-    cout << "Usage: ./findMarkerBundles <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame> <median filt size> <list of bundle XML files...>" << endl;
+    cout << "Usage: ./findMarkerBundles <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame> <median filt size> <inverse tf or not (true or false)> <list of bundle XML files...>" << endl;
     std::cout << std::endl;
     return 0;
   }
@@ -787,7 +794,12 @@ int main(int argc, char *argv[])
   cam_info_topic = argv[5];
   output_frame = argv[6];
   med_filt_size = atoi(argv[7]);
-  int n_args_before_list = 8;
+  std::stringstream tmpss(argv[8]);
+  if(!(tmpss >> std::boolalpha >> inverse_tf)) {
+    // parse failed
+    inverse_tf = false;
+  }
+  int n_args_before_list = 9;
   n_bundles = argc - n_args_before_list;
 
   marker_detector.SetMarkerSize(marker_size);

ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp

@@ -82,6 +82,8 @@ void GetMultiMarkerPoses(IplImage *image);
 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);
 void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker);
 
+bool inverse_tf = false;
+
 
 // Updates the bundlePoses of the multi_marker_bundles by detecting markers and using all markers in a bundle to infer the master tag's position
 void GetMultiMarkerPoses(IplImage *image) {
@@ -128,8 +130,14 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_
     out << id;
     std::string id_string = out.str();
     markerFrame += id_string;
-    tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
-    tf_broadcaster->sendTransform(camToMarker);
+    if (inverse_tf) {
+      tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame.c_str(), output_frame);
+      tf_broadcaster->sendTransform(markerToCam);
+    } else {
+      tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
+      tf_broadcaster->sendTransform(camToMarker);
+    }
+
   }
 
   //Create the rviz visualization message
@@ -281,10 +289,10 @@ int main(int argc, char *argv[])
   ros::init (argc, argv, "marker_detect");
   ros::NodeHandle n;
 
-  if(argc < 8){
+  if(argc < 9){
     std::cout << std::endl;
     cout << "Not enough arguments provided." << endl;
-    cout << "Usage: ./findMarkerBundles <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame> <list of bundle XML files...>" << endl;
+    cout << "Usage: ./findMarkerBundles <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame> <inverse tf or not (true or false)> <list of bundle XML files...>" << endl;
     std::cout << std::endl;
     return 0;
   }
@@ -296,7 +304,12 @@ int main(int argc, char *argv[])
   cam_image_topic = argv[4];
   cam_info_topic = argv[5];
   output_frame = argv[6];
-  int n_args_before_list = 7;
+  std::stringstream tmpss(argv[7]);
+  if(!(tmpss >> std::boolalpha >> inverse_tf)) {
+    // parse failed
+    inverse_tf = false;
+  }
+  int n_args_before_list = 8;
   n_bundles = argc - n_args_before_list;
 
   marker_detector.SetMarkerSize(marker_size);

ar_track_alvar/nodes/IndividualMarkers.cpp

@@ -86,6 +86,8 @@ std::string output_frame;
 int marker_resolution = 5; // default marker resolution
 int marker_margin = 2; // default marker margin
 
+bool inverse_tf = false;
+
 
 //Debugging utility function
 void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad)
@@ -374,6 +376,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
 	{
 	  //Get the pose relative to the camera
 	  int id = (*(marker_detector.markers))[i].GetId();
+	  std::string content_str = (*(marker_detector.markers))[i].data.str;
 	  Pose p = (*(marker_detector.markers))[i].pose;
 
 	  double px = p.translation[0]/100.0;
@@ -397,8 +400,13 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
 	  out << id;
 	  std::string id_string = out.str();
 	  markerFrame += id_string;
-	  tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
-	  tf_broadcaster->sendTransform(camToMarker);
+          if (inverse_tf) {
+            tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame.c_str(), output_frame);
+            tf_broadcaster->sendTransform(markerToCam);
+          } else {
+            tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
+            tf_broadcaster->sendTransform(camToMarker);
+          }
 
 	  //Create the rviz visualization messages
 	  tf::poseTFToMsg (markerPose, rvizMarker_.pose);
@@ -463,6 +471,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
 	  ar_pose_marker.header.frame_id = output_frame;
 	  ar_pose_marker.header.stamp = image_msg->header.stamp;
 	  ar_pose_marker.id = id;
+	  ar_pose_marker.str = content_str;
 	  arPoseMarkers_.markers.push_back (ar_pose_marker);
 	}
       arPoseMarkers_.header.stamp = image_msg->header.stamp;
@@ -500,7 +509,7 @@ int main(int argc, char *argv[])
       std::cout << std::endl;
       cout << "Not enough arguments provided." << endl;
       cout << "Usage: ./individualMarkers <marker size in cm> <max new marker error> <max track error> "
-           << "<cam image topic> <cam info topic> <output frame> [ <max frequency> <marker_resolution> <marker_margin>]";
+           << "<cam image topic> <cam info topic> <output frame> [ <max frequency> <marker_resolution> <marker_margin> <inverse tf or not (true or false)> ]";
       std::cout << std::endl;
       return 0;
     }
@@ -522,6 +531,16 @@ int main(int argc, char *argv[])
     if (argc > 9)
       marker_margin = atoi(argv[9]);
 
+    if (argc > 10) {
+      std::stringstream tmpss(argv[10]);
+      if(!(tmpss >> std::boolalpha >> inverse_tf)) {
+        // parse failed
+        inverse_tf = false;
+      }
+    } else {
+      inverse_tf = false;
+    }
+
   } else {
     // Get params from ros param server.
     pn.param("marker_size", marker_size, 10.0);
@@ -532,6 +551,7 @@ int main(int argc, char *argv[])
     pn.param("marker_resolution", marker_resolution, 5);
     pn.param("marker_margin", marker_margin, 2);
     pn.param("output_frame_from_msg", output_frame_from_msg, false);
+    pn.param("inverse_tf", inverse_tf, false);
 
     if (!output_frame_from_msg && !pn.getParam("output_frame", output_frame)) {
       ROS_ERROR("Param 'output_frame' has to be set if the output frame is not "

ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp

@@ -75,6 +75,8 @@ std::string output_frame;
 int marker_resolution = 5; // default marker resolution
 int marker_margin = 2; // default marker margin
 
+bool inverse_tf = false;
+
 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);
 
 
@@ -109,6 +111,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
 			{
 				//Get the pose relative to the camera
         		int id = (*(marker_detector.markers))[i].GetId();
+        		std::string content_str = (*(marker_detector.markers))[i].data.str;
 				Pose p = (*(marker_detector.markers))[i].pose;
 				double px = p.translation[0]/100.0;
 				double py = p.translation[1]/100.0;
@@ -139,8 +142,14 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
 				out << id;
 				std::string id_string = out.str();
 				markerFrame += id_string;
-				tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
-    			tf_broadcaster->sendTransform(camToMarker);
+				if (inverse_tf) {
+				  tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame.c_str(), output_frame);
+				  tf_broadcaster->sendTransform(markerToCam);
+				} else {
+				  tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
+				  tf_broadcaster->sendTransform(camToMarker);
+				}
+
 
 				//Create the rviz visualization messages
 				tf::poseTFToMsg (markerPose, rvizMarker_.pose);
@@ -205,6 +214,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
       			ar_pose_marker.header.frame_id = output_frame;
 			    ar_pose_marker.header.stamp = image_msg->header.stamp;
 			    ar_pose_marker.id = id;
+			    ar_pose_marker.str = content_str;
 			    arPoseMarkers_.markers.push_back (ar_pose_marker);
 			}
 			arMarkerPub_.publish (arPoseMarkers_);
@@ -247,7 +257,7 @@ int main(int argc, char *argv[])
       std::cout << std::endl;
       cout << "Not enough arguments provided." << endl;
       cout << "Usage: ./individualMarkersNoKinect <marker size in cm> <max new marker error> <max track error> "
-           << "<cam image topic> <cam info topic> <output frame> [ <max frequency> <marker_resolution> <marker_margin>]";
+           << "<cam image topic> <cam info topic> <output frame> [ <max frequency> <marker_resolution> <marker_margin> <inverse tf or not (true or false)> ]";
       std::cout << std::endl;
       return 0;
     }
@@ -268,6 +278,15 @@ int main(int argc, char *argv[])
       marker_resolution = atoi(argv[8]);
     if (argc > 9)
       marker_margin = atoi(argv[9]);
+    if (argc > 10) {
+      std::stringstream tmpss(argv[10]);
+      if(!(tmpss >> std::boolalpha >> inverse_tf)) {
+        // parse failed
+        inverse_tf = false;
+      }
+    } else {
+      inverse_tf = false;
+    }
 
   } else {
     // Get params from ros param server.
@@ -278,6 +297,7 @@ int main(int argc, char *argv[])
     pn.setParam("max_frequency", max_frequency);  // in case it was not set.
     pn.param("marker_resolution", marker_resolution, 5);
     pn.param("marker_margin", marker_margin, 2);
+    pn.param("inverse_tf", inverse_tf, false);
     if (!pn.getParam("output_frame", output_frame)) {
       ROS_ERROR("Param 'output_frame' has to be set.");
       exit(EXIT_FAILURE);

ar_track_alvar/src/Marker.cpp

@@ -512,6 +512,10 @@ void Marker::SetMarkerSize(double _edge_length, int _res, double _margin) {
 	marker_content = cvCreateMat(res, res, CV_8U);
 	cvSet(marker_content, cvScalar(255));
 }
+int Marker::GetMarkerRes(void){
+	return res;
+}
+
 Marker::~Marker() {
 	if (marker_content) cvReleaseMat(&marker_content);
 }
@@ -838,7 +842,7 @@ void MarkerData::Read6bitStr(BitsetExt *bs, char *s, size_t s_max_len) {
 		if (bitpos < 0) {
 			if (c == 000)                      s[len] = ':';
 			else if ((c >= 001) && (c <= 032)) s[len] = 'a' + (char)c - 1;
-			else if ((c >= 033) && (c <= 044)) s[len] = '0' + (char)c - 1;
+			else if ((c >= 033) && (c <= 044)) s[len] = '0' + (char)c - 033;
 			else if (c == 045)                 s[len] = '+';
 			else if (c == 046)                 s[len] = '-';
 			else if (c == 047)                 s[len] = '*';