diff --git a/ccny_vision/ar_pose/include/ar_pose/ar_multi.h b/ccny_vision/ar_pose/include/ar_pose/ar_multi.h
index 5b84e05..aa47648 100644
--- a/ccny_vision/ar_pose/include/ar_pose/ar_multi.h
+++ b/ccny_vision/ar_pose/include/ar_pose/ar_multi.h
@@ -69,6 +69,7 @@ namespace ar_pose
 
       ros::NodeHandle n_;
       tf::TransformBroadcaster broadcaster_;
+      ros::Publisher tfPub_;
       ros::Subscriber sub_;
       image_transport::Subscriber cam_sub_;
       ros::Publisher arMarkerPub_;
@@ -84,11 +85,10 @@ namespace ar_pose
     // **** parameters
     ARParam cam_param_;         // Camera Calibration Parameters
     ARMultiMarkerInfoT *config; // AR Marker Info
-      ar_object::ObjectData_T * object;
+    ar_object::ObjectData_T * object;
     int objectnum;
-    char pattern_filename_[FILENAME_MAX];
-
-      ar_pose::ARMarkers arPoseMarkers_;
+    std::string pattern_filename;
+    ar_pose::ARMarkers arPoseMarkers_;
     int threshold_;
     bool getCamInfo_;
     bool publishTf_;
diff --git a/ccny_vision/ar_pose/include/ar_pose/object.h b/ccny_vision/ar_pose/include/ar_pose/object.h
index 8352a96..a0493b6 100755
--- a/ccny_vision/ar_pose/include/ar_pose/object.h
+++ b/ccny_vision/ar_pose/include/ar_pose/object.h
@@ -17,6 +17,6 @@ namespace ar_object
     double marker_center[2];
   } ObjectData_T;
 
-  ObjectData_T *read_ObjData (char *name, int *objectnum);
+  ObjectData_T *read_ObjData (std::string name, int *objectnum);
 }
 #endif
diff --git a/ccny_vision/ar_pose/src/ar_multi.cpp b/ccny_vision/ar_pose/src/ar_multi.cpp
index 7f86936..73f389a 100644
--- a/ccny_vision/ar_pose/src/ar_multi.cpp
+++ b/ccny_vision/ar_pose/src/ar_multi.cpp
@@ -25,8 +25,8 @@
 
 int main (int argc, char **argv)
 {
-  ros::init (argc, argv, "ar_single");
-  ros::NodeHandle n;
+  ros::init (argc, argv, "ar_multi");
+  ros::NodeHandle n("~");
   ar_pose::ARSinglePublisher ar_single (n);
   ros::spin ();
   return 0;
@@ -38,26 +38,30 @@ namespace ar_pose
   {
     std::string local_path;
     std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
-    ros::NodeHandle n_param ("~");
     XmlRpc::XmlRpcValue xml_marker_center;
 
     // **** get parameters
 
-    if (!n_param.getParam ("publish_tf", publishTf_))
+    if (!n_.getParam ("publish_tf", publishTf_))
       publishTf_ = true;
+
     ROS_INFO ("\tPublish transforms: %d", publishTf_);
 
-    if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
+    if (!n_.getParam ("publish_visual_markers", publishVisualMarkers_))
       publishVisualMarkers_ = true;
     ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
 
-    if (!n_param.getParam ("threshold", threshold_))
+    if (!n_.getParamCached ("threshold", threshold_)) {
       threshold_ = 100;
+      ROS_INFO ("Did not get a threshold parameter.");
+    }
     ROS_INFO ("\tThreshold: %d", threshold_);
 
-    n_param.param ("marker_pattern_list", local_path, std::string ("data/object_data2"));
-    sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
-    ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);
+    if (!n_.getParam("marker_pattern_list", pattern_filename)) {
+        pattern_filename = package_path + "/data/object_data2"; 
+    }
+
+    ROS_INFO ("Marker Pattern Filename: %s", pattern_filename.c_str());
 
     // **** subscribe
 
@@ -71,7 +75,8 @@ namespace ar_pose
     if(publishVisualMarkers_)
     {
 		rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
-	 }
+    }
+  
   }
 
   ARSinglePublisher::~ARSinglePublisher (void)
@@ -103,15 +108,20 @@ namespace ar_pose
       cam_param_.mat[1][3] = cam_info_.P[7];
       cam_param_.mat[2][3] = cam_info_.P[11];
 
