You can subscribe to this list here.
2008 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
(4) |
Dec
|
---|---|---|---|---|---|---|---|---|---|---|---|---|
2009 |
Jan
|
Feb
|
Mar
(7) |
Apr
|
May
|
Jun
(40) |
Jul
(28) |
Aug
(51) |
Sep
(63) |
Oct
(42) |
Nov
(47) |
Dec
(19) |
2010 |
Jan
(5) |
Feb
(12) |
Mar
(23) |
Apr
(102) |
May
(2) |
Jun
(11) |
Jul
(32) |
Aug
(51) |
Sep
(64) |
Oct
(40) |
Nov
(86) |
Dec
(29) |
2011 |
Jan
(1) |
Feb
(10) |
Mar
(5) |
Apr
|
May
(3) |
Jun
(5) |
Jul
(3) |
Aug
(2) |
Sep
(11) |
Oct
(5) |
Nov
|
Dec
(1) |
2012 |
Jan
(6) |
Feb
(8) |
Mar
(2) |
Apr
(2) |
May
|
Jun
(2) |
Jul
(3) |
Aug
(2) |
Sep
|
Oct
|
Nov
(1) |
Dec
|
From: <tum...@li...> - 2009-03-18 14:55:51
|
Revision: 7 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=7&view=rev Author: marton_z Date: 2009-03-18 14:55:44 +0000 (Wed, 18 Mar 2009) Log Message: ----------- changed default values Modified Paths: -------------- drivers/composite/composite_node/src/composite_node.cpp Modified: drivers/composite/composite_node/src/composite_node.cpp =================================================================== --- drivers/composite/composite_node/src/composite_node.cpp 2008-11-17 13:44:10 UTC (rev 6) +++ drivers/composite/composite_node/src/composite_node.cpp 2009-03-18 14:55:44 UTC (rev 7) @@ -692,9 +692,9 @@ CompositeNode c; c.dump_to_disk_ = false; - c.sr_enable_ = true; - c.flir_enable_ = true; - c.stoc_enable_ = true; //c.stoc_just_left_ = true; + c.sr_enable_ = false; + c.flir_enable_ = false; + c.stoc_enable_ = false; //c.stoc_just_left_ = true; if (c.start () == 0) c.spin (); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2008-11-17 13:44:14
|
Revision: 6 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=6&view=rev Author: veedee Date: 2008-11-17 13:44:10 +0000 (Mon, 17 Nov 2008) Log Message: ----------- visualizer fusion is very messy, will do some code cleanup today. param_visualizer got 4 more parameters for undistorting the SwissRanger images and filtering the PCD in-driver Modified Paths: -------------- drivers/composite/visualizer_fusion/src/param_visualizer.cpp Added Paths: ----------- drivers/composite/visualizer_fusion/src/visualizer_fusion.cpp Modified: drivers/composite/visualizer_fusion/src/param_visualizer.cpp =================================================================== --- drivers/composite/visualizer_fusion/src/param_visualizer.cpp 2008-11-17 12:32:04 UTC (rev 5) +++ drivers/composite/visualizer_fusion/src/param_visualizer.cpp 2008-11-17 13:44:10 UTC (rev 6) @@ -37,6 +37,8 @@ #include <cfloat> #include "terminal.h" +#define BOOL2STR_ED(b) ( b ? "enabled" : "disabled" ) + using namespace std; class ParamVisualizer: public ros::node @@ -67,6 +69,11 @@ fprintf (stderr, " -stoc_tt X = STOC <Texture filter threshold> parameter\n"); fprintf (stderr, " -stoc_nd X = STOC <Number of disparities> parameter\n"); fprintf (stderr, " -stoc_zm X = STOC <Z-max cutoff> parameter\n"); + fprintf (stderr, "\n"); + fprintf (stderr, " -sr_rd X = SwissRanger <Rectify Distance Image> parameter\n"); + fprintf (stderr, " -sr_ra X = SwissRanger <Rectify Amplitude Image> parameter\n"); + fprintf (stderr, " -sr_rc X = SwissRanger <Rectify Confidence Image> parameter\n"); + fprintf (stderr, " -sr_fpcd X = SwissRanger <Filter PCD> parameter\n"); return (-1); } @@ -158,6 +165,29 @@ } + int sr_rd, sr_ra, sr_rc, sr_fpcd; + if (parseArgument (argc, argv, "-sr_rd", sr_rd) != -1) + { + print_info (stderr, "Changing the <Rectify Distance Image> settings on the SwissRanger camera to: "); print_value (stderr, "%s\n", BOOL2STR_ED (sr_rd)); + p.set_param ("/composite/sr_rectify_distance", sr_rd); + } + + if (parseArgument (argc, argv, "-sr_ra", sr_ra) != -1) + { + print_info (stderr, "Changing the <Rectify Amplitude Image> settings on the SwissRanger camera to: "); print_value (stderr, "%s\n", BOOL2STR_ED (sr_ra)); + p.set_param ("/composite/sr_rectify_amplitude", sr_ra); + } + if (parseArgument (argc, argv, "-sr_ra", sr_rc) != -1) + { + print_info (stderr, "Changing the <Rectify Confidence Image> settings on the SwissRanger camera to: "); print_value (stderr, "%s\n", BOOL2STR_ED (sr_rc)); + p.set_param ("/composite/sr_rectify_confidence", sr_rc); + } + if (parseArgument (argc, argv, "-sr_fpcd", sr_fpcd) != -1) + { + print_info (stderr, "Changing the <Filter PCD> settings on the SwissRanger camera to: "); print_value (stderr, "%s\n", BOOL2STR_ED (sr_fpcd)); + p.set_param ("/composite/sr_pcd_filter", sr_fpcd); + } + // Shutdown p.shutdown (); Added: drivers/composite/visualizer_fusion/src/visualizer_fusion.cpp =================================================================== --- drivers/composite/visualizer_fusion/src/visualizer_fusion.cpp (rev 0) +++ drivers/composite/visualizer_fusion/src/visualizer_fusion.cpp 2008-11-17 13:44:10 UTC (rev 6) @@ -0,0 +1,743 @@ +/* + * 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, Inc. 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. + */ + +/* author: Radu Bogdan Rusu <ru...@cs...> */ + +/** + +@mainpage + +@htmlinclude manifest.html + +@b visualizer_fusion is a driver which subscribes to 5 different topics (see below), gets the data, applies the necessary transformation, +and visualizes the results on screen using VTK. + +<hr> + +@section usage Usage +@verbatim +$ visualizer_fusion [standard ROS args] +@endverbatim + +@par Example + +@verbatim +$ visualizer_fusion +@endverbatim + +<hr> + +@section topic ROS topics + +Subscribes to (name/type): +- @b "cloud_sr"/<a href="../../std_msgs/html/classstd__msgs_1_1PointCloud.html">PointCloud</a> : Point cloud data from the SwissRanger. +- @b "cloud_stoc"/<a href="../../std_msgs/html/classstd__msgs_1_1PointCloud.html">PointCloud</a> : Point cloud data from the STOC. +- @b "images_sr"/<a href="../../std_msgs/html/classstd__msgs_1_1ImageArray.html">ImageArray</a> : Distance and intensity camera images from the SwissRanger +- @b "images_stoc"/<a href="../../std_msgs/html/classstd__msgs_1_1ImageArray.html">ImageArray</a> : Left, right, and disparity camera images from the STOC. +- @b "image_flir"/<a href="../../std_msgs/html/classstd__msgs_1_1Image.html">Image</a> : Thermal camera image from the FLIR. + + **/ + +// ROS core +#include "ros/node.h" +#include "ros/time.h" +#include "ros/common.h" + +#include "std_msgs/PointCloud.h" +#include "std_msgs/Point32.h" +#include "std_msgs/ImageArray.h" +#include "std_msgs/Image.h" + +#include "tf/tf.h" +#include "tf/transform_listener.h" +#include "tf/transform_broadcaster.h" + +// VTK includes +#include "vtk_tools.h" + +using namespace std; + +class VisualizerFusion: public ros::node +{ + public: + + // Flags + bool dump_to_disk_, sr_enable_, stoc_enable_, flir_enable_; + + // VTK stuff + XtAppContext app_context_; + vtkRenderer *ren_; + vtkRenderWindow *ren_win_; + vtkXRenderWindowInteractor *iren_; + vtkWGInteractorStyle* style_; +// vtkInteractorStyleTrackballCamera* style_; + +// vtkTextActor* fps_txt_; +// vtkFPSCallback* fps_cbk_; + + vtkPolyData *sr_vdata_, *stoc_vdata_; + vtkPolyDataMapper *sr_vmapper_, *stoc_vmapper_; + vtkActor *sr_vactor_, *stoc_vactor_; + vtkCriticalSection *sr_lock_;//, *stoc_lock_; + + // ROS messages + std_msgs::PointCloud sr_msg_cloud_, stoc_msg_cloud_; + std_msgs::ImageArray sr_msg_images_, stoc_msg_images_; + std_msgs::Image flir_msg_image_, stoc_msg_left_image_; + + // TF transformations +// tf::TransformListener sr_stoc_tfl_; +// tf::TransformBroadcaster *sr_stoc_tfl_b_; + + bool stoc_image_just_left_; + + NEWMAT::Matrix + getIntrinsic_SR4k () + { + NEWMAT::Matrix mat (3, 3); + mat + << 243.3872401954 << 0.0000000000 << 93.3830107632 + << 0.0000000000 << 242.8973902804 << 89.0503222681 + << 0.0000000000 << 0.0000000000 << 1.0000000000 + ; + // Camera Distortion Coefficients: << -0.8598466633 << 0.6007425277 << -0.0268755414 << -0.0162393427 + return (mat); + } + + NEWMAT::Matrix + getIntrinsic_STOCLeftforSR4k () + { + NEWMAT::Matrix mat (3, 3); + mat +// << 416.3264248449 << 0.0000000000 << 325.4263388542 +// << 0.0000000000 << 415.4679821276 << 232.6518187203 +// << 0.0000000000 << 0.0000000000 << 1.0000000000 + +// << 422.477840 << 0.0000000000 << 299.101309 +// << 0.0000000000 << 421.924602 << 229.911884 +// << 0.0000000000 << 0.0000000000 << 1.0000000000 + +/* << 433.6819 << 0.0000 << 326.9448 + << 0.0000 << 433.3292 << 208.2214 + << 0.0000 << 0.0000 << 1.0000 + ;*/ + + << 429.609702 << 0.000000 << 309.444590 + << 0.000000 << 429.070702 << 222.472089 + << 0.000000 << 0.000000 << 1.000000 + + ; + // Camera Distortion Coefficients: << -0.0315072078 << 0.0711552567 << 0.0095189250 << 0.0104319110 + return (mat); + } + + NEWMAT::Matrix + getIntrinsic_STOCLeftforThermal () + { + NEWMAT::Matrix mat (3, 3); + mat + << 419.6911150447 << 0.0000000000 << 357.7189020786 + << 0.0000000000 << 417.8570568447 << 211.5310351584 + << 0.0000000000 << 0.0000000000 << 1.0000000000 + ; + // Camera Distortion Coefficients: << 0.0560644870 << -0.2416229823 << -0.0095165121 << 0.0338270278 + return (mat); + } + NEWMAT::Matrix + getDistortion_STOCLeftForThermal () + { + NEWMAT::Matrix mat (1, 4); + mat + << 0.0015 << -0.0124 << 0.0043 << 0.0121 + ; + return (mat); + } + + NEWMAT::Matrix + getIntrinsic_Thermal () + { + NEWMAT::Matrix mat (3, 3); + mat + << 353.1375800333 << 0.0000000000 << 152.8744430024 + << 0.0000000000 << 372.2732976233 << 116.4645386707 + << 0.0000000000 << 0.0000000000 << 1.0000000000 + ; + // Camera Distortion Coefficients: << -0.4499672276 << 0.3231831656 << -0.0038213355 << -0.0079903325 + return (mat); + } + + NEWMAT::Matrix + getExtrinsic_STOCLeftThermal () + { + NEWMAT::Matrix mat (4, 4); + mat + << 0.9991 << 0.0196 << -0.0382 << 6.2801 + << -0.0180 << 0.9990 << 0.0409 << -58.7377 + << 0.0390 << -0.0402 << 0.9984 << - 8.7151 + << 0 << 0 << 0 << 1 + ; + return (mat); + } + + NEWMAT::Matrix + getExtrinsic_ThermalSTOCLeft () + { + NEWMAT::Matrix mat (4, 4); + mat + << 0.9999300360 << -0.0031469852 << -0.0114026163 << -0.2611269426 + << 0.0033616696 << 0.9998165272 << 0.0188576552 << 50.2231430676 + << 0.0113411794 << -0.0188946677 << 0.9997571551 << -14.0321004046 + << 0.0000000000 << 0.0000000000 << 0.0000000000 << 1.0000000000 + ; + return (mat); + } + + NEWMAT::Matrix + getExtrinsic_STOCLeftSR4k () + { + NEWMAT::Matrix mat (4, 4); + mat + << 0.9987 << 0.0456 << 0.0248 << -68.9398 + << -0.0448 << 0.9985 << -0.0306 << -57.5900 + << -0.0262 << 0.0295 << 0.9992 << 43.9617 + << 0.0000000000 << 0.0000000000 << 0.0000000000 << 1.0000000000 + ; + return (mat); + } + + NEWMAT::Matrix + getExtrinsic_SR4kSTOCLeft () + { + NEWMAT::Matrix mat (4, 4); + mat +/* GOOD << 0.9998348686 << 0.0121139309 << 0.0135457856 << -105.2099177879 + << -0.0122397937 << 0.9998823270 << 0.0092476829 << -46.9201656270 + << -0.0134321658 << -0.0094119534 << 0.9998654870 << 29.2877791604 + << 0.0000000000 << 0.0000000000 << 0.0000000000 << 1.0000000000 +*/ + +/*good2*/ + << 0.9998 << -0.0116 << 0.0162 << 100.9246 + << 0.0116 << 0.9999 << 0.0018 << 54.6890 + << -0.0162 << -0.0016 << 0.9999 << -31.0171 + << 0 << 0 << 0 << 1 + ; + for (int d = 1; d < 4; d++) + mat (d, 4) /= 1000; + + return (mat); + } + + NEWMAT::Matrix sr_to_stoc_; + + VisualizerFusion () : ros::node ("visualizer_fusion"), dump_to_disk_(false), sr_enable_(true), + stoc_enable_(true), flir_enable_(true), + sr_vdata_(NULL), + stoc_vdata_(NULL), + sr_vmapper_(NULL), + stoc_vmapper_(NULL), + sr_vactor_(NULL), + stoc_vactor_(NULL), + stoc_image_just_left_(false) +// sr_stoc_tfl_ (*this, true, 10000000000ULL, 0) // node, interpoation, cache_time, extrapolation_distance + { + ren_ = vtkRenderer::New (); + +// fps_txt_ = vtkTextActor::New (); +// fps_cbk_ = vtkFPSCallback::New (); +// fps_cbk_->SetTextActor (fps_txt_); + + ren_win_ = vtkRenderWindow:: New (); + ren_win_->SetWindowName ("Visualizer Fusion"); + sr_lock_ = vtkCriticalSection::New (); +// stoc_lock_ = vtkCriticalSection::New (); + +// sr_stoc_tfl_b_ = new tf::TransformBroadcaster (*this); + +// NEWMAT::Matrix sr_stoc_calib = getExtrinsic_STOCLeftSR4k (); + sr_to_stoc_ = getExtrinsic_SR4kSTOCLeft (); + } + + ~VisualizerFusion () + { + } + + //////////////////////////////////////////////////////////////////////////////// + void + rotateCloud (std_msgs::PointCloud &cloud, NEWMAT::Matrix transform) + { + double t1x, t1y, t1z; + for (unsigned int i = 0; i < cloud.get_pts_size (); i++) + { + // Rotate first + t1x = cloud.pts[i].x * transform(1, 1) + + cloud.pts[i].y * transform(1, 2) + + cloud.pts[i].z * transform(1, 3) + + transform(1, 4); + + t1y = cloud.pts[i].x * transform(2, 1) + + cloud.pts[i].y * transform(2, 2) + + cloud.pts[i].z * transform(2, 3) + + transform(2, 4); + + t1z = cloud.pts[i].x * transform(3, 1) + + cloud.pts[i].y * transform(3, 2) + + cloud.pts[i].z * transform(3, 3) + + transform(3, 4); + cloud.pts[i].x = t1x; + cloud.pts[i].y = t1y; + cloud.pts[i].z = t1z; + } + } + + //////////////////////////////////////////////////////////////////////////////// + void + printMatrix (NEWMAT::Matrix mat) + { + for (int i = 1; i < 5; i++) + { + for (int j = 1; j < 5; j++) + cerr << mat(i, j) << " "; + cerr << endl; + } + } + + //////////////////////////////////////////////////////////////////////////////// + // Rotate an image with 180 in place + void + rotateImage180 (std_msgs::Image &image) + { + NEWMAT::Matrix rotation180 (3, 3); + + rotation180 << -1 << 0 << int(image.width) + << 0 << -1 << -int(image.height) + << 0 << 0 << 1; + + vector<uint8_t> imgdata; + memcpy (&(imgdata[0]), &(image.data[0]), image.get_data_size ()); + + for (unsigned int i = 0; i < image.height; i++) + { + for (unsigned int j = 0; j < image.width; j++) + { + // Rotate + int ni = j * rotation180(1, 1) + + i * rotation180(1, 2) + + rotation180(1, 3); + + int nj = j * rotation180(2, 1) + + i * rotation180(2, 2) + + rotation180(2, 3); + image.data[ni * image.width + nj] = imgdata[i * image.width + j]; + } + } + + + } + + //////////////////////////////////////////////////////////////////////////////// + void + rotateCloud (std_msgs::PointCloud &cloud) + { + // Construct a matrix for rotating the pointcloud with 180 degrees + for (unsigned int i = 0; i < cloud.get_pts_size (); i++) + { + cloud.pts[i].x *= - 1.0; + cloud.pts[i].y *= - 1.0; + } + + // Transform the pointcloud from the SwissRanger coordinate system to the STOC coordinate system + rotateCloud (cloud, sr_to_stoc_); + + +/* sr_stoc_tfl_b_->sendTransform (tf::Stamped<tf::Transform> (tf::Transform ( + tf::Quaternion (pdata->pos.pa, 0, 0), + tf::Point(pdata->pos.px, pdata->pos.py, 0.0) + ), ros::Time::now (), "SR-STOC-left"));*/ + +/* sr_stoc_tfl_.setTransform (tf::Stamped<btTransform> (btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), 0, "base_laser", "base")); + + try + { + this->tf.transformPointCloud ("map", cloud, global_cloud); + } + catch (tf::Exception& ex) + { + ROS_WARN("%s\n", ex.what ()); + }*/ + } + + //////////////////////////////////////////////////////////////////////////////// + void + convertROSPointCloudToVTK (std_msgs::PointCloud msg_cloud, vtkPolyData **pdata) + { + int nr_pts = msg_cloud.get_pts_size (); + int dim = msg_cloud.get_chan_size (); + + // These are all local + vtkPoints *points; + vtkUnsignedCharArray *colors; + vtkCellArray *verts; + + // First time this is called, create the objects + if (*pdata == NULL) + { + *pdata = vtkPolyData::New (); + + points = vtkPoints::New (); + points->SetDataTypeToFloat (); + + verts = vtkCellArray::New (); + + colors = vtkUnsignedCharArray::New (); + ROS_INFO ("[VisualizerFusion::convertROSPointCloudToVTK] Init VTK data structure."); + } + // After that, just point back to them + else + { + points = reinterpret_cast<vtkPolyData*>(*pdata)->GetPoints (); + verts = reinterpret_cast<vtkPolyData*>(*pdata)->GetVerts (); + colors = reinterpret_cast<vtkUnsignedCharArray*>(reinterpret_cast<vtkPolyData*>(*pdata)->GetPointData ()->GetScalars ()); + } + + // Re-init structures + points->SetNumberOfPoints (nr_pts); + colors->SetNumberOfComponents (dim); + // It's *extremely* important that SetNumberOfTuples be called *after* SetNumberOfComponents (!) + colors->SetNumberOfTuples (nr_pts); + verts->Initialize (); + verts->InsertNextCell (nr_pts); + + // Copy data + double p[3]; + for (vtkIdType i = 0; i < nr_pts; i++) + { + p[0] = msg_cloud.pts[i].x; p[1] = msg_cloud.pts[i].y; p[2] = msg_cloud.pts[i].z; + points->SetPoint (i, p); + verts->InsertCellPoint (i); + + + for (int d = 0; d < dim; d++) + { + colors->SetComponent (i, d, msg_cloud.chan[d].vals[i]); +// if (msg_cloud.chan[d].name == "r") +// colors->SetComponent (i, 0, msg_cloud.chan[d].vals[i]); +// if (msg_cloud.chan[d].name == "g") +// colors->SetComponent (i, 1, msg_cloud.chan[d].vals[i]); +// if (msg_cloud.chan[d].name == "b") +// colors->SetComponent (i, 2, msg_cloud.chan[d].vals[i]); + } + } + + reinterpret_cast<vtkPolyData*>(*pdata)->SetPoints (points); + reinterpret_cast<vtkPolyData*>(*pdata)->SetVerts (verts); + reinterpret_cast<vtkPolyData*>(*pdata)->GetPointData ()->SetScalars (colors); + } + + //////////////////////////////////////////////////////////////////////////////// + void + getPixelColor (std_msgs::PointCloud &cloud, NEWMAT::Matrix intrinsic_mat, std_msgs::Image image) + { + cloud.set_chan_size (3); + cloud.chan[0].name = "r"; + cloud.chan[1].name = "g"; + cloud.chan[2].name = "b"; + cloud.chan[0].set_vals_size (cloud.get_pts_size ()); + cloud.chan[1].set_vals_size (cloud.get_pts_size ()); + cloud.chan[2].set_vals_size (cloud.get_pts_size ()); + + double p[3], ptr[3]; + for (unsigned int i = 0; i < cloud.get_pts_size (); i++) + { + p[0] = cloud.pts[i].x; + p[1] = cloud.pts[i].y; + p[2] = cloud.pts[i].z; +// p[0] = (cloud.pts[i].x / cloud.pts[i].z); +// p[1] = (cloud.pts[i].y / cloud.pts[i].z); +// p[2] = 1.0; + + ptr[0] = p[0] * intrinsic_mat(1, 1) + p[1] * intrinsic_mat(1, 2) + p[2] * intrinsic_mat(1, 3); + ptr[1] = p[0] * intrinsic_mat(2, 1) + p[1] * intrinsic_mat(2, 2) + p[2] * intrinsic_mat(2, 3); + ptr[2] = p[0] * intrinsic_mat(3, 1) + p[1] * intrinsic_mat(3, 2) + p[2] * intrinsic_mat(3, 3); + +// if (ptr[2] < 1e-5) +// continue; + ptr[0] /= ptr[2]; + ptr[1] /= ptr[2]; + + if ( (ptr[0] > 640) || (ptr[1] > 480) || (ptr[0] < 0) || (ptr[1] < 0)); + else + { + int u = (ptr[0]); + int v = (ptr[1]); +// cerr << u << " " << v << " " << u + v * 640 << endl; + cloud.chan[0].vals[i] = image.data[ (u + v * 640) * 3 + 0]; + cloud.chan[1].vals[i] = image.data[ (u + v * 640) * 3 + 1]; + cloud.chan[2].vals[i] = image.data[ (u + v * 640) * 3 + 2]; + } + } + } + + //////////////////////////////////////////////////////////////////////////////// + void + getPixelColor (std_msgs::PointCloud &cloud, std_msgs::Image image) + { + NEWMAT::Matrix homography (3, 3); + homography + << 2.1402 << 0.0623 << 160.3371 + << 0.2101 << 1.9134 << 89.9841 + << 0.0006 << 0.0002 << 1.0000 + ; + + cloud.set_chan_size (4); + cloud.chan[0].name = "pid"; + cloud.chan[1].name = "r"; + cloud.chan[2].name = "g"; + cloud.chan[3].name = "b"; + + cloud.chan[0].set_vals_size (cloud.get_pts_size ()); + cloud.chan[1].set_vals_size (cloud.get_pts_size ()); + cloud.chan[2].set_vals_size (cloud.get_pts_size ()); + cloud.chan[3].set_vals_size (cloud.get_pts_size ()); + + int img_width = image.width, img_height = image.height, sr_width = 176, sr_height = 144; + + std_msgs::Point32 p, q; + for (unsigned int i = 0; i < cloud.get_pts_size (); i++) + { + int pid = cloud.chan[0].vals[i]; + p.y = round (pid / sr_width); + p.x = round (pid % sr_width); + p.z = 1; + + q.x = p.x * homography(1, 1) + p.y * homography(1, 2) + p.z * homography(1, 3); + q.y = p.x * homography(2, 1) + p.y * homography(2, 2) + p.z * homography(2, 3); + q.z = p.x * homography(3, 1) + p.y * homography(3, 2) + p.z * homography(3, 3); + + int nv = round (q.x / q.z); + int nu = round (q.y / q.z); + + cloud.chan[0].vals[i] = pid; + cloud.chan[1].vals[i] = image.data[(nu * img_width + nv) * 3 + 0]; + cloud.chan[2].vals[i] = image.data[(nu * img_width + nv) * 3 + 1]; + cloud.chan[3].vals[i] = image.data[(nu * img_width + nv) * 3 + 2]; + +// cloud.chan[1].vals[i] = image.data[(p.x * img_width + p.y) * 2 + 1]; +// cloud.chan[2].vals[i] = image.data[(p.x * img_width + p.y) * 2 + 1]; +// cloud.chan[3].vals[i] = image.data[(p.x * img_width + p.y) * 2 + 1]; + } + } + + //////////////////////////////////////////////////////////////////////////////// + void + sr_cloud_cb () + { + // Apply the necessary transformations + + rotateCloud (sr_msg_cloud_); + sr_lock_->Lock (); + + if (stoc_image_just_left_ && stoc_msg_left_image_.get_data_size () > 0) + getPixelColor (sr_msg_cloud_, getIntrinsic_STOCLeftforSR4k (), stoc_msg_left_image_); + else if (stoc_msg_images_.get_images_size () > 0) + getPixelColor (sr_msg_cloud_, getIntrinsic_STOCLeftforSR4k (), stoc_msg_images_.images[0]); +// if (sr_msg_images_.get_images_size () > 0) +// getPixelColor (sr_msg_cloud_, sr_msg_images_.images[1]); +// if (stoc_msg_images_.get_images_size () > 0) +// getPixelColor (sr_msg_cloud_, stoc_msg_images_.images[0]); + sr_lock_->Unlock (); + + // Convert the data to VTK format + sr_lock_->Lock (); + convertROSPointCloudToVTK (sr_msg_cloud_, &sr_vdata_); + sr_vdata_->Update (); + sr_lock_->Unlock (); + fprintf (stderr, "[sr_cloud_cb] Received data packet with %d points.\n", sr_vdata_->GetNumberOfPoints ()); + + + + // First time we get data, we create the actor and add it to the renderer + if (sr_vactor_ == NULL) + { + ROS_INFO ("[VisualizerFusion::sr_cloud_cb] Init VTK actor."); + sr_lock_->Lock (); + sr_vmapper_ = vtkPolyDataMapper::New (); + sr_vmapper_->SetInput (sr_vdata_); + sr_vmapper_->SetScalarModeToUsePointData (); + sr_vmapper_->ScalarVisibilityOn (); + + sr_vactor_ = vtkLODActor::New (); + reinterpret_cast<vtkLODActor*>(sr_vactor_)->SetNumberOfCloudPoints (sr_vdata_->GetNumberOfPoints () / 10); + sr_vactor_->SetMapper (sr_vmapper_); + sr_vactor_->GetProperty ()->SetRepresentationToPoints (); + sr_vactor_->GetProperty ()->SetPointSize (4); + sr_vactor_->GetProperty ()->SetInterpolationToFlat (); + ren_->AddActor (sr_vactor_); + sr_lock_->Unlock (); + } + else + { + sr_lock_->Lock (); + sr_vdata_->Modified (); + sr_vmapper_->Modified (); + sr_vactor_->Modified (); + sr_lock_->Unlock (); + } + } + + //////////////////////////////////////////////////////////////////////////////// + void + stoc_cloud_cb () + { + // Convert the data to VTK format + sr_lock_->Lock (); + convertROSPointCloudToVTK (stoc_msg_cloud_, &stoc_vdata_); + sr_lock_->Unlock (); + fprintf (stderr, "[stoc_cloud_cb] Received data packet with %d points.\n", stoc_vdata_->GetNumberOfPoints ()); + + // First time we get data, we create the actor and add it to the renderer + if (stoc_vactor_ == NULL) + { + sr_lock_->Lock (); + ROS_INFO ("[VisualizerFusion::stoc_cloud_cb] Init VTK actor."); + stoc_vmapper_ = vtkPolyDataMapper::New (); + stoc_vmapper_->SetInput (stoc_vdata_); + stoc_vmapper_->SetScalarModeToUsePointData (); + stoc_vmapper_->ScalarVisibilityOn (); + + stoc_vactor_ = vtkLODActor::New (); + reinterpret_cast<vtkLODActor*>(stoc_vactor_)->SetNumberOfCloudPoints (stoc_vdata_->GetNumberOfPoints () / 10); + stoc_vactor_->SetMapper (stoc_vmapper_); + stoc_vactor_->GetProperty ()->SetRepresentationToPoints (); + stoc_vactor_->GetProperty ()->SetInterpolationToFlat (); + ren_->AddActor (stoc_vactor_); + sr_lock_->Unlock (); + } + else + { + sr_lock_->Lock (); + stoc_vdata_->Modified (); + stoc_vmapper_->Modified (); + stoc_vactor_->Modified (); + sr_lock_->Unlock (); + } + } + + // We don't bother with these callbacks for now + void stoc_images_cb () {} + void sr_images_cb () {} + void flir_image_cb () {} + + //////////////////////////////////////////////////////////////////////////////// + void + init (int width, int height) + { + // VTK related init + ren_->SetBackground (1.0, 1.0, 1.0); +// ren_->AddObserver (vtkCommand::EndEvent, fps_cbk_); +// fps_txt_->GetProperty ()->SetColor (0.0, 0.0, 1.0); +// ren_->AddActor (fps_txt_); + + ren_win_->SetSize (width, height); + ren_win_->AddRenderer (ren_); + +// vtkCriticalSection* lock1 = vtkCriticalSection::New (); +// vtkCriticalSection* lock2 = vtkCriticalSection::New (); + + style_ = vtkWGInteractorStyle::New (); + +// style_ = vtkInteractorStyleTrackballCamera::New (); + style_->ren_win_ = ren_win_; +// style_->lock_timer_ = sr_lock_;//lock1; +// style_->lock_char_ = sr_lock_;//;lock2; +// style_->UseTimersOn (); + + iren_ = vtkXRenderWindowInteractor::New (); + iren_->SetRenderWindow (ren_win_); + iren_->Initialize (app_context_); + iren_->SetInteractorStyle (style_); + + if (flir_enable_) + { + subscribe ("image_flir", flir_msg_image_, &VisualizerFusion::flir_image_cb, 1); + + } + + if (stoc_enable_) + { +// subscribe ("cloud_stoc", stoc_msg_cloud_, &VisualizerFusion::stoc_cloud_cb, 1); + if (stoc_image_just_left_) + subscribe ("image_stoc_left", stoc_msg_left_image_, &VisualizerFusion::stoc_images_cb, 1); + else + subscribe ("images_stoc", stoc_msg_images_, &VisualizerFusion::stoc_images_cb, 1); + } + + if (sr_enable_) + { + subscribe ("cloud_sr", sr_msg_cloud_, &VisualizerFusion::sr_cloud_cb, 1); +// subscribe ("images_sr", sr_msg_images_, &VisualizerFusion::sr_images_cb, 1); + } + } + +}; + +/* ---[ */ +int + main (int argc, char** argv) +{ + ros::init (argc, argv); + + XtToolkitInitialize (); + + // Here is where we create our ROS node + VisualizerFusion f; + f.app_context_ = XtCreateApplicationContext (); + f.init (1000, 800); + + // Initialize the VTK interactor and start the ROS node + while (f.ok ()) + { + usleep (100000); + f.sr_lock_->Lock (); + if (XtAppPending (f.app_context_)) + XtAppProcessEvent (f.app_context_, XtIMAll); + + f.ren_win_->Render (); + f.sr_lock_->Unlock (); + } + +// f.iren_->Initialize (); +// f.iren_->CreateRepeatingTimer (10); +// f.iren_->Start (); + + ros::fini (); + + return (0); +} +/* ]--- */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2008-11-17 12:32:06
|
Revision: 5 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=5&view=rev Author: veedee Date: 2008-11-17 12:32:04 +0000 (Mon, 17 Nov 2008) Log Message: ----------- cleanups for the SR4k driver Modified Paths: -------------- drivers/composite/swissranger_driver/src/swissranger.cpp drivers/composite/swissranger_driver/src/swissranger.h Modified: drivers/composite/swissranger_driver/src/swissranger.cpp =================================================================== --- drivers/composite/swissranger_driver/src/swissranger.cpp 2008-11-17 10:13:49 UTC (rev 4) +++ drivers/composite/swissranger_driver/src/swissranger.cpp 2008-11-17 12:32:04 UTC (rev 5) @@ -56,10 +56,15 @@ cvmSet (intrinsic_, 1, 0, 0.0); cvmSet (intrinsic_, 1, 1, 252.9221); cvmSet (intrinsic_, 1, 2, 72.6264); cvmSet (intrinsic_, 2, 0, 0.0); cvmSet (intrinsic_, 2, 1, 0.0); cvmSet (intrinsic_, 2, 2, 1.0); cvmSet (distortion_, 0, 0, -0.0612); cvmSet (distortion_, 0, 1, 0.1706); cvmSet (distortion_, 0, 2, 0); cvmSet (distortion_, 0, 3, 0); + + pcd_filter_ = false; + undistort_distance_ = undistort_amplitude_ = undistort_confidence_ = false; } swissranger::SwissRanger::~SwissRanger () { + cvReleaseMat (&intrinsic_); + cvReleaseMat (&distortion_); } //////////////////////////////////////////////////////////////////////////////// @@ -242,36 +247,56 @@ // Stores a copy of the rotated distance image uint8_t *img_tmp = (uint8_t*)malloc (image_size); - memcpy (img_tmp, distance_image, image_size); - // Rotate the distance image with 180 on spot -// rotateImage180 (img_tmp, distance_image, imgEntryArray_->width, imgEntryArray_->height); - rotateImage180 (distance_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height); - // Undistort the distance image - undistort (img_tmp, distance_image, imgEntryArray_->width, imgEntryArray_->height); + if (undistort_distance_) + { + // Rotate the distance image with 180 on spot + rotateImage180 (distance_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height); + // Undistort the distance image + undistort (img_tmp, distance_image, imgEntryArray_->width, imgEntryArray_->height); + } + else + // Rotate the distance image with 180 on spot + rotateImage180 (img_tmp, distance_image, imgEntryArray_->width, imgEntryArray_->height); + memcpy (img_tmp, amplitude_image, image_size); - // Rotate the amplitude image with 180 on spot - //rotateImage180 (amplitude_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height); - rotateImage180 (img_tmp, amplitude_image, imgEntryArray_->width, imgEntryArray_->height); - // Undistort the amplitude image -// undistort (img_tmp, amplitude_image, imgEntryArray_->width, imgEntryArray_->height); + + if (undistort_amplitude_) + { + // Rotate the amplitude image with 180 on spot + rotateImage180 (amplitude_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height); + // Undistort the amplitude image + undistort (img_tmp, amplitude_image, imgEntryArray_->width, imgEntryArray_->height); + } + else + // Rotate the amplitude image with 180 on spot + rotateImage180 (img_tmp, amplitude_image, imgEntryArray_->width, imgEntryArray_->height); memcpy (img_tmp, confidence_image, image_size); - // Rotate the confidence image with 180 on spot -// rotateImage180 (confidence_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height); -//contours (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height, 1); - rotateImage180 (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height); - // Undistort the confidence image -// undistort (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height); - + + if (undistort_confidence_) + { + // Rotate the confidence image with 180 on spot + rotateImage180 (confidence_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height); + //contours (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height, 1); + // Undistort the confidence image + undistort (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height); + } + else + // Rotate the confidence image with 180 on spot + rotateImage180 (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height); + // Points array res = SR_CoordTrfFlt (srCam_, xp_, yp_, zp_, sizeof (float), sizeof (float), sizeof (float)); // Filter points cloud.set_pts_size (imgEntryArray_->width * imgEntryArray_->height); cloud.set_chan_size (2); - cloud.chan[0].name = "pid"; + if (pcd_filter_) // if in-driver filtering is enabled, do not send the point confidence anymore, but rather the original point index + cloud.chan[0].name = "pid"; + else + cloud.chan[0].name = "con"; cloud.chan[0].set_vals_size (imgEntryArray_->width * imgEntryArray_->height); cloud.chan[1].name = "i"; cloud.chan[1].set_vals_size (imgEntryArray_->width * imgEntryArray_->height); @@ -283,22 +308,30 @@ for (int v = 0; v < imgEntryArray_->width; v++) { int i = nu + v; - unsigned short conf_val = (confidence_image[i * 2 + 0] << 0) + (confidence_image[i * 2 + 1] << 8); - unsigned short dist_val = (distance_image[i * 2 + 0] << 0) + (distance_image[i * 2 + 1] << 8); - if ( (conf_val > 0xffff * 3 / 4) && (dist_val < 0xff00) ) + + if (pcd_filter_) // is in-driver filtering enabled ? { -// distance_image[i + 0] = 0; -// distance_image[i + 1] = 0; - if (getAngle (xp_[i], yp_[i], zp_[i], xp_[i+1], yp_[i+1], zp_[i+1]) > 0.0004) - continue; - cloud.pts[nr_pts].x = xp_[i]; - cloud.pts[nr_pts].y = yp_[i]; - cloud.pts[nr_pts].z = zp_[i]; + unsigned short conf_val = (confidence_image[i * 2 + 0] << 0) + (confidence_image[i * 2 + 1] << 8); + unsigned short dist_val = (distance_image[i * 2 + 0] << 0) + (distance_image[i * 2 + 1] << 8); + if ( (conf_val > 0xffff * 3 / 4) && (dist_val < 0xff00) ) + { + if (getAngle (xp_[i], yp_[i], zp_[i], xp_[i+1], yp_[i+1], zp_[i+1]) > 0.0004) + continue; + } + } + // Save the XYZ coordinates + cloud.pts[nr_pts].x = xp_[i]; + cloud.pts[nr_pts].y = yp_[i]; + cloud.pts[nr_pts].z = zp_[i]; + + if (pcd_filter_) // is in-driver filtering enabled ? cloud.chan[0].vals[nr_pts] = i; - cloud.chan[1].vals[nr_pts] = amplitude_image[i*2 + 1]; - nr_pts++; - } + else + cloud.chan[0].vals[nr_pts] = (confidence_image[i * 2 + 0] << 0) + (confidence_image[i * 2 + 1] << 8); + + cloud.chan[1].vals[nr_pts] = (amplitude_image[i * 2 + 0] << 0) + (amplitude_image[i * 2 + 1] << 8); //amplitude_image[i*2 + 1]; + nr_pts++; } } cloud.pts.resize (nr_pts); @@ -306,7 +339,7 @@ cloud.chan[1].vals.resize (nr_pts); // Fill in the ROS PointCloud message -/* for (int i = 0; i < imgEntryArray_->width * imgEntryArray_->height; i++) + /*for (int i = 0; i < imgEntryArray_->width * imgEntryArray_->height; i++) { cloud.pts[i].x = xp_[i]; cloud.pts[i].y = yp_[i]; @@ -326,7 +359,7 @@ images.images[1].width = cols_; images.images[1].height = rows_; -// if (MODE & AM_CONV_GRAY) + //if (MODE & AM_CONV_GRAY) images.images[1].colorspace = "mono16"; images.images[1].compression = "raw"; images.images[1].label = "sr4k-intensity"; @@ -408,7 +441,7 @@ //////////////////////////////////////////////////////////////////////////////// // Obtain the device product name -string +std::string swissranger::SwissRanger::getDeviceString () { char *buf = new char[256]; @@ -416,13 +449,13 @@ SR_GetDeviceString (srCam_, buf, *buflen); // VendorID:0x%04x, ProductID:0x%04x, Manufacturer:'%s', Product:'%s' - string sensor (buf); - string::size_type loc = sensor.find ("Product:", 0); - if (loc != string::npos) + std::string sensor (buf); + std::string::size_type loc = sensor.find ("Product:", 0); + if (loc != std::string::npos) { sensor = sensor.substr (loc + 9, *buflen); loc = sensor.find ("'", 0); - if (loc != string::npos) + if (loc != std::string::npos) sensor = sensor.substr (0, loc); } else @@ -435,12 +468,12 @@ //////////////////////////////////////////////////////////////////////////////// // Obtain the libusbSR library version -string +std::string swissranger::SwissRanger::getLibraryVersion () { unsigned short version[4]; char buf[80]; SR_GetVersion (version); snprintf (buf, sizeof (buf), "%d.%d.%d.%d", version[3], version[2], version[1], version[0]); - return (string (buf)); + return (std::string (buf)); } Modified: drivers/composite/swissranger_driver/src/swissranger.h =================================================================== --- drivers/composite/swissranger_driver/src/swissranger.h 2008-11-17 10:13:49 UTC (rev 4) +++ drivers/composite/swissranger_driver/src/swissranger.h 2008-11-17 12:32:04 UTC (rev 5) @@ -43,6 +43,10 @@ // For some reason 1.0.10-541 does not provide this #define DWORD unsigned int +#define SR_IMG_DISTANCE 0 +#define SR_IMG_AMPLITUDE 1 +#define SR_IMG_CONFIDENCE 2 + #include <libusbSR.h> // ROS include @@ -52,7 +56,6 @@ // Older library: #define MODE (AM_COR_FIX_PTRN | AM_COR_LED_NON_LIN | AM_MEDIAN) #define MODE (AM_CONF_MAP | AM_COR_FIX_PTRN | AM_SW_ANF | AM_MEDIAN | AM_DENOISE_ANF | AM_MEDIANCROSS | AM_CONV_GRAY)// | AM_SHORT_RANGE) -using namespace std; namespace swissranger { @@ -91,9 +94,32 @@ // SwissRanger specific values unsigned int rows_, cols_, inr_; - string device_id_; - string lib_version_; + std::string device_id_; + std::string lib_version_; + + // Get/Set filtering values + bool getPCDFilter () { return this->pcd_filter_; } + void setPCDFilter (bool filter) { this->pcd_filter_ = filter; } + bool getUndistortImage (int img_type) + { + switch (img_type) + { + case SR_IMG_DISTANCE: return (this->undistort_distance_); + case SR_IMG_AMPLITUDE: return (this->undistort_amplitude_); + case SR_IMG_CONFIDENCE: return (this->undistort_confidence_); + } + } + void setUndistortImage (int img_type, bool filter) + { + switch (img_type) + { + case SR_IMG_DISTANCE: { this->undistort_distance_ = filter; break; } + case SR_IMG_AMPLITUDE: { this->undistort_amplitude_ = filter; break; } + case SR_IMG_CONFIDENCE: { this->undistort_confidence_ = filter; break; } + } + } + private: // device identifier CMesaDevice* srCam_; @@ -103,8 +129,8 @@ int integration_time_, modulation_freq_; - string getDeviceString (); - string getLibraryVersion (); + std::string getDeviceString (); + std::string getLibraryVersion (); // Used for rotating images with 180 deg around their centre internally void rotateImage180 (uint8_t *img, uint8_t *rot_img, int width, int height); @@ -114,6 +140,9 @@ void undistort (uint8_t *img, uint8_t *un_img, int width, int height); void contours (uint8_t *img, uint8_t *con_img, int width, int height, int threshold); double getAngle (float px, float py, float pz, float qx, float qy, float qz); + + bool pcd_filter_; // in-driver PCD filtering + bool undistort_distance_, undistort_amplitude_, undistort_confidence_; }; }; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2008-11-17 10:13:55
|
Revision: 4 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=4&view=rev Author: veedee Date: 2008-11-17 10:13:49 +0000 (Mon, 17 Nov 2008) Log Message: ----------- skel for visualizer Added Paths: ----------- drivers/composite/visualizer_fusion/ drivers/composite/visualizer_fusion/CMakeLists.txt drivers/composite/visualizer_fusion/Makefile drivers/composite/visualizer_fusion/manifest.xml drivers/composite/visualizer_fusion/src/ drivers/composite/visualizer_fusion/src/param_visualizer.cpp drivers/composite/visualizer_fusion/src/terminal.cpp drivers/composite/visualizer_fusion/src/terminal.h drivers/composite/visualizer_fusion/src/vtk_tools.cpp drivers/composite/visualizer_fusion/src/vtk_tools.h Added: drivers/composite/visualizer_fusion/CMakeLists.txt =================================================================== --- drivers/composite/visualizer_fusion/CMakeLists.txt (rev 0) +++ drivers/composite/visualizer_fusion/CMakeLists.txt 2008-11-17 10:13:49 UTC (rev 4) @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.6) +include(rosbuild) +set(ROS_BUILD_TYPE Debug) +rospack(visualizer_fusion) + +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + +# Move this as a dependency for this ROS node +find_package(VTK REQUIRED) +if(NOT VTK_USE_RENDERING) + message(FATAL_ERROR "Example ${PROJECT_NAME} requires VTK_USE_RENDERING.") +endif(NOT VTK_USE_RENDERING) +include(${VTK_USE_FILE}) + +set(VTK_LIBS vtkCommon vtkRendering vtkWidgets vtkHybrid vtkGraphics) + +rospack_add_executable(visualizer_fusion src/visualizer_fusion.cpp src/vtk_tools.cpp) +target_link_libraries(visualizer_fusion ${VTK_LIBS}) + +rospack_add_executable(param_visualizer src/param_visualizer.cpp src/terminal.cpp) +target_link_libraries(param_visualizer) Added: drivers/composite/visualizer_fusion/Makefile =================================================================== --- drivers/composite/visualizer_fusion/Makefile (rev 0) +++ drivers/composite/visualizer_fusion/Makefile 2008-11-17 10:13:49 UTC (rev 4) @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk Added: drivers/composite/visualizer_fusion/manifest.xml =================================================================== --- drivers/composite/visualizer_fusion/manifest.xml (rev 0) +++ drivers/composite/visualizer_fusion/manifest.xml 2008-11-17 10:13:49 UTC (rev 4) @@ -0,0 +1,10 @@ +<package> + <description>This node takes the data from 3 sensing devices (STOC, SR4k, FLIR) through the composite_node, applies the necessary transformations and visualizes the data on screen using VTK.</description> + <author>Radu Bogdan Rusu (ru...@cs...)</author> + <license>BSD</license> + <depend package="roscpp" /> + <depend package="std_msgs" /> + <depend package="tf" /> + <depend package="string_utils" /> + <url></url> +</package> Added: drivers/composite/visualizer_fusion/src/param_visualizer.cpp =================================================================== --- drivers/composite/visualizer_fusion/src/param_visualizer.cpp (rev 0) +++ drivers/composite/visualizer_fusion/src/param_visualizer.cpp 2008-11-17 10:13:49 UTC (rev 4) @@ -0,0 +1,168 @@ +/* + * 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, Inc. 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. + */ + +/* author: Radu Bogdan Rusu <ru...@cs...> */ + +// ROS core +#include "ros/node.h" +#include "ros/time.h" +#include "ros/common.h" + +#include <cfloat> +#include "terminal.h" + +using namespace std; + +class ParamVisualizer: public ros::node +{ + public: + ParamVisualizer () : ros::node ("param_visualizer") { } + ~ParamVisualizer () { } +}; + +/* ---[ */ +int + main (int argc, char** argv) +{ + if (argc < 2) + { + print_error (stderr, "Syntax is: %s [options]\n", argv[0]); + fprintf (stderr, "where options are:\n"); + fprintf (stderr, " -sr_ai X = SwissRanger <Auto-Illumination> parameter\n"); + fprintf (stderr, " -sr_it X = SwissRanger <Integration time> parameter\n"); + fprintf (stderr, " -sr_mf X = SwissRanger <Modulation frequency> parameter\n"); + fprintf (stderr, " -sr_at X = SwissRanger <Amplitude threshold> parameter\n"); + fprintf (stderr, "\n"); + fprintf (stderr, " -stoc_sd X = STOC <Minimum disparity region size> parameter\n"); + fprintf (stderr, " -stoc_ss X = STOC <Disparity region neighbor diff> parameter\n"); + fprintf (stderr, " -stoc_ho X = STOC <Horopter (X Offset)> parameter\n"); + fprintf (stderr, " -stoc_cs X = STOC <Correlation window size> parameter\n"); + fprintf (stderr, " -stoc_un X = STOC <Uniqueness filter threshold> parameter\n"); + fprintf (stderr, " -stoc_tt X = STOC <Texture filter threshold> parameter\n"); + fprintf (stderr, " -stoc_nd X = STOC <Number of disparities> parameter\n"); + fprintf (stderr, " -stoc_zm X = STOC <Z-max cutoff> parameter\n"); + return (-1); + } + + int sr_ai = INT_MIN, sr_it = INT_MIN, sr_mf = INT_MIN, sr_at = INT_MIN; + int stoc_sd = INT_MIN, stoc_ss = INT_MIN, stoc_ho = INT_MIN, stoc_cs = INT_MIN, stoc_un = INT_MIN, stoc_tt = INT_MIN, stoc_nd = INT_MIN; + double stoc_zm = DBL_MIN; + + parseArgument (argc, argv, "-sr_ai", sr_ai); + parseArgument (argc, argv, "-sr_it", sr_it); + parseArgument (argc, argv, "-sr_mf", sr_mf); + parseArgument (argc, argv, "-sr_at", sr_at); + + parseArgument (argc, argv, "-stoc_sd", stoc_sd); + parseArgument (argc, argv, "-stoc_ss", stoc_ss); + parseArgument (argc, argv, "-stoc_ho", stoc_ho); + parseArgument (argc, argv, "-stoc_cs", stoc_cs); + parseArgument (argc, argv, "-stoc_un", stoc_un); + parseArgument (argc, argv, "-stoc_tt", stoc_tt); + parseArgument (argc, argv, "-stoc_nd", stoc_nd); + parseArgument (argc, argv, "-stoc_zm", stoc_zm); + + ros::init (argc, argv); + + // Here is where we create our ROS node + ParamVisualizer p; + + // Swissranger related settings + if (sr_ai != INT_MIN) + { + print_info (stderr, "Changing the <Auto Illumination> settings on the Swissranger camera to: "); print_value (stderr, "%d\n", sr_ai); + p.set_param ("/composite/sr_auto_illumination", sr_ai); + } + if (sr_it != INT_MIN) + { + print_info (stderr, "Changing the <Integration time> settings on the Swissranger camera to: "); print_value (stderr, "%d\n", sr_it); + p.set_param ("/composite/sr_integration_time", sr_it); + } + if (sr_mf != INT_MIN) + { + print_info (stderr, "Changing the <Modulation frequency> settings on the Swissranger camera to: "); print_value (stderr, "%d\n", sr_mf); + p.set_param ("/composite/sr_modulation_freq", sr_mf); + } + if (sr_at != INT_MIN) + { + print_info (stderr, "Changing the <Amplitude threshold> settings on the Swissranger camera to: "); print_value (stderr, "%d\n", sr_at); + p.set_param ("/composite/sr_amp_threshold", sr_at); + } + + // STOC related settings + if (stoc_sd != INT_MIN) + { + print_info (stderr, "Changing the <Minimum disparity region size> settings on the STOC camera to: "); print_value (stderr, "%d\n", stoc_sd); + p.set_param ("/composite/stoc_speckle_size", stoc_sd); + } + if (stoc_ss != INT_MIN) + { + print_info (stderr, "Changing the <Disparity region neighbor diff> settings on the STOC camera to: "); print_value (stderr, "%d\n", stoc_ss); + p.set_param ("/composite/stoc_speckle_diff", stoc_ss); + } + if (stoc_ho != INT_MIN) + { + print_info (stderr, "Changing the <Horopter (X Offset)> settings on the STOC camera to: "); print_value (stderr, "%d\n", stoc_ho); + p.set_param ("/composite/stoc_horopter", stoc_ho); + } + if (stoc_cs != INT_MIN) + { + print_info (stderr, "Changing the <Correlation window size> settings on the STOC camera to: "); print_value (stderr, "%d\n", stoc_cs); + p.set_param ("/composite/stoc_corrsize", stoc_cs); + } + if (stoc_un != INT_MIN) + { + print_info (stderr, "Changing the <Uniqueness filter threshold> settings on the STOC camera to: "); print_value (stderr, "%d\n", stoc_un); + p.set_param ("/composite/stoc_unique", stoc_un); + } + if (stoc_tt != INT_MIN) + { + print_info (stderr, "Changing the <Texture filter threshold> settings on the STOC camera to: "); print_value (stderr, "%d\n", stoc_tt); + p.set_param ("/composite/stoc_tex_thresh", stoc_tt); + } + if (stoc_nd != INT_MIN) + { + print_info (stderr, "Changing the <Number of disparities> settings on the STOC camera to: "); print_value (stderr, "%d\n", stoc_nd); + p.set_param ("/composite/stoc_ndisp", stoc_nd); + } + if (stoc_zm != DBL_MIN) + { + print_info (stderr, "Changing the <Z-max cutoff> settings on the STOC camera to: "); print_value (stderr, "%g\n", stoc_zm); + p.set_param ("/composite/stoc_zmax", stoc_zm); + } + + + // Shutdown + p.shutdown (); + + ros::fini (); + + return (0); +} +/* ]--- */ Added: drivers/composite/visualizer_fusion/src/terminal.cpp =================================================================== --- drivers/composite/visualizer_fusion/src/terminal.cpp (rev 0) +++ drivers/composite/visualizer_fusion/src/terminal.cpp 2008-11-17 10:13:49 UTC (rev 4) @@ -0,0 +1,455 @@ +/* + * 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, Inc. 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. + */ + +/* author: Radu Bogdan Rusu <ru...@cs...> */ + +#include "terminal.h" + +//////////////////////////////////////////////////////////////////////////////// +// ---[ Change the text color (on either stdout or stderr) with an attr:fg:bg +void + changeTextColor (FILE *stream, int attribute, int fg, int bg) +{ + char command[13]; + // Command is the control command to the terminal + sprintf (command, "%c[%d;%d;%dm", 0x1B, attribute, fg + 30, bg + 40); + fprintf (stream, "%s", command); +} + +//////////////////////////////////////////////////////////////////////////////// +// ---[ Change the text color (on either stdout or stderr) with an attr:fg +void + changeTextColor (FILE *stream, int attribute, int fg) +{ + char command[13]; + // Command is the control command to the terminal + sprintf (command, "%c[%d;%dm", 0x1B, attribute, fg + 30); + fprintf (stream, "%s", command); +} + +//////////////////////////////////////////////////////////////////////////////// +// ---[ Reset the text color (on either stdout or stderr) to its original state +void + ResetTextColor (FILE *stream) +{ + char command[13]; + // Command is the control command to the terminal + sprintf (command, "%c[0;m", 0x1B); + fprintf (stream, "%s", command); +} + +//////////////////////////////////////////////////////////////////////////////// +// ---[ Print a message on stream with colors +void + print_color (FILE *stream, int attr, int fg, const char *format, ...) +{ + changeTextColor (stream, attr, fg); + va_list ap; + + va_start (ap, format); + vfprintf (stream, format, ap); + va_end (ap); + + ResetTextColor (stream); +} + +//////////////////////////////////////////////////////////////////////////////// +// ---[ Print an info message on stream without colors +void + print_info (FILE *stream, const char *format, ...) +{ + changeTextColor (stream, INFO_COLOR); + fprintf (stream, "> "); + ResetTextColor (stream); + + va_list ap; + + va_start (ap, format); + vfprintf (stream, format, ap); + va_end (ap); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Parse for a specific given command line argument. +// Returns: the value sent as a string. +int + parseArgument (int argc, char** argv, const char* str, std::string &val) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + val = std::string (argv[i]); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for a specific given command line argument. +// Returns: the value sent as a boolean. +int + parseArgument (int argc, char** argv, const char* str, bool &val) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + val = (bool)atoi (argv[i]); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for a specific given command line argument. +// Returns: the value sent as a double. +int + parseArgument (int argc, char** argv, const char* str, double &val) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + val = atof (argv[i]); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for a specific given command line argument. +// Returns: the value sent as an int. +int + parseArgument (int argc, char** argv, const char* str, int &val) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + val = atoi (argv[i]); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for a specific given command line argument. +// // Returns: the value sent as an unsigned int. +int + parseArgument (int argc, char** argv, const char* str, unsigned int &val) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + val = atoi (argv[i]); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse command line arguments for file names. +// Returns: a vector with file names indices. +std::vector<int> + parseFileNamesArgument (int argc, char** argv) +{ + std::vector<int> indices; + for (int i = 1; i < argc; i++) + { + std::string fname = std::string (argv[i]); + + // Needs to be at least 4: .ext + if (fname.size () <= 4) + continue; + + // For being case insensitive + std::transform (fname.begin(), fname.end(), fname.begin(), tolower); + + // if OBJ + if (fname.compare (fname.size () - 3, 3, "obj") == 0) + indices.push_back (i); + // or PLY + else if (fname.compare (fname.size () - 3, 3, "ply") == 0) + indices.push_back (i); + // or NCXYZ + else if (fname.compare (fname.size () - 5, 5, "ncxyz") == 0) + indices.push_back (i); + // or CXYZ + else if (fname.compare (fname.size () - 4, 4, "cxyz") == 0) + indices.push_back (i); + // or NXYZ + else if (fname.compare (fname.size () - 4, 4, "nxyz") == 0) + indices.push_back (i); + // or MESH + else if (fname.compare (fname.size () - 4, 4, "mesh") == 0) + indices.push_back (i); + // or XYZR + else if (fname.compare (fname.size () - 4, 4, "xyzr") == 0) + indices.push_back (i); + // or PVTP + else if (fname.compare (fname.size () - 4, 4, "pvtp") == 0) + indices.push_back (i); + // or XYZ + else if (fname.compare (fname.size () - 3, 3, "xyz") == 0) + indices.push_back (i); + // or VTK + else if (fname.compare (fname.size () - 3, 3, "vtk") == 0) + indices.push_back (i); + // or VTP + else if (fname.compare (fname.size () - 3, 3, "vtp") == 0) + indices.push_back (i); + } + return indices; +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse command line arguments for file names. +// Returns: a vector with file names indices. +std::vector<int> + parseFileExtensionArgument (int argc, char** argv, const char *extension) +{ + std::vector<int> indices; + for (int i = 1; i < argc; i++) + { + std::string fname = std::string (argv[i]); + std::string ext = std::string (extension); + + // Needs to be at least 4: .ext + if (fname.size () <= 4) + continue; + + // For being case insensitive + std::transform(fname.begin(), fname.end(), fname.begin(), tolower); + std::transform(ext.begin(), ext.end(), ext.begin(), tolower); + + // Check if found + if (fname.find (ext) != std::string::npos) + indices.push_back (i); + } + return indices; +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for specific given command line arguments (3x values comma separated). +// Returns: the values sent as doubles. +int + parse3xArguments (int argc, char** argv, const char* str, double &f, double &s, double &t, bool debug) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + // look for ',' as a separator + std::vector<std::string> values; + string_utils::split (argv[i], values, ","); + if ((values.size () != 3) && (debug)) + { + print_error (stderr, "Number of values for %s different than 3!\n", str); + return (-2); + } + f = atof (values.at (0).c_str ()); + s = atof (values.at (1).c_str ()); + t = atof (values.at (2).c_str ()); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for specific given command line arguments (3x values comma separated). +// Returns: the values sent as ints. +int + parse3xArguments (int argc, char** argv, const char* str, int &f, int &s, int &t, bool debug) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + // look for ',' as a separator + std::vector<std::string> values; + string_utils::split (argv[i], values, ","); + if ((values.size () != 3) && (debug)) + { + print_error (stderr, "Number of values for %s different than 3!\n", str); + return (-2); + } + f = atoi (values.at (0).c_str ()); + s = atoi (values.at (1).c_str ()); + t = atoi (values.at (2).c_str ()); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for specific given command line arguments (2x values comma separated). +// Returns: the values sent as ints. +int + parse2xArguments (int argc, char** argv, const char* str, int &f, int &s, bool debug) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + // look for ',' as a separator + std::vector<std::string> values; + string_utils::split (argv[i], values, ","); + if ((values.size () != 2) && (debug)) + { + print_error (stderr, "Number of values for %s different than 2!\n", str); + return (-2); + } + f = atoi (values.at (0).c_str ()); + s = atoi (values.at (1).c_str ()); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for specific given command line arguments (range values X-Y). +// Returns: the values sent as doubles. +int + parseRangeArguments (int argc, char** argv, const char* str, double &s, double &e) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + // look for ',' as a separator + std::vector<std::string> values; + string_utils::split (argv[i], values, ","); + if (values.size () != 2) + print_error (stderr, "Number of values for %s different than 2!\n", str); + s = atof (values.at (0).c_str ()); + e = atof (values.at (1).c_str ()); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for specific given command line arguments (range values X-Y). +// Returns: the values sent as ints. +int + parseRangeArguments (int argc, char** argv, const char* str, int &s, int &e) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + // look for ',' as a separator + std::vector<std::string> values; + string_utils::split (argv[i], values, ","); + if (values.size () != 2) + print_error (stderr, "Number of values for %s different than 2!\n", str); + s = atoi (values.at (0).c_str ()); + e = atoi (values.at (1).c_str ()); + return (i - 1); + } + } + return (-1); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for specific given command line arguments (multiple occurances of the +// same command line parameter). +// Returns: the values sent as a vector. +bool + parseMultipleArguments (int argc, char** argv, const char* str, std::vector<int> &values) +{ + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + int val = atoi (argv[i]); + values.push_back (val); + } + } + if (values.size () == 0) + return (false); + else + return (true); +} + +//////////////////////////////////////////////////////////////////////////////// +// Parse for specific given command line arguments (multiple occurances of 3x +// argument groups, separated by commas) +// Returns: 3 vectors holding the given values +bool + parseMultiple3xArguments (int argc, char** argv, const char* str, + std::vector<double> &valuesF, std::vector<double> &valuesS, std::vector<double> &valuesT) +{ + double f, s, t; + for (int i = 1; i < argc; i++) + { + // Search for the string + if ((strcmp (argv[i], str) == 0) && (++i < argc)) + { + // look for ',' as a separator + std::vector<std::string> values; + string_utils::split (argv[i], values, ","); + if (values.size () != 3) + print_error (stderr, "Number of values for %s different than 3!\n", str); + f = atof (values.at (0).c_str ()); + s = atof (values.at (1).c_str ()); + t = atof (values.at (2).c_str ()); + valuesF.push_back (f); + valuesS.push_back (s); + valuesT.push_back (t); + } + } + if (valuesF.size () == 0) + return (false); + else + return (true); +} Added: drivers/composite/visualizer_fusion/src/terminal.h =================================================================== --- drivers/composite/visualizer_fusion/src/terminal.h (rev 0) +++ drivers/composite/visualizer_fusion/src/terminal.h 2008-11-17 10:13:49 UTC (rev 4) @@ -0,0 +1,111 @@ +/* + * 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, Inc. 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. + */ + +/* author: Radu Bogdan Rusu <ru...@cs...> */ + +#ifndef INCLUDE_COMMONTERMINALROUTINES_H +#define INCLUDE_COMMONTERMINALROUTINES_H + +#include <sys/time.h> +#include <cmath> +#include <stdarg.h> +#include <stdio.h> +#include <stdlib.h> +#include <string> +#include <algorithm> +#include <string.h> +#include <vector> +#include "string_utils/string_utils.h" + +#define RESET 0 +#define BRIGHT 1 +#define DIM 2 +#define UNDERLINE 3 +#define BLINK 4 +#define REVERSE 7 +#define HIDDEN 8 +#define BLACK 0 +#define RED 1 +#define GREEN 2 +#define YELLOW 3 +#define BLUE 4 +#define MAGENTA 5 +#define CYAN 6 +#define WHITE 7 + +#define MSG_OUTPUT_STREAM stderr + +#define ERROR_COLOR BRIGHT,RED +#define WARNING_COLOR BRIGHT,BLUE +#define VALUE_COLOR BRIGHT,YELLOW +#define INFO_COLOR BRIGHT,GREEN + +#define print_error(stream,...) print_color(stream, ERROR_COLOR,__VA_ARGS__) +#define print_warning(stream,...) print_color(stream, WARNING_COLOR,__VA_ARGS__) +#define print_value(stream,...) print_color(stream, VALUE_COLOR,__VA_ARGS__) + +void changeTextColor (FILE *stream, int attribute, int fg, int bg); +void changeTextColor (FILE *stream, int attribute, int fg); +void resetTextColor (FILE *stream); + +void print_color (FILE *stream, int attr, int fg, const char *format, ...); +void print_info (FILE *stream, const char *format, ...); + +inline long int print_progressbar (long int counter, long int total, long int decicounter) +{ + if (counter >= decicounter * total / 10.0) + { + if (decicounter < 10) + print_color (stderr, INFO_COLOR, "%ld%%-", decicounter*10); + else + print_color (stderr, INFO_COLOR, "%ld%% ", decicounter*10); + decicounter++; + } + return decicounter; +} + +std::vector<int> parseFileNamesArgument (int argc, char** argv); +int parseArgument (int argc, char** argv, const char* str, std::string &val); +int parseArgument (int argc, char** argv, const char* str, bool &val); +int parseArgument (int argc, char** argv, const char* str, double &val); +int parseArgument (int argc, char** argv, const char* str, int &val); +int parseArgument (int argc, char** argv, const char* str, unsigned int &val); +int parse3xArguments (int argc, char** argv, const char* str, double &f, double &s, double &t, bool debug = true); +int parse3xArguments (int argc, char** argv, const char* str, int &f, int &s, int &t, bool debug = true); +int parse2xArguments (int argc, char** argv, const char* str, int &f, int &s, bool debug = true); +int parseRangeArguments (int argc, char** argv, const char* str, double &s, double &e); +int parseRangeArguments (int argc, char** argv, const char* str, int &s, int &e); +bool parseMultipleArguments (int argc, char** argv, const char* str, std::vector<int> &values); + +bool parseMultiple3xArguments (int argc, char** argv, const char* str, + std::vector<double> &valuesF, std::vector<double> &valuesS, std::vector<double> &valuesT); + +std::vector<int> parseFileExtensionArgument (int argc, char** argv, const char* ext); + +#endif Added: drivers/composite/visualizer_fusion/src/vtk_tools.cpp =================================================================== --- drivers/composite/visualizer_fusion/src/vtk_tools.cpp (rev 0) +++ drivers/composite/visualizer_fusion/src/vtk_tools.cpp 2008-11-17 10:13:49 UTC (rev 4) @@ -0,0 +1,113 @@ +/* + * 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, Inc. 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. + */ + +/* author: Radu Bogdan Rusu <ru...@cs...> */ + +#include "vtk_tools.h" + +vtkStandardNewMacro (vtkWGInteractorStyle); + +void + vtkWGInteractorStyle::OnChar () +{ + if (pointsize_ < 1) + pointsize_ = 1; + +// lock_char_->Lock (); + // ---[ Check the rest of the key codes + switch (this->Interactor->GetKeyCode ()) + { + case KEY_PLUS: + { + vtkActorCollection *ac; + vtkActor *anActor, *aPart; + vtkAssemblyPath *path; + ac = this->CurrentRenderer->GetActors (); + vtkCollectionSimpleIterator ait; + for (ac->InitTraversal (ait); (anActor = ac->GetNextActor (ait)); ) + { + for (anActor->InitPathTraversal (); (path=anActor->GetNextPath ()); ) + { + aPart=(vtkActor *)path->GetLastNode ()->GetViewProp (); + aPart->GetProperty ()->SetPointSize (aPart->GetProperty ()->GetPointSize () + 1); + } + } + break; + } + case KEY_MINUS: + { + vtkActorCollection *ac; + vtkActor *anActor, *aPart; + vtkAssemblyPath *path; + ac = this->CurrentRenderer->GetActors (); + vtkCollectionSimpleIterator ait; + for (ac->InitTraversal (ait); (anActor = ac->GetNextActor (ait)); ) + { + for (anActor->InitPathTraversal (); (path=anActor->GetNextPath ()); ) + { + aPart=(vtkActor *)path->GetLastNode ()->GetViewProp (); + aPart->GetProperty ()->SetPointSize (aPart->GetProperty ()->GetPointSize () - 1); + } + } + break; + } + } + this->Superclass::OnChar (); +// lock_char_->Unlock (); +} + +//////////////////////////////////////////////////////////////////////////////// +void + vtkFPSCallback::SetTextActor (vtkTextActor *txt) +{ + this->TextActor = txt; +} +void + vtkFPSCallback::Execute (vtkObject *caller, unsigned long, void*) +{ + vtkRenderer *ren = reinterpret_cast<vtkRenderer *> (caller); + + long int nr_points = 0; + vtkActorCollection *ac = ren->GetActors (); + vtkActor *anActor, *aPart; + vtkCollectionSimpleIterator ait; + vtkAssemblyPath *path; + for (ac->InitTraversal (ait); (anActor = ac->GetNextActor (ait)); ) + { + for (anActor->InitPathTraversal(); (path=anActor->GetNextPath ()); ) + { + aPart=(vtkActor *)path->GetLastNode ()->GetViewProp (); + nr_points += aPart->GetMapper ()->GetInputAsDataSet ()->GetNumberOfPoints (); + } + } + + float fps = 1.0/ren->GetLastRenderTimeInSeconds (); + snprintf (this->TextBuff, 127, "%.1f FPS, %ld points", fps, nr_points); + this->TextActor->SetInput (this->TextBuff); +} Added: drivers/composite/visualizer_fusion/src/vtk_tools.h =================================================================== --- drivers/composite/visualizer_fusion/src/vtk_tools.h (rev 0) +++ drivers/composite/visualizer_fusion/src/vtk_tools.h 2008-11-17 10:13:49 UTC (rev 4) @@ -0,0 +1,108 @@ +/* + * 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, Inc. 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. + */ + +/* author: Radu Bogdan Rusu <ru...@cs...> */ + +#ifndef VTK_TOOLS_HH +#define VTK_TOOLS_HH + +// VTK includes +#include <vtkCellArray.h> +#include <vtkPointData.h> +#include <vtkLODActor.h> +#include <vtkProperty.h> +#include <vtkRenderer.h> +#include <vtkRenderWindow.h> +#include <vtkRenderWindowInteractor.h> +#include <vtkPolyData.h> +#include <vtkPolyDataMapper.h> +#include <vtkPolyDataWriter.h> +#include <vtkSmartPointer.h> +#include <vtkInteractorStyleTrackballCamera.h> +#include <vtkXRenderWindowInteractor.h> +#include <vtkDataSetMapper.h> +#include <vtkPNGWriter.h> +#include <vtkWindowToImageFilter.h> +#include <vtkObjectFactory.h> +#include <vtkCamera.h> +#include <vtkAssemblyPath.h> +#include <vtkRendererCollection.h> +#include <vtkMultiThreader.h> +#include <vtkCommand.h> +#include <vtkCallbackCommand.h> +#include <vtkCriticalSection.h> +#include <vtkTextActor.h> +#include <vtkProperty2D.h> + +#define KEY_PLUS 43 +#define KEY_MINUS 45 + +//////////////////////////////////////////////////////////////////////////////// +class vtkWGInteractorStyle : public vtkInteractorStyleTrackballCamera +{ + public: + static vtkWGInteractorStyle *New (); + + void OnChar (); + + void + OnTimer () + { +// lock_timer_->Lock (); +// ren_win_->Render (); +// lock_timer_->Unlock (); +// this->Superclass::OnTimer (); + } + + vtkPNGWriter* writer_; + vtkWindowToImageFilter *filter_; + vtkRenderWindow *ren_win_; + + vtkCriticalSection *lock_timer_, *lock_char_; + + int pointsize_; + protected: + char fileName_[80]; + char camFN_[80]; +}; + + +//////////////////////////////////////////////////////////////////////////////// +class vtkFPSCallback : public vtkCommand +{ + public: + static vtkFPSCallback *New () { return new vtkFPSCallback;} + void SetTextActor (vtkTextActor *txt); + virtual void Execute (vtkObject *caller, unsigned long, void*); + protected: + vtkTextActor *TextActor; + char TextBuff[128]; +}; + +#endif This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2008-11-17 09:53:50
|
Revision: 3 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=3&view=rev Author: veedee Date: 2008-11-17 09:53:45 +0000 (Mon, 17 Nov 2008) Log Message: ----------- clean script Added Paths: ----------- drivers/composite/clean.sh Added: drivers/composite/clean.sh =================================================================== --- drivers/composite/clean.sh (rev 0) +++ drivers/composite/clean.sh 2008-11-17 09:53:45 UTC (rev 3) @@ -0,0 +1,6 @@ +#!/bin/bash +for i in `find ./ -name "build"` +do + echo $i + rm -rf $i +done Property changes on: drivers/composite/clean.sh ___________________________________________________________________ Added: svn:executable + * This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |