Abstract:
A photogrammetry system and method provide for determining the relative position between two objects. The system utilizes one or more imaging devices, such as high speed cameras, that are mounted on a first body, and three or more photogrammetry targets of a known location on a second body. The system and method can be utilized with cameras having fish-eye, hyperbolic, omnidirectional, or other lenses. The system and method do not require overlapping fields-of-view if two or more cameras are utilized. The system and method derive relative orientation by equally weighting information from an arbitrary number of heterogeneous cameras, all with non-overlapping fields-of-view. Furthermore, the system can make the measurements with arbitrary wide-angle lenses on the cameras.

Description:
STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH OR DEVELOPMENT 
     The invention described herein was made by employees of the United States Government, and may be manufactured and used by or for the Government of the United States of America for governmental purposes without the payment of any royalties thereon or therefor. 
    
    
     BACKGROUND OF THE INVENTION 
     1. Field of the Invention 
     The present application is related to photogrammetry utilized to determine the relative position of two bodies. 
     2. Description of the Related Art 
     Photogrammetry is the science of making measurements from photographs. In general, the output of photogrammetry is typically a map, a drawing, or a three-dimensional (3D) model of a real-world object or scene. Photographic images are the result of a perspective projection of a 3D object onto two dimensions. Consequently, one or more photographic images can be reverse-engineered to derive the 3D shape of the original object. The solution provides a quantitative relationship between a 3D shape and the two-dimensional (2D) images acquired by cameras. 
     Accurate photogrammetric measurements require the photographing of high contrast surface features that appear in at least one image. However, many objects commonly measured do not naturally exhibit such features. Accordingly, retro-reflective targets may be attached to the object to artificially provide these high contrast features. When illuminated, these targets reflect light directly back to the illuminating source, causing the targets to appear very bright in contrast to the background. 
     Various photogrammetry systems for determining relative motion between two bodies have been developed. Although some known photogrammetry systems utilize a single camera, other systems may require two or more cameras having overlapping fields-of-view (FOV). Existing photogrammetry systems may require specific types of camera lenses, and may not permit use of cameras having fish-eye or hyperbolic lenses. 
     SUMMARY OF THE PRESENT INVENTION 
     One aspect of the present invention is a photogrammetry system for determining the relative positions of first and second bodies. The two bodies are preferably rigid, or substantially rigid, and one or more imaging devices, such as high speed digital cameras, are mounted on the first body. The cameras may include wide-angle fish-eye lenses, conventional lenses, or an arbitrary lens or mirror. The cameras may have non-overlapping fields-of-view. The second body is provided with at least three targets, such that the camera or cameras can determine a line-of-sight from the cameras to the targets. In a preferred embodiment, the targets comprise a reflective material that is illuminated by a light source, such as an LED. The targets may also comprise LED lights or the like. 
     The camera or cameras can be still-frame or high speed cameras, preferably capable of at least 60 frames per second. In general, the cameras have a frame rate that is twice the speed of the fastest object being observed. The cameras are operably connected to controllers, and the cameras are synchronized utilizing a clock synchronization signal. An initial target location for each of the at least three targets is measured or otherwise determined. The camera or other imaging device is then utilized to generate a plurality of lines-of-sight from the camera or cameras to each of the at least three targets at a second time that is after the first time. In general, the second time is later than the first time by an amount equal to the inverse of the frame rate of the camera or cameras. 
     A plurality of equations are then developed and utilized to determine the new location of the second body relative to the first body at the second time. In general, the equations include the distances between the cameras and the targets multiplied by the lines-of-sight and added to the known camera positions, wherein the lines-of-sight are determined by the camera at the second time. This result is set equal to the product of the original target locations multiplied by the unknown rotation and translation of the second body relative to the first body between the first and second times. That is, the matrix equation at a high level is:
 
