# US 12,002,235 B2 · Apparatus and method for estimating camera orientation relative to ground surface

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 12,002,235 B2
- Document ID: US12002235B2
- Status: Granted
- Kind: B2
- Application number: 17/197,069
- Filed: 2021-03-10
- Published: 2022-02-17
- Granted: 2024-06-04
- Earliest claimed: 2020-08-12
- Assignees: Hong Kong Applied Science and Technology Research Institute Company Limited
- Applicants: Hong Kong Applied Science and Technology Research Institute Company Limited
- Inventors: Wang Kit Wilson Thong; Jihui Zhang; Man Wai Kwan; Yiu Man Lam; Cheng Hsiung Liu; Jiaxin Yan; Zhuobin Lin
- Web page: https://patentgrep.com/patent/US12002235B2

## CPC

- G06T 7/73: using feature-based methods
- G05D 1/0011: {associated with a remote control arrangement}
- G05D 1/0088: {characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours (using knowledge based models G06N5/00)}
- G05D 1/0231: {using optical position detecting means (position-fixing by using electromagnetic waves other than radio waves, e.g. optical position detecting means G01S5/16)}
- G05D 1/223: Command input arrangements on the remote controller, e.g. joysticks or touch screens
- G05D 1/227: Handing over between remote control and on-board control; Handing over between remote control arrangements
- G05D 1/249: from positioning sensors located off-board the vehicle, e.g. from cameras
- G06F 17/11: for solving equations {, e.g. nonlinear equations, general mathematical optimization problems (optimization specially adapted for a specific administrative, business or logistic context G06Q10/04)}
- G06T 7/13: Edge detection
- G06T 7/536: from perspective effects, e.g. by using vanishing points
- G06T 7/80: Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
- G06N 7/01: Probabilistic graphical models, e.g. probabilistic networks
- G06T 2207/10016: Video; Image sequence
- G06T 2207/20061: Hough transform
- G06T 2207/30244: Camera pose
- G06T 2207/30252: Vehicle exterior; Vicinity of vehicle
- G06T: IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G06F: ELECTRIC DIGITAL DATA PROCESSING (computer systems based on specific computational models G06N)
- G06N: COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS

## Abstract

An iterative multi-image camera orientation estimation comprising: capturing an image of a scene before the camera; detecting line segments in the scene; computing a maximum likelihood (ML) camera orientation by maximizing a likelihood objective by rotating the camera's X-Y-Z coordinate system such that it is being optimally aligned with the line segments in at least two of the frontal, the lateral, and the vertical orthogonal directions; estimating a maximum a-posteriori (MAP) camera orientation that maximizes an a-posteriori objective such that the MAP camera orientation is an optimal value in between the priori camera orientation and the ML camera orientation, and is closer to the one with smaller uncertainty; iterating the multi-image camera orientation estimation with the priori camera orientation and its corresponding priori camera orientation uncertainty set to the computed MAP camera orientation and its corresponding uncertainty respectively until the uncertainty is lower than a threshold.

## Claims

### Claim 1

A method for estimating camera orientation of a camera relative to a ground, comprising:
initializing a priori camera orientation and its corresponding priori camera orientation uncertainty with a best guess or random camera orientation of the camera;
executing an iterative multi-image camera orientation estimation, comprising:
capturing a new image or extracting a new video frame from a video of a scene before the camera;
detecting one or more line segments in the scene in the image or video frame;
classifying and grouping the line segments of the image or video frame into a frontal, a lateral, and a vertical line segment groups;
computing a maximum likelihood (ML) camera orientation by taking a calibrated matrix of the camera and maximizing a likelihood objective by rotating a X-Y-Z coordinate system under the camera orientation such that it is being optimally aligned with the line segments in at least two of the frontal, the lateral, and the vertical orthogonal directions;
estimating a maximum a-posteriori (MAP) camera orientation that maximizes an a-posteriori objective such that the MAP camera orientation is an optimal value in between the priori camera orientation and the ML camera orientation, and is closer to the one with smaller uncertainty;
comparing the MAP camera orientation with a pre-defined MAP camera orientation uncertainty; and
if the MAP camera orientation uncertainty is higher than the MAP camera orientation uncertainty threshold, iterating the multi-image camera orientation estimation with the priori camera orientation and its corresponding priori camera orientation uncertainty set to the computed MAP camera orientation and its corresponding MAP camera orientation uncertainty respectively; and
if the MAP camera orientation uncertainty is equal and lower than the MAP camera orientation uncertainty threshold, taking the MAP camera orientation that is corresponding to the MAP camera orientation uncertainty that is equal and lower than the MAP camera orientation uncertainty threshold value as the camera orientation estimation method result.

