Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23607] pkg/trunk
|
From: <ge...@us...> - 2009-09-01 22:04:55
|
Revision: 23607
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23607&view=rev
Author: gerkey
Date: 2009-09-01 22:04:47 +0000 (Tue, 01 Sep 2009)
Log Message:
-----------
Rearranged header locations
Modified Paths:
--------------
pkg/trunk/deprecated/dcam/include/dcam/dcam.h
pkg/trunk/deprecated/dcam/include/imwin/im3Dwin.h
pkg/trunk/deprecated/dcam/src/libdcam/stereodcam.cpp
pkg/trunk/deprecated/dcam/src/nodes/dcam.cpp
pkg/trunk/deprecated/dcam/src/nodes/stereodcam.cpp
pkg/trunk/deprecated/dcam/src/nodes/videre_no_STOC.cpp
pkg/trunk/deprecated/dcam/src/ost/ost.cpp
pkg/trunk/deprecated/dcam/src/python/dcam.cpp
pkg/trunk/stacks/image_pipeline/CMakeLists.txt
pkg/trunk/stacks/image_pipeline/image_proc/src/nodes/image_proc.cpp
pkg/trunk/stacks/image_pipeline/image_proc/src/proc/image.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/CMakeLists.txt
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/image.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/imageproc.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/imageproc_new.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/nodes/stereo_image_proc.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/nodes/stereoproc.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/proc/findplane.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/proc/imageproc.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/proc/stereoimage.cpp
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/proc/stereolib.c
pkg/trunk/stacks/image_pipeline/stereo_image_proc/src/proc/stereolib2.cpp
Added Paths:
-----------
pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/
pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/cam_bridge.h
pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/image.h
pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereo_image_proc/
pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereo_image_proc/cam_bridge_old.h
pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereo_image_proc/stereoimage.h
pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereo_image_proc/stereolib.h
Removed Paths:
-------------
pkg/trunk/stacks/image_pipeline/image_proc/include/cam_bridge.h
pkg/trunk/stacks/image_pipeline/image_proc/include/image.h
pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/cam_bridge_old.h
pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereoimage.h
pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereolib.h
Modified: pkg/trunk/deprecated/dcam/include/dcam/dcam.h
===================================================================
--- pkg/trunk/deprecated/dcam/include/dcam/dcam.h 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/deprecated/dcam/include/dcam/dcam.h 2009-09-01 22:04:47 UTC (rev 23607)
@@ -37,7 +37,7 @@
#include <stdexcept>
#include "dc1394/dc1394.h"
-#include "stereoimage.h"
+#include "stereo_image_proc/stereoimage.h"
// Pixel raw modes
// Videre stereo:
Modified: pkg/trunk/deprecated/dcam/include/imwin/im3Dwin.h
===================================================================
--- pkg/trunk/deprecated/dcam/include/imwin/im3Dwin.h 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/deprecated/dcam/include/imwin/im3Dwin.h 2009-09-01 22:04:47 UTC (rev 23607)
@@ -63,7 +63,7 @@
#include "FL/gl.h"
#include <GL/glut.h>
#include <GL/glu.h>
-#include "stereoimage.h"
+#include "stereo_image_proc/stereoimage.h"
using namespace std;
Modified: pkg/trunk/deprecated/dcam/src/libdcam/stereodcam.cpp
===================================================================
--- pkg/trunk/deprecated/dcam/src/libdcam/stereodcam.cpp 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/deprecated/dcam/src/libdcam/stereodcam.cpp 2009-09-01 22:04:47 UTC (rev 23607)
@@ -40,7 +40,7 @@
//
#include "dcam/stereodcam.h"
-#include "stereolib.h"
+#include "stereo_image_proc/stereolib.h"
#define PRINTF(a...) printf(a)
Modified: pkg/trunk/deprecated/dcam/src/nodes/dcam.cpp
===================================================================
--- pkg/trunk/deprecated/dcam/src/nodes/dcam.cpp 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/deprecated/dcam/src/nodes/dcam.cpp 2009-09-01 22:04:47 UTC (rev 23607)
@@ -35,7 +35,7 @@
#include <cstdio>
#include "dcam/dcam.h"
-#include "cam_bridge_old.h"
+#include "stereo_image_proc/cam_bridge_old.h"
#include "ros/node.h"
#include "sensor_msgs/Image.h"
Modified: pkg/trunk/deprecated/dcam/src/nodes/stereodcam.cpp
===================================================================
--- pkg/trunk/deprecated/dcam/src/nodes/stereodcam.cpp 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/deprecated/dcam/src/nodes/stereodcam.cpp 2009-09-01 22:04:47 UTC (rev 23607)
@@ -134,7 +134,7 @@
#include "ros/node.h"
#include "stereo_msgs/RawStereo.h"
-#include "cam_bridge_old.h"
+#include "stereo_image_proc/cam_bridge_old.h"
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/update_functions.h>
Modified: pkg/trunk/deprecated/dcam/src/nodes/videre_no_STOC.cpp
===================================================================
--- pkg/trunk/deprecated/dcam/src/nodes/videre_no_STOC.cpp 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/deprecated/dcam/src/nodes/videre_no_STOC.cpp 2009-09-01 22:04:47 UTC (rev 23607)
@@ -74,7 +74,7 @@
#include "dcam/dcam.h"
#include "dcam/stereodcam.h"
-#include "cam_bridge_old.h"
+#include "stereo_image_proc/cam_bridge_old.h"
#include "ros/node.h"
Modified: pkg/trunk/deprecated/dcam/src/ost/ost.cpp
===================================================================
--- pkg/trunk/deprecated/dcam/src/ost/ost.cpp 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/deprecated/dcam/src/ost/ost.cpp 2009-09-01 22:04:47 UTC (rev 23607)
@@ -69,7 +69,7 @@
#endif
#include "stereogui.h"
#include "imwin/im3Dwin.h"
-#include "stereolib.h"
+#include "stereo_image_proc/stereolib.h"
#include "dcam/stereodcam.h"
#include <cv.h>
Modified: pkg/trunk/deprecated/dcam/src/python/dcam.cpp
===================================================================
--- pkg/trunk/deprecated/dcam/src/python/dcam.cpp 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/deprecated/dcam/src/python/dcam.cpp 2009-09-01 22:04:47 UTC (rev 23607)
@@ -15,7 +15,7 @@
#include <sys/time.h>
#endif
#include "imwin/im3Dwin.h"
-#include "stereolib.h"
+#include "stereo_image_proc/stereolib.h"
#include "dcam/stereodcam.h"
#include <cv.h>
Modified: pkg/trunk/stacks/image_pipeline/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/image_pipeline/CMakeLists.txt 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/stacks/image_pipeline/CMakeLists.txt 2009-09-01 22:04:47 UTC (rev 23607)
@@ -15,5 +15,6 @@
# properly. CMake 2.4 seems to have unpredictable scoping rules for such
# variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+list(APPEND CPACK_SOURCE_IGNORE_FILES /image_geometry)
rosbuild_make_distribution(0.1.0)
Deleted: pkg/trunk/stacks/image_pipeline/image_proc/include/cam_bridge.h
===================================================================
--- pkg/trunk/stacks/image_pipeline/image_proc/include/cam_bridge.h 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/stacks/image_pipeline/image_proc/include/cam_bridge.h 2009-09-01 22:04:47 UTC (rev 23607)
@@ -1,153 +0,0 @@
-
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef CAM_BRIDGE_H
-#define CAM_BRIDGE_H
-
-namespace cam_bridge
-{
-
- color_coding_t GetColorCoding(const sensor_msgs::Image& msg)
- {
- using namespace sensor_msgs::image_encodings;
-
- if (msg.encoding == MONO8) return COLOR_CODING_MONO8;
- if (msg.encoding == MONO16) return COLOR_CODING_MONO16;
- if (msg.encoding == BAYER_RGGB8) return COLOR_CODING_BAYER8_RGGB;
- if (msg.encoding == BAYER_BGGR8) return COLOR_CODING_BAYER8_BGGR;
- if (msg.encoding == BAYER_GBRG8) return COLOR_CODING_BAYER8_GBRG;
- if (msg.encoding == BAYER_GRBG8) return COLOR_CODING_BAYER8_GRBG;
- if (msg.encoding == RGB8) return COLOR_CODING_RGB8;
- if (msg.encoding == RGBA8) return COLOR_CODING_RGBA8;
-
- ROS_ERROR("cam_bridge: Encoding '%s' is not supported", msg.encoding.c_str());
- return COLOR_CODING_NONE;
- }
-
- std::string ColorCodingToImageEncoding(color_coding_t coding)
- {
- using namespace sensor_msgs::image_encodings;
-
- if (coding == COLOR_CODING_MONO8) return MONO8;
- if (coding == COLOR_CODING_MONO16) return MONO16;
- if (coding == COLOR_CODING_BAYER8_RGGB) return BAYER_RGGB8;
- if (coding == COLOR_CODING_BAYER8_BGGR) return BAYER_BGGR8;
- if (coding == COLOR_CODING_BAYER8_GBRG) return BAYER_GBRG8;
- if (coding == COLOR_CODING_BAYER8_GRBG) return BAYER_GRBG8;
- if (coding == COLOR_CODING_RGB8) return RGB8;
- if (coding == COLOR_CODING_RGBA8) return RGBA8;
-
- ROS_WARN("cam_bridge: Don't know image encoding string for color coding %i", coding);
- return "";
- }
-
- void extractImage(std::vector<uint8_t> data, size_t* sz, uint8_t **d)
- {
- size_t new_size = data.size();
-
- if (*sz < new_size)
- {
- MEMFREE(*d);
- *d = (uint8_t *)MEMALIGN(new_size);
- *sz = new_size;
- }
- memcpy((char*)(*d), (char*)(&data[0]), new_size);
- }
- void extractImage(std::vector<uint8_t> data, size_t* sz, int16_t **d)
- {
- size_t new_size = data.size();
-
- if (*sz < new_size)
- {
- MEMFREE(*d);
- *d = (int16_t *)MEMALIGN(new_size);
- *sz = new_size;
- }
- memcpy((char*)(*d), (char*)(&data[0]), new_size);
- }
-
-
- void RawToCamData(const sensor_msgs::Image& im_msg,
- const sensor_msgs::CameraInfo& info_msg,
- uint8_t type, cam::ImageData* im)
- {
-
- im->imRawType = COLOR_CODING_NONE;
- im->imType = COLOR_CODING_NONE;
- im->imColorType = COLOR_CODING_NONE;
- im->imRectType = COLOR_CODING_NONE;
- im->imRectColorType = COLOR_CODING_NONE;
-
- if (type == cam::IMAGE_RAW)
- {
- extractImage(im_msg.data, &im->imRawSize, &im->imRaw);
- im->imRawType = GetColorCoding(im_msg);
- }
- else if (type == cam::IMAGE)
- {
- extractImage(im_msg.data, &im->imSize, &im->im);
- im->imType = COLOR_CODING_MONO8;
- }
- else if (type == cam::IMAGE_COLOR)
- {
- extractImage(im_msg.data, &im->imColorSize, &im->imColor);
- im->imColorType = GetColorCoding(im_msg);
- }
- else if (type == cam::IMAGE_RECT)
- {
- extractImage(im_msg.data, &im->imRectSize, &im->imRect);
- im->imRectType = GetColorCoding(im_msg);
- }
- else if (type == cam::IMAGE_RECT_COLOR)
- {
- extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
- im->imRectColorType = GetColorCoding(im_msg);
- }
-
- // @todo: this OK when right image empty (disparity image requested instead)?
- im->imHeight = im_msg.height;
- im->imWidth = im_msg.width;
-
- // @todo: possible to NOT have rectification?
- memcpy((char*)(im->D), (char*)(&info_msg.D[0]), 5*sizeof(double));
- memcpy((char*)(im->K), (char*)(&info_msg.K[0]), 9*sizeof(double));
- memcpy((char*)(im->R), (char*)(&info_msg.R[0]), 9*sizeof(double));
- memcpy((char*)(im->P), (char*)(&info_msg.P[0]), 12*sizeof(double));
- im->hasRectification = true;
- }
-}
-
-
-#endif // CAM_BRIDGE_H
Deleted: pkg/trunk/stacks/image_pipeline/image_proc/include/image.h
===================================================================
--- pkg/trunk/stacks/image_pipeline/image_proc/include/image.h 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/stacks/image_pipeline/image_proc/include/image.h 2009-09-01 22:04:47 UTC (rev 23607)
@@ -1,255 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef IMAGE_H
-#define IMAGE_H
-
-#include <stdlib.h>
-
-#ifdef __APPLE__
-#include <malloc/malloc.h>
-#else
-#include <malloc.h>
-#endif
-
-#include <stdarg.h>
-#include <math.h>
-#include <ctype.h>
-
-#ifdef WIN32
-#pragma warning(disable:4996) // get rid of POSIX deprecation errors
-#include <time.h>
-#include "pstdint.h" // MSVC++ doesn't have stdint.h
-#else
-#include <sys/time.h>
-#include <stdint.h>
-#endif
-
-#include <cv.h>
-#include <cxmisc.h>
-#include <cvaux.h>
-
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CameraInfo.h>
-//#include <sensor_msgs/fill_image.h>
-
-// alignment on allocation
-#ifdef __APPLE__
-#define MEMALIGN(x) malloc(x)
-#define MEMFREE(x) {if (x) free(x);}
-#else
-#define MEMALIGN(x) memalign(16,x)
-#define MEMFREE(x) {if (x) free(x);}
-#endif
-
-
-#ifndef COLOR_CODING_T
-typedef enum {
- COLOR_CODING_MONO8 = 3000,
- COLOR_CODING_MONO16,
- COLOR_CODING_BAYER8_RGGB,
- COLOR_CODING_BAYER8_BGGR,
- COLOR_CODING_BAYER8_GBRG,
- COLOR_CODING_BAYER8_GRBG,
- COLOR_CODING_BAYER16_RGGB,
- COLOR_CODING_BAYER16_BGGR,
- COLOR_CODING_BAYER16_GBRG,
- COLOR_CODING_BAYER16_GRBG,
- COLOR_CODING_RGB8, // RGB order
- COLOR_CODING_RGBA8, // RGBA order
- COLOR_CODING_RGB16, // RGB order
- COLOR_CODING_RGBA16, // RGBA order
-
- // these are stereo interlace encodings
- // Videre stereo:
- // Mono has left/right pixels interlaced
- // Color has left/right pixels interlace, bayer pixels
- // STOC modes have rectified images, raw encodings, disparity, etc
- VIDERE_STEREO_MONO,
- VIDERE_STEREO_RGGB,
- VIDERE_STEREO_GRBG,
- VIDERE_STEREO_BGGR,
- VIDERE_STOC_RECT_RECT, // left and right rectified mono
- VIDERE_STOC_RECT_DISP, // left rectified mono, right disparity
- VIDERE_STOC_RAW_DISP_MONO, // left raw mono, right disparity
- VIDERE_STOC_RAW_DISP_RGGB, // left raw color, right disparity
- VIDERE_STOC_RAW_DISP_GRBG, // left raw color, right disparity
- VIDERE_STOC_RAW_RAW_MONO, // left and right raw, mono
- VIDERE_STOC_RAW_RAW_RGGB, // left and right raw, color
- VIDERE_STOC_RAW_RAW_GRBG, // left and right raw, color
-
- COLOR_CODING_NONE // no image info
-} color_coding_t;
-#define COLOR_CODING_T
-#endif
-
-
-#ifndef COLOR_CONVERSION_T
-typedef enum {
- COLOR_CONVERSION_BILINEAR,
- COLOR_CONVERSION_EDGE
-} color_conversion_t;
-#define COLOR_CONVERSION_T
-#endif
-
-
-typedef enum
-{
- NORMAL_ALGORITHM,
- SCANLINE_ALGORITHM,
- DP_ALGORITHM,
- MW_ALGORITHM,
- LS_ALGORITHM,
- NCC_ALGORITHM
-} stereo_algorithm_t;
-
-
-//
-// structured points in a 4xN point array
-//
-
-typedef struct
-{
- float X;
- float Y;
- float Z;
- int32_t A; // negative for undefined point, otherwise arbitrary index
-} pt_xyza_t;
-
-
-namespace cam
-{
- // monocular data structure
- // generally, all images should be on 16-byte alignment
-
- // internal types for conversion routines
- const static uint8_t NONE = 0;
- const static uint8_t IMAGE_RAW = 1;
- const static uint8_t IMAGE = 2;
- const static uint8_t IMAGE_COLOR = 3;
- const static uint8_t IMAGE_RECT = 4;
- const static uint8_t IMAGE_RECT_COLOR = 5;
-
- class ImageData
- {
-
- public:
- ImageData();
- ~ImageData();
-
- // image parameters
- int imWidth;
- int imHeight;
-
- // image data
- // these can be NULL if no data is present
- // the Type info is COLOR_CODING_NONE if the data is not current
- // the Size info gives the buffer size, for allocation logic
- // NOTE: all data buffers should be 16-byte aligned
- // @todo: can we just use IplImages for these...
- uint8_t *imRaw; // raw image
- color_coding_t imRawType; // type of raw data
- size_t imRawSize;
- uint8_t *im; // monochrome image
- color_coding_t imType;
- size_t imSize;
- uint8_t *imColor; // color image, always RGB32
- color_coding_t imColorType;
- size_t imColorSize;
- uint8_t *imRect; // rectified monochrome image
- color_coding_t imRectType;
- size_t imRectSize;
- uint8_t *imRectColor; // rectified color image, always RGB32
- color_coding_t imRectColorType;
- size_t imRectColorSize;
-
- // timing
- uint64_t im_time; // us time when the frame finished DMA into the host
-
- // calibration parameters
- // row major order
- bool initRect; // whether arrays are initialized or not
- double D[5]; // distortion: k1, k2, t1, t2, k3
- double K[9]; // original camera matrix
- double R[9]; // rectification matrix
- double P[12]; // projection/camera matrix
-
- // raw parameter string
- char *params; // on-camera parameters
-
- // buffers
- void releaseBuffers(); // get rid of all buffers
-
- // rectification
- bool hasRectification; // true if valid rectification present
- bool doRectify(); // try to rectify images
- bool initRectify(bool force=false); // initializes the rectification internals from the
- // calibration parameters
-
- // color conversion
- color_conversion_t colorConvertType; // BILINEAR or EDGE conversion
- void doBayerColorRGB(); // does Bayer => color and mono
- void doBayerMono(); // does Bayer => mono
-
-
- protected:
- // rectification arrays from OpenCV
- CvMat *rK;
- CvMat *rD;
- CvMat *rR;
- CvMat *rKp;
-
- CvMat* rMapxy; // rectification table, integer format
- CvMat* rMapa;
- CvMat* mx,* my;
- IplImage* srcIm; // temps for rectification
- IplImage* dstIm;
-
- private:
- // various color converters
- void convertBayerGRBGColorRGB(uint8_t *src, uint8_t *dstc, uint8_t *dstm,
- int width, int height, color_conversion_t colorAlg);
- void convertBayerBGGRColorRGB(uint8_t *src, uint8_t *dstc, uint8_t *dstm,
- int width, int height, color_conversion_t colorAlg);
- void convertBayerGRBGMono(uint8_t *src, uint8_t *dstm,
- int width, int height, color_conversion_t colorAlg);
- void convertBayerBGGRMono(uint8_t *src, uint8_t *dstm,
- int width, int height, color_conversion_t colorAlg);
- };
-
-}
-
-
-
-#endif // IMAGE_H
Copied: pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/cam_bridge.h (from rev 23600, pkg/trunk/stacks/image_pipeline/image_proc/include/cam_bridge.h)
===================================================================
--- pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/cam_bridge.h (rev 0)
+++ pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/cam_bridge.h 2009-09-01 22:04:47 UTC (rev 23607)
@@ -0,0 +1,153 @@
+
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef CAM_BRIDGE_H
+#define CAM_BRIDGE_H
+
+namespace cam_bridge
+{
+
+ color_coding_t GetColorCoding(const sensor_msgs::Image& msg)
+ {
+ using namespace sensor_msgs::image_encodings;
+
+ if (msg.encoding == MONO8) return COLOR_CODING_MONO8;
+ if (msg.encoding == MONO16) return COLOR_CODING_MONO16;
+ if (msg.encoding == BAYER_RGGB8) return COLOR_CODING_BAYER8_RGGB;
+ if (msg.encoding == BAYER_BGGR8) return COLOR_CODING_BAYER8_BGGR;
+ if (msg.encoding == BAYER_GBRG8) return COLOR_CODING_BAYER8_GBRG;
+ if (msg.encoding == BAYER_GRBG8) return COLOR_CODING_BAYER8_GRBG;
+ if (msg.encoding == RGB8) return COLOR_CODING_RGB8;
+ if (msg.encoding == RGBA8) return COLOR_CODING_RGBA8;
+
+ ROS_ERROR("cam_bridge: Encoding '%s' is not supported", msg.encoding.c_str());
+ return COLOR_CODING_NONE;
+ }
+
+ std::string ColorCodingToImageEncoding(color_coding_t coding)
+ {
+ using namespace sensor_msgs::image_encodings;
+
+ if (coding == COLOR_CODING_MONO8) return MONO8;
+ if (coding == COLOR_CODING_MONO16) return MONO16;
+ if (coding == COLOR_CODING_BAYER8_RGGB) return BAYER_RGGB8;
+ if (coding == COLOR_CODING_BAYER8_BGGR) return BAYER_BGGR8;
+ if (coding == COLOR_CODING_BAYER8_GBRG) return BAYER_GBRG8;
+ if (coding == COLOR_CODING_BAYER8_GRBG) return BAYER_GRBG8;
+ if (coding == COLOR_CODING_RGB8) return RGB8;
+ if (coding == COLOR_CODING_RGBA8) return RGBA8;
+
+ ROS_WARN("cam_bridge: Don't know image encoding string for color coding %i", coding);
+ return "";
+ }
+
+ void extractImage(std::vector<uint8_t> data, size_t* sz, uint8_t **d)
+ {
+ size_t new_size = data.size();
+
+ if (*sz < new_size)
+ {
+ MEMFREE(*d);
+ *d = (uint8_t *)MEMALIGN(new_size);
+ *sz = new_size;
+ }
+ memcpy((char*)(*d), (char*)(&data[0]), new_size);
+ }
+ void extractImage(std::vector<uint8_t> data, size_t* sz, int16_t **d)
+ {
+ size_t new_size = data.size();
+
+ if (*sz < new_size)
+ {
+ MEMFREE(*d);
+ *d = (int16_t *)MEMALIGN(new_size);
+ *sz = new_size;
+ }
+ memcpy((char*)(*d), (char*)(&data[0]), new_size);
+ }
+
+
+ void RawToCamData(const sensor_msgs::Image& im_msg,
+ const sensor_msgs::CameraInfo& info_msg,
+ uint8_t type, cam::ImageData* im)
+ {
+
+ im->imRawType = COLOR_CODING_NONE;
+ im->imType = COLOR_CODING_NONE;
+ im->imColorType = COLOR_CODING_NONE;
+ im->imRectType = COLOR_CODING_NONE;
+ im->imRectColorType = COLOR_CODING_NONE;
+
+ if (type == cam::IMAGE_RAW)
+ {
+ extractImage(im_msg.data, &im->imRawSize, &im->imRaw);
+ im->imRawType = GetColorCoding(im_msg);
+ }
+ else if (type == cam::IMAGE)
+ {
+ extractImage(im_msg.data, &im->imSize, &im->im);
+ im->imType = COLOR_CODING_MONO8;
+ }
+ else if (type == cam::IMAGE_COLOR)
+ {
+ extractImage(im_msg.data, &im->imColorSize, &im->imColor);
+ im->imColorType = GetColorCoding(im_msg);
+ }
+ else if (type == cam::IMAGE_RECT)
+ {
+ extractImage(im_msg.data, &im->imRectSize, &im->imRect);
+ im->imRectType = GetColorCoding(im_msg);
+ }
+ else if (type == cam::IMAGE_RECT_COLOR)
+ {
+ extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
+ im->imRectColorType = GetColorCoding(im_msg);
+ }
+
+ // @todo: this OK when right image empty (disparity image requested instead)?
+ im->imHeight = im_msg.height;
+ im->imWidth = im_msg.width;
+
+ // @todo: possible to NOT have rectification?
+ memcpy((char*)(im->D), (char*)(&info_msg.D[0]), 5*sizeof(double));
+ memcpy((char*)(im->K), (char*)(&info_msg.K[0]), 9*sizeof(double));
+ memcpy((char*)(im->R), (char*)(&info_msg.R[0]), 9*sizeof(double));
+ memcpy((char*)(im->P), (char*)(&info_msg.P[0]), 12*sizeof(double));
+ im->hasRectification = true;
+ }
+}
+
+
+#endif // CAM_BRIDGE_H
Property changes on: pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/cam_bridge.h
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/stacks/imaging_pipeline/image_proc/include/cam_bridge.h:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/image.h (from rev 23600, pkg/trunk/stacks/image_pipeline/image_proc/include/image.h)
===================================================================
--- pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/image.h (rev 0)
+++ pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/image.h 2009-09-01 22:04:47 UTC (rev 23607)
@@ -0,0 +1,255 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_H
+#define IMAGE_H
+
+#include <stdlib.h>
+
+#ifdef __APPLE__
+#include <malloc/malloc.h>
+#else
+#include <malloc.h>
+#endif
+
+#include <stdarg.h>
+#include <math.h>
+#include <ctype.h>
+
+#ifdef WIN32
+#pragma warning(disable:4996) // get rid of POSIX deprecation errors
+#include <time.h>
+#include "pstdint.h" // MSVC++ doesn't have stdint.h
+#else
+#include <sys/time.h>
+#include <stdint.h>
+#endif
+
+#include <cv.h>
+#include <cxmisc.h>
+#include <cvaux.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+//#include <sensor_msgs/fill_image.h>
+
+// alignment on allocation
+#ifdef __APPLE__
+#define MEMALIGN(x) malloc(x)
+#define MEMFREE(x) {if (x) free(x);}
+#else
+#define MEMALIGN(x) memalign(16,x)
+#define MEMFREE(x) {if (x) free(x);}
+#endif
+
+
+#ifndef COLOR_CODING_T
+typedef enum {
+ COLOR_CODING_MONO8 = 3000,
+ COLOR_CODING_MONO16,
+ COLOR_CODING_BAYER8_RGGB,
+ COLOR_CODING_BAYER8_BGGR,
+ COLOR_CODING_BAYER8_GBRG,
+ COLOR_CODING_BAYER8_GRBG,
+ COLOR_CODING_BAYER16_RGGB,
+ COLOR_CODING_BAYER16_BGGR,
+ COLOR_CODING_BAYER16_GBRG,
+ COLOR_CODING_BAYER16_GRBG,
+ COLOR_CODING_RGB8, // RGB order
+ COLOR_CODING_RGBA8, // RGBA order
+ COLOR_CODING_RGB16, // RGB order
+ COLOR_CODING_RGBA16, // RGBA order
+
+ // these are stereo interlace encodings
+ // Videre stereo:
+ // Mono has left/right pixels interlaced
+ // Color has left/right pixels interlace, bayer pixels
+ // STOC modes have rectified images, raw encodings, disparity, etc
+ VIDERE_STEREO_MONO,
+ VIDERE_STEREO_RGGB,
+ VIDERE_STEREO_GRBG,
+ VIDERE_STEREO_BGGR,
+ VIDERE_STOC_RECT_RECT, // left and right rectified mono
+ VIDERE_STOC_RECT_DISP, // left rectified mono, right disparity
+ VIDERE_STOC_RAW_DISP_MONO, // left raw mono, right disparity
+ VIDERE_STOC_RAW_DISP_RGGB, // left raw color, right disparity
+ VIDERE_STOC_RAW_DISP_GRBG, // left raw color, right disparity
+ VIDERE_STOC_RAW_RAW_MONO, // left and right raw, mono
+ VIDERE_STOC_RAW_RAW_RGGB, // left and right raw, color
+ VIDERE_STOC_RAW_RAW_GRBG, // left and right raw, color
+
+ COLOR_CODING_NONE // no image info
+} color_coding_t;
+#define COLOR_CODING_T
+#endif
+
+
+#ifndef COLOR_CONVERSION_T
+typedef enum {
+ COLOR_CONVERSION_BILINEAR,
+ COLOR_CONVERSION_EDGE
+} color_conversion_t;
+#define COLOR_CONVERSION_T
+#endif
+
+
+typedef enum
+{
+ NORMAL_ALGORITHM,
+ SCANLINE_ALGORITHM,
+ DP_ALGORITHM,
+ MW_ALGORITHM,
+ LS_ALGORITHM,
+ NCC_ALGORITHM
+} stereo_algorithm_t;
+
+
+//
+// structured points in a 4xN point array
+//
+
+typedef struct
+{
+ float X;
+ float Y;
+ float Z;
+ int32_t A; // negative for undefined point, otherwise arbitrary index
+} pt_xyza_t;
+
+
+namespace cam
+{
+ // monocular data structure
+ // generally, all images should be on 16-byte alignment
+
+ // internal types for conversion routines
+ const static uint8_t NONE = 0;
+ const static uint8_t IMAGE_RAW = 1;
+ const static uint8_t IMAGE = 2;
+ const static uint8_t IMAGE_COLOR = 3;
+ const static uint8_t IMAGE_RECT = 4;
+ const static uint8_t IMAGE_RECT_COLOR = 5;
+
+ class ImageData
+ {
+
+ public:
+ ImageData();
+ ~ImageData();
+
+ // image parameters
+ int imWidth;
+ int imHeight;
+
+ // image data
+ // these can be NULL if no data is present
+ // the Type info is COLOR_CODING_NONE if the data is not current
+ // the Size info gives the buffer size, for allocation logic
+ // NOTE: all data buffers should be 16-byte aligned
+ // @todo: can we just use IplImages for these...
+ uint8_t *imRaw; // raw image
+ color_coding_t imRawType; // type of raw data
+ size_t imRawSize;
+ uint8_t *im; // monochrome image
+ color_coding_t imType;
+ size_t imSize;
+ uint8_t *imColor; // color image, always RGB32
+ color_coding_t imColorType;
+ size_t imColorSize;
+ uint8_t *imRect; // rectified monochrome image
+ color_coding_t imRectType;
+ size_t imRectSize;
+ uint8_t *imRectColor; // rectified color image, always RGB32
+ color_coding_t imRectColorType;
+ size_t imRectColorSize;
+
+ // timing
+ uint64_t im_time; // us time when the frame finished DMA into the host
+
+ // calibration parameters
+ // row major order
+ bool initRect; // whether arrays are initialized or not
+ double D[5]; // distortion: k1, k2, t1, t2, k3
+ double K[9]; // original camera matrix
+ double R[9]; // rectification matrix
+ double P[12]; // projection/camera matrix
+
+ // raw parameter string
+ char *params; // on-camera parameters
+
+ // buffers
+ void releaseBuffers(); // get rid of all buffers
+
+ // rectification
+ bool hasRectification; // true if valid rectification present
+ bool doRectify(); // try to rectify images
+ bool initRectify(bool force=false); // initializes the rectification internals from the
+ // calibration parameters
+
+ // color conversion
+ color_conversion_t colorConvertType; // BILINEAR or EDGE conversion
+ void doBayerColorRGB(); // does Bayer => color and mono
+ void doBayerMono(); // does Bayer => mono
+
+
+ protected:
+ // rectification arrays from OpenCV
+ CvMat *rK;
+ CvMat *rD;
+ CvMat *rR;
+ CvMat *rKp;
+
+ CvMat* rMapxy; // rectification table, integer format
+ CvMat* rMapa;
+ CvMat* mx,* my;
+ IplImage* srcIm; // temps for rectification
+ IplImage* dstIm;
+
+ private:
+ // various color converters
+ void convertBayerGRBGColorRGB(uint8_t *src, uint8_t *dstc, uint8_t *dstm,
+ int width, int height, color_conversion_t colorAlg);
+ void convertBayerBGGRColorRGB(uint8_t *src, uint8_t *dstc, uint8_t *dstm,
+ int width, int height, color_conversion_t colorAlg);
+ void convertBayerGRBGMono(uint8_t *src, uint8_t *dstm,
+ int width, int height, color_conversion_t colorAlg);
+ void convertBayerBGGRMono(uint8_t *src, uint8_t *dstm,
+ int width, int height, color_conversion_t colorAlg);
+ };
+
+}
+
+
+
+#endif // IMAGE_H
Property changes on: pkg/trunk/stacks/image_pipeline/image_proc/include/image_proc/image.h
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/stacks/imaging_pipeline/image_proc/include/image.h:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/stacks/image_pipeline/image_proc/src/nodes/image_proc.cpp
===================================================================
--- pkg/trunk/stacks/image_pipeline/image_proc/src/nodes/image_proc.cpp 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/stacks/image_pipeline/image_proc/src/nodes/image_proc.cpp 2009-09-01 22:04:47 UTC (rev 23607)
@@ -43,8 +43,8 @@
#include <opencv_latest/CvBridge.h>
#include <image_transport/image_publisher.h>
-#include "image.h"
-#include "cam_bridge.h"
+#include "image_proc/image.h"
+#include "image_proc/cam_bridge.h"
#include <boost/thread.hpp>
Modified: pkg/trunk/stacks/image_pipeline/image_proc/src/proc/image.cpp
===================================================================
--- pkg/trunk/stacks/image_pipeline/image_proc/src/proc/image.cpp 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/stacks/image_pipeline/image_proc/src/proc/image.cpp 2009-09-01 22:04:47 UTC (rev 23607)
@@ -38,7 +38,7 @@
// classes for monocular and stereo image
//
-#include "image.h"
+#include "image_proc/image.h"
#include <sstream>
#include <iostream>
Modified: pkg/trunk/stacks/image_pipeline/stereo_image_proc/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/image_pipeline/stereo_image_proc/CMakeLists.txt 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/stacks/image_pipeline/stereo_image_proc/CMakeLists.txt 2009-09-01 22:04:47 UTC (rev 23607)
@@ -16,9 +16,7 @@
rospack_add_executable(stereoproc_exe src/nodes/stereoproc.cpp)
target_link_libraries(stereoproc_exe stereoproc imageproc)
SET_TARGET_PROPERTIES(stereoproc_exe PROPERTIES OUTPUT_NAME stereoproc)
-rospack_add_compile_flags(stereoproc_exe "-msse2 -mpreferred-stack-boundary=4")
rospack_add_executable(stereoimageproc_exe src/nodes/stereo_image_proc.cpp)
target_link_libraries(stereoimageproc_exe stereoproc imageproc)
SET_TARGET_PROPERTIES(stereoimageproc_exe PROPERTIES OUTPUT_NAME stereo_image_proc)
-rospack_add_compile_flags(stereoimageproc_exe "-msse2 -mpreferred-stack-boundary=4")
Deleted: pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/cam_bridge_old.h
===================================================================
--- pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/cam_bridge_old.h 2009-09-01 21:56:20 UTC (rev 23606)
+++ pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/cam_bridge_old.h 2009-09-01 22:04:47 UTC (rev 23607)
@@ -1,342 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef CAM_BRIDGE_HH
-#define CAM_BRIDGE_HH
-
-#include "stereo_msgs/RawStereo.h"
-#include "sensor_msgs/fill_image.h"
-#include "sensor_msgs/image_encodings.h"
-#include "stereoimage.h"
-
-namespace cam_bridge
-{
-
- color_coding_t GetColorCoding(const sensor_msgs::Image& msg)
- {
- using namespace sensor_msgs::image_encodings;
-
- if (msg.encoding == MONO8) return COLOR_CODING_MONO8;
- if (msg.encoding == MONO16) return COLOR_CODING_MONO16;
- if (msg.encoding == BAYER_RGGB8) return COLOR_CODING_BAYER8_RGGB;
- if (msg.encoding == BAYER_BGGR8) return COLOR_CODING_BAYER8_BGGR;
- if (msg.encoding == BAYER_GBRG8) return COLOR_CODING_BAYER8_GBRG;
- if (msg.encoding == BAYER_GRBG8) return COLOR_CODING_BAYER8_GRBG;
- if (msg.encoding == RGB8) return COLOR_CODING_RGB8;
- if (msg.encoding == RGBA8) return COLOR_CODING_RGBA8;
-
- ROS_ERROR("cam_bridge: Encoding '%s' is not supported", msg.encoding.c_str());
- return COLOR_CODING_NONE;
- }
-
- std::string ColorCodingToImageEncoding(color_coding_t coding)
- {
- using namespace sensor_msgs::image_encodings;
-
- if (coding == COLOR_CODING_MONO8) return MONO8;
- if (coding == COLOR_CODING_MONO16) return MONO16;
- if (coding == COLOR_CODING_BAYER8_RGGB) return BAYER_RGGB8;
- if (coding == COLOR_CODING_BAYER8_BGGR) return BAYER_BGGR8;
- if (coding == COLOR_CODING_BAYER8_GBRG) return BAYER_GBRG8;
- if (coding == COLOR_CODING_BAYER8_GRBG) return BAYER_GRBG8;
- if (coding == COLOR_CODING_RGB8) return RGB8;
- if (coding == COLOR_CODING_RGBA8) return RGBA8;
-
- ROS_WARN("cam_bridge: Don't know image encoding string for color coding %i", coding);
- return "";
- }
-
- void extractImage(std::vector<uint8_t> data, size_t* sz, uint8_t **d)
- {
- size_t new_size = data.size();
-
- if (*sz < new_size)
- {
- MEMFREE(*d);
- *d = (uint8_t *)MEMALIGN(new_size);
- *sz = new_size;
- }
- memcpy((char*)(*d), (char*)(&data[0]), new_size);
- }
- void extractImage(std::vector<uint8_t> data, size_t* sz, int16_t **d)
- {
- size_t new_size = data.size();
-
- if (*sz < new_size)
- {
- MEMFREE(*d);
- *d = (int16_t *)MEMALIGN(new_size);
- *sz = new_size;
- }
- memcpy((char*)(*d), (char*)(&data[0]), new_size);
- }
-
-
- void RawToCamData(const sensor_msgs::Image& im_msg,
- const sensor_msgs::CameraInfo& info_msg,
- uint8_t type, cam::ImageData* im)
- {
-
- im->imRawType = COLOR_CODING_NONE;
- im->imType = COLOR_CODING_NONE;
- im->imColorType = COLOR_CODING_NONE;
- im->imRectType = COLOR_CODING_NONE;
- im->imRectColorType = COLOR_CODING_NONE;
-
- if (type == cam::IMAGE_RAW)
- {
- extractImage(im_msg.data, &im->imRawSize, &im->imRaw);
- im->imRawType = GetColorCoding(im_msg);
- }
- else if (type == cam::IMAGE)
- {
- extractImage(im_msg.data, &im->imSize, &im->im);
- im->imType = COLOR_CODING_MONO8;
- }
- else if (type == cam::IMAGE_COLOR)
- {
- extractImage(im_msg.data, &im->imColorSize, &im->imColor);
- im->imColorType = GetColorCoding(im_msg);
- }
- else if (type == cam::IMAGE_RECT)
- {
- extractImage(im_msg.data, &im->imRectSize, &im->imRect);
- im->imRectType = GetColorCoding(im_msg);
- }
- else if (type == cam::IMAGE_RECT_COLOR)
- {
- extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
- im->imRectColorType = GetColorCoding(im_msg);
- }
-
- // @todo: this OK when right image empty (disparity image requested instead)?
- im->imHeight = im_msg.height;
- im->imWidth = im_msg.width;
-
- // @todo: possible to NOT have rectification?
- memcpy((char*)(im->D), (char*)(&info_msg.D[0]), 5*sizeof(double));
- memcpy((char*)(im->K), (char*)(&info_msg.K[0]), 9*sizeof(double));
- memcpy((char*)(im->R), (char*)(&info_msg.R[0]), 9*sizeof(double));
- memcpy((char*)(im->P), (char*)(&info_msg.P[0]), 12*sizeof(double));
- im->hasRectification = true;
- }
-
-
- void CamDataToRawStereo(cam::ImageData* im, sensor_msgs::Image& im_msg, sensor_msgs::CameraInfo& info_msg, uint8_t& type)
- {
- // @todo: this could all be less hard-coded
- if (im->imRawType != COLOR_CODING_NONE)
- {
- std::string encoding = ColorCodingToImageEncoding(im->imRawType);
- fillImage(im_msg, encoding, im->imHeight, im->imWidth, im->imWidth, im->imRaw);
- type = stereo_msgs::RawStereo::IMAGE_RAW;
- }
- else if (im->imType != COLOR_CODING_NONE)
- {
- fillImage(im_msg, sensor_msgs::image_encodings::MONO8, im->imHeight, im->imWidth, im->imWidth, im->im);
- type = stereo_msgs::RawStereo::IMAGE;
- }
- else if (im->imColorType != COLOR_CODING_NONE && im->imColorType == COLOR_CODING_RGBA8)
- {
- fillImage(im_msg, sensor_msgs::image_encodings::RGBA8, im->imHeight, im->imWidth, 4 * im->imWidth, im->imColor);
- type = stereo_msgs::RawStereo::IMAGE_COLOR;
- }
- else if (im->imColorType != COLOR_CODING_NONE && im->imColorType == COLOR_CODING_RGB8)
- {
- fillImage(im_msg, sensor_msgs::image_encodings::RGB8, im->imHeight, im->imWidth, 3 * im->imWidth, im->imColor);
- type = stereo_msgs::RawStereo::IMAGE_COLOR;
- }
- else if (im->imRectType != COLOR_CODING_NONE)
- {
- fillImage(im_msg, sensor_msgs::image_encodings::MONO8, im->imHeight, im->imWidth, im->imWidth, im->imRect);
- type = stereo_msgs::RawStereo::IMAGE_RECT;
- }
- else if (im->imRectColorType != COLOR_CODING_NONE && im->imRectColorType == COLOR_CODING_RGBA8)
- {
- fillImage(im_msg, sensor_msgs::image_encodings::RGBA8, im->imHeight, im->imWidth, 4 * im->imWidth, im->imRectColor);
- type = stereo_msgs::RawStereo::IMAGE_RECT_COLOR;
- }
- else if (im->imRectColorType != COLOR_CODING_NONE && im->imRectColorType == COLOR_CODING_RGB8)
- {
- fillImage(im_msg, sensor_msgs::image_encodings::RGB8, im->imHeight, im->imWidth, 3 * im->imWidth, im->imRectColor);
- type = stereo_msgs::RawStereo::IMAGE_RECT_COLOR;
- }
-
- info_msg.height = im->imHeight;
- info_msg.width = im->imWidth;
-
- memcpy((char*)(&info_msg.D[0]), (char*)(im->D), 5*sizeof(double));
- memcpy((char*)(&info_msg.K[0]), (char*)(im->K), 9*sizeof(double));
- memcpy((char*)(&info_msg.R[0]), (char*)(im->R), 9*sizeof(double));
- memcpy((char*)(&info_msg.P[0]), (char*)(im->P), 12*sizeof(double));
- }
-
- void StereoDataToRawStereo(cam::StereoData* stIm, stereo_msgs::RawStereo& raw_stereo)
- {
- raw_stereo.header.stamp = ros::Time().fromNSec(stIm->imLeft->im_time * 1000);
-
- if (stIm->hasDisparity)
- {
- fillImage(raw_stereo.disparity_image,
- sensor_msgs::image_encodings::TYPE_16SC1,
- stIm->imWidth,
- stIm->imHeight,
- 2 * stIm->imWidth,
- stIm->imDisp );
-
- raw_stereo.has_disparity = true;
-
- raw_stereo.disparity_info.height = stIm->imHeight;
- raw_stereo.disparity_info.width = stIm->imWidth;
-
- raw_stereo.disparity_info.dpp = stIm->dpp;
- raw_stereo.disparity_info.num_disp = stIm->numDisp;
- raw_stereo.disparity_info.im_Dtop = stIm->imDtop;
- raw_stereo.disparity_info.im_Dleft = stIm->imDleft;
- raw_stereo.disparity_info.im_Dwidth = stIm->imDwidth;
- raw_stereo.disparity_info.im_Dheight = stIm->imDheight;
- raw_stereo.disparity_info.corr_size = stIm->corrSize;
- raw_stereo.disparity_info.filter_size = stIm->filterSize;
- raw_stereo.disparity_info.hor_offset = stIm->horOffset;
- raw_stereo.disparity_info.texture_thresh = stIm->textureThresh;
- raw_stereo.disparity_info.unique_thresh = stIm->uniqueThresh;
- raw_stereo.disparity_info.smooth_thresh = stIm->smoothThresh;
- raw_stereo.disparity_info.speckle_diff = stIm->speckleDiff;
- raw_stereo.disparity_info.speckle_region_size = stIm->speckleRegionSize;
- raw_stereo.disparity_info.unique_check = stIm->unique_check;
-
- } else {
- clearImage(raw_stereo.disparity_image);
- raw_stereo.has_disparity = false;
- }
-
- raw_stereo.stereo_info.height = stIm->imHeight;
- raw_stereo.stereo_info.width = stIm->imWidth;
- memcpy((char*)(&raw_stereo.stereo_info.T[0]), (char*)(stIm->T), 3*sizeof(double));
- memcpy((char*)(&raw_stereo.stereo_info.Om[0]), (char*)(stIm->Om), 3*sizeof(double));
- memcpy((char*)(&raw_stereo.stereo_info.RP[0]), (char*)(stIm->RP), 16*sizeof(double));
-
- CamDataToRawStereo(stIm->imLeft, raw_stereo.left_image, raw_stereo.left_info, raw_stereo.left_type);
- CamDataToRawStereo(stIm->imRight, raw_stereo.right_image, raw_stereo.right_info, raw_stereo.right_type);
- }
-
-
- void RawStereoToCamData(const sensor_msgs::Image& im_msg, const sensor_msgs::CameraInfo& info_msg, uint8_t type, cam::ImageData* im)
- {
-
- im->imRawType = COLOR_CODING_NONE;
- im->imType = COLOR_CODING_NONE;
- im->imColorType = COLOR_CODING_NONE;
- im->imRectType = COLOR_CODING_NONE;
- im->imRectColorType = COLOR_CODING_NONE;
-
- if (type == stereo_msgs::RawStereo::IMAGE_RAW)
- {
- extractImage(im_msg.data, &im->imRawSize, &im->imRaw);
- im->imRawType = GetColorCoding(im_msg);
- }
- else if (type == stereo_msgs::RawStereo::IMAGE)
- {
- extractImage(im_msg.data, &im->imSize, &im->im);
- im->imType = COLOR_CODING_MONO8;
- }
- else if (type == stereo_msgs::RawStereo::IMAGE_COLOR)
- {
- extractImage(im_msg.data, &im->imColorSize, &im->imColor);
- im->imColorType = GetColorCoding(im_msg);
- }
- else if (type == stereo_msgs::RawStereo::IMAGE_RECT)
- {
- extractImage(im_msg.data, &im->imRectSize, &im->imRect);
- im->imRectType = GetColorCoding(im_msg);
- }
- else if (type == stereo_msgs::RawStereo::IMAGE_RECT_COLOR)
- {
- extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
- im->imRectColorType = GetColorCoding(im_msg);
- }
-
- // @todo: this OK when right image empty (disparity image requested instead)?
- im->imHeight = im_msg.height;
- im->imWidth = im_msg.width;
-
- // @todo: possible to NOT have rectification?
- memcpy((char*)(im->D), (char*)(&info_msg.D[0]), 5*sizeof(double));
- memcpy((char*)(im->K), (char*)(&info_msg.K[0]), 9*sizeof(double));
- memcpy((char*)(im->R), (char*)(&info_msg.R[0]), 9*sizeof(double));
- memcpy((char*)(im->P), (char*)(&info_msg.P[0]), 12*sizeof(double));
- im->hasRectification = true;
- }
-
- void RawStereoToStereoData(const stereo_msgs::RawStereo& raw_stereo, cam::StereoData* stIm)
- {
- stIm->imLeft->im_time = raw_stereo.header.stamp.toNSec() / 1000;
- stIm->imRight->im_time = raw_stereo.header.stamp.toNSec() / 1000;
- stIm->setSize(raw_stereo.stereo_info.width, raw_stereo.stereo_info.height);
-
- stIm->hasDisparity = false;
-
- // @todo: if not, don't try to extract right image?
- if (raw_stereo.has_disparity)
- {
- extractImage(raw_stereo.disparity_image.data, &stIm->imDispSize, &stIm->imDisp);
- stIm->hasDisparity = true;
-
- stIm->dpp = raw_stereo.disparity_info.dpp;
- stIm->numDisp = raw_stereo.disparity_info.num_disp;
- stIm->imDtop = raw_stereo.disparity_info.im_Dtop;
- stIm->imDleft = raw_stereo.disparity_info.im_Dleft;
- stIm->imDwidth = raw_stereo.disparity_info.im_Dwidth;
- stIm->imDheight = raw_stereo.disparity_info.im_Dheight;
- stIm->corrSize = raw_stereo.disparity_info.corr_size;
- stIm->filterSize = raw_stereo.disparity_info.filter_size;
- stIm->horOffset = raw_stereo.disparity_info.hor_offset;
- stIm->textureThresh = raw_stereo.disparity_info.texture_thresh;
- stIm->uniqueThresh = raw_stereo.disparity_info.unique_thresh;
- stIm->smoothThresh = raw_stereo.disparity_info.smooth_thresh;
- stIm->speckleDiff = raw_stereo.disparity_info.speckle_diff;
- stIm->speckleRegionSize = raw_stereo.disparity_info.speckle_region_size;
- stIm->unique_check = raw_stereo.disparity_info.unique_check;
- }
-
- memcpy((char*)(stIm->T), (char*)(&raw_stereo.stereo_info.T[0]), 3*sizeof(double));
- memcpy((char*)(stIm->Om), (char*)(&raw_stereo.stereo_info.Om[0]), 3*sizeof(double));
- memcpy((char*)(stIm->RP), (char*)(&raw_stereo.stereo_info.RP[0]), 16*sizeof(double));
-
- RawStereoToCamData(raw_stereo.left_image, raw_stereo.left_info, raw_stereo.left_type, stIm->imLeft);
- RawStereoToCamData(raw_stereo.right_image, raw_stereo.right_info, raw_stereo.right_type, stIm->imRight);
- }
-}
-
-#endif
Copied: pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereo_image_proc/cam_bridge_old.h (from rev 23600, pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/cam_bridge_old.h)
===================================================================
--- pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereo_image_proc/cam_bridge_old.h (rev 0)
+++ pkg/trunk/stacks/image_pipeline/stereo_image_proc/include/stereo_image_proc/cam_bridge_old.h 2009-09-01 22:04:47 UTC (rev 23607)
@@ -0,0 +1,342 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef CAM_BRIDGE_HH
+#define CAM_BRIDGE_HH
+
+#include "stereo_msgs/RawStereo.h"
+#include "sensor_msgs/fill_image.h"
+#include "sensor_msgs/image_encodings.h"
+#include "stereo_image_proc/stereoimage.h"
+
+namespace cam_bridge
+{
+
+ color_coding_t GetColorCoding(const sensor_msgs::Image& msg)
+ {
+ using namespace sensor_msgs::image_encodings;
+
+ if (msg.encoding == MONO8) return COLOR_CODING_MONO8;
+ if (msg.encoding == MONO16) return COLOR_CODING_MONO16;
+ if (msg.encoding == BAYER_RGGB8) return COLOR_CODING_BAYER8_RGGB;
+ if (msg.encoding == BAYER_BGGR8) return COLOR_CODING_BAYER8_BGGR;
+ if (msg.encoding == BAYER_GBRG8) return COLOR_CODING_BAYER8_GBRG;
+ if (msg.encoding == BAYER_GRBG8) return COLOR_CODING_BAYER8_GRBG;
+ if (msg.encoding == RGB8) return COLOR_CODING_RGB8;
+ if (msg.encoding == RGBA8) return COLOR_CODING_RGBA8;
+
+ ROS_ERROR("cam_bridge: Encoding '%s' is not supported", msg.encoding.c_str());
+ return COLOR_CODING_NONE;
+ }
+
+ std::string ColorCodingToImageEncoding(color_coding_t coding)
+ {
+ using namespace sensor_msgs::image_encodings;
+
+ if (coding == COLOR_CODING_MONO8) return MONO8;
+ if (coding == COLOR_CODING_MONO16) return MONO16;
+ if (coding == COLOR_CODING_BAYER8_RGGB) return BAYER_RGGB8;
+ if (coding == COLOR_CODING_BAYER8_BGGR) return BAYER_BGGR8;
+ if (coding == COLOR_CODING_BAYER8_GBRG) return BAYER_GBRG8;
+ if (coding == COLOR_CODING_BAYER8_GRBG) return BAYER_GRBG8;
+ if (coding == COLOR_CODING_RGB8) return RGB8;
+ if (coding == COLOR_CODING_RGBA8) return RGBA8;
+
+ ROS_WARN("cam_bridge: Don't know image encoding string for color coding %i", coding);
+ return "";
+ }
+
+ void extractImage(std::vector<uint8_t> data, size_t* sz, uint8_t **d)
+ {
+ size_t new_size = data.size();
+
+ if (*sz < new_size)
+ {
+ MEMFREE(*d);
+ *d = (uint8_t *)MEMALIGN(new_size);
+ *sz = new_size;
+ }
+ memcpy((char*)(*d), (char*)(&data[0]), new_size);
+ }
+ void extractImage(std::vector<uint8_t> data, size_t* sz, int16_t **d)
+ {
+ size_t new_size = data.size();
+
+ if (*sz < new_size)
+ {
+ MEMFREE(*d);
+ *d = (int16_t *)MEMALIGN(new_size);
+ *sz = new_size;
+ }
+ memcpy((char*)(*d), (char*)(&data[0]), new_size);
+ }
+
+
+ void RawToCamData(const sensor_msgs::Image& im_msg,
+ const sensor_msgs::CameraInfo& info_msg,
+ uint8_t type, cam::ImageData* im)
+ {
+
+ im->imRawType = COLOR_CODING_NONE;
+ im->imType = COLOR_CODING_NONE;
+ im->imColorType = COLOR_CODING_NONE;
+ im->imRectType = COLOR_CODING_NONE;
+ im->imRectColorType = COLOR_CODING_NONE;
+
+ if (type == cam::IMAGE_RAW)
+ {
+ extractImage(im_msg.data, &im->imRawSize, &im->imRaw);
+ im->imRawType = GetColorCoding(im_msg);
+ }
+ else if (type == cam::IMAGE)
+ {
+ extractImage(im_msg.data, &im->imSize, &im->im);
+ im->imType = COLOR_CODING_MONO8;
+ }
+ else if (type == cam::IMAGE_COLOR)
+ {
+ extractImage(im_msg.data, &im->imColorSize, &im->imColor);
+ im->imColorType = GetColorCoding(im_msg);
+ }
+ else if (type == cam::IMAGE_RECT)
+ {
+ extractImage(im_msg.data, &im->imRectSize, &im->imRect);
+ im->imRectType = GetColorCoding(im_msg);
+ }
+ else if (type == cam::IMAGE_RECT_COLOR)
+ {
+ extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
+ im->imRectColorType = GetColorCoding(im_msg);
+ }
+
+ // @todo: this OK when right image empty (disparity image requested instead)?
+ im->imHeight = im_msg.height;
+ im->imWidth = im_msg.width;
+
+ // @todo: possible to NOT have rectification?
+ memcpy((char*)(im->D), (char*)(&info_msg.D[0]), 5*sizeof(double));
+ memcpy((char*)(im->K), (char*)(&info_msg.K[0]), 9*sizeof(double));
+ memcpy((char*)(im->R), (char*)(&info_msg.R[0]), 9*sizeof(double));
+ memcpy((char*)(im->P), (char*)(&info_msg.P[0]), 12*sizeof(double));
+...
[truncated message content] |
