diff --git a/Examples/ROS/ORB_SLAM2/DenseInput.h b/Examples/ROS/ORB_SLAM2/DenseInput.h
new file mode 100644
index 0000000000..c0b770a8d5
--- /dev/null
+++ b/Examples/ROS/ORB_SLAM2/DenseInput.h
@@ -0,0 +1,306 @@
+// Generated by gencpp from file svo_msgs/DenseInput.msg
+// DO NOT EDIT!
+
+
+#ifndef SVO_MSGS_MESSAGE_DENSEINPUT_H
+#define SVO_MSGS_MESSAGE_DENSEINPUT_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Pose.h>
+#include <sensor_msgs/Image.h>
+
+namespace svo_msgs
+{
+template <class ContainerAllocator>
+struct DenseInput_
+{
+  typedef DenseInput_<ContainerAllocator> Type;
+
+  DenseInput_()
+    : header()
+    , frame_id(0)
+    , pose()
+    , image()
+    , min_depth(0.0)
+    , max_depth(0.0)  {
+    }
+  DenseInput_(const ContainerAllocator& _alloc)
+    : header(_alloc)
+    , frame_id(0)
+    , pose(_alloc)
+    , image(_alloc)
+    , min_depth(0.0)
+    , max_depth(0.0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;
+  _header_type header;
+
+   typedef uint32_t _frame_id_type;
+  _frame_id_type frame_id;
+
+   typedef  ::geometry_msgs::Pose_<ContainerAllocator>  _pose_type;
+  _pose_type pose;
+
+   typedef  ::sensor_msgs::Image_<ContainerAllocator>  _image_type;
+  _image_type image;
+
+   typedef float _min_depth_type;
+  _min_depth_type min_depth;
+
+   typedef float _max_depth_type;
+  _max_depth_type max_depth;
+
+
+
+
+  typedef boost::shared_ptr< ::svo_msgs::DenseInput_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::svo_msgs::DenseInput_<ContainerAllocator> const> ConstPtr;
+
+}; // struct DenseInput_
+
+typedef ::svo_msgs::DenseInput_<std::allocator<void> > DenseInput;
+
+typedef boost::shared_ptr< ::svo_msgs::DenseInput > DenseInputPtr;
+typedef boost::shared_ptr< ::svo_msgs::DenseInput const> DenseInputConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::svo_msgs::DenseInput_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::svo_msgs::DenseInput_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+} // namespace svo_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
+// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'sensor_msgs': ['/opt/ros/kinetic/share/sensor_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'svo_msgs': ['/home/ayush/catkin_ws/src/svo_msgs/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::svo_msgs::DenseInput_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::svo_msgs::DenseInput_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::svo_msgs::DenseInput_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::svo_msgs::DenseInput_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::svo_msgs::DenseInput_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::svo_msgs::DenseInput_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::svo_msgs::DenseInput_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "cea677f47dcf08581cc9f5efece2f7e7";
+  }
+
+  static const char* value(const ::svo_msgs::DenseInput_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0xcea677f47dcf0858ULL;
+  static const uint64_t static_value2 = 0x1cc9f5efece2f7e7ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::svo_msgs::DenseInput_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "svo_msgs/DenseInput";
+  }
+
+  static const char* value(const ::svo_msgs::DenseInput_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::svo_msgs::DenseInput_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "Header header\n\
+uint32 frame_id\n\
+geometry_msgs/Pose pose\n\
+sensor_msgs/Image image\n\
+float32 min_depth\n\
+float32 max_depth\n\
+================================================================================\n\
+MSG: std_msgs/Header\n\
+# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Pose\n\
+# A representation of pose in free space, composed of position and orientation. \n\
+Point position\n\
+Quaternion orientation\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Point\n\
+# This contains the position of a point in free space\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Quaternion\n\
+# This represents an orientation in free space in quaternion form.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+float64 w\n\
+\n\
+================================================================================\n\
+MSG: sensor_msgs/Image\n\
+# This message contains an uncompressed image\n\
+# (0, 0) is at top-left corner of image\n\
+#\n\
+\n\
+Header header        # Header timestamp should be acquisition time of image\n\
+                     # Header frame_id should be optical frame of camera\n\
+                     # origin of frame should be optical center of cameara\n\
+                     # +x should point to the right in the image\n\
+                     # +y should point down in the image\n\
+                     # +z should point into to plane of the image\n\
+                     # If the frame_id here and the frame_id of the CameraInfo\n\
+                     # message associated with the image conflict\n\
+                     # the behavior is undefined\n\
+\n\
+uint32 height         # image height, that is, number of rows\n\
+uint32 width          # image width, that is, number of columns\n\
+\n\
+# The legal values for encoding are in file src/image_encodings.cpp\n\
+# If you want to standardize a new string format, join\n\
+# ros-users@lists.sourceforge.net and send an email proposing a new encoding.\n\
+\n\
+string encoding       # Encoding of pixels -- channel meaning, ordering, size\n\
+                      # taken from the list of strings in include/sensor_msgs/image_encodings.h\n\
+\n\
+uint8 is_bigendian    # is this data bigendian?\n\
+uint32 step           # Full row length in bytes\n\
+uint8[] data          # actual matrix data, size is (step * rows)\n\
+";
+  }
+
+  static const char* value(const ::svo_msgs::DenseInput_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::svo_msgs::DenseInput_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.header);
+      stream.next(m.frame_id);
+      stream.next(m.pose);
+      stream.next(m.image);
+      stream.next(m.min_depth);
+      stream.next(m.max_depth);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER;
+  }; // struct DenseInput_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::svo_msgs::DenseInput_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::svo_msgs::DenseInput_<ContainerAllocator>& v)
+  {
+    s << indent << "header: ";
+    s << std::endl;
+    Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + "  ", v.header);
+    s << indent << "frame_id: ";
+    Printer<uint32_t>::stream(s, indent + "  ", v.frame_id);
+    s << indent << "pose: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Pose_<ContainerAllocator> >::stream(s, indent + "  ", v.pose);
+    s << indent << "image: ";
+    s << std::endl;
+    Printer< ::sensor_msgs::Image_<ContainerAllocator> >::stream(s, indent + "  ", v.image);
+    s << indent << "min_depth: ";
+    Printer<float>::stream(s, indent + "  ", v.min_depth);
+    s << indent << "max_depth: ";
+    Printer<float>::stream(s, indent + "  ", v.max_depth);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SVO_MSGS_MESSAGE_DENSEINPUT_H
diff --git a/Examples/ROS/ORB_SLAM2/src/ros_mono.cc b/Examples/ROS/ORB_SLAM2/src/ros_mono.cc
index 2913525bfc..ee739ec42d 100644
--- a/Examples/ROS/ORB_SLAM2/src/ros_mono.cc
+++ b/Examples/ROS/ORB_SLAM2/src/ros_mono.cc
@@ -31,8 +31,14 @@
 
 #include"../../../include/System.h"
 
