Method for stereo visual odometry using points, lines and planes

The invention relates generally to computer vision, and more particularly to determining motion between two coordinate systems using points, lines and planes as geometric primitives, where each coordinate system corresponds to a camera coordinate system of a stereo camera or a red, green, blue and depth (RGBD) sensor.

Visual odometry uses data acquired by moving sensors to estimate change in motion over time. Numerous methods are known for estimating the motion using geometric primitives such as points, lines and planes. For example, one can determine the motion using correspondences between 3D points in one coordinate system to another coordinate system. As expected, different types of solvers have different advantages and disadvantages, which mean that failure modes of one particular method may not be present in another one.

One method uses three 2D to 3D point correspondences to obtain motion between stereo pairs. 3D to 3D point correspondences are used in an iterative closest point (ICP) method for determining motion between stereo cameras. Some hybrid methods determine motion by mixing two different primitives. For example, points and lines have been used together, see U.S. Pat. No. 8,442,305. Points and planes can also be used together, see U.S. Pat. No. 8,224,071 and U.S. Patent Application 20140003705.

Problem statement: Given two pairs of stereo images (Left 1 , Right 1 ) and (Left 2 , Right 2 ) in two coordinate systems C 1 and C 2 , respectively, the embodiments of the invention determine the rotational and translational motion (R,t) between C 1 and C 2 using the pairs of stereo images (Left 1 , Right 1 and Left 2 , Right 2 ).

For each of the four images in two pairs of the stereo images, 2D points and 2D lines are extracted. From the first pair (Left 1 , Right 1 ), 3D points, 3D lines and 3D planes are determined using triangulation and stereo reconstruction. The 3D primitives are represented in the first coordinate system C 1 .

Similarly, 3D primitives are extracted from the second stereo pair and represented in C 2 . The motion (R,t) between C 1 and C 2 can then be determined, for example, using three 3D points in C 1 and their corresponding three 3D points in C 2 . Similarly, we use different solvers for different combinations of corresponding primitives.

To the best of our knowledge, we are not aware of any work that uses points, lines and planes in both 2D and 3D space for estimating motion. It is not straightforward to extend the existing cases because careful analysis of the geometric primitives is required to compute the motion parameters.

It should be understood that the images can also be acquired by a red, green, blue and depth (RGBD) sensor. Thus, generally the images are 3D images that have both color and depth information, providing both 2D and 3D primitives in each coordinate system.

Each solver determines a motion hypothesis. Using a ground truth, the motion hypothesis and the primitives, parameters of a scoring function are learned. Then, the best motion hypothesis can be selected using the scoring function.

FIG. 1 is a flowchart of a method for estimating motion according to embodiments of the invention; and

FIG. 2 is a schematic of different combinations of 2D and 3D primitives that can be used by the method of FIG. 1 .

As shown in FIG. 1 , the embodiments of the invention provide a method for estimating motion. Input 101 to the method is two pairs 111 and 112 of stereo images (Left 1 , Right 1 ) and (Left 2 Right 2 ) in two coordinate systems C 1 and C 2 respectively. Each stereo image can be acquired by a pair of red, green and blue (RGB) cameras calibrated with each other. The embodiments of the invention, determine rotational and translational motion (R,t) 150 between C 1 and C 2 using the two pairs of stereo images.

It is noted that the images can also be acquired by an RGBD sensor. Hence, generally the images can be considered three-dimensional (3D) images, which can either be represented by a stereo pair or a single RGBD image. It is also possible to use more than two cameras, e.g., a trinocular stereo camera, to increase the accuracy of the depth information.

For each of the four images in two stereo pairs of images or 3D images, 2D points and 2D lines are extracted 102 , From the first stereo pair (Left 1 , Right 1 ) or 3D image, 3D points, 3D lines and 3D planes are extracted 121 using triangulation and stereo reconstruction. The 3D primitives are represented in the first coordinate system C 1 .

In the case where the 3D images are acquired by an RGBD sensor, only one image is required for each coordinate system.

Similarly, 3D primitives are extracted 122 from the second stereo pair or 3D image, and represented in C 2 . The motion (R,t) between C 1 and C 2 can then be determined, for example, using three 3D points in C 1 and their corresponding three 3D points in C 2 . Similarly we consider solvers based on other correspondences, as shown in Table 1.

