Legacy/SaveImageUtility/SaveImageUtility.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>
#include <arpa/inet.h> // htons
#endif
 
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <string>
 
#include <getopt/getopt.h>
#include <Io.hh>
 
#include <MultiSense/details/utility/Portability.hh>
#include <MultiSense/MultiSenseChannel.hh>
 
using namespace crl::multisense;
 
namespace {  // anonymous
 
volatile bool doneG = false;
 
struct UserData
{
    Channel *channelP;
    system::VersionInfo version;
    system::DeviceInfo deviceInfo;
    image::Calibration calibration;
};
 
void usage(const char *programNameP)
{
    std::cerr << "USAGE: " << programNameP << " [<options>]" << std::endl;
    std::cerr << "Where <options> are:" << std::endl;
    std::cerr << "\t-a <current_address>    : CURRENT IPV4 address (default=10.66.171.21)" << std::endl;
    std::cerr << "\t-m <mtu>                : MTU to set the camera to (default=1500)" << std::endl;
    std::cerr << "\t-r <head_id>    : remote head ID (default=0)" << std::endl;
 
    exit(1);
}
 
#ifdef WIN32
BOOL WINAPI signalHandler(DWORD dwCtrlType)
{
    CRL_UNUSED (dwCtrlType);
    doneG = true;
    return TRUE;
}
#else
void signalHandler(int sig)
{
    (void) sig;
    doneG = true;
}
#endif
 
RemoteHeadChannel getRemoteHeadIdFromString(const std::string &head_str)
{
  if (head_str == "VPB")
  {
      return Remote_Head_VPB;
  }
  else if (head_str == "0")
  {
      return Remote_Head_0;
  }
  else if (head_str == "1")
  {
      return Remote_Head_1;
  }
  else if (head_str == "2")
  {
      return Remote_Head_2;
  }
  else if (head_str == "3")
  {
      return Remote_Head_3;
  }
 
  fprintf(stderr, "Error: Unrecognized remote head\n");
  fprintf(stderr, "Please use one of the following:\n");
  fprintf(stderr, "\tVPB\n");
  fprintf(stderr, "\t0'\n");
  fprintf(stderr, "\t1\n");
  fprintf(stderr, "\t2\n");
  fprintf(stderr, "\t3\n");
  exit(EXIT_FAILURE);
}
 
system::DeviceMode getOperatingMode(const std::vector<system::DeviceMode> &modes)
{
    //
    // Get the full resolution image
 
    system::DeviceMode best_mode(0, 0, 0, -1);
    for (size_t i = 0 ; i < modes.size() ; ++i)
    {
        const system::DeviceMode &mode = modes[i];
        if (mode.width >= best_mode.width && mode.height >= best_mode.height && mode.disparities >= best_mode.disparities) {
            best_mode = mode;
        }
    }
 
    //
    // Target 1/4 of the full res setting. Check to see if there is a setting with a larger number of disparities
 
    system::DeviceMode target_mode(best_mode.width / 2, best_mode.height / 2, 0, best_mode.disparities);
 
    for (size_t i = 0 ; i < modes.size() ; ++i)
    {
        const system::DeviceMode &mode = modes[i];
        if (mode.width == target_mode.width && mode.height == target_mode.height && mode.disparities >= target_mode.disparities) {
            target_mode = mode;
        }
    }
 
    //
    // The camera does not support our targeted 1/4 res setting
 
    if (target_mode.supportedDataSources == 0) {
        return best_mode;
    }
 
    return target_mode;
}
 
 
std::string writeMatrix(const float* data, size_t width, size_t height)
{
    std::stringstream ss;
    ss << std::fixed << std::setprecision(6);
 
    for (size_t h = 0 ; h < height ; ++h) {
        for (size_t w = 0 ; w < width ; ++w) {
            ss << data[w + width * h] << ",";
        }
    }
 
    return ss.str();
}
 
std::string assembledInfoString(const image::Header&       header,
                                const system::DeviceInfo&  info,
                                const system::VersionInfo& version,
                                const image::Calibration&  calibration)
{
    std::stringstream ss;
 
    bool 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 ||
                        info.hardwareRevision == system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21i;
 
    ss << "SN," << info.serialNumber << ",";
    ss << "HWRev," << info.hardwareRevision << ",";
    ss << "APIVersion," << version.apiVersion << ",";
    ss << "FirmwareVersion," << version.sensorFirmwareVersion << ",";
    ss << "timeSec," << header.timeSeconds << ",";
    ss << "timeMiroSec," << header.timeMicroSeconds << ",";
    ss << "exposure," << header.exposure << ",";
    ss << "gain," << header.gain << ",";
    ss << "fps," << header.framesPerSecond << ",";
    ss << "leftM," << writeMatrix(&calibration.left.M[0][0], 3, 3);
    ss << "leftD," << writeMatrix(calibration.left.D, 8, 1);
    ss << "leftR," << writeMatrix(&calibration.left.R[0][0], 3, 3);
    ss << "leftP," << writeMatrix(&calibration.left.P[0][0], 4, 3);
    ss << "rightM," << writeMatrix(&calibration.right.M[0][0], 3, 3);
    ss << "rightD," << writeMatrix(calibration.right.D, 8, 1);
    ss << "rightR," << writeMatrix(&calibration.right.R[0][0], 3, 3);
    ss << "rightP," << writeMatrix(&calibration.right.P[0][0], 4, 3);
    if (hasAuxCamera) {
        ss << "auxM," << writeMatrix(&calibration.aux.M[0][0], 3, 3);
        ss << "auxD," << writeMatrix(calibration.aux.D, 8, 1);
        ss << "auxR," << writeMatrix(&calibration.aux.R[0][0], 3, 3);
        ss << "auxP," << writeMatrix(&calibration.aux.P[0][0], 4, 3);
    }
 
    return ss.str();
}
 
bool savePgm(const std::string&         fileName,
             const image::Header&       header,
             const system::DeviceInfo&  info,
             const system::VersionInfo& version,
             const image::Calibration&  calibration)
{
    const std::string comment = assembledInfoString(header, info, version, calibration);
 
    return io::savePgm(fileName,
                       header.width,
                       header.height,
                       header.bitsPerPixel,
                       comment,
                       header.imageDataP);
}
 
void ppsCallback(const pps::Header& header,
                 void              *userDataP)
{
    (void) userDataP;
        std::cerr << "PPS: " << header.sensorTime << " ns" << std::endl;
}
 
void laserCallback(const lidar::Header& header,
                   void                *userDataP)
{
    (void) userDataP;
    std::cerr << "lidar: " << header.pointCount << std::endl;
}
 
void imageCallback(const image::Header& header,
                   void                *userDataP)
{
    UserData *userData = reinterpret_cast<UserData*>(userDataP);
 
    static int64_t lastFrameId = -1;
 
    if (-1 == lastFrameId)
    {
        savePgm("test.pgm",
                header,
                userData->deviceInfo,
                userData->version,
                userData->calibration);
    }
 
    lastFrameId = header.frameId;
 
    image::Histogram histogram;
 
        if (Status_Ok != userData->channelP->getImageHistogram(header.frameId, histogram))
                std::cerr << "failed to get histogram for frame " << header.frameId << std::endl;
}
 
} // anonymous
 