-      cam_param_.dist_factor[0] = cam_info_.K[3];       // x0 = cX from openCV calibration
-      cam_param_.dist_factor[1] = cam_info_.K[6];       // y0 = cY from openCV calibration
-      cam_param_.dist_factor[2] = -100*cam_info_.D[0];  // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
-      cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...
-      
+      /* FIXME : don't work with ROS distortion factor
+       * ROS Calibration : D=[0.025751483065329935, -0.10530741936574876, -0.0024821434601277623, -0.0031632353637182972, 0.0000]
+       * ARToolkit Calibration : D=[265.000000, 257.000000, 4.600000,   1.002303, 0.0000]*/
+      // A fix is to deactivate the distorsion correction
+      // and feed this program with rectified images
+      cam_param_.dist_factor[0] = 0.0;
+      cam_param_.dist_factor[1] = 0.0;
+      cam_param_.dist_factor[2] = 0.0;
+      cam_param_.dist_factor[3] = 1.0;  //Scaling factor
       arInit ();
 
       ROS_INFO ("Subscribing to image topic");
       cam_sub_ = it_.subscribe (cameraImageTopic_, 1, &ARSinglePublisher::getTransformationCallback, this);
+      printf("Worked!\n");
       getCamInfo_ = true;
     }
   }
@@ -123,7 +133,7 @@ namespace ar_pose
     arParamDisp (&cam_param_);
 
     // load in the object data - trained markers and associated bitmap files
-    if ((object = ar_object::read_ObjData (pattern_filename_, &objectnum)) == NULL)
+    if ((object = ar_object::read_ObjData (pattern_filename, &objectnum)) == NULL)
       ROS_BREAK ();
     ROS_DEBUG ("Objectfile num = %d", objectnum);
 
@@ -137,6 +147,11 @@ namespace ar_pose
     ARMarkerInfo *marker_info;
     int marker_num;
     int i, k, j;