+#include<ros/ros.h>
+#include "tf/transform_datatypes.h"
+#include <tf/transform_broadcaster.h>
+#include <DenseInput.h>
 using namespace std;
-
+cv::Mat pose;
+ros::Publisher pose_pub; 
+ros::Publisher pub_dense;
 class ImageGrabber
 {
 public:
@@ -62,7 +68,8 @@ int main(int argc, char **argv)
 
     ros::NodeHandle nodeHandler;
     ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);
-
+    pose_pub = nodeHandler.advertise<geometry_msgs::PoseStamped>("/camera_pose",1);
+    pub_dense = nodeHandler.advertise<svo_msgs::DenseInput>("/ORB/DenseInput",1);
     ros::spin();
 
     // Stop all threads
@@ -90,7 +97,90 @@ void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
         return;
     }
 
-    mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
+    pose = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
+    if (pose.empty())
+        return;
+    //std::cout<<pose<<std::endl;
+    /* global left handed coordinate system */
+        static cv::Mat pose_prev = cv::Mat::eye(4,4, CV_32F);
+        static cv::Mat world_lh = cv::Mat::eye(4,4, CV_32F);
+        // matrix to flip signs of sinus in rotation matrix, not sure why we need to do that
+        static const cv::Mat flipSign = (cv::Mat_<float>(4,4) <<   1,-1,-1, 1,
+                                                               -1, 1,-1, 1,
+                                                               -1,-1, 1, 1,
+                                                                1, 1, 1, 1);
+    cv::Mat translation =  (pose * pose_prev.inv()).mul(flipSign);
+        world_lh = world_lh * translation;
+        pose_prev = pose.clone();
+    /* transform into global right handed coordinate system, publish in ROS*/
+    tf::Matrix3x3 cameraRotation_rh(  - world_lh.at<float>(0,0),   world_lh.at<float>(0,1),   world_lh.at<float>(0,2),
+                                  - world_lh.at<float>(1,0),   world_lh.at<float>(1,1),   world_lh.at<float>(1,2),
+                                    world_lh.at<float>(2,0), - world_lh.at<float>(2,1), - world_lh.at<float>(2,2));
+
+    tf::Vector3 cameraTranslation_rh( world_lh.at<float>(0,3),world_lh.at<float>(1,3), - world_lh.at<float>(2,3) );
+
+        //rotate 270deg about x and 270deg about x to get ENU: x forward, y left, z up
+        const tf::Matrix3x3 rotation270degXZ(   0, 1, 0,
+                                            0, 0, 1,
+                                    1, 0, 0);
+        static tf::TransformBroadcaster br;
+    tf::Matrix3x3 globalRotation_rh = cameraRotation_rh * rotation270degXZ;
+        tf::Vector3 globalTranslation_rh = cameraTranslation_rh * rotation270degXZ;
+        tf::Transform transform = tf::Transform(globalRotation_rh, globalTranslation_rh);
+        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_link", "camera_pose"));
+        geometry_msgs::PoseStamped _pose;
+        _pose.pose.position.x = transform.getOrigin().x();
+        _pose.pose.position.y = transform.getOrigin().y();
+        _pose.pose.position.z = transform.getOrigin().z();
+        _pose.pose.orientation.x = transform.getRotation().x();
+        _pose.pose.orientation.y = transform.getRotation().y();
+        _pose.pose.orientation.z = transform.getRotation().z();
+        _pose.pose.orientation.w = transform.getRotation().w();
+      
+        _pose.header.stamp = ros::Time::now();
+        _pose.header.frame_id = "camera_link";
+        pose_pub.publish(_pose);
+    //     ROS_INFO("REACHED 1");
+    double min_z = std::numeric_limits<double>::max();  
+    double max_z = std::numeric_limits<double>::min();  
+    mpSLAM->mpTracker->mCurrentFrame.getSceneDepth(mpSLAM->mpTracker->mCurrentFrame,max_z,min_z);
+    //ROS_INFO("REACHED 2");
+    //ROS_INFO("Max: %f Min: %f",max_z,min_z);
+
+    svo_msgs::DenseInput msg_dense;
+    msg_dense.header.stamp = ros::Time::now();
+    msg_dense.header.frame_id = "world";
+
+    cv_bridge::CvImage img_msg;  
+    img_msg.header.stamp=msg_dense.header.stamp;  
+    img_msg.header.frame_id="camera";  
+    img_msg.image=cv_ptr->image;  
+  
+    img_msg.encoding = sensor_msgs::image_encodings::MONO8;  
+    msg_dense.image = *img_msg.toImageMsg();
+
+    msg_dense.min_depth = (float)min_z;
+    msg_dense.max_depth = (float)max_z;
+
+
+    cv::Mat  TWC=mpSLAM->mpTracker->mCurrentFrame.mTcw.inv();  
+    cv::Mat RWC=TWC.rowRange(0,3).colRange(0,3);  
+    cv::Mat tWC=TWC.rowRange(0,3).col(3);
+    Eigen::Matrix<double,3,3> M;
+
+    M << RWC.at<float>(0,0), RWC.at<float>(0,1), RWC.at<float>(0,2),
+         RWC.at<float>(1,0), RWC.at<float>(1,1), RWC.at<float>(1,2),
+         RWC.at<float>(2,0), RWC.at<float>(2,1), RWC.at<float>(2,2);
+    Eigen::Quaterniond q(M);
+    msg_dense.pose.position.x = tWC.at<float>(0,0);  
+    msg_dense.pose.position.y = tWC.at<float>(1,0);  
+    msg_dense.pose.position.z = tWC.at<float>(2,0);  
+    msg_dense.pose.orientation.x = q.x();
+    msg_dense.pose.orientation.y = q.y();
+    msg_dense.pose.orientation.z = q.z();
+    msg_dense.pose.orientation.w = q.w();
+    pub_dense.publish(msg_dense); 
+
 }
 
 
