Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix wide_stereo camera model parameters #403

Closed
wants to merge 1 commit into from
Closed

Fix wide_stereo camera model parameters #403

wants to merge 1 commit into from

Conversation

YutoUchimi
Copy link
Contributor

cx and cy field of euslisp camera model should be about half of width and height, but parameters of wide_stereo are far from those for now.
This can cause wrong output in ...

(send (send *pr2* :wide_stereo-left) :screen-point (send button-coords :worldpos))

so I fixed.

Parameters fixed in this PR are collected from PR1040's /wide_stereo/left/camera_info and /wide_stereo/right/camera_info.

@k-okada
Copy link
Member

k-okada commented Oct 18, 2019 via email

@YutoUchimi
Copy link
Contributor Author

OK, but could you please tell me how to run make-pr2-model-file.l?
Should I install latest euscollada from source?
My environment (Ubuntu16.04 + kinetic):

yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ apt show ros-kinetic-collada-urdf
Package: ros-kinetic-collada-urdf
Version: 1.12.12-0xenial-20190913-215040+0000
Priority: extra
Section: misc
Maintainer: Chris Lalancette <[email protected]>
Installed-Size: 586 kB
Depends: libassimp3v5, libboost-filesystem1.58.0, libboost-program-options1.58.0, libboost-system1.58.0, libc6 (>= 2.14), libcollada-dom2.4-dp0, libgcc1 (>= 1:3.0), libstdc++6 (>= 5.2), liburdfdom-model0.4, liburdfdom-world0.4, libassimp-dev, libcollada-dom2.4-dp-dev, liburdfdom-dev, liburdfdom-headers-dev, ros-kinetic-collada-parser, ros-kinetic-geometric-shapes, ros-kinetic-resource-retriever, ros-kinetic-rosconsole, ros-kinetic-urdf
Homepage: http://ros.org/wiki/collada_urdf
Download-Size: 159 kB
APT-Manual-Installed: no
APT-Sources: http://packages.ros.org/ros/ubuntu xenial/main amd64 Packages
Description: This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents.
 Implements robot-specific COLLADA extensions as defined by http://openrave.programmingvision.com/index.php/Started:COLLADA

yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ apt show ros-kinetic-euscollada
Package: ros-kinetic-euscollada
Version: 0.4.2-0xenial-20190913-215712+0000
Priority: extra
Section: misc
Maintainer: Yohei Kakiuchi <[email protected]>
Installed-Size: 596 kB
Depends: libassimp3v5, libboost-filesystem1.58.0, libboost-program-options1.58.0, libboost-system1.58.0, libc6 (>= 2.14), libcollada-dom2.4-dp0, libgcc1 (>= 1:3.0), libqhull7, libstdc++6 (>= 5.2), liburdfdom-model0.4, libyaml-cpp0.5v5, libcollada-dom2.4-dp-dev, libqhull-dev, liburdfdom-dev, libyaml-cpp-dev, ros-kinetic-assimp-devel, ros-kinetic-collada-parser, ros-kinetic-collada-urdf, ros-kinetic-resource-retriever, ros-kinetic-roscpp, ros-kinetic-rospack, ros-kinetic-rostest, ros-kinetic-tf, ros-kinetic-urdf
Homepage: http://ros.org/wiki/euscollada
Download-Size: 161 kB
APT-Manual-Installed: yes
APT-Sources: http://packages.ros.org/ros/ubuntu xenial/main amd64 Packages
Description: euscollada

I tried:

[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ rossetmaster pr1040
set ROS_MASTER_URI to http://pr1040:11311
[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ roseus make-pr2-model-file.l 
configuring by "/opt/ros/kinetic/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin 
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x5531300[16374] --> 0x59b11a0[32748] top=3d61
irtgl irtglc irtviewer 
EusLisp 9.26( 1.2.1) for Linux64 created on ip-172-30-1-168(Wed Jun 12 16:16:30 PST 2019)
roseus ;; loading roseus("1.7.4-17-gea59620") on euslisp((9.26 ip-172-30-1-168 Wed Jun 12 16:16:30 PST 2019  1.2.1))
eustf roseus_c_util 1.irteusgl$ (ros::roseus "make_pr2_model_file")
t
2.irteusgl$ (make-pr2-model-file)

Document successfully written to /tmp/pr2_31533.dae
;; Input file is: /tmp/pr2_31533.dae
[ INFO] [1571388362.858544356] [${node}]: joint base_footprint_joint is passive, but adding to hierarchy

[ INFO] [1571388362.858691191] [${node}]: joint base_bellow_joint is passive, but adding to hierarchy

[ INFO] [1571388362.858893852] [${node}]: joint base_laser_joint is passive, but adding to hierarchy

[ INFO] [1571388362.892299279] [${node}]: joint head_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388362.892408032] [${node}]: joint head_plate_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.892520218] [${node}]: joint head_mount_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893477205] [${node}]: joint head_mount_kinect_ir_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893561336] [${node}]: joint head_mount_kinect_ir_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893630839] [${node}]: joint head_mount_kinect_rgb_joint is passive, but adding to hierarchy