+    std::vector< geometry_msgs::TransformStamped > tf_vec;
+
+
+    //adjust the threshold
+    n_.getParamCached ("threshold", threshold_);
 
     /* Get the image from ROSTOPIC
      * NOTE: the dataPtr format is BGR because the ARToolKit library was
@@ -217,83 +232,112 @@ namespace ar_pose
       ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
 
       // **** publish the marker
+      // only publish if the marker is in front of the camera
+      if (pos[2] > 0.0) {
+          ar_pose::ARMarker ar_pose_marker;
+          ar_pose_marker.header.frame_id = image_msg->header.frame_id;
+          ar_pose_marker.header.stamp = image_msg->header.stamp;
+          ar_pose_marker.id = object[i].id;
+
+          ar_pose_marker.pose.pose.position.x = pos[0];
+          ar_pose_marker.pose.pose.position.y = pos[1];
+          ar_pose_marker.pose.pose.position.z = pos[2];
+
+          ar_pose_marker.pose.pose.orientation.x = quat[0];
+          ar_pose_marker.pose.pose.orientation.y = quat[1];
+          ar_pose_marker.pose.pose.orientation.z = quat[2];
+          ar_pose_marker.pose.pose.orientation.w = quat[3];
+
+          ar_pose_marker.confidence = marker_info->cf;
+          
+          arPoseMarkers_.markers.push_back (ar_pose_marker);
+
+          // **** publish transform between camera and marker
+          btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
+          btVector3 origin (pos[0], pos[1], pos[2]);
+          btTransform t (rotation, origin);
+
+
+
+          if (publishTf_)
+          {
+              geometry_msgs::Quaternion rot;
+              rot.x = quat[0];
+              rot.y = quat[1];
+              rot.z = quat[2];
+              rot.w = quat[3];
+              geometry_msgs::Vector3 trans;
+              trans.x = pos[0];
+              trans.y = pos[1];
+              trans.z = pos[2];
+              geometry_msgs::TransformStamped camToMarker;
+              camToMarker.header.stamp = image_msg->header.stamp;
+              camToMarker.header.frame_id = image_msg ->header.frame_id;
+              camToMarker.transform.translation = trans;
+              camToMarker.transform.rotation = rot;
+              camToMarker.child_frame_id = object[i].name;
+              tf_vec.push_back( camToMarker );
+          }
+
+          // **** publish visual marker
+
+          if (publishVisualMarkers_)
+          {
+            btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
+            btTransform m (btQuaternion::getIdentity (), markerOrigin);
+            btTransform markerPose = t * m; // marker pose in the camera frame
+
+            tf::poseTFToMsg (markerPose, rvizMarker_.pose);
+
+            rvizMarker_.header.frame_id = image_msg->header.frame_id;
+            rvizMarker_.header.stamp = image_msg->header.stamp;
+            rvizMarker_.id = object[i].id;
+
+            rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
+            rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
+            rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
+            rvizMarker_.ns = "basic_shapes";
+            rvizMarker_.type = visualization_msgs::Marker::CUBE;
+            rvizMarker_.action = visualization_msgs::Marker::ADD;
+            switch (i)
+            {
+              case 0:
+                rvizMarker_.color.r = 0.0f;
+                rvizMarker_.color.g = 0.0f;
+                rvizMarker_.color.b = 1.0f;
+                rvizMarker_.color.a = 1.0;
+                break;
+              case 1:
+                rvizMarker_.color.r = 1.0f;
+                rvizMarker_.color.g = 0.0f;
+                rvizMarker_.color.b = 0.0f;
+                rvizMarker_.color.a = 1.0;
+                break;
+              default:
+                rvizMarker_.color.r = 0.0f;
+                rvizMarker_.color.g = 1.0f;
+                rvizMarker_.color.b = 0.0f;
+                rvizMarker_.color.a = 1.0;
+            }
+            rvizMarker_.lifetime = ros::Duration ();
+
+            rvizMarkerPub_.publish (rvizMarker_);
+            ROS_DEBUG ("Published visual marker");
+          }
+        }
+     }
+    arMarkerPub_.publish (arPoseMarkers_);
 
-      ar_pose::ARMarker ar_pose_marker;
-      ar_pose_marker.header.frame_id = image_msg->header.frame_id;
-      ar_pose_marker.header.stamp = image_msg->header.stamp;
-      ar_pose_marker.id = object[i].id;
-
-      ar_pose_marker.pose.pose.position.x = pos[0];
-      ar_pose_marker.pose.pose.position.y = pos[1];
-      ar_pose_marker.pose.pose.position.z = pos[2];
-
-      ar_pose_marker.pose.pose.orientation.x = quat[0];
-      ar_pose_marker.pose.pose.orientation.y = quat[1];
-      ar_pose_marker.pose.pose.orientation.z = quat[2];
-      ar_pose_marker.pose.pose.orientation.w = quat[3];
-
-      ar_pose_marker.confidence = marker_info->cf;
-      arPoseMarkers_.markers.push_back (ar_pose_marker);
-
-      // **** publish transform between camera and marker
-
-      btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
-      btVector3 origin (pos[0], pos[1], pos[2]);
-      btTransform t (rotation, origin);
-
-      if (publishTf_)
-      {
-			tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, object[i].name);
-			broadcaster_.sendTransform(camToMarker);
-      }
+    if (publishTf_) {
+        if (tf_vec.size() > 0) {
+            // sendTransform (const std::vector< geometry_msgs::TransformStamped > &transforms)
+            broadcaster_.sendTransform(tf_vec);
+        }
+    }
 
-      // **** publish visual marker
 
-      if (publishVisualMarkers_)
-      {
-        btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
-        btTransform m (btQuaternion::getIdentity (), markerOrigin);
-        btTransform markerPose = t * m; // marker pose in the camera frame
-
-        tf::poseTFToMsg (markerPose, rvizMarker_.pose);
-
-        rvizMarker_.header.frame_id = image_msg->header.frame_id;
-        rvizMarker_.header.stamp = image_msg->header.stamp;
-        rvizMarker_.id = object[i].id;
-
-        rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
-        rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
-        rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
-        rvizMarker_.ns = "basic_shapes";
-        rvizMarker_.type = visualization_msgs::Marker::CUBE;
-        rvizMarker_.action = visualization_msgs::Marker::ADD;
-        switch (i)
-        {
-          case 0:
-            rvizMarker_.color.r = 0.0f;
-            rvizMarker_.color.g = 0.0f;
-            rvizMarker_.color.b = 1.0f;
-            rvizMarker_.color.a = 1.0;
-            break;
-          case 1:
-            rvizMarker_.color.r = 1.0f;
-            rvizMarker_.color.g = 0.0f;
-            rvizMarker_.color.b = 0.0f;
-            rvizMarker_.color.a = 1.0;
-            break;
-          default:
-            rvizMarker_.color.r = 0.0f;
-            rvizMarker_.color.g = 1.0f;
-            rvizMarker_.color.b = 0.0f;
-            rvizMarker_.color.a = 1.0;
-        }
-        rvizMarker_.lifetime = ros::Duration ();
+    //Publish to TF
 
-        rvizMarkerPub_.publish (rvizMarker_);
-        ROS_DEBUG ("Published visual marker");
-      }
-    }
-    arMarkerPub_.publish (arPoseMarkers_);
     ROS_DEBUG ("Published ar_multi markers");
   }
 }                               // end namespace ar_pose
diff --git a/ccny_vision/ar_pose/src/ar_single.cpp b/ccny_vision/ar_pose/src/ar_single.cpp
index f1f663d..bebde85 100755
--- a/ccny_vision/ar_pose/src/ar_single.cpp
+++ b/ccny_vision/ar_pose/src/ar_single.cpp
@@ -57,7 +57,7 @@ namespace ar_pose
     ROS_INFO ("\tThreshold: %d", threshold_);
 
     if (!n_param.getParam("marker_width", markerWidth_))
-      markerWidth_ = 80.0;
+      markerWidth_ = 20.0;
     ROS_INFO ("\tMarker Width: %.1f", markerWidth_);
 
     if (!n_param.getParam("reverse_transform", reverse_transform_))
@@ -75,7 +75,7 @@ namespace ar_pose
       useHistory_ = true;
     ROS_INFO("\tUse history: %d", useHistory_);
 
-    n_param.param ("marker_pattern", local_path, std::string ("data/patt.hiro"));
+    n_param.param ("marker_pattern", local_path, std::string ("data/4x4/4x4_1.patt"));
     sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
     ROS_INFO ("\tMarker Pattern Filename: %s", pattern_filename_);
 
@@ -126,11 +126,18 @@ namespace ar_pose
       cam_param_.mat[1][3] = cam_info_.P[7];
       cam_param_.mat[2][3] = cam_info_.P[11];
      
-	   cam_param_.dist_factor[0] = cam_info_.K[3];       // x0 = cX from openCV calibration
-      cam_param_.dist_factor[1] = cam_info_.K[6];       // y0 = cY from openCV calibration
-      cam_param_.dist_factor[2] = -100*cam_info_.D[0];  // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
-      cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...
-	     
+      /* FIXME : don't work with OpenCV or Matlab distortion factor
+       * OpenCV Calibration : D=[0.025751483065329935, -0.10530741936574876,
+	   *	-0.0024821434601277623, -0.0031632353637182972, 0.0000]
+	   * ARToolkit Calibration : D=[265.000000, 257.000000, 4.600000,
+	   *	1.002303, 0.0000] */
+      // A fix is to deactivate the distorsion correction
+      // and feed this program with rectified images
+      cam_param_.dist_factor[0] = 0.0;
+	  cam_param_.dist_factor[1] = 0.0;
+	  cam_param_.dist_factor[2] = 0.0;
+	  cam_param_.dist_factor[3] = 1.0;  //Scaling factor
+     
       arInit();
 
       ROS_INFO ("Subscribing to image topic");
diff --git a/ccny_vision/ar_pose/src/object.cpp b/ccny_vision/ar_pose/src/object.cpp
index 7e0dd3a..3d7f77d 100755
--- a/ccny_vision/ar_pose/src/object.cpp
+++ b/ccny_vision/ar_pose/src/object.cpp
@@ -37,17 +37,18 @@ namespace ar_object
 
   static char *get_buff (char *buf, int n, FILE * fp);
 
-  ObjectData_T *read_ObjData (char *name, int *objectnum)
+  ObjectData_T *read_ObjData (std::string name, int *objectnum)
   {
     FILE *fp;
     ObjectData_T *object;
     char buf[256], buf1[256];
     int i;
-      std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
+   
+    std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
 
-      ROS_INFO ("Opening Data File %s", name);
+    ROS_INFO ("Opening Data File %s", name.c_str());
 
-    if ((fp = fopen (name, "r")) == NULL)
+    if ((fp = fopen (name.c_str(), "r")) == NULL)
     {
       ROS_INFO ("Can't find the file - quitting");
       ROS_BREAK ();
