Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21715] pkg/trunk
|
From: <sy...@us...> - 2009-08-12 21:46:59
|
Revision: 21715
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21715&view=rev
Author: syrnick
Date: 2009-08-12 21:46:50 +0000 (Wed, 12 Aug 2009)
Log Message:
-----------
camera offsetter. The package to calibrate the camera by hand.
Modified Paths:
--------------
pkg/trunk/calibration/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h
pkg/trunk/pr2/teleop/teleop_spacenav/manifest.xml
pkg/trunk/stacks/pr2/pr2_alpha/prg.launch
pkg/trunk/stacks/pr2/pr2_defs/calibration/prg_cal.xml
Added Paths:
-----------
pkg/trunk/calibration/sandbox/camera_offsetter/config/
pkg/trunk/calibration/sandbox/camera_offsetter/config/prg.narrow_stereo.offset
pkg/trunk/calibration/sandbox/camera_offsetter/launch/
pkg/trunk/calibration/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch
pkg/trunk/calibration/sandbox/camera_offsetter/test/
pkg/trunk/calibration/sandbox/camera_offsetter/test/test_stereo_offsetter.launch
pkg/trunk/stacks/stereo/stereo_image_proc/narrow_stereoproc_offset.launch
pkg/trunk/stacks/stereo/stereo_image_proc/throttle_narrow_stereo_offset.launch
Removed Paths:
-------------
pkg/trunk/calibration/sandbox/camera_offsetter/test_stereo_offsetter.launch
Property Changed:
----------------
pkg/trunk/calibration/sandbox/camera_offsetter/
Property changes on: pkg/trunk/calibration/sandbox/camera_offsetter
___________________________________________________________________
Added: svn:ignore
+ build
.build_version
lib
bin
Added: pkg/trunk/calibration/sandbox/camera_offsetter/config/prg.narrow_stereo.offset
===================================================================
--- pkg/trunk/calibration/sandbox/camera_offsetter/config/prg.narrow_stereo.offset (rev 0)
+++ pkg/trunk/calibration/sandbox/camera_offsetter/config/prg.narrow_stereo.offset 2009-08-12 21:46:50 UTC (rev 21715)
@@ -0,0 +1,2 @@
+-0.000235494 -0.000141299 0.0269986
+0.00261665 -0.00523329 9.12921e-06 0.999983
\ No newline at end of file
Modified: pkg/trunk/calibration/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h
===================================================================
--- pkg/trunk/calibration/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h 2009-08-12 21:46:47 UTC (rev 21714)
+++ pkg/trunk/calibration/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h 2009-08-12 21:46:50 UTC (rev 21715)
@@ -48,6 +48,8 @@
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
+#include <fstream>
+
namespace camera_offsetter
{
@@ -61,8 +63,55 @@
nh_.param("~position_scaling", position_scaling_, 1.0);
nh_.param("~angular_scaling", angular_scaling_, 1.0);
+
+ nh_.param("~config_file", config_file_, std::string("N/A"));
+
+ readConfig();
}
+ void readConfig()
+ {
+ if(config_file_==std::string("N/A"))
+ {
+ ROS_WARN("No config file is set");
+ return;
+ }
+
+ geometry_msgs::PosePtr p(new geometry_msgs::Pose());
+ std::fstream f (config_file_.c_str(), std::fstream::in);
+ if(!f.is_open())
+ {
+ ROS_WARN("Couldn't open config file");
+ return;
+ }
+ f >> p->position.x;
+ f >> p->position.y;
+ f >> p->position.z;
+ f >> p->orientation.x;
+ f >> p->orientation.y;
+ f >> p->orientation.z;
+ f >> p->orientation.w;
+ ROS_INFO_STREAM(p->orientation.x);
+ ROS_INFO_STREAM(p);
+ poseCb(p);
+ }
+ void saveConfig()
+ {
+ if(config_file_==std::string("N/A"))
+ {
+ ROS_WARN("No config file is set");
+ return;
+ }
+ std::fstream f (config_file_.c_str(), std::fstream::out);
+ f << virtual_offset_.getOrigin().x() << " ";
+ f << virtual_offset_.getOrigin().y() << " ";
+ f << virtual_offset_.getOrigin().z() << std::endl;
+ f << virtual_offset_.getRotation().x()<< " ";
+ f << virtual_offset_.getRotation().y()<< " ";
+ f << virtual_offset_.getRotation().z()<< " ";
+ f << virtual_offset_.getRotation().w();
+ }
+
void poseCb(const geometry_msgs::PoseConstPtr& msg)
{
boost::mutex::scoped_lock lock(offset_mutex_);
@@ -89,6 +138,8 @@
btTransform incrementalT(rot, trans);
virtual_offset_ = virtual_offset_ * incrementalT;
+
+ saveConfig();
}
void publishTransform(const ros::Time& time, const std::string frame_id, const std::string parent_id)
@@ -116,6 +167,8 @@
double position_scaling_;
double angular_scaling_;
+ std::string config_file_;
+
boost::mutex offset_mutex_;
btTransform virtual_offset_;
} ;
Added: pkg/trunk/calibration/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch
===================================================================
--- pkg/trunk/calibration/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch (rev 0)
+++ pkg/trunk/calibration/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch 2009-08-12 21:46:50 UTC (rev 21715)
@@ -0,0 +1,14 @@
+<launch>
+
+ <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
+ <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
+ <remap from="~virtual_twist" to="keyboard_twist" />
+ <param name="cam_name" type="string" value="/narrow_stereo" />
+ <param name="position_scaling" type="double" value=".0001" />
+ <param name="angular_scaling" type="double" value="1.0" />
+ <param name="cam_name" type="string" value="/narrow_stereo" />
+ <param name="config_file" type="string" value="$(find camera_offsetter)/config/$(env ROBOT).narrow_stereo.offset" />
+
+ </node>
+
+</launch>
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/camera_offsetter/test/test_stereo_offsetter.launch
===================================================================
--- pkg/trunk/calibration/sandbox/camera_offsetter/test/test_stereo_offsetter.launch (rev 0)
+++ pkg/trunk/calibration/sandbox/camera_offsetter/test/test_stereo_offsetter.launch 2009-08-12 21:46:50 UTC (rev 21715)
@@ -0,0 +1,11 @@
+<launch>
+
+ <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
+ <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
+ <remap from="~virtual_twist" to="keyboard_twist" />
+ <param name="cam_name" type="string" value="/narrow_stereo" />
+ <param name="position_scaling" type="double" value=".0001" />
+ <param name="angular_scaling" type="double" value="1.0" />
+ </node>
+
+</launch>
\ No newline at end of file
Deleted: pkg/trunk/calibration/sandbox/camera_offsetter/test_stereo_offsetter.launch
===================================================================
--- pkg/trunk/calibration/sandbox/camera_offsetter/test_stereo_offsetter.launch 2009-08-12 21:46:47 UTC (rev 21714)
+++ pkg/trunk/calibration/sandbox/camera_offsetter/test_stereo_offsetter.launch 2009-08-12 21:46:50 UTC (rev 21715)
@@ -1,11 +0,0 @@
-<launch>
-
- <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
- <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
- <remap from="~virtual_twist" to="keyboard_twist" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="position_scaling" type="double" value=".0001" />
- <param name="angular_scaling" type="double" value="1.0" />
- </node>
-
-</launch>
\ No newline at end of file
Modified: pkg/trunk/pr2/teleop/teleop_spacenav/manifest.xml
===================================================================
--- pkg/trunk/pr2/teleop/teleop_spacenav/manifest.xml 2009-08-12 21:46:47 UTC (rev 21714)
+++ pkg/trunk/pr2/teleop/teleop_spacenav/manifest.xml 2009-08-12 21:46:50 UTC (rev 21715)
@@ -11,6 +11,8 @@
<depend package="robot_mechanism_controllers" />
<depend package="mechanism_control" />
<depend package="spacenav_node" />
+ <depend package="joy" />
+
<url>http://pr.willowgarage.com</url>
<export>
</export>
Modified: pkg/trunk/stacks/pr2/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/prg.launch 2009-08-12 21:46:47 UTC (rev 21714)
+++ pkg/trunk/stacks/pr2/pr2_alpha/prg.launch 2009-08-12 21:46:50 UTC (rev 21715)
@@ -50,13 +50,13 @@
<node machine="four" pkg="imu_node" type="imu_node" output="screen"/>
<!-- Prosilica camera setup -->
- <group ns="prosilica">
+ <!--group ns="prosilica">
<include file="$(find prosilica_cam)/cam_settings.xml"/>
<param name="acquisition_mode" type="str" value="Triggered"/>
<param name="ip_address" type="str" value="10.68.7.20"/>
</group>
- <node machine="three" name="prosilica" pkg="prosilica_cam" type="prosilica_node" output="screen" respawn="true"/>
+ <node machine="three" name="prosilica" pkg="prosilica_cam" type="prosilica_node" output="screen" respawn="true"/-->
<!-- Double stereo setup -->
<!-- Wide is on robot right, goes to four -->
Modified: pkg/trunk/stacks/pr2/pr2_defs/calibration/prg_cal.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/calibration/prg_cal.xml 2009-08-12 21:46:47 UTC (rev 21714)
+++ pkg/trunk/stacks/pr2/pr2_defs/calibration/prg_cal.xml 2009-08-12 21:46:50 UTC (rev 21715)
@@ -2,56 +2,41 @@
<root>
<!-- *************************************************************** -->
- <!-- * Calibration values for the PRG2-Gandolf robot at Willow Garage * -->
+ <!-- * Calibration values for the PR2-Frodo robot at Willow Garage * -->
<!-- *************************************************************** -->
-
- <!-- Stereocam Position (Nominally 0) -->
- <property name="cal_stereo_x" value="0.0" />
- <property name="cal_stereo_y" value="0.0" />
- <property name="cal_stereo_z" value="0.0" />
- <property name="cal_stereo_roll" value="0.0" />
- <property name="cal_stereo_pitch" value="0.0" />
- <property name="cal_stereo_yaw" value="0.0" />
-
- <!-- Prosilica Cam Pose (Nominally 0) -->
- <property name="cal_high_def_x" value="0.0" />
- <property name="cal_high_def_y" value="0.0" />
- <property name="cal_high_def_z" value="0.0" />
- <property name="cal_high_def_roll" value="0.0" />
- <property name="cal_high_def_pitch" value="0.0" />
- <property name="cal_high_def_yaw" value="0.0" />
-
-
- <!-- Sensor Head Pose (Nominally 0) -->
- <property name="cal_head_x" value="0.0" />
- <property name="cal_head_y" value="0.0" />
- <property name="cal_head_z" value="0.0" />
- <property name="cal_head_roll" value="0.0" />
- <property name="cal_head_pitch" value="0.0" />
- <property name="cal_head_yaw" value="0.0" />
-
- <!-- Head joint gearing (Nominally 1) -->
- <property name="cal_head_pan_gearing" value="0.0" />
- <property name="cal_head_tilt_gearing" value="0.0" />
-
- <!-- Head joint offsets -->
- <property name="cal_head_pan_flag" value="0.0" />
- <property name="cal_head_tilt_flag" value="0.0" />
-
- <!-- Arm joint offsets (Nominally 0) -->
- <property name="cal_r_shoulder_pan_flag" value="0.0" />
- <property name="cal_r_shoulder_lift_flag" value="0.0" />
- <property name="cal_r_upper_arm_roll_flag" value="0.0" />
- <property name="cal_r_elbow_flex_flag" value="0.0" />
- <property name="cal_r_forearm_roll_flag" value="0.0" />
- <property name="cal_r_wrist_flex_flag" value="0.0" />
- <property name="cal_r_wrist_roll_flag" value="0.4" /> <!-- Manually Set -->
-
- <!-- Arm Gearing (Nominally 1) -->
- <property name="cal_r_shoulder_pan_gearing" value="1.0" />
- <property name="cal_r_shoulder_lift_gearing" value="1.0" />
- <property name="cal_r_upper_arm_roll_gearing" value="1.0" />
+ <property name="cal_stereo_x" value="0.039115" />
+ <property name="cal_stereo_y" value="0.006051" />
+ <property name="cal_stereo_z" value="-0.038254" />
+ <property name="cal_stereo_roll" value="0.011096" />
+ <property name="cal_stereo_pitch" value="-0.007152" />
+ <property name="cal_stereo_yaw" value="-0.009792" />
+ <property name="cal_high_def_x" value="0.009476" />
+ <property name="cal_high_def_y" value="0.003106" />
+ <property name="cal_high_def_z" value="-0.035546" />
+ <property name="cal_high_def_roll" value="0.011479" />
+ <property name="cal_high_def_pitch" value="0.006041" />
+ <property name="cal_high_def_yaw" value="0.013658" />
+ <property name="cal_head_x" value="-0.000819" />
+ <property name="cal_head_y" value="0.002317" />
+ <property name="cal_head_z" value="0.038099" />
+ <property name="cal_head_roll" value="0.000000" />
+ <property name="cal_head_pitch" value="0.000000" />
+ <property name="cal_head_yaw" value="0.000000" />
+ <property name="cal_r_shoulder_pan_flag" value="0.000000" />
+ <property name="cal_r_shoulder_lift_flag" value="-0.014003" />
+ <property name="cal_r_upper_arm_roll_flag" value="-0.008987" />
+ <property name="cal_r_elbow_flex_flag" value="-0.021100" />
+ <property name="cal_r_forearm_roll_flag" value="-0.032453" />
+ <property name="cal_r_wrist_flex_flag" value="0.034690" />
+ <property name="cal_r_wrist_roll_flag" value="0.4" /> <!-- Manually Set -->
+ <property name="cal_r_shoulder_pan_gearing" value="0.996635" />
+ <property name="cal_r_shoulder_lift_gearing" value="0.981056" />
+ <property name="cal_r_upper_arm_roll_gearing" value="1.005774" />
+ <property name="cal_head_pan_flag" value="-0.020095" />
+ <property name="cal_head_tilt_flag" value="0.000000" />
+ <property name="cal_head_pan_gearing" value="0.991906" />
+ <property name="cal_head_tilt_gearing" value="0.970987" />
-</root>
+</root>
\ No newline at end of file
Copied: pkg/trunk/stacks/stereo/stereo_image_proc/narrow_stereoproc_offset.launch (from rev 21634, pkg/trunk/stacks/stereo/stereo_image_proc/narrow_stereoproc.launch)
===================================================================
--- pkg/trunk/stacks/stereo/stereo_image_proc/narrow_stereoproc_offset.launch (rev 0)
+++ pkg/trunk/stacks/stereo/stereo_image_proc/narrow_stereoproc_offset.launch 2009-08-12 21:46:50 UTC (rev 21715)
@@ -0,0 +1,41 @@
+<launch>
+ <!-- videre_mode should be one of the following:
+ disparity: Disparity and rectification done on chip.
+ Provides: Disparity and left mono image
+ disparity_raw: Disparity done on chip (debayer and rectification in software).
+ Provides: disparity and left color image.
+ none: No stereo on chip (all processing done in software).
+ Provides: all 3 images available
+ rectified: Rectification on chip
+ Provides: all 3 images available but no color
+ -->
+ <!-- stereo processing parameters
+ texture_thresh: Threshold for removing bad disparities based on texture
+ Default value: 30
+ unique_thresh: Threshold for removing bad disparities based on ambiguity
+ Default value: 36
+ speckle_diff: Threshold for region-growing (1/16 pixel disparity)
+ Default value: 10
+ speckle_size: Threshold for disparity region size (pixels)
+ Default value: 100
+ horopter: X offset for close-in stereo (pixels)
+ Default value: 0
+ corr_size: Correlation window size (pixels)
+ Default value: 15
+ num_disp: Number of disparities (pixels)
+ Default value: 64
+ -->
+ <group ns="narrow_stereo_offset">
+ <node pkg="stereo_image_proc" type="stereoproc" respawn="false" output="screen" name="stereoproc">
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="do_colorize" type="bool" value="True"/>
+ <param name="do_rectify" type="bool" value="True"/>
+ <param name="do_stereo" type="bool" value="True"/>
+ <param name="do_calc_points" type="bool" value="True"/>
+ <param name="do_keep_coords" type="bool" value="True"/>
+ <param name="num_disp" type="int" value="128"/>
+ <param name="corr_size" type="int" value="7"/>
+ </node>
+ </group>
+</launch>
+
Property changes on: pkg/trunk/stacks/stereo/stereo_image_proc/narrow_stereoproc_offset.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/vision/stereo_image_proc/narrow_stereoproc.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Added: pkg/trunk/stacks/stereo/stereo_image_proc/throttle_narrow_stereo_offset.launch
===================================================================
--- pkg/trunk/stacks/stereo/stereo_image_proc/throttle_narrow_stereo_offset.launch (rev 0)
+++ pkg/trunk/stacks/stereo/stereo_image_proc/throttle_narrow_stereo_offset.launch 2009-08-12 21:46:50 UTC (rev 21715)
@@ -0,0 +1,9 @@
+<launch>
+
+<node pkg="topic_tools" type="throttle" name="stereo_cloud" args="messages /narrow_stereo_offset/cloud 1.0" />
+
+<node pkg="topic_tools" type="throttle" name="stereo_cloud" args="messages /narrow_stereo_offset/left/image_rect 1.0" />
+
+
+
+</launch>
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