diff --git a/include/Frame.h b/include/Frame.h
index a6a8032f57..dbfadc1b6d 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -187,7 +187,7 @@ class Frame
 
     static bool mbInitialComputations;
 
-
+    bool getSceneDepth(Frame& frame, double& depth_mean, double& depth_min);
 private:
 
     // Undistort keypoints given OpenCV distortion parameters.
@@ -206,6 +206,8 @@ class Frame
     cv::Mat mtcw;
     cv::Mat mRwc;
     cv::Mat mOw; //==mtwc
+
+    
 };
 
 }// namespace ORB_SLAM
diff --git a/include/System.h b/include/System.h
index 59f4491e90..3486cb111b 100644
--- a/include/System.h
+++ b/include/System.h
@@ -112,6 +112,11 @@ class System
     // SaveMap(const string &filename);
     // LoadMap(const string &filename);
 
+        // Tracker. It receives a frame and computes the associated camera pose.
+    // It also decides when to insert a new keyframe, create some new MapPoints and
+    // performs relocalization if tracking fails.
+    Tracking* mpTracker;
+
 private:
 
     // Input sensor
@@ -126,10 +131,7 @@ class System
     // Map structure that stores the pointers to all KeyFrames and MapPoints.
     Map* mpMap;
 
-    // Tracker. It receives a frame and computes the associated camera pose.
-    // It also decides when to insert a new keyframe, create some new MapPoints and
-    // performs relocalization if tracking fails.
-    Tracking* mpTracker;
+
 
     // Local Mapper. It manages the local map and performs local bundle adjustment.
     LocalMapping* mpLocalMapper;
