diff --git a/imagesift/CMakeLists.txt b/imagesift/CMakeLists.txt index 0a6f349a33..abf1ea503d 100644 --- a/imagesift/CMakeLists.txt +++ b/imagesift/CMakeLists.txt @@ -60,6 +60,11 @@ jsk_feature("STAR" "SURF" "imagestar") jsk_feature("SIFT" "SURF" "imagesift_surf") jsk_feature("SIFT" "SIFT" "imagesift_sift") jsk_feature("ORB" "ORB" "imageorb") +elseif(("${OpenCV_VERSION}" VERSION_LESS 3.2.0) OR ("${OpenCV_VERSION}" VERSION_EQUAL 3.2.0)) +jsk_feature("ORB" "ORB" "imageorb") +jsk_feature("MSER" "MSER" "imagemser") +jsk_feature("KAZE" "KAZE" "imagekaze") +jsk_feature("AKAZE" "AKAZE" "imageakaze") endif() jsk_feature("BRISK" "BRISK" "imagebrisk") diff --git a/imagesift/src/imagefeatures.cpp.in b/imagesift/src/imagefeatures.cpp.in index 3a4645c101..799fa1e74b 100644 --- a/imagesift/src/imagefeatures.cpp.in +++ b/imagesift/src/imagefeatures.cpp.in @@ -122,7 +122,7 @@ public: _srvDetect = _node.advertiseService("Feature0DDetect", &SiftNode::detectCb, this); } else { - _subImage = _it.subscribe("image", 1, &SiftNode::imageCb, this); + _subImage = _it.subscribe(_node.resolveName("image"), 1, &SiftNode::imageCb, this); _subInfo = _node.subscribe("camera_info", 1, &SiftNode::infoCb, this); _srvDetect = _node.advertiseService("Feature0DDetect", &SiftNode::detectCb, this); } diff --git a/jsk_perception/.gitignore b/jsk_perception/.gitignore index 8b7dd14ac2..732276a2bc 100644 --- a/jsk_perception/.gitignore +++ b/jsk_perception/.gitignore @@ -3,4 +3,5 @@ launch/eusmodel_detection_elevator-panels-eng8.launch template/*png template/*xml sample/milktea-box.launch -sample/rimokon-pose.launch \ No newline at end of file +sample/rimokon-pose.launch +sample/jetson-box.launch diff --git a/jsk_perception/CMakeLists.txt b/jsk_perception/CMakeLists.txt index 07b572b0c2..96b9764846 100644 --- a/jsk_perception/CMakeLists.txt +++ b/jsk_perception/CMakeLists.txt @@ -372,13 +372,14 @@ add_custom_command( COMMAND ROS_PACKAGE_PATH=${PROJECT_SOURCE_DIR}:$ENV{ROS_PACKAGE_PATH} roseus ${PROJECT_SOURCE_DIR}/euslisp/eusmodel_template_gen.l WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}) add_custom_command( - OUTPUT ${PROJECT_SOURCE_DIR}/sample/rimokon-pose.launch ${PROJECT_SOURCE_DIR}/sample/milktea-box.launch + OUTPUT ${PROJECT_SOURCE_DIR}/sample/rimokon-pose.launch ${PROJECT_SOURCE_DIR}/sample/milktea-box.launch ${PROJECT_SOURCE_DIR}/sample/jetson-box.launch DEPENDS ${PROJECT_SOURCE_DIR}/sample/pose_detector_auto_gen_sample.l COMMAND ROS_PACKAGE_PATH=${PROJECT_SOURCE_DIR}:$ENV{ROS_PACKAGE_PATH} roseus ./pose_detector_auto_gen_sample.l WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/sample) add_custom_target(eusmodel_template ALL DEPENDS ${PROJECT_SOURCE_DIR}/sample/rimokon-pose.launch ${PROJECT_SOURCE_DIR}/sample/milktea-box.launch + ${PROJECT_SOURCE_DIR}/sample/jetson-box.launch ${PROJECT_SOURCE_DIR}/launch/eusmodel_detection_elevator-panels-eng8.launch ${PROJECT_SOURCE_DIR}/launch/eusmodel_detection_elevator-panels-eng2.launch) diff --git a/jsk_perception/euslisp/euslaunch.l b/jsk_perception/euslisp/euslaunch.l index b4de3a6fb4..ba2f7b47a3 100644 --- a/jsk_perception/euslisp/euslaunch.l +++ b/jsk_perception/euslisp/euslaunch.l @@ -36,8 +36,8 @@ (setq attrstr (apply #'concatenate string attrstr)) (setq nodestr (apply #'concatenate string nodestr)) (if nodes - (str+ "<" tagstr attrstr ">" nodestr "" newline) - (str+ "<" tagstr attrstr "/>" newline))) + (str+ "<" tagstr attrstr ">" newline nodestr "" newline) + (str+ " <" tagstr attrstr "/>" newline))) (t "") )))) ) diff --git a/jsk_perception/euslisp/eusmodel_template_gen_utils.l b/jsk_perception/euslisp/eusmodel_template_gen_utils.l index 075060fdd4..9c21a52799 100755 --- a/jsk_perception/euslisp/eusmodel_template_gen_utils.l +++ b/jsk_perception/euslisp/eusmodel_template_gen_utils.l @@ -23,8 +23,16 @@ (origin (send (car (rassoc #f(0 0) texco)) :pvertex aface)) (vx (v- (send (car (rassoc #f(1 0) texco)) :pvertex aface) origin)) (vy (v- (send (car (rassoc #f(0 1) texco)) :pvertex aface) origin)) - (width (norm vx)) (height (norm vy)) + (width (norm vx)) (height (norm vy)) obj_tmp depth (trans-rot (make-matrix 3 3))) + (cond ((derivedp obj faceset) + (setq obj_tmp obj)) + ((derivedp obj cascaded-link) + (setq obj_tmp (apply #'convex-hull-3d (send-all (send obj :bodies) :vertices)))) + (t + (warning-message 3 "unknown object class ~A~%" (send obj :super)) + )) + (setq depth (norm (v- (cadr (car (send obj_tmp :faces-intersect-with-point-vector (cadr (send aface :centroid)) (v- (send aface :normal))))) (cadr (send aface :centroid))))) (setq vx (scale (/ 1.0 width) vx) vy (scale (/ 1.0 height) vy)) (set-matrix-column trans-rot 0 vy) ;; ny (set-matrix-column trans-rot 1 vx) ;; nx @@ -41,17 +49,15 @@ (if (null (probe-file (send (pathname template) :directory-string))) (unix::mkdir (send (pathname template) :directory-string))) (unix::system (format nil "cp -f ~a ~a" imgfile template))) - (list template-include width height trans) + (list template-include width height trans depth) )))) -(defun gen-detection-nodes (obj) +(defun gen-detection-nodes (obj &key remap-args (publish-tf nil) (child-frame-id "matching")) (let (nodes) (setq nodes (mapcar #'(lambda(aface) (let* ((teximg (send aface :get :gl-textureimage)) - (imgpath (pathname (send teximg :name))) - (imgname (str+ (send imgpath :name) "." (send imgpath :type))) (tmpl-info (get-template-info obj aface)) trans quat) (when (and teximg tmpl-info) @@ -59,19 +65,36 @@ (setq quat (matrix2quaternion (send trans :rot))) (instance rosnode :init "jsk_perception" "point_pose_extractor" :name (format nil "point_pose_extractor_~a" (send obj :name)) + :remap_args remap-args :params (list (instance rosparam :init :name "window_name" :value (send obj :name)) (instance rosparam :init :name "template_filename" :value (car tmpl-info)) (instance rosparam :init :name "object_width" :value (* (elt tmpl-info 1) 0.001)) (instance rosparam :init :name "object_height" :value (* (elt tmpl-info 2) 0.001)) + (instance rosparam :init :name "object_depth" :value (* (elt tmpl-info 4) 0.001)) (instance rosparam :init :name "relative_pose" :value (concatenate float-vector (scale 0.001 (send trans :pos)) (subseq quat 1) (subseq quat 0 1))) - (instance rosparam :init :name "viewer_window" :value "false")) + (instance rosparam :init :name "viewer_window" :value "false") + (instance rosparam :init :name "publish_tf" :value (if publish-tf "true" "false")) + (instance rosparam :init :name "child_frame_id" :value child-frame-id) + ) )))) (send obj :faces))) (remove nil nodes))) -(defun gen-all-launch (objects launch-file-name &key (ns nil)) +(defun gen-all-launch (objects launch-file-name &key (ns nil) + (image-remap-args nil) + (gen-image-feature nil) + (image-topic "image") + (image-topic-transport "raw") + (image-feature-pkg "imagesift") (image-feature-type "imagesift") + (gen-object-marker nil) + (objectdetection-solve-tf nil) + (objectdetection-remap-args nil) + (object-builder "(make-cube 60 60 60)") + (object-frame-id "/map") + (publish-tf nil) (child-frame-id "matching") + ) ;; generate launchfile (with-open-file (launch-f launch-file-name :direction :output) @@ -84,7 +107,26 @@ (send obj :name))) (setq obj-include (format nil "$(find ~A)/template/_~x_~A.xml" *pkgname* (system::address obj) (send obj :name))) - (setq nodes (gen-detection-nodes obj)) + (setq nodes (gen-detection-nodes obj :remap-args image-remap-args :publish-tf publish-tf :child-frame-id child-frame-id)) + (if gen-image-feature + (push (instance rosnode :init image-feature-pkg image-feature-type + :name (format nil "~A_~A" image-feature-type (send obj :name)) + :remap_args image-remap-args + :params (list (instance rosparam :init :name "image_transport" :value image-topic-transport)) + ) nodes)) + (when gen-object-marker + (let* ((aface (car (mapcan #'(lambda (aface) (if (send aface :get :gl-textureimage) (list aface))) (send obj :faces)))) + (tmpl-info (get-template-info obj aface))) + (push (instance rosnode :init "jsk_perception" "kalman-filtered-objectdetection-marker.l" + :name (format nil "objectdetection_marker_~A" (send obj :name)) + :remap_args objectdetection-remap-args + :params (list (instance rosparam :init :name "object_builder" :value object-builder) + (instance rosparam :init :name "frame_id" :value object-frame-id) + (instance rosparam :init :name "solve_tf" :value (if objectdetection-solve-tf "true" "false")) + (instance rosparam :init :name "object_width" :value (* (elt tmpl-info 1) 0.001)) + (instance rosparam :init :name "object_height" :value (* (elt tmpl-info 2) 0.001)) + ) + ) nodes))) (send-all nodes :namespace ns) (setq xmllist (append (list "launch" nil) (send-all nodes :list))) diff --git a/jsk_perception/euslisp/kalman-filtered-objectdetection-marker.l b/jsk_perception/euslisp/kalman-filtered-objectdetection-marker.l index 30babf9fa9..078c0c8f17 100755 --- a/jsk_perception/euslisp/kalman-filtered-objectdetection-marker.l +++ b/jsk_perception/euslisp/kalman-filtered-objectdetection-marker.l @@ -6,8 +6,8 @@ (ros::roseus "marker-publish") (ros::roseus-add-msgs "posedetection_msgs") -(setq *map-id* "/map") -(setq *solve-tf* t) +(setq *map-id* (ros::get-param "~frame_id" "/map")) +(setq *solve-tf* (ros::get-param "~solve_tf" t)) (setq *object-width* (ros::get-param "~object_width" 0)) (setq *object-height* (ros::get-param "~object_height" 0)) (setq *relative-pose-str* (ros::get-param "~relative_pose" "0 0 0 0 0 0 1")) @@ -17,7 +17,8 @@ (setq *calc-kalman* t) (setq *detect-flag* t) (setq *tf-force* nil) -(setq *target-obj* (make-cube 60 60 60)) +(setq *target-obj-code* (read-from-string (ros::get-param "~object_builder" "(make-cube 60 60 60)"))) +(setq *target-obj* (eval *target-obj-code*)) (send *target-obj* :reset-coords) (setf (get *target-obj* :type) (ros::get-param "~target_type" nil)) (setq *map-frame-objectdetection* (instance posedetection_msgs::ObjectDetection :init)) @@ -122,7 +123,7 @@ (object-name *map-frame-objectdetection*) (text-name nil) (text-color nil) (obj-color nil)) (let ((mf-obj-lst (send object-name :objects)) - (tmp-tgtobj (make-cube 60 60 60)) + (tmp-tgtobj (eval *target-obj-code*)) (tp (get target-obj :type))) (when obj-color (send tmp-tgtobj :set-color obj-color)) @@ -164,7 +165,7 @@ (defun catch-outlier (target-obj) (let ((mf-obj-lst (send *map-frame-objectdetection* :objects)) - (tmp-tgtobj (make-cube 60 60 60)) + (tmp-tgtobj (eval *target-obj-code*)) (marker-life 700) (outlierflag nil) (tp (get target-obj :type))) diff --git a/jsk_perception/include/jsk_perception/point_pose_extractor.h b/jsk_perception/include/jsk_perception/point_pose_extractor.h index ddcdfd00ed..2b74463acc 100644 --- a/jsk_perception/include/jsk_perception/point_pose_extractor.h +++ b/jsk_perception/include/jsk_perception/point_pose_extractor.h @@ -104,6 +104,7 @@ class Matching_Template int _original_height_size; double _template_width; // width of template [m] double _template_height; // height of template [m] + double _template_depth; // depth of template [m] tf::Transform _relativepose; cv::Mat _affine_matrix; std::string _window_name; @@ -122,6 +123,7 @@ class Matching_Template int original_height_size, double template_width, double template_height, + double template_depth, tf::Transform relativepose, cv::Mat affine_matrix, double reprojection_threshold, @@ -135,6 +137,7 @@ class Matching_Template _original_height_size = original_height_size; _template_width = template_width; _template_height = template_height; + _template_depth = template_depth; _relativepose = relativepose; _affine_matrix = affine_matrix; _window_name = window_name; @@ -438,10 +441,10 @@ class Matching_Template tf::Vector3(0, _template_width, 0), tf::Vector3(_template_height, _template_width,0), tf::Vector3(_template_height, 0, 0), - tf::Vector3(0, 0, -0.03), - tf::Vector3(0, _template_width, -0.03), - tf::Vector3(_template_height, _template_width, -0.03), - tf::Vector3(_template_height, 0, -0.03)}; + tf::Vector3(0, 0, -1.0*_template_depth), + tf::Vector3(0, _template_width, -1.0*_template_depth), + tf::Vector3(_template_height, _template_width, -1.0*_template_depth), + tf::Vector3(_template_height, 0, -1.0*_template_depth)}; projected_top = std::vector(8); @@ -512,9 +515,9 @@ class Matching_Template if ( err_success ) { tf::Vector3 coords[4] = { tf::Vector3(0,0,0), - tf::Vector3(0.05,0,0), - tf::Vector3(0,0.05,0), - tf::Vector3(0,0,0.05)}; + tf::Vector3(_template_height/2,0,0), + tf::Vector3(0,_template_width/2,0), + tf::Vector3(0,0,_template_depth/2)}; std::vector ps(4); for(int i=0; i<4; i++) { @@ -591,7 +594,7 @@ namespace jsk_perception double template_width, double template_height, double th_step, double phi_step); virtual bool add_new_template(cv::Mat img, std::string typestr, tf::Transform relative_pose, - double template_width, double template_height, + double template_width, double template_height, double template_depth, double theta_step, double phi_step); virtual bool settemplate_cb (jsk_perception::SetTemplate::Request &req, jsk_perception::SetTemplate::Response &res); @@ -682,7 +685,7 @@ namespace jsk_perception Matching_Template* tmplt = new Matching_Template (tmp_warp_template, "sample", tmp_warp_template.size().width, tmp_warp_template.size().height, - width, height, + width, height, 0.05, tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)), M, mt->_reprojection_threshold, diff --git a/jsk_perception/sample/config/realsense-torso-jetson-box.rviz b/jsk_perception/sample/config/realsense-torso-jetson-box.rviz new file mode 100644 index 0000000000..94b3b22f60 --- /dev/null +++ b/jsk_perception/sample/config/realsense-torso-jetson-box.rviz @@ -0,0 +1,633 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /DepthCloud1/Auto Size1 + - /DepthCloud1/Occlusion Compensation1 + Splitter Ratio: 0.3970276117324829 + Tree Height: 383 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Camera +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base: + Value: true + collision_head_link_1: + Value: true + collision_head_link_2: + Value: true + display: + Value: true + dummyhead1: + Value: true + head: + Value: true + head_camera: + Value: true + jetson_box: + Value: true + left_arm_itb: + Value: true + left_arm_mount: + Value: true + left_gripper: + Value: true + left_gripper_base: + Value: true + left_hand: + Value: true + left_hand_accelerometer: + Value: true + left_hand_camera: + Value: true + left_hand_camera_axis: + Value: true + left_hand_range: + Value: true + left_lower_elbow: + Value: true + left_lower_forearm: + Value: true + left_lower_shoulder: + Value: true + left_torso_itb: + Value: true + left_upper_elbow: + Value: true + left_upper_elbow_visual: + Value: true + left_upper_forearm: + Value: true + left_upper_forearm_visual: + Value: true + left_upper_shoulder: + Value: true + left_wrist: + Value: true + matching: + Value: true + pedestal: + Value: true + realsense_torso_color_optical_frame: + Value: true + reference/base: + Value: true + reference/collision_head_link_1: + Value: true + reference/collision_head_link_2: + Value: true + reference/display: + Value: true + reference/dummyhead1: + Value: true + reference/head: + Value: true + reference/head_camera: + Value: true + reference/left_arm_itb: + Value: true + reference/left_arm_mount: + Value: true + reference/left_gripper: + Value: true + reference/left_gripper_base: + Value: true + reference/left_gripper_mass: + Value: true + reference/left_gripper_object_mass: + Value: true + reference/left_hand: + Value: true + reference/left_hand_accelerometer: + Value: true + reference/left_hand_camera: + Value: true + reference/left_hand_camera_axis: + Value: true + reference/left_hand_range: + Value: true + reference/left_lower_elbow: + Value: true + reference/left_lower_forearm: + Value: true + reference/left_lower_shoulder: + Value: true + reference/left_torso_itb: + Value: true + reference/left_upper_elbow: + Value: true + reference/left_upper_elbow_visual: + Value: true + reference/left_upper_forearm: + Value: true + reference/left_upper_forearm_visual: + Value: true + reference/left_upper_shoulder: + Value: true + reference/left_wrist: + Value: true + reference/pedestal: + Value: true + reference/right_arm_itb: + Value: true + reference/right_arm_mount: + Value: true + reference/right_gripper: + Value: true + reference/right_gripper_base: + Value: true + reference/right_gripper_mass: + Value: true + reference/right_gripper_object_mass: + Value: true + reference/right_hand: + Value: true + reference/right_hand_accelerometer: + Value: true + reference/right_hand_camera: + Value: true + reference/right_hand_camera_axis: + Value: true + reference/right_hand_range: + Value: true + reference/right_lower_elbow: + Value: true + reference/right_lower_forearm: + Value: true + reference/right_lower_shoulder: + Value: true + reference/right_torso_itb: + Value: true + reference/right_upper_elbow: + Value: true + reference/right_upper_elbow_visual: + Value: true + reference/right_upper_forearm: + Value: true + reference/right_upper_forearm_visual: + Value: true + reference/right_upper_shoulder: + Value: true + reference/right_wrist: + Value: true + reference/screen: + Value: true + reference/sonar_ring: + Value: true + reference/torso: + Value: true + right_arm_itb: + Value: true + right_arm_mount: + Value: true + right_gripper: + Value: true + right_gripper_base: + Value: true + right_hand: + Value: true + right_hand_accelerometer: + Value: true + right_hand_camera: + Value: true + right_hand_camera_axis: + Value: true + right_hand_range: + Value: true + right_lower_elbow: + Value: true + right_lower_forearm: + Value: true + right_lower_shoulder: + Value: true + right_torso_itb: + Value: true + right_upper_elbow: + Value: true + right_upper_elbow_visual: + Value: true + right_upper_forearm: + Value: true + right_upper_forearm_visual: + Value: true + right_upper_shoulder: + Value: true + right_wrist: + Value: true + screen: + Value: true + sonar_ring: + Value: true + tabletop_cpi_decomposeroutput00: + Value: false + tabletop_cpi_decomposeroutput01: + Value: false + tabletop_cpi_decomposeroutput02: + Value: false + torso: + Value: true + workspace: + Value: false + world: + Value: true + Marker Scale: 0.4000000059604645 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + base: + collision_head_link_1: + {} + collision_head_link_2: + {} + realsense_torso_color_optical_frame: + jetson_box: + {} + matching: + {} + tabletop_cpi_decomposeroutput00: + {} + tabletop_cpi_decomposeroutput01: + {} + tabletop_cpi_decomposeroutput02: + {} + torso: + head: + dummyhead1: + {} + head_camera: + {} + screen: + display: + {} + left_arm_mount: + left_upper_shoulder: + left_lower_shoulder: + left_upper_elbow: + left_lower_elbow: + left_upper_forearm: + left_arm_itb: + {} + left_lower_forearm: + left_wrist: + left_hand: + left_gripper_base: + left_gripper: + {} + left_hand_accelerometer: + {} + left_hand_camera: + {} + left_hand_camera_axis: + {} + left_hand_range: + {} + left_upper_forearm_visual: + {} + left_upper_elbow_visual: + {} + left_torso_itb: + {} + pedestal: + {} + right_arm_mount: + right_upper_shoulder: + right_lower_shoulder: + right_upper_elbow: + right_lower_elbow: + right_upper_forearm: + right_arm_itb: + {} + right_lower_forearm: + right_wrist: + right_hand: + right_gripper_base: + right_gripper: + {} + right_hand_accelerometer: + {} + right_hand_camera: + {} + right_hand_camera_axis: + {} + right_hand_range: + {} + right_upper_forearm_visual: + {} + right_upper_elbow_visual: + {} + right_torso_itb: + {} + sonar_ring: + {} + workspace: + {} + reference/base: + reference/collision_head_link_1: + {} + reference/collision_head_link_2: + {} + reference/torso: + reference/head: + reference/dummyhead1: + {} + reference/head_camera: + {} + reference/screen: + reference/display: + {} + reference/left_arm_mount: + reference/left_upper_shoulder: + reference/left_lower_shoulder: + reference/left_upper_elbow: + reference/left_lower_elbow: + reference/left_upper_forearm: + reference/left_arm_itb: + {} + reference/left_lower_forearm: + reference/left_wrist: + reference/left_hand: + reference/left_gripper_base: + reference/left_gripper: + {} + reference/left_gripper_mass: + reference/left_gripper_object_mass: + {} + reference/left_hand_accelerometer: + {} + reference/left_hand_camera: + {} + reference/left_hand_camera_axis: + {} + reference/left_hand_range: + {} + reference/left_upper_forearm_visual: + {} + reference/left_upper_elbow_visual: + {} + reference/left_torso_itb: + {} + reference/pedestal: + {} + reference/right_arm_mount: + reference/right_upper_shoulder: + reference/right_lower_shoulder: + reference/right_upper_elbow: + reference/right_lower_elbow: + reference/right_upper_forearm: + reference/right_arm_itb: + {} + reference/right_lower_forearm: + reference/right_wrist: + reference/right_hand: + reference/right_gripper_base: + reference/right_gripper: + {} + reference/right_gripper_mass: + reference/right_gripper_object_mass: + {} + reference/right_hand_accelerometer: + {} + reference/right_hand_camera: + {} + reference/right_hand_camera_axis: + {} + reference/right_hand_range: + {} + reference/right_upper_forearm_visual: + {} + reference/right_upper_elbow_visual: + {} + reference/right_torso_itb: + {} + reference/sonar_ring: + {} + Update Interval: 0 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /realsense_torso/color/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: false + - Alpha: 1 + Auto Size: + Auto Size Factor: 0.800000011920929 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.10677695274353027 + Min Value: -1.1363070011138916 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/DepthCloud + Color: 255; 255; 255 + Color Image Topic: /realsense_torso/color/image_rect_color + Color Transformer: RGB8 + Color Transport Hint: compressed + Decay Time: 0 + Depth Map Topic: /realsense_torso/aligned_depth_to_color/compressed/image_raw + Depth Map Transport Hint: compressedDepth + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: DepthCloud + Occlusion Compensation: + Occlusion Time-Out: 30 + Value: false + Position Transformer: XYZ + Queue Size: 5 + Selectable: false + Size (Pixels): 3 + Style: Flat Squares + Topic Filter: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /realsense_torso/color/point_pose_extractor_jetson_box/debug_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Features + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.019999999552965164 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /realsense_torso/color/object_pose + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /realsense_torso/color/object_detection_marker_array + Name: Detected Object + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /realsense_torso/color/image_rect_color + Name: Camera + Overlay Alpha: 0.800000011920929 + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + Visibility: + DepthCloud: true + Detected Object: true + Features: true + Grid: true + Image: true + Pose: true + TF: true + Value: true + Zoom Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.0465357303619385 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.7871444821357727 + Y: 0.04612310975790024 + Z: -0.05931009724736214 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.2002027928829193 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.9935853481292725 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Features: + collapsed: false + Height: 951 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001d9000001bcfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000019a000001bc000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000319fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000319000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000004f300000157fc0100000004fb00000010004600650061007400750072006500730100000000000002aa0000007000fffffffb0000000c00430061006d00650072006101000002b0000002430000006900fffffffb0000000a0049006d00610067006500000002b60000023d0000005e00fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000314000001bc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1267 + X: 998 + Y: 27 diff --git a/jsk_perception/sample/jetson-box.png b/jsk_perception/sample/jetson-box.png new file mode 100644 index 0000000000..85ba632bfb Binary files /dev/null and b/jsk_perception/sample/jetson-box.png differ diff --git a/jsk_perception/sample/pose_detector_auto_gen_sample.l b/jsk_perception/sample/pose_detector_auto_gen_sample.l index 396a9de234..3f2bce5b90 100755 --- a/jsk_perception/sample/pose_detector_auto_gen_sample.l +++ b/jsk_perception/sample/pose_detector_auto_gen_sample.l @@ -33,6 +33,14 @@ (send obj :name "lipton_milktea_box") obj)) +(defun make-jetson-box () + (let ((obj (make-cube 220 220 140))) + (setq aface (send obj :face 0)) + (send obj :paste-texture-to-face aface + :file "jetson-box.png") + (send obj :name "jetson_box") + obj)) + ;; < 2 > ;; . generate launch file (defun generate-launcher () @@ -40,7 +48,22 @@ (gen-all-launch (list (make-sony-rimokon) (make-sharp-rimokon)) "rimokon-pose.launch" :ns "/narrow_stereo/left" ) (gen-all-launch (list (make-lipton-tea-box)) - "milktea-box.launch" :ns "/narrow_stereo/left")) + "milktea-box.launch" :ns "/narrow_stereo/left") + (let ((obj (make-jetson-box))) + (gen-all-launch (list obj) + "jetson-box.launch" :ns "/realsense_torso/color" + :image-remap-args '(("image" "image_rect_color")) + :gen-image-feature t + :image-topic "image_rect_color" + :image-topic-transport "compressed" + :gen-object-marker t + :object-frame-id "/base" + :objectdetection-solve-tf t + :objectdetection-remap-args '(("input/ObjectDetection" "ObjectDetection")) + :object-builder (format nil "(make-cube ~A ~A ~A)" (elt (car (send obj :csg)) 1) (elt (car (send obj :csg)) 2) (elt (car (send obj :csg)) 3)) + :publish-tf t + )) + ) (warn ";; (generate-launcher)~%") ;; < 3 > diff --git a/jsk_perception/src/point_pose_extractor.cpp b/jsk_perception/src/point_pose_extractor.cpp index 3a49185548..dc368c066b 100644 --- a/jsk_perception/src/point_pose_extractor.cpp +++ b/jsk_perception/src/point_pose_extractor.cpp @@ -48,12 +48,14 @@ namespace jsk_perception std::string _pose_str; double template_width; double template_height; + double template_depth; std::string template_filename; std::string window_name; pnh_->param("child_frame_id", _child_frame_id, std::string("matching")); pnh_->param("object_width", template_width, 0.06); pnh_->param("object_height", template_height, 0.0739); + pnh_->param("object_depth", template_depth, 0.05); pnh_->param("relative_pose", _pose_str, std::string("0 0 0 0 0 0 1")); std::string default_template_file_name; try { @@ -111,7 +113,7 @@ namespace jsk_perception // add the image to template list add_new_template(template_img, window_name, transform, - template_width, template_height, _th_step, _phi_step); + template_width, template_height, template_depth, _th_step, _phi_step); } // initialize @@ -200,7 +202,7 @@ namespace jsk_perception } bool PointPoseExtractor::add_new_template(cv::Mat img, std::string typestr, tf::Transform relative_pose, - double template_width, double template_height, + double template_width, double template_height, double template_depth=0.05, double theta_step=5.0, double phi_step=5.0) { std::vector imgs; @@ -219,7 +221,7 @@ namespace jsk_perception Matching_Template * tmplt = new Matching_Template(imgs.at(i), type, img.size().width, img.size().height, - template_width, template_height, + template_width, template_height, template_depth, relative_pose, Mvec.at(i), _reprojection_threshold, _distanceratio_threshold,