DISTANCES*LINES_OF_SIGHT+CAMERA_POSITIONS=ORIGINAL_TARGETS*ROTATION*TRANSLATION
 
     These equations may be arranged into a matrix equation of the form Ax( )=b, where x( ) becomes a function of the vector t of unknowns, wherein x( ) returns the longer vector x. The vector t contains the unknown three Euler angles, three translations, and the distances to the targets. The vector x is identical to t, except that the three Euler angles are replaced with the equivalent nine elements of a standard rotation matrix. In this manner, the function x( ) can be utilized to compute a valid rotation matrix at every iteration, using the appropriate sin( ) and cos( ) functions of the Euler angles within t. An algorithm is then utilized to iterate over t and converge upon the best solution Ax(t′)−b=0. An example of an algorithm is the MATLAB® non-linear least-squares algorithm fsolve( ) which iterates over t. This produces a (nearly-zero) vector e=Ax(t′)−b, which represents a residual error. The equations can be solved at each camera frame to thereby determine the position of the second body relative to the first body at a large number of times. 
     The cameras are calibrated to determine a mathematical relationship between a two-dimensional (2D) pixel point in the image plane and a three-dimensional (3D) vector emanating from the center of the camera lenses. A MATLAB® toolbox, called the Omnidirectional Camera and Calibration Toolbox (OCamCalib) may be utilized to facilitate the calibration process for wide-angle lenses or lenses formed with mirrors. The MATLAB® Camera Calibration Toolbox can be utilized for calibration of cameras having longer lenses. 
     After the cameras have been calibrated, and the initial locations of the targets have been determined, the position and orientation of each camera in the first body coordinate system is determined. This calculation may be termed “space resection,” and it may comprise a known process. In one example of a resectioning process, the resection is accomplished by associating “dots” (potential targets) in an image with entries from a database of known target locations. An algorithm that includes three iterations may be utilized to determine the camera&#39;s precise location and orientation in the coordinate system of the first body. The first iteration involves a few (three or more) dot-target pairings that a user selects manually. In a second iteration, an automatic pairing function of the algorithm selects dot-target pairs, then recomputes the resection. The automatic pairing function selects all the targets it can reasonably match according to predefined criteria. A third iteration is then utilized to eliminate the worst of the dot-target pairs based on the individual dot-target pair residuals. The residual threshold for deleting dot-target pairs may be the mean value of the second iteration&#39;s residuals. 
     These and other aspects, objects, and features of the present invention will be understood and appreciated by those skilled in the art upon studying the following specification, claims, and appended drawings. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  is a schematic view of a first body having a plurality of cameras, and a second body having a plurality of targets according to one aspect of the present invention; 
         FIG. 2  is a fourth-order three-dimensional (3D) surface representing the focal pattern of a wide-angle camera lens; 
         FIG. 3  illustrates a space resection process, whereby unit vectors emanating from a camera are transformed so that they point to their corresponding three-dimensional (3D) target locations; 
         FIG. 4  illustrates a relative dynamics algorithm and residual error; 
         FIG. 5  illustrates dot-target automatic pairing utilized for camera resectioning; and 
         FIG. 6  illustrates a high speed camera system architecture according to one aspect of the present invention. 
     
    
    
     DETAILED DESCRIPTION OF EMBODIMENTS 
     For purposes of description herein, the terms “upper,” “lower,” “right,” “left,” “rear,” “front,” “vertical,” “horizontal,” and derivatives thereof shall relate to the invention as oriented in  FIG. 1 . However, it is to be understood that the invention may assume various alternative orientations, except where expressly specified to the contrary. It is also to be understood that the specific devices and processes illustrated in the attached drawings, and described in the following specification are simply exemplary embodiments of the inventive concepts defined in the appended claims. Hence, specific dimensions and other physical characteristics relating to the embodiments disclosed herein are not to be considered as limiting, unless the claims expressly state otherwise. 
     Referring to  FIG. 1 , a system utilizing the present invention includes first and second bodies  10  and  20 . In general, the bodies  10  and  20  may comprise various types of rigid or semi-rigid bodies capable of holding cameras and fiducial markings. In the illustrated example, first body  10  includes a crew module that is mounted to a rocket or other launch vehicle to lift the crew module into space. The second body  20  may comprise a forward faring that fits around the crew module at the time of launch, and later detaches in the event of a launch abort. However, the first and second bodies  10  and  20  may comprise other objects, such as two space vehicles that are to dock in space. Other examples of applications for the photogrammetry system and method of the present invention include in-flight refueling of aircraft, wherein the first and second bodies  10  and  20  comprise fuel delivery and fuel-receiving components that are interconnected for in-flight refueling. Still further, the first and second bodies  10  and  20  may comprise a robot or other movable tool or device and an object being fabricated or assembled. Finally, body  10  may be the Earth itself (solid ground), and body  20  is a moving object. 
     One or more cameras  1 ,  2 ,  3 , etc. are mounted to a first body  10 . The first body  10  defines a coordinate system  11 . As discussed in more detail below, the locations of the cameras  1 - 3  can be determined in the coordinate system  11 . A plurality of targets  5 ,  6 ,  7 , etc. are fixed to the second body  20 . Targets  5 - 7  should be of such size that they appear to be more than one pixel across in the imaging devices. Although as few as three non-collinear targets may be utilized, more targets are preferably utilized to improve the accuracy of the results. For example, several hundred targets may be utilized. As the second body  20  moves relative to the first body  10 , the targets  5 - 7  move from first locations  5 A,  6 A, and  7 A to second locations  5 B,  6 B, and  7 B, respectively. Lines-of-sight or rays  12 ,  13 ,  14  extend from cameras  1 ,  2 , and  3  to the targets  5 ,  6 , and  7 , respectively. The cameras  1 ,  2 ,  3 , etc. generate “dots” corresponding to the targets  5 ,  6 ,  7 , etc. 
     In general, the method described in more detail below can be utilized in systems having from 1 to N cameras. The cameras may have different lenses, and different resolutions. Also, although at least three “dots” corresponding to three or more targets must be available to determine the position of body  20  relative to body  10 , the same targets do not have to be visible to all cameras at all times. Furthermore, if more than one camera is mounted on body  10 , the relative position of body  20  at any point in time can be determined provided that the total number of unique targets that are “visible” to all the cameras on body  10  is at least three. 
     The cameras  1 ,  2 ,  3 , etc. are preferably high speed cameras providing at least 210 frames per second. The cameras are synchronized to ensure that all cameras capture frames synchronously. In addition, the time stamps of the cameras are also preferably tied together to provide another approach to characterizing synchronicity. As discussed below in connection with  FIG. 6 , the cameras may be provided with LED light sources to illuminate the targets  5 - 7 , etc. The cameras may include wide-angle fish-eye lenses. One example of a commercially available fish-eye lens that may be utilized with the cameras  1 ,  2 , and  3  is a Sunex DSL215 available from Sunex Inc., USA, Carlsbad, Calif. This lens has a 185 degree field-of-view. In general, cameras utilized in photogrammetric measurement processes must be calibrated. This involves capturing images from the cameras under controlled conditions and mathematically characterizing the results. The goal of the calibration process is to find a precise mathematical relationship between a two-dimensional (2D) pixel point in the image plane and a three-dimensional (3D) vector emanating from the center of the camera. In general, the choice of a mathematical model depends on the type of camera lens system used. For many standard camera systems with a relatively long focal length, a collinearity model is usually implemented. This model is based on the observation that a 3D object, its corresponding 2D pixel in the image, and the lens center, all lay on the same line. For any pixel in the image plane of this type of camera, a 3D vector out to the scene, also called a line-of-sight, can easily be constructed using a constant vertical vector component (the focal length). This model has been extensively developed, and well-established camera calibration procedures are available for camera systems having a relatively long focal length. 
     However, for cameras having a wide-angle fish-eye lens, the collinearity condition no longer holds true. Fish-eye lenses bend the incoming light such that a very wide angle of incident rays, even up to 90 degrees perpendicular to the optical axis, can be captured on the image plane. To reconstruct the 3D vectors emanating from the camera lens center, the vertical component of the 3D vector is no longer a constant. A calibration procedure for non-linear wide-angle lens systems has been developed and implemented in a MATLAB® toolbox by Dr. Davide Scaramuzza, called the Omnidirectional Camera and Calibration Toolbox (OCamCalib) to facilitate this calibration process. The OCamCalib toolbox is publicly available on line. In this model, the equation for the 3D vector emanating from the camera is a fourth-order polynomial. The 3D structure of this curved polynomial surface is illustrated in  FIG. 2 . The center of the wide-angle lens  25  and vectors to the 3D scene  26 A- 26 D are shown. If the cameras utilize conventional linear (not wide-angle) lenses, the Camera Calibration Toolbox for MATLAB® developed by the California Institute of Technology can be utilized instead of the OCamCalib toolbox. 
     The calibration procedure involves capturing numerous pictures of a rigid checkerboard (not shown) placed at random locations around the camera. The OCamCalib software then automatically finds the corner points throughout each checkerboard image. The OCamCalib toolbox analyzes the non-linear distortion present in the images and iteratively determines the effective focal center of the image and the polynomial coefficients that best describe the wide-angle projection pattern. A residual error is also calculated to quantify how accurately the 3D points are mapped into 2D images based on the original checkerboard images. This calibration procedure may be applied to each of the cameras  1 ,  2 ,  3 , etc. ( FIG. 1 ). With these intrinsic camera parameters, any 2D pixel coordinate capture by one of the cameras  1 ,  2 ,  3 , etc. can be mapped to a 3D vector out to the scene. 
     Referring again to  FIG. 1 , the initial target locations  5 A,  6 A,  7 A, etc. in 3D space in coordinate system  11  must be determined. Suitable commercially available photogrammetry systems may be utilized. According to one example of a suitable method, a laser metrology system first captures the location of a relatively large member of targets (e.g., 30 targets) in the vehicle coordinate frame system  11 . A photogrammetry camera then captures an image collage of all the targets&#39; precise locations with respect to each other and derives their relative orientations (this set includes the previously laser-mapped targets). This full target set is then aligned to the laser-measured set, giving three-space coordinates in the vehicle frame  11  to each target. In one example in which the first body  10  comprises a crew module, and the second body  20  comprises a forward faring, the laser metrology system was utilized to map 28 targets, and the cameras (four total in this example) were utilized to map 713 targets. 
     With further reference to  FIG. 3 , once each camera has been calibrated, and the locations of the targets in the scene have been determined, the next step is to calculate the position C and orientation R of each camera  1 ,  2 ,  3 , etc. in the coordinate system  11  of the first body  10 . This calculation is termed “space resection” in the photogrammetry literature. This process begins by finding the 2D centroid of each complete target visible to the camera before any motion of the second body  20  occurs. The camera calibration parameters obtained earlier are used to convert each of these centroid coordinates into 3D unit vectors emanating from the center of a camera (conceptually located at the origin  27 ). The goal is determine the camera rotation R and translation required to transform the unit vectors so that they point to the corresponding 3D targets P as shown in  FIG. 3 . The matrix equation for this coordinate transformation is summarized as:
 
