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.

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

Extending MoveIt with hand-eye calibration #1070

Closed
RoboticsYY opened this issue Sep 26, 2018 · 47 comments
Closed

Extending MoveIt with hand-eye calibration #1070

RoboticsYY opened this issue Sep 26, 2018 · 47 comments

Comments

@RoboticsYY
Copy link
Contributor

Hi,

I'm making some effort to add a new screen to the MoveIt Setup Assistant tool for the hand-eye calibration. I would like to get some feedback and suggestions. The reason of why I'm doing this is hand-eye calibration is a key process for the vision-based robot motion planning. If someone want to make a hand-eye calibration, he can find several ROS packages (see list below).

However, not all of these packages has well maintained and detailed tutorials, there is no benchmark tool to evaluate their performance and they are not used straightforwardly with MoveIt. For using one of these packages, one still need much work to understand the working mechanism of the package, set up the specific configuration, prepare the specific calibration board and collect data in a specific way. The result of the calibration is required in running MoveIt for the vision-based motion planning apps.
Therefore, I think it is necessary to add a new screen to the Setup Assistant tool for the hand-eye calibration. It would be much convenient for user to set up robot model and camera transform at the same time. Currently, I'm ready to design the hand-eye calibration screen with options below:

  1. Since the hand-eye calibration is usually solved by AX=XB method, which requires the transforms from robot-base-frame to robot-end-effector-frame and from camera-frame to calibration-board-frame, the hand-eye calibration screen can lookup the TFs of these transforms, so that there is no need to include any specific robot driver or object detection in the screen code. The screen is only used to take snapshots of these transforms.
  2. An image display viewer can be configured in the screen to display the calibration board pose estimation result.
  3. The AX=XB calculation can be solved by a default algorithm in the screen code or other packages(such as visp_hand2eye_calibration). User can select a solver, then the screen will trigger the solver in that package.
  4. The result of the calibration could be a yaml file or a launch file storing the static transform that can be loaded at run-time.
  5. The robot joint configuration goals during the first calibration process can be saved and reused for a later automatic calibration.

Is there some on-going project for this topic in MoveIt? Or if you have some suggestions, please let me know. Thanks in advance.

@felixvd
Copy link
Contributor

felixvd commented Sep 26, 2018

There is nothing in MoveIt to my knowledge, but I remember the easy-handeye package that was presented at ROSCon last year (video). It was apparently not easy enough for 5 different researchers that I have proposed it to, so there is room for improvement.

@RoboticsYY
Copy link
Contributor Author

@felixvd Thanks for your feedback! I think it's necessary to add this new tool in MoveIt, because as I know, for some groups who are from AI perception and want to deliver solutions to the robot arm apps, they met some difficulty of getting the precise camera and robot transform when they try to do some experiments with ROS and MoveIt.
Very similar to the easy-handeye package, my idea is to create a user-friendly UI interface in MoveIt Setup Assistant for the hand-eye calibration. Along with the new screen, a detailed tutorial will be created to demonstrate the step by step calibration process. And the new screen can also include a combo box to trigger different AX=XB solvers in different packages. In my simulation experiments, seems that the visp solver is not the best on the translation error (I can show my benchmark experiments later). Besides, different from the easy-handeye, I don't think it is a good idea to make the automatic robot motion for the first time calibration, even though the robot moves within a small range of the robot end-effector frame. I think maybe it's better to save the joint goals for the first time and run the automatic calibration for the second time. And on the stage of running the MoveIt Setup Assistant for the first time, the settings of motion planning is not ready.

@felixvd
Copy link
Contributor

felixvd commented Sep 27, 2018

I can confirm that it is still a challenge for many non-computer-vision people to determine the position of a camera, so your efforts are welcome. Just be aware of this and this and keep it simple.

It is a good idea to start by making something you want to use yourself, instead of something you think others might want to use. The latter invites feature creep, and nothing beats simplicity and good defaults when it comes to usability for newcomers. Good luck.

@RoboticsYY
Copy link
Contributor Author

@felixvd Thanks for your advice! 😄

@davetcoleman
Copy link
Member

@RoboticsYY I like this idea to make calibration easier to setup in the MoveIt! Setup Assistant! Thoughts:

  • @MohmadAyman put a lot of effort into expanding the feature set of the MSA last summer, see blog post. Perhaps he can provide some assistance.
  • @MohmadAyman's recent efforts to make all the screens plugin-based. This might be important here so that we don't require huge vision / calibration dependencies to an already large library (moveit)
  • As @felixvd said, it might make sense to take one of those previous libraries and push it further rather than create a new one from scratch
  • There are no other efforts for calibration in MoveIt! as far as I know
  • Could you provide a mock-up screenshot of the proposed addition the the MSA user interface? That would be a good starting point for further discussion

@RoboticsYY
Copy link
Contributor Author

@davetcoleman Thanks for your feedback!

  • I have seen @MohmadAyman has made a lot of efforts to add new features to the MSA. I'm designing the handeye calibration screen following his convention of adding the perception screen.
  • I noticed @MohmadAyman 's effort to make all screen plugin-based. I think it is a good idea. Once his work is merged, I can also make the handeye calibration screen plugin-based. As you indicated, the handeye calibration may depends on some calibration algorithms and object detection functions. Currently, I provide some default options and put them in MSA's tools directory. I also plan to transfer them into plugin-based, so that other algorithms or libraries may be easily used in the same framework in future.
  • I used some similar ideas to design the UI interface from easy_handeye package as @felixvd refered to. I try to keep the screen as simple as possible. You can find them below. I think there must be better ideas to make it more user friendly.

Currently, I split the calibration process into four steps:

  1. First step, set the sensor mount type and calibration solver:

gloable setting

In my current design, I only provide one option for the calibration solver: handeye. I have done a benchmark test in the simulation to the available calibrators I can find, it seems that this calibrator has the good performance in the translation error and rotation error compared to the others. visp_hand2eye_calibration also has good performance, but the license is a problem. The solver is called through a ROS service.

  1. Second step, set the object detection parameters:

object detection setting

If a rgbd camera, stereo camera or a normal usb camera is used, six default opencv calibration patterns are provided. I plan to put the default patterns in the tutorial, so that there is no need to create them, just download and print. User has to set up the rgb image topic and the corresponding camera info topic from the pop up list. The topic type filter is implemented. Once a calibration pattern is selected, the precise size of the marker or black square or the marker separation distance is required to fill in. Since the calibration will look up TF information, other patterns than the default ones can be used by choosing other option.

  1. Third step, set the name of four coordinate frames required by the handeye calibration algorithms:

frame name setting

The calibration will look up the TFs of them.

  1. Fourth step, gather the samples of robot poses, object poses or camera poses:

calibrate operation

In this step, user can take samples of the required transforms by clicking take snapshot button, or clean the samples by clicking reset button. The real time state of the robot is displayed in the embedded image view. The image comes from the topic user selected in the second step. The taken samples is shown in the left tree view. The expandable sub-items include the translation and rotation component of the sampled transforms. Once enough samples are taken, user can click compute button to get the result and publish the result through static tf to check its validity. The result will also be saved into handeye_calibration.yaml file for later usage.

Right now, I'm also adding some functions to limit user's random inputs. Welcome any comments or suggestions!

@davetcoleman
Copy link
Member

This is pretty impressive progress! I'm curious what input @drchrislewis might have given he's worked on similar stuff recently as presented at ROSCon 2018.

The one big architectural question I have is... does this advanced functionality belong in the MSA or should it be a standone application? The fact that it generates a .yaml file indicates to me that yes, its in the correct location. However its unclear to me how often a user will want to run this calibration sequence. Does this get run once a day? Once a workspace setup?

@drchrislewis
Copy link

