nerian_stereo_node_base.cpp

Go to the documentation of this file.

/*******************************************************************************
 * Copyright (c) 2019 Nerian Vision GmbH
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *******************************************************************************/

#include "nerian_stereo_node_base.h"

namespace nerian_stereo {

void StereoNodeBase::dynamicReconfigureCallback(nerian_stereo::NerianStereoConfig &config, uint32_t level) {
    if (initialConfigReceived) {
        ROS_INFO("Received a new configuration via dynamic_reconfigure");
        // Unfortunately, we have to check for each potential change (no configuration deltas provided).
        // This is done in the autogenerated external code.
        autogen_dynamicReconfigureCallback(config, level);
    } else {
        initialConfigReceived = true;
    }
    lastKnownConfig = config;
}

void StereoNodeBase::updateParameterServerFromDevice(std::map<std::string, ParameterInfo>& cfg) {
    // Publish the current config to the parameter server
    autogen_updateParameterServerFromDevice(cfg);
    // Publish reboot flag to definitely be set to false in the parameter server
    getNH().setParam("/nerian_stereo/reboot", false);
}

void StereoNodeBase::updateDynamicReconfigureFromDevice(std::map<std::string, ParameterInfo>& cfg) {
    autogen_updateDynamicReconfigureFromDevice(cfg);
}
/*
 * \brief Initialize and publish configuration with a dynamic_reconfigure server
 */
void StereoNodeBase::initDynamicReconfigure() {
    std::map<std::string, ParameterInfo> ssParams;
    // Connect to parameter server on device
    ROS_INFO("Connecting to %s for parameter service", remoteHost.c_str());
    try {
        sceneScanParameters.reset(new SceneScanParameters(remoteHost.c_str()));
    } catch(visiontransfer::ParameterException& e) {
        ROS_ERROR("ParameterException while connecting to parameter service: %s", e.what());
        throw;
    }
    try {
        ssParams = sceneScanParameters->getAllParameters();
    } catch(visiontransfer::TransferException& e) {
        ROS_ERROR("TransferException while obtaining parameter enumeration: %s", e.what());
        throw;
    } catch(visiontransfer::ParameterException& e) {
        ROS_ERROR("ParameterException while obtaining parameter enumeration: %s", e.what());
        throw;
    }
    // First make sure that the parameter server gets all *current* values
    updateParameterServerFromDevice(ssParams);
    // Initialize (and publish) initial configuration from compile-time generated header
    dynReconfServer.reset(new dynamic_reconfigure::Server<nerian_stereo::NerianStereoConfig>());
    // Obtain and publish the default, min, and max values from the device to dyn_reconf
    updateDynamicReconfigureFromDevice(ssParams);
    // Callback for future changes requested from the ROS side
    dynReconfServer->setCallback(boost::bind(&StereoNodeBase::dynamicReconfigureCallback, this, _1, _2));
}

void StereoNodeBase::init() {
    ros::NodeHandle& privateNh = getPrivateNH();

    // Read all ROS parameters
    std::string intensityChannel = "mono8";
    privateNh.getParam("point_cloud_intensity_channel", intensityChannel);
    if(intensityChannel == "none") {
        pointCloudColorMode = NONE;
    } else if(intensityChannel == "rgb8") {
        pointCloudColorMode = RGB_COMBINED;
    } else if(intensityChannel == "rgb32f") {
        pointCloudColorMode = RGB_SEPARATE;
    } else {
        pointCloudColorMode = INTENSITY;
    }

    if (!privateNh.getParam("color_code_disparity_map", colorCodeDispMap)) {
        colorCodeDispMap = "";
    }

    if (!privateNh.getParam("color_code_legend", colorCodeLegend)) {
        colorCodeLegend = false;
    }

    if (!privateNh.getParam("frame", frame)) {
        frame = "world";
    }

    if (!privateNh.getParam("remote_port", remotePort)) {
        remotePort = "7681";
    }

    if (!privateNh.getParam("remote_host", remoteHost)) {
        remoteHost = "0.0.0.0";
    }

    if (!privateNh.getParam("use_tcp", useTcp)) {
        useTcp = false;
    }

    if (!privateNh.getParam("ros_coordinate_system", rosCoordinateSystem)) {
        rosCoordinateSystem = true;
    }

    if (!privateNh.getParam("ros_timestamps", rosTimestamps)) {
        rosTimestamps = true;
    }

    if (!privateNh.getParam("calibration_file", calibFile)) {
        calibFile = "";
    }

    if (!privateNh.getParam("delay_execution", execDelay)) {
        execDelay = 0;
    }

    if (!privateNh.getParam("max_depth", maxDepth)) {
        maxDepth = -1;
    }

    if (!privateNh.getParam("q_from_calib_file", useQFromCalibFile)) {
        useQFromCalibFile = false;
    }

    // Apply an initial delay if configured
    ros::Duration(execDelay).sleep();

    // Create publishers
    disparityPublisher.reset(new ros::Publisher(getNH().advertise<sensor_msgs::Image>(
        "/nerian_stereo/disparity_map", 5)));
    leftImagePublisher.reset(new ros::Publisher(getNH().advertise<sensor_msgs::Image>(
        "/nerian_stereo/left_image", 5)));
    rightImagePublisher.reset(new ros::Publisher(getNH().advertise<sensor_msgs::Image>(
        "/nerian_stereo/right_image", 5)));

    loadCameraCalibration();

    cameraInfoPublisher.reset(new ros::Publisher(getNH().advertise<nerian_stereo::StereoCameraInfo>(
        "/nerian_stereo/stereo_camera_info", 1)));
    cloudPublisher.reset(new ros::Publisher(getNH().advertise<sensor_msgs::PointCloud2>(
        "/nerian_stereo/point_cloud", 5)));
}

void StereoNodeBase::prepareAsyncTransfer() {
    ROS_INFO("Connecting to %s:%s for data transfer", remoteHost.c_str(), remotePort.c_str());
    asyncTransfer.reset(new AsyncTransfer(remoteHost.c_str(), remotePort.c_str(),
        useTcp ? ImageProtocol::PROTOCOL_TCP : ImageProtocol::PROTOCOL_UDP));
}

void StereoNodeBase::processOneImagePair() {
    // Receive image data
    ImagePair imagePair;
    if(!asyncTransfer->collectReceivedImagePair(imagePair, 0.5)) {
        return;
    }

    // Get time stamp
    ros::Time stamp;
    if(rosTimestamps) {
        stamp = ros::Time::now();
    } else {
        int secs = 0, microsecs = 0;
        imagePair.getTimestamp(secs, microsecs);
        stamp = ros::Time(secs, microsecs*1000);
    }

    // Publish image data messages
    publishImageMsg(imagePair, 0, stamp, false, leftImagePublisher.get());
    if(imagePair.isImageDisparityPair()) {
        publishImageMsg(imagePair, 1, stamp, true, disparityPublisher.get());
    } else {
        publishImageMsg(imagePair, 1, stamp, false, rightImagePublisher.get());
    }

    if(cloudPublisher->getNumSubscribers() > 0) {
        if(recon3d == nullptr) {
            // First initialize
            initPointCloud();
        }

        publishPointCloudMsg(imagePair, stamp);
    }

    if(cameraInfoPublisher != NULL && cameraInfoPublisher->getNumSubscribers() > 0) {
        publishCameraInfo(stamp, imagePair);
    }

    // Display some simple statistics
    frameNum++;
    if(stamp.sec != lastLogTime.sec) {
        if(lastLogTime != ros::Time()) {
            double dt = (stamp - lastLogTime).toSec();
            double fps = (frameNum - lastLogFrames) / dt;
            ROS_INFO("%.1f fps", fps);
        }
        lastLogFrames = frameNum;
        lastLogTime = stamp;
    }
}

void StereoNodeBase::loadCameraCalibration() {
    if(calibFile == "" ) {
        ROS_WARN("No camera calibration file configured. Cannot publish detailed camera information!");
    } else {
        bool success = false;
        try {
            if (calibStorage.open(calibFile, cv::FileStorage::READ)) {
                success = true;
            }
        } catch(...) {
        }

        if(!success) {
            ROS_WARN("Error reading calibration file: %s\n"
                "Cannot publish detailed camera information!", calibFile.c_str());
        }
    }
}

void StereoNodeBase::publishImageMsg(const ImagePair& imagePair, int imageIndex, ros::Time stamp, bool allowColorCode,
        ros::Publisher* publisher) {

    if(publisher->getNumSubscribers() <= 0) {
        return; //No subscribers
    }

    cv_bridge::CvImage cvImg;
    cvImg.header.frame_id = frame;
    cvImg.header.stamp = stamp;
    cvImg.header.seq = imagePair.getSequenceNumber(); // Actually ROS will overwrite this

    bool format12Bit = (imagePair.getPixelFormat(imageIndex) == ImagePair::FORMAT_12_BIT_MONO);
    cv::Mat monoImg(imagePair.getHeight(), imagePair.getWidth(),
        format12Bit ? CV_16UC1 : CV_8UC1,
        imagePair.getPixelData(imageIndex), imagePair.getRowStride(imageIndex));
    string encoding = "";

    if(colorCodeDispMap == "" || colorCodeDispMap == "none" || !allowColorCode || !format12Bit) {
        cvImg.image = monoImg;
        encoding = (format12Bit ? "mono16": "mono8");
    } else {
        if(colCoder == NULL) {
            int dispMin = 0, dispMax = 0;
            imagePair.getDisparityRange(dispMin, dispMax);

            colCoder.reset(new ColorCoder(
                colorCodeDispMap == "rainbow" ? ColorCoder::COLOR_RAINBOW_BGR : ColorCoder::COLOR_RED_BLUE_BGR,
                dispMin*16, dispMax*16, true, true));
            if(colorCodeLegend) {
                // Create the legend
                colDispMap = colCoder->createLegendBorder(monoImg.cols, monoImg.rows, 1.0/16.0);
            } else {
                colDispMap = cv::Mat_<cv::Vec3b>(monoImg.rows, monoImg.cols);
            }
        }

        cv::Mat_<cv::Vec3b> dispSection = colDispMap(cv::Rect(0, 0, monoImg.cols, monoImg.rows));

        colCoder->codeImage(cv::Mat_<unsigned short>(monoImg), dispSection);
        cvImg.image = colDispMap;
        encoding = "bgr8";
    }

    sensor_msgs::ImagePtr msg = cvImg.toImageMsg();
    msg->encoding = encoding;
    publisher->publish(msg);
}

void StereoNodeBase::qMatrixToRosCoords(const float* src, float* dst) {
    dst[0] = src[8];   dst[1] = src[9];
    dst[2] = src[10];  dst[3] = src[11];

    dst[4] = -src[0];  dst[5] = -src[1];
    dst[6] = -src[2];  dst[7] = -src[3];

    dst[8] = -src[4];  dst[9] = -src[5];
    dst[10] = -src[6]; dst[11] = -src[7];

    dst[12] = src[12]; dst[13] = src[13];
    dst[14] = src[14]; dst[15] = src[15];
}

void StereoNodeBase::publishPointCloudMsg(ImagePair& imagePair, ros::Time stamp) {
    if(imagePair.getPixelFormat(1) != ImagePair::FORMAT_12_BIT_MONO) {
        return; // This is not a disparity map
    }

    // Set static q matrix if desired
    if(useQFromCalibFile) {
        static std::vector<float> q;
        calibStorage["Q"] >> q;
        imagePair.setQMatrix(&q[0]);
    }

    // Transform Q-matrix if desired
    float qRos[16];
    if(rosCoordinateSystem) {
        qMatrixToRosCoords(imagePair.getQMatrix(), qRos);
        imagePair.setQMatrix(qRos);
    }

    // Get 3D points
    float* pointMap = nullptr;
    try {
        pointMap = recon3d->createPointMap(imagePair, 0);
    } catch(std::exception& ex) {
        cerr << "Error creating point cloud: " << ex.what() << endl;
        return;
    }

    // Create message object and set header
    pointCloudMsg->header.stamp = stamp;
    pointCloudMsg->header.frame_id = frame;
    pointCloudMsg->header.seq = imagePair.getSequenceNumber(); // Actually ROS will overwrite this

    // Copy 3D points
    if(pointCloudMsg->data.size() != imagePair.getWidth()*imagePair.getHeight()*4*sizeof(float)) {
        // Allocate buffer
        pointCloudMsg->data.resize(imagePair.getWidth()*imagePair.getHeight()*4*sizeof(float));

        // Set basic data
        pointCloudMsg->width = imagePair.getWidth();
        pointCloudMsg->height = imagePair.getHeight();
        pointCloudMsg->is_bigendian = false;
        pointCloudMsg->point_step = 4*sizeof(float);
        pointCloudMsg->row_step = imagePair.getWidth() * pointCloudMsg->point_step;
        pointCloudMsg->is_dense = false;
    }

    if(maxDepth < 0) {
        // Just copy everything
        memcpy(&pointCloudMsg->data[0], pointMap,
            imagePair.getWidth()*imagePair.getHeight()*4*sizeof(float));
    } else {
        // Only copy points up to maximum depth
        if(rosCoordinateSystem) {
            copyPointCloudClamped<0>(pointMap, reinterpret_cast<float*>(&pointCloudMsg->data[0]),
                imagePair.getWidth()*imagePair.getHeight());
        } else {
            copyPointCloudClamped<2>(pointMap, reinterpret_cast<float*>(&pointCloudMsg->data[0]),
                imagePair.getWidth()*imagePair.getHeight());
        }
    }

    // Copy intensity values
    switch(pointCloudColorMode) {
        case INTENSITY:
            copyPointCloudIntensity<INTENSITY>(imagePair);
            break;
        case RGB_COMBINED:
            copyPointCloudIntensity<RGB_COMBINED>(imagePair);
            break;
        case RGB_SEPARATE:
            copyPointCloudIntensity<RGB_SEPARATE>(imagePair);
            break;
        case NONE:
            break;
    }

    cloudPublisher->publish(pointCloudMsg);
}

template <StereoNodeBase::PointCloudColorMode colorMode> void StereoNodeBase::copyPointCloudIntensity(ImagePair& imagePair) {
    // Get pointers to the beginnig and end of the point cloud
    unsigned char* cloudStart = &pointCloudMsg->data[0];
    unsigned char* cloudEnd = &pointCloudMsg->data[0]
        + imagePair.getWidth()*imagePair.getHeight()*4*sizeof(float);

    if(imagePair.getPixelFormat(0) == ImagePair::FORMAT_8_BIT_MONO) {
        // Get pointer to the current pixel and end of current row
        unsigned char* imagePtr = imagePair.getPixelData(0);
        unsigned char* rowEndPtr = imagePtr + imagePair.getWidth();
        int rowIncrement = imagePair.getRowStride(0) - imagePair.getWidth();

        for(unsigned char* cloudPtr = cloudStart + 3*sizeof(float);
                cloudPtr < cloudEnd; cloudPtr+= 4*sizeof(float)) {
            if(colorMode == RGB_SEPARATE) {// RGB as float
                *reinterpret_cast<float*>(cloudPtr) = static_cast<float>(*imagePtr) / 255.0F;
            } else if(colorMode == RGB_COMBINED) {// RGB as integer
                const unsigned char intensity = *imagePtr;
                *reinterpret_cast<unsigned int*>(cloudPtr) = (intensity << 16) | (intensity << 8) | intensity;
            } else {
                *cloudPtr = *imagePtr;
            }

            imagePtr++;
            if(imagePtr == rowEndPtr) {
                // Progress to next row
                imagePtr += rowIncrement;
                rowEndPtr = imagePtr + imagePair.getWidth();
            }
        }
    } else if(imagePair.getPixelFormat(0) == ImagePair::FORMAT_12_BIT_MONO) {
        // Get pointer to the current pixel and end of current row
        unsigned short* imagePtr = reinterpret_cast<unsigned short*>(imagePair.getPixelData(0));
        unsigned short* rowEndPtr = imagePtr + imagePair.getWidth();
        int rowIncrement = imagePair.getRowStride(0) - 2*imagePair.getWidth();

        for(unsigned char* cloudPtr = cloudStart + 3*sizeof(float);
                cloudPtr < cloudEnd; cloudPtr+= 4*sizeof(float)) {

            if(colorMode == RGB_SEPARATE) {// RGB as float
                *reinterpret_cast<float*>(cloudPtr) = static_cast<float>(*imagePtr) / 4095.0F;
            } else if(colorMode == RGB_COMBINED) {// RGB as integer
                const unsigned char intensity = *imagePtr/16;
                *reinterpret_cast<unsigned int*>(cloudPtr) = (intensity << 16) | (intensity << 8) | intensity;
            } else {
                *cloudPtr = *imagePtr/16;
            }

            imagePtr++;
            if(imagePtr == rowEndPtr) {
                // Progress to next row
                imagePtr += rowIncrement;
                rowEndPtr = imagePtr + imagePair.getWidth();
            }
        }
    } else if(imagePair.getPixelFormat(0) == ImagePair::FORMAT_8_BIT_RGB) {
        // Get pointer to the current pixel and end of current row
        unsigned char* imagePtr = imagePair.getPixelData(0);
        unsigned char* rowEndPtr = imagePtr + imagePair.getWidth();
        int rowIncrement = imagePair.getRowStride(0) - imagePair.getWidth();

        static bool warned = false;
        if(colorMode == RGB_SEPARATE && !warned) {
            warned = true;
            ROS_WARN("RGBF32 is not supported for color images. Please use RGB8!");
        }

        for(unsigned char* cloudPtr = cloudStart + 3*sizeof(float);
                cloudPtr < cloudEnd; cloudPtr+= 4*sizeof(float)) {
            if(colorMode == RGB_SEPARATE) {// RGB as float
                *reinterpret_cast<float*>(cloudPtr) = static_cast<float>(imagePtr[2]) / 255.0F;
            } else if(colorMode == RGB_COMBINED) {// RGB as integer
                *reinterpret_cast<unsigned int*>(cloudPtr) = (imagePtr[0] << 16) | (imagePtr[1] << 8) | imagePtr[2];
            } else {
                *cloudPtr = (imagePtr[0] + imagePtr[1]*2 + imagePtr[2])/4;
            }

            imagePtr+=3;
            if(imagePtr == rowEndPtr) {
                // Progress to next row
                imagePtr += rowIncrement;
                rowEndPtr = imagePtr + imagePair.getWidth();
            }
        }
    } else {
        throw std::runtime_error("Invalid pixel format!");
    }
}

template <int coord> void StereoNodeBase::copyPointCloudClamped(float* src, float* dst, int size) {
    // Only copy points that are below the minimum depth
    float* endPtr = src + 4*size;
    for(float *srcPtr = src, *dstPtr = dst; srcPtr < endPtr; srcPtr+=4, dstPtr+=4) {
        if(srcPtr[coord] > maxDepth) {
            dstPtr[0] = std::numeric_limits<float>::quiet_NaN();
            dstPtr[1] = std::numeric_limits<float>::quiet_NaN();
            dstPtr[2] = std::numeric_limits<float>::quiet_NaN();
        } else {
            dstPtr[0] = srcPtr[0];
            dstPtr[1] = srcPtr[1];
            dstPtr[2] = srcPtr[2];
        }
    }
}

void StereoNodeBase::initPointCloud() {
    //ros::NodeHandle privateNh("~"); // RYT TODO check

    // Initialize 3D reconstruction class
    recon3d.reset(new Reconstruct3D);

    // Initialize message
    pointCloudMsg.reset(new sensor_msgs::PointCloud2);

    // Set channel information.
    sensor_msgs::PointField fieldX;
    fieldX.name ="x";
    fieldX.offset = 0;
    fieldX.datatype = sensor_msgs::PointField::FLOAT32;
    fieldX.count = 1;
    pointCloudMsg->fields.push_back(fieldX);

    sensor_msgs::PointField fieldY;
    fieldY.name ="y";
    fieldY.offset = sizeof(float);
    fieldY.datatype = sensor_msgs::PointField::FLOAT32;
    fieldY.count = 1;
    pointCloudMsg->fields.push_back(fieldY);

    sensor_msgs::PointField fieldZ;
    fieldZ.name ="z";
    fieldZ.offset = 2*sizeof(float);
    fieldZ.datatype = sensor_msgs::PointField::FLOAT32;
    fieldZ.count = 1;
    pointCloudMsg->fields.push_back(fieldZ);

    if(pointCloudColorMode == INTENSITY) {
        sensor_msgs::PointField fieldI;
        fieldI.name ="intensity";
        fieldI.offset = 3*sizeof(float);
        fieldI.datatype = sensor_msgs::PointField::UINT8;
        fieldI.count = 1;
        pointCloudMsg->fields.push_back(fieldI);
    }
    else if(pointCloudColorMode == RGB_SEPARATE) {
        sensor_msgs::PointField fieldRed;
        fieldRed.name ="r";
        fieldRed.offset = 3*sizeof(float);
        fieldRed.datatype = sensor_msgs::PointField::FLOAT32;
        fieldRed.count = 1;
        pointCloudMsg->fields.push_back(fieldRed);

        sensor_msgs::PointField fieldGreen;
        fieldGreen.name ="g";
        fieldGreen.offset = 3*sizeof(float);
        fieldGreen.datatype = sensor_msgs::PointField::FLOAT32;
        fieldGreen.count = 1;
        pointCloudMsg->fields.push_back(fieldGreen);

        sensor_msgs::PointField fieldBlue;
        fieldBlue.name ="b";
        fieldBlue.offset = 3*sizeof(float);
        fieldBlue.datatype = sensor_msgs::PointField::FLOAT32;
        fieldBlue.count = 1;
        pointCloudMsg->fields.push_back(fieldBlue);
    } else if(pointCloudColorMode == RGB_COMBINED) {
        sensor_msgs::PointField fieldRGB;
        fieldRGB.name ="rgb";
        fieldRGB.offset = 3*sizeof(float);
        fieldRGB.datatype = sensor_msgs::PointField::UINT32;
        fieldRGB.count = 1;
        pointCloudMsg->fields.push_back(fieldRGB);
    }
}

void StereoNodeBase::publishCameraInfo(ros::Time stamp, const ImagePair& imagePair) {
    if(camInfoMsg == NULL) {
        // Initialize the camera info structure
        camInfoMsg.reset(new nerian_stereo::StereoCameraInfo);

        camInfoMsg->header.frame_id = frame;
        camInfoMsg->header.seq = imagePair.getSequenceNumber(); // Actually ROS will overwrite this

        if(calibFile != "") {
            std::vector<int> sizeVec;
            calibStorage["size"] >> sizeVec;
            if(sizeVec.size() != 2) {
                std::runtime_error("Calibration file format error!");
            }

            camInfoMsg->left_info.header = camInfoMsg->header;
            camInfoMsg->left_info.width = sizeVec[0];
            camInfoMsg->left_info.height = sizeVec[1];
            camInfoMsg->left_info.distortion_model = "plumb_bob";
            calibStorage["D1"] >> camInfoMsg->left_info.D;
            readCalibrationArray("M1", camInfoMsg->left_info.K);
            readCalibrationArray("R1", camInfoMsg->left_info.R);
            readCalibrationArray("P1", camInfoMsg->left_info.P);
            camInfoMsg->left_info.binning_x = 1;
            camInfoMsg->left_info.binning_y = 1;
            camInfoMsg->left_info.roi.do_rectify = false;
            camInfoMsg->left_info.roi.height = 0;
            camInfoMsg->left_info.roi.width = 0;
            camInfoMsg->left_info.roi.x_offset = 0;
            camInfoMsg->left_info.roi.y_offset = 0;

            camInfoMsg->right_info.header = camInfoMsg->header;
            camInfoMsg->right_info.width = sizeVec[0];
            camInfoMsg->right_info.height = sizeVec[1];
            camInfoMsg->right_info.distortion_model = "plumb_bob";
            calibStorage["D2"] >> camInfoMsg->right_info.D;
            readCalibrationArray("M2", camInfoMsg->right_info.K);
            readCalibrationArray("R2", camInfoMsg->right_info.R);
            readCalibrationArray("P2", camInfoMsg->right_info.P);
            camInfoMsg->right_info.binning_x = 1;
            camInfoMsg->right_info.binning_y = 1;
            camInfoMsg->right_info.roi.do_rectify = false;
            camInfoMsg->right_info.roi.height = 0;
            camInfoMsg->right_info.roi.width = 0;
            camInfoMsg->right_info.roi.x_offset = 0;
            camInfoMsg->right_info.roi.y_offset = 0;

            readCalibrationArray("Q", camInfoMsg->Q);
            readCalibrationArray("T", camInfoMsg->T_left_right);
            readCalibrationArray("R", camInfoMsg->R_left_right);
        }
    }

    double dt = (stamp - lastCamInfoPublish).toSec();
    if(dt > 1.0) {
        // Rather use the Q-matrix that we received over the network if it is valid
        const float* qMatrix = imagePair.getQMatrix();
        if(qMatrix[0] != 0.0) {
            for(int i=0; i<16; i++) {
                camInfoMsg->Q[i] = static_cast<double>(qMatrix[i]);
            }
        }

        // Publish once per second
        camInfoMsg->header.stamp = stamp;
        camInfoMsg->left_info.header.stamp = stamp;
        camInfoMsg->right_info.header.stamp = stamp;
        cameraInfoPublisher->publish(camInfoMsg);

        lastCamInfoPublish = stamp;
    }
}

template<class T> void StereoNodeBase::readCalibrationArray(const char* key, T& dest) {
    std::vector<double> doubleVec;
    calibStorage[key] >> doubleVec;

    if(doubleVec.size() != dest.size()) {
        std::runtime_error("Calibration file format error!");
    }

    std::copy(doubleVec.begin(), doubleVec.end(), dest.begin());
}

} // namespace