[ INFO] [1571388362.893726804] [${node}]: joint head_mount_kinect_rgb_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.894333946] [${node}]: joint head_mount_prosilica_joint is passive, but adding to hierarchy

[ INFO] [1571388362.894420042] [${node}]: joint head_mount_prosilica_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.944749814] [${node}]: joint projector_wg6802418_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.944872636] [${node}]: joint projector_wg6802418_child_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945010350] [${node}]: joint sensor_mount_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945107131] [${node}]: joint double_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945210209] [${node}]: joint narrow_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945298241] [${node}]: joint narrow_stereo_l_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945382999] [${node}]: joint narrow_stereo_l_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945465868] [${node}]: joint narrow_stereo_r_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945533777] [${node}]: joint narrow_stereo_r_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945605237] [${node}]: joint narrow_stereo_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945696502] [${node}]: joint wide_stereo_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945783724] [${node}]: joint wide_stereo_l_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945866959] [${node}]: joint wide_stereo_l_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.945952946] [${node}]: joint wide_stereo_r_stereo_camera_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946038079] [${node}]: joint wide_stereo_r_stereo_camera_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946124916] [${node}]: joint wide_stereo_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946273755] [${node}]: joint high_def_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.946326590] [${node}]: joint high_def_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952084248] [${node}]: joint imu_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952540294] [${node}]: joint l_upper_arm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952870564] [${node}]: joint l_forearm_cam_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.952942733] [${node}]: joint l_forearm_cam_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388362.953053357] [${node}]: joint l_forearm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.953421544] [${node}]: joint l_gripper_palm_joint is passive, but adding to hierarchy

[ INFO] [1571388362.962059801] [${node}]: joint l_gripper_led_joint is passive, but adding to hierarchy

[ INFO] [1571388362.962161721] [${node}]: joint l_gripper_motor_accelerometer_joint is passive, but adding to hierarchy

[ INFO] [1571388362.970883305] [${node}]: joint l_gripper_tool_joint is passive, but adding to hierarchy

[ INFO] [1571388362.973651670] [${node}]: joint left_arm_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388363.002012903] [${node}]: joint l_torso_lift_side_plate_joint is passive, but adding to hierarchy

[ INFO] [1571388363.002255651] [${node}]: joint laser_tilt_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005437126] [${node}]: joint r_upper_arm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005842699] [${node}]: joint r_forearm_cam_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388363.005937324] [${node}]: joint r_forearm_cam_optical_frame_joint is passive, but adding to hierarchy

[ INFO] [1571388363.006048225] [${node}]: joint r_forearm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.006478557] [${node}]: joint r_gripper_palm_joint is passive, but adding to hierarchy

[ INFO] [1571388363.014939571] [${node}]: joint r_gripper_led_joint is passive, but adding to hierarchy

[ INFO] [1571388363.015044962] [${node}]: joint r_gripper_motor_accelerometer_joint is passive, but adding to hierarchy

[ INFO] [1571388363.023737542] [${node}]: joint r_gripper_tool_joint is passive, but adding to hierarchy

[ INFO] [1571388363.026599531] [${node}]: joint right_arm_chain_cb is passive, but adding to hierarchy

[ INFO] [1571388363.054407921] [${node}]: joint r_torso_lift_side_plate_joint is passive, but adding to hierarchy

