Menu
▾
▴
personalrobots-commit
|
From: <vij...@us...> - 2009-09-04 17:27:29
|
Revision: 23835
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23835&view=rev
Author: vijaypradeep
Date: 2009-09-04 17:27:20 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
LaserCbDetector compiles. Probably buggy. Unit tests need to be written.
Modified Paths:
--------------
pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt
pkg/trunk/stacks/calibration/laser_cb_detector/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/calibration/laser_cb_detector/action/
pkg/trunk/stacks/calibration/laser_cb_detector/action/Config.action
pkg/trunk/stacks/calibration/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h
pkg/trunk/stacks/calibration/laser_cb_detector/msg/
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigAction.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionFeedback.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionGoal.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionResult.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigFeedback.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigGoal.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigResult.msg
pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt 2009-09-04 17:05:59 UTC (rev 23834)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt 2009-09-04 17:27:20 UTC (rev 23835)
@@ -16,9 +16,10 @@
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-rosbuild_add_library(${PROJECT_NAME} src/cv_laser_bridge.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/cv_laser_bridge.cpp
+ src/laser_cb_detector.cpp)
-# rosbuild_genmsg()
+rosbuild_genmsg()
# rosbuild_gensrv()
#common commands for building c++ executables and libraries
Added: pkg/trunk/stacks/calibration/laser_cb_detector/action/Config.action
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/action/Config.action (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/action/Config.action 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,20 @@
+uint32 num_x # Number of checkerboard corners in the X direction
+uint32 num_y # Number of corners in the Y direction
+float32 spacing_x # Spacing between corners in the X direction (meters)
+float32 spacing_y # Spacing between corners in the Y direction (meters)
+
+# Specify how many times we want to upsample the image.
+# This is often useful for detecting small checkerboards far away
+float32 width_scaling
+float32 height_scaling
+
+# Specifiy how intensity maps into a uint8. A specified window of
+# intensities is linearly scaled to 0-255
+float32 min_intensity
+float32 max_intensity
+
+uint32 subpixel_window
+int32 subpixel_zero_zone
+
+---
+---
Added: pkg/trunk/stacks/calibration/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,67 @@
+/*********************************************************************
+ * 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.
+ *********************************************************************/
+
+//! \author Vijay Pradeep
+
+#ifndef LASER_CB_DETECTOR_LASER_CB_DETECTOR_H_
+#define LASER_CB_DETECTOR_LASER_CB_DETECTOR_H_
+
+#include <opencv/cv.h>
+#include <calibration_msgs/DenseLaserSnapshot.h>
+#include <laser_cb_detector/ConfigAction.h>
+#include <laser_cb_detector/cv_laser_bridge.h>
+#include <camera_calibration/CalibrationPattern.h>
+
+namespace laser_cb_detector
+{
+
+class LaserCbDetector
+{
+public:
+ LaserCbDetector();
+
+ bool configure(const ConfigGoal& config);
+
+ bool detect(const calibration_msgs::DenseLaserSnapshot& snapshot,
+ camera_calibration::CalibrationPattern& result);
+
+private:
+ bool configured_;
+ ConfigGoal config_;
+ CvLaserBridge bridge_;
+};
+
+}
+
+#endif
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/manifest.xml 2009-09-04 17:05:59 UTC (rev 23834)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/manifest.xml 2009-09-04 17:27:20 UTC (rev 23835)
@@ -11,7 +11,8 @@
<depend package="calibration_msgs"/>
<depend package="roscpp"/>
<depend package="opencv_latest" />
-
+ <depend package="actionlib_msgs" />
+ <depend package="camera_calibration" />
</package>
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigAction.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigAction.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigAction.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,4 @@
+
+ConfigActionGoal action_goal
+ConfigActionResult action_result
+ConfigActionFeedback action_feedback
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionFeedback.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionFeedback.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionFeedback.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalStatus status
+ConfigFeedback feedback
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionGoal.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionGoal.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionGoal.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalID goal_id
+ConfigGoal goal
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionResult.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionResult.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionResult.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalStatus status
+ConfigResult result
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigFeedback.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigFeedback.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigFeedback.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1 @@
+
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigGoal.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigGoal.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigGoal.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,18 @@
+uint32 num_x # Number of checkerboard corners in the X direction
+uint32 num_y # Number of corners in the Y direction
+float32 spacing_x # Spacing between corners in the X direction (meters)
+float32 spacing_y # Spacing between corners in the Y direction (meters)
+
+# Specify how many times we want to upsample the image.
+# This is often useful for detecting small checkerboards far away
+float32 width_scaling
+float32 height_scaling
+
+# Specifiy how intensity maps into a uint8. A specified window of
+# intensities is linearly scaled to 0-255
+float32 min_intensity
+float32 max_intensity
+
+uint32 subpixel_window
+int32 subpixel_zero_zone
+
Added: pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,124 @@
+/*********************************************************************
+ * 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.
+ *********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include <laser_cb_detector/laser_cb_detector.h>
+
+using namespace std;
+using namespace laser_cb_detector;
+
+LaserCbDetector::LaserCbDetector() : configured_(false)
+{
+
+}
+
+
+
+bool LaserCbDetector::configure(const ConfigGoal& config)
+{
+ config_ = config;
+
+ return true;
+}
+
+
+bool LaserCbDetector::detect(const calibration_msgs::DenseLaserSnapshot& snapshot,
+ camera_calibration::CalibrationPattern& result)
+{
+
+ // ***** Convert the snapshot into an image, based on intensity window in config *****
+ if(!bridge_.fromIntensity(snapshot, config_.min_intensity, config_.max_intensity))
+ return false;
+ IplImage* image = bridge_.toIpl();
+
+ // ***** Resize the image based on scaling parameters in config *****
+ const int scaled_width = (int) (.5 + image->width * config_.width_scaling);
+ const int scaled_height = (int) (.5 + image->height * config_.height_scaling);
+ IplImage* image_scaled = cvCreateImage(cvSize( scaled_width, scaled_height),
+ image->depth,
+ image->nChannels);
+ cvResize(image, image_scaled, CV_INTER_LINEAR);
+
+ // ***** Allocate vector for found corners *****
+ vector<CvPoint2D32f> cv_corners;
+ cv_corners.resize(config_.num_x*config_.num_y);
+
+ // ***** Do the actual checkerboard extraction *****
+ CvSize board_size = cvSize(config_.num_x, config_.num_y);
+ int num_corners ;
+ int found = cvFindChessboardCorners( image_scaled, board_size, &cv_corners[0], &num_corners,
+ CV_CALIB_CB_ADAPTIVE_THRESH) ;
+
+ if(found)
+ {
+ CvSize subpixel_window = cvSize(config_.subpixel_window,
+ config_.subpixel_window);
+ CvSize subpixel_zero_zone = cvSize(config_.subpixel_zero_zone,
+ config_.subpixel_zero_zone);
+
+ // Subpixel fine-tuning stuff
+ cvFindCornerSubPix(image_scaled, &cv_corners[0], num_corners,
+ subpixel_window,
+ subpixel_zero_zone,
+ cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
+ }
+ cvReleaseImage(&image_scaled);
+
+ // ***** Downscale the detected corners and generate the CalibrationPattern message *****
+ result.header.stamp = snapshot.header.stamp;
+ result.header.frame_id = snapshot.header.frame_id;
+
+ result.object_points.resize(config_.num_x * config_.num_y);
+ for (unsigned int i=0; i < config_.num_y; i++)
+ {
+ for (unsigned int j=0; j < config_.num_x; j++)
+ {
+ result.object_points[i*config_.num_x + j].x = j*config_.spacing_x;
+ result.object_points[i*config_.num_y + j].y = j*config_.spacing_y;
+ result.object_points[i*config_.num_y + j].z = 0.0;
+ }
+ }
+
+ result.image_points.resize(num_corners);
+ for (int i=0; i < num_corners; i++)
+ {
+ result.image_points[i].x = cv_corners[i].x / config_.width_scaling;
+ result.image_points[i].y = cv_corners[i].y / config_.height_scaling;
+ }
+
+ result.success = found;
+
+ return true;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-09-04 19:09:51
|
Revision: 23841
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23841&view=rev
Author: vijaypradeep
Date: 2009-09-04 19:09:42 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Adding unittests for the LaserCbDetector
Modified Paths:
--------------
pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp
pkg/trunk/stacks/calibration/laser_cb_detector/test/CMakeLists.txt
pkg/trunk/stacks/calibration/laser_cb_detector/test/cv_laser_bridge_unittest.cpp
Added Paths:
-----------
pkg/trunk/stacks/calibration/laser_cb_detector/test/data/
pkg/trunk/stacks/calibration/laser_cb_detector/test/data/cb_3x4.png
pkg/trunk/stacks/calibration/laser_cb_detector/test/laser_cb_detector_unittest.cpp
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp 2009-09-04 18:35:50 UTC (rev 23840)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp 2009-09-04 19:09:42 UTC (rev 23841)
@@ -106,7 +106,7 @@
for (unsigned int j=0; j < config_.num_x; j++)
{
result.object_points[i*config_.num_x + j].x = j*config_.spacing_x;
- result.object_points[i*config_.num_y + j].y = j*config_.spacing_y;
+ result.object_points[i*config_.num_y + j].y = i*config_.spacing_y;
result.object_points[i*config_.num_y + j].z = 0.0;
}
}
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/test/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/test/CMakeLists.txt 2009-09-04 18:35:50 UTC (rev 23840)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/test/CMakeLists.txt 2009-09-04 19:09:42 UTC (rev 23841)
@@ -4,3 +4,7 @@
rospack_add_gtest(test/cv_laser_bridge_unittest cv_laser_bridge_unittest.cpp)
target_link_libraries(test/cv_laser_bridge_unittest laser_cb_detector)
+
+
+rospack_add_gtest(test/laser_cb_detector_unittest laser_cb_detector_unittest.cpp)
+target_link_libraries(test/laser_cb_detector_unittest laser_cb_detector)
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/test/cv_laser_bridge_unittest.cpp
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/test/cv_laser_bridge_unittest.cpp 2009-09-04 18:35:50 UTC (rev 23840)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/test/cv_laser_bridge_unittest.cpp 2009-09-04 19:09:42 UTC (rev 23841)
@@ -41,6 +41,8 @@
using namespace std;
using namespace laser_cb_detector;
+static const bool DEBUG=false;
+
calibration_msgs::DenseLaserSnapshot buildSnapshot(double start_range, double start_intensity, double increment,
const unsigned int H, const unsigned int W)
{
@@ -96,7 +98,8 @@
EXPECT_EQ(image->height, (int) NUM_SCANS);
EXPECT_EQ(image->width, (int) RAYS_PER_SCAN);
- displayImage(image);
+ if (DEBUG)
+ displayImage(image);
// Check the first and last pixel in the image
EXPECT_EQ( (int) *((unsigned char*)(image->imageData)+0), 0);
@@ -118,7 +121,8 @@
EXPECT_EQ(image->height, (int) NUM_SCANS);
EXPECT_EQ(image->width, (int) RAYS_PER_SCAN);
- displayImage(image);
+ if (DEBUG)
+ displayImage(image);
// Check to make sure some of the pixels saturated near the beginning and end of the range
EXPECT_EQ( (int) *((unsigned char*)(image->imageData)+5), 0);
Added: pkg/trunk/stacks/calibration/laser_cb_detector/test/data/cb_3x4.png
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/test/data/cb_3x4.png (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/test/data/cb_3x4.png 2009-09-04 19:09:42 UTC (rev 23841)
@@ -0,0 +1,4 @@
+\x89PNG
+
+ |
|
From: <vij...@us...> - 2009-09-04 21:35:34
|
Revision: 23877
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23877&view=rev
Author: vijaypradeep
Date: 2009-09-04 21:35:19 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Adding a LaserCbDetectorNode. Not yet tested
Modified Paths:
--------------
pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector_node.cpp
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt 2009-09-04 21:34:06 UTC (rev 23876)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt 2009-09-04 21:35:19 UTC (rev 23877)
@@ -19,6 +19,9 @@
rosbuild_add_library(${PROJECT_NAME} src/cv_laser_bridge.cpp
src/laser_cb_detector.cpp)
+rosbuild_add_executable(laser_cb_detector_node src/laser_cb_detector_node.cpp)
+target_link_libraries(laser_cb_detector_node ${PROJECT_NAME})
+
rosbuild_genmsg()
# rosbuild_gensrv()
Added: pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector_node.cpp
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector_node.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector_node.cpp 2009-09-04 21:35:19 UTC (rev 23877)
@@ -0,0 +1,145 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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.
+*********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include <ros/ros.h>
+#include <laser_cb_detector/laser_cb_detector.h>
+#include <sstream>
+
+using namespace laser_cb_detector;
+using namespace std;
+
+
+#define ROS_INFO_CONFIG(name) \
+{\
+ ostringstream ss;\
+ ss << "[" << #name << "] -> " << config.name;\
+ ROS_INFO(ss.str().c_str());\
+}
+
+
+
+laser_cb_detector::ConfigGoal getParamConfig(ros::NodeHandle &n)
+{
+ laser_cb_detector::ConfigGoal config;
+
+ int num_x;
+ n.param("~num_x", num_x, 3);
+ config.num_x = (unsigned int) num_x;
+
+ int num_y;
+ n.param("~num_y", num_y, 3);
+ config.num_y = (unsigned int) num_y;
+
+ double spacing_x;
+ n.param("~spacing_x", spacing_x, 1.0);
+ config.spacing_x = spacing_x;
+
+ double spacing_y;
+ n.param("~spacing_y", spacing_y, 1.0);
+ config.spacing_y = spacing_y;
+
+ double width_scaling;
+ n.param("~width_scaling", width_scaling, 1.0);
+ config.width_scaling = width_scaling;
+
+ double height_scaling;
+ n.param("~height_scaling", height_scaling, 1.0);
+ config.height_scaling = height_scaling;
+
+ double min_intensity;
+ n.param("~min_intensity", min_intensity, 500.0);
+ config.min_intensity = min_intensity;
+
+ double max_intensity;
+ n.param("~max_intensity", max_intensity, 5000.0);
+ config.max_intensity = max_intensity;
+
+ int subpixel_window;
+ n.param("~subpixel_window", subpixel_window, 2);
+ config.subpixel_window = config.subpixel_window;
+
+ n.param("~subpixel_zero_zone", config.subpixel_zero_zone, -1);
+
+ ROS_INFO_CONFIG(num_x);
+ ROS_INFO_CONFIG(num_y);
+ ROS_INFO_CONFIG(spacing_x);
+ ROS_INFO_CONFIG(spacing_y);
+ ROS_INFO_CONFIG(width_scaling);
+ ROS_INFO_CONFIG(height_scaling);
+ ROS_INFO_CONFIG(min_intensity);
+ ROS_INFO_CONFIG(max_intensity);
+ ROS_INFO_CONFIG(subpixel_window);
+ ROS_INFO_CONFIG(subpixel_zero_zone);
+
+ return config;
+}
+
+void snapshotCallback(ros::Publisher* pub, LaserCbDetector* detector, const calibration_msgs::DenseLaserSnapshotConstPtr& msg)
+{
+ bool detect_result;
+ camera_calibration::CalibrationPattern result;
+ detect_result = detector->detect(*msg, result);
+
+ if (!detect_result)
+ ROS_ERROR("Error during checkerboard detection. (This error is worse than simply not seeing a checkerboard");
+ else
+ {
+ pub->publish(result);
+ }
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "laser_cb_detector");
+
+ ros::NodeHandle n;
+
+ // Set up the LaserCbDetector
+ laser_cb_detector::ConfigGoal config = getParamConfig(n);
+ LaserCbDetector detector;
+ detector.configure(config);
+
+ // Output
+ ros::Publisher pub = n.advertise<camera_calibration::CalibrationPattern>("laser_checkerboard", 1);
+
+ // Input
+ boost::function<void (const calibration_msgs::DenseLaserSnapshotConstPtr&)> cb
+ = boost::bind(&snapshotCallback, &pub, &detector, _1);
+
+ ros::Subscriber sub = n.subscribe(std::string("snapshot"), 1, cb);
+
+ ros::spin();
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X