Revision: 7092 http://playerstage.svn.sourceforge.net/playerstage/?rev=7092&view=rev Author: veedee Date: 2008-10-10 16:15:16 +0000 (Fri, 10 Oct 2008)
Log Message: ----------- added driver for Videre Design STOC (Stereo on a Chip) cameras Modified Paths: -------------- code/player/trunk/server/drivers/CMakeLists.txt Added Paths: ----------- code/player/trunk/server/drivers/stereo/ code/player/trunk/server/drivers/stereo/CMakeLists.txt code/player/trunk/server/drivers/stereo/stoc.cc Modified: code/player/trunk/server/drivers/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/CMakeLists.txt 2008-10-10 12:33:26 UTC (rev 7091) +++ code/player/trunk/server/drivers/CMakeLists.txt 2008-10-10 16:15:16 UTC (rev 7092) @@ -29,6 +29,7 @@ ADD_SUBDIRECTORY (shell) ADD_SUBDIRECTORY (sonar) ADD_SUBDIRECTORY (speech) +ADD_SUBDIRECTORY (stereo) ADD_SUBDIRECTORY (vectormap) # ADD_SUBDIRECTORY (waveform) ADD_SUBDIRECTORY (wifi) Added: code/player/trunk/server/drivers/stereo/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/stereo/CMakeLists.txt (rev 0) +++ code/player/trunk/server/drivers/stereo/CMakeLists.txt 2008-10-10 16:15:16 UTC (rev 7092) @@ -0,0 +1,2 @@ +PLAYERDRIVER_OPTION (stoc build_stoc ON) +PLAYERDRIVER_ADD_DRIVER (stoc build_stoc LINKFLAGS "-lsvscap -lsvs -lsvscalc -lpthread -ldcap -lraw1394" SOURCES stoc.cc) Added: code/player/trunk/server/drivers/stereo/stoc.cc =================================================================== --- code/player/trunk/server/drivers/stereo/stoc.cc (rev 0) +++ code/player/trunk/server/drivers/stereo/stoc.cc 2008-10-10 16:15:16 UTC (rev 7092) @@ -0,0 +1,710 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2006 Radu Bogdan Rusu ([EMAIL PROTECTED]) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_stoc stoc + * @brief STOC + +The stoc driver controls Videre Design's STOC (Stereo on a Chip) cameras +through the SVS (Small Vision System) library. The driver provides a @ref +interface_stereo interface. + [EMAIL PROTECTED] Compile-time dependencies + +- none + [EMAIL PROTECTED] Provides + +- @ref interface_stereo : left and right images, disparity image, and a 3d + point cloud generated from the disparity + [EMAIL PROTECTED] Requires + +- none + [EMAIL PROTECTED] Supported configuration requests + + - none + [EMAIL PROTECTED] Properties provided + + -none yet + [EMAIL PROTECTED] Configuration file options + + - capture_type (integer) + - Default: SVS defaults + - CAP_DUAL or CAP_INTERLACE + + - format (integer) + - Default: SVS defaults + - MONOCHROME, YUV, or RGB24 + + - channel (integer) + - Default: SVS defaults + - 0, 1, 2 etc, video channel on card + + - swap (boolean) + - Default: SVS defaults + - Swapping left/right on or off + + - color_mode (integer) + - Default: SVS defaults + - Color mode: 0 (both), 1 (left only), 2 (right only) + + - color_alg (integer) + - Default: SVS defaults + - Color algorithm (see SVS manual) + + - rectification (boolean) + - Default: SVS defaults + - Image rectification: enabled (1) / disabled (0) + + - proc_mode (integer) + - Default: SVS defaults + - Processing mode for the STOC + + - rate (integer) + - Default: SVS defaults + - Image rate + + - frame_div (integer) + - Default: SVS defaults + - Image sampling + + - image_size (integer typle) + - Default: 640 480 + - Image width and height + + - z_max (float) + - Default: 5 (meters) + - Cutoff distance on Z-axis + + - ndisp (integer) + - Default: SVS defaults + - Set number of disparities, 8 to 128 + + - tex_thresh (integer) + - Default: SVS defaults + - Set texture filter threshold + + - unique (integer) + - Default: SVS defaults + - Set uniqueness filter threshold + + - corrsize (integer) + - Default: SVS defaults + - Set correlation window size, 7 to 21 + + - horopter (integer) + - Default: SVS defaults + - Set horopter (X offset) + + - speckle_size (integer) + - Default: SVS defaults + - Set minimum disparity region size + + - speckle_diff (integer) + - Default: SVS defaults + - Set disparity region neighbor difference + + - cut_di (integer) + - Default: 0 + - Number of lines to "cut" (disconsider) at the bottom of the disparity image + + - multiproc_en (boolean) + - Default: 0 + - Use SVS multiprocessing capabilities (see SVS manual) + [EMAIL PROTECTED] Example + [EMAIL PROTECTED] +driver +( + name "stoc" + provides ["stereo:0"] + + ## ---[ Misc camera parameters: all -1 values assume SVS defaults + # Color mode: 0 (both), 1 (left only), 2 (right only) + color_mode 0 + # Color algorithm: COLOR_ALG_BEST (2), or COLOR_ALG_FAST (0) - check SVS manual + color_alg 2 + + # Processing mode for STOC: PROC_MODE_OFF (0), PROC_MODE_NONE (1), PROC_MODE_TEST (2), + # PROC_MODE_RECTIFIED (3), PROC_MODE_DISPARITY (4), PROC_MODE_DISPARITY_RAW (5) + # Check your STOC camera manual for extra details. + proc_mode 5 + + # Image rate in fps + rate 15 + + # Image rectification: enabled (1) / disabled (0) + rectification 1 + + ## ---[ Stereo parameters: all -1 values assume SVS defaults + # Points to disconsider at the bottom of the disparity image (due to errors in SVS?) + cut_di 32 + + # Number of disparities, 8 to 128 + ndisp 64 + + # Texture filter threshold (confidence) + tex_thresh 4 + + # Uniqueness filter threshold + unique 3 + + # Correlation window size, 7 to 21 + corrsize 15 + + # Minimum disparity region size + speckle_size 400 +) [EMAIL PROTECTED] + [EMAIL PROTECTED] Radu Bogdan Rusu + */ +/** @} */ + +#include <stdlib.h> +#include <libplayercore/playercore.h> +#include <SVS/svsclass.h> + +#define IMAGE_WIDTH 640 +#define IMAGE_HEIGHT 480 + +#define CUTOFF_DIST 5 + +class STOC:public Driver +{ + public: + // constructor + STOC (ConfigFile* cf, int section); + ~STOC (); + + int Setup (); + int Shutdown (); + + // MessageHandler + virtual int ProcessMessage (QueuePointer &resp_queue, + player_msghdr * hdr, + void * data); + + private: + + virtual void Main (); + void RefreshData (); + + // device bookkeeping + player_devaddr_t stereo_addr; + player_stereo_data_t stereo_data; + + int capture_type, format, channel; + bool swap_mode; + int color_mode, color_alg; + int proc_mode, rate, frame_div, size_w, size_h; + bool rectification, multiproc_en; + float z_max; + + int ndisp, tex_thresh, unique, corrsize, horopter, speckle_size, speckle_diff; + int cut_di; + + const char *parameter_file; // user given parameter file for the camera + // SVS objects + svsVideoImages *video; + svsStereoProcess *process; + svsMultiProcess *multiproc; + + protected: + // Properties + IntProperty exposure, balance, gamma, brightness, saturation; + +}; + +//////////////////////////////////////////////////////////////////////////////// +//Factory creation function. This functions is given as an argument when +// the driver is added to the driver table +Driver* + STOC_Init (ConfigFile* cf, int section) +{ + return ((Driver*)(new STOC (cf, section))); +} + +//////////////////////////////////////////////////////////////////////////////// +//Registers the driver in the driver table. Called from the +// player_driver_init function that the loader looks for +void + stoc_Register (DriverTable* table) +{ + table->AddDriver ("stoc", STOC_Init); +} + +//////////////////////////////////////////////////////////////////////////////// +// Constructor. Retrieve options from the configuration file and do any +// pre-Setup() setup. +STOC::STOC (ConfigFile* cf, int section) + : Driver (cf, section), + exposure ("exposure", 0, 0), + balance ("balance", 0, 0), + gamma ("gamma", 0, 0), + brightness ("brightness", 0, 0), + saturation ("saturation", 0, 0) +{ + memset (&this->stereo_addr, 0, sizeof (player_devaddr_t)); + + this->RegisterProperty ("exposure", &this->exposure, cf, section); + this->RegisterProperty ("balance", &this->balance, cf, section); + this->RegisterProperty ("gamma", &this->gamma, cf, section); + this->RegisterProperty ("brightness", &this->brightness, cf, section); + this->RegisterProperty ("saturation", &this->saturation, cf, section); + + // Check whether the user provided a parameter file for the STOC + parameter_file = cf->ReadString (section, "param_file", NULL); + + // SVS camera parameters + capture_type = cf->ReadInt (section, "capture_type", -1); // CAP_DUAL or CAP_INTERLACE + format = cf->ReadInt (section, "format", -1); // MONOCHROME, YUV, or RGB24 + channel = cf->ReadInt (section, "channel", -1); // 0, 1, 2 etc, video channel on card + swap_mode = cf->ReadBool (section, "swap", false); // Swapping left/right on or off + color_mode = cf->ReadInt (section, "color_mode", -1); // Color mode: 0 (both), 1 (left only), 2 (right only) + color_alg = cf->ReadInt (section, "color_alg", -1); // Color algorithm (see SVS manual) + + rectification = cf->ReadBool (section, "rectification", -1); // Image rectification: enabled (1) / disabled (0) + proc_mode = cf->ReadInt (section, "proc_mode", -1); // Processing mode for STOC + rate = cf->ReadInt (section, "rate", -1); // Image rate + frame_div = cf->ReadInt (section, "frame_div", -1); // Image sampling + size_w = cf->ReadTupleInt (section, "image_size", 0, IMAGE_WIDTH); // Image width + size_h = cf->ReadTupleInt (section, "image_size", 1, IMAGE_HEIGHT); // Image height + + // Stereo parameters + z_max = cf->ReadFloat (section, "z_max", CUTOFF_DIST); // Cutoff distance on Z-axis + + ndisp = cf->ReadInt (section, "ndisp", -1); // Set number of disparities, 8 to 128 + tex_thresh = cf->ReadInt (section, "tex_thresh", -1); // Set texture filter threshold + unique = cf->ReadInt (section, "unique", -1); // Set uniqueness filter threshold + corrsize = cf->ReadInt (section, "corrsize", -1); // Set correlation window size, 7 to 21 + horopter = cf->ReadInt (section, "horopter", -1); // Set horopter (X offset) + speckle_size = cf->ReadInt (section, "speckle_size", -1); // Set minimum disparity region size + speckle_diff = cf->ReadInt (section, "speckle_dif", -1); // Set disparity region neighbor difference + + cut_di = cf->ReadInt (section, "cut_di", 0); // Cut points at the bottom of the disparity image + + multiproc_en = cf->ReadBool (section, "multiproc", 0); // SVS multiproc (check manual): enabled (1) / disabled (0) + + if (cf->ReadDeviceAddr (&(this->stereo_addr), section, "provides", PLAYER_STEREO_CODE, -1, NULL) == 0) + { + if (this->AddInterface (this->stereo_addr) != 0) + { + this->SetError (-1); + return; + } + } + + process = new svsStereoProcess (); // Compute the disparity image, and 3D points + multiproc = new svsMultiProcess (); // Multiscale processing + video = getVideoObject (); // Get access to the video stream +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor. +STOC::~STOC () +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Set up the device. Return 0 if things go well, and -1 otherwise. +int + STOC::Setup () +{ + int res; + + int nr_cam = video->Enumerate (); // Get the number of cameras available + + // We only support one camera for now + if (nr_cam == 0) + { + PLAYER_ERROR ("No camera found!"); + return (-1); + } + + for (int i = 0; i < nr_cam; i++) + PLAYER_MSG2 (0, "> Found camera %d: %s", i, video->DeviceIDs ()[i]); + + res = video->Open (nr_cam - 1); // Open camera + + if (!res) + { + PLAYER_ERROR ("Could not connect to camera!"); + return (-1); + } + + /// ---[ Set camera options + if (parameter_file != NULL) + { + video->ReadParams ((char*)parameter_file); + PLAYER_MSG1 (1, ">> Using camera parameters from %s", parameter_file); + } + else + { + if (capture_type != -1) + { + if (video->SetCapture (capture_type)) + PLAYER_MSG1 (2, ">> Set capture type to %d", capture_type); + else + PLAYER_ERROR1 (">> Error while setting capture type to %d!", capture_type); + } + + if (format != -1) + { + if (video->SetFormat (format)) + PLAYER_MSG1 (2, ">> Set format type to %d", format); + else + PLAYER_ERROR1 (">> Error setting format type to %d!", format); + } + + if (channel != -1) + { + if (video->SetChannel (channel)) + PLAYER_MSG1 (2, ">> Set channel type to %d", channel); + else + PLAYER_ERROR1 (">> Error setting channel type to %d!", channel); + } + + if (swap_mode) + { + if (video->SetSwap (swap_mode)) + PLAYER_MSG0 (2, ">> Swap mode enabled"); + else + PLAYER_ERROR (">> Error enabling swap mode!"); + } + + if (color_mode != -1) + { + bool r; + if (color_mode == 0) r = video->SetColor (true, true); + if (color_mode == 1) r = video->SetColor (true, false); + if (color_mode == 2) r = video->SetColor (false, true); + if (r) + PLAYER_MSG1 (2, ">> Color mode set to %d", color_mode); + else + PLAYER_ERROR (">> Error setting color mode to %d!"); + } + + if (color_alg != -1) + { + if (video->SetColorAlg (color_alg)) + PLAYER_MSG1 (2, ">> Color algorithm set to %d", color_alg); + else + PLAYER_ERROR1 (">> Error setting color algorithm to %d!", color_alg); + } + + if (video->SetSize (size_w, size_h)) + PLAYER_MSG2 (2, ">> Image size set to %dx%d", size_w, size_h); + else + PLAYER_ERROR2 (">> Error setting image size to %dx%d!", size_w, size_h); + + if (frame_div != -1) + { + if (video->SetFrameDiv (frame_div)) + PLAYER_MSG1 (2, ">> Image sampling set to %d", frame_div); + else + PLAYER_ERROR1 (">> Error setting image sampling to %d!", frame_div); + } + + if (rate != -1) + { + if (video->SetRate (rate)) + PLAYER_MSG1 (2, ">> Image rate set to %d", rate); + else + PLAYER_ERROR1 (">> Error setting image rate to %d!", rate); + } + + if (proc_mode != -1) + { + if (video->SetProcMode (proc_mode_type(proc_mode))) + PLAYER_MSG1 (2, ">> STOC processing mode set to %d", proc_mode); + else + PLAYER_ERROR1 (">> Error setting STOC processing mode to %d!", proc_mode); + } + + if (rectification != -1) + { + if (video->SetRect (rectification)) + PLAYER_MSG1 (2, ">> Image rectification set to %d", rectification); + else + PLAYER_ERROR1 (">> Error setting image rectification to %d!", rectification); + } + } + video->binning = 1; + + /// ---[ Stereo options + if (cut_di != 0) + PLAYER_MSG1 (2, ">> [stereo] Disconsidering the last %d lines from the bottom of the disparity image...", cut_di); + if (ndisp != -1) + { + video->SetNDisp (ndisp); + PLAYER_MSG1 (2, ">> [stereo] Number of disparities set to %d", ndisp); + } + if (tex_thresh != -1) + { + video->SetThresh (tex_thresh); + PLAYER_MSG1 (2, ">> [stereo] Texture filter threshold set to %d", tex_thresh); + } + if (unique != -1) + { + video->SetUnique (unique); + PLAYER_MSG1 (2, ">> [stereo] Uniqueness filter threshold set to %d", unique); + } + if (corrsize != -1) + { + video->SetCorrsize (corrsize); + PLAYER_MSG1 (2, ">> [stereo] Correlation window size set to %d", corrsize); + } + if (horopter != -1) + { + video->SetHoropter (horopter); + PLAYER_MSG1 (2, ">> [stereo] Horopter (X-Offset) value set to %d", horopter); + } + if (speckle_size != -1) + { + video->SetSpeckleSize (speckle_size); + PLAYER_MSG1 (2, ">> [stereo] Minimum disparity region size set to %d", speckle_size); + } + if (speckle_diff != -1) + { + video->SetSpeckleDiff (speckle_diff); + PLAYER_MSG1 (2, ">> [stereo] Disparity region neighbor diff to %d", speckle_diff); + } + + PLAYER_MSG0 (0, "> Connected to camera"); + + // Start video streaming + res = video->Start (); + + StartThread (); + return (0); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Shutdown the device +int + STOC::Shutdown () +{ + StopThread (); + + int res = video->Stop (); // Stop video streaming + res = video->Close (); // Close camera + PLAYER_MSG0 (1, "> Closed camera connection."); + return (0); +} + +//////////////////////////////////////////////////////////////////////////////// +// ProcessMessage +int + STOC::ProcessMessage (QueuePointer &resp_queue, + player_msghdr * hdr, + void * data) +{ + assert (hdr); + assert (data); + + // To do: offset, exposure, autoexposure parameters, balance, gamma, brightness, saturation + // level and horopter + + return (0); +} + + + +//////////////////////////////////////////////////////////////////////////////// +void + STOC::Main () +{ + for (;;) + { + // handle commands and requests/replies -------------------------------- + pthread_testcancel (); + ProcessMessages (); + RefreshData (); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// RefreshData function +void + STOC::RefreshData () +{ + int res; + + svsStereoImage *si = video->GetImage (10); // 10 ms timeout + if (si == NULL) + PLAYER_WARN ("No image, timed out..."); + + // Compute the disparity image + if (multiproc_en) + multiproc->CalcStereo (si); + else + process->CalcStereo (si); + + // Trim down those nasty points at the bottom of the disparity image (errors?) + for (int i = si->ip.height - cut_di; i < si->ip.height; i++) + for (int j = 0; j < si->ip.width; j++) + si->disparity[i * si->ip.width + j] = -2; + + // Compute the 3D point cloud information + if (multiproc_en) + multiproc->Calc3D (si, 0, 0, 0, 0, NULL, NULL, z_max); + else + process->Calc3D (si, 0, 0, 0, 0, NULL, NULL, z_max); + + // Save the 3D point cloud data in the structure, if present + int nr_points = 0; + stereo_data.points = NULL; + if (si->have3D) + { + stereo_data.points = new player_pointcloud3d_stereo_element_t[si->ip.height * si->ip.width]; + + svs3Dpoint *pts = si->pts3D; + for (int i = 0; i < si->ip.height; i++) + { + for (int j = 0; j < si->ip.width; j++, pts++) + { + // Check if the point is valid + if (pts->A <= 0) + continue; + + player_pointcloud3d_stereo_element_t point; + point.px = pts->X; + point.py = pts->Y; + point.pz = pts->Z; + + if (si->haveColor) + { + svsColorPixel *mpc = (svsColorPixel*)(si->color + (i*si->ip.width + j) * 4); + point.red = mpc->r; // red + point.green = mpc->g; // green + point.blue = mpc->b; // blue + } + else + { + point.red = point.green = point.blue = (unsigned char)si->Left ()[i*si->ip.width + j]; + } + stereo_data.points[nr_points] = point; + nr_points++; + } // width + } // height + + } // have3D + stereo_data.points_count = nr_points; + + // Save the stereo left/right image + stereo_data.left_channel.height = stereo_data.right_channel.height = si->ip.height; + stereo_data.left_channel.width = stereo_data.right_channel.width = si->ip.width; + stereo_data.left_channel.fdiv = stereo_data.right_channel.fdiv = frame_div; + stereo_data.left_channel.compression = stereo_data.right_channel.compression = PLAYER_CAMERA_COMPRESS_RAW; + stereo_data.left_channel.image_count = stereo_data.left_channel.width * stereo_data.left_channel.height; + stereo_data.right_channel.image_count = stereo_data.left_channel.width * stereo_data.left_channel.height; + + // Set MONO by default on both left and right + stereo_data.left_channel.format = PLAYER_CAMERA_FORMAT_MONO8; + stereo_data.left_channel.bpp = 8; + stereo_data.left_channel.image = si->Left (); + stereo_data.right_channel.format = PLAYER_CAMERA_FORMAT_MONO8; + stereo_data.right_channel.bpp = 8; + stereo_data.right_channel.image = si->Right (); + + uint8_t *in_left, *in_right, *out_left, *out_right; + // Check if <left> has color or monochrome + if (si->haveColor) + { + stereo_data.left_channel.format = PLAYER_CAMERA_FORMAT_RGB888; + stereo_data.left_channel.bpp = 24; + stereo_data.left_channel.image_count *= 3; + in_left = (uint8_t*)si->Color (); + stereo_data.left_channel.image = new unsigned char [stereo_data.left_channel.image_count]; + out_left = (uint8_t*)stereo_data.left_channel.image; + } + + // Check if <right> has color or monochrome + if (si->haveColorRight) + { + stereo_data.right_channel.format = PLAYER_CAMERA_FORMAT_RGB888; + stereo_data.right_channel.bpp = 24; + stereo_data.right_channel.image_count *= 3; + in_right = (uint8_t*)si->ColorRight (); + stereo_data.right_channel.image = new unsigned char [stereo_data.right_channel.image_count]; + out_right = (uint8_t*)stereo_data.right_channel.image; + } + + for (int i = 0; i < (si->ip.height * si->ip.width); i++) + { + if (si->haveColor) + { + // Left + out_left[0] = in_left[0]; + out_left[1] = in_left[1]; + out_left[2] = in_left[2]; + out_left += 3; + in_left += 4; + } + if (si->haveColorRight) + { + // Right + out_right[0] = in_right[0]; + out_right[1] = in_right[1]; + out_right[2] = in_right[2]; + out_right += 3; + in_right += 4; + } + } + + // Save the disparity image + stereo_data.disparity.width = stereo_data.disparity.height = 0; + stereo_data.disparity.bpp = 16; + stereo_data.disparity.fdiv = 1; + stereo_data.disparity.compression = PLAYER_CAMERA_COMPRESS_RAW; + stereo_data.disparity.format = PLAYER_CAMERA_FORMAT_MONO16; + + if (si->haveDisparity) + { + stereo_data.disparity.width = si->dp.dwidth; + stereo_data.disparity.height = si->dp.dheight; + stereo_data.disparity.image = (uint8_t*)si->disparity; + } + stereo_data.disparity.image_count = stereo_data.disparity.width * stereo_data.disparity.height * 2; + + // Stereo data mode + stereo_data.mode = 1 + 2 + 4; // left + right + disparity + + // Publish the stereo data + Publish (stereo_addr, PLAYER_MSGTYPE_DATA, PLAYER_STEREO_DATA_STATE, (void*)&stereo_data); + + // Cleanup + if (stereo_data.points != NULL) + delete [] stereo_data.points; + if (si->haveColor) + delete [] stereo_data.left_channel.image; + if (si->haveColorRight) + delete [] stereo_data.right_channel.image; + + return; +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------- This SF.Net email is sponsored by the Moblin Your Move Developer's challenge Build the coolest Linux based applications with Moblin SDK & win great prizes Grand prize is a trip for two to an Open Source event anywhere in the world http://moblin-contest.org/redirect.php?banner_id=100&url=/ _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit