# How do I estimate the position and orientation of two instruments looking at the same target?

Consider the following lab set-up:

Two instruments: a laser profiler (L) and a line scanner camera (C) are mounted on a rack so that they are facing a flat target downwards.

At time $$t=0$$, that is shown in the figure, the rack starts moving in positive x direction with a velocity $$v$$.

Every $$\Delta t_L$$ starting at $$t=0$$ the laser profiler records a "profile":

Index y z intensity
1 $$y_1$$ $$z_1$$ $$I_1$$
$$\vdots$$ $$\vdots$$ $$\vdots$$ $$\vdots$$
$$M$$ $$y_M$$ $$z_M$$ $$I_M$$

The laser profiler values are corrected for lens distortion in the internal software of the laser profiler.

Every $$\Delta t_C$$ starting at t=0 the camera records a "line":

Index angle intensity
1 $$\theta_1$$ $$I_1$$
$$\vdots$$ $$\vdots$$ $$\vdots$$
$$N$$ $$\theta_N$$ $$I_N$$

where the angle, $$\theta$$ is the angle in the $$yz$$ plane with $$-\vec{e_z}$$.

The camera sensor consists of one line of N pixels. During a calibration process the angle of each pixel has been determined. Due to lens distortion the angle is not a linear function of pixel number. However the angles $$\theta_i$$ are the same from line to line.

The position of the Laser profiler at time t is: $$(vt,y_L,z_L)$$ where $$y_L$$ and $$z_L$$ are unknown parameters that must be estimated.

The position of the Camera at time $$t$$ is: $$({x_C}_0 + vt,y_C,z_C)$$ where $${x_C}_0$$, $$y_C$$ and $$z_C$$ are unknown parameters that must be estimated.

Further the Laser profiler is unfortunately rotated slightly with the Euler angles: $$(roll_L,pitch_L,yaw_L)$$. These angles are also unknown parameters that must be estimated. The Euler angles can be used to compose a rotation matrix R: This matrix represent a change of basis from "world" coordinate system to rotated coordinate system: $$\vec{v}_{rotatedBasis} = R*\vec{v}_{unrotatedBasis}$$ The reverse change of basis is: $$\vec{v}_{unrotatedBasis} = R^T*\vec{v}_{rotatedBasis}$$

The Camera is also unfortunately rotated slightly with the Euler angles: $$(roll_C,pitch_C,yaw_C)$$. These angles are also unknown parameters that must be estimated.

The target is an aluminium chess board with 368 corners that I have "picked" in the recordings from both instruments:

CornerIndex timeIndex$$_L$$ $$y_L$$ $$z_L$$ timeIndex$$_C$$ $$\theta_C$$
1 $${t_L}_1$$ $${y_L}_1$$ $${z_L}_1$$ $${t_C}_1$$ $${\theta_C}_1$$
$$\vdots$$ $$\vdots$$ $$\vdots$$ $$\vdots$$ $$\vdots$$ $$\vdots$$
368 $${t_L}_{368}$$ $${y_L}_{368}$$ $${z_L}_{368}$$ $${t_C}_{368}$$ $${\theta_C}_{368}$$

Further each square of the chess pattern is 15 mm x 15 mm and the chessboard is 5 mm thick. The edge of the board is aligned with the x-axis.

How do I estimate the 11 unknown parameters from these 368 point correspondences and the size of the squares?

Is there a software library that could solve this problem? Otherwise I am not necessarily looking for an answer with all the details, just some pointers.

It seems that for one ordinary camera pose estimation is solved by minimizing square error in the image plane. Further it seems that for two ordinary cameras the Fundamental matrix is used to relate the points in the image plane of the two cameras to each other. Is it feasible to derive a Fundamental matrix in my case?

Alternatively the problem could be formulated in world coordinates.

• Interesting problem! I've made an attempt to reformat the tables so they are (for me) a little more readable. I've taken some liberties to change some of the notation. Feel free to edit / revert if I've got anything wrong.
– Peter K.
Jan 13 at 19:00
• Homework? This sounds like an assignment that I might give, if I were teaching a sufficiently advanced image processing class. Jan 13 at 22:41
• You are saying that the actual physical thing that you are imaging is known -- and it sounds like it's known well enough that you can consider it a reference. Correct? Jan 13 at 22:43
• You're forgetting lens distortion on the line scanner, and whatever the equivalent is on the laser scanner -- or your instructor left it out deliberately and told you to use a pinhole camera model. Please edit your question to clarify that lens distortion is or is not a problem. Jan 13 at 22:44
• @TimWescott: yes the chess target board can be considered a reference, It may however be slightly rotated with respect to the x-axis. I try to align the edge of the board with the x-axis. Assuming I do a good job at this I think perhaps this rotation can be neglected. Lens distortion is not a problem.
– Andy
Jan 14 at 10:30

As far as I know, the best way to do this is with nonlinear optimization. This is what is implied in the OpenCV page about its camera calibration routines.

• Write an equation that generates your table from your 11 independent parameters.
• Decide on a cost function that finds an overall error between your measured table and your generated one.
• Use an optimization function to find the best fit for your 11 values. If you're just going to throw the whole problem at the optimizer, blind, you'll want to use some global optimization routine like Scipy's dual_annealing.
• Declare victory (well, or check your work and repeat as necessary).

There are things that may help:

1. Split the problem into the laser scanner and the camera, and do two separate optimizations, as you mentioned in your comments.
2. See if you can find an initial approximation, perhaps from the four extremal intersections for each of the camera and laser, then use a local optimization method. If and only if you've put yourself into the neighborhood of a local minima, this should work a lot faster than a global optimization.
3. Experiment with the best cost function for your problem. The cost function that's nearly always easiest for the optimizer is sum-of-squares, i.e. the sum of the squares of the Euclidean distances of your generated points from the actual.
• For "noisy" measurements this is probably close to the best you can do, but you run the risk of having one or two outliers.
• For precise measurements and a model that's not a complete fit, something like the maximum Euclidean distance found may be best -- but optimizers really hate that sort of thing, because the slope of the cost vs. parameter curve changes, sometimes sharply, whenever the index of the worst-case error changes. Some study on "min-max optimization" may help you out here.
• A compromise that has sometimes worked for me is use $$\Delta x^{2n}$$ -- this tends to make a flat-bottomed cost function that brings the optimum closer to a min-max, but keeps the cost function continuous in all the parameters, all the time.

I'm not a super expert with optimization, but as near as I can tell, in practice you just need to try out a bunch of things and see what works -- then if you're doing this in some sort of production environment, refine as necessary as reality shatters assumptions you didn't even realize you made.