int main(int    argc,
         char **argvPP)
{
    std::string currentAddress = "10.66.171.21";
    int32_t mtu = 0;
    RemoteHeadChannel head_id = Remote_Head_VPB;
 
#if WIN32
    SetConsoleCtrlHandler (signalHandler, TRUE);
#else
    signal(SIGINT, signalHandler);
#endif
 
    //
    // Parse args
 
    int c;
 
    while(-1 != (c = getopt(argc, argvPP, "a:m:r:")))
        switch(c) {
        case 'a': currentAddress = std::string(optarg);               break;
        case 'm': mtu            = atoi(optarg);                      break;
        case 'r': head_id        = getRemoteHeadIdFromString(optarg); break;
        default: usage(*argvPP);                                      break;
        }
 
    //
    // Initialize communications.
 
    Channel *channelP = Channel::Create(currentAddress, head_id);
    if (NULL == channelP) {
                std::cerr << "Failed to establish communications with \"" << currentAddress << "\"" << std::endl;
        return -1;
    }
 
    //
    // Query version
 
    Status status;
    system::VersionInfo v;
    VersionType version;
    std::vector<system::DeviceMode> deviceModes;
    system::DeviceMode operatingMode;
    image::Calibration calibration;
    system::DeviceInfo info;
    UserData userData;
 
    status = channelP->getSensorVersion(version);
    status = channelP->getVersionInfo(v);
 
    if (Status_Ok != status) {
                std::cerr << "Failed to query sensor version: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
        std::cout << "API build date      :  " << v.apiBuildDate << "\n";
        std::cout << "API version         :  0x" << std::hex << std::setw(4) << std::setfill('0') << v.apiVersion << "\n";
        std::cout << "Firmware build date :  " << v.sensorFirmwareBuildDate << "\n";
        std::cout << "Firmware version    :  0x" << std::hex << std::setw(4) << std::setfill('0') << v.sensorFirmwareVersion << "\n";
        std::cout << "Hardware version    :  0x" << std::hex << v.sensorHardwareVersion << "\n";
        std::cout << "Hardware magic      :  0x" << std::hex << v.sensorHardwareMagic << "\n";
        std::cout << "FPGA DNA            :  0x" << std::hex << v.sensorFpgaDna << "\n";
        std::cout << std::dec;
    status = channelP->getDeviceModes(deviceModes);
    if (Status_Ok != status) {
        std::cerr << "Failed to get device modes: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    operatingMode = getOperatingMode(deviceModes);
 
    //
    // Change framerate
 
    {
        image::Config cfg;
 
        status = channelP->getImageConfig(cfg);
        if (Status_Ok != status) {
                        std::cerr << "Failed to get image config: " << Channel::statusString(status) << std::endl;
            goto clean_out;
        } else {
 
            std::cout << "Setting resolution to: " << operatingMode.width << "x" <<
                                                      operatingMode.height << "x" <<
                                                      operatingMode.disparities << std::endl;
 
            cfg.setResolution(operatingMode.width, operatingMode.height);
            cfg.setDisparities(operatingMode.disparities);
            cfg.setFps(30.0);
 
            status = channelP->setImageConfig(cfg);
            if (Status_Ok != status) {
                                std::cerr << "Failed to configure sensor: " << Channel::statusString(status) << std::endl;
                goto clean_out;
            }
        }
    }
 
    //
    // Change MTU
    if (mtu >= 1500)
        status = channelP->setMtu(mtu);
    else
        status = channelP->setBestMtu();
    if (Status_Ok != status) {
                std::cerr << "Failed to set MTU to " << mtu << ": " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    //
    // Change trigger source
 
    status = channelP->setTriggerSource(Trigger_Internal);
    if (Status_Ok != status) {
                std::cerr << "Failed to set trigger source: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    //
    // Get image Calibration
 
    status = channelP->getImageCalibration(calibration);
    if (Status_Ok != status) {
                std::cerr << "Failed to get image calibration: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    //
    // Get device info
 
    status = channelP->getDeviceInfo(info);
    if (Status_Ok != status) {
                std::cerr << "Failed to get device info: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    userData.channelP = channelP;
    userData.version = v;
    userData.deviceInfo = info;
    userData.calibration = calibration;
 
    //
    // Add callbacks
 
    channelP->addIsolatedCallback(imageCallback, Source_All, &userData);
    channelP->addIsolatedCallback(laserCallback, channelP);
    channelP->addIsolatedCallback(ppsCallback, channelP);
 
    //
    // Start streaming
 
    status = channelP->startStreams((operatingMode.supportedDataSources & Source_Luma_Rectified_Left) |
                                    (operatingMode.supportedDataSources & Source_Lidar_Scan));
    if (Status_Ok != status) {
                std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    while(!doneG)
    {
        system::StatusMessage statusMessage;
        status = channelP->getDeviceStatus(statusMessage);
 
        if (Status_Ok == status) {
 
            std::cout << "Uptime: " << statusMessage.uptime << ", " <<
            "SystemOk: " << statusMessage.systemOk << ", " <<
            "FPGA Temp: " << statusMessage.fpgaTemperature << ", " <<
            "Left Imager Temp: " << statusMessage.leftImagerTemperature << ", " <<
            "Right Imager Temp: " << statusMessage.rightImagerTemperature << ", " <<
            "Input Voltage: " << statusMessage.inputVoltage << ", " <<
            "Input Current: " << statusMessage.inputCurrent << ", " <<
            "FPGA Power: " << statusMessage.fpgaPower << ", " <<
            "Logic Power: " << statusMessage.logicPower << ", " <<
            "Imager Power: " << statusMessage.imagerPower << std::endl;
 
        }
 
 
        usleep(1000000);
    }
 
    status = channelP->stopStreams(Source_All);
    if (Status_Ok != status) {
                std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl;
    }
 
clean_out:
 
    Channel::Destroy(channelP);
    return 0;
}