How do I plot a 'field of view' in 2D space and find if a certain point lies within the space?

Question

I am trying to model a robot arm (in 2D) that's supposed to have a camera at the end that moves with the arm. Given the field of view of the camera, I need to find whether a certain target is visible in this field of view as the endpoint of the arm starts moving. The horizontal of the camera is aligned with the last link of the robot arm, so the 'triangle' moves up and down, forwards and backwards etc. While this intuitively makes sense, I'm having trouble coming up with a formula for this. Also, as it's a camera field of view, the rays are technically infinitely long.

The data I have are: (example image below)

1. Cartesian coordinates of the last two 'points' of the robot arm (x3, y3) and (x4, y4)

2. Theta angle of the field of view

3. Coordinates of the point of interest (this never changes)

Any suggestions?

Answer 1

Maybe something simple like this would work? Assume you have been able to compute your coordinates $P_3 = (x_3, y_3)$ and $P_4 = (x_4, y_4)$ and you know the position of the point you want to see with the camera $Q = (a, b)$. Form the vectors $\overrightarrow{P_3P_4} = (x_4 - x_3, \, y_4 - y_3)$ and $\overrightarrow{P_4Q} = (a - x_4, \, b - y_4)$. The dot product between the two vectors has the following meaning $$\Big(\, \overrightarrow{P_3P_4} \cdot \overrightarrow{P_4Q} \, \Big) = \|\overrightarrow{P_3P_4}\| \, \|\overrightarrow{P_4Q} \| \cos{\theta} = \|P_4 - P_3\|\, \|Q - P_4\| \cos{\theta}$$ where $\theta$ is the angle between the oriented line $P_4Q$ and the oriented horizontal of the camera, which is defined by the line $P_3P_4$. If $\theta \in \left(-\frac{\pi}{6}, \frac{\pi}{6}\right)$ then your object is visible. In terms of cosine of the angle, this translates into the condition $\cos{\theta} \in \left(\frac{\sqrt{3}}{2}, 1\right)$. If $\cos{\theta}$ is not in the interval $\left(\frac{\sqrt{3}}{2}, 1\right)$, then your point is not visible. Form the above formula \begin{align}\cos{\theta} &= \frac{\Big(\, \overrightarrow{P_3P_4} \cdot \overrightarrow{P_4Q} \, \Big)}{\|P_4 - P_3\|\, \|Q - P_4\| } = \frac{\Big(\, \overrightarrow{P_3P_4} \cdot \overrightarrow{P_4Q} \, \Big)}{\|P_4 - P_3\|\, \|Q - P_4\| } \end{align} so if we plug in the coordinates we obtain the expression

\begin{align} \cos{\theta} = f(x_3,y_3,x_4,y_4,a,b) = \frac{ (x_4 - x_3)(a- x_4) + (y_4 - y_3)(b- y_4)}{\sqrt{ (x_4 - x_3)^2 + (y_4 - y_3)^2} \,\,\, \sqrt{ (a- x_4)^2 + (b- y_4)^2}} \end{align}

If $\,\, f(x_3,y_3,x_4,y_4,a,b) \,\in \, \left(\frac{\sqrt{3}}{2}, 1\right) \,\, $ then the point with coordinates $(a,b)$ is visible. Otherwise, it is not.

Make sure I haven't made a mistake in the condition and the proper orientation of the angle. I have changed them three times already :)

Comment: Regarding the bounds of $\cos{\theta}$, I will try to explain this as follows.

The camera horizontal is the directed line $P_3P_4$, directed from $P_3$ to $P_4$. Since the directed line $P_4Q$, oriented from $P_4$ to $Q$, determines of the angle of the point $Q$ with respect to the camera horizontal $P_3P_4$, by definition the visibility occurs when the angle $\theta$ between $P_3P_4$ and $P_4Q$ (measured counterclockwise from $P_4Q$ to $P_3P_4$) changes from $-30^{\circ}$ to $30^{\circ}$. The cosine $\cos{\theta}$ is related to the orthogonal projection of $P_4Q$ onto the directed horizontal $P_3P_4$. Let us rotate $P_4Q$ around $P_4$ and see how $\theta$ changes. At first when $\theta = -30^{\circ}$ then $\cos{(-30^{\circ})} = \sqrt{3}/2$. As the direction $P_4Q$ gets more and more aligned with $P_3P_4$, the angle $\theta$ grows from $-30^{\circ}$ to $0$ and thus the cosine grows from $\sqrt{3}/2$ to $1$. After the alignment, when $\theta=0$ and $\cos{0} = 1$, the cosine $\cos{\theta}$ starts to decrease (while the angle $\theta$ keeps growing from $0$ to $30^{\circ}$) until the direction $P_4Q$ reaches $30^{\circ}$ with $P_3P_4$ and the cosine becomes $\cos{\theta} = \sqrt{3}/2$ again.

Answer 2

Here’s an approach that anticipates doing more with the camera view later.

We’ll be working in two dimensions, but the same technique applies in three. We will assume that the camera view is a perspective projection as illustrated here:

This will necessitate working in homogeneous coordinates.

The first thing to do is to switch to the camera’s coordinate system. The origin of this coordinate system is at the camera’s position and by convention, the camera sights along the negative $y'$ direction (negative $z'$ in 3-d). The world to camera transformation is thus a translation to the camera’s position followed by a rotation. The matrix of this translation is easy to produce. It’s simply $$T=\pmatrix{1&0&-x_4\\0&1&-y_4\\0&0&1}.$$ For the angle $\phi$ that the camera’s line of sight makes with the world $x$-axis, we have $$\cos\phi = {x_4-x_3\over\|P_4-P_3\|}\\\sin\phi = {y_4-y_3\over\|P_4-P_3\|}.$$ To get aligned with the camera’s line of sight, we start by rotating clockwise through this angle, but we also need to rotate clockwise by an additional 90 degrees to get it to point down the camera’s negative $y'$-axis. Putting those two rotations together produces the rotation matrix $$R=\pmatrix{-\sin\phi&\cos\phi&0\\-\cos\phi&-\sin\phi&0\\0&0&1},$$ with $\cos\phi$ and $\sin\phi$ as above. Combining these two matrices, we have $$RT = \pmatrix{-\sin\phi&\cos\phi&x_4\sin\phi-y_4\cos\phi\\-\cos\phi&-\sin\phi&x_4\cos\phi+y_4\sin\phi\\0&0&1},$$ i.e., $$\begin{align}x'&=-(x-x_4)\sin\phi+(y-y_4)\cos\phi\\y'&=-(x-x_4)\cos\phi-(y-y_4)\sin\phi.\end{align}$$

The line labeled “i” in the above diagram is the image plane, which is perpendicular to the camera’s line of sight and at a distance $f$ from the camera (the focal distance). Note that, since the camera is looking down the negative $y'$-axis, $f<0$. The perspective projection $M$ maps a point in the $x$-$y$ plane onto the intersection of the image plane with the ray emanating from the camera and passing through the point. If we take $f=-1$, then the bounds of the visible region in the image plane are $\pm\tan\theta$, so if the $x'$-coordinate of the projection of a point is in this range, then it’s visible.

In the camera coordinate system, a projection matrix is very simple: $$P=\pmatrix{1&0&0\\0&1&0\\0&\frac1f&0}.$$ Putting this all together, given a point $Q=(x,y)$, we compute $$M(Q)=PRT\pmatrix{x\\y\\1}$$ and recover the projected $x'$-coordinate by dividing the first component of the resulting vector by the third. We can save ourselves a bit of work, though, by taking advantage of $P$’s simple form. Note that $$\pmatrix{1&0&0\\0&1&0\\0&-1&0}\pmatrix{x'\\y'\\1}=\pmatrix{x'\\y'\\-y'},$$ so we really only need to transform the target point into camera coordinates, after which we can just check that $-\tan\theta\le-x'/y'\le\tan\theta$. We might have $y'=0$, however, so let’s rewrite this as $|x'|\le|y'|\tan\theta$ to avoid dividing by zero.

You might object that the projection also maps points behind the camera onto the image plane, but that’s easily dealt with: check the sign of the camera-relative $y'$-coordinate. If it’s positive, the point is behind the camera, so there’s no need to compute its projection. You can eliminate the $y'=0$ case at the same time. If this seems backwards to you, you can always have the camera point in the positive $y'$ direction instead so that visible points have a positive $y'$-coordinate, but you’ll have to modify $R$ and $P$ accordingly.

As I mentioned above, the same approach works in 3-d, except that you’ll be working with $4\times4$ matrices. The rotation matrix will be a bit more complicated, but the translation will still be straightforward. Taking $f=-1$ again, the projection will result in $(x',y',z',-z')$. Assuming that the field of view is a circular cone, the test for visibility will then be $$x'^2+y'^2\le z'^2\tan^2\theta.$$

Postscript: This is, of course, overkill when the field of view is a right circular cone, whether in two dimensions or three. Checking that $(Q-P_4)\cdot(P_4-P_3)\ge\|Q-P_4\|\,\|P_4-P_3\|\cos\theta$ is much simpler and more efficient. However, the procedure that I’ve outlined here applies generally to any size and shape aperture, which becomes much more interesting when you move to three dimensions.