Added setting of reference and update

CMakeLists.txt

@@ -53,7 +53,7 @@ find_package(Boost REQUIRED COMPONENTS system filesystem)
 
 if(BUILD_ROS_NODE)
   find_package(catkin REQUIRED COMPONENTS
-    roscpp roslib cmake_modules svo_msgs)
+    roscpp roslib cmake_modules svo_msgs cv_bridge)
   catkin_package(
       DEPENDS OpenCV Eigen Boost
       CATKIN_DEPENDS roscpp roslib svo_msgs

include/rmd/depthmap_node.h

@@ -26,11 +26,25 @@
 namespace rmd
 {
 
+namespace ProcessingStates
+{
+enum State
+{
+  UPDATE,
+  TAKE_REFERENCE_FRAME,
+};
+}
+typedef ProcessingStates::State State;
+
 class DepthmapNode
 {
 public:
+  DepthmapNode();
   void denseInputCallback(
       const svo_msgs::DenseInputConstPtr &dense_input);
+private:
+  rmd::Depthmap depthmap_;
+  State state_;
 };
 
 } // rmd namespace

package.xml

@@ -17,10 +17,12 @@
   <build_depend>roscpp</build_depend>
   <build_depend>roslib</build_depend>
   <build_depend>svo_msgs</build_depend>
+  <build_depend>cv_bridge</build_depend>
 
   <!-- Dependencies needed after this package is compiled. -->
   <run_depend>roscpp</run_depend>
   <run_depend>roslib</run_depend>
   <run_depend>svo_msgs</run_depend>
+  <run_depend>cv_bridge</run_depend>
 
 </package>

src/depthmap_node.cpp

@@ -15,10 +15,63 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
 #include <rmd/depthmap_node.h>
+#include <rmd/se3.cuh>
+
+rmd::DepthmapNode::DepthmapNode()
+  : depthmap_(752, 480, 481.2f, 319.5f, -480.0f, 239.5f)
+{
+  state_ = rmd::State::TAKE_REFERENCE_FRAME;
+}
 
 void rmd::DepthmapNode::denseInputCallback(
     const svo_msgs::DenseInputConstPtr &dense_input)
 {
-  std::cout << "svo msg received" << std::endl;
+  cv::Mat img_8uC1;
+  try
+  {
+    img_8uC1 = cv_bridge::toCvCopy(
+          dense_input->image,
+          sensor_msgs::image_encodings::MONO8)->image;
+  }
+  catch (cv_bridge::Exception& e)
+  {
+    ROS_ERROR("cv_bridge exception: %s", e.what());
+  }
+  rmd::SE3<float> T_world_curr(
+        dense_input->pose.orientation.w,
+        dense_input->pose.orientation.x,
+        dense_input->pose.orientation.y,
+        dense_input->pose.orientation.z,
+        dense_input->pose.position.x,
+        dense_input->pose.position.y,
+        dense_input->pose.position.z);
+
+  switch (state_) {
+  case rmd::State::TAKE_REFERENCE_FRAME:
+    if(depthmap_.setReferenceImage(
+         img_8uC1,
+         T_world_curr.inv(),
+         dense_input->min_depth,
+         dense_input->max_depth))
+    {
+      state_ = State::UPDATE;
+    }
+    else
+    {
+      std::cerr << "ERROR: could not set reference image" << std::endl;
+    }
+    break;
+  case rmd::State::UPDATE:
+      depthmap_.update(img_8uC1, T_world_curr.inv());
+    break;
+  default:
+    break;
+  }
+
 }
+