@davetcoleman @RoboticsYY This is quite interesting to me. I just recently refactored my calibration routines into something with a very similar look and feel. One of the calibrations available is the eye-hand or what I've called wrist-cal. I created two rviz plugins, a bit crude, but similar to the operation of what is presented here. My calibration nodes are now service call operated. This allows the robot teach pendant code to execute them without requiring MoveIt! Some companies still don't want to relinquish control of the robot motion to an automated path planner. One service call is for reset to eliminate any previously collected data, one for taking observations, one for running the optimization, one for saving the data, and one for computing the covariance. The same interface is available for intrinsic calibration, stereo calibration, and wrist calibration. Another node that is very useful is one that creates, stores and executes a set of poses where it stops pauses and collects the data at each pose. The nice thing about the setup assistant you made is that the launch files that I have to create to define the necessary transforms and targets are defined using a GUI rather than editing a yaml file. I have a couple of warnings for you though.
First, opencv does not necessarily return the same order of points from find circles grid. My ros_camera_observer has some pretty involved code that tries to resolve this. Anytime one gets the wrong ordering the answer will be way off. Second, it is quite easy and even advantageous for eye-hand calibration to rotate the camera by 180 degrees to see a target upside down. Once again, the correspondence is wrong and the answer will be wrong. However, I have a modified circle grid that works fine with opencv's find circles grid, that provides an unambiguous ordering of the points. Next, computing the covariance is important. Otherwise, one may not choose a diverse enough variety of poses to constrain the solution. Next, I implemented a mutable joint state publisher that allows one to enter in a calibrate-able transform that is dynamically adjustable through a sequence of joints. This works with moveit just fine, allowing the updated location of the camera to then be part of the collision model. Finally, another nice thing I've got is the ability to automatically generate a marker array that looks like the selected target. Its always nice when the marker shows up, and the 3D colorized point cloud overlays nicely.
The nodes I've alluded to are:
wrist_cal_srv.cpp (eye-hand, works with either target or camera on EOAT)
stereo_cal_srv.cpp (Finds pose between a stereo pair, again works with target or pair on EOAT)
target_display_node.cpp or ros_target_display.cpp ( facilities for creating marker array of desired target)
ical_srv.cpp (intrinsic cal with simplified interface uses tf to get initial poses)
joint_traj.cpp ( saves, and executes a series of poses calling the desired services at each pose)
tran_panel.cpp (rviz plugin for joint_traj.cpp that uses Moveit!)
calibration_gui.cpp (rviz plugin for calibration routines)

Our next goal is to provide robot kinematic calibration. I'm not sure when we'll get time to work on this though. I've got several projects that are in dire need of it.

Please keep me informed of any new work on this. Thanks

@RoboticsYY
Copy link
Contributor Author

@davetcoleman Thanks for your questions! In my humble opinion, maybe this is more suitable for MSA. I think, in usual cases, once the camera and its installation is determined, it is not frequently changed for a specific application, which is like the situation of the end-effector. If the location of the camera needs to be adjusted, there is no need to make the configuration again, the parameters will be loaded from the pre-configured yaml file. User only needs to spend a few minutes to calibrate again, or if the joint goals were recorded, this could be automatic for the second time. Then the result will be kept until the design of app changes or the camera is moved for some accident reason. And I also think that maybe it’s better to determine the location of camera before using the robot to make motion planning, since the environment scanning and octomap depends on it. In addition, I try to keep this handeye tool as simple as possible. Suppose there is a calibration pipeline as a standalone app in MoveIt!, maybe we also need a screen in MSA to provide some assistance for setting up the configurations of the calibration.

@drchrislewis Thanks for your feedback and introductions to the notable technique points in the calibration process! I agree with you and I believe that industrial_calibration can deliver accurate calibration result. I think I may design the handeye calibration screen of MSA from a perspective different to industrial_calibration. I try to deliver a tool that is simple and easy understandable, with which user can get a quick handson, even for someone without a professional background. Because I found sometimes it is difficult to explain the handeye calibration to some users. It may be even more difficult for them to properly configure the handeye calibration. In addition, I also try to make it as a framework without assuming about how to make object detection or what calibration algorithms to use. Besides the asymmetric circles grid pattern, I also provide the possibility that user can use other patterns, such as aruco or charuco board, or their own patterns depending on their favorite. For the calibration algorithm, I also would like to develop the possibility that other algorithms can work under the same framework. Different algorithms may be adaptive to different situations, for example industrial_calibratoin requires an initial guess of the camera transform, while Tsai’s method used in visp_hand2eye_calibration and handeye doesn’t. But it's not easy to get a conclusion of which one is better unless they go under the same environment. I haven’t found any platform for this consistent benchmark. This is very similar to the situation of path planners before.

As @felixvd indicated, there is some gap between the user and the calibration expert. Maybe we can find better ideas from this discussion.

@davetcoleman
Copy link
Member

I'm discussing this effort with @RoboticsYY and his team in China today and in summary:

  • They will create a pull request to /moveit and /moveit_tutorials soon with their initial design
  • Hopefully it can get into the moveit 1.0 release
  • For any calibration functionality that intends to automatically move the arm via executed poses, a new separate GUI should be created outside of the MSA as an Rviz plugin panel.

@athish-t
Copy link

@davetcoleman Hi, may I know if this feature is still planned to be released with MoveIt 1.0 in March?

@RoboticsYY
Copy link
Contributor Author

RoboticsYY commented Feb 27, 2019

@athish-t The code and tutorial is planned to be public in March and a PR will be submitted. Seems API is frozen before April, I think it cannot catch up with MoveIt! 1.0.

@davetcoleman
Copy link
Member

In the next week we should have a new master branch, and this will be happily accepted there!

@RoboticsYY
Copy link
Contributor Author

Architecture redesign

Recently the hand-eye calibration tool was redesigned. The previous design has some drawbacks:

  • The hand-eye tool was embedded in Setup Assistant, it had to enable some components replicating the functions of Rviz, such as the displays of image, marker, robot and point cloud.
  • The calibration board detection and calibration algorithm were coded as sub-components, which are not easy to be replaced by other substitute.

Now the new hand-eye tool is plugin based, it contains three plugins:

  • Rviz GUI plugin (Set parameters, make calibration control and call the other two plugins)
  • Default Target plugin (Create target image and detect target pose)
  • Default Solver plugin (Solve the camera-robot pose from the collected transform samples)

Interface introduction

The Rviz GUI plugin contains three tabs widgets, which contains separate functions.

First tab widget

target_create
video

On the first tab widget, user can load available target plugin, create target with necessary parameters. We are using parameters that are general to most kinds of calibration board, so that a new target detection plugin can reuse the parameters. The created target can be displayed and saved. When doing the target detection, the camera image topic and camera info topic can be selected from topic filtered combobox. And the real size and separation distance of markers in the target can be set by user.

Second tab widget

context_setting
video

On the second tab widget, user can select sensor mount type, "eye-in-hand" or "eye-to-hand". Four frame names necessary for a hand-eye calibration can be selected from available TF frames. We also allow user to set an initial guess pose of the camera. The four selected frames and the camera FOV can also be displayed. The FOV is generated from the received CameraInfo message.

Third tab widget

eye-in-hand
eye-in-hand video

eye-to-hand 2019-07-12 10-45
eye-to-hand video

On the third tab widget, user can select solver from the loaded solver plugins. This tab separates into manual calibration and auto calibration.

  • Manual calibration: user changes the pose of robot with robot teach pendent, and capture transform samples by clicking "Take Sample" button.
  • Auto calibration: once user finished a manual calibration, the joint states at which the samples are taken can be saved into a yaml file. When user making an auto calibration, these stored joint states can be loaded back, user can use "plan" and "execute" button to move the robot to each joint state and the samples and calculation will be finished automatically. In this way, the planned motion can be displayed in Rviz, so that user can check and make sure no accident happens. The result of camera-robot pose can be saved in to .launch file, which can publish the static transform tf.

Some summary

  • The new hand-eye tool is tried to be designed as a framework, that any calibration board detection and calibration algorithm can be used as plugins in the same framework.
  • The parameter settings on all tab widgets can be saved into a rviz configuration file and automatic calibration is enabled, so that configuration burden for a re-calibration can be relieved.
  • Four PRs including a tutorial will be submitted to MoveIt very soon.

@felixvd
Copy link
Contributor

felixvd commented Jul 12, 2019

Looks great. Looking forward to testing the PRs.

@drchrislewis
Copy link

Having an easy to use eye-hand calibration is definitely worth the effort. This seems to duplicate some of my work, available in industrial_calibration. I won't say I got it right. I do have a few comments.

  1. not all targets are suitable for eye-hand calibration. Open-CVs grid and circle grid finders are not well behaved. The order of returned points from the grid may change. One must use a target and a feature finder that ensure the order of the points is correct.
  2. The accuracy of finding points on a target depends on the type of fiducials used. Circles are by far the most accurate. How good are AR tags? My experience is that they are poor.
  3. The calibration is only as good as the images taken. All the AX=AB papers describe the minimum motions necessary to disambiguate the solution to make it unique. Intuition says one should use standard photogrametry where the sum of squared re-projection error is found with lots of diverse poses rather than a minimum set of observations ad AX=BX solutions.
  4. An identical calibration interface can be used for intrinsic calibration and stereo calibration, move, collect an image, move collect an image etc then solve. This is available with a moveit interface in industrial calibration (it may not be easy to use for some).
  5. How do the results get updated in the URDF? industrial calibration requires one create a sequence of 6 joints attaching the target and the camera to the robot/workcell, then merge the robot joint states with a set of mutable joint states so that the update is automatic and persistent. This makes my system hard to set up, but great for in-factory use.
  6. What mechanism is being used for estimating the accuracy of the calibration? Residual error is not a good measure. Covariance estimation is the best I know of, and it often underestimates the actual accuracy in my experience.
  7. I added another plugin that records joint poses, then replays them and calls the observation at each pose after waiting a few seconds to let the vibrations damp out. You could easily add something like this too. If someone replaces the camera or focus, this is nice. Plus once you have good poses defined, they can be easily reproduced. I find this very useful for lots of task beyond calibration.
  8. Another thing we're working on is to automatically generate a set of poses that use the robot kinematics, the field of view of the camera and the initial guess of the target/camera mount location to find an evenly distributed set of camera poses over the cone of feasible poses. This insures the maximum diversity of camera images with no operator expertise.
  9. Please let me know once you have your system ready to test.

