LidarCalUtility.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 <LibMultiSense/MultiSenseChannel.hh>

using namespace crl::multisense;

namespace {  // anonymous

void usage(const char *programNameP) 
{
    fprintf(stderr, 
            "USAGE: %s -f <calibration_file> [<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 calibration (default is query)\n");
    fprintf(stderr, "\t-y                   : disable confirmation prompts\n");
    
    exit(-1);
}

bool fileExists(const std::string& name)
{
    struct stat sbuf;
    return (0 == stat(name.c_str(), &sbuf));
}

const char *laserToSpindleNameP       = "laser_T_spindle";
const char *cameraToSpindleFixedNameP = "camera_T_spindle_fixed";


std::ostream& writeLaserCal (std::ostream& stream, lidar::Calibration const& calibration)
{
    stream << "%YAML:1.0\n";
    writeMatrix (stream, laserToSpindleNameP, 4, 4, &calibration.laserToSpindle[0][0]);
    writeMatrix (stream, cameraToSpindleFixedNameP, 4, 4, &calibration.cameraToSpindleFixed[0][0]);
    return stream;
}


}; // anonymous

int main(int    argc, 
         char **argvPP)
{
    std::string        ipAddress = "10.66.171.21";
    std::string        calFile;
    bool               setCal=false;
    bool               prompt=true;

    //
    // Parse args

    int c;

    while(-1 != (c = getopt(argc, argvPP, "a:f:sy")))
        switch(c) {
        case 'a': ipAddress = std::string(optarg);    break;
        case 'f': calFile   = std::string(optarg);    break;
        case 's': setCal    = true;                   break;
        case 'y': prompt    = false;                  break;
        default: usage(*argvPP);                      break;
        }

    //
    // Verify options

    if (calFile.empty()) {
        fprintf(stderr, "Must provide a file argument\n");
        usage(*argvPP);
    }

    if (true == setCal && false == fileExists(calFile)) {
        
        fprintf(stderr, "file not found: \"%s\"\n", calFile.c_str());
        usage(*argvPP);
    }
    
    if (false == setCal && true == prompt &&
        true == fileExists(calFile)) {
        
        fprintf(stdout, 
                "\"%s\" already exists.\n\n"
                "Really overwrite this file? (y/n): ",
                calFile.c_str());
        fflush(stdout);

        int c = getchar();
        if ('Y' != c && 'y' != c) {
            fprintf(stdout, "Aborting\n");
            return 0;
        }
    }

    //
    // 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 version

    Status status;
    VersionType version;

    status = channelP->getSensorVersion(version);
    if (Status_Ok != status) {
        fprintf(stderr, "failed to query sensor version: %s\n", 
                Channel::statusString(status));
        goto clean_out;
    }
    
    //
    // Query

    if (false == setCal) {

        lidar::Calibration c;

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

        std::ofstream cvFile (calFile.c_str (), std::ios_base::out | std::ios_base::trunc);

        if (!cvFile) {
            fprintf(stderr, "failed to open '%s' for writing\n", 
                    calFile.c_str());
            goto clean_out;
        }

        writeLaserCal (cvFile, c);

        cvFile.flush ();

    } else {

        std::map<std::string, std::vector<float> > data;
        std::ifstream cvFile (calFile.c_str ());

        if (!cvFile) {
            fprintf(stderr, "failed to open '%s' for reading\n", 
                    calFile.c_str());
            goto clean_out;
        }

        parseYaml (cvFile, data);

        cvFile.close ();

        if (data[laserToSpindleNameP].size () != 4 * 4 ||
            data[cameraToSpindleFixedNameP].size () != 4 * 4) {

            fprintf(stderr, "calibration matrices incomplete in %s, "
                    "expecting two 4x4 matrices\n", calFile.c_str());
            goto clean_out;
        }

        lidar::Calibration c;

        memcpy (&c.laserToSpindle[0][0], &data[laserToSpindleNameP].front (), data[laserToSpindleNameP].size () * sizeof (float));
        memcpy (&c.cameraToSpindleFixed[0][0], &data[cameraToSpindleFixedNameP].front (), data[cameraToSpindleFixedNameP].size () * sizeof (float));
        
        status = channelP->setLidarCalibration(c);
        if (Status_Ok != status) {
            fprintf(stderr, "failed to set lidar calibration: %s\n", 
                    Channel::statusString(status));
            goto clean_out;
        }

        fprintf(stdout, "Lidar calibration successfully updated\n");
    }

clean_out:

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