gc_pointcloud.cc
Go to the documentation of this file.
00001 /*
00002  * This file is part of the rc_genicam_api package.
00003  *
00004  * Copyright (c) 2017 Roboception GmbH
00005  * All rights reserved
00006  *
00007  * Author: Heiko Hirschmueller
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  * 1. Redistributions of source code must retain the above copyright notice,
00013  * this list of conditions and the following disclaimer.
00014  *
00015  * 2. Redistributions in binary form must reproduce the above copyright notice,
00016  * this list of conditions and the following disclaimer in the documentation
00017  * and/or other materials provided with the distribution.
00018  *
00019  * 3. Neither the name of the copyright holder nor the names of its contributors
00020  * may be used to endorse or promote products derived from this software without
00021  * specific prior written permission.
00022  *
00023  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00024  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00025  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00026  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00027  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00028  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00029  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00030  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00031  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00032  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  * POSSIBILITY OF SUCH DAMAGE.
00034  */
00035 
00036 #include <rc_genicam_api/system.h>
00037 #include <rc_genicam_api/interface.h>
00038 #include <rc_genicam_api/device.h>
00039 #include <rc_genicam_api/stream.h>
00040 #include <rc_genicam_api/buffer.h>
00041 #include <rc_genicam_api/image.h>
00042 #include <rc_genicam_api/imagelist.h>
00043 #include <rc_genicam_api/config.h>
00044 
00045 #include <rc_genicam_api/pixel_formats.h>
00046 
00047 #include <Base/GCException.h>
00048 
00049 #include <iostream>
00050 #include <fstream>
00051 #include <iomanip>
00052 #include <cmath>
00053 #include <algorithm>
00054 
00055 #ifdef _WIN32
00056 #undef min
00057 #undef max
00058 #endif
00059 
00060 namespace
00061 {
00062 
00063 /*
00064   Print help text on standard output.
00065 */
00066 
00067 void printHelp(const char *prgname)
00068 {
00069   // show help
00070 
00071   std::cout << prgname << " -h | [-o <output-filename>] [<interface-id>:]<device-id>" << std::endl;
00072   std::cout << std::endl;
00073   std::cout << "Gets the first synchronized image set of the Roboception rc_visard, consisting of left, disparity, confidence and error image, creates a point cloud and stores it in ply ascii format." << std::endl;
00074   std::cout << std::endl;
00075   std::cout << "Options:" << std::endl;
00076   std::cout << "-h        Prints help information and exits" << std::endl;
00077   std::cout << "-o <file> Set name of output file (default is 'rc_visard_<timestamp>.ply')" << std::endl;
00078   std::cout << std::endl;
00079   std::cout << "Parameters:" << std::endl;
00080   std::cout << "<interface-id> Optional GenICam ID of interface for connecting to the device" << std::endl;
00081   std::cout << "<device-id>    GenICam device ID, serial number or user defined name of device" << std::endl;
00082 }
00083 
00084 /*
00085   Get the i-th 16 bit value.
00086 
00087   @param p         Pointer to first byte of array of 16 bit values.
00088   @param bigendian True if values are given as big endian. Otherwise, litte
00089                    endian is assumed.
00090   @param i         Index of 16 bit inside the given array.
00091 */
00092 
00093 inline uint16_t getUint16(const uint8_t *p, bool bigendian, size_t i)
00094 {
00095   uint16_t ret;
00096 
00097   if (bigendian)
00098   {
00099     size_t j=i<<1;
00100     ret=static_cast<uint16_t>(((p[j]<<8)|p[j+1]));
00101   }
00102   else
00103   {
00104     size_t j=i<<1;
00105     ret=static_cast<uint16_t>(((p[j+1]<<8)|p[j]));
00106   }
00107 
00108   return ret;
00109 }
00110 
00111 /*
00112   Computes a point cloud from the given synchronized left and disparity image
00113   pair and stores it in ply ascii format.
00114 
00115   @param name  Name of output file. If empty, a standard file name with
00116                timestamp is used.
00117   @param f     Focal length factor (to be multiplicated with image width).
00118   @param t     Baseline in m.
00119   @param scale Disparity scale factor.
00120   @param left  Left camera image. The image must have format Mono8 or
00121                YCbCr411_8.
00122   @param disp  Corresponding disparity image, possibly downscaled by an integer
00123                factor. The image must be in format Coord3D_C16.
00124   @param conf  Optional corresponding confidence image in the same size as
00125                disp. The image must be in format Confidence8.
00126   @param error Optional corresponding error image in the same size as disp. The
00127                image must be in format Error8.
00128 */
00129 
00130 void storePointCloud(std::string name, double f, double t, double scale,
00131                      std::shared_ptr<const rcg::Image> left,
00132                      std::shared_ptr<const rcg::Image> disp,
00133                      std::shared_ptr<const rcg::Image> conf=0,
00134                      std::shared_ptr<const rcg::Image> error=0)
00135 {
00136   // get size and scale factor between left image and disparity image
00137 
00138   size_t width=disp->getWidth();
00139   size_t height=disp->getHeight();
00140   bool bigendian=disp->isBigEndian();
00141   size_t ds=(left->getWidth()+disp->getWidth()-1)/disp->getWidth();
00142 
00143   // convert focal length factor into focal length in (disparity) pixels
00144 
00145   f*=width;
00146 
00147   // get pointer to disparity data and size of row in bytes
00148 
00149   const uint8_t *dps=disp->getPixels();
00150   size_t dstep=disp->getWidth()*sizeof(uint16_t)+disp->getXPadding();
00151 
00152   // count number of valid disparities and store vertice index in a temporary
00153   // index image
00154 
00155   size_t vi=0;
00156   const uint32_t vinvalid=0xffffffff;
00157   std::vector<uint32_t> vindex(width*height);
00158 
00159   uint32_t n=0;
00160   for (size_t k=0; k<height; k++)
00161   {
00162     int j=0;
00163     for (size_t i=0; i<width; i++)
00164     {
00165       vindex[vi]=vinvalid;
00166       if ((dps[j]|dps[j+1]) != 0) vindex[vi]=n++;
00167 
00168       j+=2;
00169       vi++;
00170     }
00171 
00172     dps+=dstep;
00173   }
00174 
00175   dps=disp->getPixels();
00176 
00177   // count number of triangles
00178 
00179   const uint16_t vstep=static_cast<uint16_t>(std::ceil(2/scale));
00180 
00181   int tn=0;
00182   for (size_t k=1; k<height; k++)
00183   {
00184     for (size_t i=1; i<width; i++)
00185     {
00186       uint16_t v[4];
00187       v[0]=getUint16(dps, bigendian, i-1);
00188       v[1]=getUint16(dps, bigendian, i);
00189       v[2]=getUint16(dps+dstep, bigendian, i-1);
00190       v[3]=getUint16(dps+dstep, bigendian, i);
00191 
00192       uint16_t vmin=65535;
00193       uint16_t vmax=0;
00194       int valid=0;
00195 
00196       for (int jj=0; jj<4; jj++)
00197       {
00198         if (v[jj])
00199         {
00200           vmin=std::min(vmin, v[jj]);
00201           vmax=std::max(vmax, v[jj]);
00202           valid++;
00203         }
00204       }
00205 
00206       if (valid >= 3 && vmax-vmin <= vstep)
00207       {
00208         tn+=valid-2;
00209       }
00210     }
00211 
00212     dps+=dstep;
00213   }
00214 
00215   dps=disp->getPixels();
00216 
00217   // get pointer to optional confidence and error data and size of row in bytes
00218 
00219   const uint8_t *cps=0, *eps=0;
00220   size_t cstep=0, estep=0;
00221 
00222   if (conf)
00223   {
00224     cps=conf->getPixels();
00225     cstep=conf->getWidth()*sizeof(uint8_t)+conf->getXPadding();
00226   }
00227 
00228   if (error)
00229   {
00230     eps=error->getPixels();
00231     estep=error->getWidth()*sizeof(uint8_t)+error->getXPadding();
00232   }
00233 
00234   // open output file and write ASCII PLY header
00235 
00236   if (name.size() == 0)
00237   {
00238     std::ostringstream os;
00239     double timestamp=left->getTimestampNS()/1000000000.0;
00240     os << "rc_visard_" << std::setprecision(16) << timestamp << ".ply";
00241     name=os.str();
00242   }
00243 
00244   std::ofstream out(name);
00245 
00246   out << "ply" << std::endl;
00247   out << "format ascii 1.0" << std::endl;
00248   out << "comment Created with gc_pointcloud from Roboception GmbH" << std::endl;
00249   out << "comment Camera [1 0 0; 0 1 0; 0 0 1] [0 0 0]" << std::endl;
00250   out << "element vertex " << n << std::endl;
00251   out << "property float32 x" << std::endl;
00252   out << "property float32 y" << std::endl;
00253   out << "property float32 z" << std::endl;
00254   out << "property float32 scan_size" << std::endl; // i.e. size of 3D point
00255 
00256   if (cps != 0)
00257   {
00258     out << "property float32 scan_conf" << std::endl; // optional confidence
00259   }
00260 
00261   if (eps != 0)
00262   {
00263     out << "property float32 scan_error" << std::endl; // optional error in 3D along line of sight
00264   }
00265 
00266   out << "property uint8 diffuse_red" << std::endl;
00267   out << "property uint8 diffuse_green" << std::endl;
00268   out << "property uint8 diffuse_blue" << std::endl;
00269   out << "element face " << tn << std::endl;
00270   out << "property list uint8 uint32 vertex_indices" << std::endl;
00271   out << "end_header" << std::endl;
00272 
00273   // create colored point cloud
00274 
00275   for (size_t k=0; k<height; k++)
00276   {
00277     for (size_t i=0; i<width; i++)
00278     {
00279       // convert disparity from fixed comma 16 bit integer into float value
00280 
00281       double d=scale*getUint16(dps, bigendian, i);
00282 
00283       // if disparity is valid and color can be obtained
00284 
00285       if (d)
00286       {
00287         // reconstruct 3D point from disparity value
00288 
00289         double x=(i+0.5-0.5*width)*t/d;
00290         double y=(k+0.5-0.5*height)*t/d;
00291         double z=f*t/d;
00292 
00293         // compute size of reconstructed point
00294 
00295         double x2=(i-0.5*width)*t/d;
00296         double size=2*1.4*std::abs(x2-x);
00297 
00298         // get corresponding color value
00299 
00300         uint8_t rgb[3];
00301         rcg::getColor(rgb, left, static_cast<uint32_t>(ds), static_cast<uint32_t>(i),
00302                       static_cast<uint32_t>(k));
00303 
00304         // store colored point, optionally with confidence and error
00305 
00306         out << x << " " << y << " " << z << " " << size << " ";
00307 
00308         if (cps != 0)
00309         {
00310           out << cps[i]/255.0 << " ";
00311         }
00312 
00313         if (eps != 0)
00314         {
00315           out << eps[i]*scale*f*t/(d*d) << " ";
00316         }
00317 
00318         out << static_cast<int>(rgb[0]) << " ";
00319         out << static_cast<int>(rgb[1]) << " ";
00320         out << static_cast<int>(rgb[2]) << std::endl;
00321       }
00322     }
00323 
00324     dps+=dstep;
00325     cps+=cstep;
00326     eps+=estep;
00327   }
00328 
00329   dps=disp->getPixels();
00330 
00331   // create triangles
00332 
00333   uint32_t *ips=vindex.data();
00334   for (size_t k=1; k<height; k++)
00335   {
00336     for (size_t i=1; i<width; i++)
00337     {
00338       uint16_t v[4];
00339       v[0]=getUint16(dps, bigendian, i-1);
00340       v[1]=getUint16(dps, bigendian, i);
00341       v[2]=getUint16(dps+dstep, bigendian, i-1);
00342       v[3]=getUint16(dps+dstep, bigendian, i);
00343 
00344       uint16_t vmin=65535;
00345       uint16_t vmax=0;
00346       int valid=0;
00347 
00348       for (int jj=0; jj<4; jj++)
00349       {
00350         if (v[jj])
00351         {
00352           vmin=std::min(vmin, v[jj]);
00353           vmax=std::max(vmax, v[jj]);
00354           valid++;
00355         }
00356       }
00357 
00358       if (valid >= 3 && vmax-vmin <= vstep)
00359       {
00360         int j=0;
00361         uint32_t fc[4];
00362 
00363         if (ips[i-1] != vinvalid)
00364         {
00365           fc[j++]=ips[i-1];
00366         }
00367 
00368         if (ips[width+i-1] != vinvalid)
00369         {
00370           fc[j++]=ips[width+i-1];
00371         }
00372 
00373         if (ips[width+i] != vinvalid)
00374         {
00375           fc[j++]=ips[width+i];
00376         }
00377 
00378         if (ips[i] != vinvalid)
00379         {
00380           fc[j++]=ips[i];
00381         }
00382 
00383         out << "3 " << fc[0] << ' ' << fc[1] << ' ' << fc[2] << std::endl;
00384 
00385         if (j == 4)
00386         {
00387           out << "3 " << fc[2] << ' ' << fc[3] << ' ' << fc[0] << std::endl;
00388         }
00389       }
00390     }
00391 
00392     ips+=width;
00393     dps+=dstep;
00394   }
00395 
00396   out.close();
00397 }
00398 
00399 }
00400 
00401 int main(int argc, char *argv[])
00402 {
00403   int ret=0;
00404 
00405   try
00406   {
00407     // optional parameters
00408 
00409     std::string name="";
00410 
00411     int i=1;
00412 
00413     while (i+1 < argc)
00414     {
00415       if (std::string(argv[i]) == "-o")
00416       {
00417         i++;
00418         name=argv[i++];
00419       }
00420       else
00421       {
00422         std::cout << "Unknown parameter: " << argv[i] << std::endl;
00423         std::cout << std::endl;
00424 
00425         printHelp(argv[0]);
00426 
00427         return 1;
00428       }
00429     }
00430 
00431     if (i >= argc || std::string(argv[i]) == "-h")
00432     {
00433       printHelp(argv[0]);
00434       return 1;
00435     }
00436 
00437     // find specific device accross all systems and interfaces and open it
00438 
00439     std::shared_ptr<rcg::Device> dev=rcg::getDevice(argv[i]);
00440 
00441     if (dev)
00442     {
00443       dev->open(rcg::Device::CONTROL);
00444       std::shared_ptr<GenApi::CNodeMapRef> nodemap=dev->getRemoteNodeMap();
00445 
00446       // get chunk adapter (this switches chunk mode on if possible and
00447       // returns a null pointer if this is not possible)
00448 
00449       std::shared_ptr<GenApi::CChunkAdapter> chunkadapter=rcg::getChunkAdapter(nodemap, dev->getTLType());
00450 
00451       // get focal length, baseline and disparity scale factor
00452 
00453       double f=rcg::getFloat(nodemap, "FocalLengthFactor", 0, 0, false);
00454       double t=rcg::getFloat(nodemap, "Baseline", 0, 0, true);
00455       double scale=rcg::getFloat(nodemap, "Scan3dCoordinateScale", 0, 0, true);
00456 
00457       // check for special exposure alternate mode of rc_visard and
00458       // corresponding filter and set tolerance accordingly
00459 
00460       // (The exposure alternate mode is typically used with a random dot
00461       // projector connected to Out1. Alternate means that Out1 is high for
00462       // every second image. The rc_visard calculates disparities from images
00463       // with Out1=High. However, if the alternate filter is set to OnlyLow,
00464       // then it is gueranteed that Out1=Low (i.e. projector off) for all
00465       // rectified images. Thus, rectified images and disparity images are
00466       // always around 40 ms appart, which must be taken into account for
00467       // synchronization.)
00468 
00469       uint64_t tol=0;
00470 
00471       try
00472       {
00473         rcg::setEnum(nodemap, "LineSelector", "Out1", true);
00474         std::string linesource=rcg::getEnum(nodemap, "LineSource", true);
00475         std::string filter=rcg::getEnum(nodemap, "AcquisitionAlternateFilter", true);
00476 
00477         if (linesource == "ExposureAlternateActive" && filter == "OnlyLow")
00478         {
00479           tol=50*1000*1000; // set tolerance to 50 ms
00480         }
00481       }
00482       catch (const std::exception &)
00483       {
00484         // ignore possible errors
00485       }
00486 
00487       // sanity check of some parameters
00488 
00489       rcg::checkFeature(nodemap, "Scan3dOutputMode", "DisparityC");
00490       rcg::checkFeature(nodemap, "Scan3dCoordinateOffset", "0");
00491       rcg::checkFeature(nodemap, "Scan3dInvalidDataFlag", "1");
00492       rcg::checkFeature(nodemap, "Scan3dInvalidDataValue", "0");
00493 
00494       // set to color format if available
00495 
00496       rcg::setEnum(nodemap, "PixelFormat", "YCbCr411_8", false);
00497 
00498       // enable left image, disparity, confidence and error image and disable
00499       // all others
00500 
00501       {
00502         std::vector<std::string> component;
00503 
00504         rcg::getEnum(nodemap, "ComponentSelector", component, true);
00505 
00506         for (size_t k=0; k<component.size(); k++)
00507         {
00508           rcg::setEnum(nodemap, "ComponentSelector", component[k].c_str(), true);
00509 
00510           bool enable=(component[k] == "Intensity" || component[k] == "Disparity" ||
00511                        component[k] == "Confidence" || component[k] == "Error");
00512           rcg::setBoolean(nodemap, "ComponentEnable", enable, true);
00513         }
00514       }
00515 
00516       // try getting synchronized data (which only has an effect if the device
00517       // and GenTL producer support multipart)
00518 
00519       rcg::setString(nodemap, "AcquisitionMultiPartMode", "SynchronizedComponents");
00520 
00521       // open image stream
00522 
00523       std::vector<std::shared_ptr<rcg::Stream> > stream=dev->getStreams();
00524 
00525       if (stream.size() > 0)
00526       {
00527         // opening first stream
00528 
00529         stream[0]->open();
00530         stream[0]->startStreaming();
00531 
00532         // prepare buffers for time synchronization of images (buffer at most
00533         // 25 images in each list)
00534 
00535         rcg::ImageList left_list(50);
00536         rcg::ImageList disp_list(25);
00537         rcg::ImageList conf_list(25);
00538         rcg::ImageList error_list(25);
00539 
00540         bool run=true;
00541         int async=0, maxasync=50; // maximum number of asynchroneous images before giving up
00542 
00543         while (run && async < maxasync)
00544         {
00545           async++;
00546 
00547           // grab next image with timeout
00548 
00549           const rcg::Buffer *buffer=stream[0]->grab(5000);
00550           if (buffer != 0)
00551           {
00552             // check for a complete image in the buffer
00553 
00554             if (!buffer->getIsIncomplete())
00555             {
00556               // attach buffer to nodemap for accessing chunk data if possible
00557 
00558               if (chunkadapter)
00559               {
00560                 chunkadapter->AttachBuffer(
00561                   reinterpret_cast<std::uint8_t *>(buffer->getGlobalBase()),
00562                                                    static_cast<int64_t>(buffer->getSizeFilled()));
00563               }
00564 
00565               // go through all parts in case of multi-part buffer
00566 
00567               size_t partn=buffer->getNumberOfParts();
00568               for (uint32_t part=0; part<partn; part++)
00569               {
00570                 if (buffer->getImagePresent(part))
00571                 {
00572                   // store image in the corresponding list
00573 
00574                   uint64_t left_tol=0;
00575                   uint64_t disp_tol=0;
00576 
00577                   std::string component=rcg::getComponetOfPart(nodemap, buffer, part);
00578 
00579                   if (component == "Intensity")
00580                   {
00581                     left_list.add(buffer, part);
00582                     disp_tol=tol;
00583                   }
00584                   else if (component == "Disparity")
00585                   {
00586                     disp_list.add(buffer, part);
00587                     left_tol=tol;
00588                   }
00589                   else if (component == "Confidence")
00590                   {
00591                     conf_list.add(buffer, part);
00592                     left_tol=tol;
00593                   }
00594                   else if (component == "Error")
00595                   {
00596                     error_list.add(buffer, part);
00597                     left_tol=tol;
00598                   }
00599 
00600                   // get corresponding left and disparity images
00601 
00602                   uint64_t timestamp=buffer->getTimestampNS();
00603                   std::shared_ptr<const rcg::Image> left=left_list.find(timestamp, left_tol);
00604                   std::shared_ptr<const rcg::Image> disp=disp_list.find(timestamp, disp_tol);
00605 
00606                   // get confidence and error images that correspond to the
00607                   // disparity image
00608 
00609                   std::shared_ptr<const rcg::Image> conf;
00610                   std::shared_ptr<const rcg::Image> error;
00611 
00612                   if (disp)
00613                   {
00614                     conf=conf_list.find(disp->getTimestampNS());
00615                     error=error_list.find(disp->getTimestampNS());
00616                   }
00617 
00618                   if (left && disp && conf && error)
00619                   {
00620                     // compute and store point cloud from synchronized image pair
00621 
00622                     storePointCloud(name, f, t, scale, left, disp, conf, error);
00623 
00624                     // remove all images from the buffer with the current or an
00625                     // older time stamp
00626 
00627                     async=0;
00628                     left_list.removeOld(timestamp);
00629                     disp_list.removeOld(timestamp);
00630                     conf_list.removeOld(timestamp);
00631                     error_list.removeOld(timestamp);
00632 
00633                     // in this example, we exit the grabbing loop after receiving
00634                     // the first synchronized image pair
00635 
00636                     run=false;
00637                   }
00638                 }
00639               }
00640 
00641               // detach buffer from nodemap
00642 
00643               if (chunkadapter) chunkadapter->DetachBuffer();
00644             }
00645           }
00646           else
00647           {
00648             std::cerr << "Cannot grab images" << std::endl;
00649             break;
00650           }
00651         }
00652 
00653         // report if synchronization failed
00654 
00655         if (async >= maxasync && run)
00656         {
00657           std::cerr << "Cannot grab synchronized left and disparity image" << std::endl;
00658           ret=1;
00659         }
00660 
00661         // stopping and closing image stream
00662 
00663         stream[0]->stopStreaming();
00664         stream[0]->close();
00665       }
00666       else
00667       {
00668         std::cerr << "No streams available" << std::endl;
00669         ret=1;
00670       }
00671 
00672       // closing the communication to the device
00673 
00674       dev->close();
00675     }
00676     else
00677     {
00678       std::cerr << "Device '" << argv[1] << "' not found!" << std::endl;
00679       ret=1;
00680     }
00681   }
00682   catch (const std::exception &ex)
00683   {
00684     std::cerr << ex.what() << std::endl;
00685     ret=2;
00686   }
00687   catch (const GENICAM_NAMESPACE::GenericException &ex)
00688   {
00689     std::cerr << "Exception: " << ex.what() << std::endl;
00690     ret=2;
00691   }
00692   catch (...)
00693   {
00694     std::cerr << "Unknown exception!" << std::endl;
00695     ret=2;
00696   }
00697 
00698   rcg::System::clearSystems();
00699 
00700   return ret;
00701 }


rc_genicam_api
Author(s): Heiko Hirschmueller
autogenerated on Thu Jun 6 2019 18:42:47