diff --git a/src/Frame.cc b/src/Frame.cc
index 0e37d49335..b15e13323e 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -22,7 +22,6 @@
 #include "Converter.h"
 #include "ORBmatcher.h"
 #include <thread>
-
 namespace ORB_SLAM2
 {
 
@@ -679,4 +678,40 @@ cv::Mat Frame::UnprojectStereo(const int &i)
         return cv::Mat();
 }
 
+bool Frame::getSceneDepth(Frame& frame, double& depth_max, double& depth_min)
+{
+  vector<double> depth_vec;
+  depth_vec.reserve(frame.mvpMapPoints.size());
+  depth_min = std::numeric_limits<double>::max();
+  for(auto it=frame.mvpMapPoints.begin(), ite=frame.mvpMapPoints.end(); it!=ite; ++it)
+  {
+    if((*it)!= NULL)
+    {
+      
+      cv::Mat mpPosHg = cv::Mat::ones(4,1,CV_32F);
+      cv::Mat mpPos = (*it)->GetWorldPos();
+      mpPosHg.at<double>(0) = mpPos.at<double>(0);
+      mpPosHg.at<double>(1) = mpPos.at<double>(1);
+      mpPosHg.at<double>(2) = mpPos.at<double>(2);
+      cv::Mat v3Temp = frame.mTcw*mpPosHg;
+      const double z = v3Temp.at<float>(2)/v3Temp.at<float>(3);
+      depth_vec.push_back(z);
+      // std::cout << "Mat: " << v3Temp << std::endl;
+      // std::cout << "at: " << v3Temp.at<float>(2) << std::endl;
+      // std::cout << "Z: " << z << std::endl;
+      depth_min = fmin(z, depth_min);
+      depth_max = fmax(z, depth_max);
+    }
+  }
+  if(depth_vec.empty())
+  {
+    //SLAM_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!");
+    return false;
+  }
+  // std::cout << "Min: " << depth_min << std::endl;
+  // // depth_max = *std::max_element(depth_vec.begin(),depth_vec.end());
+  // std::cout << "Max: " << depth_max << std::endl;
+  return true;
+}
+
 } //namespace ORB_SLAM