nerian_sp1_node.cpp
Go to the documentation of this file.
00001 /*******************************************************************************
00002  * Copyright (c) 2016 Nerian Vision Technologies
00003  *
00004  * Permission is hereby granted, free of charge, to any person obtaining a copy
00005  * of this software and associated documentation files (the "Software"), to deal
00006  * in the Software without restriction, including without limitation the rights
00007  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008  * copies of the Software, and to permit persons to whom the Software is
00009  * furnished to do so, subject to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included in
00012  * all copies or substantial portions of the Software.
00013  *******************************************************************************/
00014 
00015 #include <ros/ros.h>
00016 #include <visiontransfer/asynctransfer.h>
00017 #include <visiontransfer/reconstruct3d.h>
00018 #include <sensor_msgs/PointCloud2.h>
00019 #include <sensor_msgs/Image.h>
00020 #include <opencv2/opencv.hpp>
00021 #include <cv_bridge/cv_bridge.h>
00022 #include <iostream>
00023 #include <iomanip>
00024 #include <nerian_sp1/StereoCameraInfo.h>
00025 #include <boost/smart_ptr.hpp>
00026 #include <colorcoder.h>
00027 
00028 using namespace std;
00029 
00049 class Sp1Node {
00050 public:
00051     Sp1Node(): frameNum(0) {
00052     }
00053 
00054     ~Sp1Node() {
00055     }
00056 
00060     void init() {
00061         ros::NodeHandle privateNh("~");
00062 
00063         // Read all ROS parameters
00064         if (!privateNh.getParam("point_cloud_intensity_channel", intensityChannel)) {
00065             intensityChannel = true;
00066         }
00067 
00068         if (!privateNh.getParam("color_code_disparity_map", colorCodeDispMap)) {
00069             colorCodeDispMap = true;
00070         }
00071 
00072         if (!privateNh.getParam("color_code_legend", colorCodeLegend)) {
00073             colorCodeLegend = false;
00074         }
00075 
00076         if (!privateNh.getParam("frame", frame)) {
00077             frame = "world";
00078         }
00079 
00080         if (!privateNh.getParam("remote_port", remotePort)) {
00081             remotePort = "7681";
00082         }
00083 
00084         if (!privateNh.getParam("remote_host", remoteHost)) {
00085             remoteHost = "0.0.0.0";
00086         }
00087 
00088         if (!privateNh.getParam("local_port", localPort)) {
00089             localPort = "7681";
00090         }
00091 
00092         if (!privateNh.getParam("local_host", localHost)) {
00093             localHost = "0.0.0.0";
00094         }
00095 
00096         if (!privateNh.getParam("use_tcp", useTcp)) {
00097             useTcp = false;
00098         }
00099 
00100         if (!privateNh.getParam("ros_coordinate_system", rosCoordinateSystem)) {
00101             rosCoordinateSystem = true;
00102         }
00103 
00104         if (!privateNh.getParam("calibration_file", calibFile)) {
00105             calibFile = "";
00106         }
00107 
00108         if (!privateNh.getParam("delay_execution", execDelay)) {
00109             execDelay = 0;
00110         }
00111 
00112         if (!privateNh.getParam("max_depth", maxDepth)) {
00113             maxDepth = -1;
00114         }
00115 
00116         // Apply an initial delay if configured
00117         ros::Duration(execDelay).sleep();
00118 
00119         // Create publishers
00120         disparityPublisher.reset(new ros::Publisher(nh.advertise<sensor_msgs::Image>(
00121             "/nerian_sp1/disparity_map", 5)));
00122         leftImagePublisher.reset(new ros::Publisher(nh.advertise<sensor_msgs::Image>(
00123             "/nerian_sp1/left_image", 5)));
00124         rightImagePublisher.reset(new ros::Publisher(nh.advertise<sensor_msgs::Image>(
00125             "/nerian_sp1/right_image", 5)));
00126 
00127         loadCameraCalibration();
00128 
00129         cameraInfoPublisher.reset(new ros::Publisher(nh.advertise<nerian_sp1::StereoCameraInfo>(
00130             "/nerian_sp1/stereo_camera_info", 1)));
00131         cloudPublisher.reset(new ros::Publisher(nh.advertise<sensor_msgs::PointCloud2>(
00132             "/nerian_sp1/point_cloud", 5)));
00133     }
00134 
00138     int run() {
00139         try {
00140             ros::Time lastLogTime;
00141             int lastLogFrames = 0;
00142 
00143             AsyncTransfer asyncTransfer(useTcp ? ImageTransfer::TCP_CLIENT : ImageTransfer::UDP,
00144                 remoteHost.c_str(), remotePort.c_str(), localHost.c_str(), localPort.c_str());
00145 
00146             while(ros::ok()) {
00147                 // Receive image data
00148                 ImagePair imagePair;
00149                 if(!asyncTransfer.collectReceivedImagePair(imagePair, 0.5)) {
00150                     continue;
00151                 }
00152 
00153                 ros::Time stamp = ros::Time::now();
00154 
00155                 // Publish the selected messages
00156                 if(leftImagePublisher->getNumSubscribers() > 0) {
00157                     publishImageMsg(imagePair, stamp);
00158                 }
00159 
00160                 if(disparityPublisher->getNumSubscribers() > 0 || rightImagePublisher->getNumSubscribers() > 0) {
00161                     publishDispMapMsg(imagePair, stamp);
00162                 }
00163 
00164                 if(cloudPublisher->getNumSubscribers() > 0) {
00165                     if(recon3d == nullptr) {
00166                         // First initialize
00167                         initPointCloud();
00168                     }
00169 
00170                     publishPointCloudMsg(imagePair, stamp);
00171                 }
00172 
00173                 if(cameraInfoPublisher != NULL && cameraInfoPublisher->getNumSubscribers() > 0) {
00174                     publishCameraInfo(stamp, imagePair);
00175                 }
00176 
00177                 // Display some simple statistics
00178                 frameNum++;
00179                 if(stamp.sec != lastLogTime.sec) {
00180                     if(lastLogTime != ros::Time()) {
00181                         double dt = (stamp - lastLogTime).toSec();
00182                         double fps = (frameNum - lastLogFrames) / dt;
00183                         ROS_INFO("%.1f fps", fps);
00184                     }
00185                     lastLogFrames = frameNum;
00186                     lastLogTime = stamp;
00187                 }
00188             }
00189         } catch(const std::exception& ex) {
00190             ROS_FATAL("Exception occured: %s", ex.what());
00191         }
00192     }
00193 
00194 private:
00195     // ROS related objects
00196     ros::NodeHandle nh;
00197     boost::scoped_ptr<ros::Publisher> cloudPublisher;
00198     boost::scoped_ptr<ros::Publisher> disparityPublisher;
00199     boost::scoped_ptr<ros::Publisher> leftImagePublisher;
00200     boost::scoped_ptr<ros::Publisher> rightImagePublisher;
00201     boost::scoped_ptr<ros::Publisher> cameraInfoPublisher;
00202 
00203     // Parameters
00204     bool intensityChannel;
00205     bool useTcp;
00206     bool colorCodeDispMap;
00207     bool colorCodeLegend;
00208     bool rosCoordinateSystem;
00209     std::string remotePort;
00210     std::string localPort;
00211     std::string frame;
00212     std::string remoteHost;
00213     std::string localHost;
00214     std::string calibFile;
00215     double execDelay;
00216     double maxDepth;
00217 
00218     // Other members
00219     int frameNum;
00220     boost::scoped_ptr<Reconstruct3D> recon3d;
00221     boost::scoped_ptr<ColorCoder> colCoder;
00222     cv::Mat_<cv::Vec3b> colDispMap;
00223     sensor_msgs::PointCloud2Ptr pointCloudMsg;
00224     cv::FileStorage calibStorage;
00225     nerian_sp1::StereoCameraInfoPtr camInfoMsg;
00226     ros::Time lastCamInfoPublish;
00227 
00231     void loadCameraCalibration() {
00232         if(calibFile == "" ) {
00233             ROS_WARN("No camera calibration file configured. Cannot publish detailed camera information!");
00234         } else {
00235             bool success = false;
00236             try {
00237                 if (calibStorage.open(calibFile, cv::FileStorage::READ)) {
00238                     success = true;
00239                 }
00240             } catch(...) {
00241             }
00242 
00243             if(!success) {
00244                 ROS_WARN("Error reading calibration file: %s\n"
00245                     "Cannot publish detailed camera information!", calibFile.c_str());
00246             }
00247         }
00248     }
00249 
00253     void publishImageMsg(const ImagePair& imagePair, ros::Time stamp) {
00254         cv_bridge::CvImage cvImg;
00255         cvImg.header.frame_id = frame;
00256         cvImg.header.stamp = stamp;
00257         cvImg.header.seq = imagePair.getSequenceNumber(); // Actually ROS will overwrite this
00258 
00259         cvImg.image = cv::Mat_<unsigned char>(imagePair.getHeight(),
00260             imagePair.getWidth(), imagePair.getPixelData(0), imagePair.getRowStride(0));
00261         sensor_msgs::ImagePtr msg = cvImg.toImageMsg();
00262 
00263 
00264         msg->encoding = "mono8";
00265         leftImagePublisher->publish(msg);
00266     }
00267 
00272     void publishDispMapMsg(const ImagePair& imagePair, ros::Time stamp) {
00273         cv_bridge::CvImage cvImg;
00274         cvImg.header.frame_id = frame;
00275         cvImg.header.stamp = stamp;
00276         cvImg.header.seq = imagePair.getSequenceNumber(); // Actually ROS will overwrite this
00277 
00278         bool format12Bit = (imagePair.getPixelFormat(1) == ImagePair::FORMAT_12_BIT);
00279         cv::Mat monoImg(imagePair.getHeight(), imagePair.getWidth(),
00280             format12Bit ? CV_16UC1 : CV_8UC1,
00281             imagePair.getPixelData(1), imagePair.getRowStride(1));
00282         string encoding = "";
00283 
00284         if(!colorCodeDispMap || !format12Bit) {
00285             cvImg.image = monoImg;
00286             encoding = (format12Bit ? "mono16": "mono8");
00287         } else {
00288             if(colCoder == NULL) {
00289                 colCoder.reset(new ColorCoder(ColorCoder::COLOR_RED_BLUE_BGR, 0, 16*111, true, true));
00290                 if(colorCodeLegend) {
00291                     // Create the legend
00292                     colDispMap = colCoder->createLegendBorder(monoImg.cols, monoImg.rows, 1.0/16.0);
00293                 } else {
00294                     colDispMap = cv::Mat_<cv::Vec3b>(monoImg.rows, monoImg.cols);
00295                 }
00296             }
00297 
00298             cv::Mat_<cv::Vec3b> dispSection = colDispMap(cv::Rect(0, 0, monoImg.cols, monoImg.rows));
00299 
00300             colCoder->codeImage(cv::Mat_<unsigned short>(monoImg), dispSection);
00301             cvImg.image = colDispMap;
00302             encoding = "bgr8";
00303         }
00304 
00305         sensor_msgs::ImagePtr msg = cvImg.toImageMsg();
00306         msg->encoding = encoding;
00307 
00308         if(disparityPublisher->getNumSubscribers() > 0 && format12Bit) {
00309             disparityPublisher->publish(msg);
00310         }
00311         if(rightImagePublisher->getNumSubscribers() > 0 && !format12Bit) {
00312             rightImagePublisher->publish(msg);
00313         }
00314     }
00315 
00320     void qMatrixToRosCoords(const float* src, float* dst) {
00321         dst[0] = src[8];   dst[1] = src[9];
00322         dst[2] = src[10];  dst[3] = src[11];
00323 
00324         dst[4] = -src[0];  dst[5] = -src[1];
00325         dst[6] = -src[2];  dst[7] = -src[3];
00326 
00327         dst[8] = -src[4];  dst[9] = -src[5];
00328         dst[10] = -src[6]; dst[11] = -src[7];
00329 
00330         dst[12] = src[12]; dst[13] = src[13];
00331         dst[14] = src[14]; dst[15] = src[15];
00332     }
00333 
00338     void publishPointCloudMsg(ImagePair& imagePair, ros::Time stamp) {
00339         if(imagePair.getPixelFormat(1) != ImagePair::FORMAT_12_BIT) {
00340             return; // This is not a disparity map
00341         }
00342 
00343         // Transform Q-matrix if desired
00344         float qRos[16];
00345         if(rosCoordinateSystem) {
00346             qMatrixToRosCoords(imagePair.getQMatrix(), qRos);
00347             imagePair.setQMatrix(qRos);
00348         }
00349 
00350         // Get 3D points
00351         float* pointMap = nullptr;
00352         try {
00353             pointMap = recon3d->createPointMap(imagePair, 0);
00354         } catch(std::exception& ex) {
00355             cerr << "Error creating point cloud: " << ex.what() << endl;
00356             return;
00357         }
00358 
00359         // Create message object and set header
00360         pointCloudMsg->header.stamp = stamp;
00361         pointCloudMsg->header.frame_id = frame;
00362         pointCloudMsg->header.seq = imagePair.getSequenceNumber(); // Actually ROS will overwrite this
00363 
00364         // Copy 3D points
00365         if(pointCloudMsg->data.size() != imagePair.getWidth()*imagePair.getHeight()*4*sizeof(float)) {
00366             // Allocate buffer
00367             pointCloudMsg->data.resize(imagePair.getWidth()*imagePair.getHeight()*4*sizeof(float));
00368 
00369             // Set basic data
00370             pointCloudMsg->width = imagePair.getWidth();
00371             pointCloudMsg->height = imagePair.getHeight();
00372             pointCloudMsg->is_bigendian = false;
00373             pointCloudMsg->point_step = 4*sizeof(float);
00374             pointCloudMsg->row_step = imagePair.getWidth() * pointCloudMsg->point_step;
00375             pointCloudMsg->is_dense = false;
00376         }
00377 
00378         if(maxDepth < 0) {
00379             // Just copy everything
00380             memcpy(&pointCloudMsg->data[0], pointMap,
00381                 imagePair.getWidth()*imagePair.getHeight()*4*sizeof(float));
00382         } else {
00383             // Only copy points up to maximum depth
00384             if(rosCoordinateSystem) {
00385                 copyPointCloudClamped<0>(pointMap, reinterpret_cast<float*>(&pointCloudMsg->data[0]),
00386                     imagePair.getWidth()*imagePair.getHeight());
00387             } else {
00388                 copyPointCloudClamped<2>(pointMap, reinterpret_cast<float*>(&pointCloudMsg->data[0]),
00389                     imagePair.getWidth()*imagePair.getHeight());
00390             }
00391         }
00392 
00393         // Copy intensity values
00394         if(intensityChannel) {
00395             // Get pointers to the beginnig and end of the point cloud
00396             unsigned char* cloudStart = &pointCloudMsg->data[0];
00397             unsigned char* cloudEnd = &pointCloudMsg->data[0]
00398                 + imagePair.getWidth()*imagePair.getHeight()*4*sizeof(float);
00399 
00400             // Get pointer to the current pixel and end of current row
00401             unsigned char* imagePtr = imagePair.getPixelData(0);
00402             unsigned char* rowEndPtr = imagePtr + imagePair.getWidth();
00403             int rowIncrement = imagePair.getRowStride(0) - imagePair.getWidth();
00404 
00405             for(unsigned char* cloudPtr = cloudStart + 3*sizeof(float);
00406                     cloudPtr < cloudEnd; cloudPtr+= 4*sizeof(float)) {
00407                 *cloudPtr = *imagePtr;
00408 
00409                 imagePtr++;
00410                 if(imagePtr == rowEndPtr) {
00411                     // Progress to next row
00412                     imagePtr += rowIncrement;
00413                     rowEndPtr = imagePtr + imagePair.getWidth();
00414                 }
00415             }
00416         }
00417 
00418         cloudPublisher->publish(pointCloudMsg);
00419     }
00420 
00425     template <int coord> void copyPointCloudClamped(float* src, float* dst, int size) {
00426         // Only copy points that are below the minimum depth
00427         float* endPtr = src + 4*size;
00428         for(float *srcPtr = src, *dstPtr = dst; srcPtr < endPtr; srcPtr+=4, dstPtr+=4) {
00429             if(srcPtr[coord] > maxDepth) {
00430                 dstPtr[0] = std::numeric_limits<float>::quiet_NaN();
00431                 dstPtr[1] = std::numeric_limits<float>::quiet_NaN();
00432                 dstPtr[2] = std::numeric_limits<float>::quiet_NaN();
00433             } else {
00434                 dstPtr[0] = srcPtr[0];
00435                 dstPtr[1] = srcPtr[1];
00436                 dstPtr[2] = srcPtr[2];
00437             }
00438         }
00439     }
00440 
00445     void initPointCloud() {
00446         ros::NodeHandle privateNh("~");
00447 
00448         // Initialize 3D reconstruction class
00449         recon3d.reset(new Reconstruct3D);
00450 
00451         // Initialize message
00452         pointCloudMsg.reset(new sensor_msgs::PointCloud2);
00453 
00454         // Set channel information.
00455         sensor_msgs::PointField fieldX;
00456         fieldX.name ="x";
00457         fieldX.offset = 0;
00458         fieldX.datatype = sensor_msgs::PointField::FLOAT32;
00459         fieldX.count = 1;
00460         pointCloudMsg->fields.push_back(fieldX);
00461 
00462         sensor_msgs::PointField fieldY;
00463         fieldY.name ="y";
00464         fieldY.offset = sizeof(float);
00465         fieldY.datatype = sensor_msgs::PointField::FLOAT32;
00466         fieldY.count = 1;
00467         pointCloudMsg->fields.push_back(fieldY);
00468 
00469         sensor_msgs::PointField fieldZ;
00470         fieldZ.name ="z";
00471         fieldZ.offset = 2*sizeof(float);
00472         fieldZ.datatype = sensor_msgs::PointField::FLOAT32;
00473         fieldZ.count = 1;
00474         pointCloudMsg->fields.push_back(fieldZ);
00475 
00476         if(intensityChannel) {
00477             sensor_msgs::PointField fieldI;
00478             fieldI.name ="intensity";
00479             fieldI.offset = 3*sizeof(float);
00480             fieldI.datatype = sensor_msgs::PointField::UINT8;
00481             fieldI.count = 1;
00482             pointCloudMsg->fields.push_back(fieldI);
00483         }
00484     }
00485 
00489     void publishCameraInfo(ros::Time stamp, const ImagePair& imagePair) {
00490         if(camInfoMsg == NULL) {
00491             // Initialize the camera info structure
00492             camInfoMsg.reset(new nerian_sp1::StereoCameraInfo);
00493 
00494             camInfoMsg->header.frame_id = frame;
00495             camInfoMsg->header.seq = imagePair.getSequenceNumber(); // Actually ROS will overwrite this
00496 
00497             if(calibFile != "") {
00498                 std::vector<int> sizeVec;
00499                 calibStorage["size"] >> sizeVec;
00500                 if(sizeVec.size() != 2) {
00501                     std::runtime_error("Calibration file format error!");
00502                 }
00503 
00504                 camInfoMsg->left_info.header = camInfoMsg->header;
00505                 camInfoMsg->left_info.width = sizeVec[0];
00506                 camInfoMsg->left_info.height = sizeVec[1];
00507                 camInfoMsg->left_info.distortion_model = "plumb_bob";
00508                 calibStorage["D1"] >> camInfoMsg->left_info.D;
00509                 readCalibrationArray("M1", camInfoMsg->left_info.K);
00510                 readCalibrationArray("R1", camInfoMsg->left_info.R);
00511                 readCalibrationArray("P1", camInfoMsg->left_info.P);
00512                 camInfoMsg->left_info.binning_x = 1;
00513                 camInfoMsg->left_info.binning_y = 1;
00514                 camInfoMsg->left_info.roi.do_rectify = false;
00515                 camInfoMsg->left_info.roi.height = 0;
00516                 camInfoMsg->left_info.roi.width = 0;
00517                 camInfoMsg->left_info.roi.x_offset = 0;
00518                 camInfoMsg->left_info.roi.y_offset = 0;
00519 
00520                 camInfoMsg->right_info.header = camInfoMsg->header;
00521                 camInfoMsg->right_info.width = sizeVec[0];
00522                 camInfoMsg->right_info.height = sizeVec[1];
00523                 camInfoMsg->right_info.distortion_model = "plumb_bob";
00524                 calibStorage["D2"] >> camInfoMsg->right_info.D;
00525                 readCalibrationArray("M2", camInfoMsg->right_info.K);
00526                 readCalibrationArray("R2", camInfoMsg->right_info.R);
00527                 readCalibrationArray("P2", camInfoMsg->right_info.P);
00528                 camInfoMsg->right_info.binning_x = 1;
00529                 camInfoMsg->right_info.binning_y = 1;
00530                 camInfoMsg->right_info.roi.do_rectify = false;
00531                 camInfoMsg->right_info.roi.height = 0;
00532                 camInfoMsg->right_info.roi.width = 0;
00533                 camInfoMsg->right_info.roi.x_offset = 0;
00534                 camInfoMsg->right_info.roi.y_offset = 0;
00535 
00536                 readCalibrationArray("Q", camInfoMsg->Q);
00537                 readCalibrationArray("T", camInfoMsg->T_left_right);
00538                 readCalibrationArray("R", camInfoMsg->R_left_right);
00539             }
00540         }
00541 
00542         double dt = (stamp - lastCamInfoPublish).toSec();
00543         if(dt > 1.0) {
00544             // Rather use the Q-matrix that we received over the network if it is valid
00545             const float* qMatrix = imagePair.getQMatrix();
00546             if(qMatrix[0] != 0.0) {
00547                 for(int i=0; i<16; i++) {
00548                     camInfoMsg->Q[i] = static_cast<double>(qMatrix[i]);
00549                 }
00550             }
00551 
00552             // Publish once per second
00553             camInfoMsg->header.stamp = stamp;
00554             camInfoMsg->left_info.header.stamp = stamp;
00555             camInfoMsg->right_info.header.stamp = stamp;
00556             cameraInfoPublisher->publish(camInfoMsg);
00557 
00558             lastCamInfoPublish = stamp;
00559         }
00560     }
00561 
00565     template<class T>
00566     void readCalibrationArray(const char* key, T& dest) {
00567         std::vector<double> doubleVec;
00568         calibStorage[key] >> doubleVec;
00569 
00570         if(doubleVec.size() != dest.size()) {
00571             std::runtime_error("Calibration file format error!");
00572         }
00573 
00574         std::copy(doubleVec.begin(), doubleVec.end(), dest.begin());
00575     }
00576 };
00577 
00578 int main(int argc, char** argv) {
00579     try {
00580         ros::init(argc, argv, "nerian_sp1");
00581         Sp1Node node;
00582         node.init();
00583         return node.run();
00584     } catch(const std::exception& ex) {
00585         ROS_FATAL("Exception occured: %s", ex.what());
00586         return 1;
00587     }
00588 }


nerian_sp1
Author(s): Nerian Vision Technologies
autogenerated on Thu Jun 6 2019 18:25:49