IBVS Local Minima Issue with Tilted Eye-in-Hand Camera for a Small Object – Seeking Advice

[aarist]

Dear ViSP Community,

I recently discovered the ViSP platform and have been impressed by its clean architecture and the quality of its visual servoing modules. Your IBVS implementation has served as an excellent baseline for my work, where I am developing a general purpose IBVS control law in Python for industrial applications. However, my current setup exposes a challenging configuration in which the standard formulations of ViSP appears to struggle. Since I have not yet been able to resolve this, I was hoping to draw on the expertise of the community to better understand how such cases are typically handled.

---

Application Context
I am implementing IBVS for a planar pick-and-place pipeline for a very small metallic part, requiring sub-millimetric accuracy. The system is:

• Hardware
  • UR10e robot
  • Eye-in-hand calibrated camera, tilted by approximately 30° about the TCP x-axis

• Object
  • 2.8mm x 1.8mm x 0.8mm (L x W x H) rectangular metallic part with a flat surface
  • 4mm x 4mm ArUco markers on a 4x4 grid for testing
  • The part is under NDA; the red rectangle in the image represents its size

• IBVS Features:
  • ~90 point features extracted on the part
  • For testing: Corners of the ArUco markers
  • Constant-depth approximation for all features
  • Standard point-feature interaction matrix
• Control Law:
  • Damped and regularized least squares $${v}_c=-\lambda \left( \left(\widehat{{L}_{{e}}}\ \widehat{{L}_{{e}}}^\top +\mu^2{I} \right)^{-1}\widehat{{L}_{{e}}}^\top \right){e}\\\,\,\,\text{with}\,\,\,\widehat{{L}_{{e}}}=1/2({L}_{{e}}+{L}_{{e}^*})$$
  • Camera twist mapped to the TCP via the adjoint transformation

The current system performs well for pure translational displacements for the metallic part (≤0.2 mm error). However, pure rotation around the TCP z-axis (i.e., in-plane rotation of the part) consistently fails: the image error decreases but the robot converges to an incorrect pose. Here are some results showcasing the systems behaviour:

The absolute error between the achieved TCP pose and target TCP pose is significant in x, y and z-positions. The error in the z-rotation decreases, but so does the y-rotation and the algorithm is stuck in a local minima. I get similar results for running my algorithm on a single ArUco marker, which is slightly larger compared to the metallic part:

aruco.mov

Here, the absolute error in the z-orientation is reduced nicely, but there is a significant error in the y-position and orientation between the target TCP frame and reached TCP frame. As a sanity check, I ran the algorithm with a larger target (full ArUco 4x4 grid), and in that case the rotation is handled nicely (the algorithm converges to the target pose with an accuracy of approx. 0.25mm and 0.5 deg):

out.mov

Here the error for the position is under 0.25 mm and for orientation under 0.05 deg.

---

My Question
Given this setup — especially the small size of the object and the ~30° camera tilt in the eye-in-hand configuration — I would greatly appreciate your insights on the following:

• How would you recommend handling pure in-plane rotation under these conditions for IBVS?
• What strategies have proven to be robust under such conditions?
  • Alternative feature parameterisations
  • Modifications to the interaction matrix
  • Better handling of depth approximation
  • Additional constraints to keep the camera/TCP orientation aligned
  • Task sequencing or hierarchical control
  • Different regularisation or weighting strategies

Any guidance or recommendations you might have would be extremely valuable.

[chaumett]

Dear aarist,

Here are some suggestions to solve your issues :

1. If I understand well, you would like to keep the end-effector parallel to your object, so that, expressed in the end-effector frame, only v_x, v_y, v_z and w_z are controlled by IBVS while w_x and w_y always remain 0. This is easy to implement in ViSP using the class vpServo with EYEINHAND_L_cVe_eJe as vpServoType. This is explained in page 116 of Part II of the RAM VS tutorial with a small hack to take the parallel constraint into account. In your case, L is the classical interaction matrix expressed in the camera frame, cVe the adjoint transformation from camera frame to TCP, and as for eJe, you should just implement the following 6x4 matrix:
   (1 0 0 0)
   (0 1 0 0)
   (0 0 1 0)
   (0 0 0 0)
   (0 0 0 0)
   (0 0 0 1).
   Doing so, the output of the servo will be the 6 components of the end-effector velocity expressed in the end-effector frame (with w_x and w_y always at 0), and you will just have to use the inverse Jacobian of the robot to obtain the robot joint velocities.
   This should solve in large part your issue.
2. Then, due to the tilt of the camera with respect to the object, it is a very bad approximation to consider that the depth is the same for all the points. It would be really better to use in the interaction matrix as inverse depth approximation for a 30dg tilt something like (if I did not do any mistake):
   1/Z_i = [sin(30dg) y_i + cos(30dg)]/d* where y_i is the y position of your image point i expressed in homogeneous coordinates and d* the height of the camera wrt object plane. Using the desired height should be sufficient even if it changes during the servo.
