all_camera_viewer.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


//##################
//#### includes ####

// standard includes
//--

// ROS includes
#include <ros/ros.h>
#include <cv_bridge/CvBridge.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#ifdef __ROS_1_1__
        #include <message_filters/sync_policies/approximate_time.h>
        #include <message_filters/synchronizer.h>
#endif

// ROS message includes
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/fill_image.h>

#include <cob_camera_sensors/AcquireCalibrationImages.h>

// external includes
#include <cob_vision_utils/VisionUtils.h>

#include <opencv/highgui.h>
#include <boost/thread/mutex.hpp>

using namespace message_filters;

#ifdef __ROS_1_1__
typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> ThreeImageSyncPolicy;
typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> TwoImageSyncPolicy;
#endif

class AllCameraViewer
{
private:
        ros::NodeHandle node_handle_;

        image_transport::ImageTransport image_transport_;

        // Subscriptions
        image_transport::SubscriberFilter left_color_camera_image_sub_; 
        image_transport::SubscriberFilter right_color_camera_image_sub_;        
        image_transport::SubscriberFilter tof_camera_grey_image_sub_;   
        message_filters::Subscriber<sensor_msgs::CameraInfo> left_camera_info_sub_;     
        message_filters::Subscriber<sensor_msgs::CameraInfo> right_camera_info_sub_;    
        image_transport::Subscriber sub;

#ifdef __ROS_1_1__
        message_filters::Synchronizer<ThreeImageSyncPolicy> shared_sub_sync_;
        message_filters::Synchronizer<TwoImageSyncPolicy> stereo_sub_sync_;
        message_filters::Synchronizer<ThreeImageSyncPolicy> all_sub_sync_;
#else
        message_filters::TimeSynchronizer<sensor_msgs::Image,  
                                                sensor_msgs::Image, sensor_msgs::Image> shared_sub_sync_;
        message_filters::TimeSynchronizer<sensor_msgs::Image,   
                                                sensor_msgs::Image> stereo_sub_sync_;
        message_filters::TimeSynchronizer<sensor_msgs::Image, 
                                                sensor_msgs::Image, sensor_msgs::Image> all_sub_sync_;
#endif

        int sub_counter_; 
        int tof_image_counter_; 

        sensor_msgs::Image color_image_msg_;
        sensor_msgs::Image xyz_image_msg_;
        sensor_msgs::Image confidence_mask_msg_;

        IplImage* right_color_image_8U3_;       
        IplImage* left_color_image_8U3_;        
        IplImage* xyz_image_32F3_;      
        IplImage* grey_image_32F1_;     

        cv::Mat right_color_mat_8U3_;   
        cv::Mat left_color_mat_8U3_;    
        cv::Mat xyz_mat_32F3_;  
        cv::Mat grey_mat_32F1_; 
        cv::Mat grey_mat_8U1_;  
        std::vector<cv::Mat> vec_grey_mat_32F1_; 

        int image_counter_; 

        sensor_msgs::CvBridge cv_bridge_0_; 
        sensor_msgs::CvBridge cv_bridge_1_; 
        sensor_msgs::CvBridge cv_bridge_2_; 

        ros::ServiceServer save_camera_images_service_;

        boost::mutex m_ServiceMutex;

        std::string absolute_output_directory_path_; 

        bool use_tof_camera_;
        bool use_left_color_camera_;
        bool use_right_color_camera_;
public:
        AllCameraViewer(const ros::NodeHandle& node_handle)
        : node_handle_(node_handle),
          image_transport_(node_handle),
#ifdef __ROS_1_1__
          shared_sub_sync_(ThreeImageSyncPolicy(3)),
          stereo_sub_sync_(TwoImageSyncPolicy(3)),
          all_sub_sync_(ThreeImageSyncPolicy(3)),
#else
          shared_sub_sync_(3),
          stereo_sub_sync_(3),
          all_sub_sync_(3),
#endif
          sub_counter_(0),
          right_color_image_8U3_(0),
          left_color_image_8U3_(0),
          xyz_image_32F3_(0),
          grey_image_32F1_(0)
        {
                use_tof_camera_ = true;
                use_left_color_camera_ = true;
                use_right_color_camera_ = true;
                image_counter_ = 0;
                tof_image_counter_ = 0;
        }

        ~AllCameraViewer()
        {
        }

