diff --git a/src/sc.cpp b/src/sc.cpp
index c7a0e04..fda086a 100644
--- a/src/sc.cpp
+++ b/src/sc.cpp
@@ -21,6 +21,16 @@
 #define LOG_PERIOD_S 		10
 #define NODE_NAME 			"sc"
 
+bool isMono(const ST::ColorFrame &visFrame)
+{
+	return visFrame.width() * visFrame.height() == visFrame.rgbSize();
+}
+
+std::string getEncoding(const ST::ColorFrame &visFrame)
+{
+	return isMono(visFrame) ? sensor_msgs::image_encodings::MONO8 : sensor_msgs::image_encodings::RGB8;
+}
+
 struct SessionDelegate : ST::CaptureSessionDelegate {
 private:
     std::mutex lock;
@@ -198,25 +208,16 @@ struct SessionDelegate : ST::CaptureSessionDelegate {
 			return;
 		}
 
-		cv::Mat img(visFrame.height(), visFrame.width(), CV_8UC1);
-
-		const uint8_t* buf = visFrame.rgbData();
+		const std::string encoding = getEncoding(visFrame);
 
-		for(int y = 0; y < visFrame.height(); y++)
-		{
-			for(int x = 0; x < visFrame.width(); x++)
-			{
-				std::size_t pixelOffset = (y * visFrame.width()) + x;
-				img.at<uchar>(y, x) = buf[pixelOffset];
-			}
-		}
+		cv::Mat img(visFrame.height(), visFrame.width(), cv_bridge::getCvType(encoding), static_cast<void*>(const_cast<uint8_t*>(visFrame.rgbData())));
 
 		std_msgs::Header header;
 		header.frame_id = frame_id_;
 		// TODO: convert timstamp to ROS time
 		// header.stamp =  infraredFrame.timestamp();
 
-		sensor_msgs::ImagePtr msg = cv_bridge::CvImage(header, "mono8", img).toImageMsg();
+		sensor_msgs::ImagePtr msg = cv_bridge::CvImage(header, encoding, img).toImageMsg();
 		vis_pub_.publish(msg);
 
 		sensor_msgs::CameraInfo cam_info;
@@ -238,7 +239,7 @@ struct SessionDelegate : ST::CaptureSessionDelegate {
 			for(int x = 0; x < depthFrame.width(); x++)
 			{
 				std::size_t pixelOffset = (y * depthFrame.width()) + x;
-				img.at<float>(y, x) = buf[pixelOffset];
+				img.at<float>(y, x) = buf[pixelOffset] * 0.001f;
 			}
 		}
 
@@ -351,6 +352,8 @@ class SCNode
 
 	bool imu_enable_;
 
+	bool depth_correction_enable_;
+
 	SessionDelegate *delegate_ = nullptr;
 	ST::CaptureSessionSettings sessionConfig_;
 	ST::CaptureSession captureSession_;
@@ -374,6 +377,9 @@ class SCNode
 		ros::param::param<bool>("~imu_enable", imu_enable_, false);
 		ROS_INFO_STREAM(NODE_NAME << ": imu_enable = " << imu_enable_);
 
+		ros::param::param<bool>("~depth_correction_enable", depth_correction_enable_, false);
+		ROS_INFO_STREAM(NODE_NAME << ": depth_correction_enable = " << depth_correction_enable_);
+
 		std::string frame_id;
 		ros::param::param<std::string>("~frame_id", frame_id, DEFAULT_FRAME_ID);
 		ROS_INFO_STREAM(NODE_NAME << ": frame_id = " << frame_id);
@@ -393,6 +399,7 @@ class SCNode
 
 		sessionConfig_.source = ST::CaptureSessionSourceId::StructureCore;
 		sessionConfig_.structureCore = scConfig;
+		sessionConfig_.applyExpensiveCorrection = depth_correction_enable_;
 
 		delegate_ = new SessionDelegate(nh_, frame_id, sessionConfig_);
 		captureSession_.setDelegate(delegate_);