diff --git a/aruco_pose/src/aruco_pose.cpp b/aruco_pose/src/aruco_pose.cpp
index def8541e..77f17d80 100644
--- a/aruco_pose/src/aruco_pose.cpp
+++ b/aruco_pose/src/aruco_pose.cpp
@@ -3,7 +3,6 @@
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/imgproc/imgproc.hpp>
-//#include <tf2_ros/transform_broadcaster.h>
 #include <pluginlib/class_list_macros.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/PoseStamped.h>
@@ -17,13 +16,9 @@
 #include <stdio.h>
 #include <tf/transform_broadcaster.h>
 
-// #include <aruco_pose/MarkerArray.h>
-// #include <aruco_pose/Marker.h>
-
 namespace aruco_pose {
 
 class ArucoPose : public nodelet::Nodelet {
-//    tf2_ros::TransformBroadcaster br;
     tf::TransformBroadcaster br;
     cv::Ptr<cv::aruco::Dictionary> dictionary;
     cv::Ptr<cv::aruco::DetectorParameters> parameters;
@@ -37,15 +32,10 @@ class ArucoPose : public nodelet::Nodelet {
 
     virtual void onInit();
     void createBoard();
-    void publishVisualizationMarkers();
     cv::Point3f getObjPointsCenter(cv::Mat objPoints);
     void detect(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&);
     void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr&, cv::Mat&, cv::Mat&);
     tf::Transform aruco2tf(cv::Mat rvec, cv::Mat tvec);
-
-public:
-      ArucoPose() {};
-      virtual ~ArucoPose() {};
 };
 
 void ArucoPose::onInit() {
@@ -67,8 +57,6 @@ void ArucoPose::onInit() {
 
     pose_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose", 1);
 
-    publishVisualizationMarkers();
-
     ROS_INFO("aruco_pose nodelet inited");
 }
 
@@ -152,7 +140,7 @@ void ArucoPose::createBoard()
     else if (type == "custom")
     {
         // Not implemented yet
-        ROS_FATAL("Custom boards are not implemented yet.")
+        ROS_FATAL("Custom boards are not implemented yet.");
     }
     else
     {
@@ -160,34 +148,6 @@ void ArucoPose::createBoard()
     }
 }
 
-void ArucoPose::publishVisualizationMarkers()
-{
-    // Create latched publisher
-    static auto viz_markers_pub = nh_.advertise<visualization_msgs::MarkerArray>("viz", 1, true);
-    visualization_msgs::MarkerArray viz;
-    visualization_msgs::Marker marker;
-    marker.header.frame_id = "world";
-    marker.type = visualization_msgs::Marker::CUBE;
-    marker.scale.x = 1;
-    marker.scale.y = 1;
-    marker.scale.z = 0.001;
-    marker.color.r = 1;
-    marker.color.g = 1;
-    marker.color.b = 1;
-    marker.color.a = 0.9;
-    marker.frame_locked = true;
-    viz.markers.push_back(marker);
-    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-    marker.scale.z = 0.3;
-    marker.color.r = 0;
-    marker.color.g = 0;
-    marker.color.b = 0;
-    marker.color.a = 0.8;
-    marker.text = "240";
-    viz.markers.push_back(marker);
-    viz_markers_pub.publish(viz);
-}
-
 cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) {
     float min_x = std::numeric_limits<float>::max();
     float max_x = std::numeric_limits<float>::min();
@@ -217,75 +177,14 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs:
     cv::Mat distCoeffs(8, 1, CV_64F);
     parseCameraInfo(cinfo, cameraMatrix, distCoeffs);
 
-    // std::cout << "dist " << distCoeffs << " mat " << cameraMatrix;
-    // std::cout << markerIds.size() << std::endl;
-
-    // cv::Vec3d rvec, tvec;
-    // int valid = cv::aruco::estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, rvec, tvec);
-
-    // std::vector< cv::Vec3d > rvecs, tvecs;
-    //cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.3362, cameraMatrix, distCoeffs, rvecs, tvecs);
-    // cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.15, cameraMatrix, distCoeffs, rvecs, tvecs);
-
-
-    // Publish markers
-    // aruco_pose::MarkerArray markerArray;
-    // markerArray.header.frame_id = msg->header.frame_id;
-    // markerArray.header.stamp = msg->header.stamp;
-    // markerArray.markers.resize(markerIds.size());
-    // for (int i = 0; i < markerIds.size(); i++) {
-        // markerArray.markers[i].id = markerIds[i];
-        // markerArray.markers[i].pose.x = tvect[0];
-        // markerArray.markers[i].pose.y = tvect[1];
-        // markerArray.markers[i].pose.z = tvect[2];
-        // markerArray.markers[i].header.stamp = msg->header.stamp;
-        // markerArray.markers[i].header.frame_id = msg->header.frame_id;
-    // }
-    // marker_pub.publish(markerArray);
-
-    /*
-    for (int i = 0; i < markerIds.size(); i++) {
-        //if (markerIds[i] == 242) {
-        if (markerIds[i] == 9) {
-            tf::Transform transform = aruco2tf(rvecs[i], tvecs[i]);
-            tf::StampedTransform stampedTransform(transform, msg->header.stamp, msg->header.frame_id, frame_id);
-            br.sendTransform(stampedTransform);
-
-            // geometry_msgs::TransformStamped transformStamped;
-            // transformStamped.header.stamp = msg->header.stamp;
-            // transformStamped.header.frame_id = cinfo->header.frame_id;
-            // transformStamped.child_frame_id = frame_id;
-            // transformStamped.transform = aruco2tf(rvecs[i], tvecs[i]);
-            // transformStamped.transform.translation = transformStamped.transform.translation.normalize();
-            // br.sendTransform(transformStamped);
-            break;
-        }
-    }
-    */
-
-    // std::cout << "markers: ";
-    // for (auto const& c : markerIds) std::cout << c << ' ';
-
-//    return;
-
     if (markerIds.size() > 0) {
-//        for (auto const& c : markerCorners) std::cout << c << ' ';
-//        for (auto const& c : markerIds) std::cout << c << ' ';
-
-//        cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds);
-
-        // std::vector< cv::Vec3d > rvecs, tvecs;
-        // cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.3362, cameraMatrix, distCoeffs, rvecs, tvecs);
-        // draw axis for each marker
-        // for(int i=0; i<markerIds.size(); i++)
-            // cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
 
         cv::Mat rvec, tvec, objPoints;
         int valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs,
                                        rvec, tvec, false, objPoints);
 
         if (valid) {
-
+            // Send map transform
             tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_);
             br.sendTransform(transform);
 
@@ -296,7 +195,7 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs:
             ps.pose.orientation.w = 1;
             pose_pub.publish(ps);
 
-            // Publish reference point
+            // Send reference point
             cv::Point3f ref = getObjPointsCenter(objPoints);
             tf::Vector3 ref_vector3 = tf::Vector3(ref.x, ref.y, ref.z);
             tf::Quaternion q(0, 0, 0);
@@ -308,37 +207,15 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs:
             ref_transform.setRotation(q);
             br.sendTransform(ref_transform);
 
-//            geometry_msgs::TransformStamped transformMsg;
-            // transform.header.stamp = msg->header.stamp;
-            // transform.header.frame_id = cinfo->header.frame_id;
-            // transform.child_frame_id = frame_id;
-            // transform.transform = aruco2tf(rvec, tvec);
-//            tf::transformStampedTFToMsg(transform, transformMsg);
-//            br.sendTransform(transformMsg);
-//            std::cout << rvec << ";" << tvec << std::endl;
-//            geometry_msgs::TransformStamped transformStamped;
-//            transformStamped.header.stamp = msg->header.stamp;
-//            transformStamped.header.frame_id = cinfo->header.frame_id;
-//            transformStamped.child_frame_id = frame_id;
-//            transformStamped.transform.translation.x = tvec[0];
-//            transformStamped.transform.translation.y = tvec[1];
-//            transformStamped.transform.translation.z = tvec[1];
-//            transformStamped.transform.rotation.w = 1;
-//            br.sendTransform(transformStamped);
-
             if(img_pub.getNumSubscribers() > 0)
             {
-                //show input with augmented information
-                // for(int i=0; i<markerIds.size(); i++) {
-                    // cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
-                // }
-
+                // Publish debug image
                 cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds);
                 cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3);
                 cv_bridge::CvImage out_msg;
                 out_msg.header.frame_id = msg->header.frame_id;
                 out_msg.header.stamp = msg->header.stamp;