@RoboticsYY
Copy link
Contributor Author

@drchrislewis Thanks for your comments!

My effort intends to develop an flexible and easy-to-use hand-eye calibration tool, and mainly focus on two directions:

  • Provide an GUI tool for user to make hand-eye calibration with parameters easily configured, inter process visible and understandable to users, even for someone without professional background.
  • Provide a flexible framework that different target detector or calibration solver can be used as plugins under the same framework. This has no mean to make any algorithm unique or standard.

1. not all targets are suitable for eye-hand calibration. Open-CVs grid and circle grid finders are not well behaved. The order of returned points from the grid may change. One must use a target and a feature finder that ensure the order of the points is correct.
2. The accuracy of finding points on a target depends on the type of fiducials used. Circles are by far the most accurate. How good are AR tags? My experience is that they are poor.

I agree with you that OpenCV target detection is not the best choice for hand-eye calibration. Here I only provide it as a default plugin, other target detection algorithm can also be used as a plugin here and collects parameters from the GUI. The reason I use OpenCV because it comes as a default installation with ROS, which is easy to compile against.

3. The calibration is only as good as the images taken. All the AX=AB papers describe the minimum motions necessary to disambiguate the solution to make it unique. Intuition says one should use standard photogrametry where the sum of squared re-projection error is found with lots of diverse poses rather than a minimum set of observations ad AX=BX solutions.

I provide an AX=XB library as a default plugin. But other algorithm can also works as a plugin here. Actually, the functions of setting the initial guess pose of a camera developed on GUI is ready for some iterative calibration algorithms other than the AX=XB algorithms. The GUI only works as data collection here. Maybe the "AX=XB Solver" label on the third tab widget make some confusion, I will change the name.

4. An identical calibration interface can be used for intrinsic calibration and stereo calibration, move, collect an image, move collect an image etc then solve. This is available with a moveit interface in industrial calibration (it may not be easy to use for some).

I agree that other calibrations are also important for industrial robots. Here my work only focus on the hand-eye calibration problem. But it is also possible to add the other calibration functions as new tab widgets on the panel, if this tool is proved necessary to become an fully functional calibration tool.

5. How do the results get updated in the URDF? industrial calibration requires one create sequence of 6 joints attaching the target and the camera to the robot/workcell, then merge the robot joint states with a set of mutable joint states so that the update is automatic and persistent. This makes my system hard to set up, but great for in-factory use.

In my design, the pose of a camera is displayed as a coordinate axis marker, and it is updated when TF system is updated. The 3D model of a camera is not considered in the calibration process. The calibration result is used as a .launch file.

6. What mechanism is being used for estimating the accuracy of the calibration? Residual error is not a good measure. Covariance estimation is the best I know of, and it often underestimates the actual accuracy in my experience.

In my current version, a covariance estimation is not added yet.

7. I added another plugin that records joint poses, then replays them and calls the observation at each pose after waiting a few seconds to let the vibrations damp out. You could easily add something like this too. If someone replaces the camera or focus, this is nice. Plus once you have good poses defined, they can be easily reproduced. I find this very useful for lots of task beyond calibration.

The tool works in a similar way. The joint poses are recorded into a .yaml file when user makes a manual calibration with a teach pendent. User can load these joint poses back, and plan and execute them, or skip any one of them.

8. Another thing we're working on is to automatically generate a set of poses that use the robot kinematics, the field of view of the camera and the initial guess of the target/camera mount location to find an evenly distributed set of camera poses over the cone of feasible poses. This insures the maximum diversity of camera images with no operator expertise.

Actually, I don't implement any method to automatically generate a set of poses with the kinematics computation. In my opinion, there are some pros and cons for doing this. It truly brings some convenience of automation, but also brings some risks and complexity that may not be suitable for some non-professional users.

@felixvd
Copy link
Contributor

felixvd commented Jul 15, 2019

If I may add as an outsider (I was not personally responsible for camera calibration in my past robot projects, but I noticed people were struggling and I could not point them to a package that was simple enough to use for them. I have also never heard about the industrial_calibration package):

  1. not all targets are suitable for eye-hand calibration. Open-CVs grid and circle grid finders are not well behaved. The order of returned points from the grid may change. One must use a target and a feature finder that ensure the order of the points is correct.

I remember that the orientation of checkerboard patterns can flip between images too, causing the markers to be misordered.

  1. How do the results get updated in the URDF? industrial calibration requires one create a sequence of 6 joints attaching the target and the camera to the robot/workcell, then merge the robot joint states with a set of mutable joint states so that the update is automatic and persistent. This makes my system hard to set up, but great for in-factory use.

Could you explain why this is set up in this way, and what you mean by "merge the robot joint states with a set of mutable joint states"? Why would this sort of update be automatic and persistent, but not e.g. a TF publisher?

  1. Another thing we're working on is to automatically generate a set of poses that use the robot kinematics, the field of view of the camera and the initial guess of the target/camera mount location to find an evenly distributed set of camera poses over the cone of feasible poses. This insures the maximum diversity of camera images with no operator expertise.

Great idea too, +1

@drchrislewis
Copy link

@felixvd We felt that one would like the extrinsic calibration parameters should act in a way similar to the intrinsic parameters of a camera info manager. The camera info manager reads a file, publishes the intrinsics, and can accept updates. Once the updates are written, the next time the camera node is launched the updated values are retained. TF does not have this in native form. It does have a floating transform, but MoveIt! did not support it at the time we wrote our code. Instead, we created a mutable_joint_states node that reads the joint values from a file, and publishes them as joint_states so that MoveIt can work with them and so that obstacle avoidance will avoid collisions with the camera even after one changes the extrinsic calibration. For this to work, one needs to combine the robot joint states with the ones we publish into a single topic called joint_states. The robot state publisher I believe requires it. Once its all set up, one can call a service an manually adjust the extrinsic calibration until it is approximately correct. MoveIt works fine to avoid camera collisions etc. Then one can run the calibration, once the new values are computed, and updated, the save service is called, and the original yaml file is updated. The next time the system is run, the latest calibration values are automatically installed. One does not have to manually edit a URDF. However, one has to attach the camera to the environment using a set of joints, x,y,z,roll,pitch,yaw using a naming convention so that we can automatically update them once we compute them. P.S. I developed the modified circle grid and wrapper code around open-cv's circle grid finder to find the circles in the correct order even for square targets. I've only seen errors when the target is viewed in a very oblique way, such that the largest observed circle is not the largest circle. I believe we fixed that bug too.

@felixvd
Copy link
Contributor

felixvd commented Jul 17, 2019

I see. That seems to be essentially equivalent to a TF publisher node that reads the calibration data from a file and publishes the camera_base frame. Am I understanding that right?

@drchrislewis
Copy link

@felixvd This is true except for collision detection. When the camera is on the robot you need it to be part of the kinematic chain and therefor in the urdf. Otherwise, its essentially a transform publisher that reads its value from a file and provides a service to update the value when necessary. TF supports a floating frame, but MoveIt's collision detection did not, at least the last time I investigated.

@felixvd
Copy link
Contributor

felixvd commented Jul 18, 2019

I see! I think that's still correct, as the CollisionRobot is created from the initial robot_description and not updated except for attached objects. I haven't looked into it, though. Maybe a 6-DOF joint would be more convenient for these cases.

We did not expect the camera to move enough to affect the collision checking, and preferred to use copious safe padding zones instead.

@felixvd
Copy link
Contributor

felixvd commented Jul 22, 2019

@RoboticsYY

This is really great and immensely useful. We tried out the commit on your repo that merged all of your open PRs combined today, and compiled comments and impressions.

We first had a compile time error from rviz_visual_tools, likely because the binaries for rviz_visual_tools have not been updated with your new updates. This problem can be solved by a clean rebuild after cloning rviz_visual_tools into the workspace.

We then added the plugin in Rviz through "Panels/Add new panel" (hard to find this without documentation) and tried giving it a go. I will list impressions from a user perspective:

Target tab

  • This is brilliant. I love it.
  • It would be useful to load marker settings from a file. It could be saved in the same yaml file as the joint states used for calibration, because the tools and the robot usually go together anyway.
  • It is confusing that you can select /handeye_calibration/image_detection as the image topic.

