From 4168d274c7b775fa426267de5ecc9a95dfd569a7 Mon Sep 17 00:00:00 2001 From: pizzoli Date: Sun, 25 Oct 2015 23:24:14 +0100 Subject: [PATCH] added dataset publisher to test depthmap node --- CMakeLists.txt | 10 ++++ include/rmd/depthmap_node.h | 1 - src/depthmap_node.cpp | 9 ++- src/main_ros.cpp | 1 + test/publish_dataset.cpp | 106 ++++++++++++++++++++++++++++++++++++ 5 files changed, 125 insertions(+), 2 deletions(-) create mode 100644 test/publish_dataset.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 543a3f7..4b15221 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,6 +122,16 @@ target_link_libraries( open_remode open_remode_cuda ) +# Build a node to publish the dataset +add_executable( + dataset_publisher + test/dataset.cpp + test/publish_dataset.cpp +) +target_link_libraries( + dataset_publisher + ${LINK_LIBS} +) endif(BUILD_ROS_NODE) ## Test on dataset diff --git a/include/rmd/depthmap_node.h b/include/rmd/depthmap_node.h index 41723aa..bab162d 100644 --- a/include/rmd/depthmap_node.h +++ b/include/rmd/depthmap_node.h @@ -18,7 +18,6 @@ #ifndef RMD_DEPTHMAP_NODE_H #define RMD_DEPTHMAP_NODE_H -#include #include #include diff --git a/src/depthmap_node.cpp b/src/depthmap_node.cpp index d2d9bc5..5bfe711 100644 --- a/src/depthmap_node.cpp +++ b/src/depthmap_node.cpp @@ -15,6 +15,7 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . +#include #include #include #include @@ -23,7 +24,7 @@ #include rmd::DepthmapNode::DepthmapNode() - : depthmap_(752, 480, 481.2f, 319.5f, -480.0f, 239.5f) + : depthmap_(640, 480, 481.2f, 319.5f, -480.0f, 239.5f) { state_ = rmd::State::TAKE_REFERENCE_FRAME; } @@ -51,6 +52,12 @@ void rmd::DepthmapNode::denseInputCallback( dense_input->pose.position.y, dense_input->pose.position.z); + std::cout << "DEPTHMAP NODE: received image " + << img_8uC1.cols << "x" << img_8uC1.rows + << std::endl; + std::cout << "T_world_curr:" << std::endl; + std::cout << T_world_curr << std::endl; + switch (state_) { case rmd::State::TAKE_REFERENCE_FRAME: if(depthmap_.setReferenceImage( diff --git a/src/main_ros.cpp b/src/main_ros.cpp index 3f5b645..d8076ec 100644 --- a/src/main_ros.cpp +++ b/src/main_ros.cpp @@ -15,6 +15,7 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . +#include #include #include diff --git a/test/publish_dataset.cpp b/test/publish_dataset.cpp new file mode 100644 index 0000000..8bde466 --- /dev/null +++ b/test/publish_dataset.cpp @@ -0,0 +1,106 @@ + +// This file is part of REMODE - REgularized MOnocular Depth Estimation. +// +// Copyright (C) 2014 Matia Pizzoli +// Robotics and Perception Group, University of Zurich, Switzerland +// http://rpg.ifi.uzh.ch +// +// REMODE is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// REMODE is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include "dataset.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "dataset_publisher"); + + rmd::test::Dataset dataset("first_200_frames_traj_over_table_input_sequence.txt"); + if(!dataset.loadPathFromEnv()) + { + std::cerr << "ERROR: could not retrieve dataset path from the environment variable '" + << rmd::test::Dataset::getDataPathEnvVar() <<"'" << std::endl; + } + if (!dataset.readDataSequence(20, 100)) + { + std::cerr << "ERROR: could not read dataset" << std::endl; + return EXIT_FAILURE; + } + + ros::NodeHandle nh; + ros::Publisher publisher = nh.advertise( + "/svo/dense_input", + 1); + + ros::Rate loop_rate(3); + for(const auto data : dataset) + { + if(!ros::ok()) + { + break; + } + cv::Mat img_8uC1; + if(!dataset.readImage(img_8uC1, data)) + { + std::cerr << "ERROR: could not read image " << data.getImageFileName() << std::endl; + continue; + } + + cv::Mat depth_32FC1; + if(!dataset.readDepthmap(depth_32FC1, data, img_8uC1.cols, img_8uC1.rows)) + { + std::cerr << "ERROR: could not read depthmap " << data.getDepthmapFileName() << std::endl; + continue; + } + double min_depth, max_depth; + cv::minMaxLoc(depth_32FC1, &min_depth, &max_depth); + + rmd::SE3 T_world_curr; + dataset.readCameraPose(T_world_curr, data); + + std::cout << "DATASET PUBLISHER: image " << data.getImageFileName() << std::endl; + std::cout << "T_world_curr:" << std::endl; + std::cout << T_world_curr << std::endl; + + svo_msgs::DenseInput msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "/dense_input_frame_id"; + + cv_bridge::CvImage cv_image; + cv_image.header.stamp = ros::Time::now(); + cv_image.header.frame_id = "/greyscale_image_frame_id"; + cv_image.encoding = sensor_msgs::image_encodings::MONO8; + cv_image.image = img_8uC1; + msg.image = *cv_image.toImageMsg(); + + msg.pose.orientation.w = data.getQuaternion().w(); + msg.pose.orientation.x = data.getQuaternion().x(); + msg.pose.orientation.y = data.getQuaternion().y(); + msg.pose.orientation.z = data.getQuaternion().z(); + + msg.pose.position.x = data.getTranslation().x(); + msg.pose.position.y = data.getTranslation().y(); + msg.pose.position.z = data.getTranslation().z(); + + msg.min_depth = min_depth; + msg.max_depth = max_depth; + + publisher.publish(msg); + + loop_rate.sleep(); + } + + return EXIT_SUCCESS; +}