-                out_msg.encoding = sensor_msgs::image_encodings::BGR8; // sensor_msgs::image_encodings::RGB8;
+                out_msg.encoding = sensor_msgs::image_encodings::BGR8;
                 out_msg.image = image;
                 img_pub.publish(out_msg.toImageMsg());
             }
@@ -362,14 +239,6 @@ tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
     cv::Mat rot;
     cv::Rodrigues(rvec, rot);
 
-//    rot = rot.t();          // inverse rotation
-    //tvec = -rot * tvec;   // translation of inverse
-
-    // camPose is a 4x4 matrix with the pose of the camera in the object frame
-    // cv::Mat camPose = cv::Mat::eye(4, 4, R.type());
-    // R.copyTo(camPose.rowRange(0, 3).colRange(0, 3)); // copies R into camPose
-    // tvec.copyTo(camPose.rowRange(0, 3).colRange(3, 4)); // copies tvec into camPose
-
     tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
                          rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
                          rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
@@ -377,82 +246,6 @@ tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
     return tf::Transform(tf_rot, tf_orig);
 }
 
-/*
-tf::Transform ArucoPose::aruco2tf(cv::Vec3d rvec, cv::Vec3d tvec) {
-    cv::Mat rot(3, 3, CV_64FC1);
-    // cv::Mat Rvec64;
-    // rvec.convertTo(rvec, CV_64FC1);
-    cv::Rodrigues(rvec, rot);
-    cv::Mat tran64;
-    // tvec.convertTo(tran64, CV_64FC1);
-
-    cv::Mat rotate_to_ros(3, 3, CV_64FC1);
-    rotate_to_ros.at<float>(0,0) = 1.0;
-    rotate_to_ros.at<float>(0,1) = 0.0;
-    rotate_to_ros.at<float>(0,2) = 0.0;
-    rotate_to_ros.at<float>(1,0) = 0.0;
-    rotate_to_ros.at<float>(1,1) = -1.0;
-    rotate_to_ros.at<float>(1,2) = 0.0;
-    rotate_to_ros.at<float>(2,0) = 0.0;
-    rotate_to_ros.at<float>(2,1) = 0.0;
-    rotate_to_ros.at<float>(2,2) = -1.0;
-
-    rot = rot*rotate_to_ros.t();
-
-    tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
-                         rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
-                         rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
-
-    tf::Vector3 tf_orig(tvec[0], tvec[1], tvec[2]);
-
-
-    return tf::Transform(tf_rot, tf_orig);
-}
-*/
-
-// tf::Transform ArucoPose::aruco2tf(cv::Vec3d rvec, cv::Vec3d tvec) {
-//     /* Code it based on https://github.com/Sahloul/ar_sys/blob/master/src/utils.cpp#L44 */
-//     /* TODO: rewrite */
-
-//     cv::Mat rot(3, 3, CV_64FC1);
-//     cv::Rodrigues(rvec, rot);
-
-//     cv::Mat rotate_to_sys(3, 3, CV_64FC1);
-//     /**
-//     /* Fixed the rotation to meet the ROS system
-//     /* Doing a basic rotation around X with theta=PI
-//     /* By Sahloul
-//     /* See http://en.wikipedia.org/wiki/Rotation_matrix for details
-//     */
-
-//     //    1    0    0
-//     //    0    -1    0
-//     //    0    0    -1
-
-//     rotate_to_sys.at<float>(0,0) = 1.0;
-//     rotate_to_sys.at<float>(0,1) = 0.0;
-//     rotate_to_sys.at<float>(0,2) = 0.0;
-//     rotate_to_sys.at<float>(1,0) = 0.0;
-//     rotate_to_sys.at<float>(1,1) = -1.0;
-//     rotate_to_sys.at<float>(1,2) = 0.0;
-//     rotate_to_sys.at<float>(2,0) = 0.0;
-//     rotate_to_sys.at<float>(2,1) = 0.0;
-//     rotate_to_sys.at<float>(2,2) = -1.0;
-
-//     rot = rot * rotate_to_sys.t();
-
-
-//     tf::Matrix3x3 tf_rot(rot.at<float>(0,0), rot.at<float>(0,1), rot.at<float>(0,2),
-//         rot.at<float>(1,0), rot.at<float>(1,1), rot.at<float>(1,2),
-//         rot.at<float>(2,0), rot.at<float>(2,1), rot.at<float>(2,2));
-
-//     tf::Vector3 tf_orig(tvec[0], tvec[1], tvec[2]);
-
-//     tf::Transform tft(tf_rot, tf_orig);
-//     return tft;
-// }
-
 PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet)
 
 }
-