;; Config file is: /opt/ros/kinetic/share/euscollada/pr2.yaml
;; Output file is: /tmp/pr2_31533.l
;; robot_name is: pr2
torso@5
larm@7
rarm@15
head@23
load model file from parameter server /robot_description to /tmp/pr2_31533.l
received narrow_stereo/left/camera_info #<sensor_msgs::camerainfo #X58253b0> narrow_stereo_optical_frame
received narrow_stereo/right/camera_info #<sensor_msgs::camerainfo #X5929bd0> narrow_stereo_optical_frame
received wide_stereo/left/camera_info #<sensor_msgs::camerainfo #X5b01d28> wide_stereo_optical_frame
received wide_stereo/right/camera_info #<sensor_msgs::camerainfo #X5a67008> wide_stereo_optical_frame
received l_forearm_cam/camera_info #<sensor_msgs::camerainfo #X58cde30> l_forearm_cam_optical_frame
received r_forearm_cam/camera_info #<sensor_msgs::camerainfo #X64aa218> r_forearm_cam_optical_frame
received prosilica/camera_info #<sensor_msgs::camerainfo #X64b40c8> high_def_optical_frame
received kinect_head/rgb/camera_info #<sensor_msgs::camerainfo #X64cdb40> head_mount_kinect_rgb_optical_frame
[ERROR] [1571388372.414675320] [/make_pr2_model_file_1571388345260520413]: could not receive kinect_head/depth/camera_info kinect_head-depth
copy model file from /tmp/pr2_31533.l to /home/yutouchimi/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus/pr2.l
0
3.irteusgl$ (exit)
[ INFO] [1571388541.216984430] [/make_pr2_model_file_1571388345260520413]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [1571388541.217055228] [/make_pr2_model_file_1571388345260520413]: exiting roseus 0
[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ git status
ブランチ wide_stereo_camera_info
Changes not staged for commit:
  (use "git add <file>..." to update what will be committed)
  (use "git checkout -- <file>..." to discard changes in working directory)

	modified:   pr2.l

no changes added to commit (use "git add" and/or "git commit -a")
[http://pr1040:11311][133.11.216.166] yutouchimi@libra:~/Projects/pr2_subway/src/jsk-ros-pkg/jsk_pr2eus/pr2eus [wide_stereo_camera_info]
$ ls -lh pr2.l 
-rw-rw-r-- 1 yutouchimi yutouchimi 4.7M 10月 18 17:46 pr2.l

Large diff is generated (12.7 MB -> 4.7MB), so I can hardly check if this is correct way.

@k-okada
Copy link
Member

k-okada commented Oct 18, 2019 via email

@YutoUchimi YutoUchimi force-pushed the wide_stereo_camera_info branch from 52b8dd4 to c0dca8d Compare October 21, 2019 11:25
@YutoUchimi
Copy link
Contributor Author

With newly generated PR2 model, I confirmed that the center of wide_stereo is correctly set.

2.irteusgl$ (send (send *pr2* :wide_stereo-left) :cx)
321.406
3.irteusgl$ (send (send *pr2* :wide_stereo-left) :cy)
241.11

@YutoUchimi
Copy link
Contributor Author

Travis passed!

@knorth55 knorth55 mentioned this pull request Oct 26, 2019
Closed
3 tasks
@k-okada
Copy link
Member

k-okada commented Dec 14, 2019

I am not sure why this fix your problem, because make-camera-from-ros-camera-info-aux and camera parameter seems almost same for me.

-(defun make-camera-from-ros-camera-info-aux (pwidth pheight p frame-coords &rest args)
-  (let* ((fx (elt p 0))(fy (elt p 5))
-         (cx (elt p 2))(cy (elt p 6))
-         (fx*tx (elt p 3))(fy*ty (elt p 7))
-         (tx (* 1000 (/ fx*tx fx)))
-         (ty (* 1000 (/ fy*ty fy)))
-         )
-    (apply #'make-camera-from-param :pwidth pwidth :pheight pheight
-          :fx fx :fy fy :cx cx :cy cy
-          :tx tx :ty ty :parent-coords frame-coords args)))
+(defun make-camera-from-ros-camera-info-aux (pwidth pheight p frame-coords &rest args) (let* ((fx (elt p 0)) (fy (elt p 5)) (cx (elt p 2)) (cy (elt p 6)) (fx*tx (elt p 3)) (fy*ty (elt p 7)) (tx (* 1000 (/ fx*tx fx))) (ty (* 1000 (/ fy*ty fy)))) (apply #'make-camera-from-param :pwidth pwidth :pheight pheight :fx fx :fy fy :cx cx :cy cy :tx tx :ty ty :parent-coords frame-coords args)))

-   ;; #<sensor_msgs::camerainfo #X63fd3f0> #f(445.634 0.0 240.474 0.0 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0)
-   (setq wide_stereo-left (make-camera-from-ros-camera-info-aux 640 480 #f(445.634 0.0 240.474 0.0 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0) wide_stereo_optical_frame_lk :name :wide_stereo/left))
+   ;; #<sensor_msgs::camerainfo #X62f1578> #f(386.179 0.0 321.406 0.0 0.0 386.179 241.11 0.0 0.0 0.0 1.0 0.0)
+   (setq wide_stereo-left (make-camera-from-ros-camera-info-aux 640 480 #f(386.179 0.0 321.406 0.0 0.0 386.179 241.11 0.0 0.0 0.0 1.0 0.0) wide_stereo_optical_frame_lk :name :wide_stereo/left))

@YutoUchimi
Copy link
Contributor Author

YutoUchimi commented Dec 14, 2019 •
edited
Loading

make-camera-from-ros-camera-info-aux has no functional change in this PR.

But the camera matrix was definitely changed!
#f(445.634 0.0 240.474 0.0 0.0 445.634 193.274 0.0 0.0 0.0 1.0 0.0) -> #f(386.179 0.0 321.406 0.0 0.0 386.179 241.11 0.0 0.0 0.0 1.0 0.0)
Especially (cx, cy): (240.474, 193.274) -> (321.406, 241.11) is a dramatical improvement.
I came up with the idea that previous parameter was the result of too rough camera calibration.
This is why I added a rosbag file for PR1040, from which camera parameter above is generated.

@k-okada k-okada mentioned this pull request Dec 15, 2019
Merged
@YutoUchimi
Copy link
Contributor Author

Move to #426

@YutoUchimi YutoUchimi closed this Dec 20, 2019
@YutoUchimi YutoUchimi deleted the wide_stereo_camera_info branch December 20, 2019 04:41
@tkmtnt7000 tkmtnt7000 mentioned this pull request Dec 7, 2023
Open
2 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees
No one assigned
Labels
question
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@YutoUchimi @k-okada