Context tab

  • Are the sensor mount types "Eye-to-hand"/"Eye-in-hand" clear to all audiences? What about adding alternative terms like "robot-mounted"/"scene-mounted"?
  • It is not clear which coordinate system the initial guess is in. There is a typo in "Inital guess". I would change to "Camera Pose Initial Guess (in robot base frame)"
  • I don't understand what the Object Frame is for, and I couldn't select any options for it when we had multiple planning groups.
  • Which sensor frame should be selected? I assume FOV will project the camera image into the scene, but we could not get this to work, no matter which sensor_frame was selected. We could see the images in rqt_image_view, however, and /handeye_calibration/image_detection republished the frames in black and white.

Calibrate tab
I got very confused on this tab. It was very unclear what is going on in terms of joint states and samples. Below is a play-by-play of my notes as we stumbled through this:

  • "Save joint states" gives an error saying "No joint states or joint state doesn't match joint names".
  • "Save Camera Pose" saves straight to a launch file. This is confusing. I would expect some other format.
  • It is not intuitive what "Take Sample" means. Record an image? Record joint states of the planning group? Record a camera pose? The field on the left just says "Pose_sample".
  • Clicking on "Take Sample" records a "Sample 1", but the names of its entries "bTe" and "cTo" are cryptic. What does this mean?
  • After recording a few samples, the "Save joint states" button opened a file save dialog, but I was surprised. The samples only contained Poses, not joint states. Are the joint states calculated from the Poses? That seems like the joint states would not be unique.
  • It seems that joint states are recorded when you click "Take Sample", but there is no way to inspect the joint states. This is really confusing.
  • To add to the confusion: When you click Plan/Execute, a new sample is added to the list, but not a new joint state
  • When you click "Plan" in Auto Calibration, almost no feedback is given to the user. The visualization of the plan might play for less than a second, but no confirmation of either planning success or the next targeted state remains.
  • "Reset Sample" deletes all the samples, instead of deleting one. Adding another button to delete single samples would make a lot of sense.
  • "Reset Sample" also deletes all the joint states, silently and without confirmation. That's very unintuitive, and also destructive. How do you know the user saved the joint states? Don't delete the calibration steps they recorded without a confirmation dialogue.
  • There is no way to select joint states or go forward/backward in them.

After we figured out the joint_states and samples, we wondered how you actually calibrate the camera. There was no "Calibrate" or "Calculate pose" button. We deduced from this post that the final button in the pipeline is "Save Camera Pose", but neither the button's position, name nor size imply that it is the final button to press, or that it is the result of the process. It can even be mistaken as "a pose to move the camera to".

Thoughts on getting the calibration result:

  • Our resulting pose was all zeroes, because we had no markers in our image. There was no user alert about this, however. How does the user know if the marker was seen in all the images?
  • How does the user know if the result of the calibration was good? There is no feedback about the number of images, the quality of the calibration, or success.

Other minor thoughts/issues about the tab:

  • No planning group could be selected when we had multiple ones in our moveit config. Not sure if we just had to regenerate it, or if there is a problem with multiple planning groups.
  • The bottom progress bar in the tab is moving constantly even when 0 joint states exist, which gives the impression that something is loading. It looks like a bug.
  • It seems that the Execute function immediately takes a sample after the move_group returns, but it should probably wait some time for the robot to stop moving.
  • The "Auto Calibration" isn't really automatic, it is still going through a list manually. It would be very convenient to let the robot move through all the joint poses automatically, with wait times, and collect the result at the end.

If I had to give one main feedback, it is that the "Calibrate" tab tries to do too many things at once. I think splitting it into "Set up collection", "Collect samples" and "Calculate camera pose" would make sense. Those tabs would have clearer functions:

  • Set up collection: Define the joint states at which samples should be recorded, with an interface to create, inspect and reorder joint poses, skip forward and backward through them, and finally save a set of joint states that could be cycled through safely and fully automatically.
  • Collect samples: In this tab, the user would load a set of joint states (or use the ones that were defined in the other tab), and then either manually (or fully automatically, with a sufficient wait time after each motion) record samples at each location, and allow the manual addition of more samples. A "Sample" here would be a camera image + extracted marker pose (I suppose). The user should be alerted if no marker is detected in a sample.
  • Calculate camera pose: This tab would calculate the camera pose from the sample, tell the user if it was successful, and give feedback on the quality of the calibration. It should allow the pose to be saved as a launch file or copied to the clipboard, ideally in different formats.

Let me repeat that I am a huge fan of this effort and I will look forward to seeing the polished end result. Thanks for putting in the work!

@RoboticsYY
Copy link
Contributor Author

@felixvd Thanks for your detailed feedback! They are very important to me to further improve the tool. Seems some design are not easy to understand directly. I will try my best to clarify some of your confusion. At the same time, I'm also preparing a tutorial to introduce how to use the tool.

We first had a compile time error from rviz_visual_tools, likely because the binaries for rviz_visual_tools have not been updated with your new updates. This problem can be solved by a clean rebuild after cloning rviz_visual_tools into the workspace.

Yes, recently I added a function to rviz_visual_tool to display the camera FOV as a pyramid mesh marker.

Target tab

  • This is brilliant. I love it.

It's happy to hear someone love it. 😊

  • It would be useful to load marker settings from a file. It could be saved in the same yaml file as the joint states used for calibration, because the tools and the robot usually go together anyway.

The tool use rviz config file (.rviz) to save the settings in all the tab widgets. In this way, if the stored .rviz config file is loaded back, the parameters of the marker will be set as the previous. The image topic and camera_info topic will also be set back if they are being published by the camera. They can also be used with other rviz settings at the same time, such as robot state, marker or point cloud display settings.

  • It is confusing that you can select /handeye_calibration/image_detection as the image topic.

Currently, when user click the Image Topic combobox, it picks up all the ROS topic with the sensor_msgs/Image format. The topic /handeye_calibration/image_detection is published by the tool to show the calibration board detection result. If the detection goes correctly, it should contain a coordinate axes (red/green/blue) painted over the first corner of the marker board. You are correct, this topic should be removed from the pop-up list.

In addition, for detecting the aruco marker, it's better to use OpenCV version != 3.2.

Context tab

  • Are the sensor mount types "Eye-to-hand"/"Eye-in-hand" clear to all audiences? What about adding alternative terms like "robot-mounted"/"scene-mounted"?

I struggled here. I agree that "Eye-to-hand"/"Eye-in-hand" are not obviously clear, but they are the phrases used most frequently in papers. I'm trying to put two photos in the tutorial to distinguish them. "robot-mounted"/"scene-mounted" are also good options.

  • It is not clear which coordinate system the initial guess is in. There is a typo in "Initial guess". I would change to "Camera Pose Initial Guess (in robot base frame)"

It depends on which sensor-mount type is selected. If "Eye-to-hand" is used, the pose will be w.r.t the user selected Robot Base Frame. If "Eye-in-hand" is used, the pose will be w.r.t the user selected End-Effector Frame.

  • I don't understand what the Object Frame is for, and I couldn't select any options for it when we had multiple planning groups.

Object Frame is for the TF frame published by the target detection plugin. If the default aruco marker plugin is used, user should select handeye_target. Basically, this combobox will lists TF frames not from robot links and not with "camera*" in its name. If no frames popped up, it means there is no target detected by the target detection plugin. Since all the transform recordings are looking up from the TF system, this also allows 3rd party target detection method or different names of target tf frame used.

In addition, Sensor Frame will list all the TF frames with "camera*" in its name. End-Effector Frame and Robot Base Frame will list all the TF frames from the robot body. The selected frames will be displayed as rviz axes markers.

  • Which sensor frame should be selected? I assume FOV will project the camera image into the scene, but we could not get this to work, no matter which sensor_frame was selected. We could see the images in rqt_image_view, however, and /handeye_calibration/image_detection republished the frames in black and white.

The sensor frame should be the root frame of the camera system, such as "camera_link" for most cameras. The FOV is the yellow marker in the example video, which shows the view direction and range of the camera. If the point cloud is enabled in rviz, it will be exactly contained in the boundary of the FOV. It is w.r.t. the frame indicated by the frame_id of the camera info msg. The FOV marker will show up only when the frames are correctly set up with the camera mount type. At init setup, the camera tf system and robot tf system are not connected. Therefore, for "eye-to-hand", if there is no valid sensor frame or robot base frame selected, the FOV will not show up. Similar for the "eye-in-hand", if there is no valid sensor frame or end-effector frame selected, the FOV will not show up either. And sometimes, if the FOV marker does not show up immediately after configuring the frames, user can trigger "FOV ON/OFF" button to display it.

Calibrate tab

  • I got very confused on this tab. It was very unclear what is going on in terms of joint states and samples. Below is a play-by-play of my notes as we stumbled through this:

