テンプレートマッチした結果からeusのモデルの位置姿勢を生成したい

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")
 

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);
     }

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
+sample/rimokon-pose.launch
+sample/jetson-box.launch

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)
 

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 "</" tagstr ">" newline)
-          (str+ "<" tagstr attrstr "/>" newline)))
+            (str+ "<" tagstr attrstr ">" newline nodestr "</" tagstr ">" newline)
+          (str+ " <" tagstr attrstr "/>" newline)))
        (t "")
        ))))
   )

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,37 +49,52 @@
           (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)
 		   (setq trans (elt tmpl-info 3))
 		   (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)))

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)))

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<cv::Point2f>(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<cv::Point2f> 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,