3. In my view, using the value of the interaction matrix at the desired configuration should be sufficient in the control scheme (i.e., not the mean between the current value and the desired value since you would need to estimate the current height of the camera to update the inverse depth of the points), and no damped/Levenberg-Marquardt least square that introduces undesirable motion if 1) above is not implemented.
4. If possible, for improving the conditioning of your system and reducing its sensitivity to image noise, it would be better to use a larger focal length so that the object appears as large as possible in the image (but I understand it will reduce the workspace for lateral motion so a compromise has to be found).
5. Finally, I may suggest you use as visual features:

• vpMomentGravityCenter
• vpMomentArea
• vpMomentAlpha
  or even better:
• vpMomentGravityCenterNormalized
• vpMomentAreaNormalized
• vpMomentAlpha
  In both cases, you will obtain a well conditioned 4x4 full rank system instead of a 2Nx4 system using the image points.
  Do not hesitate to contact us again if something is not clear.
  Hope this helps.
  François Chaumette.

[aarist]

Dear François,

Thanks for the swift and extensive response!

---

1. Yes, exactly. I couldn’t find the tutorial you mentioned, so here is my attempt to formalize your proposal mathematically.

We can use the camera velocity expressed in the end effector frame $\mathbf{v}_e = (v_x, v_y, v_z,\omega_x,\omega_y,\omega_z)$ to describe the relationship between the time variation of the vector of $k$ visual features $\dot{\mathbf{s}}$ and the camera velocity expressed in the camera frame $\mathbf{v}_c$ as

$$\dot{\mathbf{s}} = \mathbf{L}_s\mathbf{v}_c = \mathbf{L}_s {^c}{\mathbf V}_e \mathbf{v}_e,$$

where

$${^c}{\mathbf V}_e = \left[\begin{array}{cc} {^c}{\mathbf R}_e & {[{^c}{\mathbf t}_e}]_\times \; {^c}{\mathbf R}_e \\ 0_{3\times3} & {^c}{\mathbf R}_e \end{array} \right] \;$$

is the velocity twist matrix corresponding to transformation matrix from the end effector frame to camera frame

$${^c}{\mathbf T}_e = \left[\begin{array}{cc} {^c}{\mathbf R}_e & {{^c}{\mathbf t}_e} \\ 0 & 1 \end{array} \right].$$

We can force the IBVS controller to only control $v_x, v_y, v_z$ and $\omega_z$, while $\omega_x$ and $\omega_y$ always remain 0, by introducing a selection matrix $\mathbf{S}$:

$$\mathbf{S}=\begin{bmatrix} 1 & 0 & 0 & 0 \\\ 0 & 1 & 0 & 0 \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \in\mathbb{R}^{6\times4}.$$

The relationship between $\dot{\mathbf{s}}$ and the camera velocity $\mathbf{v}_e$ in the end effector frame is then modified to

$$\begin{align*} \dot{\mathbf{s}}=\mathbf{L}_s {^c}{\mathbf V}_e \mathbf{S} \mathbf{v}_e^* =\mathbf{L}_s^*\mathbf{v}_e^*, \end{align*}$$

where $\mathbf{L}_s^* \in\mathbb{R}^{k\times4}$ is the modified interaction matrix for point features of size $k\times4$ and $\mathbf{v}_e^*:=(v_x, v_y, v_z,\omega_z)$ is the modified camera velocity vector consisting only of the velocities which are controlled by IBVS.
The control law is then

$$\mathbf{v}_e^*=-\lambda(\mathbf{L}_s^*)^+\mathbf{e}.$$

By not considering $\omega_x$ and $\omega_y$ in the control law, the IBVS controller is forced to minimize the error by only utilizing the remaining DOFs.

As I understood this correctly, to accomplish this in ViSP, one can use the EYEINHAND_L_cVe_eJe servo type and ‘abuse’ the eJe matrix as the selection matrix.

In fact, I just implemented and tested your suggestions in my framework and it solves my issue! Here are the results for correcting a pure orientation around the TCP z-axis (10 degs):

The absolute error for the z orientation decreases exponentially, while the remaining errors remain in an acceptable tolerance for my problem. No undesired end effector motions.

The proposed strategy also converges well for a pure linear displacement in the x-y-plane:

And also for the more general displacement with a linear displacement in the x-y-plane and a rotation around the TCP z-axis:

The z-error has not converged in this case though, I probably need to run the loop longer.

---

2. Yes, this criticism is very justified. The IBVS controller seems to be quite robust to the current bad approximation though. Here is my attempt of the derivation of the proposed depth approximation for the tilted camera.

I have the following set up:

In the camera frame, the object plane can be written as

$$\mathbf{n}^\top\mathbf{X}=d^*,$$

where $\mathbf{n}$ ist unit normal of the plane expressed in the camera frame, $\mathbf{X}$ are the 3D coordinates in camera frame of a point lying on that plane and $d^*$ is the distance from the camera center to the plane along that normal. Given a tilt of $\varphi$, the normal in my setup is