### Claim 2 (depends on 1)

The method of claim 1, wherein the classification and grouping of the line segments of the image or video frame into the frontal, the lateral, and the vertical line segment groups comprises:
projecting a three-dimensional (3D) x-axis infinity point, a 3D y-axis infinity point, and a 3D z-axis infinity point infinity points corresponding to an initial orientation of the camera on to the image or video frame to obtain a two-dimensional (2D) X-directional orthogonal vanishing point, a 2D Y-directional orthogonal vanishing point, and a 2D Z-directional orthogonal vanishing point respectively of the scene in the image or video frame; and
classifying and grouping the line segments into a frontal line segment group, which contains line segments having shortest perpendicular distances to the X-directional orthogonal vanishing point in comparison to the other vanishing points; a lateral line segment group, which contains line segments having shortest perpendicular distances to the Y-directional orthogonal vanishing point in comparison to the other vanishing points, and a vertical line segment group, which contains line segments having the shortest perpendicular distances to the Z-directional orthogonal vanishing point in comparison to the other vanishing points;
wherein the initial orientation of the camera is obtained from the camera's calibrated matrix, a best guess orientation, a randomly set orientation, or measurements using an orientation sensor.

### Claim 3 (depends on 1)

The method of claim 1, wherein the MAP camera orientation estimation comprises:
initializing a currently estimated camera orientation rotation matrix to a prior camera orientation rotation matrix of the prior camera orientation;
initializing a currently estimated camera orientation uncertainty to a prior camera orientation uncertainty of the prior camera orientation;
executing an iterative a-posteriori objective maximization comprising:
computing a X-directional orthogonal vanishing point, a Y-directional orthogonal vanishing point, and a Z-directional orthogonal vanishing point of a X-Y-Z coordinate system under the currently estimated camera orientation;
projecting the X-directional orthogonal vanishing point, the Y-directional orthogonal vanishing point, and the Z-directional orthogonal vanishing point on to the image or video frame;
measuring a perpendicular distance between each line segment in the frontal line segment group and the X-directional orthogonal vanishing point, a perpendicular distance between each line segment in the lateral line segment group and the Y-directional orthogonal vanishing point, and a perpendicular distance between each line segment in the vertical line segment group and the Z-directional orthogonal vanishing point;
computing a camera rotation Euler-angle for rotating from the currently estimated camera orientation rotation matrix, currently estimated camera orientation uncertainty, and the perpendicular distances so to maximize the a-posteriori objective;
updating the currently estimated camera orientation rotation matrix by perturbing it by the camera rotation Euler-angle;
updating currently estimated camera orientation uncertainty by setting it to a co-variance of the camera rotation Euler-angle; and
if the camera rotation Euler-angle is higher than a pre-defined camera rotation threshold, iterating the a-posteriori objective maximization;
if the camera rotation Euler-angle equal or lower than the camera rotation threshold, outputting the currently estimated camera orientation as the MAP camera orientation, and the currently estimated camera orientation uncertainty as the MAP camera orientation uncertainty.

### Claim 4 (depends on 3)

