RectifiedFocalLengthUtility.cc

Go to the documentation of this file.

#ifdef WIN32
#ifndef WIN32_LEAN_AND_MEAN
#define WIN32_LEAN_AND_MEAN 1
#endif

#include <windows.h>
#include <winsock2.h>
#else
#include <unistd.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fstream>
#include <map>
#include <string.h>

#include <Utilities/portability/getopt/getopt.h>
#include <Utilities/shared/CalibrationYaml.hh>
#include <MultiSense/MultiSenseChannel.hh>
#include <MultiSense/MultiSenseTypes.hh>

using namespace crl::multisense;

namespace {  // anonymous

void usage(const char *programNameP)
{
    fprintf(stderr,
            "USAGE: %s -f <rectified_focal_length> [<options>]\n",
            programNameP);
    fprintf(stderr, "Where <options> are:\n");
    fprintf(stderr, "\t-a <ip_address>      : ip address (default=10.66.171.21)\n");
    fprintf(stderr, "\t-s                   : set the rectified focal length (default is query)\n");

    exit(-1);
}


} // anonymous

int main(int    argc,
         char **argvPP)
{
    std::string ipAddress = "10.66.171.21";
    double rectifiedFocalLength = -1.0;
    bool set = false;
    bool hasAuxCamera = false;
    double currentFocalLength = 0.0;
    double tx = 0.0;
    double auxTx = 0.0;
    double auxTy = 0.0;

    //
    // Parse args

    int c;

    while(-1 != (c = getopt(argc, argvPP, "a:f:s")))
        switch(c) {
        case 'a': ipAddress            = std::string(optarg);    break;
        case 'f': rectifiedFocalLength = atof(optarg);           break;
        case 's': set                  = true;                   break;
        default: usage(*argvPP);                                 break;
        }

    //
    // Verify options. New focal lengths should be positive and large enough for the onboard
    // stereo pipeline

    if (rectifiedFocalLength < 200.0) {
        fprintf(stderr, "Invalid rectified focal length\n");
        usage(*argvPP);
    }

    //
    // Initialize communications.

    Channel *channelP = Channel::Create(ipAddress);
    if (NULL == channelP) {
        fprintf(stderr, "Failed to establish communications with \"%s\"\n",
                ipAddress.c_str());
        return -1;
    }

    //
    // Query Device Info

    Status status;

    system::DeviceInfo info;
    status = channelP->getDeviceInfo(info);
    if (Status_Ok != status)
    {
        fprintf(stderr, "Failed to query device info: %s\n",
                Channel::statusString(status));
        goto clean_out;
    }

    hasAuxCamera = info.hardwareRevision == system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27 ||
                   info.hardwareRevision == system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30 ||
                   info.hardwareRevision == system::DeviceInfo::HARDWARE_REV_MULTISENSE_MONOCAM;

    image::Calibration calibration;

    status = channelP->getImageCalibration(calibration);
    if (Status_Ok != status) {
        fprintf(stderr, "failed to query image calibration: %s\n",
                Channel::statusString(status));
        goto clean_out;
    }

    currentFocalLength = calibration.left.P[0][0];
    fprintf(stdout, "updating focal length from %f to %f\n",
            currentFocalLength, rectifiedFocalLength);

    if (set) {

        tx = calibration.right.P[0][3] / calibration.right.P[0][0];

        calibration.left.P[0][0]  = static_cast<float> (rectifiedFocalLength);
        calibration.left.P[1][1]  = static_cast<float> (rectifiedFocalLength);
        calibration.right.P[0][0] = static_cast<float> (rectifiedFocalLength);
        calibration.right.P[1][1] = static_cast<float> (rectifiedFocalLength);
        calibration.right.P[0][3] = static_cast<float> (rectifiedFocalLength * tx);

        if (hasAuxCamera) {

            auxTx = calibration.aux.P[0][3] / calibration.aux.P[0][0];
            auxTy = calibration.aux.P[1][3] / calibration.aux.P[1][1];

            calibration.aux.P[0][0] = static_cast<float> (rectifiedFocalLength);
            calibration.aux.P[1][1] = static_cast<float> (rectifiedFocalLength);
            calibration.aux.P[0][3] = static_cast<float> (rectifiedFocalLength * auxTx);
            calibration.aux.P[1][3] = static_cast<float> (rectifiedFocalLength * auxTy);
        }

        status = channelP->setImageCalibration(calibration);
        if (Status_Ok != status) {
            fprintf(stderr, "failed to set image calibration: %s\n",
                    Channel::statusString(status));
            goto clean_out;
        }

        fprintf(stdout, "Rectified focal length successfully updated to %f\n",
                rectifiedFocalLength);
    } else {

        fprintf(stdout, "Please specify -s to update the rectified focal length to %f\n",
                rectifiedFocalLength);
    }

clean_out:

    Channel::Destroy(channelP);
    return 0;
}