LidarCalUtility.cc
Go to the documentation of this file.
00001 
00037 #ifdef WIN32
00038 #ifndef WIN32_LEAN_AND_MEAN
00039 #define WIN32_LEAN_AND_MEAN 1
00040 #endif
00041 
00042 #include <windows.h>
00043 #include <winsock2.h>
00044 #else
00045 #include <unistd.h>
00046 #endif
00047 
00048 #include <stdio.h>
00049 #include <stdlib.h>
00050 #include <sys/types.h>
00051 #include <sys/stat.h>
00052 #include <fstream>
00053 #include <map>
00054 #include <string.h>
00055 
00056 #include <Utilities/portability/getopt/getopt.h>
00057 #include <Utilities/shared/CalibrationYaml.hh>
00058 #include <LibMultiSense/MultiSenseChannel.hh>
00059 
00060 using namespace crl::multisense;
00061 
00062 namespace {  // anonymous
00063 
00064 void usage(const char *programNameP) 
00065 {
00066     fprintf(stderr, 
00067             "USAGE: %s -f <calibration_file> [<options>]\n", 
00068             programNameP);
00069     fprintf(stderr, "Where <options> are:\n");
00070     fprintf(stderr, "\t-a <ip_address>      : ip address (default=10.66.171.21)\n");
00071     fprintf(stderr, "\t-s                   : set the calibration (default is query)\n");
00072     fprintf(stderr, "\t-y                   : disable confirmation prompts\n");
00073     
00074     exit(-1);
00075 }
00076 
00077 bool fileExists(const std::string& name)
00078 {
00079     struct stat sbuf;
00080     return (0 == stat(name.c_str(), &sbuf));
00081 }
00082 
00083 const char *laserToSpindleNameP       = "laser_T_spindle";
00084 const char *cameraToSpindleFixedNameP = "camera_T_spindle_fixed";
00085 
00086 
00087 std::ostream& writeLaserCal (std::ostream& stream, lidar::Calibration const& calibration)
00088 {
00089     stream << "%YAML:1.0\n";
00090     writeMatrix (stream, laserToSpindleNameP, 4, 4, &calibration.laserToSpindle[0][0]);
00091     writeMatrix (stream, cameraToSpindleFixedNameP, 4, 4, &calibration.cameraToSpindleFixed[0][0]);
00092     return stream;
00093 }
00094 
00095 
00096 }; // anonymous
00097 
00098 int main(int    argc, 
00099          char **argvPP)
00100 {
00101     std::string        ipAddress = "10.66.171.21";
00102     std::string        calFile;
00103     bool               setCal=false;
00104     bool               prompt=true;
00105 
00106     //
00107     // Parse args
00108 
00109     int c;
00110 
00111     while(-1 != (c = getopt(argc, argvPP, "a:f:sy")))
00112         switch(c) {
00113         case 'a': ipAddress = std::string(optarg);    break;
00114         case 'f': calFile   = std::string(optarg);    break;
00115         case 's': setCal    = true;                   break;
00116         case 'y': prompt    = false;                  break;
00117         default: usage(*argvPP);                      break;
00118         }
00119 
00120     //
00121     // Verify options
00122 
00123     if (calFile.empty()) {
00124         fprintf(stderr, "Must provide a file argument\n");
00125         usage(*argvPP);
00126     }
00127 
00128     if (true == setCal && false == fileExists(calFile)) {
00129         
00130         fprintf(stderr, "file not found: \"%s\"\n", calFile.c_str());
00131         usage(*argvPP);
00132     }
00133     
00134     if (false == setCal && true == prompt &&
00135         true == fileExists(calFile)) {
00136         
00137         fprintf(stdout, 
00138                 "\"%s\" already exists.\n\n"
00139                 "Really overwrite this file? (y/n): ",
00140                 calFile.c_str());
00141         fflush(stdout);
00142 
00143         int c = getchar();
00144         if ('Y' != c && 'y' != c) {
00145             fprintf(stdout, "Aborting\n");
00146             return 0;
00147         }
00148     }
00149 
00150     //
00151     // Initialize communications.
00152 
00153     Channel *channelP = Channel::Create(ipAddress);
00154     if (NULL == channelP) {
00155         fprintf(stderr, "Failed to establish communications with \"%s\"\n",
00156                 ipAddress.c_str());
00157         return -1;
00158     }
00159 
00160     //
00161     // Query version
00162 
00163     Status status;
00164     VersionType version;
00165 
00166     status = channelP->getSensorVersion(version);
00167     if (Status_Ok != status) {
00168         fprintf(stderr, "failed to query sensor version: %s\n", 
00169                 Channel::statusString(status));
00170         goto clean_out;
00171     }
00172     
00173     //
00174     // Query
00175 
00176     if (false == setCal) {
00177 
00178         lidar::Calibration c;
00179 
00180         status = channelP->getLidarCalibration(c);
00181         if (Status_Ok != status) {
00182             fprintf(stderr, "failed to query lidar calibration: %s\n", 
00183                     Channel::statusString(status));
00184             goto clean_out;
00185         }
00186 
00187         std::ofstream cvFile (calFile.c_str (), std::ios_base::out | std::ios_base::trunc);
00188 
00189         if (!cvFile) {
00190             fprintf(stderr, "failed to open '%s' for writing\n", 
00191                     calFile.c_str());
00192             goto clean_out;
00193         }
00194 
00195         writeLaserCal (cvFile, c);
00196 
00197         cvFile.flush ();
00198 
00199     } else {
00200 
00201         std::map<std::string, std::vector<float> > data;
00202         std::ifstream cvFile (calFile.c_str ());
00203 
00204         if (!cvFile) {
00205             fprintf(stderr, "failed to open '%s' for reading\n", 
00206                     calFile.c_str());
00207             goto clean_out;
00208         }
00209 
00210         parseYaml (cvFile, data);
00211 
00212         cvFile.close ();
00213 
00214         if (data[laserToSpindleNameP].size () != 4 * 4 ||
00215             data[cameraToSpindleFixedNameP].size () != 4 * 4) {
00216 
00217             fprintf(stderr, "calibration matrices incomplete in %s, "
00218                     "expecting two 4x4 matrices\n", calFile.c_str());
00219             goto clean_out;
00220         }
00221 
00222         lidar::Calibration c;
00223 
00224         memcpy (&c.laserToSpindle[0][0], &data[laserToSpindleNameP].front (), data[laserToSpindleNameP].size () * sizeof (float));
00225         memcpy (&c.cameraToSpindleFixed[0][0], &data[cameraToSpindleFixedNameP].front (), data[cameraToSpindleFixedNameP].size () * sizeof (float));
00226         
00227         status = channelP->setLidarCalibration(c);
00228         if (Status_Ok != status) {
00229             fprintf(stderr, "failed to set lidar calibration: %s\n", 
00230                     Channel::statusString(status));
00231             goto clean_out;
00232         }
00233 
00234         fprintf(stdout, "Lidar calibration successfully updated\n");
00235     }
00236 
00237 clean_out:
00238 
00239     Channel::Destroy(channelP);
00240     return 0;
00241 }


multisense_lib
Author(s):
autogenerated on Thu Aug 27 2015 14:01:11