Since some qt components are both needed by a manual and an auto calibration process, I struggled on this tab to make them reusable and integrated tightly together. But it looks not good idea to mix the manual and auto calibration together.

  • "Save joint states" gives an error saying "No joint states or joint state doesn't match joint names".

The joint state will be recorded each time when user takes transform sample at a robot pose. The number of recorded joint states are displayed as the maximum value at the right side of the bottom progressbar. The error message means there is no joint states recorded or the number of joints in the joint state doesn't match the number of joint names from the selected planning_group.

  • "Save Camera Pose" saves straight to a launch file. This is confusing. I would expect some other format.

It saves the camera pose into a static tf publisher in a launch file. If an application needs to use the result, it needs to launch this launch file or include it in its launch file. All the rest things will be left to TF system.

  • It is not intuitive what "Take Sample" means. Record an image? Record joint states of the planning group? Record a camera pose? The field on the left just says "Pose_sample".
  • Clicking on "Take Sample" records a "Sample 1", but the names of its entries "bTe" and "cTo" are cryptic. What does this mean?

It actually takes samples of two kind of transforms. i.e. detected object (O) w.r.t. camera (C), and end-effector (E) w.r.t. robot base (B). With transform names specified in the "Context" tab, this will look up from the tf system to get the two kinds of transforms. Maybe I should use more tooltips or put more info beside the buttons to provide some help information.

  • After recording a few samples, the "Save joint states" button opened a file save dialog, but I was surprised. The samples only contained Poses, not joint states. Are the joint states calculated from the Poses? That seems like the joint states would not be unique.
  • It seems that joint states are recorded when you click "Take Sample", but there is no way to inspect the joint states. This is really confusing.

A joint state is recorded at the moment when user takes a sample. Currently, the joint states are not displayed in the entries of the samples, looks this make them completely happen in the background. I think I should make this more transparent.

  • To add to the confusion: When you click Plan/Execute, a new sample is added to the list, but not a new joint state

Actually every time when a new sample is added, a new joint state is recorded. Plan/Execute uses the same method as the "Take Sample" button to add sample to the tree view area, it loses the joint state. This should be changed.

  • When you click "Plan" in Auto Calibration, almost no feedback is given to the user. The visualization of the plan might play for less than a second, but no confirmation of either planning success or the next targeted state remains.

I was originally planning to put a status bar to indicate the planning result. But finally, I don't have enough space on the "Calibrate" tab to do that. As you suggested, if I split the functions in "calibrate" tab, maybe I can put some qt component for this.

  • "Reset Sample" deletes all the samples, instead of deleting one. Adding another button to delete single samples would make a lot of sense.
  • "Reset Sample" also deletes all the joint states, silently and without confirmation. That's very unintuitive, and also destructive. How do you know the user saved the joint states? Don't delete the calibration steps they recorded without a confirmation dialogue.

Yes, "Reset Sample" deletes all the pose samples and joint states. You are correct, this shouldn't happen silently without confirmation. I think I didn't think too much of it. And I think it's great idea to use a button to delete only one sample.

  • There is no way to select joint states or go forward/backward in them.

Currently, the "Plan/Execute" can only go through joint states forward, i.e. making the robot goes from the first joint state to the last joint state. This is designed to repeat the joint state sequence of a previous calibration process.

  • After we figured out the joint_states and samples, we wondered how you actually calibrate the camera. There was no "Calibrate" or "Calculate pose" button. We deduced from this post that the final button in the pipeline is "Save Camera Pose", but neither the button's position, name nor size imply that it is the final button to press, or that it is the result of the process. It can even be mistaken as "a pose to move the camera to".

When the number of samples >= 5, the calibrate computation will run every time when a new sample is taken. It expects user to see the change of camera pose with the number of samples increasing. "Save Camera Pose" serves as saving the camera-robot pose, the initial value is all zeros.

  • Our resulting pose was all zeroes, because we had no markers in our image. There was no user alert about this, however. How does the user know if the marker was seen in all the images?

The target tab serves as everything about marker board detection. The target detection plugin publishes the detection result to the "/handeye_calibration/target_detection" topic. The image of this topic contains the original camera image and a coordinate axes painted on the first corner of the marker board. At the same time, the tf of the marker board is continuously updated. The calibration only takes samples of tf transforms, all the pose detection is done in the "target" tab. If the tf frame doesn't exist, the tf look-up will throw exception. User can check if there is a coordinate axes on the image stream of "/handeye_calibration/target_detection" topic. And if he selects the tf frame of the marker board in "Context" tab, he can also check it moving in the FOV of camera.

  • How does the user know if the result of the calibration was good? There is no feedback about the number of images, the quality of the calibration, or success.

Currently, user can check the calibration result by checking if the FOV of the camera matches the real scenario, and if the point cloud of robot overlays on the right part of the 3D model of robot. This only provides an approximate check. I haven't implemented covariance estimation, which may provide more information than the intuitive evaluation.

  • No planning group could be selected when we had multiple ones in our moveit config. Not sure if we just had to regenerate it, or if there is a problem with multiple planning groups.

I think there could be a bug here. The list of planning groups are freshed at the init stage of the rviz plugin. So if it is started before a move_group, the list will be empty. I have tested it previously with the moveit config of universal_robot and panda_moveit_config, it can list the arm and gripper groups. Actually, it reads the group names by a planning scene monitor, which listening to the scene topic "move_group/monitored_planning_scene".

  • The bottom progress bar in the tab is moving constantly even when 0 joint states exist, which gives the impression that something is loading. It looks like a bug.

I used the maximum value of the progressbar to indicate the total number of joint states, and the current value to indicate the number of completed joint states. When both values are set to zero, i.e. no joint states recorded and no joint states completed, it is moving constantly.

  • It seems that the Execute function immediately takes a sample after the move_group returns, but it should probably wait some time for the robot to stop moving.

Yes, I haven't put a wait time here. I should do that.

  • The "Auto Calibration" isn't really automatic, it is still going through a list manually. It would be very convenient to let the robot move through all the joint poses automatically, with wait times, and collect the result at the end.

I also struggled here. I have considered to do this fully automatic calibration. Two things changed my mind. I found it could be dangerous for some non-professional users to run the full automatic process if the robot or its ROS driver is not configured properly. And sometimes when a collision happens, it's not easy to stop the robot from the GUI widget if the robot has started moving. If an emergency button is used, that could break the pipeline, and everything may need restart over, except a robust robot ROS driver is used. Another thing is, the plan() of move_group seems need to run in a new thread if it is not run in the main thread, otherwise it will permanently stuck in its callback and cannot return. But if it runs in a new thread, the other operations will also be run in this thread, such as adding samples to the tree view, updating the progressbar and calculating the camera pose. To my experience, this very often brings in some strange segment fault in the qt. Now, the plan() and execute() thread emits a qt signal at its end, and a qt slot is used to deal with the rest operations, that makes it much more robust.

If I had to give one main feedback, it is that the "Calibrate" tab tries to do too many things at once. I think splitting it into "Set up collection", "Collect samples" and "Calculate camera pose" would make sense. Those tabs would have clearer functions:

  • Set up collection: Define the joint states at which samples should be recorded, with an interface to create, inspect and reorder joint poses, skip forward and backward through them, and finally save a set of joint states that could be cycled through safely and fully automatically.
  • Collect samples: In this tab, the user would load a set of joint states (or use the ones that were defined in the other tab), and then either manually (or fully automatically, with a sufficient wait time after each motion) record samples at each location, and allow the manual addition of more samples. A "Sample" here would be a camera image + extracted marker pose (I suppose). The user should be alerted if no marker is detected in a sample.
  • Calculate camera pose: This tab would calculate the camera pose from the sample, tell the user if it was successful, and give feedback on the quality of the calibration. It should allow the pose to be saved as a launch file or copied to the clipboard, ideally in different formats.

Let me repeat that I am a huge fan of this effort and I will look forward to seeing the polished end result. Thanks for putting in the work!

From your feedback, I see that "Calibrate" tag needs to be polished and improved a lot. I agree that it is necessary to split the mixed functions. I will take some time to make the change.

@felixvd
Copy link
Contributor

felixvd commented Jul 24, 2019

Thanks for the long reply! I can't respond in detail right now, but for now here are some short comments from our experience trying to get it to run today:

The tool use rviz config file (.rviz) to save the settings in all the tab widgets. In this way, if the stored .rviz config file is loaded back, the parameters of the marker will be set as the previous. The image topic and camera_info topic will also be set back if they are being published by the camera. They can also be used with other rviz settings at the same time, such as robot state, marker or point cloud display settings.

Fair enough. I would prefer to save it as an external file, so that one can save a whole calibration procedure for a robot. That seems more logical and modular than saving a whole rviz config.

