Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23835] pkg/trunk/stacks/calibration/ laser_cb_detector
|
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.
|
