ImageCalUtility.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 -e <extrinisics_file> -i <intrinsics_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 std::ostream& writeImageIntrinics (std::ostream& stream, image::Calibration const& calibration)
00084 {
00085     stream << "%YAML:1.0\n";
00086     writeMatrix (stream, "M1", 3, 3, &calibration.left.M[0][0]);
00087     writeMatrix (stream, "D1", 1, 8, &calibration.left.D[0]);
00088     writeMatrix (stream, "M2", 3, 3, &calibration.right.M[0][0]);
00089     writeMatrix (stream, "D2", 1, 8, &calibration.right.D[0]);
00090     return stream;
00091 }
00092 
00093 std::ostream& writeImageExtrinics (std::ostream& stream, image::Calibration const& calibration)
00094 {
00095     stream << "%YAML:1.0\n";
00096     writeMatrix (stream, "R1", 3, 3, &calibration.left.R[0][0]);
00097     writeMatrix (stream, "P1", 3, 4, &calibration.left.P[0][0]);
00098     writeMatrix (stream, "R2", 3, 3, &calibration.right.R[0][0]);
00099     writeMatrix (stream, "P2", 3, 4, &calibration.right.P[0][0]);
00100     return stream;
00101 }
00102 
00103 }; // anonymous
00104 
00105 int main(int    argc, 
00106          char **argvPP)
00107 {
00108     std::string ipAddress = "10.66.171.21";
00109     std::string intrinsicsFile;
00110     std::string extrinsicsFile;
00111     bool        setCal=false;
00112     bool        prompt=true;
00113 
00114     //
00115     // Parse args
00116 
00117     int c;
00118 
00119     while(-1 != (c = getopt(argc, argvPP, "a:e:i:sy")))
00120         switch(c) {
00121         case 'a': ipAddress      = std::string(optarg);    break;
00122         case 'i': intrinsicsFile = std::string(optarg);    break;
00123         case 'e': extrinsicsFile = std::string(optarg);    break;
00124         case 's': setCal         = true;                   break;
00125         case 'y': prompt         = false;                  break;
00126         default: usage(*argvPP);                           break;
00127         }
00128 
00129     //
00130     // Verify options
00131 
00132     if (intrinsicsFile.empty() || extrinsicsFile.empty()) {
00133         fprintf(stderr, "Both intrinsics and extrinsics files must be set\n");
00134         usage(*argvPP);
00135     }
00136 
00137     if (true == setCal &&
00138         (false == fileExists(intrinsicsFile) ||
00139          false == fileExists(extrinsicsFile))) {
00140         
00141         fprintf(stderr, "intrinsics or extrinsics file not found\n");
00142         usage(*argvPP);
00143     }
00144 
00145     if (false == setCal && true == prompt &&
00146         (true == fileExists(intrinsicsFile) ||
00147          true == fileExists(extrinsicsFile))) {
00148         
00149         fprintf(stdout, 
00150                 "One or both of \"%s\" and \"%s\" already exists.\n\n"
00151                 "Really overwrite these files? (y/n): ",
00152                 intrinsicsFile.c_str(),
00153                 extrinsicsFile.c_str());
00154         fflush(stdout);
00155 
00156         int c = getchar();
00157         if ('Y' != c && 'y' != c) {
00158             fprintf(stdout, "Aborting\n");
00159             return 0;
00160         }
00161     }       
00162 
00163     //
00164     // Initialize communications.
00165 
00166     Channel *channelP = Channel::Create(ipAddress);
00167     if (NULL == channelP) {
00168         fprintf(stderr, "Failed to establish communications with \"%s\"\n",
00169                 ipAddress.c_str());
00170         return -1;
00171     }
00172 
00173     //
00174     // Query version
00175 
00176     Status status;
00177     VersionType version;
00178 
00179     status = channelP->getSensorVersion(version);
00180     if (Status_Ok != status) {
00181         fprintf(stderr, "failed to query sensor version: %s\n", 
00182                 Channel::statusString(status));
00183         goto clean_out;
00184     }
00185     
00186     //
00187     // Query
00188 
00189     if (false == setCal) {
00190 
00191         image::Calibration c;
00192 
00193         status = channelP->getImageCalibration(c);
00194         if (Status_Ok != status) {
00195             fprintf(stderr, "failed to query image calibration: %s\n", 
00196                     Channel::statusString(status));
00197             goto clean_out;
00198         }
00199 
00200         std::ofstream inFile, exFile;
00201 
00202         inFile.open (intrinsicsFile.c_str (), std::ios_base::out | std::ios_base::trunc);
00203 
00204         if (!inFile) {
00205             fprintf(stderr, "failed to open '%s' for writing\n", 
00206                     intrinsicsFile.c_str());
00207             goto clean_out;
00208         }
00209 
00210         exFile.open (extrinsicsFile.c_str (), std::ios_base::out | std::ios_base::trunc);
00211 
00212         if (!exFile) {
00213             fprintf(stderr, "failed to open '%s' for writing\n", 
00214                     extrinsicsFile.c_str());
00215             goto clean_out;
00216         }
00217 
00218         writeImageIntrinics (inFile, c);
00219         writeImageExtrinics (exFile, c);
00220 
00221         inFile.flush ();
00222         exFile.flush ();
00223 
00224     } else {
00225 
00226         std::ifstream inFile, exFile;
00227         std::map<std::string, std::vector<float> > data;
00228 
00229         inFile.open (intrinsicsFile.c_str ());
00230         
00231         if (!inFile) {
00232             fprintf(stderr, "failed to open '%s' for reading\n", 
00233                     intrinsicsFile.c_str());
00234             goto clean_out;
00235         }
00236 
00237         parseYaml (inFile, data);
00238 
00239         inFile.close ();
00240 
00241         if (data["M1"].size () != 3 * 3 ||
00242             (data["D1"].size () != 5 && data["D1"].size () != 8) ||
00243             data["M2"].size () != 3 * 3 ||
00244             (data["D2"].size () != 5 && data["D2"].size () != 8)) {
00245             fprintf(stderr, "intrinsic matrices incomplete in %s\n",
00246                     intrinsicsFile.c_str());
00247             goto clean_out;
00248         }
00249 
00250         exFile.open (extrinsicsFile.c_str ());
00251 
00252         if (!exFile) {
00253             fprintf(stderr, "failed to open '%s' for reading\n", 
00254                     extrinsicsFile.c_str());
00255             goto clean_out;
00256         }
00257 
00258         parseYaml (exFile, data);
00259 
00260         exFile.close ();
00261 
00262         if (data["R1"].size () != 3 * 3 ||
00263             data["P1"].size () != 3 * 4 ||
00264             data["R2"].size () != 3 * 3 ||
00265             data["P2"].size () != 3 * 4) {
00266             fprintf(stderr, "extrinsic matrices incomplete in %s\n",
00267                     extrinsicsFile.c_str());
00268             goto clean_out;
00269         }
00270 
00271         image::Calibration c;
00272 
00273         memcpy (&c.left.M[0][0], &data["M1"].front (), data["M1"].size () * sizeof (float));
00274         memset (&c.left.D[0], 0, sizeof (c.left.D));
00275         memcpy (&c.left.D[0], &data["D1"].front (), data["D1"].size () * sizeof (float));
00276         memcpy (&c.left.R[0][0], &data["R1"].front (), data["R1"].size () * sizeof (float));
00277         memcpy (&c.left.P[0][0], &data["P1"].front (), data["P1"].size () * sizeof (float));
00278 
00279         memcpy (&c.right.M[0][0], &data["M2"].front (), data["M2"].size () * sizeof (float));
00280         memset (&c.right.D[0], 0, sizeof (c.right.D));
00281         memcpy (&c.right.D[0], &data["D2"].front (), data["D2"].size () * sizeof (float));
00282         memcpy (&c.right.R[0][0], &data["R2"].front (), data["R2"].size () * sizeof (float));
00283         memcpy (&c.right.P[0][0], &data["P2"].front (), data["P2"].size () * sizeof (float));
00284 
00285         status = channelP->setImageCalibration(c);
00286         if (Status_Ok != status) {
00287             fprintf(stderr, "failed to set image calibration: %s\n", 
00288                     Channel::statusString(status));
00289             goto clean_out;
00290         }
00291 
00292         fprintf(stdout, "Image calibration successfully updated\n");
00293     }
00294 
00295 clean_out:
00296 
00297     Channel::Destroy(channelP);
00298     return 0;
00299 }


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