Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7272] pkg/trunk
|
From: <jl...@us...> - 2008-11-25 07:10:38
|
Revision: 7272
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7272&view=rev
Author: jleibs
Date: 2008-11-25 07:10:27 +0000 (Tue, 25 Nov 2008)
Log Message:
-----------
Changes to make stereo_view cleanly work with color calibration.
Modified Paths:
--------------
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
pkg/trunk/image_msgs/manifest.xml
pkg/trunk/vision/color_calib/CMakeLists.txt
pkg/trunk/vision/color_calib/calib_node.cpp
pkg/trunk/vision/color_calib/calib_node_dcam.cpp
pkg/trunk/vision/color_calib/calib_test.cpp
pkg/trunk/vision/color_calib/libcolorcalib.cpp
pkg/trunk/vision/stereo_view/manifest.xml
pkg/trunk/vision/stereo_view/stereo_view.cpp
Added Paths:
-----------
pkg/trunk/vision/color_calib/calibration.cpp
pkg/trunk/vision/color_calib/color_calib.h
Removed Paths:
-------------
pkg/trunk/vision/color_calib/colorcalib.h
Modified: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-25 07:10:27 UTC (rev 7272)
@@ -38,7 +38,6 @@
#include "image_msgs/Image.h"
#include "opencv/cxcore.h"
-
namespace image_msgs
{
@@ -48,21 +47,21 @@
IplImage* rosimg_;
IplImage* cvtimg_;
- void reallocCvtIfNeeded(CvSize sz, int depth, int channels)
+ void reallocIfNeeded_(IplImage** img, CvSize sz, int depth, int channels)
{
- if (cvtimg_ != 0)
- if (cvtimg_->width != sz.width ||
- cvtimg_->height != sz.height ||
- cvtimg_->depth != depth ||
- cvtimg_->nChannels != channels)
+ if ((*img) != 0)
+ if ((*img)->width != sz.width ||
+ (*img)->height != sz.height ||
+ (*img)->depth != depth ||
+ (*img)->nChannels != channels)
{
- cvReleaseImage(&cvtimg_);
- cvtimg_ = 0;
+ cvReleaseImage(img);
+ *img = 0;
}
- if (cvtimg_ == 0)
+ if (*img == 0)
{
- cvtimg_ = cvCreateImage(sz, depth, channels);
+ *img = cvCreateImage(sz, depth, channels);
}
}
@@ -109,13 +108,13 @@
{
if (encoding == "bgr" && rosimg.encoding == "rgb")
{
- reallocCvtIfNeeded(cvGetSize(rosimg_), IPL_DEPTH_8U, 3);
+ reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, 3);
cvCvtColor(rosimg_, cvtimg_, CV_RGB2BGR);
img_ = cvtimg_;
}
else if (encoding == "bgr" && rosimg.encoding == "mono" )
{
- reallocCvtIfNeeded(cvGetSize(rosimg_), IPL_DEPTH_8U, 3);
+ reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, 3);
cvCvtColor(rosimg_, cvtimg_, CV_GRAY2BGR);
img_ = cvtimg_;
}
@@ -126,8 +125,15 @@
}
return true;
}
- private:
+ void reallocIfNeeded(IplImage** img, int depth = -1, int channels = -1)
+ {
+ if (depth == -1)
+ depth = img_->depth;
+ if (channels == -1)
+ channels = img_->nChannels;
+ reallocIfNeeded_(img, cvGetSize(img_), depth, channels);
+ }
};
}
Modified: pkg/trunk/image_msgs/manifest.xml
===================================================================
--- pkg/trunk/image_msgs/manifest.xml 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/image_msgs/manifest.xml 2008-11-25 07:10:27 UTC (rev 7272)
@@ -4,8 +4,8 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="std_msgs"/>
+ <depend package="dcam"/>
<depend package="opencv_latest"/>
- <depend package="dcam"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
</export>
Modified: pkg/trunk/vision/color_calib/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/color_calib/CMakeLists.txt 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/vision/color_calib/CMakeLists.txt 2008-11-25 07:10:27 UTC (rev 7272)
@@ -5,7 +5,7 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-rospack_add_library(colorcalib libcolorcalib.cpp)
+rospack_add_library(colorcalib libcolorcalib.cpp calibration.cpp)
rospack_add_executable(calib_test calib_test.cpp)
target_link_libraries(calib_test colorcalib)
Modified: pkg/trunk/vision/color_calib/calib_node.cpp
===================================================================
--- pkg/trunk/vision/color_calib/calib_node.cpp 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/vision/color_calib/calib_node.cpp 2008-11-25 07:10:27 UTC (rev 7272)
@@ -44,9 +44,10 @@
#include <sys/stat.h>
-#include "colorcalib.h"
+#include "color_calib.h"
using namespace std;
+using namespace color_calib;
class ColorCalib : public ros::node
{
@@ -55,21 +56,15 @@
ros::thread::mutex cv_mutex;
- CvMat* color_cal;
-
bool first;
- ColorCalib() : node("color_calib", ros::node::ANONYMOUS_NAME), first(true)
+ Calibration color_cal;
+
+ ColorCalib() : node("color_calib", ros::node::ANONYMOUS_NAME), first(true), color_cal(this)
{
subscribe("images", image_msg, &ColorCalib::image_cb, 1);
- color_cal = cvCreateMat( 3, 3, CV_32FC1);
}
- ~ColorCalib()
- {
- cvReleaseMat(&color_cal);
- }
-
void image_cb()
{
if (!first)
@@ -96,27 +91,12 @@
find_calib(img2, color_cal, COLOR_CAL_BGR | COLOR_CAL_COMPAND_DISPLAY);
- printf("Color calibration:\n");
- for (int i = 0; i < 3; i ++)
- {
- for (int j = 0; j < 3; j++)
- {
- printf("%f ", cvmGet(color_cal, i, j));
- }
- printf("\n");
- }
-
IplImage* corrected_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 3);
- cvTransform(img2, corrected_img, color_cal);
- XmlRpc::XmlRpcValue xml_color_cal;
- for (int i = 0; i < 3; i++)
- for (int j = 0; j < 3; j++)
- xml_color_cal[3*i + j] = cvmGet(color_cal, i, j);
-
- set_param(map_name("images") + std::string("/") + l + std::string("/color_cal"), xml_color_cal);
-
+ cvTransform(img2, corrected_img, color_cal.getCal());
+ color_cal.setParam(map_name("images") + std::string("/") + l);
+
cvNamedWindow("color_rect", CV_WINDOW_AUTOSIZE);
cvShowImage("color_rect", corrected_img);
Modified: pkg/trunk/vision/color_calib/calib_node_dcam.cpp
===================================================================
--- pkg/trunk/vision/color_calib/calib_node_dcam.cpp 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/vision/color_calib/calib_node_dcam.cpp 2008-11-25 07:10:27 UTC (rev 7272)
@@ -44,9 +44,10 @@
#include <sys/stat.h>
-#include "colorcalib.h"
+#include "color_calib.h"
using namespace std;
+using namespace color_calib;
class ColorCalib : public ros::node
{
@@ -56,21 +57,15 @@
ros::thread::mutex cv_mutex;
- CvMat* color_cal;
+ Calibration color_cal;
bool first;
- ColorCalib() : node("color_calib", ros::node::ANONYMOUS_NAME), first(true)
+ ColorCalib() : node("color_calib", ros::node::ANONYMOUS_NAME), color_cal(this), first(true)
{
subscribe("image", image, &ColorCalib::image_cb, 1);
- color_cal = cvCreateMat( 3, 3, CV_32FC1);
}
- ~ColorCalib()
- {
- cvReleaseMat(&color_cal);
- }
-
void image_cb()
{
if (!first)
@@ -90,26 +85,11 @@
find_calib(cv_img_decompand, color_cal, COLOR_CAL_BGR | COLOR_CAL_COMPAND_DISPLAY);
- printf("Color calibration:\n");
- for (int i = 0; i < 3; i ++)
- {
- for (int j = 0; j < 3; j++)
- {
- printf("%f ", cvmGet(color_cal, i, j));
- }
- printf("\n");
- }
+ color_cal.setParam("image");
IplImage* cv_img_correct = cvCreateImage(cvGetSize(cv_img), IPL_DEPTH_32F, 3);
- cvTransform(cv_img_decompand, cv_img_correct, color_cal);
+ cvTransform(cv_img_decompand, cv_img_correct, color_cal.getCal(COLOR_CAL_BGR));
- XmlRpc::XmlRpcValue xml_color_cal;
- for (int i = 0; i < 3; i++)
- for (int j = 0; j < 3; j++)
- xml_color_cal[3*i + j] = cvmGet(color_cal, i, j);
-
- set_param(map_name("image") + std::string("/") + std::string("/color_cal"), xml_color_cal);
-
cvNamedWindow("color_rect", CV_WINDOW_AUTOSIZE);
cvShowImage("color_rect", cv_img_correct);
Modified: pkg/trunk/vision/color_calib/calib_test.cpp
===================================================================
--- pkg/trunk/vision/color_calib/calib_test.cpp 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/vision/color_calib/calib_test.cpp 2008-11-25 07:10:27 UTC (rev 7272)
@@ -32,14 +32,14 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include "ros/node.h"
-
#include "opencv/cxcore.h"
#include "opencv/cv.h"
#include "opencv/highgui.h"
-#include "colorcalib.h"
+#include "color_calib.h"
+using namespace color_calib;
+
int main(int argc, char** argv)
{
IplImage* img = cvLoadImage(argv[1]);
@@ -47,22 +47,23 @@
// Load image
if (img)
{
- CvMat* color_cal = cvCreateMat( 3, 3, CV_32FC1);
IplImage* corrected_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
- find_calib(img, color_cal);
+ Calibration cal(0);
+ find_calib(img, cal);
+
printf("Color calibration:\n");
for (int i = 0; i < 3; i ++)
{
for (int j = 0; j < 3; j++)
{
- printf("%f ", cvmGet(color_cal, i, j));
+ printf("%f ", cvmGet(cal.getCal(), i, j));
}
printf("\n");
}
- cvTransform(img, corrected_img, color_cal);
+ cvTransform(img, corrected_img, cal.getCal());
cvNamedWindow("color_rect", CV_WINDOW_AUTOSIZE);
cvShowImage("color_rect", corrected_img);
Added: pkg/trunk/vision/color_calib/calibration.cpp
===================================================================
--- pkg/trunk/vision/color_calib/calibration.cpp (rev 0)
+++ pkg/trunk/vision/color_calib/calibration.cpp 2008-11-25 07:10:27 UTC (rev 7272)
@@ -0,0 +1,162 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "color_calib.h"
+
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+
+using namespace color_calib;
+
+color_calib::Calibration::Calibration(ros::node* node) : node_(node), color_cal_(NULL)
+{
+ color_cal_ = cvCreateMat(3, 3, CV_32FC1);
+ color_cal_bgr_ = cvCreateMat(3, 3, CV_32FC1);
+ cvSetIdentity(color_cal_, cvScalar(1.0));
+ cvSetIdentity(color_cal_bgr_, cvScalar(1.0));
+}
+
+color_calib::Calibration::~Calibration()
+{
+ if (color_cal_)
+ cvReleaseMat(&color_cal_);
+ if (color_cal_bgr_)
+ cvReleaseMat(&color_cal_bgr_);
+}
+
+bool
+color_calib::Calibration::getFromParam(std::string topic_name)
+{
+ if (node_)
+ {
+ std::string color_cal_str = (node_->map_name(topic_name) + std::string("/color_cal"));
+
+ if (node_->has_param(color_cal_str))
+ {
+ XmlRpc::XmlRpcValue xml_color_cal;
+ node_->get_param(color_cal_str, xml_color_cal);
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ {
+ cvmSet(color_cal_, i, j, (double)(xml_color_cal[3*i + j]));
+ }
+ populateBGR();
+ return true;
+ }
+ }
+ return false;
+}
+
+bool
+color_calib::Calibration::setParam(std::string topic_name)
+{
+ if (node_)
+ {
+ std::string color_cal_str = (node_->map_name(topic_name) + std::string("/color_cal"));
+
+ XmlRpc::XmlRpcValue xml_color_cal;
+ for (int i = 2; i >= 0; i--)
+ for (int j = 0; j < 3; j++)
+ xml_color_cal[3*i + j] = cvmGet(color_cal_, i, j);
+
+ node_->set_param(color_cal_str, xml_color_cal);
+
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+
+CvMat*
+color_calib::Calibration::getCal(uint32_t flag)
+{
+ if (flag & COLOR_CAL_BGR)
+ return color_cal_bgr_;
+ else
+ return color_cal_;
+}
+
+bool
+color_calib::Calibration::setCal(CvMat* cal, uint32_t flag)
+{
+ if (cal->height == 3 && cal->width==3)
+ {
+ if (flag & COLOR_CAL_BGR)
+ {
+ cvCopy(cal, color_cal_bgr_);
+ populateRGB();
+ } else {
+ cvCopy(cal, color_cal_);
+ populateBGR();
+ }
+ return true;
+ }
+ return false;
+}
+
+void
+color_calib::Calibration::populateBGR()
+{
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ cvmSet(color_cal_bgr_, 2-i, 2-j, cvmGet(color_cal_, i, j));
+}
+
+void
+color_calib::Calibration::populateRGB()
+{
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ cvmSet(color_cal_, 2-i, 2-j, cvmGet(color_cal_bgr_, i, j));
+}
+
+
+void
+color_calib::Calibration::correctColor(IplImage* src, IplImage* dst, bool do_decompand, bool do_recompand, uint32_t flags)
+{
+ if (do_decompand)
+ decompand(src, dst);
+
+ if (flags & COLOR_CAL_BGR)
+ cvTransform(dst, dst, color_cal_bgr_);
+ else
+ cvTransform(dst, dst, color_cal_);
+
+ if (do_recompand)
+ compand(dst, dst);
+}
Copied: pkg/trunk/vision/color_calib/color_calib.h (from rev 7259, pkg/trunk/vision/color_calib/colorcalib.h)
===================================================================
--- pkg/trunk/vision/color_calib/color_calib.h (rev 0)
+++ pkg/trunk/vision/color_calib/color_calib.h 2008-11-25 07:10:27 UTC (rev 7272)
@@ -0,0 +1,78 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef COLORCALIB_HH
+#define COLORCALIB_HH
+
+#include "opencv/cxcore.h"
+#include <string>
+#include "ros/node.h"
+
+static const uint32_t COLOR_CAL_BGR = 1;
+static const uint32_t COLOR_CAL_COMPAND_DISPLAY = 1 << 2;
+
+namespace color_calib
+{
+
+ class Calibration
+ {
+ ros::node* node_;
+
+ CvMat* color_cal_;
+ CvMat* color_cal_bgr_;
+ public:
+ Calibration(ros::node* node);
+ ~Calibration();
+
+ bool getFromParam(std::string topic_name_);
+ bool setParam(std::string topic_name_);
+
+ CvMat* getCal(uint32_t flag = 0);
+ bool setCal(CvMat* cal, uint32_t flag = 0);
+
+ void correctColor(IplImage* src, IplImage* dst, bool do_decompand, bool do_recompand, uint32_t flags = 0);
+
+ private:
+ void populateBGR();
+ void populateRGB();
+ };
+
+ float srgb2lrgb(float x);
+ float lrgb2srgb(float x);
+ void decompand(IplImage* src, IplImage* dst);
+ void compand(IplImage* src, IplImage* dst);
+ bool find_calib(IplImage* img, Calibration& cal, uint32_t flags=0);
+}
+
+#endif
Deleted: pkg/trunk/vision/color_calib/colorcalib.h
===================================================================
--- pkg/trunk/vision/color_calib/colorcalib.h 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/vision/color_calib/colorcalib.h 2008-11-25 07:10:27 UTC (rev 7272)
@@ -1,50 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef COLORCALIB_HH
-#define COLORCALIB_HH
-
-#include "opencv/cxcore.h"
-
-static const uint32_t COLOR_CAL_BGR = 1;
-static const uint32_t COLOR_CAL_FLOAT = 1 << 1;
-static const uint32_t COLOR_CAL_COMPAND_DISPLAY = 1 << 2;
-
-float srgb2lrgb(float x);
-float lrgb2srgb(float x);
-void decompand(IplImage* src, IplImage* dst);
-void compand(IplImage* src, IplImage* dst);
-bool find_calib(IplImage* img, CvMat* m, int flags=0);
-
-#endif
Modified: pkg/trunk/vision/color_calib/libcolorcalib.cpp
===================================================================
--- pkg/trunk/vision/color_calib/libcolorcalib.cpp 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/vision/color_calib/libcolorcalib.cpp 2008-11-25 07:10:27 UTC (rev 7272)
@@ -34,12 +34,14 @@
#include "ros/node.h"
-#include "colorcalib.h"
+#include "color_calib.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
#include "opencv/highgui.h"
+using namespace color_calib;
+
IplImage* g_img;
IplImage* g_disp_img;
@@ -84,7 +86,7 @@
-float srgb2lrgb(float x)
+float color_calib::srgb2lrgb(float x)
{
if (x < 0.04045)
{
@@ -95,7 +97,7 @@
return x;
}
-float lrgb2srgb(float x)
+float color_calib::lrgb2srgb(float x)
{
if (x < 0.0031308)
{
@@ -106,7 +108,7 @@
return x;
}
-void compand(IplImage* src, IplImage* dst)
+void color_calib::compand(IplImage* src, IplImage* dst)
{
static int compandmap[4096];
static bool has_map = false;
@@ -161,7 +163,7 @@
}
-void decompand(IplImage* src, IplImage* dst)
+void color_calib::decompand(IplImage* src, IplImage* dst)
{
static float compandmap[1024];
static bool has_map = false;
@@ -197,13 +199,6 @@
for (int k = 0; k < src->nChannels; k++)
((float *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels + k] =
compandmap[((uchar *)(src->imageData + i*src->widthStep))[j*src->nChannels + k] << 2];
- } else if (src->depth == IPL_DEPTH_8U && dst->depth == IPL_DEPTH_8U)
- {
- for (int i = 0; i < src->height; i++)
- for (int j = 0; j < src->width; j++)
- for (int k = 0; k < src->nChannels; k++)
- ((uchar *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels + k] =
- compandmap[((uchar *)(src->imageData + i*src->widthStep))[j*src->nChannels + k] << 2]*255;
} else if (src->depth == IPL_DEPTH_32F && dst->depth == IPL_DEPTH_32F)
{
for (int i = 0; i < src->height; i++)
@@ -215,11 +210,18 @@
val = MIN(val, 1024);
((float *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels + k] = compandmap[val];
}
- } else if (src->depth == IPL_DEPTH_32F && dst->depth == IPL_DEPTH_8U)
+ } else if (src->depth == IPL_DEPTH_8U && dst->depth == IPL_DEPTH_8U) // This probably shouldn't be allowed (loss of information)
{
for (int i = 0; i < src->height; i++)
for (int j = 0; j < src->width; j++)
for (int k = 0; k < src->nChannels; k++)
+ ((uchar *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels + k] =
+ compandmap[((uchar *)(src->imageData + i*src->widthStep))[j*src->nChannels + k] << 2]*255;
+ } else if (src->depth == IPL_DEPTH_32F && dst->depth == IPL_DEPTH_8U) // This probably shouldn't be allowed (loss of information)
+ {
+ for (int i = 0; i < src->height; i++)
+ for (int j = 0; j < src->width; j++)
+ for (int k = 0; k < src->nChannels; k++)
{
int val = int(((float *)(src->imageData + i*src->widthStep))[j*src->nChannels + k]*1024.0);
val = MAX(val, 0);
@@ -319,7 +321,7 @@
}
}
-bool find_calib(IplImage* img, CvMat* mat, int flags)
+bool color_calib::find_calib(IplImage* img, Calibration& cal, uint32_t flags)
{
bool use_bgr = flags & COLOR_CAL_BGR;
@@ -345,7 +347,7 @@
CvScalar a = cvAvg(g_img);
printf("Avg value of g: %f %f %f\n", a.val[0], a.val[1], a.val[2]);
- if (g_img && mat)
+ if (g_img)
{
//Allocate matrices
g_real_corners = cvCreateMat( 4, 1, CV_32FC2);
@@ -389,8 +391,7 @@
g_meas_colors = cvCreateMat( 24, 3, CV_32FC1);
g_reproj_colors = cvCreateMat( 24, 3, CV_32FC1);
-
- g_color_cal = mat;
+ g_color_cal = cvCreateMat( 3, 3, CV_32FC1);
// Create display
cvNamedWindow("macbeth image", CV_WINDOW_AUTOSIZE);
@@ -403,6 +404,8 @@
usleep(1000);
}
+ cal.setCal(g_color_cal, flags);
+
cvDestroyWindow("macbeth image");
cvReleaseImage(&g_img);
@@ -412,6 +415,7 @@
cvReleaseMat(&g_real_colors);
cvReleaseMat(&g_meas_colors);
cvReleaseMat(&g_reproj_colors);
+ cvReleaseMat(&g_color_cal);
cvReleaseMat(&g_colors_pos);
cvReleaseMat(&g_hom);
Modified: pkg/trunk/vision/stereo_view/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_view/manifest.xml 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/vision/stereo_view/manifest.xml 2008-11-25 07:10:27 UTC (rev 7272)
@@ -11,4 +11,5 @@
<depend package="image_msgs"/>
<depend package="roscpp"/>
<depend package="topic_synchronizer"/>
+ <depend package="color_calib"/>
</package>
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-25 07:07:21 UTC (rev 7271)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-25 07:10:27 UTC (rev 7272)
@@ -43,6 +43,8 @@
#include "image_msgs/Image.h"
#include "image_msgs/CvBridge.h"
+#include "color_calib.h"
+
#include "topic_synchronizer.h"
using namespace std;
@@ -60,11 +62,24 @@
image_msgs::CvBridge rbridge;
image_msgs::CvBridge dbridge;
+ color_calib::Calibration lcal;
+ color_calib::Calibration rcal;
+
+ IplImage* lcalimage;
+ IplImage* rcalimage;
+
TopicSynchronizer<StereoView> sync;
bool subscribe_color_;
+ bool calib_color_;
+ bool recompand_;
- StereoView() : ros::node("stereo_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout), subscribe_color_(false)
+ ros::thread::mutex cv_mutex;
+
+ StereoView() : ros::node("stereo_view"),
+ lcal(this), rcal(this), lcalimage(NULL), rcalimage(NULL),
+ sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout),
+ subscribe_color_(false), calib_color_(false), recompand_(false)
{
cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
@@ -84,14 +99,44 @@
sync.subscribe("dcam/stereo_info", stinfo, 1);
}
+ ~StereoView()
+ {
+ if (lcalimage)
+ cvReleaseImage(&lcalimage);
+ if (rcalimage)
+ cvReleaseImage(&rcalimage);
+ }
+
void image_cb_all(ros::Time t)
{
+ cv_mutex.lock();
+
if (lbridge.fromImage(limage, "bgr"))
- cvShowImage("left", lbridge.toIpl());
+ {
+ if (calib_color_)
+ {
+ lbridge.reallocIfNeeded(&lcalimage, IPL_DEPTH_32F);
+ lcal.correctColor(lbridge.toIpl(), lcalimage, true, recompand_, COLOR_CAL_BGR);
+
+ cvShowImage("left", lcalimage);
+ } else
+ cvShowImage("left", lbridge.toIpl());
+ }
+
if (rbridge.fromImage(rimage, "bgr"))
- cvShowImage("right", rbridge.toIpl());
+ {
+ if (calib_color_)
+ {
+ rbridge.reallocIfNeeded(&rcalimage, IPL_DEPTH_32F);
+ rcal.correctColor(rbridge.toIpl(), rcalimage, true, recompand_, COLOR_CAL_BGR);
+
+ cvShowImage("right", rcalimage);
+ } else
+ cvShowImage("right", rbridge.toIpl());
+ }
+
if (dbridge.fromImage(dimage))
{
// Disparity has to be scaled to be be nicely displayable
@@ -101,7 +146,8 @@
cvReleaseImage(&disp);
}
- cvWaitKey(5);
+ cv_mutex.unlock();
+
}
void image_cb_timeout(ros::Time t)
@@ -118,6 +164,35 @@
//Proceed to show images anyways
image_cb_all(t);
}
+
+ bool spin()
+ {
+ while (ok())
+ {
+ cv_mutex.lock();
+ int key = cvWaitKey(3);
+
+ switch (key) {
+ case 10:
+ calib_color_ = !calib_color_;
+ break;
+ case 32:
+ recompand_ = !recompand_;
+ }
+
+ // Fetch color calibration parameters as necessary
+ if (calib_color_)
+ {
+ lcal.getFromParam("dcam/left/image_rect_color");
+ rcal.getFromParam("dcam/right/image_rect_color");
+ }
+
+ cv_mutex.unlock();
+ usleep(10000);
+ }
+
+ return true;
+ }
};
int main(int argc, char **argv)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