Table 1 shows several combinations of primitives that can generate 105 the motion hypotheses. In other words, we consider a set of different motion estimation methods that use correspondences between different sets of 2D and 3D primitives. FIG. 2 visually shows some of the combinations of primitives. Some embodiments can use mixed sets of correspondences as shown in Table 1 and FIG. 2 .

We also obtain 103 ground truth (GT) motion for the stereo or RGBD sensors using, e.g., a global positioning system (GPS) or inertial measurement units (combinations of accelerometers and gyroscopes), to learn parameters for our method.

We use a RANdom SAmple Consensus (RANSAC) procedure to sample the correspondences and determine the motion hypotheses 110 . Each of these motion estimation methods can be thought of a function that produces motion (R,t) given a minimal set of correspondences.

In the RANSAC procedure, we use a scoring function 120 that uses all the geometric primitives to determine a score. We use the score to rank the motion hypotheses 110 , and select the best motion 106 . We use a learning method 104 to determine the parameters of die scoring function 120 .

Computation of 2D and 3D Primitives

Points:

We can compute the 2D point primitives using interest point detectors such as a Harris affine region detector. Matching between the 2D point primitives can be computed using descriptors such as speeded up robust feature (SURF). In order to compute the corresponding 3D point primitives for every 2D point, we find the 2D point matches between the left and the right images. Once the matches are identified, we use the intrinsic and extrinsic calibration parameters to obtain the projection matrices for both the images in a common coordinate system. Using triangulation we reconstruct the 3D point corresponding to the 2D point matches.

Lines:

We first detect 2D lines from individual images using line detectors such as LSD (line segment detector). We match the 2D lines in images using SURF descriptors on the end points of the line segments in the images. Once the 2D lines are matched in two images, then the reconstruction of 3D lines can be achieved by intersecting two planes defined by back-projecting the 2D lines based on the projection matrices for the left and right images.

Planes:

We use a stereo matching procedure such as semi-global stereo matching to obtain a depth map from the left and right images. Once we obtain the depth map, we use a plane fitting procedure, such as the 3-point RANSAC, to compute the planar segments in the 3D point cloud corresponding to the depth map.

There are two main novel components in this method that previous approaches have not considered before: motion estimation using hybrid correspondences and a learning procedure for the scoring function in RANSAC.

Motion Estimation Using Hybrid Correspondences

We compute 6 degrees of freedom (DOF) motion parameters (R,t) using both 2D and 3D correspondences. While considering 2D to 3D correspondences, the following configurations have been already solved using the ideas proposed in U.S. Pat. No. 8,442,305:

(1) Three 2D points in C 1 are matched to three 3D points in C 2 . Using these matches we can compute the 6 DOF motion (R,t) using a 4 th degree equation. (2) Two 2D points and one 2D line in C 1 are matched to two 3D points and one 3D line in C 2 . Using these matches, we can compute 6 DOF motion (R,t) using a 4 th degree equation. (3) One 2D point and two 2D lines in C 1 are matched to one 3D point and two 3D lines in C 2 . Using these matches, we can compute 6 DOF motion (R,t) using an 8 th degree equation. (4) Three 2D lines in C 1 are matched to three 3D lines in C 2 . Using these matches, we can compute the 6 DOF motion (R,t) using an 8 th degree equation.

We can also compute the motion (R,t) using 3D points in C 1 that lie on 3D planes in C 2 using the ideas explored in U.S. Pat. No. 8,224,071. Here, we can compute the motion (R,t) using six 3D points in C 1 that are matched to six 3D planes in C 2 . In U.S. Patent Application 20140003705, motion estimation using 3D point to 3D point and 3D plane to 3D plane correspondences are considered.

To the best of our knowledge, the following cases, shown bolded in Table 1, are not described in the prior art. It is not straightforward to derive the motion estimation procedure for the following cases using the existing cases.

(1) Three 2D to 2D point correspondences and one 3D to 3D point correspondences:

Using a single 3D point to 3D point correspondence, we can compute the translation (3 DOF) between the two stereo pairs. We use the three 2D to 2D point correspondences to compute the rotation matrix (3 DOF).