Another problem: when we had the plugin in the rviz config file, the list of Planning Groups would be empty. To get the planning groups to display, we had to restart the plugin, but that resets the parameter values.

The FOV is the yellow marker in the example video, which shows the view direction and range of the camera. If the point cloud is enabled in rviz, it will be exactly contained in the boundary of the FOV.

What do you mean by "the point cloud"? The topic of the camera, or the topic that is republished by your plugin?

Does the plugin publish the initial guess as the position of the camera? We sometimes see our frames in weird places, although they were defined at roughly the correct position in the URDF.

When the number of samples >= 5, the calibrate computation will run every time when a new sample is taken. It expects user to see the change of camera pose with the number of samples increasing. "Save Camera Pose" serves as saving the camera-robot pose, the initial value is all zeros.

It looks like there is something going wrong in our setup. If you are up for it, we could troubleshoot via Skype. Our time zones are close enough. My handle is the same as here.

Other small comments from today:

  • Deleting the HandEyeCalibration panel via Panels/Delete Panel seems to crash rviz systematically
  • Sometimes the marker recognition seems to not work and the plugin has to be restarted. It would be good to know how a successful recognition result is supposed to look. See these pictures for some examples of what we get: 1 2

@RoboticsYY
Copy link
Contributor Author

Another problem: when we had the plugin in the rviz config file, the list of Planning Groups would be empty. To get the planning groups to display, we had to restart the plugin, but that resets the parameter values.

I think it's a bug in my code that the list of planning groups cannot be refreshed when user clicks on it. I will try to fix it. A work-around now can be resuming rviz from the .rviz config file and avoid restarting only the plugin.

What do you mean by "the point cloud"? The topic of the camera, or the topic that is republished by your plugin?

"the point cloud" I mean the topic of the camera.

Does the plugin publish the initial guess as the position of the camera? We sometimes see our frames in weird places, although they were defined at roughly the correct position in the URDF.

Yes, the plugin publishes the initial guess pose of the camera as a static tf. If the camera pose is defined in a URDF, this tf predefined in URDF will be broken and replaced by the new tf from the plugin.

It looks like there is something going wrong in our setup. If you are up for it, we could troubleshoot via Skype. Our time zones are close enough. My handle is the same as here.

Yes, I would very much like to help debug the problem. Can you please send me a skype link?

  • Deleting the HandEyeCalibration panel via Panels/Delete Panel seems to crash rviz systematically

I tested on my side, it happens as you said. I think I must forgot to reset a shared_ptr in the destruction of the "calibrate" tab.

  • Sometimes the marker recognition seems to not work and the plugin has to be restarted. It would be good to know how a successful recognition result is supposed to look. See these pictures for some examples of what we get: 1
    22

I used the same marker on my side, it works well. I suspect there are two possible reasons for this:

  1. If the OpenCV version is "3.2" on your machine, this could happen. There are some odd changes in this version.
  2. If the camera info msgs passed in is wrong, this could happen. I have made a debug print in the plugin, if the debug level print has been enabled for the moveit_ros_visualization package, the received camera intrinsic parameters will be print when the camera info topic is changed.

@felixvd
Copy link
Contributor

felixvd commented Jul 26, 2019

You can find me on Skype under this link, apparently.

edit:
Some results of our troubleshooting in the call:

  • We are using the same camera (Realsense D435) with the official package, using ROS Melodic.
  • When the marker is set up correctly, the coordinate system drawn in the camera view republished by the plugin should be drawn on the corner of the marker array (not in the middle), as shown here:

Screen Shot 2019-07-26 at 17 43 03

  • Our marker frame seems to be published outside of the FOV frame, which is confusing

Screen Shot 2019-07-26 at 16 55 12

  • It seems to be a related bug that the marker in the scene seems to have a different rotation than in the image view, as if it was flipped by 180 degrees.

Screen Shot 2019-07-26 at 17 45 35

Screen Shot 2019-07-26 at 17 31 45

  • The FOV is published via a MarkerArray, which has to be added to the Rviz displays before it is displayed.
  • We found a bug: Selecting the wrong frame scene_camera_color_optical_frame and then scene_camera_camera_link resulted in a permanent change. I think what happens is this: The plugin publishes the initial guess for scene_camera_color_optical_frame, which overwrites the original URDF's definition of the frame relative to the camera_link frame, thus breaking the scene. I would suggest filtering for camera_link instead of camera in this dropdown menu. If I remember correctly, camera_link is the standard for cameras in ROS, but I don't know if there is a source for that.

edit2:

  • We were discussing how strange it is that the handeye_target marker is not in the FOV. Even stranger: After restarting Rviz, the marker is in the FOV, but the marker's orientation does not look right. I don't know what that means – for now we are just logging the phenomenon.

Screenshot 1, Screenshot 2

@felixvd
Copy link
Contributor

felixvd commented Jul 29, 2019

We also found that setting the camera and camera info topics when the marker settings are not yet configured seems to make the marker tracking and detection fail systematically. It might not be updated with new marker settings once it is initialized. This could be the cause behind other bugs, too.

@RoboticsYY
Copy link
Contributor Author

RoboticsYY commented Jul 30, 2019

@felixvd Thanks for testing the tool and summarizing the problems. I will try to fix the exposed bugs in this week.

I found an answer to one of your questions:

  • We were discussing how strange it is that the handeye_target marker is not in the FOV. Even stranger: After restarting Rviz, the marker is in the FOV, but the marker's orientation does not look right. I don't know what that means – for now we are just logging the phenomenon.

The function drawing the axes in 2D image is actually the drawFrameAxis of OpenCV. I just found that it draws the XYZ axes in blue, green, red color (link), not in the traditional red, green, blue color. This is different from their function description (link), otherwise it doesn't obey the right-hand rule of coordinate system. I may create a new draw-axes function in the plugin.

@felixvd
Copy link
Contributor

felixvd commented Jul 30, 2019

@RoboticsYY As requested in our Skype call, I prepared some sketches of the tabs I would suggest for the plugin. These should replace and add to the current "Calibrate" tab, which is too broad in scope in my opinion:

  1. Motion tab, to create series of joint states to repeat calibration motions
    IMG_20190730_105941033
    Note the arrows to rearrange joint poses and the "Delete" button to remove some.
    It would be helpful if the robot pose was displayed when a joint state is selected.

  2. Collect tab, to collect samples
    IMG_20190730_105946710
    In this tab, the user records samples using either manual operation and/or the joint poses recorded in the Motion tab. Single selected samples should be able to be discarded via the "Remove sample" button.
    It would be extremely useful for troubleshooting, finding degenerate samples and understanding results if there was a way to save and display the image to a sample.
    It would also be nice for the user to be able to move forward/backward through joint poses, but not very important.

  3. Calculate tab, to calculate the camera pose from the collected samples
    IMG_20190730_105951751
    This tab also gives the user feedback to evaluate the calibration result and lets them save it. Displaying it directly in the tab for quick copy and paste would also be very convenient. Often enough you want to update the parameters of a URDF and not create a new file. Would be nice if the pose could be copy-pasted in both "launch" and "URDF" format.

  4. Test tab, to test the calibrated camera pose
    IMG_20190730_130744725
    We noticed that this feature will certainly be desired by almost all users. We made a simple example script ourselves, which moves the robot to the marker, but a tab would have been nicer.
    As the marker's orientation is complicated to use and likely to put the robot into a messy state, the default setting should be to change only the translation of the robot, and to keep the orientation as is. The "Target offset" should have a default of x = -0.02 in the marker coordinate system, so the gripper does not move into the calibration board but moves in front of it.

Would be glad to hear what others think about this. Once again, it's a great effort and we are looking forward to polishing and using it. We managed to calibrate one camera relatively comfortably (except for the bugs).

PS: Hope your ROSCon talk got in, mine didn't :I

@felixvd
Copy link
Contributor

felixvd commented Jul 30, 2019

Also this bag file (link valid for 3 more days I believe) shows the issue we had with the transform being on the wrong side of the camera.

@RoboticsYY
Copy link
Contributor Author

@felixvd Thanks so much for sketching out the tabs. It helped me clear straight on how to split the "calibrate" tab. Great idea for the "test" tab! I never thought about it.

I'm still thinking about the recording of joint states and transform samples. Each transform sample usually corresponds to a joint state. Sometimes, when user takes a sample, he may also want to record the joint state. And sometimes, when user wants to remove a sample, this may mean that the corresponding joint state is not good either. Putting the two operations together seems to create confusion, but splitting them apart seems to create some burden of switching between the two tabs. And if not listing the sample and its corresponding joint state together, it's very easy to lose their correspondence, if multiple add/remove operations are done.

PS: We are fortunate to hit one short presentation at ROSCon this year.

@felixvd
Copy link
Contributor

felixvd commented Aug 8, 2019

I think adding a "Record joint state" button to the Collect tab would be perfectly reasonable and solve those concerns.

