00001
00002 #include <ros/ros.h>
00003 #include <cv_bridge/cv_bridge.h>
00004 #include <image_transport/image_transport.h>
00005 #include <opencv2/imgproc/imgproc.hpp>
00006 #include <opencv2/opencv.hpp>
00007 #include <sensor_msgs/image_encodings.h>
00008
00009 #include "omnicamera/LookupStitcher.h"
00010
00011 namespace {
00012 int inWidth, inHeight;
00013 int outWidth, outHeight;
00014 bool alpha;
00015 int radius;
00016 string lutPath;
00017
00018 omnicamera::LookupStitcher stitcher;
00019 image_transport::Publisher outPub;
00020 ros::Publisher camPub;
00021 }
00022
00023 void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
00024
00025 cv_bridge::CvImageConstPtr cvPtr;
00026 try {
00027 cvPtr = cv_bridge::toCvShare(msg);
00028 if (cvPtr->encoding.compare(sensor_msgs::image_encodings::BGR8)) {
00029 ROS_ERROR("Encoding not supported: %s.", cvPtr->encoding.data());
00030 return;
00031 }
00032 } catch (cv_bridge::Exception& e) {
00033 ROS_ERROR("cv_bridge exception: %s", e.what());
00034 return;
00035 }
00036
00037 if (cvPtr->image.rows != inHeight || cvPtr->image.cols != inWidth) {
00038 ROS_ERROR("Unexpected image dimensions: %dx%d.", cvPtr->image.cols, cvPtr->image.rows);
00039 }
00040
00041 Mat image;
00042 stitcher.stitchImage(cvPtr->image, image);
00043
00044 cv_bridge::CvImage out;
00045 out.encoding = sensor_msgs::image_encodings::BGR8;
00046 out.header = msg->header;
00047 out.image = image;
00048 outPub.publish(out.toImageMsg());
00049
00050 sensor_msgs::CameraInfo camMsg;
00051 camMsg.header = out.header;
00052 camMsg.height = out.image.rows;
00053 camMsg.width = out.image.cols;
00054 camPub.publish(camMsg);
00055 }
00056
00057 int main(int argc, char ** argv) {
00058
00059 ros::init(argc, argv, "img2pano");
00060 ros::NodeHandle nh;
00061
00062
00063 ros::NodeHandle pnh("~");
00064 pnh.param("inWidth", inWidth, 1616);
00065 pnh.param("inHeight", inHeight, 1232);
00066 ROS_INFO("Input image size: %dx%d.", inWidth, inHeight);
00067
00068 pnh.param("outWidth", outWidth, 960);
00069 pnh.param("outHeight", outHeight, 480);
00070 ROS_INFO("Output image size: %dx%d.", outWidth, outHeight);
00071
00072 pnh.param("alpha", alpha, true);
00073 ROS_INFO("Alpha blending: %s.", alpha ? "enabled" : "disabled");
00074
00075 pnh.param("radius", radius, 20);
00076 ROS_INFO("Sphere radius: %d.", radius);
00077
00078 pnh.param("lutPath", lutPath, string("LUT_img_%dx%d_pano_%dx%dr%d_%s%d.pgm"));
00079 ROS_INFO("LUT path template: %s.", lutPath.data());
00080
00081 stitcher.clearLookupTables();
00082 for (int iCam = 1; iCam <= 6; iCam++) {
00083 int len = 1024;
00084 char xPath[len], yPath[len], aPath[len];
00085 snprintf(xPath, len, lutPath.data(), inWidth, inHeight, outWidth, outHeight, radius, "x", iCam);
00086 snprintf(yPath, len, lutPath.data(), inWidth, inHeight, outWidth, outHeight, radius, "y", iCam);
00087 snprintf(aPath, len, lutPath.data(), inWidth, inHeight, outWidth, outHeight, radius, "a", iCam);
00088 ROS_INFO("Camera %d look-up tables: %s, %s, %s.", iCam, xPath, yPath, aPath);
00089 stitcher.addLookupTables(string(xPath), string(yPath), string(aPath));
00090 }
00091
00092 image_transport::ImageTransport it(nh);
00093 image_transport::Subscriber sub = it.subscribe("in", 1, imageCallback);
00094 outPub = it.advertise("out", 1);
00095 camPub = nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
00096
00097 ros::spin();
00098
00099 return 0;
00100 }
00101