(2) One 2D to 2D point correspondence and two 3D to 3D point correspondences:

Using two 3D to 3D point correspondences, we can compute 5 DOF (3 DOF translation and 2DOF rotation). Using a single 2D to 2D correspondence, we can compute the remaining 1 DOF rotation.

Parameter Learning for Scoring Function

To the best of our knowledge, we are not aware of any procedure that uses parameter learning techniques for generating the scoring function in RANSAC for stereo-odometry procedures. Most prior techniques use heuristics and trial-and-error procedures to develop the scoring function. We use data with ground truth motion for generating positive and negative examples used in the parameter learning procedures.

We briefly describe the formulation of our parameter learning procedure. We describe a family of different motion estimation procedures that takes a few correspondences using a minimal set of correspondences based on points, lines and planes, as shown in Table 1. Given the minimal set of correspondences, we can compute the motion (R,t). Based on a given (R,t), we can compute miters from all the available correspondences; we check the points, lines and planes that agree with, the motion based on a threshold. Typically, the motion (R,t) that generates the maximum number of inliers is selected as the best motion. However, when we have hybrid correspondences we are not sure how to select the best motion. Choosing some motion based on only the number of inliers might not be the right criteria, because lines and planes are stronger cues compared to points. Thus we would like to learn a function that gives different weights to the different types of inliers and also gives priority based on the location of these inliers in the images. The weights learned using the structured learning are different for points, lines and planes, and different for different locations of an image. The weights are also learned for the motion parameters.

Let the different motion estimation procedures be represented by M i , where iε{1, 2, 3, . . . , n}. Each of these motion estimation procedures takes a minimal set of correspondences and generates a motion (R,t). Based on this motion we can compute the inlier set I i which consists of point, line and plane correspondences that agree with the motion. In other words, we use the motion to project the point from one stereo pair to another and check if the points are within a threshold. Similarly we can check for line and plane correspondences. Summarizing, every motion hypothesis M i =(R i ,t i ) generates an inlier set I i . We can represent a pair consisting of a motion and its corresponding inlier as (M i ,I i ). We describe our learning procedure using ground truth motion that can be collected with high accuracy GPS or other sensors.

Given inliers {I 1 , I 2 , . . . , I n }=I and corresponding motions {M 1 , M 2 , . . . , M n }=M, we learn a mapping g: I, M→R to identify the optimal motion. We learn the mapping in such a manner that for the correct motion M i and its corresponding inlier set I i , the function g(M i ,I I ) is large. We also ensure that for any motion M j , i≠j that is not the correct motion, g(M j I i ) decreases as the deviation between the motions Δ(M j ,M i ) increases. Usually, the function g is of the form g(I)=w T ψ(M,I), where w is a parameter vector that we learn, and ψ(M,I) is a vector of features that can be determined from the motion and the inlier set. The mapping function can be teamed discriminatively using a quadratic program formulated by structured output techniques.

We now describe how to build the features from the motion and the inlier set. We discretize the image into several rectangular blocks x={x 1 , x 2 , . . . , x n } where each block is denoted by the variable x i . For each of the inlier, whether it is a 2D or a 3D primitive, we find a specific location in the image based on the subblock x i . The function is p(x i )=1 if there is at least one inlier point correspondence in the subblock, and p(x i )=0 otherwise. Similarly the function is l(x i )=1 if there is at least one inlier line correspondence in the subblock, and l(x i )=0 otherwise. In the same manner, this function is π(x i )=1 if there is at least one inlier plane correspondence in the subblock x i , and π(x i )=0 otherwise. We construct a feature vector using these intermediate functions as follows:

Ψ( M,I )=( a 1 a 2 a 3 t 1 t 2 t 3 p ( x i ) p ( x 2 ) . . . l ( x i ) l ( x 2 ) . . . π( x i ) π( x 2 ) . . . ) T

Here the parameters (a 1 ,a 2 ,a 3 ) denote the three Euler angles for the rotation matrix. The parameters (t 1 ,t 2 ,t 3 ) denote the three translation parameters of the motion.

Although the invention has been described by way of examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.