CN1199054C - System for determining spatial position and orientation of body - Google Patents

Abstract

A system for determining the spatial position and orientation of each of a plurality of bodies. Each one of the bodies has at least three markers in a predetermined, relative geometric relationship. The markers are adapted to emit energy in response to an activation signal and/or to reflect energy impinging upon such passive marker from an activatable energy source. A common energy detector is provided for detecting the energy emitted by the active marker and the energy reflected by the passive marker. A common processor is provided. The processor has a memory. The memory has stored therein the predetermined, relative geometric relation of the markers for each one of the bodies. The processor compares the stored predetermined geometric relation of the markers for each of the bodies with the energy detected by the energy detector to identify the bodies emitting, or reflecting the detected energy. With such an arrangement, a body can be tracked using a very simple sequence of operation in real-time with robust positive marker identification by taking advantage of a simple marker placement methodology. Multiple bodies can thereby be tracked simultaneously.

Description

Systems for determining the spatial position and orientation of objects

Background technique

The present invention generally relates to a system for determining the spatial position and angular orientation (ie pose) of an object or object.

As is known in the art, there are systems that determine the spatial position and angular orientation of an object (or objects). One such system includes a passive retroreflector as a point marker or measurement target attached to the object, while a second system includes an active radiation source as an additional point marker. Both techniques operate by projecting high-contrast marker images onto spaced sensors and using mathematical processing to determine the 3D coordinates of each point marker. These three-dimensional coordinates are then used as discrete points, or can be considered as a set if their geometric arrangement is known, which leads to the determination of the position and angular orientation of the object in space with respect to the three-dimensional coordinate system (i.e., 6 degrees of freedom: x, y and z and pitch, deflection and rotational angle orientation), where the coordinate system is centered at a predetermined point in space, typically at a point fixed with respect to the sensor.

Determining the spatial position and vector angle or angular orientation of an object has several uses. For example, a pointing device can recognize an object when the terminal of the pointing device is at a known location relative to a marker. This pointing device can be used as a handheld digital pointing device in reverse engineering applications. The operator moves the pointing device to various known locations on a manufactured part, and the accuracy of the manufacturing process is determined from the analysis of the determined end positions of the pointing device. This application requires a highly accurate system.

In another application, such as image-guided surgery, the state of the surgical instrument is tracked with respect to the patient. Certain surgical instruments have markings affixed to them. This information can be used to allow the surgeon to see where the instrument is pointed on the MR or CT scanner, as well as what is behind the terminal end of the surgical instrument. This application also requires a highly accurate system.

In an emissive tag (ie, active tag) system, multiple charge-coupled device (CCD) sensors are used to measure the energy emitted by the tag. A dot marker is energized to emit infrared energy every sensor cycle. During each sensor cycle, the emitted energy focused on the sensor is collected (ie, combined) and moved to the sensor processing circuitry. In order to determine the 3D position of a marker, the marker must be measured in at least three sensor axes (ie comprising at least 3 orthogonal planes). There are many advantages to a system using emissive markers, including the ability to produce high contrast images on the sensor, control of the excitation of each marker to provide accurate and automatic marker recognition, and the ability to use high speed linear sensors. High-speed linear sensors are relatively expensive and can only track one marker in a single sensor cycle.

In one type of retroreflective marking (ie, passive marking) system, an energy source is energized to emit infrared energy in the general orientation of the retroreflective marking. Then, multiple CCD sensors are used to measure the energy reflected by the markers. During each sensor cycle, the reflected energy focused on the sensor is collected (ie, focused) together and transferred to the sensor processing circuitry. In order to determine the 3D position of a marker, the marker must be measured in at least three sensor axes (ie including a minimum of 3 orthogonal planes). There are many advantages to a retroreflective marker system, including the use of radio markers, and the ability to use inexpensive low speed sectored array sensors. However, these systems suffer from problems with effectively identifying the marks.

It is desirable to use an inexpensive partitioned array sensor capable of tracking multiple markers in a single sensor cycle. As is known in the art, there are some systems that utilize a single sectorized array sensor and inexpensive components. DeMenthon (Patent No. 5,227,985) discloses a system that uses a single sensor and matrix technology to determine the pose of an object. Such systems are limited to non-coplanar markers and are based on projective methods to extract 6D information from 2D images. This method is not accurate enough for medical applications. As is known in the prior art, with systems of this type the errors in the depth measurement are prohibitively large. For depth accuracy, triangulation has clear advantages over projective methods. Triangulation, also known as stereometry, has been resisted due to the expensive hardware required to perform real-time calculations. Multiple marker triangulation by zoned array sensors additionally does not suffer from poor marker recognition, which is typically resolved through human intervention. Previous systems worked poorly in the presence of stray IR sources and real conditions resulting in undesirable and undesired mark reflections. Previous systems could also work poorly in the presence of multiple objects in close proximity to each other.

Summary of the invention

According to the present invention, there is provided a system for determining the spatial position and orientation of each of a plurality of objects. Each object has at least three markers with predetermined relative geometric relationships. The marker is adapted to emit energy in response to an excitation signal and/or reflected energy impinging on such a passive marker from an energizable energy source. A common energy detector is provided for measuring the energy emitted by the stimulated markers and the energy reflected by the passive markers. A common processor is provided. The processor has memory. Predetermined relative geometric relationships of the markers of each object are stored in the memory. The processor compares its stored predetermined geometry of signatures about objects with the energy measured by the energy detectors to identify objects that emit/reflect the measured energy.