$$\mathbf{n}=\begin{bmatrix} 0 \\\ -\sin(\varphi) \\\ \cos(\varphi) \end{bmatrix}.$$

Hence, the plane equation can be simplified to

$$\begin{align} \mathbf{n}^\top\mathbf{X}&=d^*\\\ \begin{bmatrix} 0 \, \, -\sin(\varphi) \,\,\cos(\varphi) \end{bmatrix} \begin{bmatrix} X \\\ Y \\\ Z \end{bmatrix}&=d^* \\\ -\sin(\varphi)Y+\cos(\varphi)Z&=d^*. \end{align}$$

Using the pinhole projection, the normalized $y$ image coordinate of the 3D point can be expressed as

$$y=\frac{Y}{Z},$$

which we can use to derive the proposed approximation

$$\begin{align*} -\sin(\varphi)Y+\cos(\varphi)Z&=d^*\\\ -\sin(\varphi)yZ+\cos(\varphi)Z&=d^*\\\ (-\sin(\varphi)y+\cos(\varphi))Z&=d^*\\\ Z&=\frac{d^*}{(-\sin(\varphi)y+\cos(\varphi))}\\\ \frac{1}{Z}&=\frac{(-\sin(\varphi)y+\cos(\varphi))}{d^*}. \end{align*}$$

Using the relationship $d^*=\cos(\varphi)d$ we finally obtain

$$\frac{1}{Z}=\frac{(-\sin(\varphi)y+\cos(\varphi))}{\cos(\varphi)d}.$$

In my derivation, the sign of $\sin(\varphi)$ is different, because of the camera frame coordinates (unless I did a mistake).

I will implement this suggestion in the coming days and report how it boosts the performance of the controller.

---

3. I agree that keeping the system as simple as possible is generally preferable, and in that sense using the interaction matrix evaluated at the desired configuration is a very reasonable choice. In my experiments, I also noticed that there is only a small difference between using source, mean or target values for the depth approximation.

However, I found that introducing a small amount of damping in the least-squares inversion has a significant positive effect on the smoothnes of the controller in practice. In my setup this resulted in noticeably smoother and more robust behavior, without introducing any large undesirable motion.

---

4. Yes, I agree. In this project I’m bound to the camera that is already in place, but for future IBVS work I would definitely choose the hardware based on the object.

---

5. Thank you for your suggestions, I plan to evaluate them in the near future! I have also come across your paper “Image Moments: A General and Useful Set of Features for Visual Servoing” (IEEE Transactions on Robotics, 2004) and intend to study it more detail in order to integrate the proposed visual features into my framework.

---

As a next step, I am considering applying a smoothing technique to the resulting velocities, such as an exponential moving average. Have you previously implemented a similar approach in ViSP?

Many thanks for your help!

[chaumett]

Perfect understanding, wonderful!

As for 5), I would suggest you look at:
O. Tahri, F. Chaumette. Point-based and region-based image moments for visual servoing of planar objects. IEEE Trans. on Robotics, 21(6):1116-1127, December 2005
available at http://rainbow-doc.irisa.fr/pdf/2005_itro_tahri.pdf
from http://rainbow-doc.irisa.fr/publi/author/Francois.Chaumette-eng.html (where all my publications are available).
Indeed, in the paper you reference, image moments are computed from dense objects while you need to consider discrete moments (computed from a set of discrete points) in your case. The equations are not exactly the same in both cases (but both are implemented in ViSP). This paper also explains the interest of using the normalized version (better 3D trajectory).

As for a smoothing strategy, nothing is implemented in ViSP since we are not used with working in "quite poor" conditions (due to the fact that your object is very small in the image), leading to a low conditioning of the system and a high sensibility to image noise. But feel free to implement your strategy, it is a good idea that should just decrease the time-to-convergence and exponential decrease of the error. I also feel that using the moments as suggested in 5) should fix, at least in part, the influence of noise thanks to their averaging effect.
Finally, I would like to mention the tutorial https://visp-doc.inria.fr/doxygen/visp-3.5.0/tutorial-boost-vs.html where are explained two strategies for:

1. speeding up the time-to-convergence with an adaptive gain;
2. starting smoothly from a motionless (or not) robot with a continuous sequencing.

All the best!

François.

[aarist]

Following up from my side:

Indeed, the proposed depth approximation does improve stability. I think its strength will increase with larger objects, but in my setup, it already adds a performance gain.

As anticipated, the smoothing strategy mainly introduces a delay in exchange for smoother velocity profiles. With greater smoothing factors, I found that the controller starts to oscillate slightly (consistent with the behaviour of a feedback control system subject to delay in the loop).

As for the tutorial:I have already discovered it a while ago and implemented the adaptive gain strategy. With some tuning of its parameters, it does improve convergence speed.

Thanks again for taking the time to help—your suggestions were extremely helpful and much appreciated!