The method of claim 3, wherein the computation of the camera rotation Euler-angle, comprises:
computing Φ0 from R0 by solving [Φ0]x=ln R0;
computing a precision of the priori camera orientation, ΛΦ0, by pseudo inversing of the priori camera orientation uncertainty, ΣΦ0, that is ΛΦ0=ΣΦ0+;
computing Φ such that a rate of change of E(Φ)=Σi∈i2/JiΣgJi is 0, where ∈i=liKRPi, by solving ΦML=ΣΦML b, where:
H
i
τ
=
[
P
i
]
×
⁢
R
0
⁢
K
τ
⁢
l
i
;
∂
l
i
∂
g
=
[
0
1
0
1
-
⁢
1
0
1
0
q
-
⁢
p
-
⁢
v
u
]
;
J
i
=
P
i
τ
⁢
R
τ
⁢
K
τ
⁢
∂
l
i
∂
g
;
wi=JiΣgJi;
A=ΣiHiwiHi;
ΣΦML=A+;
b=ΣiHiwiϵi; and
computing the camera rotation Euler-angle, ΔΦMAP, by solving ΔΦMAP=ΣΔΦMAP d, where:
C=A+ΛΦ0;
d=b+ΛΦ0(ΦML−Φ0);
ΣΔΦMAP=C+;
li represents the line segment i, that is li=(pi, qi, 1)×(ui, vi, 1) between two end points (pi, qi) and (ui, vi);
P
i
=
{
[
0
,
0
,
1
]
τ
⁢
⁢
for
⁢
⁢
l
i
⁢
⁢
in
⁢
⁢
the
⁢
⁢
vertical
⁢
⁢
line
⁢
⁢
segment
⁢
⁢
group
[
0
,
1
,
0
]
τ
⁢
⁢
for
⁢
⁢
l
i
⁢
⁢
in
⁢
⁢
the
⁢
⁢
lateral
⁢
⁢
line
⁢
⁢
segment
⁢
⁢
group
[
1
,
0
,
0
]
τ
⁢
⁢
for
⁢
⁢
l
i
⁢
⁢
in
⁢
⁢
the
⁢
⁢
frontal
⁢
⁢
line
⁢
⁢
segment
⁢
⁢
group
;
K is the camera's calibrated matrix; and
Σg is a user-defined pixel noise co-variance at both ends of the line segment, li.

### Claim 5 (depends on 1)

The method of claim 1, further comprising a ground plane normal vector, n, of the scene before the camera is computed by solving:
n=R*[0, 0, 1]; where R* is a rotation matrix of the camera orientation estimation method result.

### Claim 6 (depends on 1)

The method of claim 1, wherein the detection of one or more line segments in the scene in the image or video frame comprises:
converting the image or video frame into a 2D array containing only zeros and ones using Canny edge detection; and
detecting the line segments from the 2D array using statistical Hough transform.

### Claim 7 (depends on 1)

A method for guiding a vehicle or a mobile robot having a front-facing camera, comprising:
executing a method for estimating camera orientation of the front-facing camera of claim 1; and
controlling motions of the vehicle by a remote processing server in response to the estimated camera orientation.

### Claim 8 (depends on 1)

A remote processing server for estimating camera orientation of a front-facing camera of an autonomous guided vehicle (AGV) or a mobile robot, comprising:
a processor in data communication with an AGV or a mobile robot;
wherein the processor is configured to receive a video file or data stream from the AGV or the mobile robot and to execute the method for estimating camera orientation of claim 1 with respect to the front-facing camera of the AGV or the mobile robot.

### Claim 9 (depends on 1)

An autonomous guided vehicle (AGV), comprising:
a front-facing camera installed at a front side of the AGV body and configured to capture a scene before the AGV;
a processor configured to receive a video file or data stream from the front-facing camera and to execute the method for estimating camera orientation of claim 1 with respect to the front-facing camera of the AGV.

### Claim 10 (depends on 1)

A mobile robot, comprising:
a front-facing camera installed at a front side of the mobile robot body and configured to capture a scene before the mobile robot;
a processor configured to receive a video file or data stream from the front-facing camera and to execute the method for estimating camera orientation of claim 1 with respect to the front-facing camera of the mobile robot.