format7stereo.cpp
Go to the documentation of this file.
00001 /* $Id: format7.cpp 35691 2011-02-02 04:28:58Z joq $ */
00002 
00003 /*********************************************************************
00004 * Software License Agreement (BSD License)
00005 *
00006 *  Copyright (c) 2010 Ken Tossell, Jack O'Quin
00007 *  All rights reserved.
00008 *
00009 *  Redistribution and use in source and binary forms, with or without
00010 *  modification, are permitted provided that the following conditions
00011 *  are met:
00012 *
00013 *   * Redistributions of source code must retain the above copyright
00014 *     notice, this list of conditions and the following disclaimer.
00015 *   * Redistributions in binary form must reproduce the above
00016 *     copyright notice, this list of conditions and the following
00017 *     disclaimer in the documentation and/or other materials provided
00018 *     with the distribution.
00019 *   * Neither the name of the author nor other contributors may be
00020 *     used to endorse or promote products derived from this software
00021 *     without specific prior written permission.
00022 *
00023 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 *  POSSIBILITY OF SUCH DAMAGE.
00035 *********************************************************************/
00036 
00043 #include <stdint.h>
00044 #include "yuv.h"
00045 #include <sensor_msgs/image_encodings.h>
00046 #include "format7.h"
00047 #include "modes.h"
00048 
00049 
00058 bool Format7::start(dc1394camera_t *camera,
00059                     dc1394video_mode_t mode,
00060                     Config &newconfig)
00061 {
00062   active_ = false;
00063 
00064   // copy Format7 parameters for updateCameraInfo()
00065   binning_x_ = newconfig.binning_x;
00066   binning_y_ = newconfig.binning_y;
00067   roi_.x_offset = newconfig.x_offset;
00068   roi_.y_offset = newconfig.y_offset;
00069   roi_.width = newconfig.roi_width;
00070   roi_.height = newconfig.roi_height;
00071 
00072   uint32_t packet_size = newconfig.format7_packet_size;
00073 
00074 /*******************************************************************************
00075  * Commented by JOAN PAU to allow debayering on format7 modes
00076   // Built-in libdc1394 Bayer decoding (now deprecated) is not
00077   // supported at all in Format7 modes.
00078   if (newconfig.bayer_method != "")
00079     {
00080       ROS_WARN_STREAM("Bayer method [" << newconfig.bayer_method
00081                       << "] not supported for Format7 modes."
00082                       << "  Using image_proc instead.");
00083       newconfig.bayer_method = "";
00084     }
00085  ******************************************************************************/
00086 
00087   // scan all Format7 modes to determine the full-sensor image size,
00088   // from which we will calculate the binning values
00089   uint32_t sensor_width = 0, sensor_height = 0;
00090 
00091   for (int scan_mode = DC1394_VIDEO_MODE_FORMAT7_MIN;
00092        scan_mode <= DC1394_VIDEO_MODE_FORMAT7_MAX;
00093        ++scan_mode)
00094     {
00095       uint32_t scan_width, scan_height;
00096 
00097       // TODO: only scan modes supported by this device
00098       if (dc1394_format7_get_max_image_size(camera,
00099                                             (dc1394video_mode_t) scan_mode,
00100                                             &scan_width, &scan_height)
00101           != DC1394_SUCCESS)
00102         continue;
00103 
00104       if (scan_width > sensor_width)
00105         sensor_width = scan_width;
00106 
00107       if (scan_height > sensor_height)
00108         sensor_height = scan_height;
00109     }
00110 
00111   if (DC1394_SUCCESS != dc1394_format7_get_max_image_size(camera, mode,
00112                                                       &maxWidth_,
00113                                                       &maxHeight_))
00114     {
00115       ROS_ERROR("Could not get max image size");
00116       return false;
00117     }
00118 
00119   if (newconfig.binning_x == 0 || newconfig.binning_y == 0)
00120     {
00121       binning_x_ = sensor_width / maxWidth_;
00122       binning_y_ = sensor_height / maxHeight_;
00123     }
00124   else
00125     {
00126       binning_x_ = newconfig.binning_x;
00127       binning_y_ = newconfig.binning_y;
00128     }
00129 
00130   maxWidth_ *= binning_x_;
00131   maxHeight_ *= binning_y_;
00132 
00133   if ((roi_.x_offset | roi_.y_offset | roi_.width | roi_.height) == 0)
00134     {
00135       roi_.width = maxWidth_;
00136       roi_.height = maxHeight_;
00137     }
00138 
00139   uint32_t unit_w, unit_h, unit_x, unit_y;
00140 
00141   if (DC1394_SUCCESS != dc1394_format7_get_unit_size(camera, mode,
00142                                                      &unit_w, &unit_h))
00143     {
00144       ROS_ERROR("Could not get ROI size units");
00145       return false;
00146     }
00147 
00148   unit_w *= binning_x_;
00149   unit_h *= binning_y_;
00150 
00151   if (DC1394_SUCCESS != dc1394_format7_get_unit_position(camera, mode,
00152                                                          &unit_x, &unit_y))
00153     {
00154       ROS_ERROR("Could not get ROI position units");
00155       return false;
00156     }
00157 
00158   // some devices return zeros for the position units
00159   if (unit_x == 0) unit_x = 1;
00160   if (unit_y == 0) unit_y = 1;
00161 
00162   unit_x *= binning_x_;
00163   unit_y *= binning_y_;
00164 
00165   ROS_INFO_STREAM("Format7 unit size: ("
00166                   << unit_w << "x" << unit_h
00167                   << "), position: ("
00168                   << unit_x << "x" << unit_y
00169                   << ")");
00170   ROS_INFO_STREAM("Format7 region size: ("
00171                   << roi_.width << "x" << roi_.height
00172                   << "), offset: ("
00173                   << roi_.x_offset << ", " << roi_.y_offset
00174                   << ")");
00175 
00176   /* Reset ROI position to (0,0). If it was previously (x,y) and
00177    * the requested ROI size (w,h) results in (x,y) + (w,h) >
00178    * (max_w,max_h), we'll be unable to set up some valid ROIs
00179    */
00180   dc1394_format7_set_image_position(camera, mode, 0, 0);
00181 
00182   if ((roi_.width % unit_w) || (roi_.height % unit_h))
00183     {
00185       ROS_ERROR("Requested image size invalid; (w,h) must be"
00186                 " a multiple of (%d, %d)", unit_w, unit_h);
00187       return false;
00188     }
00189 
00190   uint32_t binned_width = roi_.width / binning_x_;
00191   uint32_t binned_height = roi_.height / binning_y_;
00192 
00193   uint32_t binned_x_offset = roi_.x_offset / binning_x_;
00194   uint32_t binned_y_offset = roi_.y_offset / binning_y_;
00195 
00196   if (DC1394_SUCCESS != dc1394_format7_set_image_size(camera, mode,
00197                                                       binned_width,
00198                                                       binned_height))
00199     {
00200       ROS_ERROR("Could not set size of ROI");
00201       return false;
00202     }
00203 
00204   if ((roi_.x_offset % unit_x) || (roi_.y_offset % unit_y))
00205     {
00206       ROS_ERROR("Requested image position invalid; (x,y) must"
00207                 " be a multiple of (%d, %d)", unit_x, unit_y);
00208       return false;
00209     }
00210 
00211   if (DC1394_SUCCESS != dc1394_format7_set_image_position(camera, mode,
00212                                                           binned_x_offset,
00213                                                           binned_y_offset))
00214     {
00215       ROS_ERROR("Could not set position of ROI");
00216       return false;
00217     }
00218 
00219   // Try to set requested color coding. Use current camera value if
00220   // requested coding is not supported by the camera.
00221   coding_ = Modes::getColorCoding(camera, mode,
00222                                   newconfig.format7_color_coding);
00223 
00224   if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera, mode,
00225                                                         coding_))
00226     {
00227       ROS_ERROR("Could not set color coding");
00228       return false;
00229     }
00230       
00231   uint32_t rec_packet_size;
00232 
00233   if (DC1394_SUCCESS
00234       != dc1394_format7_get_recommended_packet_size(camera, mode,
00235                                                     &rec_packet_size))
00236     {
00237       ROS_ERROR("Could not get default packet size");
00238       return false;
00239     }
00240 
00241   if (0 == packet_size)
00242     packet_size = rec_packet_size;
00243 
00244   uint32_t unit_bytes, max_bytes;
00245 
00246   if (DC1394_SUCCESS
00247       != dc1394_format7_get_packet_parameters(camera, mode,
00248                                               &unit_bytes, &max_bytes))
00249     {
00250       ROS_ERROR("Could not determine maximum and increment for packet size");
00251       return false;
00252     }
00253 
00254   if (packet_size % unit_bytes
00255       || (max_bytes > 0 && packet_size > max_bytes))
00256     {
00257       ROS_ERROR("Invalid packet size: %d. Must be a "
00258                 "multiple of %d, at most %d [%d]",
00259                 packet_size, unit_bytes, max_bytes, rec_packet_size);
00260       return false;
00261     }
00262 
00263   if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera, mode,
00264                                                        packet_size))
00265     {
00266       ROS_ERROR("Could not set packet size");
00267       return false;
00268     }
00269 
00270   if (coding_ == DC1394_COLOR_CODING_RAW8
00271       || coding_ == DC1394_COLOR_CODING_RAW16)
00272     {
00273       if (DC1394_SUCCESS != dc1394_format7_get_color_filter(camera, mode,
00274                                                             &BayerPattern_))
00275         {
00276           ROS_ERROR("Could not determine color pattern");
00277           return false;
00278         }
00279     }
00280 
00281   active_ = true;                       // Format7 mode is active
00282 
00283   return true;
00284 }
00285 
00287 void Format7::stop(void)
00288 {
00289   active_ = false;
00290 }
00291 
00292 extern std::string bayer_string(dc1394color_filter_t pattern, unsigned int bits);
00293 
00295 void Format7::unpackData(sensor_msgs::Image &image, uint8_t *capture_buffer)
00296 {
00297   int image_size;  
00298   switch (coding_)
00299     {
00300     case DC1394_COLOR_CODING_MONO8:
00301       image.step = image.width;
00302       image_size = image.height * image.step;
00303       image.encoding = sensor_msgs::image_encodings::MONO8;
00304       image.is_bigendian = false;
00305       image.data.resize(image_size);
00306       memcpy(&image.data[0], capture_buffer, image_size);
00307       break;
00308     case DC1394_COLOR_CODING_YUV411:
00309       image.step = image.width * 3;
00310       image_size = image.height * image.step;
00311       image.encoding = sensor_msgs::image_encodings::RGB8;
00312       image.data.resize(image_size);
00313       yuv::uyyvyy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00314                       reinterpret_cast<unsigned char *> (&image.data[0]),
00315                       image.width * image.height);
00316       break;
00317     case DC1394_COLOR_CODING_YUV422:
00318       image.step = image.width * 3;
00319       image_size = image.height * image.step;
00320       image.encoding = sensor_msgs::image_encodings::RGB8;
00321       image.data.resize(image_size);
00322       yuv::uyvy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00323                     reinterpret_cast<unsigned char *> (&image.data[0]),
00324                     image.width * image.height);
00325       break;
00326     case DC1394_COLOR_CODING_YUV444:
00327       image.step=image.width * 3;
00328       image_size = image.height * image.step;
00329       image.encoding = sensor_msgs::image_encodings::RGB8;
00330       image.data.resize(image_size);
00331       yuv::uyv2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00332                    reinterpret_cast<unsigned char *> (&image.data[0]),
00333                    image.width * image.height);
00334       break;
00335     case DC1394_COLOR_CODING_RGB8:
00336       image.step=image.width*3;
00337       image_size = image.height*image.step;
00338       image.encoding = sensor_msgs::image_encodings::RGB8;
00339       image.data.resize(image_size);
00340       memcpy(&image.data[0], capture_buffer, image_size);
00341       break;
00342     case DC1394_COLOR_CODING_MONO16:
00343       image.step=image.width*2;
00344       image_size = image.height*image.step;
00345       image.encoding = sensor_msgs::image_encodings::MONO16;
00346       image.is_bigendian = true;
00347       image.data.resize(image_size);
00348       memcpy(&image.data[0], capture_buffer, image_size);
00349       break;
00350     case DC1394_COLOR_CODING_RGB16:
00351       image.step = image.width * 6;
00352       image_size = image.height * image.step;
00353       image.encoding = sensor_msgs::image_encodings::TYPE_16UC3;
00354       image.is_bigendian = true;
00355       image.data.resize(image_size);
00356       memcpy(&image.data[0], capture_buffer, image_size);
00357       break;
00358     case DC1394_COLOR_CODING_MONO16S:
00359       image.step = image.width * 2;
00360       image_size = image.height * image.step;
00361       image.encoding = sensor_msgs::image_encodings::TYPE_16SC1;
00362       image.is_bigendian = true;
00363       image.data.resize(image_size);
00364       memcpy(&image.data[0], capture_buffer, image_size);
00365       break;
00366     case DC1394_COLOR_CODING_RGB16S:
00367       image.step = image.width * 6;
00368       image_size = image.height * image.step;
00369       image.encoding = sensor_msgs::image_encodings::TYPE_16SC3;
00370       image.is_bigendian = true;
00371       image.data.resize(image_size);
00372       memcpy(&image.data[0], capture_buffer, image_size);
00373       break;
00374     case DC1394_COLOR_CODING_RAW8:
00375       image.step = image.width;
00376       image_size = image.height * image.step;
00377       image.encoding = bayer_string(BayerPattern_, 8);
00378       image.data.resize(image_size);
00379       memcpy(&image.data[0], capture_buffer, image_size);
00380       break;
00381     case DC1394_COLOR_CODING_RAW16:
00382       image.step = image.width * 2;
00383       image_size = image.height * image.step;
00384       image.encoding = bayer_string(BayerPattern_, 16);
00385       image.is_bigendian = true;
00386       image.data.resize(image_size);
00387       memcpy(&image.data[0], capture_buffer, image_size);
00388       break;
00389     default:
00390       ROS_ERROR_STREAM("Driver bug: unknown Format7 color coding:"
00391                        << coding_);
00392       ROS_BREAK();
00393     }
00394 }
00395 
00406 bool Format7::checkCameraInfo(const sensor_msgs::CameraInfo &cinfo)
00407 {
00408   // see if the (full) image size matches the calibration
00409   if (cinfo.width == maxWidth_ && cinfo.height == maxHeight_)
00410     {
00411       return true;
00412     }
00413   // or if the ROI size matches the calibration
00414   else if (cinfo.width == roi_.width && cinfo.height == roi_.height)
00415     {
00416       return true;
00417     }
00418   else
00419     {
00420       ROS_WARN_STREAM_THROTTLE(30, "Calibrated image size ("
00421                                << cinfo.width << "x" << cinfo.height
00422                                << ") matches neither full Format7 size ("
00423                                << maxWidth_ << "x" << maxHeight_
00424                                << ") nor ROI size ("
00425                                << roi_.width << "x" << roi_.height << ")");
00426       return false;
00427     }
00428 }
00429 
00440 void Format7::setOperationalParameters(sensor_msgs::CameraInfo &cinfo)
00441 {
00442   // copy the operational data determined during start()
00443   cinfo.binning_x = binning_x_;
00444   cinfo.binning_y = binning_y_;
00445   cinfo.roi = roi_;
00446 
00447   // set do_rectify depending on current calibration parameters
00448   cinfo.roi.do_rectify = false;
00449 
00450   if (cinfo.K[0] == 0.0)
00451     return;                             // uncalibrated
00452 
00453   bool roiMatchesCalibration = (cinfo.width == roi_.width
00454                                 && cinfo.height == roi_.height);
00455 
00456   if (cinfo.width == maxWidth_ && cinfo.height == maxHeight_)
00457     {
00458       // calibration matches full image size
00459       if (!roiMatchesCalibration)
00460         {
00461           // ROI is not full image: tell image_pipeline to rectify
00462           cinfo.roi.do_rectify = true;
00463         }
00464     }
00465   else
00466     {
00467       // calibration differs from full image
00468       if (!roiMatchesCalibration)
00469         {
00470           // calibrated size is neither full image nor current ROI:
00471           //   tell image_pipeline to rectify the data.
00472           cinfo.roi.do_rectify = true;
00473         }
00474     }
00475 }


camera1394stereo
Author(s): Joan Pau Beltran
autogenerated on Sun Oct 5 2014 22:44:07