Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23363] pkg/trunk
|
From: <kur...@us...> - 2009-08-31 06:50:49
|
Revision: 23363
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23363&view=rev
Author: kurtkonolige
Date: 2009-08-31 06:50:29 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
revised imaging_pipeline and associated functionality, big change
Modified Paths:
--------------
pkg/trunk/deprecated/dcam/include/dcam/dcam.h
pkg/trunk/deprecated/dcam/include/dcam/stereodcam.h
pkg/trunk/deprecated/dcam/include/imwin/im3Dwin.h
pkg/trunk/deprecated/dcam/manifest.xml
pkg/trunk/deprecated/dcam/src/acquire/CMakeLists.txt
pkg/trunk/deprecated/dcam/src/nodes/CMakeLists.txt
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/CMakeLists.txt
pkg/trunk/deprecated/dcam/src/ost/ost.cpp
pkg/trunk/stacks/imaging_pipeline/image_proc/include/image.h
pkg/trunk/stacks/imaging_pipeline/image_proc/src/nodes/image_proc.cpp
pkg/trunk/stacks/imaging_pipeline/image_proc/src/proc/image.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/CMakeLists.txt
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/manifest.xml
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/image.cpp
pkg/trunk/vision/pytoro/manifest.xml
pkg/trunk/vision/pytoro/src/py.cpp
pkg/trunk/vision/vslam_demo/scripts/demowg.py
Added Paths:
-----------
pkg/trunk/stacks/imaging_pipeline/image_proc/include/cam_bridge.h
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/cam_bridge_old.h
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/nodes/
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/nodes/stereo_image_proc.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/nodes/stereoproc.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/proc/
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/proc/findplane.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/proc/imageproc.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/proc/stereoimage.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/proc/stereolib.c
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/proc/stereolib2.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/stereo_image_proc.launch
pkg/trunk/stacks/imaging_pipeline/stereo_msgs/
Removed Paths:
-------------
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/cam_bridge.h
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/image.h
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/findplane.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/stereolib.c
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/stereolib2.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/src/stereoproc.cpp
pkg/trunk/stacks/stereo/stereo_msgs/
Modified: pkg/trunk/deprecated/dcam/include/dcam/dcam.h
===================================================================
--- pkg/trunk/deprecated/dcam/include/dcam/dcam.h 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/include/dcam/dcam.h 2009-08-31 06:50:29 UTC (rev 23363)
@@ -37,7 +37,7 @@
#include <stdexcept>
#include "dc1394/dc1394.h"
-#include "image.h"
+#include "stereoimage.h"
// Pixel raw modes
// Videre stereo:
Modified: pkg/trunk/deprecated/dcam/include/dcam/stereodcam.h
===================================================================
--- pkg/trunk/deprecated/dcam/include/dcam/stereodcam.h 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/include/dcam/stereodcam.h 2009-08-31 06:50:29 UTC (rev 23363)
@@ -44,7 +44,6 @@
//
#include <stdexcept>
-#include "image.h"
#include "dcam.h"
typedef enum
Modified: pkg/trunk/deprecated/dcam/include/imwin/im3Dwin.h
===================================================================
--- pkg/trunk/deprecated/dcam/include/imwin/im3Dwin.h 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/include/imwin/im3Dwin.h 2009-08-31 06:50:29 UTC (rev 23363)
@@ -63,7 +63,7 @@
#include "FL/gl.h"
#include <GL/glut.h>
#include <GL/glu.h>
-#include "image.h"
+#include "stereoimage.h"
using namespace std;
Modified: pkg/trunk/deprecated/dcam/manifest.xml
===================================================================
--- pkg/trunk/deprecated/dcam/manifest.xml 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/manifest.xml 2009-08-31 06:50:29 UTC (rev 23363)
@@ -8,7 +8,6 @@
</export>
<depend package="libdc1394v2"/>
<depend package="opencv_latest"/>
- <depend package="stereo_image_proc"/>
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="std_srvs"/>
@@ -16,6 +15,8 @@
<depend package="diagnostic_updater"/>
<depend package="rospy"/>
<depend package="stereo_msgs"/>
+ <depend package="image_proc"/>
+ <depend package="stereo_image_proc"/>
<rosdep name="fltk"/>
<url>http://www.ros.org/wiki/dcam</url>
Modified: pkg/trunk/deprecated/dcam/src/acquire/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/dcam/src/acquire/CMakeLists.txt 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/src/acquire/CMakeLists.txt 2009-08-31 06:50:29 UTC (rev 23363)
@@ -1,7 +1,7 @@
rospack_add_executable(acquire acquire.cpp)
rospack_add_compile_flags(acquire -msse3)
-target_link_libraries(acquire dcam fltk)
+target_link_libraries(acquire dcam imageproc stereoproc fltk)
rospack_add_executable(stacquire stacquire.cpp)
rospack_add_compile_flags(stacquire -msse3)
-target_link_libraries(stacquire dcam imwin fltk fltk_gl glut)
+target_link_libraries(stacquire dcam imageproc stereoproc imwin fltk fltk_gl glut)
Modified: pkg/trunk/deprecated/dcam/src/nodes/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/dcam/src/nodes/CMakeLists.txt 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/src/nodes/CMakeLists.txt 2009-08-31 06:50:29 UTC (rev 23363)
@@ -1,14 +1,14 @@
rospack_add_executable(dcam_exe dcam.cpp)
rospack_add_compile_flags(dcam_exe -msse3)
-target_link_libraries(dcam_exe dcam)
+target_link_libraries(dcam_exe dcam imageproc stereoproc)
SET_TARGET_PROPERTIES(dcam_exe PROPERTIES OUTPUT_NAME dcam)
rospack_add_executable(stereodcam stereodcam.cpp)
rospack_add_compile_flags(stereodcam -msse3)
-target_link_libraries(stereodcam dcam)
+target_link_libraries(stereodcam dcam imageproc stereoproc)
rospack_add_executable(videre_no_STOC videre_no_STOC.cpp)
rospack_add_compile_flags(videre_no_STOC -msse3)
-target_link_libraries(videre_no_STOC dcam)
+target_link_libraries(videre_no_STOC dcam imageproc stereoproc)
rospack_add_executable(check_params check_params.cpp)
Modified: pkg/trunk/deprecated/dcam/src/nodes/dcam.cpp
===================================================================
--- pkg/trunk/deprecated/dcam/src/nodes/dcam.cpp 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/src/nodes/dcam.cpp 2009-08-31 06:50:29 UTC (rev 23363)
@@ -35,7 +35,7 @@
#include <cstdio>
#include "dcam/dcam.h"
-#include "cam_bridge.h"
+#include "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-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/src/nodes/stereodcam.cpp 2009-08-31 06:50:29 UTC (rev 23363)
@@ -134,7 +134,7 @@
#include "ros/node.h"
#include "stereo_msgs/RawStereo.h"
-#include "cam_bridge.h"
+#include "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-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/src/nodes/videre_no_STOC.cpp 2009-08-31 06:50:29 UTC (rev 23363)
@@ -74,7 +74,7 @@
#include "dcam/dcam.h"
#include "dcam/stereodcam.h"
-#include "cam_bridge.h"
+#include "cam_bridge_old.h"
#include "ros/node.h"
Modified: pkg/trunk/deprecated/dcam/src/ost/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/dcam/src/ost/CMakeLists.txt 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/src/ost/CMakeLists.txt 2009-08-31 06:50:29 UTC (rev 23363)
@@ -1,6 +1,6 @@
rospack_add_executable(ost ost.cpp stereogui.cpp calwin.cpp lodepng.cpp ostpng.cpp)
rospack_add_executable(v3d v3d.cpp)
rospack_add_compile_flags(ost -msse3)
-target_link_libraries(ost dcam imwin fltk fltk_gl glut)
+target_link_libraries(ost dcam imageproc stereoproc imwin fltk fltk_gl glut)
target_link_libraries(v3d imwin fltk fltk_gl glut)
rospack_add_compile_flags(v3d -msse3)
\ No newline at end of file
Modified: pkg/trunk/deprecated/dcam/src/ost/ost.cpp
===================================================================
--- pkg/trunk/deprecated/dcam/src/ost/ost.cpp 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/deprecated/dcam/src/ost/ost.cpp 2009-08-31 06:50:29 UTC (rev 23363)
@@ -111,7 +111,7 @@
bool isRefresh; // refresh the current stereo image display
// plane object and parameters
-FindPlanes planeObj;
+//FindPlanes planeObj;
float planeParams[4];
float planeThresh = 0.01; // 1 cm inlier threshold
@@ -456,6 +456,7 @@
isPlanar = false;
if (isPlanar)
{
+#if 0
int n,nn;
tt0 = get_ms();
// get first largest plane
@@ -470,6 +471,7 @@
// printf("Planar time: %d ms\n", (int)(tt1-tt0));
printf("Found plane %f %f %f %f with %d/%d inliers\n",
planeParams[0], planeParams[1], planeParams[2], planeParams[3], n, nn);
+#endif
}
// check for 3D window
Added: pkg/trunk/stacks/imaging_pipeline/image_proc/include/cam_bridge.h
===================================================================
--- pkg/trunk/stacks/imaging_pipeline/image_proc/include/cam_bridge.h (rev 0)
+++ pkg/trunk/stacks/imaging_pipeline/image_proc/include/cam_bridge.h 2009-08-31 06:50:29 UTC (rev 23363)
@@ -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
Modified: pkg/trunk/stacks/imaging_pipeline/image_proc/include/image.h
===================================================================
--- pkg/trunk/stacks/imaging_pipeline/image_proc/include/image.h 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/stacks/imaging_pipeline/image_proc/include/image.h 2009-08-31 06:50:29 UTC (rev 23363)
@@ -62,10 +62,8 @@
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
-#include <sensor_msgs/fill_image.h>
-#include <sensor_msgs/CameraInfo.h>
+//#include <sensor_msgs/fill_image.h>
-
// alignment on allocation
#ifdef __APPLE__
#define MEMALIGN(x) malloc(x)
@@ -250,265 +248,8 @@
int width, int height, color_conversion_t colorAlg);
};
-
-#if 0
- // stereo data structure
-
- class StereoData
- {
- public:
- StereoData();
- ~StereoData();
-
- // image parameters
- int imWidth;
- int imHeight;
- void setSize(int width, int height); // sets individual image sizes too
-
- // left and right image data
- ImageData *imLeft;
- ImageData *imRight;
-
- // rectification
- bool hasRectification;
-
- // disparity data
- int16_t *imDisp; // disparity image, negative and zero are invalid pixels
- size_t imDispSize; // size of image in bytes
- int dpp; // disparity units per pixel, e.g., 16 is 1/16 pixel per disparity
- bool hasDisparity; // true if disparity present
- int numDisp; // number of search disparities, in pixels
- int offx; // x offset of disparity search
-
- bool setHoropter(int offset); // set horopter offset
- bool setNumDisp(int ndisp); // set number of disparities
-
- // Color conversion:
- void doBayerColorRGB();
- void doBayerMono();
-
- // disparity and rectification functions
- bool doRectify(); // rectify images
- bool doDisparity(stereo_algorithm_t alg=NORMAL_ALGORITHM); // calculate disparity image
- bool doSpeckle(); // speckle filter post-processing, automatically applied by doDisparity
- bool doCalcPts(bool isArray = false); // calculate 3D points
- bool calcPt(int x, int y, float *fx, float *fy, float *fz); // single point
- bool setRangeMax(double thresh);
- bool setRangeMin(double thresh);
-
- // valid stereo data rectangle
- int imDtop, imDleft;
- int imDwidth, imDheight;
- void setDispOffsets(); // reset them, based on stereo processing params
-
- // point cloud data
- // NOTE: imPts buffer should be 16-byte aligned
- // imPts elements will have the form of a pt_xyza_t for a pt array
- float *imPts; // points, 3xN floats for vector version, 4xN for array version
- // for isPtArray = true, these next two are not needed
- int *imCoords; // image coordinates of the point cloud points, 2xN ints
- uint8_t *imPtsColor; // color vector corresponding to points, RGB
- size_t imPtsSize; // size of array in bytes, for storage manipulation
- int numPts; // number of points in array
- bool isPtArray; // true if the points are an image array, z=0.0 for no point
- pt_xyza_t *imPtArray() { return (pt_xyza_t *)imPts; }
-
- // external parameters for undistorted images
- double T[3]; // pose of right camera in left camera coords
- double Om[3]; // rotation vector
-
- // reprojection matrix
- double RP[16];
-
- // buffers
- void releaseBuffers(); // get rid of all buffers
-
- // parameters
- void extractParams(char *params, bool store = false); // extracts params from string and puts in vars
- // optionally stores into image object
- char *createParams(bool store = false); // takes parameters and puts them into a string
- // optionally stores into image object
-
- // stereo processing params
- int corrSize; // correlation window size, assumed square
- int filterSize; // size of prefilter window, assumed square (0 if none)
- int horOffset; // horopter offset
-
- // filter thresholds
- int textureThresh; // percent
- int uniqueThresh; // percent
- int smoothThresh; // percent
- int speckleDiff; // max difference between adjacent disparities in a region
- int speckleRegionSize; // minimum size of region to be not a speckle
- double rangeMax; // max Z value returned in pt cloud
- double rangeMin; // max Z value returned in pt cloud
- bool unique_check;
-
- bool setTextureThresh(int thresh);
- bool setUniqueThresh(int thresh);
- bool setSmoothnessThresh(int thresh);
- bool setSpeckleDiff(int diff);
- bool setSpeckleRegionSize(int size);
- bool setCorrSize(int size);
- bool setUniqueCheck(bool val);
-
- // buffers for stereo
- uint8_t *buf, *flim, *frim;
- int maxxim, maxyim, maxdlen, maxcorr; // for changing buffer sizes
-
- private:
- // buffers for speckle filter
- uint8_t *rbuf;
- uint32_t *lbuf, *wbuf;
-
- };
-
-
- //
- // Plane finding class
- //
-
- class FindPlanes
- {
- public:
- FindPlanes();
- ~FindPlanes();
-
- // put points
- // skip interval is decimation for speed
- void SetPointCloud(pt_xyza_t *pts, int n, int skip=1);
-
- // find a plane, return its parameters and (decimated) inlier count
- int FindPlane(float *pparams, float thresh, int tries);
-
- // set all plane inliers to an index, return number found
- // also resets decimated point cloud
- int IndexPlane(int ind, float thresh, float *pparams);
-
- private:
- pt_xyza_t *pts3d; // input vector of points
- int n_pts3d; // number of points
- pt_xyza_t *pts3d_dec; // decimated points
- int n_pts3d_dec;
- };
-#endif
-
}
-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 // IMAGE_H
Modified: pkg/trunk/stacks/imaging_pipeline/image_proc/src/nodes/image_proc.cpp
===================================================================
--- pkg/trunk/stacks/imaging_pipeline/image_proc/src/nodes/image_proc.cpp 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/stacks/imaging_pipeline/image_proc/src/nodes/image_proc.cpp 2009-08-31 06:50:29 UTC (rev 23363)
@@ -39,12 +39,12 @@
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/fill_image.h>
-#include <sensor_msgs/CameraInfo.h>
#include <opencv_latest/CvBridge.h>
#include <image_transport/image_publisher.h>
#include "image.h"
+#include "cam_bridge.h"
#include <boost/thread.hpp>
Modified: pkg/trunk/stacks/imaging_pipeline/image_proc/src/proc/image.cpp
===================================================================
--- pkg/trunk/stacks/imaging_pipeline/image_proc/src/proc/image.cpp 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/stacks/imaging_pipeline/image_proc/src/proc/image.cpp 2009-08-31 06:50:29 UTC (rev 23363)
@@ -1987,3 +1987,5 @@
}
}
+
+
Modified: pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/CMakeLists.txt 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/CMakeLists.txt 2009-08-31 06:50:29 UTC (rev 23363)
@@ -10,14 +10,13 @@
rospack_add_boost_directories()
-rospack_add_library(stereoproc src/image.cpp src/stereolib.c src/stereolib2.cpp src/findplane.cpp)
+rospack_add_library(stereoproc src/proc/stereoimage.cpp src/proc/stereolib.c src/proc/stereolib2.cpp)
rospack_add_compile_flags(stereoproc "-msse2 -mpreferred-stack-boundary=4")
-rospack_add_executable(stereoproc_exe src/stereoproc.cpp)
-rospack_add_compile_flags(stereoproc_exe "-msse2 -mpreferred-stack-boundary=4")
-target_link_libraries(stereoproc_exe stereoproc)
+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_executable(imageproc src/imageproc.cpp)
-rospack_add_compile_flags(imageproc "-msse2 -mpreferred-stack-boundary=4")
-target_link_libraries(imageproc stereoproc)
+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)
Deleted: pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/cam_bridge.h
===================================================================
--- pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/cam_bridge.h 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/cam_bridge.h 2009-08-31 06:50:29 UTC (rev 23363)
@@ -1,290 +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 "image.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 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 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 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
Added: pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/cam_bridge_old.h
===================================================================
--- pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/cam_bridge_old.h (rev 0)
+++ pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/cam_bridge_old.h 2009-08-31 06:50:29 UTC (rev 23363)
@@ -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 "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
Deleted: pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/image.h
===================================================================
--- pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/image.h 2009-08-31 04:42:23 UTC (rev 23362)
+++ pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/include/image.h 2009-08-31 06:50:29 UTC (rev 23363)
@@ -1,394 +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 "stereolib.h"
-
-
-// version of parameter files
-#define OST_MAJORVERSION 5
-#define OST_MINORVERSION 0
-
-
-// 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...
[truncated message content] |