TARGET_LOCATIONS=DISTANCES*UNIT_VECTORS*CAMERA_ROTATION+CAMERA_POSITION
 
Or simply, P=C+DVR. This matrix equation is non-linear because of sin( ) and cos( ) functions within the camera rotation matrix. The solution includes three rotation angles, three camera position coordinates, and the distances to each of the 3D targets. This answer may be iteratively determined using the non-linear least-squares solver fsolve( ) within MATLAB®, which is based on the Levenberg-Marquardt convergence algorithm. It will be understood that numerous other ways to determine camera orientation are known, and the procedure just described is but one example of a suitable approach.
 
     Matrix equations can be utilized to model the relative dynamics of the first and second bodies  10  and  20 , respectively ( FIG. 1 ). If it is assumed that the first and second bodies  10  and  20 , respectively, undergo 3D rigid body rotation and 3D rigid body translation, then the relative motion of body  20  with respect to coordinate system  11  on body  10  can be expressed in matrix form, with unknowns in bold, as:
 
NEW_TARGETS=ORIGINAL_TARGETS*ROTATION*TRANSLATION
 
At the same time, images of the targets captured by each calibrated camera  1 ,  2 ,  3 , etc. can be examined to construct 3D lines-of-sight from the calibrated cameras  1 ,  2 ,  3 , etc. to the targets:
 
NEW_TARGETS=DISTANCES*LINES_OF_SIGHT+CAMERA_POSITIONS
 
By setting these equations equal to one another, the following equation is obtained:
 
