WO2020259185A1 - Method and apparatus for implementing visual odometer - Google Patents

Method and apparatus for implementing visual odometer Download PDF

Info

Publication number
WO2020259185A1
WO2020259185A1 PCT/CN2020/092539 CN2020092539W WO2020259185A1 WO 2020259185 A1 WO2020259185 A1 WO 2020259185A1 CN 2020092539 W CN2020092539 W CN 2020092539W WO 2020259185 A1 WO2020259185 A1 WO 2020259185A1
Authority
WO
WIPO (PCT)
Prior art keywords
camera
time
angular velocity
time window
coordinate system
Prior art date
Application number
PCT/CN2020/092539
Other languages
French (fr)
Chinese (zh)
Inventor
查红彬
王欣
姜立
方奕庚
Original Assignee
京东方科技集团股份有限公司
北京大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 京东方科技集团股份有限公司, 北京大学 filed Critical 京东方科技集团股份有限公司
Publication of WO2020259185A1 publication Critical patent/WO2020259185A1/en

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/36—Videogrammetry, i.e. electronic processing of video signals from a single source or from different sources to give parallax or range information
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers

Definitions

  • the present disclosure relates to the technical field of visual odometry, and in particular to a realization method and realization device of visual odometry.
  • the goal of visual odometry is to estimate the motion trajectory and posture of the camera based on the images taken by the camera.
  • the calculation result of the camera posture obtained by the visual odometry in the related art is not accurate.
  • the present disclosure provides a realization method of visual odometer, including:
  • the objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera.
  • the force acting on the camera can be decomposed into: the force F w whose direction and magnitude remain unchanged in the world coordinate system, the force F b whose magnitude remains the same in the camera coordinate system, and, in the world coordinate system at time t u newly added force F u;
  • the optimizing the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera includes:
  • the objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the start time of the time window is determined by Fu .
  • the first model function is:
  • S (t) is the camera at the time t the position in three-dimensional space
  • t i is the i th start time within the time window
  • [Delta] t is the time in the i-th time window t and the time t i the difference
  • m is the quality of the camera
  • q(t) is the quaternion representation of the orientation of the camera at time t
  • R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
  • the second model function is:
  • q(t) is the quaternion representation of the orientation of the camera at time t
  • ⁇ (t) is the angular velocity of the camera at time t
  • t i is the starting time in the i-th time window
  • ⁇ t is the i-th time within the time window t and the time difference t i
  • (7) is quaternion multiplication.
  • the objective function is:
  • ⁇ is the set of forces acting on the camera
  • ⁇ is the set of angular velocity of the camera
  • N is the number of frames of the image in the i-th time window
  • k is the The count value of the image frames in the i time window
  • j is the count value of the feature points in the k-th frame image
  • n k is the total number of feature points in the k-th frame image
  • ⁇ ( ⁇ ) is the three-dimensional point X projection
  • p is the two-dimensional feature point corresponding to the three-dimensional feature point
  • T cw (t) [R(q(t)), s(t)]
  • s(t) is the camera at time t
  • q(t) is the quaternion representation of the camera's orientation at time t
  • R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t
  • the method further includes:
  • the optimized F u is greater than or equal to the specified threshold, it is determined that a new time window starts, and the start time of the new time window is tu .
  • the present disclosure also provides a device for implementing visual odometer, including:
  • a first establishing circuit for establishing a first model function for expressing the position change of the camera the first model function adopts the force acting on the camera to express the position change of the camera;
  • a second establishing circuit configured to establish a second model function used to express the orientation change of the camera, and the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
  • An objective function establishment circuit for obtaining an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera observed by the inertial sensor;
  • the optimization circuit is used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera.
  • the force acting on the camera can be decomposed into: the force F w whose direction and magnitude remain unchanged in the world coordinate system, the force F b whose magnitude remains the same in the camera coordinate system, and, in the world coordinate system at time t u newly added force F u;
  • the optimization circuit is also used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the starting time of the time window is determined by F u determine.
  • the first model function is:
  • S (t) is the camera at the time t the position in three-dimensional space
  • t i is the i th start time within the time window
  • [Delta] t is the time in the i-th time window t and the time t i the difference
  • m is the quality of the camera
  • q(t) is the quaternion representation of the orientation of the camera at time t
  • R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
  • the second model function is:
  • q(t) is the quaternion representation of the orientation of the camera at time t
  • ⁇ (t) is the angular velocity of the camera at time t
  • t i is the starting time in the i-th time window
  • ⁇ t is the i-th time within the time window t and the time difference t i
  • (7) is quaternion multiplication.
  • the objective function is:
  • ⁇ is the set of forces acting on the camera
  • ⁇ is the set of angular velocity of the camera
  • N is the number of frames of the image in the i-th time window
  • k is the The count value of the image frames in the i time window
  • j is the count value of the feature points in the k-th frame image
  • n k is the total number of feature points in the k-th frame image
  • ⁇ ( ⁇ ) is the three-dimensional point X projection
  • p is the two-dimensional feature point corresponding to the three-dimensional feature point
  • T cw (t) [R(q(t)), s(t)]
  • s(t) is the camera at time t
  • q(t) is the quaternion representation of the camera's orientation at time t
  • R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t
  • the device for implementing the visual odometer further includes:
  • the time window determination circuit is used to determine the start of a new time window if the optimized F u is greater than or equal to the specified threshold, and the start time of the new time window is tu .
  • the present disclosure also provides a device for implementing a visual odometer, including a processor, a memory, and a computer program stored in the memory and capable of running on the processor, and the computer program is implemented when the processor is executed.
  • the present disclosure also provides a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the steps of the foregoing method for implementing the visual odometer are realized.
  • FIG. 1 is a schematic flowchart of a method for implementing a visual odometer according to an embodiment of the disclosure
  • FIG. 2 is an exploded schematic diagram of the force acting on the camera according to the embodiment of the disclosure
  • FIG. 3 is a schematic diagram of the relationship between the angular velocity of the camera and the orientation of the camera in an embodiment of the disclosure
  • FIG. 4 is a schematic diagram of an optimization method of a kinetic model according to an embodiment of the disclosure.
  • FIG. 5 is a schematic diagram of a method for segmenting a camera motion track according to an embodiment of the disclosure
  • Fig. 6 is a schematic structural diagram of a device for implementing a visual odometer according to an embodiment of the disclosure.
  • the time domain constraint relationship between each frame of images collected by the camera is not considered, and the camera pose of each frame of image is solved independently, ignoring the physical constraints of camera movement between consecutive frames.
  • the final camera posture calculation result will jitter around the actual position, which does not have continuity and does not meet the physical constraints that the trajectory should meet, which will bring fatal problems to the control of robots and automatic driving.
  • the present disclosure provides an implementation method and implementation device of a visual odometer.
  • FIG. 1 is a schematic flowchart of a method for implementing a visual odometer according to an embodiment of the present disclosure.
  • the method for implementing the visual odometer includes:
  • Step 11 Establish a first model function for expressing the position change of the camera, where the force acting on the camera is used in the first model function to express the position change of the camera;
  • the force acting on the camera is used to model the position change of the camera (ie, the translation trajectory).
  • Step 12 Establish a second model function used to express the orientation change of the camera, where the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
  • the angular velocity of the camera is used to model the orientation change of the camera.
  • Step 13 Obtain an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera observed by the inertial sensor;
  • Step 14 optimizing the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera.
  • the force acting on the camera is related to the acceleration of the camera.
  • the acceleration is equal to the resultant force acting on the camera divided by the mass of the camera. Therefore, by obtaining the force acting on the camera, the acceleration of the camera can be obtained.
  • the dynamic model is used to continuously model the motion trajectory of the camera, so the posture of the camera (including speed, acceleration, and/or angular velocity, etc.) can be obtained at every moment, taking into account the inter-frame images
  • the physical constraints of the camera movement can ensure that the solved camera trajectory is a trajectory that meets the actual physical laws and has a continuous second-order continuous speed. This solves the problem of the related frame-by-frame method for solving the camera pose.
  • the time domain constraint between frames causes the final camera pose calculation result to jitter around the actual position, which brings harm to the control of robots or unmanned vehicles.
  • the camera attitude can be obtained at every moment, it can be fused with the data of the inertial sensor at any moment, which can make full use of the high frame rate of the inertial sensor, enhance the stability of the visual odometer, and reduce The problem of error accumulation is reduced.
  • the force acting on the camera can be decomposed into: the force F w whose direction and size remain unchanged in the world coordinate system, and the size remains unchanged in the camera coordinate system
  • the force F b and, the newly added force F u at time tu in the world coordinate system.
  • the combined force of the three forces F w , F b and F u is used to approximate the translational trajectory of the camera.
  • the camera is assumed to be mounted on a car, so the camera in Figure 2 Expressed in cars.
  • C w is the world coordinate system
  • C c is the camera coordinate system
  • V(t i ) is the speed of the camera at the time t i
  • V(t i + ⁇ t) is the speed of the camera at the time t i + ⁇ t
  • the motion trajectory of the camera can be divided into multiple segments under multiple time windows according to the F u acting on the camera. In the same time window, the motion of the camera is consistent.
  • the same optimized force acting on the camera and the angular velocity of the camera can be used to describe the posture of the camera.
  • the optimizing the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera includes: optimizing the objective function to obtain the optimized effect within a time window
  • the force on the camera and the angular velocity of the camera, and the start moment of the time window is determined by Fu .
  • the following describes how to establish the first model function used to describe the position change of the camera.
  • the first model function is:
  • S (t) is the camera at the time t the position in three-dimensional space
  • t i is the start time of the i-th time window
  • [Delta] t is the time in the i-th time window t and the time t i the difference
  • m is the quality of the camera
  • q(t) is the quaternion representation of the orientation of the camera at time t
  • R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t
  • T t i + ⁇ t in the i-th time window
  • t is any moment in the i-th time window.
  • the position change of the camera (that is, the translation trajectory of the camera) is expressed by the integral of each force (F w , F b and F u ).
  • the following describes how to establish a second model function used to express the orientation change of the camera.
  • the angular velocity of the camera can be linearly approximated, and the relationship between the angular velocity and the orientation is used to obtain the time-varying function of the orientation of the camera (ie, the second model function).
  • the angular velocity of the camera satisfies the following formula:
  • ⁇ (t) represents the angular velocity of the camera at time t
  • ⁇ ′(t) is the angular acceleration of the camera at time t
  • t i is the start time in the i-th time window
  • ⁇ t is the i-th time window the time t and the time difference t I.
  • q'(t) mathematically refers to the first derivative or first derivative of q(t), indicating the trend of change
  • q(t) is the quaternion representation of the orientation of the camera at time t
  • ⁇ (t) is the angular velocity of the camera at time t
  • t i is the starting time in the i-th time window
  • ⁇ t is the i-th time within the time window t and the time difference t i
  • (7) is quaternion multiplication.
  • first model function (s(t)) and second model function (q(t)) can be used to calculate the camera posture T cw (t) of each frame of image:
  • T cw (t) [R(q(t)),s(t)]
  • T cw (t) is the transformation from the world coordinate system to the camera coordinate system
  • s(t) is the position of the camera in the three-dimensional space at time t
  • q(t) is the direction of the camera at time t Quaternion representation
  • R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
  • multiple objective functions may be used, for example, an objective function obtained by minimizing the reprojection error may be used.
  • N is the number of image frames in the i-th time window
  • k is the count value of the image frames in the i-th time window
  • j is the count value of the feature points in the k-th frame image
  • N k is the total number of feature points in the k-th frame image
  • ⁇ ( ⁇ ) is the function of the projection of the three-dimensional point X to the two-dimensional
  • p is the two-dimensional feature point corresponding to the three-dimensional feature point
  • T cw (t) [R(q(t)), s(t)]
  • s(t) is the camera at time t
  • q(t) is the quaterni
  • an optimization algorithm can be used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera.
  • multiple types of optimization algorithms can be used, such as the Levenberg-Marquardt algorithm.
  • the posture of the camera (including speed, acceleration and/or angular velocity, etc.) can be obtained at every moment, it can be directly compared with the observation data (acceleration and angular velocity) of the inertial sensor. It is directly integrated with the visual positioning process. Please refer to Figure 4. There is no need to integrate the inertial sensor data to obtain the camera attitude and then perform the fusion. This can make full use of the high frame rate characteristics of the inertial sensor and enhance the stability of the visual odometer This reduces the problem of error accumulation.
  • the upper car trajectory curve in Figure 4 is the trajectory after optimization, and the lower curve is the initial value before optimization. During the optimization process, r will become smaller and smaller, the dynamic parameters obtained will be more accurate, and the trajectory obtained will also be More and more accurate.
  • F min is the minimum value of the artificially specified force
  • F max is the maximum value of the artificially specified force
  • ⁇ min is the minimum value of the artificially specified angular velocity
  • ⁇ max is the maximum value of the artificially specified angular velocity.
  • the method further includes: if the optimized F u is greater than or equal to a specified threshold, determining that a new time window starts, and the new time window The starting time is tu .
  • F u after if optimization is greater than or equal to a specified threshold, indicating that the new force F u has a significant effect, it is considered the current operating consistency is broken, need to start a time window, the time and emerging from F u t u
  • an embodiment of the present disclosure also provides a device for implementing a visual odometer, including:
  • the first establishing circuit 61 is configured to establish a first model function for expressing the position change of the camera, and the first model function uses the force acting on the camera to express the position change of the camera;
  • the second establishing circuit 62 is configured to establish a second model function used to express the orientation change of the camera, and the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
  • the objective function establishment circuit 63 is configured to obtain an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera observed by the inertial sensor;
  • the optimization circuit 64 is used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera.
  • the force acting on the camera can be decomposed into: the force F w whose direction and magnitude remain unchanged in the world coordinate system, the force F b whose magnitude remains the same in the camera coordinate system, and, in the world coordinate system at time t u newly added force F u;
  • the optimization circuit is also used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the starting time of the time window is determined by F u determine.
  • the first model function is:
  • S (t) is the camera at the time t the position in three-dimensional space
  • t i is the i th start time within the time window
  • [Delta] t is the time in the i-th time window t and the time t i the difference
  • m is the quality of the camera
  • q(t) is the quaternion representation of the orientation of the camera at time t
  • R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
  • the second model function is:
  • q(t) is the quaternion representation of the orientation of the camera at time t
  • ⁇ (t) is the angular velocity of the camera at time t
  • t i is the starting time in the i-th time window
  • ⁇ t is the i-th time within the time window t and the time difference t i
  • (7) is quaternion multiplication.
  • the objective function is:
  • ⁇ is the set of forces acting on the camera
  • ⁇ is the set of angular velocity of the camera
  • N is the number of frames of the image in the i-th time window
  • k is the The count value of the image frames in the i time window
  • j is the count value of the feature points in the k-th frame image
  • n k is the total number of feature points in the k-th frame image
  • ⁇ ( ⁇ ) is the three-dimensional point X projection
  • p is the two-dimensional feature point corresponding to the three-dimensional feature point
  • T cw (t) [R(q(t)), s(t)]
  • s(t) is the camera at time t
  • q(t) is the quaternion representation of the camera's orientation at time t
  • R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t
  • the device for implementing the visual odometer further includes:
  • the time window determination circuit is used to determine the start of a new time window if the optimized F u is greater than or equal to the specified threshold, and the start time of the new time window is tu .
  • circuits corresponding to the functions to be performed. It is easy to understand that these circuits are functional entities and do not necessarily correspond to physically or logically independent entities. . These functional entities can be implemented in the form of computer instructions by running software with corresponding functions through a general-purpose processor, or can be programmed to implement these functional entities in one or more hardware circuits or integrated circuits, or designed to specifically perform corresponding functions. Integrated circuits to realize these functional entities.
  • the embodiments of the present disclosure also provide a device for implementing a visual odometer, including a processor, a memory, and a computer program stored on the memory and running on the processor, the computer program being executed by the processor Time to realize the steps of the realization method of the above-mentioned visual odometer.
  • the embodiments of the present disclosure also provide a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the steps of the foregoing method for implementing the visual odometer are realized.
  • the processor may be a central processing unit (CPU), a field programmable logic array (FPGA), a single-chip microcomputer (MCU), a digital signal processor (DSP), an application specific integrated circuit (ASIC), etc.
  • Logic computing device with processing capability and/or program execution capability.
  • the computer-readable storage medium includes, but is not limited to, for example, volatile memory and/or non-volatile memory.
  • the volatile memory may include random access memory (RAM) and/or cache memory (Cache), for example.
  • the non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Signal Processing (AREA)
  • Studio Devices (AREA)
  • Image Analysis (AREA)