With such an arrangement, objects can be tracked in real time with a very simple sequence of operations through robust positive marker recognition using a single marker placement method. Thus, multiple objects can be tracked simultaneously.

According to another feature of the invention, each object must have markers, which have a known and fixed relative geometric relationship, and must have a unique line segment length among all marker pairs, where the word unique refers to the threshold difference, This value is based on the accuracy of the system (ie the difference in the geometry of the object's markers is measurable). Multiple objects can be tracked simultaneously if similar segment pairs have unique relative angles among all objects being tracked, where still, the only word refers to the threshold difference that builds on the accuracy of the system. Marker geometry can be collinear or coplanar, depending on the needs of the application.

Additionally, by such an arrangement, the system is set up to track poses of 3 or more labeled objects with known relative geometric relationships subject to a single localization rule, which is not limited to being non-coplanar or non-collinear. The system can use an inexpensive, low-speed partitioned array sensor that can track multiple markers in a single sensor cycle, thereby increasing the sampling rate at which each marker is present.

According to another aspect of the invention, the system uses a stereoscopic arrangement of sensors, thereby providing suitable accuracy for high performance applications such as surgical applications.

In addition, the system can use low-cost digital signal processors and make the processing calculation steps simple, it will automatically and deterministically identify discontinuous marks in the three-dimensional (3D) of the object, and in the presence of many false marks and multiple Works in the case of objects that are close to each other. Such a system is suitable for determining the pose of one or more objects in real time in a closed form solution using a single sensor-period image, rather than using a predictive approach, to continuously track the pose of an object given several sensor-period images .

Additionally, such systems are suitable for automatic recognition and tracking of various objects known prior to tracking.

Figure overview

Other features of the invention will become more readily apparent with reference to the following description in connection with the accompanying drawings, in which are:

1 is a block diagram of a system for determining the spatial position and orientation of a pair of rigid bodies according to the present invention;

Figure 2 is a diagram showing a pair of objects suitable for use in the system of Figure 1;

3 is a diagram of a graph stored in memory of a processor used in the system of FIG. 1;

Figures 4, 5, 6, 6A-6B, 7, 7A-7C, 8 and 8A-8B are flowcharts illustrating the sequence of operation of the system of Figure 1, wherein Figure 7 shows the relationship between Figures 7A-7C , while Figure 8 shows the relationship between Figures 8A and 8B;

9A, 9B through 18 illustrate detailed examples of elements in the memory of FIG. 3 as various stages of the system of FIG. 1 are executed.

Independent Description of the Preferred Embodiment

Referring now to FIG. 1, a system 10 is provided for determining the spatial position and orientation of one or more (here a pair of rigid bodies 11a, 11b) rigid bodies. Here, the rigid bodies 11a, 11b shown and described in more detail in FIG. 2 are different surgical instruments. Here, the rigid body 11a has multiple, here, two passive retroreflective markers 12a, 12c, and one active emitting marker 12b, and all three markers are pasted on the rigid body 11a. Here, each energy retroreflective marker 12a, 12c comprises a sphere that can be glued to the object 11a and covered with retroreflective material available and known in the art. The marks 12a, 12b, 12c are pasted on the object 11a in a predetermined fixed relative geometric relationship. The predetermined fixed relative geometry is determined by simple positioning as described below.

Referring again to FIG. 1, the system 10 includes a common energy measurement system 14 for measuring the energy emitted by the active marker 12b affixed to the object 11a, and the energy reflected by the passive markers 12a, 12c affixed to the object 11a. Common gauge system 14 includes a pair of spaced, left-mounted and right-mounted sensor devices 14L and 14R, respectively. Each sensor device 14L, 14R comprises, respectively: a two-dimensional charge-coupled device (CCD) sensor 18L, 18R (FIG. 1); a focusing lens 22L, 22R, as shown; and a plurality of optical energy emitting sources 24L, 24R (here , is an infrared energy emitting diode) as shown in the figure.

Each sensor assembly 14R and 14L has its own u, v, z _s coordinates aligned with its associated directional infrared energy source 24L, 24R, respectively. The light emitting sources 24L, 24R are evenly distributed on the circumference of each sensor device 14R and 14L z _s axis. Through the processor portion 26, electrical power is given to the plurality of light emitting sources 24L, 24R. The processor portion 26 includes a processor 28 , a host 30 , a display 32 and a controller 34 . Processor 28 imparts energy to light emitting sources 24L, 24R via a signal on line 36 . A plurality of light emitting sources 24L, 24R operate to produce an incident oriented beam of infrared energy whose propagating orientation is generally aligned to correspond to an orientation axis z _s of each sensor device 14L, 14R associated with the directional energy source 24L, 24R. The size, shape and intensity of the incident directed energy produced by the directed infrared energy source corresponds to the field of view of the measurement volume of its associated sensor device 14L, 14R and is sufficient to provide an incident directed energy beam on the measurement volume.

Each sensor arrangement 14L, 14R is capable of producing output signals on lines 39L, 39R which are representative of the energy intensity focused on the sensor assembly. Energy focused on it is harvested (ie aggregated) and transferred to processor 28 during each sensor cycle. Here, the sensor devices 14L, 14R are mounted to a fixed coordinate system and separated from each other by a predetermined distance, here 500 mm. Here, sensor devices 14L, 14R each have a field of view sufficient to view a common measurement volume of approximately 1 cubic meter, the center of which is approximately 1.9 m from the origin along the _zs axis, which is midway between lenses 22L and 22R.

As mentioned above, each sensor device 14L and 14R has its own associated lens 22L, 22R, respectively, for focusing the reflected energy from the energy retroreflective markers 12a and 12c and the transmitted energy from the energy emitting marker 12b to Energy images of emitted or reflected energy from indicia 12a, 12b, 12c are produced on lenses 22L, 22R associated with sensor mounts 14R, 14L, respectively.

Processor 28 is coupled to sensor assemblies 14L and 14R and determines the two-dimensional u, v position of the focused energy image on each sensor assembly 14L and 14R. The u,v positions of the focused energy images of the same markers 12a, 12b, 12c on each sensor mount 14L, 14R are then used to generate left and right sensor energy source position tables 50L, 50R (FIGS. 3 and 9A, 9B) , and set left-source and right-source counters 51L, 51R, which will be described below.

The processor 28 is coupled to a host computer 30 such that the spatial position of the objects 11 a, 11 b is displayed on a display 32 or further processed by the host computer 30 . As mentioned above, processor 28 is coupled to directed energy sources 24L, 24R to enable processing portion 26 to activate directed energy sources 24R and 24L at appropriate times. And the processor 28 is coupled to the controller 34 so that the processor 28 can signal the controller 34 to activate the energy emitting marker 12b through the line 27 when needed.

The operation of system 10 will be described below for object 11a, it being understood that the sequence of operations is substantially the same or equivalent for other rigid bodies, such as object 11b. Thus, as shown, active marker 12b is fed to controller 34 via cable 27 (FIG. 1). Here, as mentioned above, the energy emitting marker 12b includes an infrared energy emitting diode of the marker 12b, which emits infrared light energy when given electrical power fed by the controller 34 through the cable 27. Such infrared energy emitting diodes are commercially available and known in the art.

Referring now to FIG. 2 in more detail, rigid bodies 11a and 11b have markings 12a-12c and 12d-12f affixed to them, respectively. It should first be understood that it does not matter that markers 12a and 2c are retroreflective and that marker 12b is active. This configuration is just an example, and the method described below does not depend on the type of tag. Each object 11a, 11b has markings 12a, 12b, 12c and 12d, 12e, 12f respectively affixed thereto in a predetermined (ie known) and fixed relative geometric relationship. In addition, the relative geometry of the markings 12a, 12b and 12c must be detectably different from the markings 12c, 12d and 12f of the object 11b. Thus, as shown in FIG. 2, the markings 12a, 12b and 12c of the object 11a are separated by the line segments SLab, SLbc and Slac, respectively. Also, as shown, line segments SLab, SLbc and Slac intersect to form angles θab, θbc and θac. Similarly, the markings 12d, 12e and 12f of the object 11b are separated by the line segments SLde, SLef and Sldf, respectively. Also, as shown, line segments SLde, SLef and Sldf intersect to form angles θde, θef and θdf. In addition, the line segment length SLab must be different from the line segment length Slac and the line segment length SLbc, and the line segment length SLbc must be different from the line segment Slac. In the preferred embodiment, the variation Δ is 5.0 mm. Thus, if the length of line segment SLab is SLab, the length of line segment SLbc is at least SLab±Δ, and the length of line segment Slac is at least SLab±Δ. That is, the lengths of the line segments SLab, SLbc, and Slac must differ by Δ. Also, an object with 3 markers will have 3 line segments. However, in general, the number of line segments is equal to N*(N-1)/2, where N is the total number of markers. The object 11a has a pair of line segments SLab, SLbc, whose length is equal to the pair of line segments Slde, Slef on the object 11b; if the relative angle θab between the line segments SLab, SLbc on the object 11a is different from the line segments Slde, Slef on the object 11b relative angle θde between them, and still be able to track them. Marker relations can be on-line, off-line, coplanar or non-coplanar, depending on the needs of the application. A pair of objects 11a, 11b can be tracked simultaneously if the same pair of line segments being tracked in objects 11a, 11b have a unique relative angle, where the word unique still refers to a threshold difference based on the same accuracy of 10. That is, placing markers 12a - 12c , 12d - 12f on objects 11a , 11b respectively, provides each object 11a and 11b with a unique signature or fingerprint that can be recognized and tracked by processor 28 .

Before discussing the operation of system 10, it should first be noted that processor 28 has memory 40 (FIG. 3) which stores three sets of tables, 42-48; 50L, 50R, and 52; and 56-62. The first group of tables (i.e. rigid body determination tables 42-48) defines the predetermined geometric relationship of the marks 12a-12d, 12e-12f of each object 11a, 11b; the second group of tables (i.e. the data tables generated by the sensors, 50L, 50R, and 52) hold the information generated when each sensor 14L, 14R scans, so these tables 50L, 50R, and 52 are not related to specific objects; The third set of tables generated by the processor (ie, processing tables 56-62). These tables (and counters 51L, 51R and 53, described below) are within processor 28 and are used by processor 28 in its operating programs.

Rigid body definition table 42-48

The rigid body definition table includes: a marker position table 42 ; a marker segment table 44 ; a marker segment point (·) generation table 46 and a marker segment group table 48 . These rigid body definition tables, 42-48, are for all objects 11a, 11b and contain known information about the geometrical relationship of the marks 12a-12c and 12d-12f attached to the rigid bodies 11a, 11b respectively, Each object 11a, 11b is thereby provided with a unique signature or fingerprint that can be identified and tracked by the processor 28 . These rigid body definition tables 42-48 are initialized prior to initiation of identification by processor 28 and subsequent tracking operations.

Marker Position Table 42

Each rigid body 11a, 11b has its marker position table 42, as shown for object 11a in FIG. The marker position table 42 contains the 3D position (X', Y', Z') of each marker 12a, 12b and 12c in relation to, for example, the rigid body 11a. Referring to Figure 11, a marker position table 42 is shown for object 11a, it being understood that table 42 has a similar table for object 11b. The figure shows the 3D position of the markers 12a, 12b and 12c.

Segment Length Table, 44

Each rigid body 11a and 11b has associated therewith a marked segment length table 44 (FIG. 12) which contains the set of segment lengths for the objects 11a, 11b. A line segment is considered to be a straight line connecting a pair of marks 12a, 12b, 12c of the object 11a and marks 12d, 12e, and 12f of the object 11b. Thus, as described above in connection with FIG. 2 , object 11 a has line segments SLab, SLbc and SLac, while object 11 b has line segments SLde, Slef and SLdf. All line segments of an object are all combinations of each pair of markers. Thus, there are N*(N-1)/2 line segments for the object, where N is the number of markers pasted to the object.

Referring to Fig. 12, a marker segment length table 44 is shown for object 11a. The figure shows the segment lengths SLab, SLbc and SLac.

marker segment group table, 48

Each rigid body 11a, 11b has associated therewith a marker segment set table 48 (FIG. 13), which contains marker segment groups. There is one entry in Table 48 for each flag. Each marker entry will contain 2 or more line segments connected to that marker. For an object with N markers, there will be N-1 line segments connected to each marker. Fig. 13 shows a group of line segments of the rigid body 11a. Each marker 12a, 12b, 12c has its associated 2 segments (ie, segment 1 and segment 2, in Figure 13). Thus, as indicated in Figures 2 and 13, for object 11a, label 12a is attached to line segments SLab and SLac; label 12b is attached to line segments SLab and SLbc. Paste marker 12c to line segments SLac and SLbc. It should be understood that there is a similar table for the label 11b.

Segment points ( ) and tables, 46

Each rigid body has associated with it a line segment point (·) product table 46 (Fig. 14) which contains a list of the point (·) products produced between each combination of line segments. When the line segment is regarded as a vector for transforming the X, Y, X system 10 coordinate system, the angle θ between the point (·) and the length SL used to measure the line segment, when N is the number of line segments in the rigid body, there will be N *A combination of (N-1)/2 segment pairs. Example Figure 14 shows a set of dot (·) products for object 11a. Here, the dot (•) product of b for the angle θa,b between the line segment lengths SLab and SLbc is shown to be 3600. Likewise, the dot (·) product of the angle θa, c between the line segment lengths SLab and SLac is 0, while the dot (·) product of the angle θb, c between the line segment lengths SLbc and SLac is shown to be 2500. It should be appreciated that a similar table exists for object 11b.

Sensor Generation Data Sheet, 50L, 50R and 52

Sensor generated data tables 50L, 50R, and 52 include: left and right sensor energy source location tables 50L, 50R; and raw 3D marker table 52 .

Left and right sensor energy source meters 50L, 50R

In memory 40 there is one sensor energy source table 50L and 50R for each sectored array CCD sensor 18L, 18R (FIGS. 1, 9A, B). There is one entry for each energy point measured on the CCD sensor 18L, 18R. The left source and right source counters 51L, 51R respectively contain the number of energy points measured on the left and right sensors 18L, 18R, respectively. Each entry will have a U value and a V value corresponding to the energy point's centroid along the U and V axes of the associated sensor 18L, 18R. In a preferred embodiment, there will be left and right sensor energy source meters 50L, 50R (FIG. 3). Here, there are four energy sources S ₁ -S ₄ , S ₅ -S ₈ measured by each sensor 18L, 18R, respectively, as indicated in Figures 9A and 9B. Note that the energy sources S ₁ -S ₄ , S ₅ -S ₈ are in the u,v coordinates of the sensors 18L, 18R, as indicated in Figures 9A and 9B.

Original 3D Markup Sheet 52

In the memory 40 there is a single raw 3D marker table 52 (Fig. 3, 10) which contains every determined but failed marker position (old marker). Each entry has an X, Y, and Z (ie, coordinate system of X, Y, Z system 10 ) value corresponding to the coordinate system of the position sensor, whose origin is at the midpoint between the image sensors 18L, 18R. Protomarker counter 53 contains the number of protomarkers measured. Referring to Fig. 10, an example for 4 proto-markers is given. In this example, the markers 12a, 12b and 12c of the object 11a are measured, and one marker whose deviation is unknown. At this time, the assignment of these markings R1-R4 to the markings 12a-12c and 12d-12f of the objects 11a and 11b, respectively, is not known. The correspondence of these markers to the object 11a or object 11b will be determined using a series of operations.

Processing Forms 56-62

The processing tables are: a line segment original marker comparison table 56; a qualified line segment table 58; a measured marker position table 60; and a calculated rigid body position and orientation (posture) table 62. These processing tables 56-62 are formulated by the processor 28 for each rigid body 11a, 11b and are generated by the processor 28 when recognizing (identifying) and tracking the rigid body.

Line Segment Original Label Comparison Table, 56

Each rigid body has associated with it a segment proto-label lookup table 56 (Figs. 3, 15) which contains all pairs of proto-labels 12a, 12b and 12c separated by a distance close to the segment length defined by the rigid body. A proximity term is defined as the difference in length between the line segment and the line segment under test, which is less than some predetermined value (ie, a distance capable of being measured by the system 10). In a preferred embodiment, this value is 1.5mm. The following example (FIG. 15) illustrates a marker pair matching the predetermined line segment lengths SLab, SLbc, SLac of the object 11a. The raw tag pair data is measured by processor 28 according to a method which will be described in conjunction with FIG. 6 . However, it is sufficient to say that, in this embodiment, two sets of 4 original energy data S ₁ _ S ₈ measured by the left and right sensors 14L, 14R ( FIG. 9A , FIG. 9B ) are converted by the processor 28 into 4 proto-markers R1-R4 (in the X, Y, Z coordinate system of the system 10), and stored in the 3D proto-marker table 52 (in FIG. 10). Thus, since there are 4 original markers R1-R4, there are 6 line segment lengths (ie, SL12, SL13, SL14, SL23, SL24, and SL34). Here, in this example, the original marks R1 and R2 are separated by a line segment length SL12 which is close to the length of the line segment SLab. The original markers (R1, R4), (R2, R3) are separated by approximately the length of the line segment SLac. The original marks (R2, R4) and (R1, R3) R are separated close to the length of the line segment SLac as shown in FIG. 15 , and this data is stored in the line segment original mark comparison table 56 .

qualifying segment table, 58

For each rigid body 11a, 11b there is a qualified segment table 58 (Fig. 3, 16). This table 58 is produced during the segment verification phase, which will be described in connection with FIG. 7 . However, it suffices to say that there is one entry for each line segment of the rigid body. The example of FIG. 16 shows that the line segments SLab, SLbc, and SLac of the object 11a are all qualified.

Measurement mark position table 60

Each rigid body 11a, 11b has associated therewith a measured marker position table 60 (FIGS. 3, 17) containing the original markers R1-R4 that have been identified, verified, and mapped to the object's actual markers 12a-12c, 12d-12 3D position. The example given in Figure 17 shows the measured positions of the markers 12a, 12b and 12c of the object 11a, where the actual marker 12a corresponds to the original marker R2, the actual marker 12b corresponds to the original marker R4, and the actual marker 12c corresponds to the original marker R1 .

Table of Computed Rigid Body Positions and Orientations, 62

Each rigid body has associated therewith a computed rigid body position and orientation table 62 (Fig. 3, 18) which contains the transformation of the rigid body. This is the posture measured from the measurement mark position table 60 ( FIG. 17 ) based on the mark position table 42 ( FIG. 11 ). In other words, the pose is a transformation that transfers the marker position table 42 to the same spatial X, Y, Z coordinate system of the system 10 . The example shown in Fig. 18 illustrates the pose of the object 11a.

total order of operations

The pose or orientation of an object or target can be determined simultaneously and in real time from the work illustrated in Flowchart 4 below. In step 401, energy sources 24L and 24R (FIG. 1) and active marker 12b are energized. Referring to Figure 1, these energy sources are focused by lens systems 22L, 22R and project images onto DDC sensors 18L, 28R. The image is scanned from sensors 18L, 18R and analyzed by processor 28 for all intensities above a certain threshold. At step 402, the positions of the 2 sensor energy sources are stored in the left and right sensor energy source position tables 50L, 50R (FIGS. 3, 9A, 9B). The position is in pixels. Call the horizontal axis of the sensor U and the vertical axis V. In the preferred embodiment, left and right sensors 18L, 18R are used. The left and right energy source counters 51L, 51R, respectively, are set to the number of energy sources measured on the left and right sensors 18L, 18R, respectively. In the example described in conjunction with FIGS. 9A and 9B , there are four energy sources S ₁ -S ₄ , S ₅ -S ₈ measured by sensors 18L, 18R, respectively; thus, in this example, each counter 51L, The count in 51R is 4.

In step 403, the appropriate tables (ie, tables 52, 56) and counters 51L, 51R, and 53 are initialized for the subsequent sequence of operations. Clear the original 3D mark table 52, the original mark counter 53 and the line segment original mark comparison table 56. In step 404 , the processor 26 analyzes the energy sources stored in the left and right sensor energy source location tables 50L, 50R, and generates the original 3D marker table 52 . Set proto-mark counter S3 to the measured proto-mark number. At this time, it is not known what these markers are. Some may be markers from one or more tracked objects, others may be reflections, and still others may be artifacts caused by the marker measurement method. The triangulation method for generating 3D positions from stereo views is well known in the prior art, and one method thereof will be described in the section Generating the original 3D marker table 52 below.

In steps 405 and 406, the distances between all pairs of original 3D marker combinations are calculated, ie the line segment lengths SL12, SL13, SL14, SL23, SL24 and SL34. For each object 11a, 11b being tracked, the calculated length is compared with the line segment length table 44 (Fig. 12). A match is set for each object 11a, 11b in the line-segment original tag look-up table 56 (FIG. 15). These steps are described in detail in FIG. 6 in connection with the creation of the line segment original labeling table 56 (FIG. 15).

In step 407, each object 11a, 11b is checked for possible line segment guesses in the line segment original label look-up table 56 (FIG. 15) by comparing the relative angle between the line segment lengths SL of the objects 11a, 11b. These steps will be described in detail below in connection with the line segment verification in FIG. 7 . In step 408, the original 3D markers R1-R4 in the example given above are determined by the intersection method using the object's marker line segment setting table 48 (FIG. 13) and the object's line segment original marker comparison table 56 (FIG. 15). and the correspondence between the actual markings 12a-12c, 12d-12f of the respective objects 11a, 11b. The raw 3D markers are drawn into the object's measured marker position table 60 (FIG. 17). These steps will be described in detail below in connection with the tag correspondence extraction of FIG. 8 .

In step 409, the orientation (posture) of the object is determined by the markers contained in the object measurement marker position table 60 (FIG. 17). Methods for determining the 6 degrees of freedom of a rigid body from discrete markers are well known in the art and will not be described here. Finally, in step 410, the poses of all objects are stored in table 62 (FIG. 18) and can be displayed. The gesture can also be stored, transmitted to another computer or processed further, as required. The above sequence will be more readily apparent using the following example and detailed description.

Generation of the original 3D marker sheet, 52

The energy sources S ₁ -S ₄ , S ₅ -S ₈ stored in the left and right sensor energy source position tables 50L, 50R are analyzed (FIGS. 9A and 9B) and the original 3D markers R1-R4 are determined (FIG. 10). The protomarker counter 53 is set to the number of protomarkers measured. Referring now to FIG. 5 , a method of generating a 3D position from two stereoscopic images is described. The following methods are well known in the art, and other methods may be used.

In step 501, the original flag counter 53 is initially set to zero. A parametric linear equation is generated in steps 502 , 503 , 504 and 505 for each left sensor energy source 14L and each right sensor energy source 14R. A line is located between a point on one of the sensors 18L, 18R (ie, at V=U= _Zs , or the origin of the sensor coordinate system) and one of the energy sources S1-S4. Thus, there are 4 lines (ie, left lines) from sensor 18L to each measured energy source S1-S4. Similarly, there are 4 wires from sensor 18R to each measured energy source S5-S8. In steps 506, 507 and 512 to 515, a double loop is processed which will pair every left line with every right line. The number of loop iterations will be equal to left energy source 52L*right energy source 52R. In the above example, there are 4 left energy sources S1-S4, and 4 right energy sources S5-S8 (FIGS. 9A and 9B), which would take 16 iterations of the calculation table 52 (FIG. 10).

In steps 508 and 509, the minimum distance between the left straight line and the right straight line is determined. If the distance is less than a predetermined minimum value, the lines are considered to intersect and possible 3D markers R1-R4 have been found. Since it is not yet known whether it is a correct logo at this time, the mark is considered to be the original mark. In the example shown in Figure 10 there are 4 such matches. The minimum distance is a function of the accuracy of the system and is kept as small as possible to reduce the number of erroneous marks. In steps 510 and 511, the 3D midpoint between the left and right lines is set in the original 3D marker table 52 (FIG. 10). The original mark counter 53 is incremented.

Upon completion of steps 501 to 515, the original marker counter 53 contains the measured number of original markers, and the original 3D marker table 52 (Fig. 10) is completed. From this point on, all subsequent decisions can be made with 3D markers, and the need for 2D sensor energy positions is no longer required. For this example, there are 4 measured raw 3D markers R1-R4. At this point it is not known what each token is.

Generate the line segment original label comparison table of all rigid bodies, 56 (Fig. 15)

In general view, the ensuing procedure will be performed as follows. Compare the possible line segments between all original 3D markers (i.e. R1-R4) in Table 52 (FIG. 10) with all line segment lengths SLab, SLbc, SLac and SLde in Table 44 of FIG. 12 for each object 11a and 11b, Comparison of Slef and SLdf. If a match is found, the original 3D marker pair is added to the line segment original marker lookup table 56 (FIG. 15) for the object. The same primitive segment in the test may match several rigid bodies. Likewise, a segment of a rigid body can have several matching original test segments. These will be filtered out in a later operation, line segment validation (Fig. 7).

By using an example, and the flow chart of Fig. 6, the generated line segment original label comparison table of all rigid bodies, 56, the above content can be made obvious. In steps 601 , 602 and 613 to 616 . Controls two burn processing loops. The two outermost loops are denoted by counters N and L (but not shown), but are included in processor 26 for pairing all combinations of original 3D markers R1-R4. There are N'*(N'-1)/2 line segments (pairs), where N' is equal to eg the original token R1-R4. Here, N'=4. Calculate the length of the line segment between the original 3D markers N and L each time the outer two loops are repeated (step 603 ). This length is called the trial length and is used in the following sections.

In steps 604 , 611 and 612 , the sequence of the processing loop among all tracked rigid bodies is controlled by counter J, which is not shown in the figure but is included in processor 28 . In steps 605, 609 and 610, the sequence of all line segments of one of all objects 11a, 11b in the processing loop is controlled by a counter K not shown in the figure but included in the processor 28, wherein it is collectively represented by object J here . Matching of line segments is performed in steps 606 , 607 and 608 . Compare the line segment K of object J with the test line segment. If the difference is less than a predetermined value, the line segment is considered a matching line segment. When a match occurs, the index counter N and L values of the original 3D marker pair are placed in the next available pair of line segments in the line segment original marker look-up table 56 (FIG. 15) of the line segment K of the object J. A "total logarithm" of a counter not shown but included in processor 28 is incremented for segment K. The predefined value is a function of system precision and is kept as small as practical to reduce the number of segment matches, but large enough to avoid discarding correct segments.

In the example given with reference to Figure 15, for the object 11a, the segment length SLab has a single match, the original 3D marker pair R1 and R2. Segment length SLbc has 3 matches, the original 3D marker pairs: R1 and R4; R3 and R4; and R2 and R3. The segment length SLac has two matching pairs of original 3D markers: R2 and R2; and R1 and R3. It is clear that 3 of the 6 line segments are not part of the rigid body 11a and must be eliminated by line segment verification (Fig. 7).

line segment verification

All possible line segments defined by the original 43D marker pairs in the original marker reference table 56 (FIG. 15) for each object 11a and 11b are verified by comparing the relative angle θ between the segment lengths SL of the objects 11a and 11b. This approach will become clear by examining the flowchart shown in Figures 7, 7A-7C using the following example. The segment validation operation has five deep control loops.

In steps 701, 726 and 727, the processing loop is controlled by a counter L, which is sequenced among all tracked objects. In step 701, the qualifying segment table, 58 (FIG. 16), is cleared for the rigid body L being verified. In steps 702, 724 and 725 the processing loop is controlled by a counter J which sequences in the segment length SL of all objects L. If segment J is not qualified, enter the control loop as indicated in the qualified segment table, 58 (FIG. 16).

If the line segment J in the qualified line segment table, 58 (Fig. 16) is unqualified (promptly set to FALSE (step 703)), then in steps 704, 720 and 721, the processing loop is controlled by the counter N, which is in the object L All the original 3D marker pairs in the line segment J are sequenced in the line segment original marker comparison table 56 ( FIG. 15 ). In step 705 , a vector X is calculated for the original 3D marker pair N of the line segment J of the object L, which is transformed into the coordinate axis origin of the system 10 . In steps 706, 718 and 719, the processing loop is controlled by a counter K, which sequences through the lengths of all line segments following line J in object L.

In steps 707, 716 and 717 the processing loop is controlled by the counter M, which orders all the original 3D marker pairs in the line segment original marker look-up table 56 (FIG. 15) of the object L for the line segment K. In steps 708 to 711, the vector Y transformed to the starting point is calculated for the original 3D marker pair M of the line segment K of the object L. Determines the dot (·) product between vector X and vector Y. This dot (·) product is compared to the actual point (·) product of line segment pairs J and K stored in object L's line segment dot product table, 46 (FIG. 14). A match is found if the difference is less than a predefined value. The predefined value is a function of the accuracy of the system 10 and is kept as small as practical to reduce the number of false segment matches, but large enough to avoid unnecessarily discarding correct segments. The value used in the preferred embodiment is 200.

When a match is found, both segments are considered valid, and the original 3D marker pairs N and M for segments J and K in the segment original marker lookup table, 56 (FIG. 15) are considered true. In steps 712 to 715, the original 3D marker pair N and M is moved to the first position among the original J and K. The total proto-log counter not shown in the figure is set to 1 and all other pro-3D marker pairs are eliminated. Segments J and K are set to true values in the qualifying segment table, 58 (FIG. 16). Upon completion of each iteration of control loop J, a test is performed at step 722 . A segment J is considered bad if it fails after comparison with all subsequent segments. In step 723, all original 3D marker pairs are eliminated for line segment J, and the counter total original number of pairs is set to zero. This process is repeated for all objects 11a, 11b (steps 725-728).

An example will now be described with reference to the line segment original label comparison table, 56 (FIG. 15), the line segment dot product table, 46 (FIG. 14), the original 3D label table, 52 (FIG. 10), and the line segment verification flowchart of FIG. 7. The sequence of events below can be traced in a flowchart.

L = Object 11a

J = line segment length SLab

Segment length SLab fails.

N = pair 1 of SLab (R1, R2)

Generate vector for pair N(R1, R2)

K = line segment length SLbc

M = pair 1(R1, R4) of line segment length SLbc

To generate vectors for M(R1, R4)

Produces the dot product (·). (R1, R2) · (R1, R4) = 3599.9951117

Comparing dot product tables for SLab, 46 (Fig. 14)

SLbc=3600

match found

Pair 2(R3,R4) and Pair 3(R2,R3) of segment length SLbc are deleted.

Set the line segment length SLbc to pass.

Set the line segment length SLab to pass.

Advance M to the next pair.

This is the last pair of segment lengths SLbc.

Advance K to segment length SLac.

M = line segment length SLac for 1

Generate a vector for M(2,4)

Generate the dot product ( ), (R1, R2) (R2, R4) = 0.01304

For line segment length SLab, SLac=0 comparative dot product table, 46 (Fig. 14)

A match was found.

Delete pair 2(R1, R3) of line segment length SLac

Set line segment length SLac to qualified

Set line segment length SLab to qualified

Advance M to next pair

This is the last pair of line segment lengths SLab

Advance K to the next line segment

This is the last pair of line segments.

Advance N to the next pair

this is the last couple

Advance J to the next segment length SLbc

The line segment is qualified.

Advance J to the next segment.

This is the last line segment.

Advance L to the next object.

this is the last object

Complete, see table (step 158)

Markup Correspondence Extraction

The correspondence between the original 3D markers indicated in the Line Segment Original Marker Table, 56 (FIG. 15) and the object's actual markers is determined by the method of intersection, using the Object's Marked Line Segment Set Table 48 (FIG. 13). The original 3D markers R1-R4 are drawn into the object's measured marker position table, 60 (Fig. 17). These steps are described below by way of an example using the flow chart Figure 8, Labeled Correspondence Extraction.

The marker-to-fetch operation has three deep control loops. In steps 801, 825 and 826, the processing loop is controlled by a counter K, which is sequenced among all tracked rigid bodies. The processing loop is controlled in steps 802, 823 and 824 by a counter N, which 2 sequences among all the tokens of the object K. Set intersections are determined using registers M1 and M2. Initially set to null in step 803 . The processing loop is controlled in steps 804, 812 and 813 by counter J, which sequences in the segment lengths of all markers N attached to object K. For any given label, there will be B - 1 connecting line segments, where B is the total number of labels of the object.

In step 805, L is set to the number of segments indicated by J for the tag N of object K in the tag line segment set table, 48 (FIG. 13). In steps 807 to 811, if there is a qualified line segment length SL in the line segment original mark comparison table, 56 (FIG. 15), then the set intersection point of the mark is tested. If M1 is empty, set M1 to be the first marker in the pair, and set M2 to be the second marker in the pair. If M1 is not empty, a test is performed to determine whether M1 is equal to the first or second marker of the pair. If M1 is not equal to either of the two markers, then it does not intersect the set and thus sets to resist. Equivalently, will test whether M2 is equal to the first or second marker in the pair. If M2 is not equal to either of the two flags, then it does not intersect the set, so it will be set to culling. This is repeated for all line segments connected to marker N.

At this point, M1 and M2 may have the various states tested in steps 814, 815-817, 818-820, and 821-822. If M1 is the correct marker and M2 is knocked out, then the original marker contrasted by M1 corresponds to marker N. If M2 is the correct marker and M1 is knockout, then the original marker controlled by M2 corresponds to marker N. The original 3D marker positions compared by M1 or M2 can be copied to the measured marker position table, 60 (FIG. 17). If M1 and M2 are both rejected or empty, there is no corresponding original marker for marker N, and for this marker, the measurement marker position table, 60 (FIG. 17) is set to missing.

example

Completing an example will make the above statement clear. Referring to the line segment original mark comparison table, 56 (Fig. 15), marked line segment collection table, 48 (Fig. 13), original 3D mark table, 52 (Fig. 10), measurement mark position table, 60 and the mark corresponding extraction flow chart of Fig. 8 . The sequence of events below can be traced in the flowchart.

K = Object 11a

N = marker 12a

Set M1 and M2 to empty.

Set J as the first line segment in the marker line segment collection table (FIG. 13) of the marker 12a.

Set L as the line segment length SLab, compared with J.

There are qualified SLab line segments in the line segment original label comparison table, 56 (Fig. 16). The original marker pair is R1, R2.

M1 is empty, thus, set M1=R1, M2=R2.

Advance J to the second segment in the marker segment set table for marker 12a, 48 (FIG. 13).

There are qualified SLac line segments in the line segment original label comparison table, 56 (Fig. 15). The original marker pair is R2, R4.

M1 is not empty.

M1 is not equal to R2 or R4, thereby setting M1 to resist.

M2 is not equal to R2.

Advance J to the third segment in the marker segment set table, 48 (FIG. 13) for marker 12a.

This is the last segment marked N

Trials M1 and M2.

M1 is resisted, M2 is equal to R2, whereby the original 3D marker R2 corresponds to the actual marker N (12a).

Copy 3D to the measured marker position table, 60 (Fig. 17).

Advance N to marker 12b.

Repeat the above procedure for markers 12b and 12c. This process is repeated by mapping the original label R2 to the actual label 12a, the original label R4 to the actual label 12b, and the original label R1 to the actual label 12c.

Determining object orientation (pose)

We now have all the information we need to determine the position of the rigid body. This is well known in the art and will not be described here. The pose is stored in the computed rigid body position and orientation table, 62 (FIG. 18). Other embodiments are within the spirit and scope of the following claims.

Claims (12)

1. A system for determining the spatial position and orientation of each of a plurality of objects, characterized by comprising: at least three indicia attached to each of said objects in a predetermined relative geometric relationship, said indicia being adapted to emit energy in response to an excitation signal and/or reflect energy directed at the indicia from an energizable energy source; an energy detector for measuring the energy emitted by one of said markers and/or the energy reflected by one of said markers; a processor having a memory storing therein predetermined relative geometric relationships of said markers of said each object; and Wherein the processor compares the stored predetermined geometry of the signature of each object with the energy measured by the energy detector to identify objects that emit and/or reflect the detected energy. 2. The system according to claim 1, wherein each object has a plurality of marks, the marks are arranged in a known and fixed relative geometric relationship, and all pairs of marks on the object have a unique the segment length of . 3. The system of claim 1, wherein each object has a plurality of markers having similar line segment pairs across all objects with unique relative angles. 4. The system of claim 1, wherein the detector comprises a three-dimensional arrangement of a plurality of sensors. 5. A method for determining the spatial position and orientation of each of a plurality of objects, comprising the steps of: detecting energy emitted by active markers and/or reflected by passive markers, at least three such markers disposed on each of the plurality of objects in a predetermined relative geometric relationship, the markers being adapted to send energy and/or reflected energy impinging on the marking from an energizable energy source; providing a memory in which predetermined relative geometric relationships of said markers of said each object are stored; The stored predetermined geometry of the signature of each object is compared with the energy measured by the energy detector to identify objects that emit and/or reflect the detected energy. 6. The method of claim 5, comprising tracking the object in real time. 7. The method of claim 5, comprising tracking a plurality of such objects simultaneously. 8. The method of claim 5, comprising arranging a plurality of marks on each object in a known and fixed relative geometric relationship, with unique line segment lengths between all pairs of marks on the object. 9. The method of claim 5, comprising deploying a plurality of markers on each object, the markers having similar pairs of line segments across all objects and having unique relative angles. 10. The method of claim 8, comprising deploying a plurality of markers on each object, the markers having similar pairs of line segments across all objects and having unique relative angles. 11. The method of claim 5, comprising determining the pose of one or more objects in real-time in a closed-loop approach using one sensor cycle. 12. The method of claim 5, comprising automatically identifying and tracking objects.

Images