diff --git a/lsd_slam_core/src/DataStructures/Frame.cpp b/lsd_slam_core/src/DataStructures/Frame.cpp
index 04ec0498..9e7c751d 100644
--- a/lsd_slam_core/src/DataStructures/Frame.cpp
+++ b/lsd_slam_core/src/DataStructures/Frame.cpp
@@ -31,6 +31,35 @@ int privateFrameAllocCount = 0;
 
 
 
+Frame::Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const unsigned char* image,  const unsigned char* rgbImage)
+{
+	initialize(id, width, height, K, timestamp);
+
+	data.image[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]);
+	data.imageRGB[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]*3);
+
+	float* maxPt = data.image[0] + data.width[0]*data.height[0];
+	float* maxPtRGB = data.imageRGB[0] + data.width[0]*data.height[0]*3;
+
+	for(float* pt = data.image[0]; pt < maxPt; pt++)
+	{
+		*pt = *image;
+		image++;
+	}
+	for(float* pt = data.imageRGB[0]; pt < maxPtRGB; pt++)
+	{
+		*pt = *rgbImage;
+		rgbImage++;
+	}
+
+	data.imageValid[0] = true;
+
+	privateFrameAllocCount++;
+
+	if(enablePrintDebugInfo && printMemoryDebugInfo)
+		printf("ALLOCATED frame %d, now there are %d\n", this->id(), privateFrameAllocCount);
+
+}
 
 Frame::Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const unsigned char* image)
 {
diff --git a/lsd_slam_core/src/DataStructures/Frame.h b/lsd_slam_core/src/DataStructures/Frame.h
index 1ff2f530..ed39c0b3 100644
--- a/lsd_slam_core/src/DataStructures/Frame.h
+++ b/lsd_slam_core/src/DataStructures/Frame.h
@@ -44,6 +44,7 @@ class Frame
 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 	friend class FrameMemory;
 
+	Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const unsigned char* image, const unsigned char* rgbImage);
 
 	Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const unsigned char* image);
 
@@ -100,6 +101,7 @@ class Frame
 	inline double timestamp() const;
 	
 	inline float* image(int level = 0);
+	inline float* imageRGB(int level = 0);
 	inline const Eigen::Vector4f* gradients(int level = 0);
 	inline const float* maxGradients(int level = 0);
 	inline bool hasIDepthBeenSet() const;
@@ -242,6 +244,7 @@ class Frame
 
 		
 		float* image[PYRAMID_LEVELS];
+		float* imageRGB[PYRAMID_LEVELS];
 		bool imageValid[PYRAMID_LEVELS];
 		
 		Eigen::Vector4f* gradients[PYRAMID_LEVELS];
@@ -360,6 +363,10 @@ inline float* Frame::image(int level)
 		require(IMAGE, level);
 	return data.image[level];
 }
+inline float* Frame::imageRGB(int level)
+{
+	return data.imageRGB[level];
+}
 inline const Eigen::Vector4f* Frame::gradients(int level)
 {
 	if (! data.gradientsValid[level])
diff --git a/lsd_slam_core/src/IOWrapper/ROS/ROSImageStreamThread.cpp b/lsd_slam_core/src/IOWrapper/ROS/ROSImageStreamThread.cpp
index 8317b807..d047add9 100644
--- a/lsd_slam_core/src/IOWrapper/ROS/ROSImageStreamThread.cpp
+++ b/lsd_slam_core/src/IOWrapper/ROS/ROSImageStreamThread.cpp
@@ -114,7 +114,7 @@ void ROSImageStreamThread::vidCb(const sensor_msgs::ImageConstPtr img)
 {
 	if(!haveCalib) return;
 
-	cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
+	cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8);
 
 	if(img->header.seq < (unsigned int)lastSEQ)
 	{
diff --git a/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp b/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp
index 78faa88c..0d617b2b 100644
--- a/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp
+++ b/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp
@@ -96,16 +96,16 @@ void ROSOutput3DWrapper::publishKeyframe(Frame* f)
 
 	const float* idepth = f->idepth(publishLvl);
 	const float* idepthVar = f->idepthVar(publishLvl);
-	const float* color = f->image(publishLvl);
+	const float* color = f->imageRGB(publishLvl);
 
-	for(int idx=0;idx < w*h; idx++)
+	for(int idx=0,idxRGB=0;idx < w*h; idx++,idxRGB+=3)
 	{
 		pc[idx].idepth = idepth[idx];
 		pc[idx].idepth_var = idepthVar[idx];
-		pc[idx].color[0] = color[idx];
-		pc[idx].color[1] = color[idx];
-		pc[idx].color[2] = color[idx];
-		pc[idx].color[3] = color[idx];
+		pc[idx].color[2] = color[idxRGB];
+		pc[idx].color[1] = color[idxRGB+1];
+		pc[idx].color[0] = color[idxRGB+2];
+		pc[idx].color[3] = 100;
 	}
 
 	keyframe_publisher.publish(fMsg);
diff --git a/lsd_slam_core/src/LiveSLAMWrapper.cpp b/lsd_slam_core/src/LiveSLAMWrapper.cpp
index 45423d62..04762bc4 100644
--- a/lsd_slam_core/src/LiveSLAMWrapper.cpp
+++ b/lsd_slam_core/src/LiveSLAMWrapper.cpp
@@ -129,12 +129,12 @@ void LiveSLAMWrapper::newImageCallback(const cv::Mat& img, Timestamp imgTime)
 	// need to initialize
 	if(!isInitialized)
 	{
-		monoOdometry->randomInit(grayImg.data, imgTime.toSec(), 1);
+		monoOdometry->randomInit(grayImg.data, img.data, imgTime.toSec(), 1);
 		isInitialized = true;
 	}
 	else if(isInitialized && monoOdometry != nullptr)
 	{
-		monoOdometry->trackFrame(grayImg.data,imageSeqNumber,false,imgTime.toSec());
+		monoOdometry->trackFrame(grayImg.data, img.data, imageSeqNumber,false,imgTime.toSec());
 	}
 }
 
diff --git a/lsd_slam_core/src/SlamSystem.cpp b/lsd_slam_core/src/SlamSystem.cpp
index d1e1dbf4..c6a38cb9 100644
--- a/lsd_slam_core/src/SlamSystem.cpp
+++ b/lsd_slam_core/src/SlamSystem.cpp
@@ -854,7 +854,7 @@ void SlamSystem::gtDepthInit(uchar* image, float* depth, double timeStamp, int i
 }
 
 
-void SlamSystem::randomInit(uchar* image, double timeStamp, int id)
+void SlamSystem::randomInit(uchar* image, uchar* rgbImage, double timeStamp, int id)
 {
 	printf("Doing Random initialization!\n");
 
@@ -864,7 +864,7 @@ void SlamSystem::randomInit(uchar* image, double timeStamp, int id)
 
 	currentKeyFrameMutex.lock();
 
-	currentKeyFrame.reset(new Frame(id, width, height, K, timeStamp, image));
+	currentKeyFrame.reset(new Frame(id, width, height, K, timeStamp, image, rgbImage));
 	map->initializeRandomly(currentKeyFrame.get());
 	keyFrameGraph->addFrame(currentKeyFrame.get());
 
@@ -887,10 +887,10 @@ void SlamSystem::randomInit(uchar* image, double timeStamp, int id)
 
 }
 
-void SlamSystem::trackFrame(uchar* image, unsigned int frameID, bool blockUntilMapped, double timestamp)
+void SlamSystem::trackFrame(uchar* image, uchar* rgbImage, unsigned int frameID, bool blockUntilMapped, double timestamp)
 {
 	// Create new frame
-	std::shared_ptr<Frame> trackingNewFrame(new Frame(frameID, width, height, K, timestamp, image));
+	std::shared_ptr<Frame> trackingNewFrame(new Frame(frameID, width, height, K, timestamp, image, rgbImage));
 
 	if(!trackingIsGood)
 	{
diff --git a/lsd_slam_core/src/SlamSystem.h b/lsd_slam_core/src/SlamSystem.h
index 68492403..c5709746 100644
--- a/lsd_slam_core/src/SlamSystem.h
+++ b/lsd_slam_core/src/SlamSystem.h
@@ -73,7 +73,7 @@ friend class IntegrationTest;
 	SlamSystem& operator=(const SlamSystem&) = delete;
 	~SlamSystem();
 
-	void randomInit(uchar* image, double timeStamp, int id);
+	void randomInit(uchar* image, uchar* rgbImage, double timeStamp, int id);
 	void gtDepthInit(uchar* image, float* depth, double timeStamp, int id);
 
 	
@@ -82,7 +82,7 @@ friend class IntegrationTest;
 	// first frame will return Identity = camToWord.
 	// returns camToWord transformation of the tracked frame.
 	// frameID needs to be monotonically increasing.
-	void trackFrame(uchar* image, unsigned int frameID, bool blockUntilMapped, double timestamp);
+	void trackFrame(uchar* image, uchar* rgbImage, unsigned int frameID, bool blockUntilMapped, double timestamp);
 
 	// finalizes the system, i.e. blocks and does all remaining loop-closures etc.
 	void finalize();
diff --git a/lsd_slam_core/src/main_on_images.cpp b/lsd_slam_core/src/main_on_images.cpp
index 1aa06861..eb4570ee 100644
--- a/lsd_slam_core/src/main_on_images.cpp
+++ b/lsd_slam_core/src/main_on_images.cpp
@@ -214,6 +214,8 @@ int main( int argc, char** argv )
 
 
 	cv::Mat image = cv::Mat(h,w,CV_8U);
+	cv::Mat imageRGB = cv::Mat(h,w,CV_8UC3);
+
 	int runningIDX=0;
 	float fakeTimeStamp = 0;
 
@@ -222,6 +224,7 @@ int main( int argc, char** argv )
 	for(unsigned int i=0;i<files.size();i++)
 	{
 		cv::Mat imageDist = cv::imread(files[i], CV_LOAD_IMAGE_GRAYSCALE);
+		cv::Mat imageDistRGB = cv::imread(files[i], CV_LOAD_IMAGE_COLOR);
 
 		if(imageDist.rows != h_inp || imageDist.cols != w_inp)
 		{
@@ -234,14 +237,17 @@ int main( int argc, char** argv )
 			continue;
 		}
 		assert(imageDist.type() == CV_8U);
+		assert(imageDist.type() == CV_8UC3);
 
 		undistorter->undistort(imageDist, image);
+		undistorter->undistort(imageDistRGB, imageRGB);
 		assert(image.type() == CV_8U);
+		assert(imageRGB.type() == CV_8UC3);
 
 		if(runningIDX == 0)
-			system->randomInit(image.data, fakeTimeStamp, runningIDX);
+			system->randomInit(image.data, imageRGB.data, fakeTimeStamp, runningIDX);
 		else
-			system->trackFrame(image.data, runningIDX ,hz == 0,fakeTimeStamp);
+			system->trackFrame(image.data, imageRGB.data, runningIDX ,hz == 0,fakeTimeStamp);
 		runningIDX++;
 		fakeTimeStamp+=0.03;