Abstract

A method and apparatus for implementing a visual odometer. The method comprises: setting up a first model function for representing position change in a camera, and using the force acting on the camera in the first model function to represent the position change in the camera (11); setting up a second model function for representing orientation change in the camera, and using the angular velocity of the camera in the second model function to represent the orientation change in the camera (12); obtaining a target function according to the first model function, the second model function, and the acceleration and angular velocity of the camera sensed by an inertial sensor (13); and optimizing the target function to obtain the optimized force acting on the camera and the angular velocity of the camera (14).

Description

视觉里程计的实现方法和实现装置Realizing method and realizing device of visual odometer
相关申请的交叉引用Cross references to related applications
本申请主张在2019年6月25日在中国提交的中国专利申请号No.201910554021.0的优先权，其全部内容通过引用包含于此。This application claims the priority of Chinese Patent Application No. 201910554021.0 filed in China on June 25, 2019, the entire content of which is incorporated herein by reference.
技术领域Technical field
本公开涉及视觉里程计技术领域，尤其涉及一种视觉里程计的实现方法和实现装置。The present disclosure relates to the technical field of visual odometry, and in particular to a realization method and realization device of visual odometry.
背景技术Background technique
视觉里程计的目标是根据摄像机拍摄的图像估计摄像机的运动轨迹和姿态。然而，相关技术中的视觉里程计得到的摄像机姿态的计算结果不准确。The goal of visual odometry is to estimate the motion trajectory and posture of the camera based on the images taken by the camera. However, the calculation result of the camera posture obtained by the visual odometry in the related art is not accurate.
发明内容Summary of the invention
本公开提供一种视觉里程计的实现方法，包括：The present disclosure provides a realization method of visual odometer, including:
建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；Establishing a first model function for expressing the position change of the camera, where the force acting on the camera is used in the first model function to express the position change of the camera;
建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；Establishing a second model function used to express the orientation change of the camera, where the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；Obtaining an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera observed by the inertial sensor;
对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。The objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera.
可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力F w，摄像机坐标系下大小保持不变的力F b，和，世界坐标系下在时刻t u新增加的力F u； Optionally, the force acting on the camera can be decomposed into: the force F w whose direction and magnitude remain unchanged in the world coordinate system, the force F b whose magnitude remains the same in the camera coordinate system, and, in the world coordinate system at time t u newly added force F u;
所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：The optimizing the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera includes:
对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F u确定。 The objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the start time of the time window is determined by Fu .
可选的，所述第一模型函数为：Optionally, the first model function is:
Figure PCTCN2020092539-appb-000001
Figure PCTCN2020092539-appb-000001
其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。 Wherein, S (t) is the camera at the time t the position in three-dimensional space, t i is the i th start time within the time window, [Delta] t is the time in the i-th time window t and the time t i the difference, m is the quality of the camera, q(t) is the quaternion representation of the orientation of the camera at time t, and R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
可选的，所述第二模型函数为：Optionally, the second model function is:
Figure PCTCN2020092539-appb-000002
Figure PCTCN2020092539-appb-000002
其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，о为四元数的乘法运算。 Where q(t) is the quaternion representation of the orientation of the camera at time t, ω(t) is the angular velocity of the camera at time t, t i is the starting time in the i-th time window, and Δt is the i-th time within the time window t and the time difference t i, о is quaternion multiplication.
可选的，所述目标函数为：Optionally, the objective function is:
Figure PCTCN2020092539-appb-000003
Figure PCTCN2020092539-appb-000003
其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
Figure PCTCN2020092539-appb-000004
是重投影误差，p为三维特征点对应的二维特征点，T cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a imu(t)为惯 性传感器在时刻t观测得到的加速度值，ω imu(t)为惯性传感器在时刻t观测得到的角速度值。 Where i is the count value of the time window, θ is the set of forces acting on the camera, φ is the set of angular velocity of the camera, N is the number of frames of the image in the i-th time window, and k is the The count value of the image frames in the i time window, j is the count value of the feature points in the k-th frame image, n k is the total number of feature points in the k-th frame image, and π(·) is the three-dimensional point X projection To a two-dimensional function,
Figure PCTCN2020092539-appb-000004
Is the reprojection error, p is the two-dimensional feature point corresponding to the three-dimensional feature point, T cw (t) = [R(q(t)), s(t)], s(t) is the camera at time t The position in the three-dimensional space, q(t) is the quaternion representation of the camera's orientation at time t, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t, and h is The specified weight value, ω(t) is the angular velocity of the camera at time t, α, β are specified values, m is the mass of the camera, and a imu (t) is the acceleration value observed by the inertial sensor at time t , Ω imu (t) is the angular velocity value observed by the inertial sensor at time t.
可选的，所述对所述目标函数进行优化之后，还包括：Optionally, after the optimization of the objective function, the method further includes:
若优化后的F u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t u。 If the optimized F u is greater than or equal to the specified threshold, it is determined that a new time window starts, and the start time of the new time window is tu .
本公开还提供一种视觉里程计的实现装置，包括：The present disclosure also provides a device for implementing visual odometer, including:
第一建立电路，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；A first establishing circuit for establishing a first model function for expressing the position change of the camera, the first model function adopts the force acting on the camera to express the position change of the camera;
第二建立电路，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；A second establishing circuit, configured to establish a second model function used to express the orientation change of the camera, and the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
目标函数建立电路，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；An objective function establishment circuit for obtaining an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera observed by the inertial sensor;
优化电路，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。The optimization circuit is used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera.
可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力F w，摄像机坐标系下大小保持不变的力F b，和，世界坐标系下在时刻t u新增加的力F u； Optionally, the force acting on the camera can be decomposed into: the force F w whose direction and magnitude remain unchanged in the world coordinate system, the force F b whose magnitude remains the same in the camera coordinate system, and, in the world coordinate system at time t u newly added force F u;
所述优化电路，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F u确定。 The optimization circuit is also used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the starting time of the time window is determined by F u determine.
可选的，所述第一模型函数为：Optionally, the first model function is:
Figure PCTCN2020092539-appb-000005
Figure PCTCN2020092539-appb-000005
其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。 Wherein, S (t) is the camera at the time t the position in three-dimensional space, t i is the i th start time within the time window, [Delta] t is the time in the i-th time window t and the time t i the difference, m is the quality of the camera, q(t) is the quaternion representation of the orientation of the camera at time t, and R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
可选的，所述第二模型函数为：Optionally, the second model function is:
Figure PCTCN2020092539-appb-000006
Figure PCTCN2020092539-appb-000006
其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，о为四元数的乘法运算。 Where q(t) is the quaternion representation of the orientation of the camera at time t, ω(t) is the angular velocity of the camera at time t, t i is the starting time in the i-th time window, and Δt is the i-th time within the time window t and the time difference t i, о is quaternion multiplication.
可选的，所述目标函数为：Optionally, the objective function is:
Figure PCTCN2020092539-appb-000007
Figure PCTCN2020092539-appb-000007
其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
Figure PCTCN2020092539-appb-000008
是重投影误差，p为三维特征点对应的二维特征点，T cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a imu(t)为惯性传感器在时刻t观测得到的加速度值，ω imu(t)为惯性传感器在时刻t观测得到的角速度值。 Where i is the count value of the time window, θ is the set of forces acting on the camera, φ is the set of angular velocity of the camera, N is the number of frames of the image in the i-th time window, and k is the The count value of the image frames in the i time window, j is the count value of the feature points in the k-th frame image, n k is the total number of feature points in the k-th frame image, and π(·) is the three-dimensional point X projection To a two-dimensional function,
Figure PCTCN2020092539-appb-000008
Is the reprojection error, p is the two-dimensional feature point corresponding to the three-dimensional feature point, T cw (t) = [R(q(t)), s(t)], s(t) is the camera at time t The position in the three-dimensional space, q(t) is the quaternion representation of the camera's orientation at time t, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t, and h is The specified weight value, ω(t) is the angular velocity of the camera at time t, α, β are specified values, m is the mass of the camera, and a imu (t) is the acceleration value observed by the inertial sensor at time t , Ω imu (t) is the angular velocity value observed by the inertial sensor at time t.
可选的，所述视觉里程计的实现装置还包括：Optionally, the device for implementing the visual odometer further includes:
时间窗确定电路，用于若优化后的F u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t u。 The time window determination circuit is used to determine the start of a new time window if the optimized F u is greater than or equal to the specified threshold, and the start time of the new time window is tu .
本公开还提供一种视觉里程计的实现装置，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述视觉里程计的实现方法的步骤。The present disclosure also provides a device for implementing a visual odometer, including a processor, a memory, and a computer program stored in the memory and capable of running on the processor, and the computer program is implemented when the processor is executed The steps of the realization method of the above-mentioned visual odometer.
本公开还提供一种计算机可读存储介质，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现上述视觉里程计的实现 方法的步骤。The present disclosure also provides a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the steps of the foregoing method for implementing the visual odometer are realized.
附图说明Description of the drawings
为了更清楚地说明本公开实施例的技术方案，下面将对本公开实施例的描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。In order to explain the technical solutions of the embodiments of the present disclosure more clearly, the following will briefly introduce the drawings that need to be used in the description of the embodiments of the present disclosure. Obviously, the drawings in the following description are only some embodiments of the present disclosure. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative labor.
图1为本公开实施例的视觉里程计的实现方法的流程示意图；FIG. 1 is a schematic flowchart of a method for implementing a visual odometer according to an embodiment of the disclosure;
图2为本公开实施例的作用在摄像机上的力的分解示意图；FIG. 2 is an exploded schematic diagram of the force acting on the camera according to the embodiment of the disclosure;
图3为本公开实施例的摄像机的角速度与摄像机的朝向的关系示意图；3 is a schematic diagram of the relationship between the angular velocity of the camera and the orientation of the camera in an embodiment of the disclosure;
图4为本公开实施例的动力学模型的优化方法的示意图；FIG. 4 is a schematic diagram of an optimization method of a kinetic model according to an embodiment of the disclosure;
图5为本公开实施例的对摄像机运动轨迹的分段方法示意图；FIG. 5 is a schematic diagram of a method for segmenting a camera motion track according to an embodiment of the disclosure;
图6为本公开实施例的视觉里程计的实现装置的结构示意图。Fig. 6 is a schematic structural diagram of a device for implementing a visual odometer according to an embodiment of the disclosure.
具体实施方式Detailed ways
为使本公开实施例的目的、技术方案和优点更加清楚，下面将结合本公开实施例的附图，对本公开实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本公开的一部分实施例，而不是全部的实施例。基于所描述的本公开的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本公开保护的范围。In order to make the objectives, technical solutions, and advantages of the embodiments of the present disclosure clearer, the technical solutions of the embodiments of the present disclosure will be described clearly and completely in conjunction with the accompanying drawings of the embodiments of the present disclosure. Obviously, the described embodiments are part of the embodiments of the present disclosure, rather than all of the embodiments. Based on the described embodiments of the present disclosure, all other embodiments obtained by a person of ordinary skill in the art fall within the protection scope of the present disclosure.
相关技术中的视觉里程计的实现方法中，没有考虑摄像机采集的每帧图像之间的时域约束关系，独立求解每帧图像的摄像机姿态，忽略了连续帧图像之间摄像机运动的物理约束，这样导致最终的摄像机姿态计算结果会在实际位置周围抖动，不具有连续性且不符合轨迹应该满足的物理约束，会对机器人、自动驾驶的控制带来致命问题。有鉴于此，本公开提供一种视觉里程计的实现方法以及和实现装置。In the implementation method of visual odometry in the related art, the time domain constraint relationship between each frame of images collected by the camera is not considered, and the camera pose of each frame of image is solved independently, ignoring the physical constraints of camera movement between consecutive frames. As a result, the final camera posture calculation result will jitter around the actual position, which does not have continuity and does not meet the physical constraints that the trajectory should meet, which will bring fatal problems to the control of robots and automatic driving. In view of this, the present disclosure provides an implementation method and implementation device of a visual odometer.
请参考图1，图1为本公开实施例的视觉里程计的实现方法的流程示意图，该视觉里程计的实现方法包括：Please refer to FIG. 1. FIG. 1 is a schematic flowchart of a method for implementing a visual odometer according to an embodiment of the present disclosure. The method for implementing the visual odometer includes:
步骤11：建立用于表述摄像机的位置变化的第一模型函数，所述第一模 型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；Step 11: Establish a first model function for expressing the position change of the camera, where the force acting on the camera is used in the first model function to express the position change of the camera;
即采用作用在摄像机上的力建模摄像机的位置变化(即平移轨迹)。That is, the force acting on the camera is used to model the position change of the camera (ie, the translation trajectory).
步骤12：建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；Step 12: Establish a second model function used to express the orientation change of the camera, where the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
即采用摄像机的角速度建模摄像机的朝向变化。That is, the angular velocity of the camera is used to model the orientation change of the camera.
步骤13：根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；Step 13: Obtain an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera observed by the inertial sensor;
步骤14：对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。Step 14: optimizing the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera.
作用在摄像机上的力与摄像机的加速度相关，加速度等于作用在摄像机上的合力除以摄像机的质量。因此得到作用在摄像机的力，即可得到摄像机的加速度。The force acting on the camera is related to the acceleration of the camera. The acceleration is equal to the resultant force acting on the camera divided by the mass of the camera. Therefore, by obtaining the force acting on the camera, the acceleration of the camera can be obtained.
本公开实施例中，采用动力学模型对摄像机的运动轨迹进行连续建模，故在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，考虑了连续帧图像之间摄像机运动的物理约束，可以保证求解得到的摄像机轨迹是满足实际的物理规律的且具有速度连续的二阶连续性质的轨迹，从而解决了相关的逐帧求解摄像机姿态的方法中，没有考虑帧与帧之间的时域约束导致最终的摄像机姿态计算结果会在实际位置周围抖动，对机器人或无人驾驶车的控制带来危害的问题。In the embodiments of the present disclosure, the dynamic model is used to continuously model the motion trajectory of the camera, so the posture of the camera (including speed, acceleration, and/or angular velocity, etc.) can be obtained at every moment, taking into account the inter-frame images The physical constraints of the camera movement can ensure that the solved camera trajectory is a trajectory that meets the actual physical laws and has a continuous second-order continuous speed. This solves the problem of the related frame-by-frame method for solving the camera pose. The time domain constraint between frames causes the final camera pose calculation result to jitter around the actual position, which brings harm to the control of robots or unmanned vehicles.
同时，由于在每一个时刻都可以得到摄像机的姿态，故可以在任意时刻与惯性传感器的数据进行融合，这样能够充分利用惯性传感器的高帧率的特点，增强了视觉里程计的稳定性，减小了误差累积的问题。At the same time, since the camera attitude can be obtained at every moment, it can be fused with the data of the inertial sensor at any moment, which can make full use of the high frame rate of the inertial sensor, enhance the stability of the visual odometer, and reduce The problem of error accumulation is reduced.
在本公开的一些实施例中，请参考图2，可以将作用在所述摄像机上的力分解成：世界坐标系下方向和大小保持不变的力F w，摄像机坐标系下大小保持不变的力F b，和，世界坐标系下在时刻t u新增加的力F u。图2所示的实施例中，使用三个力F w、F b和F u的合力近似摄像机的平移轨迹，图2所示的实施例中，假设摄像机架设在汽车上，故图2中摄像机采用汽车表示。图2中C w为世界坐标系，C c为摄像机坐标系，V(t i)为摄像机在t i时刻的速度，V(t i+Δt)为摄像机在t i+Δt时刻的速度，ω为摄像机的角速度。 In some embodiments of the present disclosure, referring to Fig. 2, the force acting on the camera can be decomposed into: the force F w whose direction and size remain unchanged in the world coordinate system, and the size remains unchanged in the camera coordinate system The force F b , and, the newly added force F u at time tu in the world coordinate system. In the embodiment shown in Figure 2, the combined force of the three forces F w , F b and F u is used to approximate the translational trajectory of the camera. In the embodiment shown in Figure 2, the camera is assumed to be mounted on a car, so the camera in Figure 2 Expressed in cars. In Figure 2, C w is the world coordinate system, C c is the camera coordinate system, V(t i ) is the speed of the camera at the time t i , V(t i +Δt) is the speed of the camera at the time t i +Δt, ω Is the angular velocity of the camera.
本公开实施例中，可以根据作用在所述摄像机上的F u，将摄像机的运动轨迹划分为多个时间窗下的多个分段，在同一个时间窗内，摄像机的运动具有一致性，可以采用相同的优化后的作用在所述摄像机上的力和所述摄像机的角速度描述摄像头的姿态。 In the embodiments of the present disclosure, the motion trajectory of the camera can be divided into multiple segments under multiple time windows according to the F u acting on the camera. In the same time window, the motion of the camera is consistent. The same optimized force acting on the camera and the angular velocity of the camera can be used to describe the posture of the camera.
即，所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F u确定。 That is, the optimizing the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera includes: optimizing the objective function to obtain the optimized effect within a time window The force on the camera and the angular velocity of the camera, and the start moment of the time window is determined by Fu .
当然，在本公开的其他一些实施例中，也可以只考虑作用在所述摄像机上F w，不考虑F b和F u，从而使得动力学模型更加简化。 Of course, in some other embodiments of the present disclosure, only F w acting on the camera may be considered, and F b and F u are not considered, thereby simplifying the dynamic model.
下面对如何建立用于表述摄像机的位置变化的第一模型函数进行说明。The following describes how to establish the first model function used to describe the position change of the camera.
本公开实施例中，可选的，所述第一模型函数为：In the embodiment of the present disclosure, optionally, the first model function is:
Figure PCTCN2020092539-appb-000009
Figure PCTCN2020092539-appb-000009
其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t i是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与t i的时间差，m是所述摄像机的质量，q(t)是所述摄像机在时刻t的朝向的四元数表示，R[q(t)是在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，第i个时间窗内的t＝t i+Δt，t是第i个时间窗内的任意时刻。 Wherein, S (t) is the camera at the time t the position in three-dimensional space, t i is the start time of the i-th time window, [Delta] t is the time in the i-th time window t and the time t i the difference, m is the quality of the camera, q(t) is the quaternion representation of the orientation of the camera at time t, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t, T=t i +Δt in the i-th time window, t is any moment in the i-th time window.
上述公式中，摄像机的位置变化(即摄像机的平移轨迹)采用各个力(F w、F b和F u)的积分进行表示。 In the above formula, the position change of the camera (that is, the translation trajectory of the camera) is expressed by the integral of each force (F w , F b and F u ).
下面对如何建立用于表述所述摄像机的朝向变化的第二模型函数进行说明。The following describes how to establish a second model function used to express the orientation change of the camera.
本公开实施例中，请参考图3，可以对摄像机的角速度进行线性近似，使用角速度和朝向之间的关系，得到摄像机的朝向随时间的变化函数(即第二模型函数)。In the embodiments of the present disclosure, referring to FIG. 3, the angular velocity of the camera can be linearly approximated, and the relationship between the angular velocity and the orientation is used to obtain the time-varying function of the orientation of the camera (ie, the second model function).
摄像机的角速度满足以下公式：The angular velocity of the camera satisfies the following formula:
ω(t i+Δt)＝ω(t i)+ω′(t i)Δt……(1) ω(t i +Δt)=ω(t i )+ω′(t i )Δt……(1)
其中，ω(t)表示摄像机在时刻t的角速度，ω′(t)为摄像机在时刻t的角加 速度，t i是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与t i的时间差。 Among them, ω(t) represents the angular velocity of the camera at time t, ω′(t) is the angular acceleration of the camera at time t, t i is the start time in the i-th time window, and Δt is the i-th time window the time t and the time difference t I.
摄像机的朝向q(t)与角速度满足以下公式：The camera's orientation q(t) and angular velocity satisfy the following formula:
Figure PCTCN2020092539-appb-000010
Figure PCTCN2020092539-appb-000010
其中，q’(t)数学上指的是q(t)的一阶导数或一阶微分，表示朝向变化趋势，о为指四元数的乘法运算，t＝t i+Δt。 Among them, q'(t) mathematically refers to the first derivative or first derivative of q(t), indicating the trend of change, о refers to the multiplication operation of quaternion, t=t i +Δt.
对q(t)进行二阶泰勒展开并代入上述公式(1)，在去掉高阶项后可得摄像机朝向的函数表达(即第二模型函数)：Carry out the second-order Taylor expansion of q(t) and substitute it into the above formula (1). After removing the high-order terms, the function expression of the camera orientation (that is, the second model function) can be obtained:
Figure PCTCN2020092539-appb-000011
Figure PCTCN2020092539-appb-000011
其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t i是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与t i的时间差，о为四元数的乘法运算。 Where q(t) is the quaternion representation of the orientation of the camera at time t, ω(t) is the angular velocity of the camera at time t, t i is the starting time in the i-th time window, and Δt is the i-th time within the time window t and the time difference t i, о is quaternion multiplication.
下面对如何获得目标函数进行说明。The following describes how to obtain the objective function.
本公开实施例中，可以采用上述第一模型函数(s(t))、第二模型函数(q(t))对每帧图像的摄像机姿态T cw(t)进行计算： In the embodiments of the present disclosure, the above-mentioned first model function (s(t)) and second model function (q(t)) can be used to calculate the camera posture T cw (t) of each frame of image:
T cw(t)＝[R(q(t)),s(t)] T cw (t)=[R(q(t)),s(t)]
T cw(t)为从世界坐标系变换到摄像机坐标系的变换，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。 T cw (t) is the transformation from the world coordinate system to the camera coordinate system, s(t) is the position of the camera in the three-dimensional space at time t, and q(t) is the direction of the camera at time t Quaternion representation, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
本公开实施例中，可以采用多种目标函数，例如可以采用最小化重投影误差得到的目标函数。In the embodiments of the present disclosure, multiple objective functions may be used, for example, an objective function obtained by minimizing the reprojection error may be used.
如果是采用最小化重投影误差得到的目标函数，上述计算出T cw(t)之后，还需要计算重投影误差π(T(t)X)-p。 If the objective function obtained by minimizing the reprojection error is used, after calculating T cw (t) above, the reprojection error π(T(t)X)-p needs to be calculated.
然后，得到目标函数：Then, get the objective function:
Figure PCTCN2020092539-appb-000012
Figure PCTCN2020092539-appb-000012
其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，即θ＝{F w,F b,F u}，φ为所述摄像机的角速度的集合，即φ＝{ω,ω′}，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
Figure PCTCN2020092539-appb-000013
是重投影误差，p为三维特征点对应的二维特征点，T cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a imu(t)为惯性传感器在时刻t观测得到的加速度值，ω imu(t)为惯性传感器在时刻t观测得到的角速度值。 Where i is the count value of the time window, θ is the set of forces acting on the camera, namely θ={F w ,F b ,F u }, and φ is the set of angular velocities of the camera, namely φ= {ω,ω′}, N is the number of image frames in the i-th time window, k is the count value of the image frames in the i-th time window, j is the count value of the feature points in the k-th frame image , N k is the total number of feature points in the k-th frame image, π(·) is the function of the projection of the three-dimensional point X to the two-dimensional,
Figure PCTCN2020092539-appb-000013
Is the reprojection error, p is the two-dimensional feature point corresponding to the three-dimensional feature point, T cw (t) = [R(q(t)), s(t)], s(t) is the camera at time t The position in the three-dimensional space, q(t) is the quaternion representation of the camera's orientation at time t, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t, and h is The specified weight value, ω(t) is the angular velocity of the camera at time t, α, β are specified values, m is the mass of the camera, and a imu (t) is the acceleration value observed by the inertial sensor at time t , Ω imu (t) is the angular velocity value observed by the inertial sensor at time t.
得到上述目标函数之后，可以采用优化算法对目标函数进行优化，得到优化后的作用在摄像机上的力和摄像机的角速度。本公开实施例中，可以采用多种类型的优化算法，例如Levenberg-Marquardt算法等。After the above objective function is obtained, an optimization algorithm can be used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera. In the embodiments of the present disclosure, multiple types of optimization algorithms can be used, such as the Levenberg-Marquardt algorithm.
上述公式中的后两项(
Figure PCTCN2020092539-appb-000014
和β‖ω(t)-ω imu(t)‖ 2)为惯性传感器的约束误差项，即，直接使用动力学模型中的动力学参数与惯性传感器数据进行比较，定义误差。 The last two items in the above formula (
Figure PCTCN2020092539-appb-000014
And β‖ω(t)-ω imu (t)‖ 2 ) are the constraint error terms of the inertial sensor, that is, the dynamic parameters in the dynamic model are directly used to compare with the inertial sensor data to define the error.
本公开实施例中，由于在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，故可以直接与惯性传感器的观测数据(加速度和角速度)做比较，惯性传感器的数据直接与视觉定位过程深度融合，请参考图4，不需要再将惯性传感器的数据进行积分得到摄像机姿态再做融合，这样能够充分利用惯性传感器的高帧率的特点，增强了视觉里程计的稳定性，减小了误差累积的问题。In the embodiments of the present disclosure, since the posture of the camera (including speed, acceleration and/or angular velocity, etc.) can be obtained at every moment, it can be directly compared with the observation data (acceleration and angular velocity) of the inertial sensor. It is directly integrated with the visual positioning process. Please refer to Figure 4. There is no need to integrate the inertial sensor data to obtain the camera attitude and then perform the fusion. This can make full use of the high frame rate characteristics of the inertial sensor and enhance the stability of the visual odometer This reduces the problem of error accumulation.
图4中，θ为作用在所述摄像机上的力的集合，即θ＝{F w,F b,F u}，φ为所述摄像机的角速度的集合，即φ＝{ω,ω′}，
Figure PCTCN2020092539-appb-000015
为即上述目标函数中的
Figure PCTCN2020092539-appb-000016
图4中上面的汽车轨迹曲线是优化之后的轨迹，下面的曲线是优化之前的初始值， 优化过程中，会让r越来越小，得到的动力学参数也更加准确，得到的轨迹也会越来越准确。 In Figure 4, θ is the set of forces acting on the camera, that is θ={F w ,F b ,F u }, and φ is the set of angular velocities of the camera, that is, φ={ω,ω′} ,
Figure PCTCN2020092539-appb-000015
Is that in the above objective function
Figure PCTCN2020092539-appb-000016
The upper car trajectory curve in Figure 4 is the trajectory after optimization, and the lower curve is the initial value before optimization. During the optimization process, r will become smaller and smaller, the dynamic parameters obtained will be more accurate, and the trajectory obtained will also be More and more accurate.
本公开实施例中，由于真实世界中的力和角速度应当在合理的范围内，可选的，还可以加入了这些动力学参数(力和角速度)的范围约束：
Figure PCTCN2020092539-appb-000017
In the embodiments of the present disclosure, since the forces and angular velocities in the real world should be within a reasonable range, optionally, the range constraints of these dynamic parameters (forces and angular velocities) can be added:
Figure PCTCN2020092539-appb-000017
其中，F min为人为指定的力的最小值，F max为人为指定的力的最大值，ω min为人为指定的角速度的最小值，ω max为人为指定的角速度的最大值。 Among them, F min is the minimum value of the artificially specified force, F max is the maximum value of the artificially specified force, ω min is the minimum value of the artificially specified angular velocity, and ω max is the maximum value of the artificially specified angular velocity.
本公开实施例中，可选的，所述对所述目标函数进行优化之后，还包括：若优化后的F u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t u。即如果优化后的F u大于或等于指定阈值，说明新增力F u具有显著的作用，则认为当前的运行一致性被破坏，需要重新开始一个时间窗，并从F u出现的时刻t u开始，利用新的动力学参数描述摄像机的运动轨迹，如图5所示。图5中的状态变化函数即上述T cw(t)＝[R(q(t)),s(t)]，将时间窗开始时刻的角速度、位置等信息，变化到t时刻的角速度、位置等信息。从图5中可以看出，将摄像机的运动轨迹划分为多个分段，每一分段对应一个时间窗(时间窗i、j和k)。 In the embodiment of the present disclosure, optionally, after the optimization of the objective function, the method further includes: if the optimized F u is greater than or equal to a specified threshold, determining that a new time window starts, and the new time window The starting time is tu . I.e. F u after if optimization is greater than or equal to a specified threshold, indicating that the new force F u has a significant effect, it is considered the current operating consistency is broken, need to start a time window, the time and emerging from F u t u At the beginning, use new dynamic parameters to describe the camera's motion trajectory, as shown in Figure 5. The state change function in Figure 5 is the above-mentioned T cw (t)=[R(q(t)),s(t)], which changes the angular velocity and position at the beginning of the time window to the angular velocity and position at time t And other information. It can be seen from Figure 5 that the motion track of the camera is divided into multiple segments, and each segment corresponds to a time window (time windows i, j, and k).
请参考图6，本公开实施例还提供一种视觉里程计的实现装置，包括：Please refer to FIG. 6, an embodiment of the present disclosure also provides a device for implementing a visual odometer, including:
第一建立电路61，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；The first establishing circuit 61 is configured to establish a first model function for expressing the position change of the camera, and the first model function uses the force acting on the camera to express the position change of the camera;
第二建立电路62，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；The second establishing circuit 62 is configured to establish a second model function used to express the orientation change of the camera, and the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
目标函数建立电路63，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；The objective function establishment circuit 63 is configured to obtain an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera observed by the inertial sensor;
优化电路64，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。The optimization circuit 64 is used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera.
可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力F w，摄像机坐标系下大小保持不变的力F b，和，世界坐标系 下在时刻t u新增加的力F u； Optionally, the force acting on the camera can be decomposed into: the force F w whose direction and magnitude remain unchanged in the world coordinate system, the force F b whose magnitude remains the same in the camera coordinate system, and, in the world coordinate system at time t u newly added force F u;
所述优化电路，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F u确定。 The optimization circuit is also used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the starting time of the time window is determined by F u determine.
可选的，所述第一模型函数为：Optionally, the first model function is:
Figure PCTCN2020092539-appb-000018
Figure PCTCN2020092539-appb-000018
其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。 Wherein, S (t) is the camera at the time t the position in three-dimensional space, t i is the i th start time within the time window, [Delta] t is the time in the i-th time window t and the time t i the difference, m is the quality of the camera, q(t) is the quaternion representation of the orientation of the camera at time t, and R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
可选的，所述第二模型函数为：Optionally, the second model function is:
Figure PCTCN2020092539-appb-000019
Figure PCTCN2020092539-appb-000019
其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，о为四元数的乘法运算。 Where q(t) is the quaternion representation of the orientation of the camera at time t, ω(t) is the angular velocity of the camera at time t, t i is the starting time in the i-th time window, and Δt is the i-th time within the time window t and the time difference t i, о is quaternion multiplication.
可选的，所述目标函数为：Optionally, the objective function is:
Figure PCTCN2020092539-appb-000020
Figure PCTCN2020092539-appb-000020
其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
Figure PCTCN2020092539-appb-000021
是重投影误差，p为三维特征点对应的二维特征点，T cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐 标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a imu(t)为惯性传感器在时刻t观测得到的加速度值，ω imu(t)为惯性传感器在时刻t观测得到的角速度值。 Where i is the count value of the time window, θ is the set of forces acting on the camera, φ is the set of angular velocity of the camera, N is the number of frames of the image in the i-th time window, and k is the The count value of the image frames in the i time window, j is the count value of the feature points in the k-th frame image, n k is the total number of feature points in the k-th frame image, and π(·) is the three-dimensional point X projection To a two-dimensional function,
Figure PCTCN2020092539-appb-000021
Is the reprojection error, p is the two-dimensional feature point corresponding to the three-dimensional feature point, T cw (t) = [R(q(t)), s(t)], s(t) is the camera at time t The position in the three-dimensional space, q(t) is the quaternion representation of the camera's orientation at time t, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t, and h is The specified weight value, ω(t) is the angular velocity of the camera at time t, α, β are specified values, m is the mass of the camera, and a imu (t) is the acceleration value observed by the inertial sensor at time t , Ω imu (t) is the angular velocity value observed by the inertial sensor at time t.
可选的，所述视觉里程计的实现装置还包括：Optionally, the device for implementing the visual odometer further includes:
时间窗确定电路，用于若优化后的F u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t u。 The time window determination circuit is used to determine the start of a new time window if the optimized F u is greater than or equal to the specified threshold, and the start time of the new time window is tu .
在本公开的实施例中，为了便于理解和描述，使用了与所要执行的功能对应的电路的描述方式，容易理解，这些电路是功能实体，不一定必须与物理或逻辑上独立的实体相对应。可以通过通用处理器运行对应功能的软件以执行计算机指令的形式来实现这些功能实体，或在一个或多个硬件电路或集成电路中可编程地实现这些功能实体，或设计为专门执行对应功能的集成电路来实现这些功能实体。In the embodiments of the present disclosure, in order to facilitate understanding and description, a description method of circuits corresponding to the functions to be performed is used. It is easy to understand that these circuits are functional entities and do not necessarily correspond to physically or logically independent entities. . These functional entities can be implemented in the form of computer instructions by running software with corresponding functions through a general-purpose processor, or can be programmed to implement these functional entities in one or more hardware circuits or integrated circuits, or designed to specifically perform corresponding functions. Integrated circuits to realize these functional entities.
本公开实施例还提供一种视觉里程计的实现装置，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述视觉里程计的实现方法的步骤。The embodiments of the present disclosure also provide a device for implementing a visual odometer, including a processor, a memory, and a computer program stored on the memory and running on the processor, the computer program being executed by the processor Time to realize the steps of the realization method of the above-mentioned visual odometer.
本公开实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现上述视觉里程计的实现方法的步骤。The embodiments of the present disclosure also provide a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the steps of the foregoing method for implementing the visual odometer are realized.
在本公开的实施例中，处理器可以是中央处理器(CPU)、现场可编程逻辑阵列(FPGA)、单片机(MCU)、数字信号处理器(DSP)、专用集成电路(ASIC)等具有数据处理能力和/或程序执行能力的逻辑运算器件。In the embodiment of the present disclosure, the processor may be a central processing unit (CPU), a field programmable logic array (FPGA), a single-chip microcomputer (MCU), a digital signal processor (DSP), an application specific integrated circuit (ASIC), etc. Logic computing device with processing capability and/or program execution capability.
在本公开的实施例中，所述计算机可读存储介质包括但不限于例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(Cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。In the embodiment of the present disclosure, the computer-readable storage medium includes, but is not limited to, for example, volatile memory and/or non-volatile memory. The volatile memory may include random access memory (RAM) and/or cache memory (Cache), for example. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc.
除非另作定义，本公开中使用的技术术语或者科学术语应当为本公开所属领域内具有一般技能的人士所理解的通常意义。本公开中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区 分不同的组成部分。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也相应地改变。Unless otherwise defined, the technical terms or scientific terms used in this disclosure shall have the usual meanings understood by those with ordinary skills in the field to which this disclosure belongs. The "first", "second" and similar words used in the present disclosure do not denote any order, quantity or importance, but are only used to distinguish different components. Similar words such as "connected" or "connected" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "Down", "Left", "Right", etc. are only used to indicate the relative position relationship. When the absolute position of the object being described changes, the relative position relationship also changes accordingly.
以上所述是本公开的可选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本公开所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本公开的保护范围。The above are optional implementations of the present disclosure. It should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present disclosure, several improvements and modifications can be made. These improvements and modifications It should also be regarded as the protection scope of the present disclosure.

Claims (17)

  1. 一种视觉里程计的实现方法，包括：A realization method of visual odometer includes:
    建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；Establishing a first model function for expressing the position change of the camera, where the force acting on the camera is used in the first model function to express the position change of the camera;
    建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；Establishing a second model function used to express the orientation change of the camera, where the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
    根据所述第一模型函数、所述第二模型函数以及惯性传感器获得的所述摄像机的加速度和角速度，得到目标函数；Obtaining an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera obtained by the inertial sensor;
    对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。The objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera.
  2. 如权利要求1所述的视觉里程计的实现方法，其中，作用在所述摄像机上的力分解成：世界坐标系下方向和大小保持不变的力F w，摄像机坐标系下大小保持不变的力F b，和，世界坐标系下在时刻t u新增加的力F u； The method for implementing a visual odometer according to claim 1, wherein the force acting on the camera is decomposed into: a force F w whose direction and size remain unchanged in the world coordinate system, and the size remains unchanged in the camera coordinate system The force F b , and, the newly added force F u at time tu in the world coordinate system;
    所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：The optimizing the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera includes:
    对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F u确定。 The objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the start time of the time window is determined by Fu .
  3. 如权利要求2所述的视觉里程计的实现方法，其中，所述第一模型函数为：3. The method for implementing visual odometry according to claim 2, wherein the first model function is:
    Figure PCTCN2020092539-appb-100001
    Figure PCTCN2020092539-appb-100001
    其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。 Wherein, S (t) is the camera at the time t the position in three-dimensional space, t i is the i th start time within the time window, [Delta] t is the time in the i-th time window t and the time t i the difference, m is the quality of the camera, q(t) is the quaternion representation of the orientation of the camera at time t, and R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
  4. 如权利要求2所述的视觉里程计的实现方法，其中，所述第二模型函数为：3. The method for implementing visual odometer according to claim 2, wherein the second model function is:
    Figure PCTCN2020092539-appb-100002
    Figure PCTCN2020092539-appb-100002
    其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，ο为四元数的乘法运算。 Where q(t) is the quaternion representation of the orientation of the camera at time t, ω(t) is the angular velocity of the camera at time t, t i is the starting time in the i-th time window, and Δt is the i-th time within the time window t and the time difference t i, ο quaternion multiplication.
  5. 如权利要求2所述的视觉里程计的实现方法，其中，所述目标函数为：The method for implementing visual odometer according to claim 2, wherein the objective function is:
    Figure PCTCN2020092539-appb-100003
    Figure PCTCN2020092539-appb-100003
    其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
    Figure PCTCN2020092539-appb-100004
    是重投影误差，p为三维特征点对应的二维特征点，T cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a imu(t)为惯性传感器在时刻t观测得到的加速度值，ω imu(t)为惯性传感器在时刻t观测得到的角速度值。     Where i is the count value of the time window, θ is the set of forces acting on the camera, φ is the set of angular velocity of the camera, N is the number of frames of the image in the i-th time window, and k is the The count value of the image frames in the i time window, j is the count value of the feature points in the k-th frame image, n k is the total number of feature points in the k-th frame image, and π(·) is the three-dimensional point X projection To a two-dimensional function,
    Figure PCTCN2020092539-appb-100004
    Is the reprojection error, p is the two-dimensional feature point corresponding to the three-dimensional feature point, T cw (t) = [R(q(t)), s(t)], s(t) is the camera at time t The position in the three-dimensional space, q(t) is the quaternion representation of the camera's orientation at time t, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t, and h is The specified weight value, ω(t) is the angular velocity of the camera at time t, α, β are specified values, m is the mass of the camera, and a imu (t) is the acceleration value observed by the inertial sensor at time t , Ω imu (t) is the angular velocity value observed by the inertial sensor at time t.
  6. 如权利要求2所述的视觉里程计的实现方法，其中，所述对所述目标函数进行优化之后，还包括：3. The method for implementing a visual odometer according to claim 2, wherein, after optimizing the objective function, the method further comprises:
    若优化后的F u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t u。 If the optimized F u is greater than or equal to the specified threshold, it is determined that a new time window starts, and the start time of the new time window is tu .
  7. 一种视觉里程计的实现装置，包括：A realization device of visual odometer, including:
    第一建立电路，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；A first establishing circuit for establishing a first model function for expressing the position change of the camera, the first model function adopts the force acting on the camera to express the position change of the camera;
    第二建立电路，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；A second establishing circuit, configured to establish a second model function used to express the orientation change of the camera, and the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
    目标函数建立电路，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；An objective function establishment circuit for obtaining an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera observed by the inertial sensor;
    优化电路，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。The optimization circuit is used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera.
  8. 如权利要求7所述的视觉里程计的实现装置，其中，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力F w，摄像机坐标系下大小保持不变的力F b，和，世界坐标系下在时刻t u新增加的力F u； The device for implementing a visual odometer according to claim 7, wherein the force acting on the camera can be decomposed into: a force F w whose direction and magnitude remain unchanged in the world coordinate system, and the magnitude remains unchanged in the camera coordinate system The variable force F b , and, the newly added force F u at time tu in the world coordinate system;
    所述优化电路，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F u确定。 The optimization circuit is also used to optimize the objective function to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the starting time of the time window is determined by F u determine.
  9. 如权利要求8所述的视觉里程计的实现装置，其中，所述第一模型函数为：8. The device for implementing visual odometer according to claim 8, wherein the first model function is:
    Figure PCTCN2020092539-appb-100005
    Figure PCTCN2020092539-appb-100005
    其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。 Wherein, S (t) is the camera at the time t the position in three-dimensional space, t i is the i th start time within the time window, [Delta] t is the time in the i-th time window t and the time t i the difference, m is the quality of the camera, q(t) is the quaternion representation of the orientation of the camera at time t, and R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t.
  10. 如权利要求8所述的视觉里程计的实现装置，其中，所述第二模型函数为：The device for implementing visual odometer according to claim 8, wherein the second model function is:
    Figure PCTCN2020092539-appb-100006
    Figure PCTCN2020092539-appb-100006
    其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，ο为四元数的乘法运算。 Where q(t) is the quaternion representation of the orientation of the camera at time t, ω(t) is the angular velocity of the camera at time t, t i is the starting time in the i-th time window, and Δt is the i-th time within the time window t and the time difference t i, ο quaternion multiplication.
  11. 如权利要求8所述的视觉里程计的实现装置，其中，所述目标函数为：The device for implementing visual odometer according to claim 8, wherein the objective function is:
    Figure PCTCN2020092539-appb-100007
    Figure PCTCN2020092539-appb-100007
    其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
    Figure PCTCN2020092539-appb-100008
    是重投影误差，p为三维特征点对应的二维特征点，T cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a imu(t)为惯性传感器在时刻t观测得到的加速度值，ω imu(t)为惯性传感器在时刻t观测得到的角速度值。     Where i is the count value of the time window, θ is the set of forces acting on the camera, φ is the set of angular velocity of the camera, N is the number of frames of the image in the i-th time window, and k is the The count value of the image frames in the i time window, j is the count value of the feature points in the k-th frame image, n k is the total number of feature points in the k-th frame image, and π(·) is the three-dimensional point X projection To a two-dimensional function,
    Figure PCTCN2020092539-appb-100008
    Is the reprojection error, p is the two-dimensional feature point corresponding to the three-dimensional feature point, T cw (t) = [R(q(t)), s(t)], s(t) is the camera at time t The position in the three-dimensional space, q(t) is the quaternion representation of the camera's orientation at time t, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t, and h is The specified weight value, ω(t) is the angular velocity of the camera at time t, α, β are specified values, m is the mass of the camera, and a imu (t) is the acceleration value observed by the inertial sensor at time t , Ω imu (t) is the angular velocity value observed by the inertial sensor at time t.
  12. 如权利要求8所述的视觉里程计的实现装置，其中，还包括：The device for realizing a visual odometer according to claim 8, further comprising:
    时间窗确定电路，用于若优化后的F u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t u。 The time window determination circuit is used to determine the start of a new time window if the optimized F u is greater than or equal to the specified threshold, and the start time of the new time window is tu .
  13. 一种视觉里程计的实现装置，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现：A device for implementing a visual odometer includes a processor, a memory, and a computer program stored on the memory and running on the processor, and the computer program is implemented when the processor is executed:
    建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；Establishing a first model function for expressing the position change of the camera, where the force acting on the camera is used in the first model function to express the position change of the camera;
    建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；Establishing a second model function used to express the orientation change of the camera, where the angular velocity of the camera is used in the second model function to express the orientation change of the camera;
    根据所述第一模型函数、所述第二模型函数以及惯性传感器获得的所述摄像机的加速度和角速度，得到目标函数；Obtaining an objective function according to the first model function, the second model function, and the acceleration and angular velocity of the camera obtained by the inertial sensor;
    对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。The objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera.
  14. 如权利要求13所述的视觉里程计的实现装置，其中，作用在所述摄像机上的力分解成：世界坐标系下方向和大小保持不变的力F w，摄像机坐标系下大小保持不变的力F b，和，世界坐标系下在时刻t u新增加的力F u； The device for implementing a visual odometer according to claim 13, wherein the force acting on the camera is decomposed into: a force F w whose direction and magnitude remain unchanged in the world coordinate system, and the magnitude remains unchanged in the camera coordinate system The force F b , and, the newly added force F u at time tu in the world coordinate system;
    所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度时，所述处理器用于：When the objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera, the processor is configured to:
    对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F u确定。 The objective function is optimized to obtain the optimized force acting on the camera and the angular velocity of the camera within a time window, and the start time of the time window is determined by Fu .
  15. 如权利要求14所述的视觉里程计的实现装置，其中，所述第一模型函数为：The device for implementing visual odometer according to claim 14, wherein the first model function is:
    Figure PCTCN2020092539-appb-100009
    Figure PCTCN2020092539-appb-100009
    其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵； Wherein, S (t) is the camera at the time t the position in three-dimensional space, t i is the i th start time within the time window, [Delta] t is the time in the i-th time window t and the time t i the difference, m is the quality of the camera, q(t) is the quaternion representation of the orientation of the camera at time t, and R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t;
    其中，所述第二模型函数为：Wherein, the second model function is:
    Figure PCTCN2020092539-appb-100010
    Figure PCTCN2020092539-appb-100010
    其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t i的时间差，ο为四元数的乘法运算； Where q(t) is the quaternion representation of the orientation of the camera at time t, ω(t) is the angular velocity of the camera at time t, t i is the starting time in the i-th time window, and Δt is the i-th time within the time window t and the time difference t i, ο quaternion multiplication;
    其中，所述目标函数为：Wherein, the objective function is:
    Figure PCTCN2020092539-appb-100011
    Figure PCTCN2020092539-appb-100011
    其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n k为第 k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
    Figure PCTCN2020092539-appb-100012
    是重投影误差，p为三维特征点对应的二维特征点，T cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a imu(t)为惯性传感器在时刻t观测得到的加速度值，ω imu(t)为惯性传感器在时刻t观测得到的角速度值。     Where i is the count value of the time window, θ is the set of forces acting on the camera, φ is the set of angular velocity of the camera, N is the number of frames of the image in the i-th time window, and k is the The count value of the image frames in the i time window, j is the count value of the feature points in the k-th frame image, n k is the total number of feature points in the k-th frame image, and π(·) is the three-dimensional point X projection To a two-dimensional function,
    Figure PCTCN2020092539-appb-100012
    Is the reprojection error, p is the two-dimensional feature point corresponding to the three-dimensional feature point, T cw (t) = [R(q(t)), s(t)], s(t) is the camera at time t The position in the three-dimensional space, q(t) is the quaternion representation of the camera's orientation at time t, R[q(t) is the transformation matrix from the camera coordinate system to the world coordinate system at time t, and h is The specified weight value, ω(t) is the angular velocity of the camera at time t, α, β are specified values, m is the mass of the camera, and a imu (t) is the acceleration value observed by the inertial sensor at time t , Ω imu (t) is the angular velocity value observed by the inertial sensor at time t.
  16. 如权利要求14所述的视觉里程计的实现装置，其中，所述对所述目标函数进行优化之后，所述处理器还用于：The device for realizing a visual odometer according to claim 14, wherein, after the optimization of the objective function, the processor is further configured to:
    若优化后的F u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t u。 If the optimized F u is greater than or equal to the specified threshold, it is determined that a new time window starts, and the start time of the new time window is tu .
  17. 一种计算机可读存储介质，其中，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现如权利要求1至6中任一项所述的视觉里程计的实现方法的步骤。A computer-readable storage medium, wherein a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the visual odometer according to any one of claims 1 to 6 is realized Method steps.
PCT/CN2020/092539 2019-06-25 2020-05-27 Method and apparatus for implementing visual odometer WO2020259185A1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201910554021.0A CN112129272B (en) 2019-06-25 2019-06-25 Method and device for realizing visual odometer
CN201910554021.0 2019-06-25

Publications (1)

Publication Number Publication Date
WO2020259185A1 true WO2020259185A1 (en) 2020-12-30

Family

ID=73849130

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2020/092539 WO2020259185A1 (en) 2019-06-25 2020-05-27 Method and apparatus for implementing visual odometer

Country Status (2)

Country Link
CN (1) CN112129272B (en)
WO (1) WO2020259185A1 (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113848908A (en) * 2021-09-27 2021-12-28 浙江工业大学 Modeling and control method of vision servo system of all-directional mobile robot

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105910604A (en) * 2016-05-25 2016-08-31 武汉卓拔科技有限公司 Multi-sensor-based autonomous obstacle avoidance navigation system
CN106647784A (en) * 2016-11-15 2017-05-10 天津大学 Miniaturized unmanned aerial vehicle positioning and navigation method based on Beidou navigation system
CN107053166A (en) * 2015-12-16 2017-08-18 卡西欧计算机株式会社 Autonomous device and autonomous method
CN107077152A (en) * 2016-11-30 2017-08-18 深圳市大疆创新科技有限公司 Control method, equipment, system, unmanned plane and moveable platform
CN107092264A (en) * 2017-06-21 2017-08-25 北京理工大学 Towards the service robot autonomous navigation and automatic recharging method of bank's hall environment
CN107680133A (en) * 2017-09-15 2018-02-09 重庆邮电大学 A kind of mobile robot visual SLAM methods based on improvement closed loop detection algorithm
CN108489482A (en) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 The realization method and system of vision inertia odometer
CN109029433A (en) * 2018-06-28 2018-12-18 东南大学 Join outside the calibration of view-based access control model and inertial navigation fusion SLAM on a kind of mobile platform and the method for timing
CN109211277A (en) * 2018-10-31 2019-01-15 北京旷视科技有限公司 The state of vision inertia odometer determines method, apparatus and electronic equipment
US20190162842A1 (en) * 2017-11-29 2019-05-30 Qualcomm Incorporated Radar aided visual inertial odometry outlier removal

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106679648B (en) * 2016-12-08 2019-12-10 东南大学 Visual inertia combination SLAM method based on genetic algorithm
US10671083B2 (en) * 2017-09-13 2020-06-02 Tusimple, Inc. Neural network architecture system for deep odometry assisted by static scene optical flow
CN108846857A (en) * 2018-06-28 2018-11-20 清华大学深圳研究生院 The measurement method and visual odometry of visual odometry
CN108827315B (en) * 2018-08-17 2021-03-30 华南理工大学 Manifold pre-integration-based visual inertial odometer pose estimation method and device

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107053166A (en) * 2015-12-16 2017-08-18 卡西欧计算机株式会社 Autonomous device and autonomous method
CN105910604A (en) * 2016-05-25 2016-08-31 武汉卓拔科技有限公司 Multi-sensor-based autonomous obstacle avoidance navigation system
CN106647784A (en) * 2016-11-15 2017-05-10 天津大学 Miniaturized unmanned aerial vehicle positioning and navigation method based on Beidou navigation system
CN107077152A (en) * 2016-11-30 2017-08-18 深圳市大疆创新科技有限公司 Control method, equipment, system, unmanned plane and moveable platform
CN107092264A (en) * 2017-06-21 2017-08-25 北京理工大学 Towards the service robot autonomous navigation and automatic recharging method of bank's hall environment
CN107680133A (en) * 2017-09-15 2018-02-09 重庆邮电大学 A kind of mobile robot visual SLAM methods based on improvement closed loop detection algorithm
US20190162842A1 (en) * 2017-11-29 2019-05-30 Qualcomm Incorporated Radar aided visual inertial odometry outlier removal
CN108489482A (en) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 The realization method and system of vision inertia odometer
CN109029433A (en) * 2018-06-28 2018-12-18 东南大学 Join outside the calibration of view-based access control model and inertial navigation fusion SLAM on a kind of mobile platform and the method for timing
CN109211277A (en) * 2018-10-31 2019-01-15 北京旷视科技有限公司 The state of vision inertia odometer determines method, apparatus and electronic equipment

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
LU, WEI: "Research on Key Techniques of High-precision and Real-time Visual Localization", A DISSERTATION SUBMITTED TO ZHEJIANG UNIVERSITY FOR THE DEGREE OF DOCTOR OF PHILOSOPHY, 31 December 2015 (2015-12-31), DOI: 20200826022323A *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113848908A (en) * 2021-09-27 2021-12-28 浙江工业大学 Modeling and control method of vision servo system of all-directional mobile robot
CN113848908B (en) * 2021-09-27 2024-04-05 浙江工业大学 Modeling and control method of vision servo system of omnibearing mobile robot

Also Published As

Publication number Publication date
CN112129272A (en) 2020-12-25
CN112129272B (en) 2022-04-26

Similar Documents

Publication Publication Date Title
WO2021218124A1 (en) Method and device for detecting vehicle
JP7204823B2 (en) VEHICLE CONTROL METHOD, VEHICLE CONTROL DEVICE, AND VEHICLE
CN111566612A (en) Visual data acquisition system based on posture and sight line
JP5547139B2 (en) Recognition device, method and program
WO2020221307A1 (en) Method and device for tracking moving object
WO2020107931A1 (en) Pose information determination method and apparatus, and visual point cloud construction method and apparatus
WO2021098765A1 (en) Key frame selection method and apparatus based on motion state
JP2023021994A (en) Data processing method and device for automatic driving vehicle, electronic apparatus, storage medium, computer program, and automatic driving vehicle
WO2022021027A1 (en) Target tracking method and apparatus, unmanned aerial vehicle, system, and readable storage medium
CN110992456A (en) Avalanche simulation method based on position dynamics
WO2020259185A1 (en) Method and apparatus for implementing visual odometer
WO2023273036A1 (en) Navigation method and apparatus, and electronic device and readable storage medium
US11238604B1 (en) Densifying sparse depth maps
WO2019144286A1 (en) Obstacle detection method, mobile platform, and computer readable storage medium
WO2019047673A1 (en) Method and device for evaluating longitudinal control model of end-to-end automatic driving system
WO2019047604A1 (en) Method and device for determining data collection line
CN111027195B (en) Simulation scene generation method, device and equipment
WO2017028029A1 (en) Background model extracting method and apparatus and image processing device
WO2022247115A1 (en) Centroid trajectory generation method and apparatus, computer readable storage medium, and robot
CN113516013B (en) Target detection method, target detection device, electronic equipment, road side equipment and cloud control platform
CN115390253A (en) Scene display method and device, electronic equipment and storage medium
TW202215104A (en) Orientation predicting method, virtual reality headset and non-transitory computer-readable medium
WO2017173977A1 (en) Mobile terminal target tracking method, device, and mobile terminal
CN112348843A (en) Method and device for adjusting depth image prediction model and electronic equipment
CN113761965A (en) Motion capture method, motion capture device, electronic equipment and storage medium

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20831313

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 20831313

Country of ref document: EP

Kind code of ref document: A1

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 08/08/2022)