DISTANCES*LINES_OF_SIGHT−ORIGINAL_TARGETS*ROTATION*TRANSLATION=−CAMERA_POSITIONS
 
     The detailed matrix equations may be developed as follows. Referring to  FIG. 1 , the locations of the cameras  1 ,  2 ,  3 , etc. may be designated C 0 , C 1 , C 2 , etc., respectively. The first target locations  5 A,  6 A,  7 A, etc. may be designated G 0 , G 1 , G 2 , and the new locations  5 B,  6 B,  7 B, etc. may be designated G 0 ′, G 1 ′, G 2 ′, etc. Similarly, the first line-of-sight or “ray”  12  defines a unit vector (i 0 , j 0 , k 0 ), the “ray”  13  defines a unit vector (i 1 , j 1 , k 1 ), and “ray”  14  defines a unit vector (i 2 , j 2 , k 2 ). In general, the number of target locations is at least three, but many more target locations may be utilized to reduce the accuracy of the system and calculations. Similarly, more than one camera may be utilized, if required, for a particular application. The new locations of the targets can be expressed as follows: 
     
       
         
           
             
               
                 
                   
                     
                       [ 
                       
                         
                           
                             
                               G 
                               0 
                               
                                 x 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               0 
                               
                                 y 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               0 
                               
                                 z 
                                 ′ 
                               
                             
                           
                         
                         
                           
                             
                               G 
                               1 
                               
                                 x 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               1 
                               
                                 y 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               1 
                               
                                 z 
                                 ′ 
                               
                             
                           
                         
                         
                           
                             
                               G 
                               2 
                               
                                 x 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               2 
                               
                                 y 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               2 
                               
                                 z 
                                 ′ 
                               
                             
                           
                         
                         
                           
                             
                                 
                             
                           
                           
                             ⋮ 
                           
                           
                             
                                 
                             
                           
                         
                       
                       ] 
                     
                     
                       ( 
                       
                         n 
                         × 
                         3 
                       
                       ) 
                     
                   
                   = 
                   
                     
                       
                         [ 
                         
                           
                             
                               
                                 G 
                                 0 
                                 x 
                               
                             
                             
                               
                                 G 
                                 0 
                                 y 
                               
                             
                             
                               
                                 G 
                                 0 
                                 z 
                               
                             
                             
                               1 
                             
                           
                           
                             
                               
                                 G 
                                 1 
                                 x 
                               
                             
                             
                               
                                 G 
                                 1 
                                 y 
                               
                             
                             
                               
                                 G 
                                 1 
                                 z 
                               
                             
                             
                               1 
                             
                           
                           
                             
                               
                                 G 
                                 2 
                                 x 
                               
                             
                             
                               
                                 G 
                                 2 
                                 y 
                               
                             
                             
                               
                                 G 
                                 2 
                                 z 
                               
                             
                             
                               1 
                             
                           
                           
                             
                               
                                   
                               
                             
                             
                               ⋮ 
                             
                             
                               
                                   
                               
                             
                             
                               
                                   
                               
                             
                           
                         
                         ] 
                       
                       
                         ( 
                         
                           n 
                           × 
                           4 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       
                         [ 
                         
                           
                             
                               
                                 r 
                                 11 
                               
                             
                             
                               
                                 r 
                                 12 
                               
                             
                             
                               
                                 r 
                                 13 
                               
                             
                           
                           
                             
                               
                                 r 
                                 21 
                               
                             
                             
                               
                                 r 
                                 22 
                               
                             
                             
                               
                                 r 
                                 23 
                               
                             
                           
                           
                             
                               
                                 r 
                                 31 
                               
                             
                             
                               
                                 r 
                                 32 
                               
                             
                             
                               
                                 r 
                                 33 
                               
                             
                           
                           
                             
                               
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   t 
                                   x 
                                 
                               
                             
                             
                               
                                 t 
                                 y 
                               
                             
                             
                               
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   t 
                                   z 
                                 
                               
                             
                           
                         
                         ] 
                       
                       
                         
                           ( 
                           
                             4 
                             × 
                             3 
                           
                           ) 
                         
                         ⁢ 
                         
                           
 
                         
                         ⁢ 
                         
                           Rotation 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           and 
                         
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           Translation 
                           ⁢ 
                           
                             
 
                           
                           ( 
                           Unknowns 
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   EQUATION 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   1 
                 
               
             
           
         
       
     
     With a properly calibrated camera, the “ray” in 3D space along which an object must lie can be determined as follows:
 
 G   0   x′   =d   0   i   0   +C   0   x  
 
 G   0   y′   =d   0   j   0   +C   0   y  
 
 G   0   z′   =d   0   k   0   +C   0   z   EQUATION 2
 
where (i 0 , j 0 , k 0 ) is the unit vector from camera C 0  to target G 0 ′, and d 0  is the unknown distance between camera C 0  and target G 0 ′.
 
     This can be written as follows for a multi-camera system: 
     
       
         
           
             
               
                 
                   
                     
                       [ 
                       
                         
                           
                             
                               G 
                               0 
                               
                                 x 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               0 
                               
                                 y 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               0 
                               
                                 z 
                                 ′ 
                               
                             
                           
                         
                         
                           
                             
                               G 
                               1 
                               
                                 x 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               1 
                               
                                 y 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               1 
                               
                                 z 
                                 ′ 
                               
                             
                           
                         
                         
                           
                             
                               G 
                               2 
                               
                                 x 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               2 
                               
                                 y 
                                 ′ 
                               
                             
                           
                           
                             
                               G 
                               2 
                               
                                 z 
                                 ′ 
                               
                             
                           
                         
                         
                           
                             
                                 
                             
                           
                           
                             ⋮ 
                           
                           
                             
                                 
                             
                           
                         
                       
                       ] 
                     
                     
                       ( 
                       
                         n 
                         × 
                         3 
                       
                       ) 
                     
                   
                   = 
                   
                     
                       
                         
                           
                             [ 
                             
                               
                                 
                                   
                                     d 
                                     0 
                                   
                                 
                                 
                                   0 
                                 
                                 
                                   0 
                                 
                                 
                                   0 
                                 
                               
                               
                                 
                                   0 
                                 
                                 
                                   
                                     d 
                                     1 
                                   
                                 
                                 
                                   0 
                                 
                                 
                                   0 
                                 
                               
                               
                                 
                                   0 
                                 
                                 
                                   0 
                                 
                                 
                                   
                                     d 
                                     2 
                                   
                                 
                                 
                                   0 
                                 
                               
                               
                                 
                                   
                                       
                                   
                                 
                                 
                                   
                                       
                                   
                                 
                                 
                                   
                                     ⋱ 
                                     ⁢ 
                                     
                                         
                                     
                                   
                                 
                                 
                                   
                                       
                                   
                                 
                               
                             
                             ] 
                           
                           
                             ( 
                             
                               n 
                               × 
                               n 
                             
                             ) 
                           
                         
                         Unknowns 
                       
                       ⁢ 
                       
                         
                           
                             [ 
                             
                               
                                 
                                   
                                     i 
                                     0 
                                   
                                 
                                 
                                   
                                     j 
                                     0 
                                   
                                 
                                 
                                   
                                     k 
                                     0 
                                   
                                 
                               
                               
                                 
                                   
                                     i 
                                     1 
                                   
                                 
                                 
                                   
                                     j 
                                     1 
                                   
                                 
                                 
                                   
                                     k 
                                     1 
                                   
                                 
                               
                               
                                 
                                   
                                     i 
                                     2 
                                   
                                 
                                 
                                   
                                     j 
                                     2 
                                   
                                 
                                 
                                   
                                     k 
                                     2 
                                   
                                 
                               
                               
                                 
                                   
                                       
                                   
                                 
                                 
                                   ⋮ 
                                 
                                 
                                   
                                       
                                   
                                 
                               
                             
                             ] 
                           
                           
                             
                               ( 
                               
                                 n 
                                 × 
                                 3 
                               
                               ) 
                             
                             ⁢ 
                             
                               
 
                             
                           
                         
                         Known 
                       
                     
                     + 
                     
                       
                         
                           [ 
                           
                             
                               
                                 
                                   C 
                                   0 
                                   x 
                                 
                               
                               
                                 
                                   C 
                                   0 
                                   y 
                                 
                               
                               
                                 
                                   C 
                                   0 
                                   z 
                                 
                               
                             
                             
                               
                                 
                                   C 
                                   1 
                                   x 
                                 
                               
                               
                                 
                                   C 
                                   1 
                                   y 
                                 
                               
                               
                                 
                                   C 
                                   1 
                                   z 
                                 
                               
                             
                             
                               
                                 
                                   C 
                                   2 
                                   x 
                                 
                               
                               
                                 
                                   C 
                                   2 
                                   y 
                                 
                               
                               
                                 
                                   C 
                                   2 
                                   z 
                                 
                               
                             
                             
                               
                                 
                                     
                                 
                               
                               
                                 ⋮ 
                               
                               
                                 
                                     
                                 
                               
                             
                           
                           ] 
                         
                         
                           ( 
                           
                             n 
                             × 
                             3 
                           
                           ) 
                         
                       
                       Known 
                     
                   
                 
               
               
                 
                   EQUATION 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   3 
                 
               
             
           
         
       
     
     Equations 1 and 2 are set equal to one another, leading to the following equation: 
     
       
         
           
             
               
                 
                   
                     [ 
                     
                       
                         
                           
                             
                               
                                 d 
                                 0 
                               
                               ⁢ 
                               
                                 i 
                                 0 
                               
                             
                             + 
                             
                               C 
                               0 
                               x 
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 x 
                               
                               ⁢ 
                               
                                 r 
                                 11 
                               
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 y 
                               
                               ⁢ 
                               
                                 r 
                                 21 
                               
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 z 
                               
                               ⁢ 
                               
                                 r 
                                 31 
                               
                             
                             - 
                             
                               t 
                               x 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 d 
                                 0 
                               
                               ⁢ 
                               
                                 j 
                                 0 
                               
                             
                             + 
                             
                               C 
                               0 
                               y 
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 x 
                               
                               ⁢ 
                               
                                 r 
                                 12 
                               
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 y 
                               
                               ⁢ 
                               
                                 r 
                                 22 
                               
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 z 
                               
                               ⁢ 
                               
                                 r 
                                 32 
                               
                             
                             - 
                             
                               t 
                               y 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 d 
                                 0 
                               
                               ⁢ 
                               
                                 k 
                                 0 
                               
                             
                             + 
                             
                               C 
                               0 
                               z 
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 x 
                               
                               ⁢ 
                               
                                 r 
                                 13 
                               
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 y 
                               
                               ⁢ 
                               
                                 r 
                                 23 
                               
                             
                             - 
                             
                               
                                 G 
                                 0 
                                 z 
                               
                               ⁢ 
                               
                                 r 
                                 33 
                               
                             
                             - 
                             
                               t 
                               z 
                             
                           
                         
                       
                       
                         
                           
                               
                           
                         
                       
                       
                         
                           
                             
                               
                                 d 
                                 1 
                               
                               ⁢ 
                               
                                 i 
                                 1 
                               
                             
                             + 
                             
                               C 
                               1 
                               x 
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 x 
                               
                               ⁢ 
                               
                                 r 
                                 11 
                               
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 y 
                               
                               ⁢ 
                               
                                 r 
                                 21 
                               
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 z 
                               
                               ⁢ 
                               
                                 r 
                                 31 
                               
                             
                             - 
                             
                               t 
                               x 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 d 
                                 1 
                               
                               ⁢ 
                               
                                 j 
                                 1 
                               
                             
                             + 
                             
                               C 
                               1 
                               y 
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 x 
                               
                               ⁢ 
                               
                                 r 
                                 12 
                               
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 y 
                               
                               ⁢ 
                               
                                 r 
                                 22 
                               
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 z 
                               
                               ⁢ 
                               
                                 r 
                                 32 
                               
                             
                             - 
                             
                               t 
                               y 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 d 
                                 1 
                               
                               ⁢ 
                               
                                 k 
                                 1 
                               
                             
                             + 
                             
                               C 
                               1 
                               z 
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 x 
                               
                               ⁢ 
                               
                                 r 
                                 13 
                               
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 y 
                               
                               ⁢ 
                               
                                 r 
                                 23 
                               
                             
                             - 
                             
                               
                                 G 
                                 1 
                                 z 
                               
                               ⁢ 
                               
                                 r 
                                 33 
                               
                             
                             - 
                             
                               t 
                               z 
                             
                           
                         
                       
                       
                         
                           ⋮ 
                         
                       
                     
                     ] 
                   
                   = 
                   
                     [ 
                     
                       
                         
                           0 
                         
                       
                       
                         
                           0 
                         
                       
                       
                         
                           0 
                         
                       
                       
                         
                           
                               
                           
                         
                       
                       
                         
                           0 
                         
                       
                       
                         
                           0 
                         
                       
                       
                         
                           0 
                         
                       
                     
                     ] 
                   
                 
               
               
                 
                   EQUATION 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   4 
                 
               
             
           
         
       
     
     Equation 4 can be rearranged into the form Ax=b, where A and b are known, and the vector x is unknown. Also, let G m,n  be the n th  target observed by camera m. 
     
       
         
           
             
               
                 
                   
                     
                       [ 
                       
                         
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               i 
                               00 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                                 
                             
                           
                         
                         
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             
                               j 
                               00 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                                 
                             
                           
                         
                         
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 00 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             
                               k 
                               00 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             … 
                           
                         
                         
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               i 
                               01 
                             
                           
                           
                             0 
                           
                           
                             
                                 
                             
                           
                         
                         
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               j 
                               01 
                             
                           
                           
                             0 
                           
                           
                             
                                 
                             
                           
                         
                         
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 01 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             
                               k 
                               01 
                             
                           
                           
                             0 
                           
                           
                             
                                 
                             
                           
                         
                         
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             ⋮ 
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                         
                         
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               i 
                               10 
                             
                           
                           
                             0 
                           
                         
                         
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               j 
                               10 
                             
                           
                           
                             0 
                           
                         
                         
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 10 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               k 
                               10 
                             
                           
                           
                             0 
                           
                         
                         
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               i 
                               11 
                             
                           
                         
                         
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               j 
                               11 
                             
                           
                         
                         
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 x 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 y 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 G 
                                 11 
                                 z 
                               
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               - 
                               1 
                             
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             0 
                           
                           
                             
                               k 
                               11 
                             
                           
                         
                         
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             ⋮ 
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                           
                             
                                 
                             
                           
                         
                       
                       ] 
                     
                     ⁢ 
                     
                       
 
                     
                     [ 
                     
                       
                         
                           
                             r 
                             11 
                           
                         
                       
                       
                         
                           
                             r 
                             12 
                           
                         
                       
                       
                         
                           
                             r 
                             13 
                           
                         
                       
                       
                         
                           
                             r 
                             21 
                           
                         
                       
                       
                         
                           
                             r 
                             22 
                           
                         
                       
                       
                         
                           
                             r 
                             23 
                           
                         
                       
                       
                         
                           
                             r 
                             31 
                           
                         
                       
                       
                         
                           
                             r 
                             32 
                           
                         
                       
                       
                         
                           
                             r 
                             33 
                           
                         
                       
                       
                         
                           
                             t 
                             x 
                           
                         
                       
                       
                         
                           
                             t 
                             y 
                           
                         
                       
                       
                         
                           
                             t 
                             z 
                           
                         
                       
                       
                         
                           
                             d 
                             00 
                           
                         
                       
                       
                         
                           
                             d 
                             01 
                           
                         
                       
                       
                         
                           ⋮ 
                         
                       
                       
                         
                           
                             d 
                             10 
                           
                         
                       
                       
                         
                           
                             d 
                             11 
                           
                         
                       
                       
                         
                           ⋮ 
                         
                       
                     
                     ] 
                   
                   = 
                   
                     [ 
                     
                       
                         
                           
                             - 
                             
                               C 
                               0 
                               x 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               0 
                               y 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               0 
                               z 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               0 
                               x 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               0 
                               y 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               0 
                               z 
                             
                           
                         
                       
                       
                         
                           ⋮ 
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               1 
                               x 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               1 
                               y 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               1 
                               z 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               1 
                               x 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               1 
                               y 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               C 
                               1 
                               z 
                             
                           
                         
                       
                       
                         
                           ⋮ 
                         
                       
                     
                     ] 
                   
                 
               
               
                 
                   EQUATION 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   5 
                 
               
             
           
         
       
     
     Thus, one way to determine the solution is to solve the linear matrix equation Ax=b. Utilizing MATLAB®, this vector can be calculated as x=pinv(A)*b. The pinv( ) function is the pseudo-inverse, which is based on singular value decomposition and linear least-squares analysis. Although the Ax=b solution does not obtain a solution for a single camera, it does provide a correct answer given a perfect camera model in simulations. However, this algorithm is sensitive to Gaussian noise added to the target locations. 
     An alternative equation of the form Ax(t)=b can be developed, where x( ) becomes a function of the shorter vector t of unknowns, wherein x( ) returns the longer vector x. The vector t contains the unknown three Euler angles, three translations, and the distances to the targets. The vector x is identical to t, except that the three Euler angles are replaced with the equivalent nine elements of a standard rotation matrix. If this matrix equation is set to zero, MATLAB&#39;s non-linear least-squares algorithm fsolve( ) can be utilized to iterate over t and converge upon the best solution Ax(t′)−b=0, to solve for the unknowns (three Euler angles, three translations, and the distances to the targets). It will be understood that numerous other solvers/algorithms could be utilized to solve this equation, and the fsolve( ) algorithm is merely an example of a suitable approach. The resulting (nearly zero) vector e=Ax(t′)−b represents a residual error. Although an iterative process is preferably utilized to converge upon the best solution, criteria for an acceptable (i.e., not “best”) solution for vector e can be determined, and an acceptable solution for the equation e=Ax(t′)−b can be utilized. In 3D, these errors are the Cartesian distances between a rotated/translated target and its position along a projected line-of-sight from the camera. 
     Referring to  FIG. 4 , the original (known) target locations are designated “P,” and the new target locations are designated “P′.” The rigid body rotation is designated R, and the rigid body translation is designated T. The distances are designated D, and the unit vectors are designated U. The calibrated first camera position is designated C 1 , and the second calibrated camera position is designated C 2 . In  FIG. 4 , the residuals are designated e 1  and e 2 . The non-linear least-squares algorithm fsolve( ) minimizes the residuals e 1 , e 2 , etc. 
     As discussed above, each camera&#39;s calibration identifies the coefficients of a polynomial describing its wide-angle lens&#39; distortion, and the camera&#39;s precise location and orientation in the vehicle coordinate frame  11  ( FIG. 1 ) is determined through an implementation of the “resection” equations discussed above. The resection is accomplished by associating “dots” in an image with entries from a database of known target locations. Hereafter, when discussing this pairing, a “dot” is a target observed by a camera, or, more specifically, an object in the camera&#39;s field of view. The image processor “thinks” is a target. A “target” is an entry from a database of known targets. 
     A software program implementing this process calculates the camera position and orientation in three iterations. The first iteration involves only a few dot-target pairings that a user selects manually. This first set does resect the camera, but a higher confidence calculation requires more targets. The second iteration of the program provides a higher confidence calculation. In the second iteration, an automatic pairing function selects dot-target pairs, then recomputes the resection. The automatic pairing function selects all the targets it can reasonably match according to predefined criteria. The third iteration throws out the worst of those pairings based on the individual dot-target residuals. The residual threshold for throwing out dot-target pairs is the mean value of the second iteration&#39;s residuals. The resection program&#39;s flow is outlined as follows: 
     Camera Resection Program
         open target database   open camera information file with initial camera location guess   open image from which to derive camera location   run extract dots   limit database scope to relevant targets   manually pair 6 or more dots with targets
           calculate resection equation   product: rough camera position   
           auto pair dots with targets using rough camera position   calculate resection equation with new dot-target pairs
           product: refined camera position   
           cull dot-target pairs with high residuals   calculate resection equation with remaining dot-target pairs
           product: final camera position   
               

     The extract dots function forms the basis of both the resection and relative orientation algorithms in that it processes a raw image taken from a camera and extracts dot positions. The particular steps are as follows: 
     Extract Dots
         store input image in buffer   square root equalize (to improve contrast)   local threshold to create binary image (threshold extracts objects brighter than the local background)   remove objects from image edge   remove small objects by area filter   remove objects by compactness (compactness=area/(bounding box width·bounding box height)   remove non-ellipsoids (targets are all ellipsoids)   remove objects with holes   dilate objects   store dilated binary image as mask   retrieve original image and apply mask (thus only dots are shown)   sharpen edges with median filter (eliminates stray pixels, without removing contours)   local threshold (same as above)   centroid (x and y center of mass)       

     The auto pair function automatically pairs each dot in an image&#39;s data structure with a database target (if there is one). First, it creates a set of unit vectors pointing from the camera to each target in the database. It then calculates the angular distance between each dot&#39;s unit vector derived utilizing the camera calibration described above, and all the unit vectors just created from the target database. The target whose unit vector most closely aligns is paired with that dot. A user-defined threshold avoids dot-target pairings with large angular differences. 
     Although not applicable for the resection program, if a dot already has a target pairing, the auto pair simply checks to ensure that the dot&#39;s unit vector is aligned within the user-defined threshold. If the angular difference is too large, it finds a closer target to pair with. Another point, although it is not a factor for the resection program, when auto pair is used for relative orientation calculations, the locations of the targets is their last known location, which is derived from the previous image set. The dot-target automatic pairing process is shown in  FIG. 5 . The unit vectors from the camera C 1  to the target locations T 1 , T 2 , T 3 , T 4 , etc. at the last iteration are designated U. The rotation and translation from the last iteration is shown schematically by arrow “A.” The dot-target automatic pairing process pairs a dot  28  with a target T 2  having the smallest angular difference  29 . 
     From a performance standpoint, auto pair is approximately an O(n 2 ) function, which is a significant performance factor effecting the relative orientation calculation. The automatic nature of the relative orientation program is due in large part to auto pair. Manually pairing dots with targets may be extremely tedious and error prone. 
     The computer code utilized to calculate relative orientation is similar in concept to the resection code described above, and a significant amount of the resection code is used in the relative orientation program. The significant difference, however, is tracking dots from one frame to the next, and ensuring those dots are associated with the correct database targets. An overview of the relative orientation program is as follows: 
     Relative Orientation Program
         open target database   open camera information file with calculated camera locations   open ordered image data sets (one for each camera)   for each image
           in parallel, do the following for each camera
               extract dots from the current image   create data structure to hold dots   using track dots, determine which dots from previous image appear in the current image   preserve only persistent dots   update current data structure with previous image&#39;s data structure (i.e. preserve the last dot-target pairing if possible)   run auto pair to find dot-target pairings for non-updated dots   bundle dot-target pairs for relative orientation calculation   
               calculated relative orientation equation with initial pairings
               product: initial relative orientation   
               cull dot-target pairs with high residuals   calculate relative orientation equation with remaining pairings
               product: final relative orientation   
               output final orientation to file   
               

     The relative orientation program&#39;s extract dots and auto pair functions are exactly the same as those used for camera resection. The relative orientation program and the data germane to the relative orientation calculation are stored in the data structure. Each dot is given a unique identification (ID) number, which is passed from iteration to iteration as long as the dot persists from one frame to the next. All the other data associated with that dot is then added to the data structure during program execution. As new dots are found, new data structure elements are created. 
     The track dots function takes the coordinates of a dot in the current image, and tries to find a dot in the previous image with close to the same coordinates. If it can find one within a user-defined radius, it associates the old dot&#39;s ID number (and hence that dot&#39;s target pairing) with a new dot, and increments the persistence value. If one is not found, the function creates a new data structure with a new ID number for that dot. 
     The persistent dot function filters through the data structure collections, passing only those dots to the remainder of the program that have persisted through a user-defined number of images. 
     As indicated above, in the relative orientation program, the data from all the cameras is bundled together and sent to the relative orientation function. The function populates the equations with the dot-target pairs and camera information and calculates the relative orientation of the first and second bodies  10  and  20 . As discussed above, the equations provide residuals, and these residuals are inserted back into the data structures of the dot-target pairs. Additionally, relative orientation calculates residual statistics, and recommends a residual threshold. The threshold is set as the residual mean plus a user-defined number of standard deviations. Intrinsically, this assumes normality of the residual distribution (not necessarily true), but it functionally allows the program to cull those dot-target pairs that contribute the highest residuals to the solution. (After calculating a new relative orientation solution, that rotation-translation solution is applied to the original target database, so auto pair can automatically select dot-target pairings in the next iteration). 
     The cull function goes through the bundled data structures, and removes dot-target pairs with residuals above relative orientation&#39;s statistically-derived maximum threshold. In practice, this is very effective at eliminating dot-target pairs that contribute the most error to the overall orientation solution. The targets most often eliminated are those in the peripheral of the camera&#39;s field-of-view, the lens region with the most distortion. 
     While the relative orientation program executes, no user supervision is required. This result stems from the core function&#39;s various methods to verify dot-target pair validity, primarily from a stochastic perspective. All the functions also output robust metrics, which allow exceptional algorithmic observability. The following list illustrates the particular statistic-based sections of the code and their observable metrics that cumulatively contribute to a robust, accurate, and automatic relative-orientation calculation:
         extract dots finds dots by looking for round, bright image segments: visual evaluation is the metric confirming extracted dots are actually targets;   persistent dots associates dots from frame to frame: observability metric is the statistics on frame-to-frame dot movement, and each dot&#39;s persistence count;   auto pair uses the known target locations from the previous image and a) rechecks existing pairings against an alignment threshold, and b) finds new pairings if needed: the evaluation metric is the average angular alignment of a dot-based unit vector with a camera-to-target unit vector, (which is a slightly skewed metric, because it is based on the previous image&#39;s target position); and   cull deletes dot-target pairs contributing the most error to the final solution: evaluation metric is the mean and standard deviation of the second relative orientation calculation&#39;s residuals.       

     An architecture of a high speed camera system  50  according to one aspect of the present invention is illustrated in  FIG. 6 . The camera system  50  illustrates a camera system utilized with first and second bodies  10  and  20  including a crew module and a forward faring, respectively. The camera system  50  includes three recording systems  51 ,  52 , and  53  running in parallel. The camera system  50  includes three cameras  54 ,  55 , and  56  that are mounted to the second body  20  ( FIG. 1 ). Cameras  54 - 56  are not utilized to determine relative position between first and second bodies  10  and  20 , but rather are utilized to collect other data relative to a launch-abort system. Upon separation of second body  20  from first body  10 , the cameras  54 - 56  are disconnected along line  57 . The camera system  50  of  FIG. 6  includes four cameras  1 - 4  that are utilized to determine target locations as described in more detail above. LED light rings  58 - 61  may be utilized to illuminate the targets in second body  20 . The LED light rings  58 - 61  are operably connected to a power bus  70 , and voltage regulators  62 - 65  regulate the voltage supplied to the LED light rings  58 - 61 . A camera  67  is utilized to record data concerning separation of the rigid bodies  10  and  20 , but the camera  67  is not utilized to determine the relative orientation of the bodies  10  and  20 . The cameras  1 - 4 ,  54 - 56 , and  67  are also operably connected to power bus  70 . The cameras  1 - 4 ,  54 - 56 , and  67  and LED light rings  58 - 61  are controlled by switches  66 . 
     The camera system  50  also includes first, second, and third camera controllers  72 ,  73 , and  74 , respectively. Camera controller  72  includes a master clock synchronization unit  75 , and camera controller  73  includes a master frame synchronization feature  76 . Camera controller  74  includes a clock synchronization feature  77 . The cameras  54 - 56  are operably connected to the camera controllers  72 - 74  by CL2 fiber units  78 - 83 . The camera system  50  also includes solid state drive arrays (SSDAs)  84 ,  85 , and  86 , which are contained within a waterproof enclosure  87 . The SSDAs  84 - 86  are operably connected to the power bus  70  by voltage regulators  88 - 89  and switches  91 - 93 , respectively. An SSDA temperature monitor  94  is utilized to monitor the temperature of the SSDAs  84 - 86 , and a pressure monitor  95  is utilized to monitor the pressure within waterproof enclosure  87 . A pressurization port  96  is utilized to pressurize the waterproof enclosure  87 . An encoder  97  and a camera PSU  98  are operably connected to the camera controllers  72 - 74 . A switch  99  is operably connected to the camera controllers  72 - 74 , and disconnects along a line  100 . The switch  99  is connected to a ground support engineering (GSE) computer system  102  by an Ethernet  101 , and the GSE system  102  is operably connected to a 120-volt AC power supply  103 . 
     In the examples described above, the various algorithms and equations can be implemented in MATLAB® and LabView®. The system illustrated in  FIG. 6  records the camera imaging data during flight of the crew module and forward faring, and the equations and algorithms described in more detail above are utilized as a post-processing tool utilizing stored data. However, the algorithms and equations may be coded in other languages, such as C++, and implemented utilizing a field programmable gate array (FPGA) device to provide real-time calculations concerning the relative positions of the objects. 
     It is to be understood that variations and modifications can be made on the aforementioned structure without departing from the concepts of the present invention, and further it is to be understood that such concepts are intended to be covered by the following claims unless these claims by their language expressly state otherwise.