diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp
index 173406a6..c95fcefe 100644
--- a/aruco_pose/src/aruco_map.cpp
+++ b/aruco_pose/src/aruco_map.cpp
@@ -77,7 +77,7 @@ private:
 	visualization_msgs::MarkerArray vis_array_;
 	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
 	int image_width_, image_height_, image_margin_;
-	bool auto_flip_;
+	bool auto_flip_, image_axis_;
 
 public:
 	virtual void onInit()
@@ -104,6 +104,7 @@ public:
 		nh_priv_.param("image_width", image_width_, 2000);
 		nh_priv_.param("image_height", image_height_, 2000);
 		nh_priv_.param("image_margin", image_margin_, 200);
+		nh_priv_.param("image_axis", image_axis_, true);
 		nh_priv_.param<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
 		nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");
 
@@ -481,14 +482,15 @@ publish_debug:
 		cv_bridge::CvImage msg;
 
 		if (!board_->ids.empty()) {
-			_drawPlanarBoard(board_, size, image, image_margin_, 1);
+			_drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
+			msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
 		} else {
 			// empty map
 			image.create(size, CV_8UC1);
 			image.setTo(cv::Scalar::all(255));
+			msg.encoding = sensor_msgs::image_encodings::MONO8;
 		}
 
-		msg.encoding = sensor_msgs::image_encodings::MONO8;
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}
diff --git a/aruco_pose/src/draw.cpp b/aruco_pose/src/draw.cpp
index 3077ca48..65eb9fb3 100644
--- a/aruco_pose/src/draw.cpp
+++ b/aruco_pose/src/draw.cpp
@@ -23,12 +23,12 @@ static void _projectPoints( InputArray objectPoints,
 
 
 void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
-                      int borderBits) {
+                      int borderBits, bool drawAxis) {
 
 	CV_Assert(outSize.area() > 0);
 	CV_Assert(marginSize >= 0);
 
-	_img.create(outSize, CV_8UC1);
+	_img.create(outSize, drawAxis ? CV_8UC3 : CV_8UC1);
 	Mat out = _img.getMat();
 	out.setTo(Scalar::all(255));
 	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
@@ -90,6 +90,9 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 		side = std::max(side, 10);
 
 		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
+		if (drawAxis) {
+			cvtColor(marker, marker, COLOR_GRAY2RGB);
+		}
 
 		// interpolate tiny marker to marker position in markerZone
 		inCorners[0] = Point2f(-0.5f, -0.5f);
@@ -101,6 +104,18 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
 						BORDER_TRANSPARENT);
 	}
+
+	// draw axis
+	if (drawAxis) {
+		Size wholeSize; Point ofs;
+		out.locateROI(wholeSize, ofs);
+		auto out_copy = _img.getMat();
+
+		cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
+		line(out_copy, center, center + Point(300, 0), Scalar(255, 0, 0), 10); // x axis
+		line(out_copy, center, center + Point(0, -300), Scalar(0, 255, 0), 10); // y axis
+		line(out_copy, center, center + Point(-200, 200), Scalar(0, 0, 255), 10); // z axis
+	}
 }
 
 /* Draw a (potentially partially visible) line. */
diff --git a/aruco_pose/src/draw.h b/aruco_pose/src/draw.h
index 56a9b309..db6c6b42 100644
--- a/aruco_pose/src/draw.h
+++ b/aruco_pose/src/draw.h
@@ -3,6 +3,7 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
 
-void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
+void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img,
+                      int marginSize, int borderBits, bool drawAxis);
 void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
               cv::InputArray rvec, cv::InputArray tvec, float length);