        bool init()
        {
                if (loadParameters() == false) return false;

                image_transport::SubscriberStatusCallback imgConnect    = boost::bind(&AllCameraViewer::connectCallback, this);
                image_transport::SubscriberStatusCallback imgDisconnect = boost::bind(&AllCameraViewer::disconnectCallback, this);

                save_camera_images_service_ = node_handle_.advertiseService("save_camera_images", &AllCameraViewer::saveCameraImagesServiceCallback, this);

                // Synchronize inputs of incoming image data.
                // Topic subscriptions happen on demand in the connection callback.

                // TODO: Dynamically determine, which cameras are available

                if(use_right_color_camera_ && use_tof_camera_ && !use_left_color_camera_)
                {
                        ROS_INFO("[all_camera_viewer] Setting up subscribers for right color and tof camera");
                        shared_sub_sync_.connectInput(right_color_camera_image_sub_, tof_camera_grey_image_sub_);
                        shared_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::sharedModeSrvCallback, this, _1, _2));
                }
                else if(use_right_color_camera_ && !use_tof_camera_ && use_left_color_camera_)
                {
                        ROS_INFO("[all_camera_viewer] Setting up subscribers left and right color camera");
                        stereo_sub_sync_.connectInput(left_color_camera_image_sub_, right_color_camera_image_sub_);
                        stereo_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::stereoModeSrvCallback, this, _1, _2));
                }
                else if(use_right_color_camera_ && use_tof_camera_ && use_left_color_camera_)
                {
                        ROS_INFO("[all_camera_viewer] Setting up subscribers for left color, right color and tof camera");
                        all_sub_sync_.connectInput(left_color_camera_image_sub_, right_color_camera_image_sub_, tof_camera_grey_image_sub_);
                        all_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::allModeSrvCallback, this, _1, _2, _3));
                }
                else
                {
                        ROS_ERROR("[all_camera_viewer] Specified camera configuration not available");
                        return false;
                }


                connectCallback();

                ROS_INFO("[all_camera_viewer] Initializing [OK]");
                return true;
        }

        void connectCallback()
        {
                if (sub_counter_ == 0)
                {
                        sub_counter_++;
                        ROS_DEBUG("[all_camera_viewer] Subscribing to camera topics");

                        if (use_right_color_camera_)
                        {
                                right_color_camera_image_sub_.subscribe(image_transport_, "right/image_color", 1);
                                right_camera_info_sub_.subscribe(node_handle_, "right/camera_info", 1);
                        }
                        if (use_left_color_camera_)
                        {
                                left_color_camera_image_sub_.subscribe(image_transport_, "left/image_color", 1);
                                left_camera_info_sub_.subscribe(node_handle_, "left/camera_info", 1);
                        }
                        if (use_tof_camera_)
                        {
                                tof_camera_grey_image_sub_.subscribe(image_transport_, "image_grey", 1);
                        }
                }
        }

        void disconnectCallback()
        {
                sub_counter_--;
                if (sub_counter_ == 0)
                {
                        ROS_DEBUG("[all_camera_viewer] Unsubscribing from camera topics");

                        if (use_right_color_camera_)
                        {
                                right_color_camera_image_sub_.unsubscribe();
                                right_camera_info_sub_.unsubscribe();
                        }
                        if (use_left_color_camera_)
                        {
                                left_color_camera_image_sub_.unsubscribe();
                                left_camera_info_sub_.unsubscribe();
                        }
                        if (use_tof_camera_)
                        {
                                tof_camera_grey_image_sub_.unsubscribe();
                        }
                }
        }

        void allModeSrvCallback(const sensor_msgs::ImageConstPtr& left_camera_data,
                        const sensor_msgs::ImageConstPtr& right_camera_data,
                        const sensor_msgs::ImageConstPtr& tof_camera_grey_data)
        {
                ROS_INFO("[all_camera_viewer] allModeSrvCallback");
                boost::mutex::scoped_lock lock(m_ServiceMutex);
                // Convert ROS image messages to openCV IplImages
                try
                {
                        right_color_image_8U3_ = cv_bridge_0_.imgMsgToCv(right_camera_data, "bgr8");
                        left_color_image_8U3_ = cv_bridge_1_.imgMsgToCv(left_camera_data, "bgr8");
                        grey_image_32F1_ = cv_bridge_2_.imgMsgToCv(tof_camera_grey_data, "passthrough");

                        cv::Mat tmp = right_color_image_8U3_;
                        right_color_mat_8U3_ = tmp.clone();

                        tmp = left_color_image_8U3_;
                        left_color_mat_8U3_ = tmp.clone();

                        tmp = grey_image_32F1_;
                        grey_mat_32F1_ = tmp.clone();
                }
                catch (sensor_msgs::CvBridgeException& e)
                {
                        ROS_ERROR("[all_camera_viewer] Could not convert stereo images with cv_bridge.");
                }

                // Modifies <code>grey_mat_8U1_</code> and <code>vec_grey_mat_32F1_</code>
                // using <code>grey_mat_32F1_</code>
                updateTofGreyscaleData();

                cv::imshow("TOF grey data", grey_mat_8U1_);

                cv::Mat right_color_8U3;
                cv::resize(right_color_mat_8U3_, right_color_8U3, cv::Size(), 0.5, 0.5);
                cv::imshow("Right color data", right_color_8U3);

                cv::Mat left_color_8U3;
                cv::resize(left_color_mat_8U3_, left_color_8U3, cv::Size(), 0.5, 0.5);
                cv::imshow("Left color data", left_color_8U3);
                cv::waitKey(50);

                ROS_INFO("[all_camera_viewer] allModeSrvCallback [OK]");
        }

        void sharedModeSrvCallback(const sensor_msgs::ImageConstPtr& right_camera_data,
                        const sensor_msgs::ImageConstPtr& tof_camera_grey_data)
        {
                boost::mutex::scoped_lock lock(m_ServiceMutex);
                ROS_INFO("[all_camera_viewer] sharedModeSrvCallback");
                // Convert ROS image messages to openCV IplImages
                try
                {
                        right_color_image_8U3_ = cv_bridge_0_.imgMsgToCv(right_camera_data, "passthrough");
                        grey_image_32F1_ = cv_bridge_2_.imgMsgToCv(tof_camera_grey_data, "passthrough");

                        cv::Mat tmp = right_color_image_8U3_;
                        right_color_mat_8U3_ = tmp.clone();

                        tmp = grey_image_32F1_;
                        grey_mat_32F1_ = tmp.clone();
                }
                catch (sensor_msgs::CvBridgeException& e)
                {
                        ROS_ERROR("[all_camera_viewer] Could not convert images with cv_bridge.");
                }

                // Modifies <code>grey_mat_8U1_</code> and <code>vec_grey_mat_32F1_</code>
                // using <code>grey_mat_32F1_</code>
                updateTofGreyscaleData();

                cv::imshow("TOF grey data", grey_mat_8U1_);

                cv::Mat right_color_8U3;
                cv::resize(right_color_mat_8U3_, right_color_8U3, cv::Size(), 0.5, 0.5);
                cv::imshow("Right color data", right_color_8U3);
                cv::waitKey(1000);
        }

        void updateTofGreyscaleData()
        {
                cv::Mat filtered_grey_mat_32F1 = cv::Mat::zeros(grey_mat_32F1_.rows, grey_mat_32F1_.cols, CV_8UC1/*32FC1*/);

                // Accumulate greyscale images to remove noise
                if (vec_grey_mat_32F1_.size() <=  (unsigned int) tof_image_counter_)
                        vec_grey_mat_32F1_.push_back(grey_mat_32F1_);
                else
                        vec_grey_mat_32F1_[tof_image_counter_] = grey_mat_32F1_;
                // Update counter
                tof_image_counter_ = (++tof_image_counter_)%5;

                // Compute average out of accumulated tof greyscale data
                for (unsigned int i=0; i<vec_grey_mat_32F1_.size(); i++)
                        filtered_grey_mat_32F1 += vec_grey_mat_32F1_[i];
                filtered_grey_mat_32F1 * (1/vec_grey_mat_32F1_.size());

                // Update 8bit image
                cv::Mat temp0_grey_mat_8U1;
                cv::Mat temp1_grey_mat_8U1;

                // Convert CV_32FC1 image to CV_8UC1
                /*double m_scaleMin;
                double m_scaleMax;
                cv::minMaxLoc(filtered_grey_mat_32F1, &m_scaleMin, &m_scaleMax);
                cv::Mat minmat(filtered_grey_mat_32F1.size(), 32FC1, cv::Scalar(m_scaleMin));
                filtered_grey_mat_32F1 = cv::Mat(filtered_grey_mat_32F1 - minmat)/ m_scaleMax;
                filtered_grey_mat_32F1.convertTo(temp0_grey_mat_8U1, CV_8U, 256);*/

                // Perform histogram equalization on 8bit image
                //cv::equalizeHist(temp0_grey_mat_8U1, temp1_grey_mat_8U1);

                // Upsample image by a factor of 2 using bilinear interpolation
                // and save result to member variable
                grey_mat_8U1_ = filtered_grey_mat_32F1;//temp0_grey_mat_8U1;
                //resize(temp0_grey_mat_8U1, grey_mat_8U1_, cv::Size(), 2, 2, cv::INTER_LINEAR);
        }

        void stereoModeSrvCallback(const sensor_msgs::ImageConstPtr& left_camera_data,
                        const sensor_msgs::ImageConstPtr& right_camera_data)
        {
                ROS_INFO("[all_camera_viewer] stereoModeSrvCallback");
                boost::mutex::scoped_lock lock(m_ServiceMutex);
                // Convert ROS image messages to openCV IplImages
                try
                {
                        right_color_image_8U3_ = cv_bridge_0_.imgMsgToCv(right_camera_data, "passthrough");
                        left_color_image_8U3_ = cv_bridge_1_.imgMsgToCv(left_camera_data, "passthrough");

                        cv::Mat tmp = right_color_image_8U3_;
                        right_color_mat_8U3_ = tmp.clone();

                        tmp = left_color_image_8U3_;
                        left_color_mat_8U3_ = tmp.clone();
                }
                catch (sensor_msgs::CvBridgeException& e)
                {
                        ROS_ERROR("[all_camera_viewer] Could not convert stereo images with cv_bridge.");
                }

                cv::Mat right_color_8U3;
                cv::resize(right_color_mat_8U3_, right_color_8U3, cv::Size(), 0.5, 0.5);
                cv::imshow("Right color data", right_color_8U3);

                cv::Mat left_color_8U3;
                cv::resize(left_color_mat_8U3_, left_color_8U3, cv::Size(), 0.5, 0.5);
                cv::imshow("Left color data", left_color_8U3);
                cv::waitKey(1000);

                ROS_INFO("[all_camera_viewer] stereoModeSrvCallback [OK]");
        }

        bool saveCameraImagesServiceCallback(cob_camera_sensors::AcquireCalibrationImages::Request &req,
                        cob_camera_sensors::AcquireCalibrationImages::Response &res)
        {
                ROS_INFO("[all_camera_viewer] Service Callback");
                boost::mutex::scoped_lock lock(m_ServiceMutex);

                if (req.reset_image_counter) image_counter_ = 0;

                char counterBuffer [50];
                sprintf(counterBuffer, "%04d", image_counter_);

                if (use_right_color_camera_ && right_color_mat_8U3_.empty())
                {
                        ROS_INFO("[all_camera_viewer] Right color image not available");
                        return false;
                }
                else
                {
                        std::stringstream ss;
                        ss << "right_color_image_";
                        ss << counterBuffer;
                        ss << ".bmp";
                        cv::imwrite(absolute_output_directory_path_ + ss.str(), right_color_mat_8U3_);
                        ROS_INFO("[all_camera_viewer] Saved right color image %d to %s", image_counter_, ss.str().c_str());
                }

                if (use_left_color_camera_ && left_color_mat_8U3_.empty())
                {
                        ROS_INFO("[all_camera_viewer] Left color image not available");
                        return false;
                }
                else
                {
                        std::stringstream ss;
                        ss << "left_color_image_";
                        ss << counterBuffer;
                        ss << ".bmp";
                        cv::imwrite(absolute_output_directory_path_ + ss.str(), left_color_mat_8U3_);
                        ROS_INFO("[all_camera_viewer] Saved left color image %d to %s", image_counter_, ss.str().c_str());
                }


                if (use_tof_camera_ && grey_mat_8U1_.empty())
                {
                        ROS_INFO("[all_camera_viewer] Tof grayscale image not available");
                        return false;
                }
                else
                {
                        std::stringstream ss;
                        ss << "tof_grey_image_";
                        ss << counterBuffer;
                        ss << ".bmp";
                        cv::imwrite(absolute_output_directory_path_ + ss.str(), grey_mat_8U1_);
                        ROS_INFO("[all_camera_viewer] Saved tof gray image %d to %s", image_counter_, ss.str().c_str());
                }

                image_counter_++;

                return true;
        }

        unsigned long loadParameters()
        {
                std::string tmp_string;

                if (node_handle_.getParam("all_camera_viewer/use_tof_camera", use_tof_camera_) == false)
                {
                        ROS_ERROR("[all_camera_viewer] 'use_tof_camera' not specified");
                        return false;
                }
                ROS_INFO("use tof camera: %d", use_tof_camera_);

                if (node_handle_.getParam("all_camera_viewer/use_right_color_camera", use_right_color_camera_) == false)
                {
                        ROS_ERROR("[all_camera_viewer] 'use_right_color_camera' not specified");
                        return false;
                }
                ROS_INFO("use right color camera: %d", use_right_color_camera_);

                if (node_handle_.getParam("all_camera_viewer/use_left_color_camera", use_left_color_camera_) == false)
                {
                        ROS_ERROR("[all_camera_viewer] 'use_left_color_camera' not specified");
                        return false;
                }
                ROS_INFO("use left color camera: %d", use_left_color_camera_);

                if (node_handle_.getParam("all_camera_viewer/absolute_output_directory_path", absolute_output_directory_path_) == false)
                {
                        ROS_ERROR("[all_camera_viewer] 'absolute_output_directory_path' not specified");
                        return false;
                }
                ROS_INFO("use left color camera: %d", use_left_color_camera_);

                return true;
        }

};

//#######################
//#### main programm ####
int main(int argc, char** argv)
{
        ros::init(argc, argv, "all_camera_viewer");

        ros::NodeHandle nh;

        AllCameraViewer camera_node(nh);

        if (!camera_node.init()) return 0;

        ros::spin();

        return 0;
}