diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 53caa6c..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -set(ROSPACK_MAKEDIST true) - -# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of -# directories (or patterns, but directories should suffice) that should -# be excluded from the distro. This is not the place to put things that -# should be ignored everywhere, like "build" directories; that happens in -# rosbuild/rosbuild.cmake. Here should be listed packages that aren't -# ready for inclusion in a distro. -# -# This list is combined with the list in rosbuild/rosbuild.cmake. Note -# that CMake 2.6 may be required to ensure that the two lists are combined -# properly. CMake 2.4 seems to have unpredictable scoping rules for such -# variables. -#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) - -rosbuild_make_distribution(0.5.0) diff --git a/Makefile b/Makefile deleted file mode 100644 index 9e20158..0000000 --- a/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file diff --git a/camera_pose/CMakeLists.txt b/camera_pose/CMakeLists.txt new file mode 100644 index 0000000..d551af5 --- /dev/null +++ b/camera_pose/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(camera_pose) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/camera_pose/package.xml b/camera_pose/package.xml new file mode 100644 index 0000000..70a8667 --- /dev/null +++ b/camera_pose/package.xml @@ -0,0 +1,38 @@ + + camera_pose + 0.5.0 + This stack contains a set of tools to work with multiple rgb + camera's simultaneoulsy. + Vijay Pradeep + Wim Meeussen + Yiping Liu + + BSD + + http://ros.org/wiki/camera_pose + + + Vijay Pradeep + Wim Meeussen + Yiping Liu + + + catkin + + + + + + camera_pose_calibration + camera_pose_toolkits + kinect_depth_calibration + + + + + + + + + + diff --git a/camera_pose_calibration/CMakeLists.txt b/camera_pose_calibration/CMakeLists.txt index d81c5c8..c46322e 100644 --- a/camera_pose_calibration/CMakeLists.txt +++ b/camera_pose_calibration/CMakeLists.txt @@ -1,5 +1,17 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html +# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html +cmake_minimum_required(VERSION 2.8.3) +project(camera_pose_calibration) +# Load catkin and all dependencies required for this package +# TODO: remove all from COMPONENTS that are not catkin packages. +find_package(catkin REQUIRED COMPONENTS geometry_msgs calibration_msgs sensor_msgs image_cb_detector monocam_settler interval_intersection joint_states_settler tf_conversions tf2_ros tf2_kdl message_filters) + +# include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) +# CATKIN_MIGRATION: removed during catkin migration +# cmake_minimum_required(VERSION 2.4.6) + +# CATKIN_MIGRATION: removed during catkin migration +# include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # Set the build type. Options are: # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage @@ -9,26 +21,52 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries #set(ROS_BUILD_TYPE RelWithDebInfo) -rosbuild_init() -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) +# CATKIN_MIGRATION: removed during catkin migration +# rosbuild_init() #uncomment if you have defined messages -rosbuild_genmsg() +add_message_files( + FILES + CalibrationEstimate.msg + CameraCalibration.msg + CameraMeasurement.msg + CameraPose.msg + RobotMeasurement.msg +) #uncomment if you have defined services -rosbuild_gensrv() +add_service_files( + FILES + TriggerOne.srv +) #common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#add_library(${PROJECT_NAME} src/example.cpp) #target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) +# +# CATKIN_MIGRATION: removed during catkin migration +# rosbuild_add_boost_directories() +#find_package(Boost REQUIRED COMPONENTS thread) +#include_directories(${Boost_INCLUDE_DIRS}) +#target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) +#add_executable(example examples/example.cpp) #target_link_libraries(example ${PROJECT_NAME}) -#rosbuild_download_test_data(http://pr.willowgarage.com/data/camera_pose_calibration/camera_pose_cal_regression_data.bag test/camera_pose_cal_regression_data.bag fcd96a5c7a74a064640753fd9d82fff3) -#rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_camera_pose_calibration.launch) +#download_test_data(http://pr.willowgarage.com/data/camera_pose_calibration/camera_pose_cal_regression_data.bag test/camera_pose_cal_regression_data.bag fcd96a5c7a74a064640753fd9d82fff3) +#add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_camera_pose_calibration.launch) + +catkin_python_setup() + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES calibration_msgs geometry_msgs sensor_msgs std_msgs +) +# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package +# TODO: fill in what other packages will need to use this package +catkin_package( + DEPENDS geometry_msgs calibration_msgs sensor_msgs image_cb_detector monocam_settler interval_intersection joint_states_settler kdl tf_conversions tf2_ros tf2_kdl message_filters opencv2 + CATKIN_DEPENDS # TODO + INCLUDE_DIRS # TODO include + LIBRARIES # TODO +) diff --git a/camera_pose_calibration/Makefile b/camera_pose_calibration/Makefile deleted file mode 100644 index b75b928..0000000 --- a/camera_pose_calibration/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/camera_pose_calibration/blocks/rgb_block.launch b/camera_pose_calibration/blocks/rgb_block.launch index 61bfbfd..84b4fc4 100644 --- a/camera_pose_calibration/blocks/rgb_block.launch +++ b/camera_pose_calibration/blocks/rgb_block.launch @@ -3,14 +3,15 @@ + - + - + @@ -21,4 +22,4 @@ - \ No newline at end of file + diff --git a/camera_pose_calibration/launch_extrinsics/calibrate_2_camera.launch b/camera_pose_calibration/launch_extrinsics/calibrate_2_camera.launch index b6dc071..032f798 100644 --- a/camera_pose_calibration/launch_extrinsics/calibrate_2_camera.launch +++ b/camera_pose_calibration/launch_extrinsics/calibrate_2_camera.launch @@ -1,6 +1,8 @@ + + @@ -9,12 +11,14 @@ + + @@ -28,6 +32,7 @@ + diff --git a/camera_pose_calibration/manifest.xml b/camera_pose_calibration/manifest.xml deleted file mode 100644 index 72d9f6e..0000000 --- a/camera_pose_calibration/manifest.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - This package provides the pipeline to calibrate the relative 6D - poses between multiple camera's. The calibration can be preformed - online, and the results are published to tf in realtime. - - Vijay Pradeep, Wim Meeussen - BSD - - http://ros.org/wiki/camera_pose_calibration - - - - - - - - - - - - - - - - - - - - diff --git a/camera_pose_calibration/package.xml b/camera_pose_calibration/package.xml new file mode 100644 index 0000000..a2071f5 --- /dev/null +++ b/camera_pose_calibration/package.xml @@ -0,0 +1,66 @@ + + camera_pose_calibration + 0.5.0 + This package provides the pipeline to calibrate the relative 6D + poses between multiple camera's. The calibration can be preformed + online, and the results are published to tf in realtime. + Vijay Pradeep + Wim Meeussen + + BSD + + http://ros.org/wiki/camera_pose_calibration + + + Vijay Pradeep + Wim Meeussen + + + catkin + + + geometry_msgs + calibration_msgs + sensor_msgs + image_cb_detector + monocam_settler + interval_intersection + joint_states_settler + kdl + tf_conversions + tf2_ros + tf2_kdl + message_filters + opencv2 + + + geometry_msgs + calibration_msgs + sensor_msgs + image_cb_detector + monocam_settler + interval_intersection + joint_states_settler + kdl + tf_conversions + tf2_ros + tf2_kdl + message_filters + opencv2 + + + + + + + + + + + + + + + + + diff --git a/camera_pose_calibration/src/camera_pose_calibration/calibration_transform_publisher.py b/camera_pose_calibration/scripts/calibration_transform_publisher.py similarity index 97% rename from camera_pose_calibration/src/camera_pose_calibration/calibration_transform_publisher.py rename to camera_pose_calibration/scripts/calibration_transform_publisher.py index 0a0c495..0ce95d6 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/calibration_transform_publisher.py +++ b/camera_pose_calibration/scripts/calibration_transform_publisher.py @@ -1,6 +1,5 @@ #!/usr/bin/python -import roslib; roslib.load_manifest('camera_pose_calibration') import tf2_ros import rospy import PyKDL diff --git a/camera_pose_calibration/scripts/camera_info_converter.py b/camera_pose_calibration/scripts/camera_info_converter.py new file mode 100755 index 0000000..e5827c7 --- /dev/null +++ b/camera_pose_calibration/scripts/camera_info_converter.py @@ -0,0 +1,13 @@ +#!/usr/bin/python + +from camera_pose_calibration.camera_info_converter import CameraInfoConverter + +def main(): + rospy.init_node('camera_info_converter') + camera_converter = CameraInfoConverter() + rospy.spin() + + + +if __name__ == '__main__': + main() diff --git a/camera_pose_calibration/src/camera_pose_calibration/capture_monitor.py b/camera_pose_calibration/scripts/capture_monitor.py similarity index 96% rename from camera_pose_calibration/src/camera_pose_calibration/capture_monitor.py rename to camera_pose_calibration/scripts/capture_monitor.py index 4067383..d4ea0aa 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/capture_monitor.py +++ b/camera_pose_calibration/scripts/capture_monitor.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -import roslib; roslib.load_manifest('camera_pose_calibration') import cv from cv_bridge import CvBridge, CvBridgeError import rospy @@ -185,22 +184,22 @@ def loop(self): if self.capture_time+rospy.Duration(4.0) > rospy.Time.now(): if self.capture_time+rospy.Duration(2.0) > rospy.Time.now(): # Captured checkerboards - self.pub.publish(self.bridge.cv_to_imgmsg(self.image_captured, encoding="passthrough")) - elif self.calibrate_time+rospy.Duration(8.0) > rospy.Time.now(): + self.pub.publish(self.bridge.cv_to_imgmsg(self.image_captured, encoding="rgb8")) + elif self.calibrate_time+rospy.Duration(20.0) > rospy.Time.now(): # Succeeded optimization - self.pub.publish(self.bridge.cv_to_imgmsg(self.image_optimized, encoding="passthrough")) + self.pub.publish(self.bridge.cv_to_imgmsg(self.image_optimized, encoding="rgb8")) if beep_time+rospy.Duration(8.0) < rospy.Time.now(): beep_time = rospy.Time.now() beep([(600, 63, 0.1), (800, 63, 0.1), (1000, 63, 0.3)]) else: # Failed optimization - self.pub.publish(self.bridge.cv_to_imgmsg(self.image_failed, encoding="passthrough")) + self.pub.publish(self.bridge.cv_to_imgmsg(self.image_failed, encoding="rgb8")) if beep_time+rospy.Duration(4.0) < rospy.Time.now(): beep_time = rospy.Time.now() beep([(400, 63, 0.1), (200, 63, 0.1), (100, 63, 0.6)]) else: - self.pub.publish(self.bridge.cv_to_imgmsg(self.image_out, encoding="passthrough")) + self.pub.publish(self.bridge.cv_to_imgmsg(self.image_out, encoding="rgb8")) diff --git a/camera_pose_calibration/src/camera_pose_calibration/filter_intervals.py b/camera_pose_calibration/scripts/filter_intervals.py similarity index 94% rename from camera_pose_calibration/src/camera_pose_calibration/filter_intervals.py rename to camera_pose_calibration/scripts/filter_intervals.py index 78835f4..986a120 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/filter_intervals.py +++ b/camera_pose_calibration/scripts/filter_intervals.py @@ -2,7 +2,6 @@ -import roslib; roslib.load_manifest('camera_pose_calibration') import rospy import threading from calibration_msgs.msg import Interval, CalibrationPattern @@ -30,8 +29,10 @@ def __init__(self): def interval_cb(self, msg): with self.lock: + print "got interval!" duration = msg.end - msg.start if self.feature and diff(self.feature, self.last_feature) > self.min_motion and duration > self.min_duration: + print "interval not good enough!" self.last_feature = self.feature self.pub.publish(msg) diff --git a/camera_pose_calibration/src/camera_pose_calibration/flip_flop_kinect.py b/camera_pose_calibration/scripts/flip_flop_kinect.py similarity index 90% rename from camera_pose_calibration/src/camera_pose_calibration/flip_flop_kinect.py rename to camera_pose_calibration/scripts/flip_flop_kinect.py index e7b3361..031bc61 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/flip_flop_kinect.py +++ b/camera_pose_calibration/scripts/flip_flop_kinect.py @@ -1,6 +1,5 @@ #!/usr/bin/python -import roslib; roslib.load_manifest('camera_pose_calibration') import rospy from camera_pose_calibration.srv import TriggerOne, TriggerOneRequest diff --git a/camera_pose_calibration/src/camera_pose_calibration/msg_saver.py b/camera_pose_calibration/scripts/msg_saver.py similarity index 97% rename from camera_pose_calibration/src/camera_pose_calibration/msg_saver.py rename to camera_pose_calibration/scripts/msg_saver.py index 6b6e918..702de2b 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/msg_saver.py +++ b/camera_pose_calibration/scripts/msg_saver.py @@ -1,7 +1,5 @@ #! /usr/bin/env python -import roslib; -roslib.load_manifest('camera_pose_calibration') - +import roslib import rospy import camera_pose_calibration.msg import std_msgs.msg @@ -9,6 +7,7 @@ import sys import os import rostopic +from rostopic import ROSTopicException def callback(msg): global topic_name, bag_filename diff --git a/camera_pose_calibration/src/camera_pose_calibration/multicam_capture_exec.py b/camera_pose_calibration/scripts/multicam_capture_exec.py similarity index 98% rename from camera_pose_calibration/src/camera_pose_calibration/multicam_capture_exec.py rename to camera_pose_calibration/scripts/multicam_capture_exec.py index f6386bb..bf05d77 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/multicam_capture_exec.py +++ b/camera_pose_calibration/scripts/multicam_capture_exec.py @@ -31,7 +31,6 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import roslib; roslib.load_manifest('camera_pose_calibration') import rospy import yaml import sys @@ -59,7 +58,7 @@ def __init__(self, cam_ids): # Construct a manager for each sensor stream (Don't enable any of them) cam_info_topic = rospy.get_param('~cam_info_topic', 'camera_info') - #print "cam_ids" + #print "cam_ids" self.cam_managers = [ (cam_id, CamManager( cam_id, cam_info_topic, self.add_cam_measurement) ) for cam_id in cam_ids ] diff --git a/camera_pose_calibration/src/camera_pose_calibration/run_optimization_fake.py b/camera_pose_calibration/scripts/run_optimization_fake.py similarity index 98% rename from camera_pose_calibration/src/camera_pose_calibration/run_optimization_fake.py rename to camera_pose_calibration/scripts/run_optimization_fake.py index dedf75c..1df4951 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/run_optimization_fake.py +++ b/camera_pose_calibration/scripts/run_optimization_fake.py @@ -6,8 +6,6 @@ from numpy import * import random -import roslib -roslib.load_manifest('camera_pose_calibration') import PyKDL from tf_conversions import posemath diff --git a/camera_pose_calibration/src/camera_pose_calibration/run_optimization_online.py b/camera_pose_calibration/scripts/run_optimization_online.py similarity index 64% rename from camera_pose_calibration/src/camera_pose_calibration/run_optimization_online.py rename to camera_pose_calibration/scripts/run_optimization_online.py index cc4570f..73e6bb0 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/run_optimization_online.py +++ b/camera_pose_calibration/scripts/run_optimization_online.py @@ -1,8 +1,5 @@ #! /usr/bin/env python -import roslib -roslib.load_manifest('camera_pose_calibration') - import itertools import collections import rospy @@ -25,7 +22,7 @@ class Estimator: def __init__(self): self.lock = threading.Lock() - self.meas = [] ## + self.meas = [] ## self.reset() @@ -33,8 +30,8 @@ def __init__(self): self.tf_check_pairs = [] self.tf_listener = tf.TransformListener() - self.measurement_count = 0 - self.timestamps =[] + self.measurement_count = 0 + self.timestamps =[] self.pub = rospy.Publisher('camera_calibration', CameraCalibration) self.sub_reset = rospy.Subscriber('reset', Empty, self.reset_cb) @@ -49,35 +46,27 @@ def __init__(self): def reset_cb(self, msg): self.reset() - - - - - def reset(self): with self.lock: self.state = None - count = len(self.meas) + count = len(self.meas) self.meas = [] - self.timestamps = [] + self.timestamps = [] rospy.loginfo('Reset calibration state') - print "\033[33;1m%ld\033[0m previous measurements in the cache are deleted.\n\n" % count - + print "\033[33;1m%ld\033[0m previous measurements in the cache are deleted.\n\n" % count def getAngle(self, quaternion): - if (math.fabs(quaternion[3])>1): - print "quaternion[3] out of range" # send out a notice only - s = 2.* math.acos(quaternion[3]) - return s + if (math.fabs(quaternion[3])>1): + print "quaternion[3] out of range" # send out a notice only + s = 2.* math.acos(quaternion[3]) + return s def getAxis(self, quaternion): - s_squared = 1. - math.pow(quaternion[3], 2.) - if s_squared < 10. * 1e-5 : #FLT_EPSILON 1e-5 - return (1.0, 0.0, 0.0) #Arbitrary - s = math.sqrt(s_squared) - return (quaternion[0]/s, quaternion[1]/s, quaternion[2]/s) - - + s_squared = 1. - math.pow(quaternion[3], 2.) + if s_squared < 10. * 1e-5 : #FLT_EPSILON 1e-5 + return (1.0, 0.0, 0.0) #Arbitrary + s = math.sqrt(s_squared) + return (quaternion[0]/s, quaternion[1]/s, quaternion[2]/s) def meas_cb(self, msg): with self.lock: @@ -97,17 +86,12 @@ def meas_cb(self, msg): #for camera in msg.M_cam: # camera.cam_info = camera_info_converter.unbin(camera.cam_info) - - if rospy.has_param('optimizer_conditional_resetter_enable'): self.reset_flag = rospy.get_param('optimizer_conditional_resetter_enable') else: self.reset_flag = False - - - - if self.reset_flag == True : + if self.reset_flag == True : urdf_cam_ns = rospy.get_param("urdf_cam_ns") new_cam_ns = rospy.get_param("new_cam_ns") u_info = rospy.wait_for_message(urdf_cam_ns+'/camera_info', CameraInfo) @@ -115,7 +99,6 @@ def meas_cb(self, msg): urdf_cam_frame = u_info.header.frame_id new_cam_frame = n_info.header.frame_id mounting_frame = rospy.get_param("mounting_frame") - # if any of the frames has changed... if self.prev_3_frames != () : @@ -126,7 +109,7 @@ def meas_cb(self, msg): self.state = None self.meas = [] # clear cache self.timestamps = [] - self.measurement_count = 0 + self.measurement_count = 0 self.prev_tf_transforms = {} rospy.loginfo('Reset calibration state') print "\033[43;1mTarget frames have changed. Clean up everything and start over! \033[0m\n\n" @@ -141,62 +124,58 @@ def meas_cb(self, msg): self.prev_3_frames = (urdf_cam_frame, new_cam_frame, mounting_frame) self.prev_tf_pair = (mounting_frame, urdf_cam_frame) - TheMoment = msg.header.stamp + TheMoment = msg.header.stamp - # check for tf transform changes + # check for tf transform changes for (frame1, frame2) in self.tf_check_pairs: - transform = None + transform = None print "\nLooking up transformation \033[34;1mfrom\033[0m %s \033[34;1mto\033[0m %s ..." % (frame1, frame2) - while not rospy.is_shutdown(): - try: - self.tf_listener.waitForTransform(frame1, frame2, TheMoment, rospy.Duration(10)) - transform = self.tf_listener.lookupTransform(frame1, frame2, TheMoment) #((),()) - print "found\n" - break - except (tf.LookupException, tf.ConnectivityException): - print "transform lookup failed, retrying..." - - if self.measurement_count > 0: - prev_transform = self.prev_tf_transforms[(frame1, frame2)] - dx = transform[0][0] - prev_transform[0][0] - dy = transform[0][1] - prev_transform[0][1] - dz = transform[0][2] - prev_transform[0][2] - dAngle = self.getAngle(transform[1]) - self.getAngle(prev_transform[1]) - a=self.getAxis(transform[1]) - b=self.getAxis(prev_transform[1]) - dAxis = (a[0]*b[0] + a[1]*b[1] + a[2]*b[2])/1. # a dot b - - #print "\n" - #print "\033[34;1m-- Debug Info --\033[0m" - print "checking for pose change..." - print "measurement_count = %d" % self.measurement_count - print " dx = %10f | < 0.0002 m" % dx - print " dy = %10f | < 0.0002 m" % dy - print " dz = %10f | < 0.0002 m" % dz - print "dAngle = %10f | < 0.00174 rad" % dAngle - print " dAxis = %10f | > 0.99999\n" % dAxis - - if ( (math.fabs(dx) > 0.0002) or (math.fabs(dy) > 0.0002) or (math.fabs(dz) > 0.0002) or - (math.fabs(dAngle)>0.00174) or (math.fabs(dAxis) < 0.99999) ) : # if transform != previous_transform: - print "\033[44;1mDetect pose change: %s has changed pose relative to %s since last time!\033[0m\n\n" % (frame1, frame2) - ###self.reset() # no, you cannot call reset() here. + while not rospy.is_shutdown(): + try: + self.tf_listener.waitForTransform(frame1, frame2, TheMoment, rospy.Duration(10)) + transform = self.tf_listener.lookupTransform(frame1, frame2, TheMoment) #((),()) + print "found\n" + break + except (tf.LookupException, tf.ConnectivityException): + print "transform lookup failed, retrying..." + + if self.measurement_count > 0: + prev_transform = self.prev_tf_transforms[(frame1, frame2)] + dx = transform[0][0] - prev_transform[0][0] + dy = transform[0][1] - prev_transform[0][1] + dz = transform[0][2] - prev_transform[0][2] + dAngle = self.getAngle(transform[1]) - self.getAngle(prev_transform[1]) + a=self.getAxis(transform[1]) + b=self.getAxis(prev_transform[1]) + dAxis = (a[0]*b[0] + a[1]*b[1] + a[2]*b[2])/1. # a dot b + + #print "\n" + #print "\033[34;1m-- Debug Info --\033[0m" + print "checking for pose change..." + print "measurement_count = %d" % self.measurement_count + print " dx = %10f | < 0.0002 m" % dx + print " dy = %10f | < 0.0002 m" % dy + print " dz = %10f | < 0.0002 m" % dz + print "dAngle = %10f | < 0.00174 rad" % dAngle + print " dAxis = %10f | > 0.99999\n" % dAxis + + if ( (math.fabs(dx) > 0.0002) or (math.fabs(dy) > 0.0002) or (math.fabs(dz) > 0.0002) or + (math.fabs(dAngle)>0.00174) or (math.fabs(dAxis) < 0.99999) ) : # if transform != previous_transform: + print "\033[44;1mDetect pose change: %s has changed pose relative to %s since last time!\033[0m\n\n" % (frame1, frame2) + ###self.reset() # no, you cannot call reset() here. self.state = None - count = len(self.meas) + count = len(self.meas) self.meas = [] # clear cache self.timestamps = [] rospy.loginfo('Reset calibration state') print "\033[33;1m%ld\033[0m previous measurements in the cache are deleted.\n\n" % count - self.measurement_count = 0 + self.measurement_count = 0 - self.prev_tf_transforms[(frame1, frame2)] = transform - - - - - # add timestamp to list - self.timestamps.append(msg.header.stamp) + self.prev_tf_transforms[(frame1, frame2)] = transform + # add timestamp to list + self.timestamps.append(msg.header.stamp) # add measurements to list self.meas.append(msg) @@ -206,8 +185,8 @@ def meas_cb(self, msg): print "\n" - self.measurement_count += 1 - + self.measurement_count += 1 + print "\nNo. of measurements fed to optimizer: %d\n\n" % self.measurement_count ## self.last_stamp_pub.publish(self.meas[-1].header.stamp) @@ -230,8 +209,8 @@ def meas_cb(self, msg): res = CameraCalibration() ## initialize a new Message instance res.camera_pose = [camera.pose for camera in self.state.cameras] res.camera_id = [camera.camera_id for camera in self.state.cameras] - res.time_stamp = [timestamp for timestamp in self.timestamps] #copy - res.m_count = len(res.time_stamp); # self.measurement_count + res.time_stamp = [timestamp for timestamp in self.timestamps] #copy + res.m_count = len(res.time_stamp); # self.measurement_count self.pub.publish(res) diff --git a/camera_pose_calibration/src/camera_pose_calibration/run_optimization_prior.py b/camera_pose_calibration/scripts/run_optimization_prior.py similarity index 97% rename from camera_pose_calibration/src/camera_pose_calibration/run_optimization_prior.py rename to camera_pose_calibration/scripts/run_optimization_prior.py index 01a1ef0..0e8d161 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/run_optimization_prior.py +++ b/camera_pose_calibration/scripts/run_optimization_prior.py @@ -4,8 +4,6 @@ import itertools import collections -import roslib -roslib.load_manifest('camera_pose_calibration') import PyKDL from tf_conversions import posemath from calibration_msgs.msg import * diff --git a/camera_pose_calibration/src/camera_pose_calibration/start_cb_detector_action_old.py b/camera_pose_calibration/scripts/start_cb_detector_action_old.py similarity index 94% rename from camera_pose_calibration/src/camera_pose_calibration/start_cb_detector_action_old.py rename to camera_pose_calibration/scripts/start_cb_detector_action_old.py index eb353e9..838f704 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/start_cb_detector_action_old.py +++ b/camera_pose_calibration/scripts/start_cb_detector_action_old.py @@ -1,6 +1,5 @@ #! /usr/bin/env python -import roslib; roslib.load_manifest('camera_pose_calibration') import rospy import actionlib import sys diff --git a/camera_pose_calibration/src/camera_pose_calibration/start_interval_intersection.py b/camera_pose_calibration/scripts/start_interval_intersection.py similarity index 92% rename from camera_pose_calibration/src/camera_pose_calibration/start_interval_intersection.py rename to camera_pose_calibration/scripts/start_interval_intersection.py index 8e9d068..d0f6d5a 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/start_interval_intersection.py +++ b/camera_pose_calibration/scripts/start_interval_intersection.py @@ -1,6 +1,5 @@ #! /usr/bin/env python -import roslib; roslib.load_manifest('camera_pose_calibration') import rospy import actionlib import sys diff --git a/camera_pose_calibration/src/camera_pose_calibration/start_monocam_settler_action.py b/camera_pose_calibration/scripts/start_monocam_settler_action.py similarity index 90% rename from camera_pose_calibration/src/camera_pose_calibration/start_monocam_settler_action.py rename to camera_pose_calibration/scripts/start_monocam_settler_action.py index d53f9d5..bd307ac 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/start_monocam_settler_action.py +++ b/camera_pose_calibration/scripts/start_monocam_settler_action.py @@ -1,6 +1,5 @@ #! /usr/bin/env python -import roslib; roslib.load_manifest('camera_pose_calibration') import rospy import actionlib diff --git a/camera_pose_calibration/src/camera_pose_calibration/static_transform_publisher.py b/camera_pose_calibration/scripts/static_transform_publisher.py similarity index 96% rename from camera_pose_calibration/src/camera_pose_calibration/static_transform_publisher.py rename to camera_pose_calibration/scripts/static_transform_publisher.py index de68342..635a7b4 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/static_transform_publisher.py +++ b/camera_pose_calibration/scripts/static_transform_publisher.py @@ -1,6 +1,5 @@ #!/usr/bin/python -import roslib; roslib.load_manifest('camera_pose_calibration') import tf2_ros import rospy import PyKDL diff --git a/camera_pose_calibration/src/camera_pose_calibration/trigger_one_image.py b/camera_pose_calibration/scripts/trigger_one_image.py similarity index 91% rename from camera_pose_calibration/src/camera_pose_calibration/trigger_one_image.py rename to camera_pose_calibration/scripts/trigger_one_image.py index 6acd81f..8e193aa 100755 --- a/camera_pose_calibration/src/camera_pose_calibration/trigger_one_image.py +++ b/camera_pose_calibration/scripts/trigger_one_image.py @@ -1,6 +1,5 @@ #!/usr/bin/python -import roslib; roslib.load_manifest('camera_pose_calibration') import rospy from camera_pose_calibration.srv import TriggerOne, TriggerOneResponse from sensor_msgs.msg import Image diff --git a/camera_pose_calibration/setup.py b/camera_pose_calibration/setup.py new file mode 100644 index 0000000..bd3eb75 --- /dev/null +++ b/camera_pose_calibration/setup.py @@ -0,0 +1,12 @@ +from distutils.core import setup +import glob + +scripts = glob.glob(os.path.join('scripts', '*.py')) + +setup( + version='0.5.0', + scripts=scripts, + packages=['camera_pose_calibration'], + package_dir={'': 'src'} +) + diff --git a/camera_pose_calibration/src/camera_pose_calibration/__init__.py b/camera_pose_calibration/src/camera_pose_calibration/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/camera_pose_calibration/src/camera_pose_calibration/camera_info_converter.py b/camera_pose_calibration/src/camera_pose_calibration/camera_info_converter.py old mode 100755 new mode 100644 index 70113df..9cc7d90 --- a/camera_pose_calibration/src/camera_pose_calibration/camera_info_converter.py +++ b/camera_pose_calibration/src/camera_pose_calibration/camera_info_converter.py @@ -1,7 +1,6 @@ #!/usr/bin/python import copy -import roslib; roslib.load_manifest('camera_pose_calibration') import rospy import threading from sensor_msgs.msg import CameraInfo @@ -68,13 +67,3 @@ def cam_info_cb(self, msg): if self.last_pub + self.pub_interval < time_now: self.pub.publish(unbin(msg)) self.last_pub = time_now - -def main(): - rospy.init_node('camera_info_converter') - camera_converter = CameraInfoConverter() - rospy.spin() - - - -if __name__ == '__main__': - main() diff --git a/camera_pose_calibration/src/camera_pose_calibration/estimate.py b/camera_pose_calibration/src/camera_pose_calibration/estimate.py index 19b9ee4..c444980 100644 --- a/camera_pose_calibration/src/camera_pose_calibration/estimate.py +++ b/camera_pose_calibration/src/camera_pose_calibration/estimate.py @@ -41,7 +41,7 @@ feature_width = 2 -def enhance(cal_samples, prior_estimate, num_iterations = 20, step_size = 0.5): +def enhance(cal_samples, prior_estimate, num_iterations = 200, step_size = 0.2): set_printoptions(linewidth=4000, precision=4, threshold=5000000000000000000000000000, suppress=True) next_estimate = deepcopy(prior_estimate) lam = 1.0 # lambda diff --git a/camera_pose_calibration/src/camera_pose_calibration/init_optimization_prior.py b/camera_pose_calibration/src/camera_pose_calibration/init_optimization_prior.py old mode 100755 new mode 100644 diff --git a/camera_pose_calibration/src/camera_pose_calibration/robot_measurement_cache.py b/camera_pose_calibration/src/camera_pose_calibration/robot_measurement_cache.py index c0fc76e..c18abb9 100644 --- a/camera_pose_calibration/src/camera_pose_calibration/robot_measurement_cache.py +++ b/camera_pose_calibration/src/camera_pose_calibration/robot_measurement_cache.py @@ -31,7 +31,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import roslib; roslib.load_manifest('pr2_calibration_executive') +import roslib; roslib.load_manifest('camera_pose_calibration') import sys import rospy diff --git a/camera_pose_toolkits/CMakeLists.txt b/camera_pose_toolkits/CMakeLists.txt index 79e4a6b..0b8d352 100644 --- a/camera_pose_toolkits/CMakeLists.txt +++ b/camera_pose_toolkits/CMakeLists.txt @@ -1,39 +1,47 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -rosbuild_init() - -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() +# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html +# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html +cmake_minimum_required(VERSION 2.8.3) +project(camera_pose_toolkits) + +find_package(Boost REQUIRED COMPONENTS regex thread) + +find_package(catkin REQUIRED COMPONENTS std_msgs rospy roscpp tf geometry_msgs + sensor_msgs camera_pose_calibration kdl_parser robot_state_publisher + dynamic_reconfigure rosbag) + + #uncomment if you have defined services -rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) - -rosbuild_add_boost_directories() - -rosbuild_add_executable(transform_finder_node src/transform_finder.cpp) -rosbuild_add_executable(camera_pose_static_transform_tf_broadcaster_node src/camera_pose_static_transform_tf_broadcaster.cpp) -rosbuild_add_executable(urdf_writer_node src/urdf_writer.cpp) -rosbuild_link_boost(urdf_writer_node regex) -rosbuild_add_executable(camera_dispatcher_node src/camera_dispatcher.cpp) -rosbuild_add_executable(transform_playback_node src/transform_playback.cpp) +add_service_files( + FILES + Switch.srv +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES std_msgs +) + +# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package +# TODO: fill in what other packages will need to use this package +catkin_package( + DEPENDS std_msgs rospy roscpp tf geometry_msgs sensor_msgs camera_pose_calibration kdl kdl_parser robot_state_publisher dynamic_reconfigure + CATKIN_DEPENDS # TODO + INCLUDE_DIRS # TODO include + LIBRARIES # TODO +) + +add_executable(transform_finder_node src/transform_finder.cpp) +target_link_libraries(transform_finder_node ${catkin_LIBRARIES}) + +add_executable(camera_pose_static_transform_tf_broadcaster_node src/camera_pose_static_transform_tf_broadcaster.cpp) +target_link_libraries(camera_pose_static_transform_tf_broadcaster_node ${catkin_LIBRARIES}) + +add_executable(urdf_writer_node src/urdf_writer.cpp) +target_link_libraries(urdf_writer_node ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_executable(camera_dispatcher_node src/camera_dispatcher.cpp) +target_link_libraries(camera_dispatcher_node ${catkin_LIBRARIES}) + +add_executable(transform_playback_node src/transform_playback.cpp) +target_link_libraries(transform_playback_node ${catkin_LIBRARIES}) + diff --git a/camera_pose_toolkits/Makefile b/camera_pose_toolkits/Makefile deleted file mode 100644 index b75b928..0000000 --- a/camera_pose_toolkits/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/camera_pose_toolkits/manifest.xml b/camera_pose_toolkits/manifest.xml deleted file mode 100644 index d5862f6..0000000 --- a/camera_pose_toolkits/manifest.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - The camera_pose_toolkits is a set of tools built around camera_pose_calibration package that allows the user to easily add new camera frame to existed tf tree. - - We assume that the target tf tree already has (at least) one camera frame (let's call it urdf camera frame). The new camera frame has to be rigidly mounted on one of the frames in the tf tree, but there can be joints between the urdf camera frame and the frame on which the new camera is mounted. This package will find out the static transform between the new camera frame and its mounting frame. The package will publish that transform to tf. It can also convert that transform into a xml snippet and insert it into the existed urdf description file. - - - Yiping Liu - BSD - - http://ros.org/wiki/camera_pose_toolkits - - - - - - - - - - - - - - diff --git a/camera_pose_toolkits/package.xml b/camera_pose_toolkits/package.xml new file mode 100644 index 0000000..bc4138f --- /dev/null +++ b/camera_pose_toolkits/package.xml @@ -0,0 +1,59 @@ + + camera_pose_toolkits + 0.5.0 + The camera_pose_toolkits is a set of tools built around camera_pose_calibration package that allows the user to easily add new camera frame to existed tf tree. + + We assume that the target tf tree already has (at least) one camera frame (let's call it urdf camera frame). The new camera frame has to be rigidly mounted on one of the frames in the tf tree, but there can be joints between the urdf camera frame and the frame on which the new camera is mounted. This package will find out the static transform between the new camera frame and its mounting frame. The package will publish that transform to tf. It can also convert that transform into a xml snippet and insert it into the existed urdf description file. + Yiping Liu + + BSD + + http://ros.org/wiki/camera_pose_toolkits + + + Yiping Liu + + + catkin + + + std_msgs + rospy + roscpp + tf + rosbag + geometry_msgs + sensor_msgs + camera_pose_calibration + kdl + kdl_parser + robot_state_publisher + dynamic_reconfigure + + + std_msgs + rospy + roscpp + tf + geometry_msgs + sensor_msgs + camera_pose_calibration + kdl + kdl_parser + robot_state_publisher + dynamic_reconfigure + + + + + + + + + + + + + + + diff --git a/camera_pose_toolkits/src/camera_pose_toolkits/dispatcher_gui.py b/camera_pose_toolkits/scripts/dispatcher_gui.py similarity index 100% rename from camera_pose_toolkits/src/camera_pose_toolkits/dispatcher_gui.py rename to camera_pose_toolkits/scripts/dispatcher_gui.py diff --git a/camera_pose_toolkits/src/camera_pose_toolkits/transform_message_saver.py b/camera_pose_toolkits/scripts/transform_message_saver.py similarity index 100% rename from camera_pose_toolkits/src/camera_pose_toolkits/transform_message_saver.py rename to camera_pose_toolkits/scripts/transform_message_saver.py diff --git a/camera_pose_toolkits/setup.py b/camera_pose_toolkits/setup.py new file mode 100644 index 0000000..ecd8274 --- /dev/null +++ b/camera_pose_toolkits/setup.py @@ -0,0 +1,12 @@ +from distutils.core import setup +import glob + +scripts = glob.glob(os.path.join('scripts', '*.py')) + +setup( + version='0.5.0', + scripts=scripts, + packages=['camera_pose_toolkits'], + package_dir={} +) + diff --git a/kinect_depth_calibration/CMakeLists.txt b/kinect_depth_calibration/CMakeLists.txt index 853165b..b27e2fd 100644 --- a/kinect_depth_calibration/CMakeLists.txt +++ b/kinect_depth_calibration/CMakeLists.txt @@ -1,30 +1,32 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html +# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html +cmake_minimum_required(VERSION 2.8.3) +project(kinect_depth_calibration) +# Load catkin and all dependencies required for this package +# TODO: remove all from COMPONENTS that are not catkin packages. +find_package(catkin REQUIRED COMPONENTS rospy roslib + cv_bridge geometry_msgs sensor_msgs stereo_msgs tf_conversions + tf ) -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) -rosbuild_init() +#uncomment if you have defined services +add_service_files( + FILES + GetCheckerboardCenter.srv GetCheckerboardPose.srv +) -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) +catkin_python_setup() + +generate_messages( + DEPENDENCIES geometry_msgs std_msgs +) -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -rosbuild_gensrv() -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) +# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package +# TODO: fill in what other packages will need to use this package +catkin_package( + DEPENDS rospy roslib opencv2 cv_bridge geometry_msgs sensor_msgs stereo_msgs tf_conversions tf kdl + CATKIN_DEPENDS # TODO + INCLUDE_DIRS # TODO include + LIBRARIES # TODO +) diff --git a/kinect_depth_calibration/Makefile b/kinect_depth_calibration/Makefile deleted file mode 100644 index b75b928..0000000 --- a/kinect_depth_calibration/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/kinect_depth_calibration/manifest.xml b/kinect_depth_calibration/manifest.xml deleted file mode 100644 index 425abdf..0000000 --- a/kinect_depth_calibration/manifest.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - kinect_depth_calibration - - - Eitan Marder-Eppstein - BSD - - http://ros.org/wiki/kinect_depth_calibration - - - - - - - - - - - - - - - - - diff --git a/kinect_depth_calibration/package.xml b/kinect_depth_calibration/package.xml new file mode 100644 index 0000000..69c1f31 --- /dev/null +++ b/kinect_depth_calibration/package.xml @@ -0,0 +1,53 @@ + + kinect_depth_calibration + 0.5.0 + kinect_depth_calibration + Eitan Marder-Eppstein + + BSD + + http://ros.org/wiki/kinect_depth_calibration + + + Eitan Marder-Eppstein + + + catkin + + + rospy + roslib + opencv2 + cv_bridge + geometry_msgs + sensor_msgs + stereo_msgs + tf_conversions + tf + kdl + + + rospy + roslib + opencv2 + cv_bridge + geometry_msgs + sensor_msgs + stereo_msgs + tf_conversions + tf + kdl + + + + + + + + + + + + + + diff --git a/kinect_depth_calibration/setup.py b/kinect_depth_calibration/setup.py new file mode 100644 index 0000000..31ac9a7 --- /dev/null +++ b/kinect_depth_calibration/setup.py @@ -0,0 +1,12 @@ +from distutils.core import setup +import glob + +scripts = glob.glob(os.path.join('scripts', '*.py')) + +setup( + version='0.5.0', + scripts=scripts, + packages=['kinect_depth_calibration'], + package_dir={} +) + diff --git a/stack.xml b/stack.xml deleted file mode 100644 index 5a459a2..0000000 --- a/stack.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - This stack contains a set of tools to work with multiple rgb - camera's simultaneoulsy. - - Vijay Pradeep, Wim Meeussen, Yiping Liu - BSD - - http://ros.org/wiki/camera_pose - - - - - - - - - - - -