To my mind, the motion to record the samples and the samples that are actually recorded for calibration are separate. I imagine the motion to be set up once and reused often, and the samples to be temporary and discarded after a calibration (which shouldn't take more than 15-30 minutes).

I think recording the joint pose and camera image alongside each sample for later inspection would make sense, however.

@RoboticsYY RoboticsYY changed the title Extending MoveIt Setup Assistant with hand-eye calibration screen Extending MoveIt with hand-eye calibration Aug 29, 2019
@felixvd
Copy link
Contributor

felixvd commented Jun 2, 2020

If this is still happening I will be cheering it on.

@davetcoleman
Copy link
Member

davetcoleman commented Jun 2, 2020

This PR has been open for two years, so sorry @RoboticsYY!

There has been concerns by @rhaschke and others that these features are a bit too much of scope creep for the core purpose of the MoveIt project (motion planning / manipulation). In addition, we've had lots of infrastructure limitations because of the size of this repo. At this point, I think we should agree on a new repo name for this. We then merge all these PRs there with very little further review, as they've been reviewed and tested a lot already (@jliukkonen @JStech @felixvd ) and this is a new "experimental" feature anyway. That we're excited to use more of!

Proposed repo names:

  • moveit_hand_eye_cal
  • moveit_calibration

If I get several thumbs up I'll create the repo now.

@RoboticsYY
Copy link
Contributor Author

@davetcoleman Thanks for your continuing support on this topic! I agree that this could be an extra feature that may facilitate the usage of MoveIt in some projects but beyond the core function of Moveit. Looking forward to a new repo for this. I will help addressing the PRs and CI.

@felixvd I will continue to improve this tool as we discussed, but I may not be able to make it happen quickly. At the same time, if this tool is interesting to others, I welcome the input of others.

@v4hn
Copy link
Contributor

v4hn commented Jun 2, 2020

If I get several thumbs up I'll create the repo now.

I guess @RoboticsYY could also create a new repository on their account and transfer ownership to ros-planning once the repo looks good?

@davetcoleman
Copy link
Member

To keep things moving, I can make the repo and add @RoboticsYY as admin. Just need someone to weight in on the name. I like the first proposed option.

@davetcoleman
Copy link
Member

@felixvd and @v4hn preferred moveit_calibration (on Discord) so I've created that:
https://github.com/ros-planning/moveit_calibration

I'll work on getting it further setup....

@ros-gy
Copy link

ros-gy commented Jun 16, 2020

@RoboticsYY I'm interested in testing this, can you provide some setup/install directions, how to get the rviz plugin running? I combed through the discussions I can see, and haven't found it yet. Thank you.

@RoboticsYY
Copy link
Contributor Author

@ros-gy To test this tool, you can merge the moveit_calibration PR#4 into the master branch of https://github.com/ros-planning/moveit_calibration. Then use the moveit_calibration PR#5 to install OpenCV 3.4 debian packages. After that, compile the moveit_calibration with moveit on ROS Melodic. The plugin can be loaded from add panel of rviz. A tutorial for the calibration tool is still in progress in the moveit_tutorial PR#497.

@ros-gy
Copy link

ros-gy commented Jul 16, 2020

@RoboticsYY I've attempted using the calibration tool a few times, and could use some explanation on four things, if you don't mind:

  1. Frame selection. Can you explain what each of the four frames should be, and how they are used (sensor, object, end-effector, robot base)
  2. On the calibrate tab, I try to "save joint states" but I get an error. It seems I need a YAML file which describes these. Where should the YAML be located, and how should it be formatted?
  3. What is the process for calibration, how to use the third tab?
  4. What is the output? How is calibration data stored, or displayed? Does it change the URDF, or output a YAML file?

-- EDIT --
I've resolved the above questions thanks to @JStech, and I'll just list his tutorial in case others have similar questions.
https://github.com/JStech/moveit_tutorials/blob/new-calibration-tutorial/doc/hand_eye_calibration/hand_eye_calibration_tutorial.rst

@JStech
Copy link
Contributor

JStech commented May 7, 2021

I think this issue can be closed. Any further comments or questions should be posted on the MoveIt Calibration repository.

@JStech JStech closed this as completed May 7, 2021
@s-trinh
Copy link

s-trinh commented May 18, 2021

Hi @RoboticsYY

I have found this video:

image

I am involved in the ViSP repository and any information about the benchmark methods would be helpful:

  • for instance, is the benchmark code available somewhere?
  • otherwise, against which version of ViSP it was tested?

Since ViSP hand-eye calibration method uses a final non-linear refinement stage, I am a little bit surprised to see it performing worse than classical hand-eye calibration methods (Daniilidis method which solves simultaneously the rotation and translation part, and Park, Tsai that solve rotation and translation separately).

@ZZWang21
Copy link

@felixvd Thanks for your detailed feedback! They are very important to me to further improve the tool. Seems some design are not easy to understand directly. I will try my best to clarify some of your confusion. At the same time, I'm also preparing a tutorial to introduce how to use the tool.

We first had a compile time error from rviz_visual_tools, likely because the binaries for rviz_visual_tools have not been updated with your new updates. This problem can be solved by a clean rebuild after cloning rviz_visual_tools into the workspace.

Yes, recently I added a function to rviz_visual_tool to display the camera FOV as a pyramid mesh marker.

Target tab

  • This is brilliant. I love it.

It's happy to hear someone love it. 😊

  • It would be useful to load marker settings from a file. It could be saved in the same yaml file as the joint states used for calibration, because the tools and the robot usually go together anyway.

The tool use rviz config file (.rviz) to save the settings in all the tab widgets. In this way, if the stored .rviz config file is loaded back, the parameters of the marker will be set as the previous. The image topic and camera_info topic will also be set back if they are being published by the camera. They can also be used with other rviz settings at the same time, such as robot state, marker or point cloud display settings.

  • It is confusing that you can select /handeye_calibration/image_detection as the image topic.

Currently, when user click the Image Topic combobox, it picks up all the ROS topic with the sensor_msgs/Image format. The topic /handeye_calibration/image_detection is published by the tool to show the calibration board detection result. If the detection goes correctly, it should contain a coordinate axes (red/green/blue) painted over the first corner of the marker board. You are correct, this topic should be removed from the pop-up list.

In addition, for detecting the aruco marker, it's better to use OpenCV version != 3.2.

Context tab

  • Are the sensor mount types "Eye-to-hand"/"Eye-in-hand" clear to all audiences? What about adding alternative terms like "robot-mounted"/"scene-mounted"?

I struggled here. I agree that "Eye-to-hand"/"Eye-in-hand" are not obviously clear, but they are the phrases used most frequently in papers. I'm trying to put two photos in the tutorial to distinguish them. "robot-mounted"/"scene-mounted" are also good options.

  • It is not clear which coordinate system the initial guess is in. There is a typo in "Initial guess". I would change to "Camera Pose Initial Guess (in robot base frame)"

It depends on which sensor-mount type is selected. If "Eye-to-hand" is used, the pose will be w.r.t the user selected Robot Base Frame. If "Eye-in-hand" is used, the pose will be w.r.t the user selected End-Effector Frame.

  • I don't understand what the Object Frame is for, and I couldn't select any options for it when we had multiple planning groups.

Object Frame is for the TF frame published by the target detection plugin. If the default aruco marker plugin is used, user should select handeye_target. Basically, this combobox will lists TF frames not from robot links and not with "camera*" in its name. If no frames popped up, it means there is no target detected by the target detection plugin. Since all the transform recordings are looking up from the TF system, this also allows 3rd party target detection method or different names of target tf frame used.

In addition, Sensor Frame will list all the TF frames with "camera*" in its name. End-Effector Frame and Robot Base Frame will list all the TF frames from the robot body. The selected frames will be displayed as rviz axes markers.

  • Which sensor frame should be selected? I assume FOV will project the camera image into the scene, but we could not get this to work, no matter which sensor_frame was selected. We could see the images in rqt_image_view, however, and /handeye_calibration/image_detection republished the frames in black and white.

The sensor frame should be the root frame of the camera system, such as "camera_link" for most cameras. The FOV is the yellow marker in the example video, which shows the view direction and range of the camera. If the point cloud is enabled in rviz, it will be exactly contained in the boundary of the FOV. It is w.r.t. the frame indicated by the frame_id of the camera info msg. The FOV marker will show up only when the frames are correctly set up with the camera mount type. At init setup, the camera tf system and robot tf system are not connected. Therefore, for "eye-to-hand", if there is no valid sensor frame or robot base frame selected, the FOV will not show up. Similar for the "eye-in-hand", if there is no valid sensor frame or end-effector frame selected, the FOV will not show up either. And sometimes, if the FOV marker does not show up immediately after configuring the frames, user can trigger "FOV ON/OFF" button to display it.

Calibrate tab

  • I got very confused on this tab. It was very unclear what is going on in terms of joint states and samples. Below is a play-by-play of my notes as we stumbled through this:

Since some qt components are both needed by a manual and an auto calibration process, I struggled on this tab to make them reusable and integrated tightly together. But it looks not good idea to mix the manual and auto calibration together.

  • "Save joint states" gives an error saying "No joint states or joint state doesn't match joint names".

The joint state will be recorded each time when user takes transform sample at a robot pose. The number of recorded joint states are displayed as the maximum value at the right side of the bottom progressbar. The error message means there is no joint states recorded or the number of joints in the joint state doesn't match the number of joint names from the selected planning_group.

  • "Save Camera Pose" saves straight to a launch file. This is confusing. I would expect some other format.

It saves the camera pose into a static tf publisher in a launch file. If an application needs to use the result, it needs to launch this launch file or include it in its launch file. All the rest things will be left to TF system.

  • It is not intuitive what "Take Sample" means. Record an image? Record joint states of the planning group? Record a camera pose? The field on the left just says "Pose_sample".
  • Clicking on "Take Sample" records a "Sample 1", but the names of its entries "bTe" and "cTo" are cryptic. What does this mean?

It actually takes samples of two kind of transforms. i.e. detected object (O) w.r.t. camera (C), and end-effector (E) w.r.t. robot base (B). With transform names specified in the "Context" tab, this will look up from the tf system to get the two kinds of transforms. Maybe I should use more tooltips or put more info beside the buttons to provide some help information.

  • After recording a few samples, the "Save joint states" button opened a file save dialog, but I was surprised. The samples only contained Poses, not joint states. Are the joint states calculated from the Poses? That seems like the joint states would not be unique.
  • It seems that joint states are recorded when you click "Take Sample", but there is no way to inspect the joint states. This is really confusing.

A joint state is recorded at the moment when user takes a sample. Currently, the joint states are not displayed in the entries of the samples, looks this make them completely happen in the background. I think I should make this more transparent.

  • To add to the confusion: When you click Plan/Execute, a new sample is added to the list, but not a new joint state

Actually every time when a new sample is added, a new joint state is recorded. Plan/Execute uses the same method as the "Take Sample" button to add sample to the tree view area, it loses the joint state. This should be changed.

  • When you click "Plan" in Auto Calibration, almost no feedback is given to the user. The visualization of the plan might play for less than a second, but no confirmation of either planning success or the next targeted state remains.

I was originally planning to put a status bar to indicate the planning result. But finally, I don't have enough space on the "Calibrate" tab to do that. As you suggested, if I split the functions in "calibrate" tab, maybe I can put some qt component for this.

  • "Reset Sample" deletes all the samples, instead of deleting one. Adding another button to delete single samples would make a lot of sense.
  • "Reset Sample" also deletes all the joint states, silently and without confirmation. That's very unintuitive, and also destructive. How do you know the user saved the joint states? Don't delete the calibration steps they recorded without a confirmation dialogue.

Yes, "Reset Sample" deletes all the pose samples and joint states. You are correct, this shouldn't happen silently without confirmation. I think I didn't think too much of it. And I think it's great idea to use a button to delete only one sample.

  • There is no way to select joint states or go forward/backward in them.

Currently, the "Plan/Execute" can only go through joint states forward, i.e. making the robot goes from the first joint state to the last joint state. This is designed to repeat the joint state sequence of a previous calibration process.

  • After we figured out the joint_states and samples, we wondered how you actually calibrate the camera. There was no "Calibrate" or "Calculate pose" button. We deduced from this post that the final button in the pipeline is "Save Camera Pose", but neither the button's position, name nor size imply that it is the final button to press, or that it is the result of the process. It can even be mistaken as "a pose to move the camera to".

When the number of samples >= 5, the calibrate computation will run every time when a new sample is taken. It expects user to see the change of camera pose with the number of samples increasing. "Save Camera Pose" serves as saving the camera-robot pose, the initial value is all zeros.

  • Our resulting pose was all zeroes, because we had no markers in our image. There was no user alert about this, however. How does the user know if the marker was seen in all the images?

The target tab serves as everything about marker board detection. The target detection plugin publishes the detection result to the "/handeye_calibration/target_detection" topic. The image of this topic contains the original camera image and a coordinate axes painted on the first corner of the marker board. At the same time, the tf of the marker board is continuously updated. The calibration only takes samples of tf transforms, all the pose detection is done in the "target" tab. If the tf frame doesn't exist, the tf look-up will throw exception. User can check if there is a coordinate axes on the image stream of "/handeye_calibration/target_detection" topic. And if he selects the tf frame of the marker board in "Context" tab, he can also check it moving in the FOV of camera.

  • How does the user know if the result of the calibration was good? There is no feedback about the number of images, the quality of the calibration, or success.

Currently, user can check the calibration result by checking if the FOV of the camera matches the real scenario, and if the point cloud of robot overlays on the right part of the 3D model of robot. This only provides an approximate check. I haven't implemented covariance estimation, which may provide more information than the intuitive evaluation.

  • No planning group could be selected when we had multiple ones in our moveit config. Not sure if we just had to regenerate it, or if there is a problem with multiple planning groups.

I think there could be a bug here. The list of planning groups are freshed at the init stage of the rviz plugin. So if it is started before a move_group, the list will be empty. I have tested it previously with the moveit config of universal_robot and panda_moveit_config, it can list the arm and gripper groups. Actually, it reads the group names by a planning scene monitor, which listening to the scene topic "move_group/monitored_planning_scene".

  • The bottom progress bar in the tab is moving constantly even when 0 joint states exist, which gives the impression that something is loading. It looks like a bug.

I used the maximum value of the progressbar to indicate the total number of joint states, and the current value to indicate the number of completed joint states. When both values are set to zero, i.e. no joint states recorded and no joint states completed, it is moving constantly.

  • It seems that the Execute function immediately takes a sample after the move_group returns, but it should probably wait some time for the robot to stop moving.

Yes, I haven't put a wait time here. I should do that.

  • The "Auto Calibration" isn't really automatic, it is still going through a list manually. It would be very convenient to let the robot move through all the joint poses automatically, with wait times, and collect the result at the end.

I also struggled here. I have considered to do this fully automatic calibration. Two things changed my mind. I found it could be dangerous for some non-professional users to run the full automatic process if the robot or its ROS driver is not configured properly. And sometimes when a collision happens, it's not easy to stop the robot from the GUI widget if the robot has started moving. If an emergency button is used, that could break the pipeline, and everything may need restart over, except a robust robot ROS driver is used. Another thing is, the plan() of move_group seems need to run in a new thread if it is not run in the main thread, otherwise it will permanently stuck in its callback and cannot return. But if it runs in a new thread, the other operations will also be run in this thread, such as adding samples to the tree view, updating the progressbar and calculating the camera pose. To my experience, this very often brings in some strange segment fault in the qt. Now, the plan() and execute() thread emits a qt signal at its end, and a qt slot is used to deal with the rest operations, that makes it much more robust.

If I had to give one main feedback, it is that the "Calibrate" tab tries to do too many things at once. I think splitting it into "Set up collection", "Collect samples" and "Calculate camera pose" would make sense. Those tabs would have clearer functions:

  • Set up collection: Define the joint states at which samples should be recorded, with an interface to create, inspect and reorder joint poses, skip forward and backward through them, and finally save a set of joint states that could be cycled through safely and fully automatically.
  • Collect samples: In this tab, the user would load a set of joint states (or use the ones that were defined in the other tab), and then either manually (or fully automatically, with a sufficient wait time after each motion) record samples at each location, and allow the manual addition of more samples. A "Sample" here would be a camera image + extracted marker pose (I suppose). The user should be alerted if no marker is detected in a sample.
  • Calculate camera pose: This tab would calculate the camera pose from the sample, tell the user if it was successful, and give feedback on the quality of the calibration. It should allow the pose to be saved as a launch file or copied to the clipboard, ideally in different formats.

Let me repeat that I am a huge fan of this effort and I will look forward to seeing the polished end result. Thanks for putting in the work!

From your feedback, I see that "Calibrate" tag needs to be polished and improved a lot. I agree that it is necessary to split the mixed functions. I will take some time to make the change.

Sorry, But I still cannot find "object frame" in the RVIZ. I think it is also a plugin in Moveit Setup Assistant? Could you please help me? Thank you. @felixvd @RoboticsYY

@RoboticsYY
Copy link
Contributor Author

@ZZWang21 This is a closed issue. I think https://github.com/ros-planning/moveit_calibration repo is a more appropriate position to submit this issue.

@SARAWIN-BK-7
Copy link

SARAWIN-BK-7 commented May 10, 2023

no default handeye_target Do I need to find or determine how?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests