Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8875] pkg/trunk
|
From: <jl...@us...> - 2009-01-06 09:05:50
|
Revision: 8875
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8875&view=rev
Author: jleibs
Date: 2009-01-06 09:05:46 +0000 (Tue, 06 Jan 2009)
Log Message:
-----------
Overhaul of assorted stereo relating functions due to move of stereoinfo into disparity info and change from byte to uint8.
Modified Paths:
--------------
pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
pkg/trunk/image_msgs/include/image_msgs/FillImage.h
pkg/trunk/image_msgs/msg/CamInfo.msg
pkg/trunk/image_msgs/msg/Image.msg
pkg/trunk/image_msgs/msg/RawStereo.msg
pkg/trunk/image_msgs/msg/StereoInfo.msg
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/people/src/track_starter_gui.cpp
pkg/trunk/vision/stereo_capture/capture.launch
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_image_proc/include/cam_bridge.h
pkg/trunk/vision/stereo_image_proc/src/image.cpp
pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
pkg/trunk/vision/stereo_view/stereo_view.cpp
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
Added Paths:
-----------
pkg/trunk/drivers/cam/dcam/videre.launch
pkg/trunk/image_msgs/msg/DisparityInfo.msg
Modified: pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -33,7 +33,7 @@
*********************************************************************/
#include "ros/node.h"
-#include "std_srvs/Empty.h"
+#include "std_msgs/Empty.h"
#include <string>
@@ -42,9 +42,15 @@
public:
CheckParams() : ros::node("param_checker")
{
- std_srvs::Empty::request req;
- std_srvs::Empty::response res;
- ros::service::call("stereodcam/check_params", req, res);
+ advertise<std_msgs::Empty>("stereodcam/check_params");
+
+ usleep(100000);
+
+ std_msgs::Empty e;
+
+ publish("stereodcam/check_params", e);
+
+ usleep(100000);
}
};
Modified: pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -198,80 +198,62 @@
{
fillImage(img_, "image_raw",
img_data->imHeight, img_data->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
img_data->imRaw );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_raw"), img_);
- cam_info_.has_image = true;
- } else {
- cam_info_.has_image = false;
}
if (img_data->imType != COLOR_CODING_NONE)
{
fillImage(img_, "image",
img_data->imHeight, img_data->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
img_data->im );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image"), img_);
- cam_info_.has_image = true;
- } else {
- cam_info_.has_image = false;
}
if (img_data->imColorType != COLOR_CODING_NONE && img_data->imColorType == COLOR_CODING_RGB8)
{
fillImage(img_, "image_color",
img_data->imHeight, img_data->imWidth, 3,
- "rgba", "byte",
+ "rgba", "uint8",
img_data->imColor );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_color"), img_);
- cam_info_.has_image_color = true;
- } else {
- cam_info_.has_image_color = false;
}
if (img_data->imRectType != COLOR_CODING_NONE)
{
fillImage(img_, "image_rect",
img_data->imHeight, img_data->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
img_data->imRect );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_rect"), img_);
- cam_info_.has_image_rect = true;
- } else {
- cam_info_.has_image_rect = false;
}
if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGB8)
{
fillImage(img_, "image_rect_color",
img_data->imHeight, img_data->imWidth, 3,
- "rgb", "byte",
+ "rgb", "uint8",
img_data->imRectColor );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_rect_color"), img_);
- cam_info_.has_image_rect_color = true;
- } else {
- cam_info_.has_image_rect_color = false;
}
if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGBA8)
{
fillImage(img_, "image_rect_color",
img_data->imHeight, img_data->imWidth, 4,
- "rgba", "byte",
+ "rgba", "uint8",
img_data->imRectColor );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_rect_color"), img_);
- cam_info_.has_image_rect_color = true;
- } else {
- cam_info_.has_image_rect_color = false;
}
cam_info_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
Modified: pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -38,26 +38,91 @@
@htmlinclude manifest.html
-stereodcam node uses the parameters:
+Stereodcam is a driver primarily for communicating with the Videre stereocameras.
-- @b ~exposure (int)
-- @b ~gain (int)
-- @b ~brightness (int)
-- @b ~exposure_auto (bool)
-- @b ~gain_auto (bool)
-- @b ~brightness_auto (bool)
-- @b ~companding (bool)
-- @b ~hdr (bool)
-- @b ~unique_check (bool)
-- @b ~texture_thresh (int)
-- @b ~unique_thresh (int)
-- @b ~smoothness_thresh (int)
-- @b ~horopter (int)
-- @b ~speckle_size (int)
-- @b ~speckle_diff (int)
-- @b ~corr_size (int)
-- @b ~num_disp (int)
+<hr>
+@section behavior Behavior
+
+The Stereodcam node outputs a "raw_stereo" message, defined in the
+"image_msgs" package. This message may either contain a left and
+right image, or, in the event of STOC processing, will contain a left
+image and disparity image. It additionally contains the relevant
+intrinsic and extrinsic parameters for computing stereo.
+
+<hr>
+
+@section names Names
+
+The default name for the node is "stereodcam", however, this private
+namespace is not actually used internally to the node. This node
+primarily makes use of the "stereo" namespace, which it shares with
+the "stereoproc" node. This namespace is both where it looks for
+parameters and publishes topics.
+
+The "stereo" name can be remapped through standard topic remapping in
+the event that two cameras are sharing the same ROS system.
+
+<hr>
+
+@par Example
+
+Running in the stereo namespace:
+@verbatim
+$ rosrun dcam stereodcam
+@endverbatim
+
+Running in a different namespace
+@verbatim
+$ rosrun dcam stereodcam stereo:=head_cam
+@endverbatim
+
+<hr>
+
+@section topics Topics
+
+Subscribes to (name/type):
+- @b "stereo/check_params" : std_msgs/Empty : signal to recheck all of the parameters
+
+Publishes to (name : type : description):
+- @b "stereo/raw_stereo" : image_msgs/RawStereo : raw stereo information from camera
+
+<hr>
+
+@section parameters Parameters
+
+The camera will set the following parameters after running:
+- @b stereo/exposure_max (int) : maximum value for exposure
+- @b stereo/exposure_min (int) : maximum value for exposure
+- @b stereo/brightness_max (int) : maximum value for brightness
+- @b stereo/brightness_min (int) : maximum value for brightness
+- @b stereo/gain_max (int) : maximum value for gain
+- @b stereo/gain_min (int) : maximum value for gain
+
+The camera will read from the following parameters:
+- @b stereo/videre_mode (string) : The processing type that a Videre
+ STOC will use. Value can be "none", "rectified", "disparity" or
+ "disparity_raw"
+- @b stereo/fps (double) : Target fps of the camera
+- @b stereo/speed (string) : Firewire isospeed: S100, S200, or S400
+- @b stereo/exposure (int)
+- @b stereo/gain (int)
+- @b stereo/brightness (int)
+- @b stereo/exposure_auto (bool)
+- @b stereo/gain_auto (bool)
+- @b stereo/brightness_auto (bool)
+- @b stereo/companding (bool)
+- @b stereo/hdr (bool)
+- @b stereo/unique_check (bool)
+- @b stereo/texture_thresh (int)
+- @b stereo/unique_thresh (int)
+- @b stereo/smoothness_thresh (int)
+- @b stereo/horopter (int)
+- @b stereo/speckle_size (int)
+- @b stereo/speckle_diff (int)
+- @b stereo/corr_size (int)
+- @b stereo/num_disp (int)
+
**/
@@ -76,7 +141,7 @@
#include <boost/function.hpp>
#include <boost/bind.hpp>
-#include "std_srvs/Empty.h"
+#include "std_msgs/Empty.h"
using namespace std;
@@ -94,6 +159,10 @@
std::map<std::string, int> paramcache_;
+ std_msgs::Empty check_params_msg_;
+
+ std::string stereo_name_;
+
public:
dcam::StereoDcam* stcam_;
@@ -112,28 +181,30 @@
// Register a frequency status updater
diagnostic_.addUpdater( &StereoDcamNode::freqStatus );
+ stereo_name_ = map_name("stereo") + std::string("/");
+
// If there is a camera...
if (num_cams > 0)
{
// Check our gui parameter, or else use first camera
uint64_t guid;
- if (has_param("~guid"))
+ if (has_param(stereo_name_+ std::string("guid")))
{
string guid_str;
- get_param("~guid", guid_str);
+ get_param(stereo_name_ + std::string("guid"), guid_str);
guid = strtoll(guid_str.c_str(), NULL, 16);
} else {
guid = dcam::getGuid(0);
}
- param("~frame_id", frame_id_, string("stereo"));
+ param(stereo_name_ + std::string("frame_id"), frame_id_, string("stereo"));
// Get the ISO speed parameter if available
string str_speed;
dc1394speed_t speed;
- param("~speed", str_speed, string("S400"));
+ param(stereo_name_ + std::string("speed"), str_speed, string("S400"));
if (str_speed == string("S100"))
speed = DC1394_ISO_SPEED_100;
else if (str_speed == string("S200"))
@@ -144,7 +215,7 @@
// Get the FPS parameter if available;
double dbl_fps;
dc1394framerate_t fps;
- param("~fps", dbl_fps, 30.0);
+ param(stereo_name_ + std::string("fps"), dbl_fps, 30.0);
desired_freq_ = dbl_fps;
if (dbl_fps >= 240.0)
fps = DC1394_FRAMERATE_240;
@@ -169,7 +240,7 @@
// Get the videre processing mode if available:
string str_videre_mode;
videre_proc_mode_t videre_mode = PROC_MODE_NONE;
- param("~videre_mode", str_videre_mode, string("none"));
+ param(stereo_name_ + std::string("videre_mode"), str_videre_mode, string("none"));
if (str_videre_mode == string("rectified"))
videre_mode = PROC_MODE_RECTIFIED;
else if (str_videre_mode == string("disparity"))
@@ -185,14 +256,14 @@
// Fetch the camera string and send it to the parameter server if people want it (they shouldn't)
std::string params(stcam_->getParameters());
- set_param("~params", params);
+ set_param(stereo_name_ + std::string("params"), params);
- set_param("~exposure_max", (int)stcam_->expMax);
- set_param("~exposure_min", (int)stcam_->expMin);
- set_param("~gain_max", (int)stcam_->gainMax);
- set_param("~gain_min", (int)stcam_->gainMin);
- set_param("~brightness_max", (int)stcam_->brightMax);
- set_param("~brightness_min", (int)stcam_->brightMin);
+ set_param(stereo_name_ + std::string("exposure_max"), (int)stcam_->expMax);
+ set_param(stereo_name_ + std::string("exposure_min"), (int)stcam_->expMin);
+ set_param(stereo_name_ + std::string("gain_max"), (int)stcam_->gainMax);
+ set_param(stereo_name_ + std::string("gain_min"), (int)stcam_->gainMin);
+ set_param(stereo_name_ + std::string("brightness_max"), (int)stcam_->brightMax);
+ set_param(stereo_name_ + std::string("brightness_min"), (int)stcam_->brightMin);
// Configure camera
stcam_->setFormat(mode, fps, speed);
@@ -203,9 +274,10 @@
stcam_->setSpeckleDiff(10);
stcam_->setCompanding(true);
- advertise<image_msgs::RawStereo>("~raw_stereo", 1);
- advertise_service("~check_params", &StereoDcamNode::checkFeatureService);
+ advertise<image_msgs::RawStereo>(stereo_name_ + std::string("raw_stereo"), 1);
+ subscribe(stereo_name_ + std::string("check_params"), check_params_msg_, &StereoDcamNode::checkParams, 1);
+
// Start the camera
stcam_->start();
@@ -220,11 +292,9 @@
}
- bool checkFeatureService(std_srvs::Empty::request &req,
- std_srvs::Empty::response &res)
+ void checkParams()
{
checkAndSetAll();
- return true;
}
void checkAndSetAll()
@@ -247,14 +317,14 @@
void checkAndSetIntBool(std::string param_name, boost::function<void(int, bool)> setfunc)
{
- if (has_param(std::string("~") + param_name) || has_param(std::string("~") + param_name + std::string("_auto")))
+ if (has_param(stereo_name_ + param_name) || has_param(stereo_name_ + param_name + std::string("_auto")))
{
int val = 0;
bool isauto = false;
- param( std::string("~") + param_name, val, 0);
- param( std::string("~") + param_name + std::string("_auto"), isauto, false);
+ param( stereo_name_ + param_name, val, 0);
+ param( stereo_name_ + param_name + std::string("_auto"), isauto, false);
int testval = (val * (!isauto));
@@ -270,11 +340,11 @@
void checkAndSetBool(std::string param_name, boost::function<bool(bool)> setfunc)
{
- if (has_param(std::string("~") + param_name))
+ if (has_param(stereo_name_ + param_name))
{
bool on = false;
- param(std::string("~") + param_name, on, false);
+ param(stereo_name_ + param_name, on, false);
std::map<std::string, int>::iterator cacheval = paramcache_.find(param_name);
@@ -289,12 +359,12 @@
void checkAndSetInt(std::string param_name, boost::function<bool(int)> setfunc)
{
- if (has_param(std::string("~") + param_name))
+ if (has_param(stereo_name_ + param_name))
{
int val = 0;
- param(std::string("~") + param_name, val, 0);
+ param(stereo_name_ + param_name, val, 0);
std::map<std::string, int>::iterator cacheval = paramcache_.find(param_name);
@@ -331,7 +401,7 @@
}
cam_bridge::StereoDataToRawStereo(stcam_->stIm, raw_stereo_);
- publish("~raw_stereo", raw_stereo_);
+ publish(stereo_name_ + std::string("raw_stereo"), raw_stereo_);
count_++;
return true;
Added: pkg/trunk/drivers/cam/dcam/videre.launch
===================================================================
--- pkg/trunk/drivers/cam/dcam/videre.launch (rev 0)
+++ pkg/trunk/drivers/cam/dcam/videre.launch 2009-01-06 09:05:46 UTC (rev 8875)
@@ -0,0 +1,18 @@
+<launch>
+ <group ns="stereo">
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="exposure" type="int" value="450"/>
+ <param name="exposure_auto" type="bool" value="false"/>
+ <param name="brightness" type="int" value="50"/>
+ <param name="brightness_auto" type="bool" value="false"/>
+ <param name="gain" type="int" value="10"/>
+ <param name="gain_auto" type="bool" value="false"/>
+ <param name="do_colorize" type="bool" value="True"/>
+ <param name="do_rectify" type="bool" value="True"/>
+ <param name="do_stereo" type="bool" value="True"/>
+ <param name="do_calc_points" type="bool" value="True"/>
+ </group>
+ <node pkg="dcam" type="stereodcam" respawn="false"/>
+ <node pkg="stereo_image_proc" type="stereoproc" respawn="false"/>
+</launch>
+
Modified: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2009-01-06 09:05:46 UTC (rev 8875)
@@ -90,11 +90,11 @@
bool fromImage(Image& rosimg, std::string encoding = "")
{
- if (rosimg.depth == "byte")
+ if (rosimg.depth == "uint8")
{
- cvInitImageHeader(rosimg_, cvSize(rosimg.byte_data.layout.dim[1].size, rosimg.byte_data.layout.dim[0].size),
- IPL_DEPTH_8U, rosimg.byte_data.layout.dim[2].size);
- cvSetData(rosimg_, &(rosimg.byte_data.data[0]), rosimg.byte_data.layout.dim[1].stride);
+ cvInitImageHeader(rosimg_, cvSize(rosimg.uint8_data.layout.dim[1].size, rosimg.uint8_data.layout.dim[0].size),
+ IPL_DEPTH_8U, rosimg.uint8_data.layout.dim[2].size);
+ cvSetData(rosimg_, &(rosimg.uint8_data.data[0]), rosimg.uint8_data.layout.dim[1].stride);
img_ = rosimg_;
} else if (rosimg.depth == "uint16") {
cvInitImageHeader(rosimg_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size),
Modified: pkg/trunk/image_msgs/include/image_msgs/FillImage.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/FillImage.h 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/image_msgs/include/image_msgs/FillImage.h 2009-01-06 09:05:46 UTC (rev 8875)
@@ -88,8 +88,8 @@
if (height_step == 0)
height_step = height_arg * width_step;
- if (image.depth == "byte")
- fillImageHelper(image.byte_data,
+ if (image.depth == "uint8")
+ fillImageHelper(image.uint8_data,
height_arg, height_step,
width_arg, width_step,
channel_arg, channel_step,
@@ -108,16 +108,20 @@
width_arg, width_step,
channel_arg, channel_step,
data_arg);
+ else
+ {
+ return false;
+ }
return true;
}
- bool clearImage(Image& image)
+ void clearImage(Image& image)
{
image.label = "none";
image.encoding = "other";
- if (image.depth == "byte")
- clearImageHelper(image.byte_data);
+ if (image.depth == "uint8")
+ clearImageHelper(image.uint8_data);
else if (image.depth == "uint16")
clearImageHelper(image.uint16_data);
else if (image.depth == "int16")
Modified: pkg/trunk/image_msgs/msg/CamInfo.msg
===================================================================
--- pkg/trunk/image_msgs/msg/CamInfo.msg 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/image_msgs/msg/CamInfo.msg 2009-01-06 09:05:46 UTC (rev 8875)
@@ -2,9 +2,6 @@
# camera namespace and accompanied by up to 5 image topics named:
#
# image_raw, image, image_color, image_rect, and image_rect_color
-#
-# Whether these other topics are being published is defined by
-# the has_* variables within this message.
Header header
@@ -16,10 +13,4 @@
float64[9] R # rectification matrix
float64[12] P # projection/camera matrix
-byte has_image_raw
-byte has_image
-byte has_image_color
-byte has_image_rect
-byte has_image_rect_color
-
-# Should put exposure, gain, etc. information here as well
\ No newline at end of file
+# Should put exposure, gain, etc. information here as well
Added: pkg/trunk/image_msgs/msg/DisparityInfo.msg
===================================================================
--- pkg/trunk/image_msgs/msg/DisparityInfo.msg (rev 0)
+++ pkg/trunk/image_msgs/msg/DisparityInfo.msg 2009-01-06 09:05:46 UTC (rev 8875)
@@ -0,0 +1,22 @@
+# This message defines meta information for a computed disparity image
+
+Header header
+
+uint32 height
+uint32 width
+
+int32 dpp
+int32 num_disp
+int32 im_Dtop
+int32 im_Dleft
+int32 im_Dwidth
+int32 im_Dheight
+int32 corr_size
+int32 filter_size
+int32 hor_offset
+int32 texture_thresh
+int32 unique_thresh
+int32 smooth_thresh
+int32 speckle_diff
+int32 speckle_region_size
+byte unique_check
Modified: pkg/trunk/image_msgs/msg/Image.msg
===================================================================
--- pkg/trunk/image_msgs/msg/Image.msg 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/image_msgs/msg/Image.msg 2009-01-06 09:05:46 UTC (rev 8875)
@@ -14,7 +14,7 @@
# other
string depth # Specifies the depth of the data:
# Acceptable values:
- # byte, uint16, int16, uint32, int32, uint64, int64, float32, float64
+ # uint8, int8, uint16, int16, uint32, int32, uint64, int64, float32, float64
# Based on depth ONE of the following MultiArrays may contain data.
# The multi-array MUST have 3 dimensions, labeled as "height",
@@ -26,7 +26,8 @@
# Height, width, and number of channels are specified in the dimension
# sizes within the appropriate MultiArray
-std_msgs/ByteMultiArray byte_data
+std_msgs/UInt8MultiArray uint8_data
+std_msgs/Int8MultiArray int8_data
std_msgs/UInt16MultiArray uint16_data
std_msgs/Int16MultiArray int16_data
std_msgs/UInt32MultiArray uint32_data
Modified: pkg/trunk/image_msgs/msg/RawStereo.msg
===================================================================
--- pkg/trunk/image_msgs/msg/RawStereo.msg 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/image_msgs/msg/RawStereo.msg 2009-01-06 09:05:46 UTC (rev 8875)
@@ -5,19 +5,21 @@
# used. This is the preferred message to log when generating log
# files, as it is the minimal representation of the information.
-byte NONE=0
-byte IMAGE_RAW=1
-byte IMAGE=2
-byte IMAGE_COLOR=3
-byte IMAGE_RECT=4
-byte IMAGE_RECT_COLOR=5
+uint8 NONE=0
+uint8 IMAGE_RAW=1
+uint8 IMAGE=2
+uint8 IMAGE_COLOR=3
+uint8 IMAGE_RECT=4
+uint8 IMAGE_RECT_COLOR=5
-Header header
-StereoInfo stereo_info
-CamInfo left_info
-byte left_type
-Image left_image
-CamInfo right_info
-byte right_type
-Image right_image
-Image disparity_image
+Header header
+StereoInfo stereo_info
+CamInfo left_info
+uint8 left_type
+Image left_image
+CamInfo right_info
+uint8 right_type
+Image right_image
+uint8 has_disparity
+DisparityInfo disparity_info
+Image disparity_image
Modified: pkg/trunk/image_msgs/msg/StereoInfo.msg
===================================================================
--- pkg/trunk/image_msgs/msg/StereoInfo.msg 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/image_msgs/msg/StereoInfo.msg 2009-01-06 09:05:46 UTC (rev 8875)
@@ -12,23 +12,3 @@
float64[3] T # Pose of right camera in left camera coords
float64[3] Om # rotation vector
float64[16] RP # Reprojection Matrix
-
-int32 dpp
-int32 num_disp
-int32 im_Dtop
-int32 im_Dleft
-int32 im_Dwidth
-int32 im_Dheight
-int32 corr_size
-int32 filter_size
-int32 hor_offset
-int32 texture_thresh
-int32 unique_thresh
-int32 smooth_thresh
-int32 speckle_diff
-int32 speckle_region_size
-byte unique_check
-
-byte has_disparity
-
-# Should put exposure, gain, etc. information here as well
\ No newline at end of file
Modified: pkg/trunk/vision/people/src/face_detection.cpp
===================================================================
--- pkg/trunk/vision/people/src/face_detection.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/people/src/face_detection.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -43,6 +43,7 @@
#include "CvStereoCamModel.h"
#include <robot_msgs/PositionMeasurement.h>
#include "image_msgs/StereoInfo.h"
+#include "image_msgs/DisparityInfo.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "image_msgs/CvBridge.h"
@@ -73,6 +74,7 @@
image_msgs::Image limage_; /**< Left image msg. */
image_msgs::Image dimage_; /**< Disparity image msg. */
image_msgs::StereoInfo stinfo_; /**< Stereo info msg. */
+ image_msgs::DisparityInfo dispinfo_; /**< Disparity info msg. */
image_msgs::CamInfo rcinfo_; /**< Right camera info msg. */
image_msgs::CvBridge lbridge_; /**< ROS->OpenCV bridge for the left image. */
image_msgs::CvBridge dbridge_; /**< ROS->OpenCV bridge for the disparity image. */
@@ -131,12 +133,13 @@
// Subscribe to the images and parameters
std::list<std::string> left_list;
- //left_list.push_back(std::string("stereodcam/left/image_rect"));
- left_list.push_back(std::string("stereodcam/left/image_rect_color"));
+ //left_list.push_back(std::string("stereo/left/image_rect"));
+ left_list.push_back(std::string("stereo/left/image_rect_color"));
sync_.subscribe(left_list,limage_,1);
- sync_.subscribe("stereodcam/disparity",dimage_,1);
- sync_.subscribe("stereodcam/stereo_info",stinfo_,1);
- sync_.subscribe("stereodcam/right/cam_info",rcinfo_,1);
+ sync_.subscribe("stereo/disparity",dimage_,1);
+ sync_.subscribe("stereo/stereo_info",stinfo_,1);
+ sync_.subscribe("stereo/disparity_info",dispinfo_,1);
+ sync_.subscribe("stereo/right/cam_info",rcinfo_,1);
sync_.ready();
// Advertise a position measure message.
@@ -260,7 +263,7 @@
double Crx = Clx;
double Cy = rcinfo_.P[6];
double Tx = -rcinfo_.P[3]/Fx;
- cam_model_ = new CvStereoCamModel(Fx,Fy,Tx,Clx,Crx,Cy,1.0/(double)stinfo_.dpp);
+ cam_model_ = new CvStereoCamModel(Fx,Fy,Tx,Clx,Crx,Cy,1.0/(double)dispinfo_.dpp);
if ( cv_image_left_ ) {
im_size = cvGetSize(cv_image_left_);
@@ -338,7 +341,7 @@
if (!cv_image_disp_out_) {
cv_image_disp_out_ = cvCreateImage(im_size,IPL_DEPTH_8U,1);
}
- cvCvtScale(cv_image_disp_,cv_image_disp_out_,4.0/stinfo_.dpp);
+ cvCvtScale(cv_image_disp_,cv_image_disp_out_,4.0/dispinfo_.dpp);
cvShowImage("Face detector: Face Detection",cv_image_left_);
cvShowImage("Face detector: Disparity",cv_image_disp_out_);
Modified: pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
===================================================================
--- pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -44,6 +44,7 @@
#include "color_calib.h"
#include <robot_msgs/PositionMeasurement.h>
#include "image_msgs/StereoInfo.h"
+#include "image_msgs/DisparityInfo.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "image_msgs/CvBridge.h"
@@ -70,6 +71,7 @@
image_msgs::Image limage_;
image_msgs::Image dimage_;
image_msgs::StereoInfo stinfo_;
+ image_msgs::DisparityInfo dispinfo_;
image_msgs::CamInfo rcinfo_;
image_msgs::CvBridge lbridge_;
image_msgs::CvBridge dbridge_;
@@ -137,10 +139,11 @@
people_ = new People();
// Subscribe to the images and parameters
- sync_.subscribe("stereodcam/left/image_rect_color",limage_,1);
- sync_.subscribe("stereodcam/disparity",dimage_,1);
- sync_.subscribe("stereodcam/stereo_info",stinfo_,1);
- sync_.subscribe("stereodcam/right/cam_info",rcinfo_,1);
+ sync_.subscribe("stereo/left/image_rect_color",limage_,1);
+ sync_.subscribe("stereo/disparity",dimage_,1);
+ sync_.subscribe("stereo/stereo_info",stinfo_,1);
+ sync_.subscribe("stereo/disparity_info",dispinfo_,1);
+ sync_.subscribe("stereo/right/cam_info",rcinfo_,1);
sync_.ready();
// Advertise a 3d position measurement for each head.
@@ -282,7 +285,7 @@
cv_image_left_ = lbridge_.toIpl();
}
}
- else if (calib_color_ && lcolor_cal_.getFromParam("stereodcam/left/image_rect_color")) {
+ else if (calib_color_ && lcolor_cal_.getFromParam("stereo/left/image_rect_color")) {
// Exit if color calibration hasn't been performed.
do_calib = true;
if (lbridge_.fromImage(limage_,"bgr")) {
@@ -310,7 +313,7 @@
double Crx = Clx;
double Cy = rcinfo_.P[6];
double Tx = -rcinfo_.P[3]/Fx;
- cam_model_ = new CvStereoCamModel(Fx,Fy,Tx,Clx,Crx,Cy,1.0/stinfo_.dpp);
+ cam_model_ = new CvStereoCamModel(Fx,Fy,Tx,Clx,Crx,Cy,1.0/dispinfo_.dpp);
im_size = cvGetSize(cv_image_left_);
@@ -489,7 +492,7 @@
if (!cv_image_disp_out_) {
cv_image_disp_out_ = cvCreateImage(im_size,IPL_DEPTH_8U,1);
}
- cvCvtScale(cv_image_disp_,cv_image_disp_out_,4.0/stinfo_.dpp);
+ cvCvtScale(cv_image_disp_,cv_image_disp_out_,4.0/dispinfo_.dpp);
cvShowImage("Face color tracker: Face Detection", cv_image_left_);
cvShowImage("Face color tracker: Disparity", cv_image_disp_out_);
#endif
Modified: pkg/trunk/vision/people/src/track_starter_gui.cpp
===================================================================
--- pkg/trunk/vision/people/src/track_starter_gui.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/people/src/track_starter_gui.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -42,6 +42,7 @@
#include "ros/node.h"
#include "image_msgs/StereoInfo.h"
+#include "image_msgs/DisparityInfo.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "image_msgs/CvBridge.h"
@@ -101,6 +102,7 @@
image_msgs::Image limage_;
image_msgs::Image dimage_;
image_msgs::StereoInfo stinfo_;
+ image_msgs::DisparityInfo dispinfo_;
image_msgs::CamInfo rcinfo_;
image_msgs::CvBridge lbridge_;
image_msgs::CvBridge dbridge_;
@@ -141,12 +143,13 @@
advertise<robot_msgs::PositionMeasurement>("people_tracker_measurements",1);
std::list<std::string> left_list;
- left_list.push_back(std::string("stereodcam/left/image_rect_color"));
- //left_list.push_back(std::string("stereodcam/left/image_rect"));
+ left_list.push_back(std::string("stereo/left/image_rect_color"));
+ //left_list.push_back(std::string("stereo/left/image_rect"));
sync_.subscribe(left_list,limage_,1);
- sync_.subscribe("stereodcam/disparity",dimage_,1);
- sync_.subscribe("stereodcam/stereo_info", stinfo_,1);
- sync_.subscribe("stereodcam/right/cam_info",rcinfo_,1);
+ sync_.subscribe("stereo/disparity",dimage_,1);
+ sync_.subscribe("stereo/stereo_info", stinfo_,1);
+ sync_.subscribe("stereo/disparity_info", dispinfo_,1);
+ sync_.subscribe("stereo/right/cam_info",rcinfo_,1);
sync_.ready();
//subscribe("person_measurement",pos,&TrackStarterGUI::point_cb,1);
@@ -188,7 +191,7 @@
bool do_calib = false;
if (limage_.encoding != "mono") {
// If this is a color image, set the calibration and convert it.
- if (calib_color_ && lcolor_cal_.getFromParam("stereodcam/left/image_rect_color")) {
+ if (calib_color_ && lcolor_cal_.getFromParam("stereo/left/image_rect_color")) {
do_calib = true;
}
// Convert the images to OpenCV format.
@@ -212,7 +215,7 @@
if (!cv_disp_image_out_) {
cv_disp_image_out_ = cvCreateImage(im_size,IPL_DEPTH_8U, 1);
}
- cvCvtScale(cv_disp_image_, cv_disp_image_out_, 4.0/stinfo_.dpp);
+ cvCvtScale(cv_disp_image_, cv_disp_image_out_, 4.0/dispinfo_.dpp);
}
// Convert the stereo calibration into a camera model.
@@ -226,7 +229,7 @@
double Cy = rcinfo_.P[6];
double Tx = -rcinfo_.P[3]/Fx;
//printf("%f %f %f %f %f %f %f\n", Fx,Fy,Tx,Clx,Crx,Cy,1.0/stinfo_.dpp);
- cam_model_ = new CvStereoCamModel(Fx,Fy,Tx,Clx,Crx,Cy,1.0/stinfo_.dpp);
+ cam_model_ = new CvStereoCamModel(Fx,Fy,Tx,Clx,Crx,Cy,1.0/dispinfo_.dpp);
//cv_mutex_.lock(); ///
Modified: pkg/trunk/vision/stereo_capture/capture.launch
===================================================================
--- pkg/trunk/vision/stereo_capture/capture.launch 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/stereo_capture/capture.launch 2009-01-06 09:05:46 UTC (rev 8875)
@@ -1,18 +1,13 @@
<launch>
- <node name="dcam" pkg="dcam_node" type="dcam_node" respawn="false">
- <!-- video_mode should be one of the following:
- 640x480_videre_disparity: Disparity and rectification done on chip.
- Provides: Disparity and left mono image
- 640x480_videre_disparity_raw: Disparity done on chip (debayer and rectification in software).
- Provides: disparity and left color image.
- 640x480_videre_none: No stereo on chip (all processing done in software).
- Provides: all 3 images available
- -->
- <param name="video_mode" type="str" value="640x480_videre_none"/>
+ <group ns="stereo">
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="do_colorize" type="bool" value="True"/>
<param name="do_rectify" type="bool" value="True"/>
<param name="do_stereo" type="bool" value="True"/>
<param name="do_calc_points" type="bool" value="True"/>
- </node>
+ </group>
+ <node pkg="dcam" type="stereodcam" respawn="false"/>
+ <node pkg="stereo_image_proc" type="stereoproc" respawn="false"/>
<node name="stereo_capture" pkg="stereo_capture" type="stereo_capture" respawn="false" output="screen"/>
</launch>
Modified: pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
===================================================================
--- pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -44,6 +44,7 @@
#include "ros/node.h"
#include "image_msgs/StereoInfo.h"
+#include "image_msgs/DisparityInfo.h"
#include "image_msgs/Image.h"
#include "std_msgs/PointCloud.h"
#include "std_msgs/UInt8.h" //for projector status
@@ -62,6 +63,7 @@
image_msgs::Image rimage;
image_msgs::Image dimage;
image_msgs::StereoInfo stinfo;
+ image_msgs::DisparityInfo dispinfo;
image_msgs::CvBridge lbridge;
image_msgs::CvBridge rbridge;
@@ -107,20 +109,21 @@
cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
std::list<std::string> left_list;
- left_list.push_back(std::string("dcam/left/image_rect_color"));
- left_list.push_back(std::string("dcam/left/image_rect"));
+ left_list.push_back(std::string("stereo/left/image_rect_color"));
+ left_list.push_back(std::string("stereo/left/image_rect"));
std::list<std::string> right_list;
- right_list.push_back(std::string("dcam/right/image_rect_color"));
- right_list.push_back(std::string("dcam/right/image_rect"));
+ right_list.push_back(std::string("stereo/right/image_rect_color"));
+ right_list.push_back(std::string("stereo/right/image_rect"));
sync.subscribe(left_list, limage, 1);
sync.subscribe(right_list, rimage, 1);
- sync.subscribe("dcam/disparity", dimage, 1);
- sync.subscribe("dcam/stereo_info", stinfo, 1);
+ sync.subscribe("stereo/disparity", dimage, 1);
+ sync.subscribe("stereo/stereo_info", stinfo, 1);
+ sync.subscribe("stereo/disparity_info", dispinfo, 1);
- sync.subscribe("dcam/cloud", cloud, 1);
+ sync.subscribe("stereo/cloud", cloud, 1);
subscribe("projector_status", projector_status, &StereoView::projector_status_change, 1);
}
@@ -181,7 +184,7 @@
if (dbridge.fromImage(dimage))
{
IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
- cvCvtScale(dbridge.toIpl(), disp, 4.0/stinfo.dpp);
+ cvCvtScale(dbridge.toIpl(), disp, 4.0/dispinfo.dpp);
if(projector_status.data)
{
Modified: pkg/trunk/vision/stereo_image_proc/include/cam_bridge.h
===================================================================
--- pkg/trunk/vision/stereo_image_proc/include/cam_bridge.h 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/stereo_image_proc/include/cam_bridge.h 2009-01-06 09:05:46 UTC (rev 8875)
@@ -41,13 +41,13 @@
namespace cam_bridge
{
- void CamDataToRawStereo(cam::ImageData* im, image_msgs::Image& im_msg, image_msgs::CamInfo& info_msg, int8_t& type)
+ void CamDataToRawStereo(cam::ImageData* im, image_msgs::Image& im_msg, image_msgs::CamInfo& info_msg, uint8_t& type)
{
if (im->imRawType != COLOR_CODING_NONE)
{
fillImage(im_msg, "image_raw",
im->imHeight, im->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
im->imRaw );
type = image_msgs::RawStereo::IMAGE_RAW;
}
@@ -55,7 +55,7 @@
{
fillImage(im_msg, "image",
im->imHeight, im->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
im->im );
type = image_msgs::RawStereo::IMAGE;
}
@@ -63,7 +63,7 @@
{
fillImage(im_msg, "image_color",
im->imHeight, im->imWidth, 4,
- "rgba", "byte",
+ "rgba", "uint8",
im->imColor );
type = image_msgs::RawStereo::IMAGE_COLOR;
}
@@ -71,7 +71,7 @@
{
fillImage(im_msg, "image_color",
im->imHeight, im->imWidth, 3,
- "rgb", "byte",
+ "rgb", "uint8",
im->imColor );
type = image_msgs::RawStereo::IMAGE_COLOR;
}
@@ -79,7 +79,7 @@
{
fillImage(im_msg, "image_rect",
im->imHeight, im->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
im->imRect );
type = image_msgs::RawStereo::IMAGE_RECT;
}
@@ -87,7 +87,7 @@
{
fillImage(im_msg, "image_rect_color",
im->imHeight, im->imWidth, 4,
- "rgba", "byte",
+ "rgba", "uint8",
im->imRectColor );
type = image_msgs::RawStereo::IMAGE_RECT_COLOR;
}
@@ -95,7 +95,7 @@
{
fillImage(im_msg, "image_rect_color",
im->imHeight, im->imWidth, 3,
- "rgb", "byte",
+ "rgb", "uint8",
im->imRectColor );
type = image_msgs::RawStereo::IMAGE_RECT_COLOR;
}
@@ -120,29 +120,34 @@
"mono", "int16",
stIm->imDisp );
- raw_stereo.stereo_info.has_disparity = true;
+ 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.stereo_info.has_disparity = false;
+ raw_stereo.has_disparity = false;
}
raw_stereo.stereo_info.height = stIm->imHeight;
raw_stereo.stereo_info.width = stIm->imWidth;
- raw_stereo.stereo_info.dpp = stIm->dpp;
- raw_stereo.stereo_info.num_disp = stIm->numDisp;
- raw_stereo.stereo_info.im_Dtop = stIm->imDtop;
- raw_stereo.stereo_info.im_Dleft = stIm->imDleft;
- raw_stereo.stereo_info.im_Dwidth = stIm->imDwidth;
- raw_stereo.stereo_info.im_Dheight = stIm->imDheight;
- raw_stereo.stereo_info.corr_size = stIm->corrSize;
- raw_stereo.stereo_info.filter_size = stIm->filterSize;
- raw_stereo.stereo_info.hor_offset = stIm->horOffset;
- raw_stereo.stereo_info.texture_thresh = stIm->textureThresh;
- raw_stereo.stereo_info.unique_thresh = stIm->uniqueThresh;
- raw_stereo.stereo_info.smooth_thresh = stIm->smoothThresh;
- raw_stereo.stereo_info.speckle_diff = stIm->speckleDiff;
- raw_stereo.stereo_info.speckle_region_size = stIm->speckleRegionSize;
- raw_stereo.stereo_info.unique_check = stIm->unique_check;
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));
@@ -152,7 +157,7 @@
}
- void extractImage(std_msgs::ByteMultiArray& arr, size_t* sz, uint8_t **d)
+ void extractImage(std_msgs::UInt8MultiArray& arr, size_t* sz, uint8_t **d)
{
size_t new_size = arr.layout.dim[0].stride;
@@ -178,7 +183,7 @@
memcpy((char*)(*d), (char*)(&arr.data[0]), new_size);
}
- void RawStereoToCamData(image_msgs::Image& im_msg, image_msgs::CamInfo& info_msg, int8_t& type, cam::ImageData* im)
+ void RawStereoToCamData(image_msgs::Image& im_msg, image_msgs::CamInfo& info_msg, uint8_t& type, cam::ImageData* im)
{
im->imRawType = COLOR_CODING_NONE;
@@ -189,37 +194,37 @@
if (type == image_msgs::RawStereo::IMAGE_RAW)
{
- extractImage(im_msg.byte_data, &im->imRawSize, &im->imRaw);
+ extractImage(im_msg.uint8_data, &im->imRawSize, &im->imRaw);
im->imRawType = COLOR_CODING_BAYER8_RGGB;
}
else if (type == image_msgs::RawStereo::IMAGE)
{
- extractImage(im_msg.byte_data, &im->imSize, &im->im);
+ extractImage(im_msg.uint8_data, &im->imSize, &im->im);
im->imType = COLOR_CODING_MONO8;
}
else if (type == image_msgs::RawStereo::IMAGE_COLOR && im_msg.encoding == "rgba")
{
- extractImage(im_msg.byte_data, &im->imColorSize, &im->imColor);
+ extractImage(im_msg.uint8_data, &im->imColorSize, &im->imColor);
im->imColorType = COLOR_CODING_RGBA8;
}
else if (type == image_msgs::RawStereo::IMAGE_COLOR && im_msg.encoding == "rgb")
{
- extractImage(im_msg.byte_data, &im->imColorSize, &im->imColor);
+ extractImage(im_msg.uint8_data, &im->imColorSize, &im->imColor);
im->imColorType = COLOR_CODING_RGB8;
}
else if (type == image_msgs::RawStereo::IMAGE_RECT)
{
- extractImage(im_msg.byte_data, &im->imRectSize, &im->imRect);
+ extractImage(im_msg.uint8_data, &im->imRectSize, &im->imRect);
im->imRectType = COLOR_CODING_MONO8;
}
else if (type == image_msgs::RawStereo::IMAGE_RECT_COLOR && im_msg.encoding == "rgba")
{
- extractImage(im_msg.byte_data, &im->imRectColorSize, &im->imRectColor);
+ extractImage(im_msg.uint8_data, &im->imRectColorSize, &im->imRectColor);
im->imRectColorType = COLOR_CODING_RGBA8;
}
else if (type == image_msgs::RawStereo::IMAGE_RECT_COLOR && im_msg.encoding == "rgb")
{
- extractImage(im_msg.byte_data, &im->imRectColorSize, &im->imRectColor);
+ extractImage(im_msg.uint8_data, &im->imRectColorSize, &im->imRectColor);
im->imRectColorType = COLOR_CODING_RGB8;
}
@@ -241,28 +246,28 @@
stIm->hasDisparity = false;
- if (raw_stereo.stereo_info.has_disparity)
+ if (raw_stereo.has_disparity)
{
extractImage(raw_stereo.disparity_image.int16_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;
}
-
- stIm->dpp = raw_stereo.stereo_info.dpp;
- stIm->numDisp = raw_stereo.stereo_info.num_disp;
- stIm->imDtop = raw_stereo.stereo_info.im_Dtop;
- stIm->imDleft = raw_stereo.stereo_info.im_Dleft;
- stIm->imDwidth = raw_stereo.stereo_info.im_Dwidth;
- stIm->imDheight = raw_stereo.stereo_info.im_Dheight;
- stIm->corrSize = raw_stereo.stereo_info.corr_size;
- stIm->filterSize = raw_stereo.stereo_info.filter_size;
- stIm->horOffset = raw_stereo.stereo_info.hor_offset;
- stIm->textureThresh = raw_stereo.stereo_info.texture_thresh;
- stIm->uniqueThresh = raw_stereo.stereo_info.unique_thresh;
- stIm->smoothThresh = raw_stereo.stereo_info.smooth_thresh;
- stIm->speckleDiff = raw_stereo.stereo_info.speckle_diff;
- stIm->speckleRegionSize = raw_stereo.stereo_info.speckle_region_size;
- stIm->unique_check = raw_stereo.stereo_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));
Modified: pkg/trunk/vision/stereo_image_proc/src/image.cpp
===================================================================
--- pkg/trunk/vision/stereo_image_proc/src/image.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/stereo_image_proc/src/image.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -594,7 +594,22 @@
}
+bool
+StereoData::setSpeckleRegionSize(int val)
+{
+ speckleRegionSize = val;
+ return true;
+}
+bool
+StereoData::setSpeckleDiff(int val)
+{
+ speckleDiff = val;
+ return true;
+}
+
+
+
//
// param sting parsing routines
//
Modified: pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
===================================================================
--- pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -61,32 +61,39 @@
image_msgs::RawStereo raw_stereo_;
- image_msgs::Image img_;
- std_msgs::PointCloud cloud_;
- image_msgs::CamInfo cam_info_;
- image_msgs::StereoInfo stereo_info_;
+ image_msgs::Image img_;
+ std_msgs::PointCloud cloud_;
+ image_msgs::CamInfo cam_info_;
+ image_msgs::DisparityInfo disparity_info_;
+ image_msgs::StereoInfo stereo_info_;
+
DiagnosticUpdater<StereoProc> diagnostic_;
int count_;
double desired_freq_;
string frame_id_;
+ std::string stereo_name_;
+
public:
cam::StereoData* stdata_;
StereoProc() : ros::node("stereoproc"), diagnostic_(this), count_(0), stdata_(NULL)
{
+
+ stereo_name_ = map_name("stereo") + std::string("/");
+
diagnostic_.addUpdater( &StereoProc::freqStatus );
- param("do_colorize", do_colorize_, false);
+ param(stereo_name_ + std::string("do_colorize"), do_colorize_, false);
- param("do_rectify", do_rectify_, false);
+ param(stereo_name_ + std::string("do_rectify"), do_rectify_, false);
- param("do_stereo", do_stereo_, false);
+ param(stereo_name_ + std::string("do_stereo"), do_stereo_, false);
- param("do_calc_points", do_calc_points_, false);
+ param(stereo_name_ + std::string("do_calc_points"), do_calc_points_, false);
// Must do stereo if calculating points
do_stereo_ = do_stereo_ || do_calc_points_;
@@ -96,7 +103,12 @@
stdata_ = new cam::StereoData;
- subscribe("raw_stereo", raw_stereo_, &StereoProc::rawCb, 1);
+ stdata_->setUniqueThresh(36);
+ stdata_->setTextureThresh(30);
+ stdata_->setSpeckleRegionSize(100);
+ stdata_->setSpeckleDiff(10);
+
+ subscribe(stereo_name_ + std::string("raw_stereo"), raw_stereo_, &StereoProc::rawCb, 1);
}
@@ -141,11 +153,35 @@
void publishCam()
{
- publishImages("left/", stdata_->imLeft);
- publishImages("right/", stdata_->imRight);
+ publishImages(stereo_name_ + std::string("left/"), stdata_->imLeft);
+ publishImages(stereo_name_ + std::string("right/"), stdata_->imRight);
- if (stdata_->imDisp)
+ if (stdata_->hasDisparity)
{
+ disparity_info_.header.stamp = raw_stereo_.header.stamp;
+ disparity_info_.header.frame_id = raw_stereo_.header.frame_id;
+
+ disparity_info_.height = stdata_->imHeight;
+ disparity_info_.width = stdata_->imWidth;
+
+ disparity_info_.dpp = stdata_->dpp;
+ disparity_info_.num_disp = stdata_->numDisp;
+ disparity_info_.im_Dtop = stdata_->imDtop;
+ disparity_info_.im_Dleft = stdata_->imDleft;
+ disparity_info_.im_Dwidth = stdata_->imDwidth;
+ disparity_info_.im_Dheight = stdata_->imDheight;
+ disparity_info_.corr_size = stdata_->corrSize;
+ disparity_info_.filter_size = stdata_->filterSize;
+ disparity_info_.hor_offset = stdata_->horOffset;
+ disparity_info_.texture_thresh = stdata_->textureThresh;
+ disparity_info_.unique_thresh = stdata_->uniqueThresh;
+ disparity_info_.smooth_thresh = stdata_->smoothThresh;
+ disparity_info_.speckle_diff = stdata_->speckleDiff;
+ disparity_info_.speckle_region_size = stdata_->speckleRegionSize;
+ disparity_info_.unique_check = stdata_->unique_check;
+
+ publish(stereo_name_ + std::string("disparity_info"), disparity_info_);
+
fillImage(img_, "disparity",
stdata_->imHeight, stdata_->imWidth, 1,
"mono", "uint16",
@@ -153,11 +189,7 @@
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
- publish("disparity", img_);
-
- stereo_info_.has_disparity = true;
- } else {
- stereo_info_.has_disparity = false;
+ publish(stereo_name_ + std::string("disparity"), img_);
}
if (do_calc_points_)
@@ -182,7 +214,7 @@
cloud_.chan[0].vals[i] = *(float*)(&rgb);
}
- publish("cloud", cloud_);
+ publish(stereo_name_ + std::string("cloud"), cloud_);
}
stereo_info_.header.stamp = raw_stereo_.header.stamp;
@@ -190,28 +222,11 @@
stereo_info_.height = stdata_->imHeight;
stereo_info_.width = stdata_->imWidth;
- stereo_info_.dpp = stdata_->dpp;
- stereo_info_.num_disp = stdata_->numDisp;
- stereo_info_.im_Dtop = stdata_->imDtop;
- stereo_info_.im_Dleft = stdata_->imDleft;
- stereo_info_.im_Dwidth = stdata_->imDwidth;
- stereo_info_.im_Dheight = stdata_->imDheight;
- stereo_info_.corr_size = stdata_->corrSize;
- stereo_info_.filter_size = stdata_->filterSize;
- stereo_info_.hor_offset = stdata_->horOffset;
- stereo_info_.texture_thresh = stdata_->textureThresh;
- stereo_info_.unique_thresh = stdata_->uniqueThresh;
- stereo_info_.smooth_thresh = stdata_->smoothThresh;
- stereo_info_.speckle_diff = stdata_->speckleDiff;
- stereo_info_.speckle_region_size = stdata_->speckleRegionSize;
- stereo_info_.unique_check = stdata_->unique_check;
-
-
memcpy((char*)(&stereo_info_.T[0]), (char*)(stdata_->T), 3*sizeof(double));
memcpy((char*)(&stereo_info_.Om[0]), (char*)(stdata_->Om), 3*sizeof(double));
memcpy((char*)(&stereo_info_.RP[0]), (char*)(stdata_->RP), 16*sizeof(double));
- publish("stereo_info", stereo_info_);
+ publish(stereo_name_ + std::string("stereo_info"), stereo_info_);
}
void publishImages(std::string base_name, cam::ImageData* img_data)
@@ -220,87 +235,69 @@
{
fillImage(img_, "image_raw",
img_data->imHeight, img_data->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
img_data->imRaw );
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
publish(base_name + std::string("image_raw"), img_);
- cam_info_.has_image = true;
- } else {
- cam_info_.has_image = false;
}
if (img_data->imType != COLOR_CODING_NONE)
{
fillImage(img_, "image",
img_data->imHeight, img_data->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
img_data->im );
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
publish(base_name + std::string("image"), img_);
- cam_info_.has_image = true;
- } else {
- cam_info_.has_image = false;
}
if (img_data->imColorType != COLOR_CODING_NONE && img_data->imColorType == COLOR_CODING_RGB8)
{
fillImage(img_, "image_color",
img_data->imHeight, img_data->imWidth, 3,
- "rgb", "byte",
+ "rgb", "uint8",
img_data->imColor );
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
publish(base_name + std::string("image_color"), img_);
- cam_info_.has_image_color = true;
- } else {
- cam_info_.has_image_color = false;
}
if (img_data->imRectType != COLOR_CODING_NONE)
{
fillImage(img_, "image_rect",
img_data->imHeight, img_data->imWidth, 1,
- "mono", "byte",
+ "mono", "uint8",
img_data->imRect );
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
publish(base_name + std::string("image_rect"), img_);
- cam_info_.has_image_rect = true;
- } else {
- cam_info_.has_image_rect = false;
}
if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGB8)
{
fillImage(img_, "image_rect_color",
img_data->imHeight, img_data->imWidth, 3,
- "rgb", "byte",
+ "rgb", "uint8",
img_data->imRectColor );
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
publish(base_name + std::string("image_rect_color"), img_);
- cam_info_.has_image_rect_color = true;
- } else {
- cam_info_.has_image_rect_color = false;
}
if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGBA8)
{
fillImage(img_, "image_rect_color",
img_data->imHeight, img_data->imWidth, 4,
- "rgba", "byte",
+ "rgba", "uint8",
img_data->imRectColor );
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
publish(base_name + std::string("image_rect_color"), img_);
- cam_info_.has_image_rect_color = true;
- } else {
- cam_info_.has_image_rect_color = false;
}
@@ -342,16 +339,19 @@
void advertiseCam()
{
- advertise<image_msgs::StereoInfo>("stereo_info", 1);
+ advertise<image_msgs::StereoInfo>(stereo_name_ + std::string("stereo_info"), 1);
- advertiseImages("left/", stdata_->imLeft);
- advertiseImages("right/", stdata_->imRight);
+ advertiseImages(stereo_name_ + std::string("left/"), stdata_->imLeft);
+ advertiseImages(stereo_name_ + std::string("right/"), stdata_->imRight);
- if (stdata_->imDisp)
- advertise<image_msgs::Image>("disparity", 1);
+ if (stdata_->hasDisparity)
+ {
+ advertise<image_msgs::DisparityInfo>(stereo_name_ + std::string("disparity_info"), 1);
+ advertise<image_msgs::Image>(stereo_name_ + std::string("disparity"), 1);
+ }
if (do_calc_points_)
- advertise<std_msgs::PointCloud>("cloud",1);
+ advertise<std_msgs::PointCloud>(stereo_name_ + std::string("cloud"),1);
}
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -42,6 +42,7 @@
#include "ros/node.h"
#include "image_msgs/StereoInfo.h"
+#include "image_msgs/DisparityInfo.h"
#include "image_msgs/Image.h"
@@ -59,6 +60,7 @@
image_msgs::Image rimage;
image_msgs::Image dimage;
image_msgs::StereoInfo stinfo;
+ image_msgs::DisparityInfo dinfo;
image_msgs::CvBridge lbridge;
image_msgs::CvBridge rbridge;
@@ -88,19 +90,20 @@
cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
std::list<std::string> left_list;
- left_list.push_back(map_name("stereodcam") + std::string("/left/image_rect_color"));
- left_list.push_back(map_name("stereodcam") + std::string("/left/image_rect"));
+ left_list.push_back(map_name("stereo") + std::string("/left/image_rect_color"));
+ left_list.push_back(map_name("stereo") + std::string("/left/image_rect"));
std::list<std::string> right_list;
- right_list.push_back(map_name("stereodcam") + std::string("/right/image_rect_color"));
- right_list.push_back(map_name("stereodcam") + std::string("/right/image_rect"));
+ right_list.push_back(map_name("stereo") + std::string("/right/image_rect_color"));
+ right_list.push_back(map_name("stereo") + std::string("/right/image_rect"));
sync.subscribe(left_list, limage, 1);
sync.subscribe(right_list, rimage, 1);
- sync.subscribe(map_name("stereodcam") + "/disparity", dimage, 1);
- sync.subscribe(map_name("stereodcam") + "/stereo_info", stinfo, 1);
+ sync.subscribe(map_name("stereo") + "/disparity", dimage, 1);
+ sync.subscribe(map_name("stereo") + "/stereo_info", stinfo, 1);
+ sync.subscribe(map_name("stereo") + "/disparity_info", dinfo, 1);
sync.ready();
}
@@ -147,7 +150,7 @@
{
// Disparity has to be scaled to be be nicely displayable
IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
- cvCvtScale(dbridge.toIpl(), disp, 4.0/stinfo.dpp);
+ cvCvtScale(dbridge.toIpl(), disp, 4.0/dinfo.dpp);
cvShowImage("disparity", disp);
cvReleaseImage(&disp);
}
@@ -189,8 +192,8 @@
// Fetch color calibration parameters as necessary
if (calib_color_)
{
- lcal.getFromParam(map_name("stereodcam") + "/left/image_rect_color");
- rcal.getFromParam(map_name("stereodcam") + "/right/image_rect_color");
+ lcal.getFromParam(map_name("stereo") + "/left/image_rect_color");
+ rcal.getFromParam(map_name("stereo") + "/right/image_rect_color");
}
cv_mutex.unlock();
Modified: pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-01-06 07:08:25 UTC (rev 8874)
+++ pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-01-06 09:05:46 UTC (rev 8875)
@@ -50,6 +...
[truncated message content] |
