Background:
I am performing some laser scanner simulations with a multi-plane lidar. For each scan ray, I perform a ray-polygon intersection test between the ray and all polygons in my simulated scene.
Previously, for single-plane laser scanners, I could perform a quick check to see if either:
- Any vertex of a world polygon falls exactly on the scan plane, or
- At least one vertex was below the scan plane and at least one other vertex was above the scan plane.
All polygons that failed this test did not intersect the scan plane and could be ignored. This greatly improved my simulation speed because it allowed me to drop a huge percentage of scene polygons before I had to run through the polygons that did lie on the scan plane for the ray-polygon intersection test.
The scenario:
Now, with a multi-plane laser scanner, each "plane" has a unique, non-zero angle relative to the plane that is normal to the axis of rotation of the laser scanner. The issue here is that these scan "planes" aren't really planes - the non-zero angles are given as the angles between the scan rays at $\theta=0$ for each plane.
When these rays are revolved around the axis of rotation, only the ray with an angle of zero makes a true plane. Each other ray rotates to form a cone. Velodyne makes 16, 32, and 64 "plane" laser scanners, but again, there's maybe one true plane, and then 15, 31, or 63 cones for me to work with.
The problem:
I can't do quick plane-polygon intersections if the "plane" is actually a cone.
I'm not sure how to describe this, but I'm looking for a transform that could "curl" or flatten a cone into a plane. My idea would be that I could apply this flattening transform at the optical center of the laser scanner to curl the points that make up the scene.
I understand this would introduce aberrations or similar flaws beyond the cone that gets transformed, but I'm not going to use the transformed points for anything beyond using them as a metric to decide if I should keep or eliminate the corresponding un-warped facets for use with the ray-polygon intersection test.
I've looked online and all I can seem to find is the procedure to flatten curves drawn on a cone. Ideally I'm looking for something like an affine transform matrix that I could use to quickly warp the world. The only affine transforms I've seen though are for planar operations; rotate, skew, mirror, etc.
Spherical coordinates certainly work for this, but the Cartesian-to-spherical conversion is relatively expensive—a square root and two inverse trig function for each point to be tested. There’s a different way to do this culling that doesn’t require these expensive operations and is similar to what you’re likely already doing for the planar case. I presume that to test against a plane you’re doing something equivalent to plugging the coordinates of the point into the equation of the plane and examining the sign of the result to determine which side of the plane the point is on. A similar calculation can be done to determine if a point is on the inside or outside of a quadric surface, though it requires evaluating a quadratic form instead of a dot product. An advantage of this approach is that it introduces no distortions or aberrations.
Working in homogeneous coordinates makes this fairly simple to express. In the following, a tilde indicates an inhomogeneous (Cartesian) vector, while symbols without tildes are homogeneous. In the planar case, let $\tilde{\mathbf p}$ be the scanner’s location and $\tilde{\mathbf n}$ a normal to the plane. We’ll let the direction of the normal define “up.” The plane can be represented by the homogeneous vector $$\mathbf\pi=\begin{bmatrix}\tilde{\mathbf n} \\ -\tilde{\mathbf n}\cdot\tilde{\mathbf p}\end{bmatrix}.$$ Points on this plane satisfy the equation $\mathbf\pi\cdot\mathbf x=0$, but we can apply $\mathbf\pi$ to any point. The sign of the result tells us where the point is in relation to the plane: positive means that it is “above” the plane, negative, “below.”
The equation of a quadric surface can be written in the homogeneous form $Q(\mathbf x)=\mathbf x^T\Omega\mathbf x=0$, where $\Omega$ is a $4\times4$ matrix. ($\Omega$ is usually taken to be symmetric since the contributions of its skew-symmetric part cancel.) For points not on the surface, the sign of $Q(\mathbf x)$ tells us whether the point is inside or outside of the quadric. A cone is a degenerate quadric surface, so we can use this to cull polygons in the same way that you did for a plane: if all of the vertices are on the same side of the cone, then it doesn’t intersect the polygon.
Now, it looks like you’re only interested in one nappe of the cone, so this will be a two-stage test. First, check against the $\theta=0$ plane to see if the point is on the same side as the nappe that you’re interested in, then test against the quadric if it is.
All that remains to do be done is to find $\Omega$. Let’s start with a cone with apex at the origin and the $z$-axis as its axis. The equation of this cone is $$(x^2+y^2)\tan^2\theta=z^2$$ with $\theta$ the angle of elevation from the $x$-$y$ plane as in your question. Clearly, if $z^2\gt(x^2+y^2)\tan^2\theta$, then the point is above the surface—inside of the cone—and vice-versa when $z^2\lt(x^2+y^2)\tan^2\theta$.
We can write down the matrix for this quadric directly: $\Omega=\operatorname{diag}(\tan^2\theta,\tan^2\theta,-1,0)$ or, if you prefer, $\operatorname{diag}(1,1,-\cot^2\theta,0)$. (This is a homogeneous matrix, so multiplying by a non-zero scalar doesn’t affect the quadric surface represented by the matrix.) For reasons that will soon become clear, we’ll take the negative of this matrix, i.e., $$\Omega=\begin{bmatrix}-\tan^2\theta&0&0&0\\0&-\tan^2\theta&0&0\\0&0&1&0\\0&0&0&0\end{bmatrix}$$ and $$Q(\mathbf x)=\mathbf x^T\Omega\mathbf x=z^2-(x^2+y^2)\tan^2\theta.$$ We then have $$Q([1,0,0,1]^T)=-\tan^2\theta\lt0 \\ Q([1,0,\tan\theta,1]^T)=0 \\ Q([0,0,1,1]^T) = 1\gt0.$$ That is, $Q(\mathbf x)$ is positive if the point is inside the cone and negative if it’s outside. (These signs would’ve been reversed had we taken the original $\Omega$ above.)
To obtain the matrix for the cone in general position, we need only transform this $\Omega$ by a translation and rotation: $$\Omega'=(RT)^T\Omega RT=T^TR^T\Omega RT.$$ The translation matrix is easily constructed. It’s simply $$T=\left[\begin{array}{c|c}I&-\tilde{\mathbf p}\\\hline\mathbf 0^T&1\end{array}\right].$$ The rotation matrix takes a little bit more work, but is also fairly simple to construct. Let $\tilde{\mathbf w}=\tilde{\mathbf n}/\|\tilde{\mathbf n}\|=(w_x,w_y,w_z)$. At least two of $(-w_z,0,w_x)$, $(0,-w_z,w_y)$ and $(-w_y,w_x,0)$ are non-zero. Choose one of them and normalize to get the unit vector $\tilde{\mathbf u}$. The required rotation matrix is then $$R=\left[\begin{array}{c|c}\tilde{\mathbf u}^T & 0 \\ (\tilde{\mathbf w}\times\tilde{\mathbf u})^T & 0 \\ \tilde{\mathbf w}^T & 0 \\ \hline \mathbf0^T&1 \end{array}\right]$$ i.e., the $3\times3$ matrix with $\tilde{\mathbf u}$, $\tilde{\mathbf w}\times\tilde{\mathbf u}$ and $\tilde{\mathbf w}$ for its rows, padded out to a $4\times4$ transformation matrix. (For numerical stability, it’s best to choose for $\tilde{\mathbf u}$ the vector that’s farthest from the $z$-axis.)
Setting up $\Omega'$ does require computing a few square roots to get the rotation matrix, but this is only done once, and $\tan^2\theta$ is computed once per scan, so if you have many points to test this can be a lot less computationally expensive than a Cartesian-to-spherical conversion (although, that, too, need only be done once if you’re willing to store all of the converted coordinates).
For example, let $\theta=\pi/6$, $\tilde{\mathbf p}=(3,-10,4)^T$ and $\tilde{\mathbf n}=(3,7,-1)$. We then have $\Omega=\operatorname{diag}(-1/3,-1/3,1,0)$. Normalizing $\tilde{\mathbf n}$ gives $\tilde{\mathbf w}=\frac1{\sqrt{59}}(3,7,-1)$ and the best choice for $\tilde{\mathbf u}$ is $\frac1{\sqrt{58}}(-7,3,0)$, giving $$R=\begin{bmatrix}-0.919145& 0.393919& 0.& 0\\ 0.0512839& 0.119662& 0.991489& 0\\ 0.390567& 0.911322& -0.130189& 0\\ 0& 0& 0& 1 \end{bmatrix}.$$ The resulting quadric matrix is $$\Omega'=\begin{bmatrix}-0.129944& 0.474576& -0.0677966& 5.40678\\ 0.474576& 0.774011& -0.158192& 6.94915\\ -0.0677966& -0.158192& -0.310734& -0.135593\\ 5.40678& 6.94915& -0.135593& 53.8136 \end{bmatrix}.$$ We test this with a few points. $Q'(\tilde{\mathbf p})=7.99361E-15$, so the scanner is indeed on the cone. The point $\tilde{\mathbf p}+\tilde{\mathbf n}$ should be inside the cone, and we have $Q'(\tilde{\mathbf p}+\tilde{\mathbf n})=59.0$, so it is. $\tilde{\mathbf w}$ is orthogonal to $\tilde{\mathbf n}$, so $\tilde{\mathbf p}+\tilde{\mathbf w}$ is on the plane through the scanner that’s orthogonal to $\tilde{\mathbf n}$, so should be outside the cone: $Q'(\tilde{\mathbf p}+\tilde{\mathbf w})=-19.3333$.
As a final implementation note, if you’re doing this culling for a series of scans from the same position and orientation of the scanner, it might be simpler and faster to transform each point by $RT$ and then compute $Q(\mathbf x')=z'^2-(x'^2+y'^2)\tan^2\theta$. The transformation $RT$ is fixed for a given position and orientation of the scanner, so you only need to recompute $\tan^2\theta$ and can avoid computing a new $\Omega'$ for each scan. You’ll probably have to test both to see which of the two implementations is actually faster for your application.