Use the state of the robotic device to map the environment

Abstract

Certain examples described herein enable a robotic device to accurately map a surrounding environment. The robotic device uses an image capture device, and at least one of the image capture device and the robotic device moves within the environment. Measurements associated with the movement of at least one of the image capture device and the robotic device are used to determine a state of the robotic device. The state of the robotic device models the image capture device and the robotic device relative to a model of the environment constructed by a mapping engine. By comparing the state of the robotic device to measured changes in the robotic device, an accurate representation of the state of the robotic device can be constructed. The mapping engine uses this state to update the model of the environment.

Description

Technical Field

[0001] The present invention relates to mapping an environment using a robotic device. The present invention has particular, but not exclusive, relevance to generating a model of an environment using states of a robotic device, wherein the states are updated based on measurements as the robotic device moves within the environment. Background Art

[0002] In the fields of computer vision and robotics, it is often necessary to construct a representation of an environment, such as a three-dimensional space that can be navigated using a robotic device. Constructing a representation of a three-dimensional space allows the real-world environment to be mapped into a virtual or digital realm, where the environmental map can be used and manipulated by an electronic device. For example, a mobile robotic device may require a representation of a three-dimensional space to allow simultaneous localization and mapping (commonly referred to as "SLAM") to navigate in its environment. The robotic device can operate in an indoor home or commercial environment or an outdoor natural environment. The representation of the environment can enable models of objects within the space to be identified and / or extracted. These can be used to perform measurements on the real-world environment and / or for producing three-dimensional replicas, such as via an additive manufacturing system. Similarly, detecting human body parts in three-dimensional space can enable new human-computer interactions, making it possible to manipulate virtual representations of objects using actions in the physical world.

[0003] There are a variety of techniques that can be used to construct a representation of an environment. For example, structure from motion and multi-view stereo are two techniques that can be used to achieve this. Many techniques extract features from images of the environment, which are then associated from image to image to construct a three-dimensional representation. Certain techniques that use a reduced number of points or features to generate a representation are called "sparse" techniques. For example, these techniques may use ten to a hundred features and / or points to generate a representation. These may be contrasted with "dense" techniques that generate representations with thousands or millions of points. The advantage of "sparse" techniques is that they are easier to implement in real time, such as at a frame rate of around 30 frames per second; using a limited number of points or features limits the scope of processing required to construct a three-dimensional representation. In contrast, it is more difficult to perform real-time "dense" mapping of an environment due to computational requirements. For example, it is often preferred to perform "dense" mapping offline, such as it may take 10 hours to generate a "dense" representation from 30 minutes of image data provided.

[0004] When generating representations of an environment from a mobile capture device, there are challenges in determining the position and orientation of the capture device within space and dealing with unpredictable motion (such as extended "stuttering" or "looping" motion). For example, uneven terrain and operating conditions may result in frequent changes in the position and orientation of the capture device, and an autonomous robotic device may revisit previously captured locations within three-dimensional space. The capture of image data may also be continuous in real time, resulting in large amounts of captured data. These factors all present challenges for real-world systems; many techniques may succeed with limited data or strictly defined capture trajectories, but it is difficult to build representations in real time from a mobile robotic device.

[0005] In their paper “Keyframe-Based Visual-InertialSLAM Using Nonlinear Optimization” (Robotics: Science and Systems 2013, Berlin, Germany, June 24-28, 2013), Stefan Leutenegger et al. describe a method for fusing visual and inertial cues in a SLAM system. The paper describes the use of an inertial measurement unit (IMU), which contains a series of accelerometers and gyroscopes, to measure the forces experienced by a robotic device. However, in practice, it has been found that the use of the IMU proposed in the paper exhibits drift and degradation, which leads to problems in building three-dimensional models. The proposed solution also operates on keyframes and requires a mapping system based on a pose graph; it must build a consistent world model from keyframes, which may present different views of the scene and the objects in it. Another problem is the ambiguity of modeling.

[0006] WO 2016 / 189274 A1 describes a method for modeling a three-dimensional space. Image data from at least one capture device is used to generate a three-dimensional model of the three-dimensional space. In some examples, the three-dimensional model is segmented into an active portion and an inactive portion based on at least one model attribute. These examples are configured to update the three-dimensional model over time using the active portion. A registration is also performed to align the active portion of the three-dimensional model with the inactive portion of the three-dimensional model over time. The registration aligns the active portion of the three-dimensional model generated after observing an area of ​​the three-dimensional space with the inactive portion of the model generated after at least one previous observation of the area.

[0007] The methods of WO 2016 / 189274 A1 operate without a pose graph. This makes them incompatible with previous methods, such as those proposed by Leutenegger et al., which construct a pose graph while mapping the environment. Although the methods proposed in WO 2016 / 189274 A1 provide improvements for building environmental models, there have been problems in applying them to certain real-world robotic devices. For example, some robotic devices do not move in a smooth manner, for example they typically have an image path that is different from the smooth path tracked by a handheld device. They also often traverse non-planar landscapes that cause unstable camera movements. It has been found that in some cases, even the embodiments of WO 2016 / 189274 A1 have difficulty in processing images from these robotic devices in real time. Summary of the invention

[0008] According to one aspect of the present invention, a mapping system for a robotic device is provided, the robotic device including one or more actuators for moving the robotic device within an environment and a movable support for an image capture device. The mapping system includes a state engine to jointly optimize the current state of the robotic device and the previous state of the robotic device based on kinematic errors, odometer errors, and geometric errors. Each state of the robotic device includes data indicating the posture of the robotic device relative to a model of the environment and data indicating the posture of the image capture device relative to the model. The kinematic error includes a function of kinematic measurements associated with the movable support and the posture of the image capture device relative to the robotic device derived from the current state. The odometer error includes a function of odometer measurements associated with one or more actuators and the odometer difference between the current state and the previous state. The geometric error includes a comparison of image data from the capture device with a model of the environment, the comparison using a projection based on the current state. The mapping system is configured to update the model using the current state optimized by the state engine.

[0009] In some examples, the movable support includes a plurality of joints, and the pose of the image capture device relative to the robotic device is determined using forward kinematics.

[0010] In some examples, the pose of the robotic device relative to the model and the pose of the image capture device relative to the model are defined within six degrees of freedom.

[0011] In some examples, the state engine is configured to jointly optimize the current state and the previous state using a linear prior, and the state engine is configured to update the optimized linear prior based on kinematic error, odometry error, geometric error, and marginalization of the previous state. In such examples, the mapping engine can be configured to align a first set of portions of the model with a second set of portions of the model using a deformation map, the deformation map indicating a set of neighbors for a given position in the model to be used to modify the model at the given position during alignment. In these examples, the mapping engine can be configured to align the first set of portions of the model with the second set of portions of the model during loop closure, wherein after the loop closure, the linear prior and the previous state are reinitialized.

[0012] In some examples, the model includes a surface element model, wherein each surface element in the surface element model includes at least data defining a position of the surface element in three dimensions and data defining a normal vector of the surface element in three dimensions, wherein each surface element represents a two-dimensional region in three-dimensional space.

[0013] In some examples, the mapping system is configured to generate a model for the image capture device without a pose graph.

[0014] According to a second aspect of the present invention, there is provided a robotic device, comprising: an image capture device movably mounted relative to the robotic device; one or more actuators for moving one or more of the robotic device and the image capture device relative to an environment; a state engine for updating a state of the robotic device; and a mapping engine for generating a three-dimensional model of the environment using the state of the robotic device. The state of the robotic device includes at least a transformation of the image capture device relative to the three-dimensional model. The state engine is configured to update the state of the robotic device based on a comparison of the state of the robotic device with measurements obtained from the one or more actuators. The mapping engine is configured to compare an image from the image capture device with the three-dimensional model based on the state of the robotic device to update the three-dimensional model.

[0015] In some examples, the mapping engine is configured to determine an iterative closest point error, and wherein the state engine is configured to additionally update the state of the robotic device using the iterative closest point error.

[0016] In some examples, the one or more actuators include: a first set of actuators for moving the robotic device relative to the environment, and a second set of actuators for moving the image capture device relative to the robotic device, wherein the state of the robotic device also includes a transformation of the robotic device relative to the three-dimensional model. In such examples, the first set of actuators can provide multiple degrees of freedom between the robotic device and the environment, and the second set of actuators can provide multiple degrees of freedom between the image capture device and the robotic device.

[0017] In some examples, the image capture device is configured to output color image data and a depth map, and the mapping engine is configured to update the three-dimensional model using the color image data and the depth map.

[0018] According to a third aspect of the invention, there is provided a method for updating a three-dimensional model of an environment using a robotic device having a movably coupled image capture device. The method comprises instructing at least one of the image capture device and the robotic device to move within the environment; obtaining image data representing an observation of the environment using the image capture device; obtaining a measurement of the movement; updating a model state of the robotic device, the model state of the robotic device comprising pose data of the robotic device and the image capture device, the updating comprising optimizing a function of the model state and the measurement of the movement; and updating the three-dimensional model of the environment using the updated model state based on a comparison of the image data with the three-dimensional model, the comparison comprising using a projection of the updated model state.

[0019] In some examples, the pose data includes a transformation of the robotic device relative to the three-dimensional model and a transformation of the image capture device relative to at least one of the robotic device and the three-dimensional model.

[0020] In some examples, instructing the movement includes one or more of: instructing the first set of actuators to move the robotic device within the environment; and instructing the second set of actuators to move the image capture device relative to the robotic device. In these examples, obtaining measurements of the movement from the robotic device includes one or more of: obtaining odometer data from the first set of actuators; and obtaining kinematic data from the second set of actuators.

[0021] In some examples, updating the model states of the robotic device and the image capture device includes optimizing a cost function, the cost function including an error term associated with a kinematic error and an error term associated with an odometry error. In such examples, the kinematic error includes a comparison of a spatial relationship between the image capture device and the robotic device with kinematic measurements from the robotic device, and the odometry error includes a comparison of a change in the spatial relationship between the robotic device and a three-dimensional model of the environment with odometry data from the robotic device. In these examples, the cost function may include an error term associated with a linear prior, wherein the linear prior is constructed using the error value and the model state of the robotic device from a previous update operation. In these examples, the cost function may include an error term associated with a geometric alignment error, wherein the geometric alignment error includes a comparison of depth data derived from acquired image data with a three-dimensional model using the model state of the robotic device prior to the update.

[0022] According to a fourth aspect of the present invention, there is provided a non-transitory computer-readable storage medium comprising computer-executable instructions which, when executed by a processor, cause a computing device to perform any of the methods described above.

[0023] Further features and advantages of the present invention will become apparent from the following description of preferred embodiments of the invention, given by way of example only, with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1A is a schematic diagram showing a first example of an environment to be mapped;

[0025] Figure 1B is a schematic diagram showing available degrees of freedom of an exemplary device;

[0026] Figure 1C and Figure 1D are schematic diagrams respectively showing a robot apparatus according to a first example and states of the robot apparatus;

[0027] Figure 1E and Figure 1F are schematic diagrams respectively showing a robot apparatus according to a second example and states of the robot apparatus;

[0028] Figure 1G and Figure 1H are schematic diagrams respectively showing a robot apparatus according to a third example and states of the robot apparatus;

[0029] FIG. 2A to FIG. 2C is a schematic diagram illustrating an exemplary capture device;

[0030] Figure 3A is a schematic diagram of a mapping system for a robotic device according to a first example;

[0031] Figure 3B is a schematic diagram of a mapping system for a robotic device according to a second example;

[0032] Figure 4A is a schematic diagram of a state engine according to an example;

[0033] Figure 4B is a schematic diagram showing components of a state of a robot apparatus according to an example;

[0034] Figure 5 is a schematic diagram illustrating certain components of a mapping engine according to an example;

[0035] Fig. 6A is a schematic diagram showing exemplary data of a three-dimensional surface metamodel;

[0036] Figure 6Bis a schematic diagram showing properties of a surface element according to an example;

[0037] Figure 7 is a schematic diagram of processing of image data by a mapping system according to an example;

[0038] Figure 8 is a schematic diagram showing an example of using stored representations and deformation graphs;

[0039] Fig. 9 is a flow chart illustrating a method of updating a model of an environment according to an example;

[0040] Fig.10 is a schematic diagram showing movement of a robot apparatus and drift correction in a model space according to usage state optimization of an example;

[0041] Fig.11 is a state diagram for state optimization according to an example;

[0042] Fig.12 is a graph illustrating exemplary mapping performance; and

[0043] Fig.13 is a schematic diagram illustrating a non-transitory computer-readable medium according to an example. DETAILED DESCRIPTION

[0044] Certain examples described herein enable a robotic device to accurately map a surrounding environment. The robotic device includes an image capture device, and at least one of the image capture device and the robotic device moves within the environment. Measurements associated with at least one of the image capture device and the robotic device (e.g., measurements of the image capture device relative to the robotic device or movement of the robotic device relative to the environment) are used to determine a state of the robotic device. The state of the robotic device models the image capture device and the robotic device relative to a model of the environment constructed by a mapping engine. The state can be updated using measurements associated with the robotic device. The mapping engine uses this state to update the model of the environment. By improving the accuracy of the state of the robotic device, the accuracy of the mapping can be improved.

[0045] Certain examples described herein may be applied to robotic devices having complex mechanical supports for one or more image capture devices. They may also be applied to robotic devices navigating complex internal and / or external environments with non-planar surfaces. In these applications, the described examples provide systems and methods that are capable of maintaining an accurate map of the environment. Thus, they provide improvements in mapping in areas such as autonomous search and rescue and disaster recovery. These are areas where many comparative methods have difficulty providing accurate mapping.

[0046] Some of the examples described herein can be integrated with mapping systems that do not use pose graphs. Thus, they are able to build on their accuracy in three-dimensional spatial mapping, for example, their demonstrated applicability in indoor scene mapping using handheld devices. They allow for the robust use of these methods in a wider range of scenarios and environments.

[0047] In some examples, a dense, consistent, and comprehensive model of a three-dimensional space can be generated from "looping" and / or "intermittent" capture device trajectories generated by a robotic device having a movable stand for an image capture device. Certain examples described herein include features that enable state optimization and model building to occur in real time or at near real time frame rates. For example, certain features enable incremental simultaneous localization and dense mapping on a frame-by-frame basis, for example, without using keyframe-based approaches that introduce certain issues and can reduce mapping accuracy.

[0048] In some examples, a three-dimensional model or map (referred to herein as a "model") is segmented into at least an active portion and an inactive portion based on at least one attribute of the model. For example, position elements of such a model can be classified as active or inactive based on the time when the model was modified (e.g., older portions of the model can be classified as inactive) and / or the distance from the capture device in three-dimensional space (e.g., position elements that are more than a certain distance from the capture device or a defined position in space can be classified as inactive). In these cases, the active portion of the three-dimensional model is used to update the model, wherein the inactive portion is not used to update the model. Such an update can include fusing frames of image data with the model, such as determining new position elements in the model based on the image data.

[0049] Some examples described herein also provide alignment of active model parts and inactive model parts. When the capture device revisits or re-observes a given position in three-dimensional space, this can achieve so-called "loop" closure. This alignment (which can be performed frame by frame as frequently as processing captured image data) and accurate tracking of the robot device state maintains the accuracy and stability of the model and provides the ability to robustly handle "loop" and / or "intermittent" capture device trajectories, such as captured by a real robot device with a movable camera stand. In some cases, alignment may include two aspects: "local loop closure", which attempts to align the predicted frames generated from each active model and inactive model; and "global loop closure", which attempts to align a given frame of image data with a representation of a previously processed frame of image data. Alignment can be performed by deforming the model (e.g., via spatial deformation). In some cases, this deformation may be non-rigid, and a deformation map can be used to apply transformations to the positional elements of the model. This can further improve the accuracy and consistency of the model in three dimensions, such as by reducing the construction as a modeling process and not reflecting the discontinuities of the three-dimensional space being modeled. In particular, such deformation graphs may be sparse and / or may be embedded in space, e.g. associated with positional elements of a model. These techniques differ from techniques that require a pose graph, e.g. a probabilistic representation of the position and orientation of a camera device, for rigidly transforming independent keyframes of image data.

[0050] Certain examples described herein provide tracking functionality based on movement measurements such as kinematic and odometer measurements. Accuracy can be further improved by incorporating constraints based on geometric errors. Kinematic errors can be defined relative to an image capture device that is movably mounted on a robotic device. Odometer errors can be defined to constrain the posture of the robotic device relative to both a previous state and a current state of the robotic device. In some cases, tracking can be performed on video data on a frame-by-frame basis, where a state graph including a previous state component and a current state component is solved when each frame is received. The previous state and error may be marginalized to a linear prior, which may also limit the state estimate. Loop closures as described above can be handled. For loop closures, a consistent model can be maintained by smoothly deforming the model and then updating at least the previous state of the robotic device.

[0051] Figure 1A , Figure 1B and Figure 1C Examples of capture of an environment and image data associated with the environment are schematically shown. These examples are presented to better explain certain features described herein and should not be considered limiting; certain features have been omitted and simplified for ease of explanation.

[0052] Figure 1AAn example 100 of an environment is shown in the form of a three-dimensional space 110. The three-dimensional space 110 can be an interior and / or exterior physical space, such as at least a portion of a room or a geographic location. The three-dimensional space 110 can be a location on a planet. The three-dimensional space 110 in this example 100 includes a plurality of physical objects 115 positioned with the three-dimensional space. These objects 115 may include one or more of the following: people, electronic devices, furniture, animals, parts of buildings, and equipment. Example 100 also shows an image capture device 120 that is capturing image data associated with the three-dimensional space 110. The image capture device 120 may include a camera in digital or analog form that is arranged to record data generated by observing the three-dimensional space 110. Figure 1A , image capture device 120 is mounted on a robotic device 130 that is arranged to navigate in three-dimensional space 110 using actuators 135. In this example, these actuators include wheels; in other cases, they may include tracks, burrowing mechanisms, legs, rotors, etc. For example, robotic device 130 may include controlled or autonomous aerial and / or ground devices or vehicles. Although Figure 1A The three-dimensional space 110 in FIG. 1 is shown as having a bottom surface, but this need not be the case in all embodiments, for example the environment may be in the air or in extraterrestrial space. Figure 1A In the embodiment of the present invention, the image capture device 120 is movably mounted on the robotic device 130. For example, the image capture device 120 may be mechanically coupled to the robotic device 130, wherein the robotic device is arranged to move within the three-dimensional space 110.

[0053] Figure 1B An example 140 of degrees of freedom available for an exemplary image capture device 120 and an exemplary robotic device 130 is shown. In the case of the image capture device 120, the orientation 150 of the image capture device 120 can be co-linear with the axis of the lens or other imaging device. As an example of rotation about one of the three axes, a normal axis 155 is shown. Similarly, in the case of the robotic device 130, an alignment direction of the robotic device 145 can be defined. This can indicate the orientation and / or direction of travel of the robotic device. The normal axis 155 is also shown. Although only a single normal axis is shown with reference to the image capture device 120 or the robotic device 130, as described below, these devices can rotate about any one or more of the axes schematically shown as 140.

[0054] More generally, the position and orientation of a device can be defined in three dimensions with reference to the six degrees of freedom: the position or orientation can be defined in each of the three dimensions, for example by [x, y, z] coordinates, and the orientation can be defined by an angular vector representing a rotation about each of the three axes, for example [θ x ,θ y,θ z ]. In the examples described herein, the position and orientation of the device is defined as the pose of the device. The pose of the device may change over time, such that the device has a different pose at time t+1 than at time t. This may apply to both the image capture device and the robotic device. The pose of the device may also change over time, for example as the robotic device moves around within the environment or as the image capture device moves relative to the robotic device. This will be referred to below Figure 1C to Figure 1H Further description.

[0055] Figure 1C to Figure 1H Three exemplary robotic devices are shown 160, 180, and 190. These robotic devices are illustrative examples, and the methods and systems described herein may be applied to robotic devices having a variety of configurations and constructions.

[0056] Figure 1C A first example of a robotic device 160 is shown. The robotic device 160 includes a chassis 162, a set of actuators 164, a movable support 165, and an image capture device 166. The set of actuators 164 in this example includes a set of wheels. The movable support 165 is mechanically coupled to the chassis 162 at one end and to the image capture device 166 at the other end (at an "end effector" position). The movable support 165 provides one or more degrees of freedom for movement of the image capture device 166, for example relative to the chassis 162 of the robotic device 160. Figure 1C The movable support 165 in the embodiment includes a plurality of joints. The exact configuration of the movable support 165 and the possible range of movement at each joint may vary from device to device. In this example, the movable support enables the image capture device 166 to have a position and orientation different from the chassis 162 of the robotic device 160. The group of actuators 164 allows the chassis 162 of the robotic device 160 to have a changing position and orientation within the surrounding environment. Therefore, each of the chassis 162 and the image capture device 166 can have different states represented by up to six variables, for example, the state of each of the chassis 162 and the image capture device 166 can be defined by a matrix indicating a coordinate in a three-dimensional space and a set of rotations around each axis in the space. The three-dimensional space can be defined relative to a model used by the robotic device to navigate in the environment. In one case, the movable support may include an arm or manipulator mounted to an omnidirectional robotic device. In this case, the robotic device can move in any direction on a surface with a relatively constant height.

[0057] Figure 1D It is shown how the state of the robotic device 160 is modeled. Figure 1D shows a model space with a defined coordinate system. Model space can be Figure 1BThe origin in the model space may be arbitrarily chosen, for example, based on an initial position of the robotic device 160 when the device is activated and / or a calibrated position relative to one or more objects in the environment (e.g., using encoded data, such as a multi-dimensional barcode). Figure 1D In the embodiment of the present invention, the state of the robotic device 160 includes data 168 indicating the pose of the robotic device 160 relative to the model space. In this case, the pose of the robotic device 160 is considered to be the position and orientation of the model of the chassis 162 of the robotic device. For example, the data 168 can be stored as a transformation T relative to the model space origin (e.g., the corner of a defined cuboid) of a given point associated with the three-dimensional representation of the chassis 162. WB- Here "W" refers to the "world" of the model space, and "B" refers to the base of the robotic device provided by chassis 162. Figure 1D In the embodiment of the present invention, the state of the robotic device 160 also includes data 170 indicating the pose of the image capture device 166 relative to the model space. In this case, the pose of the image capture device 166 is considered to be the position and orientation of the image capture device 166. In a manner similar to the data 168, the data 170 can be stored as a transformation of the origin of the model space relative to a given point associated with the three-dimensional representation of the image capture device 166 - T WC- Likewise, "W" refers to the "world" of the model space, and "C" refers to the capture device. The pose of the image capture device 166 may be expressed as the position of the image capture device's coordinate system relative to the model space.

[0058] Figure 1E A second example of a robotic device 180 is shown. The robotic device 180 is configured as a legged robot having a body 182 and, in this case, four legs 184. Each leg may be powered by a series of actuators. The head 186 of the robotic device 180 is mechanically coupled to the body 182 via a neck 185, which serves as a movable mount for a plurality of image capture devices mounted to the head 186. Figure 1F Data 168 defining the pose of the subject 182 and data 170A, 170B respectively defining the poses of a plurality of image capture devices mounted on the head 186 are shown. In this case, the neck 185 may be movable and the head 186 may rotate on the neck 185. In some configurations, the image capture devices may be further movable on the head 186.

[0059] Figure 1GA third example of a robotic device 190 is shown. The robotic device 180 comprises an aerial vehicle (e.g., a "drone"). The robotic device 190 comprises a chassis 192 and four actuators 194. The four actuators may comprise rotor blades to propel the robotic device 190 in air space. The robotic device 190 also comprises a movable support 195 in the form of a universal joint that supports an image capture device 196. The movable support 195 allows the image capture device 196 to move independently of the chassis 192. Figure 1H Data 168 defining the pose of chassis 168 and data 170 defining the pose of image capture device 196 are shown.

[0060] Figure 1C , Figure 1E and Figure 1G Some non-limiting examples of forms that a robotic device may take are shown. Figure 1D , Figure 1F and Figure 1H It is shown how the states of various robotic devices may be represented, namely in the form of data 168 representing the pose of the robotic device and data 170 representing the pose of at least one image capture device mechanically coupled to the robotic device using a movable support. In this example, the robotic device and movable support may take a variety of forms. The movable support may include one or more actuators to enable movement relative to the robotic device. The robotic device may also have one or more actuators to enable movement relative to the surrounding environment. A set of actuators may include one or more mechanical, electrical, hydraulic, pneumatic, polymer, thermal and / or magnetic mechanisms, including so-called "soft" actuators, which include deformable polymers and / or systems that combine different mechanisms to provide movement.

[0061] Figure 2A , Figure 2B and Figure 2C Three non-exhaustive examples of different image capture device configurations are schematically shown. These configurations may be applied to one or more image capture devices movably mounted on a robotic device. Figure 2A In example 200, image capture device 210 is configured to generate image data 220. If image capture device 210 is a digital camera, this may be performed directly, for example, image data 220 may include processed data from a charge coupled device or complementary metal oxide semiconductor (CMOS) sensor. Image data 220 may also be generated indirectly, for example by processing other image sources, such as converting analog signal sources.

[0062] In one instance, image data 220 includes image data captured over time. Figure 2BAn example of such a format is shown in FIG. 230. In this case, image data 220 includes a plurality of frames 240. Each frame 240 may correspond to a particular time t (i.e., F ) in a time period during which an image of an environment, such as environment 110 in FIG. 1, was captured. t ). Thus, image data 220 may be considered video data, and image capture device 210 may be considered a camera. Frame 240 typically consists of a two-dimensional representation of the measured data. For example, frame 240 may include a two-dimensional array or matrix of pixel values ​​recorded at time t. Figure 2B In the example of , all frames 240 within the image data have the same size, although this is not necessarily the case in all examples. The pixel values ​​within the frame 230 represent the measurement values ​​of a specific portion of the three-dimensional space of the environment. Figure 2B In the example of FIG. 2 , each frame 240 includes values ​​of two different forms of image data. A first set of values ​​and depth data 250 (eg, D t ). The depth data may include an indication of the distance from the capture device, for example, each pixel or image element value may represent the distance of a portion of the three-dimensional space from the image capture device 210. The second set of values ​​is related to the photometric data 260 (e.g., color data C t ) are related. For a given resolution, these values ​​may include red, green, blue pixel values. In other examples, other color spaces may be used and / or the photometric data 260 may include monochrome or grayscale pixel values. In one case, the image data may include a compressed video stream or file. In this case, frames of the image data may be reconstructed from the stream or file, for example as output of a video decoder. After preprocessing of the video stream or file, the image data may be retrieved from a memory location.

[0063] Figure 2B The image capture device 210 may include a so-called RGB-D camera arranged to capture both RGB data 260 and depth ("D") data 250. In one case, the RGB-D camera is arranged to capture video data over time. One or more of the depth data 250 and the RGB data may be used at any one time. The depth data 250 may be generated by one or more techniques known in the art, such as a structured light approach, in which an infrared laser projector projects an infrared light pattern on an observed portion of a three-dimensional space, which is then imaged by a monochrome CMOS image sensor.

[0064] In one case, the image capture device 210 may be arranged to store the image data 220 in a connected data storage device. The data storage device may be located in one or more of the image capture device 210, a movable bracket that connects the image capture device 210 to the robotic device, and the robotic device. In another case, the image capture device 210 may transmit the image data 220 to a connected computing device. The computing device may be remote from both the image capture device 210 and the robotic device. In other cases, the computing device may include an embedded computing device of the robotic device. The connected computing device may be directly connected, such as via a universal serial bus (USB) connection, or indirectly connected, such as the image data 220 may be transmitted through one or more computer networks. In yet another case, the image capture device 210 may be configured to transmit the image data 220 across one or more computer networks for storage in a network attached storage device. The image data 220 may be stored and / or transmitted on a frame-by-frame basis or in batches, such as multiple frames may be bundled together. The depth data 240 does not need to have the same resolution or frame rate as the photometric data 250. For example, depth data 250 may be measured at a lower resolution than photometric data 260. One or more pre-processing operations may also be performed on image data 220 before the image data is used in examples described later. Other configurations not described herein are also possible.

[0065] Figure 2C Another possible configuration 270 for obtaining image data 220 is shown. In this example, two image capture devices are used: a first image capture device 210-A, which is arranged to capture image data 220-A including frames of depth data 280; and a second image capture device 210-B, which is arranged to capture image data 220-B including frames of photometric data 290. These data channels can be processed separately in any computing device or merged into a common image processing pipeline. For example, Figure 2C Configuration 270 can be applied to Figure 1E configuration. In one case, the first image capture device 210-A can be an infrared, ultrasonic and / or radar device that is arranged to measure a three-dimensional space (e.g., the depth therein) and convert the measurements into frames of measurement data. The second image capture device 210-B can then include an RGB camera arranged to capture frames of color image data 290. The frame size and / or resolution of the depth data 280 and the photometric data 290 can be different, and the frame rate captured can also be different. In one case, pre-processing can be applied so that the two frame sets have a common size and resolution. In this case, it can be Figure 2C The data is preprocessed to make it Figure 2B The form shown.

[0066] In some examples, the image capture device may be incorporated into a robotic device, such as the robotic device, the movable support, and the image capture device may comprise a single unit. In other examples, the image capture device may be a detachable device that may be coupled to the robotic device. In some examples, the movable support may be built into the robotic device. In other examples, it may be removably coupled to the robotic device. In some cases, two or more of the robotic device, the movable support, and the image capture device may be separate modular components that may be coupled to a variety of different devices.

[0067] supply Figure 2A , Figure 2B and Figure 2C As multiple examples, and as will be appreciated, image data 220 may be generated using configurations different than those shown in the figures for use in the methods and systems described below. Image data 220 may also include any measured sensory input arranged in a two-dimensional form that represents a view of an environment captured or recorded using an image capture device. For example, this may include only depth data or one of photometric data, electromagnetic imaging, ultrasonic imaging, and radar output, among others. In these cases, only an imaging device associated with a particular form of data may be required, such as in one case an image capture device may include a monocular RGB device without depth data. Similarly, in some cases, the image data may include only depth data, such as photometric data such as 260 and 290 may be omitted.

[0068] In some cases, the image capture device may be arranged to perform pre-processing to generate depth data. For example, a hardware sensing device may generate data in the form of disparity data or multiple stereo images, where one or more of software and hardware is used to process the data to calculate depth information. In other cases, image data from a monocular RGB camera may be processed by a reconstruction system to generate depth data. Similarly, the depth data may come from a time-of-flight camera that outputs a phase image that can be used to reconstruct depth information. In examples utilizing depth data, any suitable technique may be used to generate depth data that forms part of the image data 220.

[0069] Figure 2A image data and Figure 2B and Figure 2C Photometric data C t Can include color images with a resolution of x R1 ×y R1 Each [x, y] pixel value in the frame comprises an RGB vector [R, G, B]. For example, the resolution may be 640×480 pixels. Figure 2B and Figure 2C The depth data D tA two-dimensional matrix may be included that contains depth values. This may be represented as a grayscale image, for example, where the resolution is x R2 ×y R2 Each [x, y] pixel value in a frame of comprises a depth value d representing the distance of the capture device from the surface in three-dimensional space.

[0070] In some cases, image data 220 may be generated by combining multiple data sources, such as multiple cameras viewing a particular three-dimensional space. In some cases, image data 220 need not be video data. It may instead include a series of static images captured from different locations over time using one or more capture devices. In some cases, depth data may be generated from photometric data, such as from processing photometric data representing the motion of a capture device around in space.

[0071] The previous figure shows that FIG. 3A to FIG. 8 An example of combining the use of a robotic device and an example of an image capture device.

[0072] Figure 3A Components of a robotic device 310 according to an example are shown. The robotic device 310 may be, for example, Figures 1A to 1H Although the components of the robotic device 310 are shown as integrated components in the figure, they can be implemented separately and communicatively coupled to the robotic device 310, for example, as described earlier with respect to FIG. 2A to FIG. 2C The image capture device 210 is explained.

[0073] exist Figure 3A In the embodiment, the robotic device 310 includes an image capture device 320, a set of one or more actuators 330, a state engine 340, and a mapping engine 350. The image capture device 320 may be, for example, Figures 1A to 1H One of the image capture devices 120, 166, 186, and 196 shown, FIG. 2A to FIG. 2C The image capture device 320 is configured to capture image data. The set of actuators 330 may include actuators such as actuators 164, 184, and 194 and / or in Figure 1C to Figure 1H The actuator 330 may be configured to move one or more of the robotic device and the image capture device relative to the environment. For example, the actuator 330 may be configured to move a base or chassis of the robotic device within the environment and / or to move the image capture device 320 relative to the base or chassis.

[0074] The state engine 340 and the mapping engine 350 include processing devices for the robot device 310. The state engine 340 and the mapping engine 350 may include dedicated processing electronic devices and / or may be implemented by a computer program code executed by a processor of at least one computing device. One or more of the state engine 340 and the mapping engine 350 may be implemented by one or more embedded computing devices, for example, may include separate embedded computing devices or a single public embedded computing device. Each computing device may include at least one processor that operates in association with a memory to execute a computer program code loaded on a computer-readable medium. The medium may include a solid-state storage device, such as an erasable programmable read-only memory, and the computer program code may include firmware. In other cases, one or more of the state engine 340 and the mapping engine 350 may include a properly configured system on chip, a dedicated integrated circuit, and / or one or more properly programmed field programmable gate arrays. In some cases, one or more of the state engine 340 and the mapping engine 350, as well as or in lieu of the previous case, may be implemented by one or more graphics processing units that execute computer program code. In some cases, one or more of state engine 340 and mapping engine 350 may be implemented by one or more functions being implemented in parallel on cores of multiple processors and / or graphics processing units, for example.

[0075] The state engine 340 is configured to determine the state of the robotic device 360. The state of the robotic device 360 ​​may include data stored in a memory and / or storage device of the robotic device 310. The mapping engine 350 is configured to generate a three-dimensional model of the environment 370 using the state of the robotic device 360.

[0076] The three-dimensional model 370 generated by the mapping engine 350 may include any model or map having position elements that represent positions or locations within the three-dimensional space of the environment represented within the image data. The three-dimensional model 370 may also include data stored in a memory and / or storage device of the robotic device 310. In some cases, the three-dimensional model 370 is a "dense" model of the three-dimensional space. In this case, there are a large number of position elements that form the model, such as hundreds of thousands or millions of elements. This can be compared to a feature-based or "sparse" model, where there may be only tens or hundreds of defined model points. In one case, the position elements may be based on a voxel model of the space, where a surface having the space is defined by reference to voxel values ​​within the voxel space at a particular resolution in the three dimensions. In another case, a surface element ("surface element") model may be used. Reference Fig. 6A and Figure 6BThe surface metamodel is defined in more detail. In the surface metamodel, multiple oriented surfaces at a specific location are defined as location elements. The scope of the three-dimensional model 370 may depend on the scope of the environment observed by the image capture device 320.

[0077] exist Figure 3A In the example of , the state of the robotic device 360 ​​includes at least a transformation of the image capture device 320 relative to the three-dimensional model 370. For example, the three-dimensional model 370 may represent a "world coordinate system" (represented herein by "W") and the state of the robotic device 360 ​​may include the pose of the image capture device 320 (represented herein by "C") in the world coordinate system and the pose of the robotic device 310 (e.g., the base, body, main chassis or other reference point of the robotic device - represented herein by "B") in the world coordinate system. In some cases, the pose of the image capture device 320 in the world coordinate system may be stored as a translation. W r WC Data and orientation q WC Similarly, the posture of the robot device 310 in the world coordinate system can be stored as a data representing the translation W r WB Data and orientation q WB In this case, the states of the robotic device 360 ​​exist as defined points on a 12-dimensional manifold.

[0078] exist Figure 3AIn the example of , the state engine 340 is configured to determine the state of the robotic device 360 ​​as the robotic device 310 explores the environment. For example, over time, the position and orientation of at least one of the image capture device 320 and the robotic device 310 may change spatially. The changes may be due to active movement using one or more actuators 330 and / or noise disturbances of the image capture device 320 and the robotic device 310 within the environment (e.g., due to sliding, wind, mechanical bounce, uneven surfaces, etc.). Therefore, it is necessary to update the state of the robotic device 360 ​​to reflect these changes. In this example, the state engine 340 is configured to update the state of the robotic device 360 ​​based on a comparison of the change in the state of the robotic device with the measurement values ​​obtained from the one or more actuators 330. In some cases, the change in the state of the robotic device is determined between two times that the image capture device 320 captures consecutive images. If the image capture device 320 includes a camera, the state engine 340 can update the state of the robotic device 360 ​​on a frame-by-frame basis. In some cases, the state engine 340 can be configured to perform state updates on a selected subset of captured frames, such as every nth frame. As described in more detail with reference to subsequent examples, in some cases, the state of the robotic device 360 ​​may include a current state and a previous state of the robotic device. In these cases, the state engine 340 may be configured to jointly optimize components of the current state and the previous state. For example, this may include calculating a new updated state and storing the existing current state as the previous state for the next update.

[0079] Figure 3A The state engine 340 and mapping engine 350 are configured to work together. Figure 3A In an example of , the mapping engine 350 is configured to compare the image from the image capture device 320 with the projection of the three-dimensional model 370 based on the state of the robotic device 360, and use the result of the comparison to update the three-dimensional model 370. The projection may include generating a synthetic image representing a two-dimensional view of the model from the current pose of the image capture device 320. The projection may be performed for the entire image or pixel by pixel. For example, the pixel values ​​of the synthetic image may be calculated by projecting the three-dimensional model 370 onto a camera plane, where the position and orientation of the camera plane is determined by the current pose of the image capture device 320 as represented by the state of the robotic device 360.

[0080] In one case, the three-dimensional model 370 can be considered "dense" because the pixel values ​​within the image data from the image capture device 320 are processed and contribute to the modeling of the three-dimensional space. For example, in a "dense" representation, each pixel in the image can contribute as much information as possible to the tracking and mapping estimation process. This enables the three-dimensional model 370 (i.e., the resulting representation of the scene map) to be projected back to the synthetic capture device or camera to reconstruct a "dense" image, i.e., an image at the resolution of the image capture device 320, where the vast majority of pixels in the synthetic image have data synthesized based on information stored with the model. This projection can be performed using the state of the robotic device 360, which at least represents the pose of the image capture device 320. In contrast, a "sparse" system (such as a system that utilizes key points or extracted features) only uses a small subset of the pixel values ​​in the image data to generate the model. In the "sparse" case, it is impossible to create a synthetic image at the capture device resolution because there is not enough information within the model. In this way, a "dense" system is used to estimate one or more surfaces in the three-dimensional space with high accuracy, such as within a given tolerance of the real environment. "Dense" systems can be thought of as analogous to quantized continuous systems, whereas "sparse" systems operate on a small set of discrete points.

[0081] Figure 3B A robotic device 380 is shown, which is Figure 3A A variation of the example of . In this variation, Figure 3A The one or more actuators 330 include two separate sets of actuators. A first set of robotic actuators 332 is provided to move the robotic device 380 relative to the environment. For example, these may include a set of wheels, legs, or rotors, such as Figure 1C to Figure 1H As shown. A second set of capture device actuators 334 are then provided to move the image capture device 320 relative to the robotic device 380. For example, these may include a set of actuators for coupling the image capture device 320 to a movable support of the robotic device 380. The capture device actuators 334 may include a series of motors for moving one or more joints of a movable support having multiple joints. In some examples, the robotic actuators 332 provide multiple degrees of freedom between the robotic device 380 and the environment, and the capture device actuators 334 provide multiple degrees of freedom between the image capture device 320 and the robotic device 380.

[0082] Figure 3B The robot device 380 and Figure 3AThe robotic device 310 of is also different in that it includes a mobile controller 390. The mobile controller 390 may include additional processing devices as described above for controlling the movement of the robotic device 380 within the environment. The mobile controller 390 may allow one or more of manual control and autonomous control. The mobile controller 390 is communicatively coupled to the robotic actuator 332 and the capture device actuator 334. The mobile controller 390 is thus able to control the movement of the robotic device 380 and the image capture device 320. When exploring a new environment, the robotic device 380 may implement an exploration strategy involving the movement of the robotic device 380 and the movement of the image capture device 320. For example, in a search and rescue situation, the mobile controller 390 may control the robotic actuator 332 to move the robotic device 380 to a new location. Once in the new location, when the robotic device 380 is stationary, the mobile controller 390 may control the capture device actuator 334 to translate around the environment of the location.

[0083] exist Figure 3B In the example of FIG. 3 , mobile controller 390 is configured to navigate in an environment using three-dimensional model 370. For example, three-dimensional model 370 may indicate physical boundaries and obstacles within the environment (e.g., Figure 1A In one embodiment, the mobile controller 390 may determine a movement path to avoid physical boundaries and obstacles while moving the robotic device within the environment. In another embodiment, the three-dimensional model 370 of the room or other interior location may indicate an exit point to allow the robotic device 380 to leave the room. The mobile controller 390 may also need to control the position of the image capture device 320 to avoid physical boundaries and obstacles, such as walls, objects, or low-height passages.

[0084] exist Figure 3B 370, the state engine 340 is further configured to update the state of the robotic device 360 ​​using image data from the image capture device 320 and the three-dimensional model 370. In one case, the state engine 340 is configured to update the state of the robotic device 360 ​​in a manner that seeks to minimize the point-to-plane error between back-projected points of the image captured by the image capture device 320 and corresponding points in the three-dimensional model 370. In this case, the back-projection is based on the current pose of the image capture device 320 indicated by the state of the robotic device 360. The image data in this case may include depth data and the update of the state may seek to minimize the geometric error between the depth data and the three-dimensional model 370.

[0085] In some cases, the mapping engine 350 may be configured to determine an iterative closest point error. In such cases, the state engine 340 may be configured to additionally use the iterative closest point error to determine the state of the robotic device 360.

[0086] In some cases, image capture device 320 is configured to output color image data and a depth map, such as Figure 2B and Figure 2C In this case, the mapping engine 350 is configured to update the three-dimensional model 370 using the color image data and the depth map. Figure 5 and Figure 7 In some cases, state engine 340 may use only the depth map. Although state engine 340 and mapping engine 350 are Figure 3A and Figure 3B Although described as separate components in the examples of the present invention, it should be noted that in some cases their functions may be combined into a single component, or certain functions may be moved from one component to another without losing the overall system functionality.

[0087] In one case, Figure 3A and Figure 3B The robotic devices 310, 380 may be arranged to perform one or more functions. For example, the robotic devices may be arranged to perform mapping functions, locate specific people and / or objects (e.g., in emergency situations), transport objects, perform cleaning or maintenance, etc. To perform one or more functions, the robotic devices may include additional components, such as other sensing devices to interact with the environment, vacuum systems, and / or actuators.

[0088] Figure 4A An exemplary mapping system 400 for a robotic device is shown. The mapping system 400 can be used as Figure 3A and Figure 3B The mapping system 400 may be used as part of the robotic device 310, 380 in the example embodiment, or as part of another exemplary robotic device. The mapping system 400 is configured to operate with a robotic device that includes one or more actuators to move the robotic device within an environment, such as Figure 3A and Figure 3B Actuators 330 and 332, or Figures 1A to 1H In this case, the robotic device further comprises a movable support for the image capture device. The movable support may include Figure 1C , Figure 1E and Figure 1G One of the brackets 165, 185, 195 shown in FIG. 1 includes Figure 3BThe capture device actuator 334 of the robot device, or another movable mechanical coupling device extending between the robot device and the image capture. In this example, the "movable support" may include one or more joints that are movable by a set of corresponding motors. Therefore, in some cases, the movable support is movable relative to the robot device, for example, it can be mounted on the robot device but still move independently. When moving on the robot device, the movable support can achieve movement within the environment. The movable support can have a variety of possible implementations, including simple rotating elements and pivoting elements, as well as more complex multi-joint linkage devices.

[0089] Mapping system 400 operates on error data 405. Error data 405 in this example includes kinematic error 410, odometer error 420, and geometric error 430. Error data 405 is accessed by state engine 440. State engine 440 may include Figure 3A and Figure 3B or another processing device in the state engine 340 of the robot device. The state engine 440 is configured to update state data 445 of the robot device. In this example, the state data 445 is divided into data representing a current state of the robot device 450 and data representing a previous state of the robot device 460. For ease of explanation, reference will be made to "current state" and "previous state", where this should be understood to refer to data indicating at least one configuration of the robot device stored in a memory or storage device. The state engine 440 is configured to jointly optimize the current state of the robot device 450 and the previous state of the robot device 460. This results in updated state data 445, including an updated current state of the robot device 450, which can be used by the mapping system 400 to update the model of the environment. For example, the updated current state of the robot device 450 can be represented by a number similar to Figure 3A and Figure 3B Components of mapping engine 350 are used to update a three-dimensional model similar to model 370 .

[0090] In this example, each of the current state of the robotic device 450 and the previous state of the robotic device 460 includes two data components. Figure 4B , with reference to the general state of the robotic device 462. Figure 4B , the general state of the robotic device 462 includes data indicating the pose of the robotic device relative to the model of the environment 464 and data indicating the pose of the image capture device relative to the model 466. Figure 4B In the above figure, these poses are represented as the previous reference Figure 1A to Figure 1H The two transformations T described WB and WCThe pose can be stored in a number of different ways, for example, the pose can be decomposed into translational and rotational aspects, which can be stored separately.

[0091] The kinematic error 410 includes kinematic measurements associated with the movable support of the robotic device and a function of the pose of the image capture device relative to the robotic device derived from the current state of the robotic device 450. For example, the kinematic measurements may include one or more measurements obtained directly or indirectly from one or more actuators of the robotic device controlling the movable support.

[0092] Odometer error 420 includes a function of odometer measurements associated with one or more actuators and the odometer difference between the current state and the previous state. In this case, the one or more actuators may include actuators such as Figure 3B The robot actuator 332 or Figure 1C , Figure 1E and Figure 1G , i.e., actuators that move the robotic device within the environment. Odometry measurements may include one or more measurements obtained directly or indirectly from these actuators. Odometry error 420 is associated with movement of the robotic device within the environment, while kinematic error 410 is associated with movement of the image capture device affected by the movable support.

[0093] The geometric error 430 includes a comparison of image data from the image capture device to a model of the environment, wherein the comparison uses a projection based on the current state of the robotic device 450. For example, the image data may be back-projected onto the model, or the model may be projected onto the plane of the image data. The back-projection may use a pose (e.g., T ) of the image capture device relative to the model from the current state of the robotic device. WC ), and the projection may use the inverse of the calculated pose. Any method may be applied. The geometric error 430 may be based on a comparison of depth data of a frame of provided image data with a model of the environment. The geometric error 430 may be calculated for a plurality of pixels from the image data, such as from a Figure 2B and Figure 2C The plurality of pixels may include a subset of the image data or all pixels in the image data. In one embodiment, the geometric error 430 may be calculated based on a point-to-plane iterative closest point (ICP) function.

[0094] In one instance, state engine 440 optimizes a cost function including kinematic error 410, odometer error 420, and geometric error 430. For example, state engine 440 may determine values ​​of a previous state and a current state that minimize the cost function, wherein each of errors 410, 420, and 430 is defined as a function having variables associated with one or more of the current state and the previous state. For example, state engine 440 may determine at least a transformation T for the current state. WB and T WC and the transformation T for the previous state WB The value that minimizes the cost function. In this way, the pose of the image capture device relative to the model and the pose of the robotic device relative to the model can be determined as constrained by the geometric tracking and the kinematic and odometry data associated with the robotic device. The kinematic error 410 can constrain the two transformations T represented within the current state of the robotic device 450. WB and T WC The odometer error 420 can be transformed into T WB The value of is constrained within the current state of the robotic device 450 and the previous state of the robotic device 460. The geometric error 430 can transform T WC The value of is constrained within the current state of the robotic device 450.

[0095] The kinematic measurements used to determine the kinematic error 410 may be obtained from, for example, Figure 3B The kinematic measurements may be obtained from a support actuator of the capture device actuator 334 shown in , i.e., an actuator for moving the movable support. The kinematic measurements may be received directly from the support actuator, for example as a data packet or signal transmitted via a system bus of the robotic device, and / or may be received indirectly, for example the state engine 440 may access a memory or register of the robotic device to retrieve the kinematic measurements. In one case, the kinematic measurements may be measured via observation of the movable support, for example determined based on image data characterizing the robotic device. In another case, the kinematic measurements may be obtained from a controller of the actuator (such as the mobile controller 390 in FIG. 3 ), which is arranged to receive feedback from the actuator.

[0096] Kinematic measurements may include measurements of the transformation of the image capture device pose relative to the robotic device, such as T BC. If the movable support includes multiple joints, each of which is moved using one or more motors, the kinematic measurements can be determined using forward kinematics. Forward kinematics refers to the use of kinematic equations associated with a robotic device that are capable of determining the position of an end effector (e.g., a point at the end of the movable support) based on specified values ​​of joint parameters. In this case, the specified values ​​can be derived from the measurements of the motor positions associated with the joints. In this case, the movable support can be defined as a series of links, two of which are joined to a joint. A first rigid transformation can be defined for each joint to characterize the possible relative movement at the joint, and a second rigid transformation can define the dimensions of each joint. The transformation will depend on the form of the movable support and / or the robotic device. A sequence of first rigid transformations and second rigid transformations can be defined that model the movement of the movable support from a base on the robotic device to an end effector, where the image capture device is mounted at the end effector. The state engine 440 can receive "raw" joint measurements and calculate the pose of the image capture device relative to the robotic device by evaluating the sequence of rigid transformations Or the gesture may be received from an external component, such as a microcontroller associated with the moveable base located within one of the moveable base and the robotic device.

[0097] In some cases, the state engine 440 initializes the state of the robotic device using kinematic measurements and odometry measurements. The first set of values ​​for the previous state of the robotic device 460 may be a set of zero values ​​or a predefined starting configuration (e.g., a known starting position and orientation). The first set of values ​​for the current state of the robotic device 450 may then be determined based on the initialized previous state and the initial kinematic measurements and initial odometry measurements (e.g., measured between the starting position and the frame of the received video data). For example, the robotic device may be initialized with an arbitrary body pose and an image capture device pose determined based on a forward kinematic determination.

[0098] In one instance, the kinematic error 410 includes a difference between a measured translation indicating the position of the image capture device relative to the robotic device and a translation based on the current state of the robotic device 450. The kinematic error 410 may also include a difference between a measured orientation of the image capture device relative to the robotic device and a translation based on the current state of the robotic device 450. The pose of the image capture device relative to the robotic device may be calculated as In one case, the kinematic error 410 is determined as:

[0099]

[0100] in is the measured translation of the image capture device relative to the robotic device,B r BC is the translation of the image capture device relative to the robotic device as represented by the state of the robotic device, is the measured orientation of the image capture device relative to the robotic device, and q BC is the orientation of the image capture device relative to the robotic device as represented by the state of the robotic device. Optimizing the state of the robotic device may include using the provided and The value of B r BC and q BC The value of .

[0101] In one case, the odometry error 420 may be determined in a manner similar to the kinematic error. The odometry error 420 may be an error involving both the current state and the previous state of the robotic device. For example, the odometry error may consider the error between the measured change in the pose of the robotic device between the times of the two states and the change in the pose of the robotic device between the current state and the previous state. For example, the change in pose may be determined by the transformation , where 0 indicates the previous state and 1 indicates the current state. Following the above example, in one case, the odometer error 420 is determined as:

[0102]

[0103] in is the measured change in the position of the robotic device, B Br r B0B1 is the change in position of the robotic device as represented by the current and previous states of the robotic device, is the measured change in the orientation of the robotic device, and q B0B1 is a change in the orientation of the robotic device as represented by the current and previous states of the robotic device. Optimizing the state of the robotic device may include using the provided and The value of B0 r B0B1 and q B0B1 The value of .

[0104] In one case, for the kth pixel of the depth map, the geometric error 430 is determined as:

[0105] e G = W n k ·( W v k -T WC c v k )

[0106] The bracketed parts represent the back-projected points obtained from the image data. c v k (Using the image capture device from the current state of the robotic device 450 relative to the model T WC The projection of the posture) and the model W v k The difference between corresponding points in the model along the surface normal of the indicated point W n k projection.

[0107] In one case, an information matrix is ​​constructed for each error term to model the measurement uncertainty. For example, the information matrix of the kinematic error 410 and / or the odometry error 420 may be determined as:

[0108]

[0109] where σ r is the variance of the translation measurement, and σ α is the variance of the orientation measurements. The information matrix of geometric error 430 may be the inverse covariance matrix associated with the depth measurements.

[0110] The state engine 440 may operate iteratively to maintain the current state of the robotic device 450 over time. For example, the state engine 440 may be activated after a new frame of video data arrives from the image capture device. It can be seen that the state engine 440 operates to improve the prediction of the current state of the robotic device 450 made based on the kinematic measurements and the odometry measurements.

[0111] In one case, the state engine 440 is configured to determine a change in the state of the robotic device, which is modeled as a local perturbation to the state δx. This local perturbation can be divided into a translation component and an orientation component. In this case, the state engine 440 can optimize the cost function discussed above by determining the minimum local perturbation that minimizes the cost function. The optimization can be performed using any known optimization process. In one case, a Jacobian matrix and a set of residuals are calculated for each term in the cost function. The Jacobian matrix and residuals for each term can be combined into a Jacobian matrix J and a set of residuals b for the cost function. In one case, the optimization can be performed by calculating a least squares solution, such as:

[0112]

[0113] In one instance, local perturbations to each of the previous state of the robotic device 460 and the current state of the robotic device 450 are calculated. For example, state updates during an iteration of the state engine 440 may include determining Where 0 represents the previous state and 1 represents the current state. The optimization may consist of a 24×24 system of normal equations. This may be solved using one or more processors. In one case, this may be solved using a Gauss-Newton iterative method with a three-level coarse-to-fine pyramid. In some cases, the one or more processors may form part of one or more graphics processing units. These graphics processing units may be provided on one or more embedded processors of the robotic device.

[0114] In some cases, Figure 4A The state engine 440 is configured to jointly optimize the current state and the previous state using a linear prior. For example, the cost function optimized by the state engine 440 to determine the state update includes an additional term representing a linear prior. The linear prior can be determined by marginalizing the kinematic error, the odometer error, and the geometric error and / or preparing the previous state for subsequent iterations. The state engine 440 can be configured to calculate the Jacobian matrix H* and the residual set b* used in the state optimization as described above. In one case, Schur-Complement can be used so that:

[0115]

[0116]

[0117] Among them, J i is the Jacobian matrix of the cost function for state i (e.g., where 0 is the previous state and 1 is the current state), and b i is the set of residuals of the cost function for state i (e.g., where 0 is the previous state and 1 is the current state). The Jacobian matrix H* and the set of residuals b* can be used as a linear prior for the next optimization iteration, e.g., when a new frame of video data is received. In some cases, a first-order correction can be performed on the residual set of the subsequent update, e.g., b *′ =b * +H * Δx, where Δx is the difference between the subsequent linearization point and the previous linearization point. The linear prior can be represented in the cost function as an error term with the following form:

[0118]

[0119] That is, where the term indicates the difference in translation and orientation of the robotic device ("B") and the image capture device ("C") relative to the model ("W"). This error term can be used within the optimization, and / or for one or more of initialization and loop closure correction of the robotic device. During optimization, the error term and the previous Jacobian matrix and residual set can be used. A new Jacobian matrix and residual set can be obtained at the end of each optimization process, and these can be used for optimization of subsequent frames.

[0120] In some cases, the capture device intrinsic transformation relative to the end effector of the movable support can be calibrated using visual labels or the like. For example, the camera intrinsic transformation can be determined by jointly optimizing the image capture device extrinsics and the relative pose of the visual labels relative to the model to minimize the reprojection error of the detected corner points. The capture device intrinsic transformation can be determined during the calibration phase and is assumed to be constant for the operation of the robotic device.

[0121] exist Figure 4A In the example of , state engine 440 can be configured to determine at least one set of updated values ​​for the current state of robotic device 450. Mapping system 400 is then configured to update the model of the environment using the updated current state of robotic device 450. For example, as represented by the transformation T WC The pose of the image capture device relative to the model can be used to fuse image data from the image capture device with the model of the environment. Figure 5 , Figure 7 and Figure 8 The example of explains the process of updating the model in more detail. In one case, the state engine 440 can be used as such Figure 3A and Figure 3B The tracking module of the mapping engine of the mapping engine 350 shown in FIG.

[0122] In some cases, the mapping system 400 is configured to use the deformation map to align the first set of parts of the model with the second set of parts of the model. Figure 7 and Figure 8 The deformation map indicates a set of neighbors for a given location in the model that are used to modify the model at the given location during alignment. This alignment can be performed to establish a "loop closure" with respect to the model. The mapping system 400 can be configured to detect and perform "loop closure" as described in more detail below. In the case of "loop closure", the mapping system 400 can calculate a new pose of the image capture device (e.g., T WC， ). In this case, state engine 440 can be obtained from, for example, Figure 3A and Figure 3BThe mapping engine 350 in the mapping engine receives the new pose. The new pose can be used to reset the previous state of the robotic device 460, for example, when determining the current state of the next frame of video data. The pose of the image capture device in the previous state can be replaced based on the new pose received from the mapping engine, and the updated pose of the image capture device can be used to update the pose of the robotic device, for example Where T BC is the calculated pose of the image capture device relative to the robotic device (e.g., calculated using the state of the robotic device before receiving the new pose). If a linear prior is used, it may be initialized in a manner similar to the initial startup of the state engine 440. An initial subject pose and / or an initial camera pose may be measured to initialize the linear prior.

[0123] Figure 5 An example 500 of a mapping engine 510 that can be used to build a model of an environment is shown. The mapping engine 510 is presented as an example to better explain how the current state of a robotic device can be used to update a model of an environment, however it should be noted that different embodiments of the mapping engine can also use this state in a similar manner. The mapping engine 510 is configured to obtain image data 520. This can be image data that has been actively or previously generated by an image capture device (such as the image capture devices 120, 210, and 320 in the previous figures). The image data 520 is associated with an observation of the environment using the capture device, such as a view of a three-dimensional space over time. For example, the image data 520 may include video data within a given time period, wherein the robotic device moves within the environment within the given time period. The mapping engine 510 is configured to process the image data 520 and calculate a model 530 of the environment. In this example, this model is a three-dimensional model of the three-dimensional space.

[0124] The mapping engine 510 of this example includes at least a model segmenter 540 and a registration engine 550. The model segmenter 540 is configured to segment the three-dimensional model 530 into at least an active portion and an inactive portion based on at least one model attribute. The registration engine 550 is configured to align the active portion of the three-dimensional model 530 with the inactive portion of the three-dimensional model 530 over time. The mapping engine 510 is also configured to use the active portion of the three-dimensional model 530 to update the model over time, i.e., the inactive portion is not used to update the model.

[0125] In some cases, the mapping engine 510 is configured to operate on a frame-by-frame basis. In one embodiment, the mapping engine 510 may be arranged to map successive frames of image data to one another. tThe frames may be stored in a data storage device internal to the robotic device or obtained from an external data storage device. In other embodiments, the mapping engine 510 may retrieve one or more frames of image data from a memory internal to the robotic device. In one embodiment, a portion of the internal memory may hold frame data at a particular time t and may be overwritten when new image data 520 is received from the capture device.

[0126] When the mapping engine 510 is configured to operate on a frame-by-frame basis, the mapping engine 510 can be configured to update the three-dimensional model 530 on a frame-by-frame basis. This can include "merging" a frame of specific image data 520 with the three-dimensional model 530, that is, using the frame of image data 520 to modify and update the three-dimensional model 530. This can include including new position elements that can be derived from the frame of image data 520. Some specific examples of how the three-dimensional model 530 can be generated or updated are discussed in more detail below. It should be noted that the methods discussed herein can be applied to frames of incomplete and / or noisy image data. Updating the three-dimensional model 530 can be performed using one or more of the luminance and depth data components of the image data. In one case, only the depth data can be captured and used to update the three-dimensional model 530.

[0127] In one embodiment, the mapping engine 510 is configured to use the current state of the robotic device (e.g. Figure 3A and Figure 3B Status 360 or Figure 4A and Figure 4B The state of the robotic device may include data representing the posture of the capture device represented in the state 450, 462 in the robotic device. In the case of using two or more capture devices, the state of the robotic device may include data representing the posture of each capture device.

[0128] The model segmenter 540 may be configured to segment the three-dimensional model 530 by modifying the model data. For example, in one case, a given position element of the three-dimensional model 530 may have a variable indicating whether it forms part of an active portion or an inactive portion of the model. In another case, the model segmenter 540 may be configured to segment the three-dimensional model 530 into a function applied to the model. For example, the three-dimensional model 530 may be input to the model segmenter 540 and the model segmenter 540 may be configured to output one or more of the active portion and the inactive portion of the three-dimensional model 530. Any method may be used.

[0129] The model attributes used by the model segmenter 540 to segment the three-dimensional model 530 may indicate a level of certainty in the three-dimensional model 530. For example, the model segmenter 540 may segment the three-dimensional model 530 based on one or more of time and distance. In the first case, the model attributes may include one of a model generation time and a model update time for a given position in the three-dimensional model. In this case, the inactive portion may indicate a past observation time that differs from a current observation time by more than a predetermined amount. For example, the model segmenter 540 may be arranged to process the time data of each position element constituting the three-dimensional model 530 to divide the set of position elements for the three-dimensional model 530 into two disjoint sets θ representing active elements and a set Ψ representing inactive elements. To this end, the model segmenter 540 may process the timestamp t of each position element P (e.g., the time when the position element was last modified), so that for a given segmentation time t (e.g., a frame F associated with a particular processing of the image data t Related), Model P c For each position element in (where c is a coordinate in three-dimensional space), if tt P <δ t , then the position element is in the set θ (i.e. P c ∈θ), and if tt P ≥δ t , then the position element is in the set Ψ (i.e. P c ∈Ψ), where δ t is a defined time period. This form of segmentation or classification of model elements will gradually be t Position elements that do not appear in the three-dimensional model 530 are marked as inactive. It can be considered as the application of a time window. As described above, the mapping engine 510 can be configured to fuse new frames of image data into the active part of the three-dimensional model 530, wherein the inactive part of the model is not used for tracking and / or data fusion. In this case, after the registration engine 550 aligns the active and inactive parts, the aligned inactive parts can be modified so that they now become active parts. For example, when the inactive parts are aligned, the registration engine 550 can update the last modification time associated with each position element in the inactive part. This may have the following effect: after the model segmenter 540 processes the three-dimensional model 530, the aligned inactive part now becomes active. This enables continuous frame-to-model tracking and model-to-model tracking, and allows viewpoint-invariant loop closure.

[0130] In one embodiment, the mapping engine 510 is configured to calculate an active model frame based on projections of active portions from the three-dimensional model 530 for updating the model. For example, such projections can provide a two-dimensional viewpoint or virtual frame representing a predicted view or observation of the active portion of the three-dimensional model 530. In one embodiment, the active model frame can be generated based on a pose of the image capture device relative to the model, such as obtained from the current state of the robotic device at a given time (e.g., for a given frame of video data). In one embodiment, a predicted frame can be calculated for each data set that constitutes the image data 520. For example, when processing similar to Figure 2B and Figure 2C Each active model frame may include depth data when the image data is shown in The predicted frames, and in some cases, photometric data These can be compared with the recorded or captured image frames F from the image data 520. t Have the same resolution. Figure 5 The use of the activity model frame is described in more detail.

[0131] As described above, the mapping engine 510 may be arranged to generate the three-dimensional model 530 over time, for example, as multiple frames of recorded image data 520 are processed. In one instance, the image data 520 represents observations of the three-dimensional space over time as the image capture device moves (e.g., through one or more of movement of a robotic device and movement of a movable support on the robotic device), such that as frames of the image data 520 are processed by the mapping engine 510, the size of the three-dimensional model 530 grows, for example, incorporating more positional elements representing different portions of the three-dimensional space. In addition, the registration engine 550 is configured to perform alignment of active and inactive portions of the model over time; this may occur when portions of the three-dimensional space are revisited or re-observed, i.e., when a "loop" in the motion of the capture device is closed. This means that as more frames of the image data 520 are processed, the accuracy and consistency of the three-dimensional model 530 also increases.

[0132] The output of the mapping engine 510 can be considered to include a three-dimensional model 530 of the observed three-dimensional space. The model 530 can include at least position elements defined with reference to the three dimensions. Each position element can also be associated with data indicating the presence of a solid surface within the three-dimensional space. For example, in a voxel-based model, a surface can be represented as a zero value or a crossing point of a variable representing free space; in a surface metamodel, position elements can be defined for surfaces within the three-dimensional space, so that each position element can indicate a specific surface in the model.

[0133] Fig. 6AA schematic diagram of a data structure 610 is shown that can be used to store location elements of a model of an environment, such as updated in the examples described herein. Data structure 610 is shown only as an example and should not be considered limiting; other methods and formats may be used to store data depending on the implementation. In this example, the model includes a three-dimensional surface element model. The surface element model includes data definitions for a plurality of surface elements or "facets," where each surface element represents a two-dimensional region (i.e., a surface) in a three-dimensional space. This Fig. 6A , the data opens with the definition of a list of surface elements (e.g., "{surface element: ...}". The list may be an unordered list. In this case, each surface element in the surface element model includes at least data defining the position of the surface element in three dimensions (i.e., position elements or components) and data defining the normal vector of the surface element in three dimensions (i.e., the "facing" direction of the surface associated with the element). This is shown in Figure 6B A description of "surfels" can be found in the paper "Surfels: Surface elements as rendering primitives", published by Pfister, Hanspeter et al. in the Proceedings of the 27th Annual Conference on Computer Graphics and Interactive Techniques, ACM Press / Edison-Wesley, July 2000.

[0134] Figure 6B A schematic diagram of a surface element or "surface element" 620 is shown. Surface element 620 includes a three-dimensional surface s. Surface element has a position p in three dimensions. Fig. 6A In , the position is defined using three-dimensional coordinates, such as the data "Position: p x , p y , p z ;" defines the x, y, and z coordinates. In other examples, other coordinate systems can be used. Figure 6B In the example, the surface element also has a normal vector n , which is defined in three dimensions. Fig. 6A In , the position is defined using a three-dimensional vector, such as the data "Normal: n x , n y , n z ;". You can use signed values ​​within a vector definition to indicate direction. Fig. 6A The example 610 in FIG. 6 also has additional data that can be used to describe the surface element in some examples. In the current case, the surface of the surface element is a circle or disk in three-dimensional space. Therefore, Fig. 6A and Figure 6BThe surface in is defined by a radius r, as illustrated by the data "radius: r;". The radius of each facet is intended to represent the local surface area around a given point while minimizing visible holes. In other examples, different data may be used to define the surface, such as using different geometric definitions and / or variables indicating the extent of the surface in three dimensions. Fig. 6A As shown, in this example, the bin definition also includes data defining: the color of the surface ("color: R, G, B;" - in this case, the RGB color space is used, but any known color space is possible); the weight of the bin ("weight: w;" - this may be a real number that is used when fusion of new measurements with the bin); an initialization time ("initial_time: t0;"), which indicates when the bin was first generated; and a last modification time ("last_modification: t;"), which indicates when the bin was last updated or modified. A bin is considered to be a modified bin when the time since the bin was last updated or modified (e.g., had an original image data value associated with it for data fusion) is greater than δ. t When the facets in the model representation are inactive, the facets in the model representation can be declared inactive. It should be understood that data defining the attributes of the facets can be added, modified or omitted depending on the implementation. Fig. 6A As shown, multiple facets may be defined in a list (eg, between "{...}" in the example).

[0135] Figure 7 A mapping system 700 according to an example is shown. The mapping system may include Figure 4A Implementation of a mapping system 400. Figure 7 The mapping system 700 includes a state engine 705 and a mapping engine 710. The state engine 705 may include Figure 3A and Figure 3B The state engine 340, and / or Figure 4A The state engine 440. The mapping engine 710 may include Figure 3A and Figure 3B The mapping engine 350 and / or Figure 5 A mapping engine 510 is provided. Figure 7 Example 700 schematically illustrates how certain components of mapping engine 710 may interact with state engine 705 to update a three-dimensional model upon receiving a frame of image data.

[0136] Figure 7 The state engine 705 and the mapping engine 710 access the image data 715. The image data 715 is provided by an image capture device, such as Figures 1A to 1H The image capture device 120 or Figure 3A and Figure 3B The image capture device 320 in FIG. 715 may include the following: FIG. 2A to FIG. 2CThe image data 220 shown. The image data 715 is accessed by the state engine 705 and the model fusion component 725 of the mapping engine 710. The image data 715 may include a frame of image data. In one embodiment, the state engine 705 may receive depth data from the image data at time t to determine the reference Figure 4A The model fusion component can receive depth data at time t and photometric data In a simple case, these may include data stored in two two-dimensional arrays in memory at time t, for example 640x480x1 depth values ​​and 640x480x3 color values. The data may be retrieved from a time indexed data structure representing captured or previously recorded image data and / or may be provided as part of a live video feed, in each case relating to live frames currently provided by at least one capture device. The model fusion component 725 is configured to update the three-dimensional model 730 based on frames from the image data 715.

[0137] exist Figure 7 In the example of FIG. 7 , the state engine 705 is configured to determine the pose P of at least one image capture device providing image data 715 at time t. t The posture is determined using the current state of the robotic device including the image capture device. For example, the posture may include Figure 4A and Figure 4B The transformation T after the state optimization process described WC The state engine 705 is communicatively coupled to the active model frame generator 720 of the mapping engine 710. The active model frame generator 720 is configured to calculate the active model frame -AMF t .

[0138] Figure 7 Also shown is a model segmenter 735. This may include reference to Figure 5 735 is arranged to divide the three-dimensional model 730 into two parts or sets: the first part or set is referred to herein as the "active" part 740; and the second part or set is referred to herein as the "inactive" part 745. This may include applying a time window to the three-dimensional model 730. Figure 7 It can be seen that the active portion 740 is used by the active model frame generator 720 and the model fusion component 725. The inactive portion 745 is not used by the active model frame generator 720 and the model fusion component 725.

[0139] In particular, the active model frame generator 720 is arranged to access the active part 740 to generate an active model frame. This may be performed based on a projection of the active part 740 from the three-dimensional model. The active model frame generator 720 is configured to use the pose estimate P at time t t To determine the active model frame AMF at time t t This may include using the variable values ​​of the pose estimate to determine the projected geometry using position elements of the active portion 740 comprising the three-dimensional model.

[0140] In addition to the active model frame generator 720, Figure 7 The mapping engine 710 also includes an inactive model frame generator 750. The inactive model frame generator 750 is similar to the active model frame generator 720, but is configured to generate an inactive model frame IMF from the inactive portion 745 of the 3D model. t For example, the inactive model frame generator 750 may compute the inactive model frame based on the geometric projection from the inactive portion 745. In one case, the active model frame generator 720 and the inactive model frame generator 750 may be implemented by a common frame generator configured to receive the active portion and the inactive portion of the model as distinguishing inputs to generate the active model frame and the inactive model frame, respectively. As described with reference to the active model frame, each inactive model frame may include depth data. The predicted frames and photometric data The predicted frame.

[0141] exist Figure 7 In the embodiment, the registration engine 760 is arranged to receive the active model frame AMF from the active model frame generator 720. t and receives an inactive model frame IMF from the inactive model frame generator 750 t In this example, the registration engine 760 is configured to compare the two frames to determine a transformation that aligns the active model frame with the inactive model frame. Figure 7 As shown by the arrows in , the transformation can then be used to update the three-dimensional model 730 to align the active and inactive parts of the model. The transformation can be used to apply a non-rigid spatial deformation of the position elements for both the active and inactive parts. In one case, the deformation can be applied using a deformation map. This reference Figure 8 Described in more detail. The deformation or alignment can be estimated using the current pose P t In some cases, the registration of the active model frame and the inactive model frame may use the current state of the robotic device as determined by the state engine 705. In one case, rather than attempting to align a previously predicted frame with a newly received frame of image data, the registration engine 760 is configured to determine a transformation H that aligns the active frame and the inactive frame. tIf alignment is performed, the registration engine 760 may set all visible inactive position elements (eg, those visible in the inactive model frame) to active.

[0142] The alignment or deformation performed by the registration engine 760 can enact a "loop" closure, i.e., aligning positional elements of a model generated from newly received image data with positional elements corresponding to the same region of three-dimensional space previously generated and / or modified based on previously received image data. For example, without the registration engine 760, when the capture device completes a motion cycle, such as returning to view a previously observed region of space, previous portions of the model may not align with newer portions of the model. This misalignment or "drift" in the model occurs when the generation of the model uses estimates and seeks to minimize an error function (e.g., operating non-deterministically), such that small errors in the pose estimate and the model may accumulate as the model is generated. Figure 7 The registration engine 760 in attempts to continuously (e.g., for each frame) register the active portion of the model within the currently estimated capture device (i.e., active model) frame with the inactive portion of the model within the same frame. If the registration is successful, such as based on an alignment metric or a desired level of deformation, the motion loop of the capture device has been closed and the newer active portion has been aligned with the older inactive portion, wherein the entire three-dimensional model 730 can be non-rigidly deformed into place to reflect this registration. In some cases, the registration engine 760 is configured to reactivate the inactive portions of the model used to perform the registration, such as those used in the generation of the inactive model frame. This enables model fusion of the model fusion component 725 to occur between registered regions of the model. As described above, fusion data is performed in the area of ​​the three-dimensional model 730 that was recently observed (i.e., the active portion 740 or region of the model), while older portions of the model that have not been observed for a period of time can be segmented by the model segmenter 735 into inactive portions 745 or regions of the model that are not used for tracking or data fusion.

[0143] Figure 8 An example 800 of additional features of a registration engine 810 that may be used in certain implementations is shown. Figure 8 In the embodiment of the present invention, the registration engine 810 may have the same functionality as any of the previously described registration engines 550 or 760, and is communicatively coupled to a data storage device 820. The data storage device 820 stores representations 830 ("ε") of the three-dimensional model over time. These representations 830 may include active model frames AMF t representations as they are generated by the model engine; for example, they can include depth data Representation and photometric data representation. In one case, the representation can be a compressed and / or downsampled representation of the active model frame. In some cases, the current frame from the image data can be used to fill in partial predicted views of the active model frame that do not have any mapped surfaces. In some cases, the representation can include a fern encoding of the active model frame. Each representation can include data associated with one or more of the following: a fern encoding string; a downsampled predicted frame of depth data; a downsampled predicted frame of photometric data; a current pose estimate; and an initialization time for the representation. A downsampled frame size can be used, such as 80×60.

[0144] exist Figure 8 In the example of FIG. 8 , the registration engine 810 receives an active model frame AMF t Then try to convert the active model frame AMF t and one of the representations 830 in the data storage device 820. For example, the received active model frame AMF t A new representation is generated and compared to a representation 830 stored in a data storage device 820, for example a fern coded representation may be used to perform the match. A match may be indicated by the output of a comparison function, for example an error or matching imaging metric may indicate the similarity between the compared frames or representations. For example, a comparison may be made between fern codes. In this case, the fern codes may include binary codes, where the two images may be compared by summing the "distance" between the two binary codes. The smallest distance is considered the "best" match, where the distance may be compared to a threshold to determine if the match is "good". Active Model Frame AMF t The images may be downsampled to perform the matching, for example, the downsampled frames may be fern encoded. In this case, a metric value indicating the best match (e.g., the lowest value of an imaging metric indicating a difference or error) may be used to indicate the stored representation of the match ε j The stored representations may also include representations of the model frames, e.g., and In one case, the imaging metric values ​​of the best match can be further compared to a predefined threshold to determine whether the match is valid. For example, even a "best" match may be a poor actual match, e.g., having a high match error; thus, the threshold comparison avoids using these poor matches. The registration engine 810 may use downsampling, as if a relatively "rough" match is found and is correct, then subsequent active model frames and inactive model frames will be close enough to enable such Figure 8 , e.g. they will be aligned enough after a "global" loop closure to make a more precise "local" loop closure possible.

[0145] In one case, if no match is found, for example, if the matching imaging metric is above a given error threshold, then an active model frame AMF is performed. t and registration of inactive model frames, e.g. Figure 7 On the other hand, if a “best” match is found, for example if the imaging metric of the match is also below a given error threshold, then the match is represented by ε j The data is retrieved from the data storage device 820 and accessed by the registration engine 810. The registration engine 810 is then configured to indicate the Figure 7 The alignment operation described is similar to the alignment operation described in ; however, in the present case, the alignment is performed on the active model frame AMF t and the matching representation ε 1 For example, the components of the activity model frame (e.g. and ) can be matched with the representation ε j (For example and ) components. In one case, the alignment metric can be evaluated and compared to a threshold to determine whether to perform alignment, such as requiring a low level of misalignment compared to a predefined threshold to proceed with alignment. If alignment is not performed, the registration engine 810 can attempt to perform the alignment as described in the reference. Figure 7 If an alignment is to be performed, the registration engine 810 may be arranged to instruct the model deformer 840 to deform the existing three-dimensional model 850, for example to determine a three-dimensional deformation that is used to align components of the active model frame with components of the matched representation. In some cases, after such an alignment, which may be considered a "global loop closure", the active and inactive parts of the three-dimensional model are not updated. This may be because the alignment brings the active and inactive parts into closer alignment so that the alignment can be performed. Figure 7 Not updating these parts also enables correction or recovery of potentially inaccurate alignments in subsequent registration operations.

[0146] In this example, the model deformer 840 is arranged to access an existing three-dimensional model 850 and deform the model using a deformation graph 860 to generate an aligned three-dimensional model 870. The deformation graph 860 includes a collection of nodes and edges associated with distributed position elements of the existing three-dimensional model 850. In one case, each node may include: a timestamp; a position in three dimensions; a transformation definition; and a neighborhood set. The neighborhood of each node constitutes the edge of the graph, which may be directed. In this way, the deformation graph connects parts of the three-dimensional model that influence each other when performing deformation of the model. The number of neighborhoods may be limited, such as four neighborhoods in one embodiment. The transformation definition may include a definition of an affine transformation, such as represented by a 3×3 matrix (initialized to an identity matrix) and a 3×1 vector (initialized to zero) or by a dual quaternion. When performing deformation, the transformation definition of each node may be optimized according to a set of surface constraints. When applying deformation, a set of influencing nodes in the graph of a specific position element of the three-dimensional model is identified. Based on this, the position of the position element of the three-dimensional model can be deformed based on a weighted sum of the transformations of the influencing nodes, for example, a weighted sum of the transformations defined by each influencing node according to the distance of the positions of those nodes from the current position element. When using a surface element model, for example as described in reference Fig. 6A and Figure 6B As described, both the position and normal of the surface element can be deformed in this manner. For example, nodes in the deformation graph can be associated with the surface element based on their initialization time. The list of these nodes can then be sorted by this timestamp. When a deformation is indicated for a surface element, a binary search can be performed through the list of the node to fill in a set of nodes that are adjacent in time. From this set, a set of k nearest nodes is determined for the surface element based on a distance metric. These nodes are then used to deform the element. This process is very fast and helps to achieve real-time or near real-time performance.

[0147] In one example, a deformation graph can be constructed on a frame-by-frame basis. In one particular case, a new deformation graph of the three-dimensional model can be constructed for each frame of image data. This can include determining connectivity of the deformation graph, such as a neighborhood set for each graph node. In one case, the deformation graph is initialized using the three-dimensional model. For example, the node positions of a frame can be determined from the positions of position elements within the three-dimensional model (e.g., p in a facet model), and the node timestamps can be set to the position element timestamps (e.g., Fig. 6A"initial_time"). In one case, nodes for the deformation graph can be generated based on a sampling of positional elements in the three-dimensional model (such as systematic sampling). Such sampling can be evenly distributed overall, resulting in the spatial density of the deformation graph reflecting the spatial density of the three-dimensional model. The set of sampled nodes can be sorted based on the node timestamps. Thereafter, the connectivity of the deformation graph can be determined using the temporal information. In one case, a set of temporally adjacent nodes can be selected, for example based on the node timestamps. For example, if there are four neighbors and a given node is being considered, the nodes with the first two timestamps (in the time-ordered list) and the nodes with the subsequent two timestamps (in the list) can be selected as neighbors of the given node. This has the advantages of being computationally efficient and preventing temporally unrelated regions of the three-dimensional model from influencing each other (e.g., preventing active regions from influencing inactive regions). For example, when considered spatially, a deformation graph associated with multiple passes through a common three-dimensional space can be complex and entangled within itself; however, sorting and considering the graph in time allows the multiple passes to be disjoint and freely aligned. This may be contrasted with comparative approaches that determine graph connectivity based on pose timing (e.g. in the case of using pose graphs).

[0148] An exemplary process that may be applied by the model deformer 840 to deform the existing three-dimensional model 850 using the deformation map 860 to generate the deformed model 870 will now be described in more detail. The model deformer 840 performs the deformation of the existing three-dimensional model 850 by accessing a given position element of the existing three-dimensional model 850 (e.g., as referenced in FIG. 8A ). Fig. 6A The model deformer 840 begins with a deformation graph 860 that is closest in time to the given position element. The time interval is stored as a variable. Next, the model deformer 840 locates nodes that are adjacent in time, such as moving away from the time interval to explore a predefined number of nodes. These adjacent nodes can then be sorted by a distance metric such as Euclidean distance with reference to the position of the given position element. A given number of "neighborhood" nodes (e.g., using the neighborhood restrictions discussed above) can then be selected as the closest k nodes. A set of weights for each of these neighborhoods can then be generated based on the normalized distance between the node and the given position element. The sum of the weights can also be determined. Then as a last operation, a transformation definition of the neighborhood can be applied, such as weighted by the individual calculated weights of each neighborhood and normalized by the sum of the weights. This can include applying a variable of the affine transformation discussed above with reference to the given position element to deform the position and normal vector of the given position element. In Fig. 6A In the case of a surface element, other aspects of the element at a given position remain the same (e.g., can be copied to the deformed model 870).

[0149] In one example, the alignment performed by the registration engine 550, 760, or 810 is performed using the model deformer 840. In this example, this is achieved by optimizing the parameters of the deformation map 860. Given a set of surface correspondences set based on the output of the registration engine 550, 760, or 810, the optimization can reflect the surface registration in the three-dimensional model. These surface correspondences can indicate that a particular source position at a first time will reach a particular target position or coincide with a particular target position at a second time. Each individual surface correspondence can be absolute (associating the deformed position with an absolute position in three-dimensional space) or relative (associating the deformed position with a different deformed position). When aligning active frames and inactive frames (e.g., as reference frames), the surface correspondences can be determined based on the alignment of the active and inactive frames. Figure 7 The source point may represent a point in the active model frame, and the target point may represent a point in the inactive model frame, which is used to align the active part and the inactive part of the model. For example, the first time may include an initialization time for generating an inactive model position element for a given inactive model frame; the second time may be the current frame time; the source point may be a point for generating an active model frame (e.g., for calculating ) of the sampling position element, such as P based on the current posture estimate t and the target point can be used to generate the active model frame (for example, for calculating ) of the sampling position element, such as P based on the current posture estimate t plus the defined transformation H in 3D. In this case, the target point is equivalent to the point in the inactive model frame, because you are applying a defined transformation (H) that is calculated to map the active part of the model to the inactive part, e.g., the transformation is at least Figure 7 760 in the alignment performed by the registration engine 760. These surface correspondences can be used to apply "local loop closure". Similarly, when aligning the live frame with a stored representation (e.g., as referenced Figure 8 The first time may include the initialization time of the stored representation; the second time may be the current frame time; the source point may be a time for generating an active model frame (e.g., for calculating The sampling position element, such as based on the current posture estimate P t and the target point can be used to generate the active model frame (for example, for calculating ) as viewed based on the pose estimate represented plus a defined transformation H in three dimensions. Likewise, the target point represents the alignment of the active model and the inactive model when the defined transformation is applied to the active model portion. These surface correspondences can be used to apply "global loop closure". Surface correspondences can be calculated for sampled sets of pixels, such as for the frames used. In these cases, alignment or registration can include resolving the defined transformation in three dimensions for the set of surface correspondences.

[0150] In the above example, surface correspondence can be used in one or more cost functions to optimize the parameters of the deformation map. For example, a cost function can include an error function equal to the sum of the distance errors between the deformation source point (e.g., when the deformation map is applied) and the target point, and the source point and the target point are those used in the surface correspondence. The time parameterization of the three-dimensional model as described herein allows multiple passes of the same part of the three-dimensional space to be non-rigidly deformed into alignment, thereby allowing modeling to continue and merge new data into the revisited area of ​​the three-dimensional model. Another cost function can also be used to "fix" the inactive part of the three-dimensional model in place, that is, to deform the active part of the model into an inactive part. This cost function can include an error function equal to the sum of the distance errors between the deformation source point (e.g., when the deformation map is applied) and the non-deformed target point, and the target point is those used in the surface correspondence. Another cost function can also be used to keep the previously registered area of ​​the three-dimensional model in place, that is, when deforming different areas of the map, it may be necessary to constrain the relative position of the previously registered area to remain unchanged. This cost function can include an error function equal to the sum of the distance errors between the deformation source point (e.g., when the deformation map is applied) and the deformation target point. This cost function prevents loop closure and its associated deformations from pulling previously registered regions of the three-dimensional model apart. Error functions can also be defined to maximize rigidity in the definition transformation of the deformation map (e.g., by minimizing a distance metric between a transformation multiplied by its transpose and an identity matrix) and to ensure smooth deformation (e.g., based on a distance metric that incorporates neighborhood transformations). One or more of these described error functions can be minimized (e.g., within a weighted sum) to determine the transformation definition of the deformation map. If one or more cost functions output an error value below a predefined threshold (e.g., such as a cost function comparing a deformed source point to a target point), the alignment is accepted; if the error value is above a predefined threshold, the alignment is rejected (with equal cases being appropriately distributed).

[0151] Fig. 9 A method 900 for updating a three-dimensional model of an environment using a robotic device is shown. The robotic device may be, for example, Figures 1A to 1H The robot device 130, 160, 180, 190 shown, or as Figure 3A and Figure 3B The robotic device 310, 380 shown. In this example, the robotic device has a movably coupled image capture device, i.e., the image capture device is coupled to the robotic device in a manner that allows the image capture device to move relative to the robotic device. For example, as shown in FIG. Figure 1C , Figure 1E and Figure 1G The image capture device is mounted as shown and described. Alternatively, method 900 can be performed using another exemplary robotic device.

[0152] At block 910, movement of at least one of an image capture device and a robotic device within an environment is indicated. For example, this may be accomplished by, for example, Figure 3B The movement may be manually instructed, such as using a remote control, and / or be part of an autonomous movement, such as instructed by a navigation system of the robotic device. The movement may include moving only the robotic device, moving only the image capture device (e.g. using a movable support), or moving both the robotic device and the image capture device. In the case where the movement is performed iteratively, different parts of the robotic device may be moved in each iteration, for example, there may be an exploration phase in which the robotic device moves within the environment and a scanning phase in which the movable support is moved to move the image capture device. These phases may be repeated periodically.

[0153] At block 920, image data representing an observation of an environment using an image capture device is obtained. For example, this may include obtaining FIG. 2A to FIG. 2C Image data 220 is shown. The image data may include frames of depth data and / or photometric data.

[0154] At block 930, measurements of movement are obtained. This may include measurements of one or more of the robotic device and the image capture device. Measurements may be obtained from a set of actuators, such as the robotic actuator 332 and / or the capture device actuator 334, such as Figure 3B In other cases, the measurements may be obtained from observations of one or more of the robotic device and the image capture device. The measurements may be obtained from one or more motors, such as drive motors for moving parts and / or joint motors for articulated capture device supports.

[0155] At block 940, the model state of the robotic device is updated. The model state may include the state of the robotic device, such as reference Figure 3A and Figure 3B The model state may be represented using posture data defining the posture of one or more of the robotic device and the image capture device. For example, the model state may include defining the transformation T described previously. WC and T WB In one case, block 920 includes obtaining a frame of video data and determining a model state for a current frame of video data and a previous frame of video data. In this case, block 940 may include determining a previous model state and a current model state, such as Figure 4AThe current state 450 and the previous state 460 in the block 940 may be viewed as a tracking operation of the robotic device. In this case, the tracking operation uses the measured values ​​of the movement to improve the model state. The update at block 940 includes optimizing a function of the model state and the measured values ​​of the movement. For example, this may include constraining the update of the model state based on the measured values ​​of the movement. Block 940 may include a function related to the relative Figure 4A Operations similar to those described for state engine 440 .

[0156] At block 950, the three-dimensional model of the environment is updated based on a comparison of the image data and the three-dimensional model. The comparison uses the updated model state from block 940. The comparison includes using a projection of the updated model state, such as a back-projection of the image data from block 920 onto the three-dimensional model or a projection of the three-dimensional model onto a plane associated with the current pose of the image capture device. The comparison may thus be in two-dimensional image space or in three-dimensional model space. The updating of the three-dimensional model may include comparing the image data with respect to the image data. Figure 5 , Figure 7 and Figure 8 Operations similar to those described.

[0157] In some cases, block 910 includes one or more of: instructing a first set of actuators to move a robotic device within an environment; and instructing a second set of actuators to move an image capture device relative to the robotic device. In this case, block 930 includes obtaining odometer data from the first set of actuators and obtaining kinematic data from the second set of actuators.

[0158] As about Figure 4A As described by the state engine 440 of the robot device, updating the model state of the robot device may include optimizing a cost function. The cost function may include a set of selectable error terms. In one case, the cost function includes an error term associated with a kinematic error and an error term associated with an odometer error. In this case, the kinematic error includes a comparison of a spatial relationship between the image capture device and the robot device with kinematic measurements from the robot device. For example, the spatial relationship may be modeled using forward kinematics applied to a movable support of the image capture device. The odometer error may include a comparison of changes in the spatial relationship between the robot device and a three-dimensional model of the environment with odometer data from the robot device. The odometer data may be provided by a drive system of the robot device.

[0159] In some cases, the cost function includes an error term associated with a linear prior. The linear prior may be constructed using an error value and a model state of the robotic device from a previous update operation. In some cases, block 940 includes optimizing a previous state of the robotic device and a current state of the robotic device. In some cases, the cost function includes an error term associated with a geometric alignment error. In this case, the geometric alignment error may include a comparison of data derived from acquired image data using a model state of the robotic device prior to block 940 with the three-dimensional model. Exemplary error terms may include a reference Figure 4A Describes the error term.

[0160] Fig.10 and Fig.11 Provides examples of status updates, such as using Figure 3A and Figure 3B The robot devices 310, 380, Figure 4A The state engine 440 and / or use Fig. 9 The method 900 is executed. Fig.10 An example 1000 of movement in the real world and corresponding changes in the state of the robotic device is shown. Fig.11 An example showing how to model states.

[0161] exist Fig.10 , the left hand portion 1010 shows a robotic device moving from a first position 1015 in an environment to a second position 1020 in the environment. The robotic device in this example is Figure 1C and Figure 1D The movement may include one or more of movement of a chassis of the robotic device and movement of the image capture device (eg, via movement of an arm coupling the image capture device to the chassis).

[0162] Fig.10 The right hand portion 1030 shows the state of the robotic device in the model space, for example using the relative Figure 1D In a first position 1015, the robotic device has a state represented by data defining a pose of the chassis 1032 and data defining a pose of the image capture device 1034, both poses being defined relative to the model space.

[0163] Fig.10 shows how the state of the robotic device in the model space may drift when using the comparison method. For example, model drift may occur, such as Fig.101044 and the data defining the pose of the chassis 1042 and the data defining the pose of the image capture device 1044 are shown in FIG. Model drift may occur if kinematic and odometry measurements are used without state optimization, e.g., without an additional geometric error term. Similarly, model drift may also occur using combined photometric and geometric errors without using kinematic and odometry measurements, e.g., by matching frames of video data from an image capture device to a model of the environment. In this example, using movement measurements within the state optimization process helps the states in the model space more accurately reflect the real-world configuration of the robotic device. Therefore, in Fig.10 , data defining the posture of chassis 1052 after state optimization and data defining the posture of image capture device 1054 after state optimization are shown, for example, in Figure 4A The data 1052, 1054 more accurately reflects the second position 1020 of the robotic device. This increased accuracy is particularly useful for complex combinations of image capture device and robotic device movement, where the comparison method may easily drift and "lose" the current position and orientation of the robotic device and the coupled image capture device.

[0164] Fig.11 An exemplary state diagram 1100 is shown for two states of a robotic device. The state diagram 1100 is divided into four parts 1102 to 1108. The first part 1102 represents the previous component, the second part 1104 represents the component of the previous state, the third part 1106 represents the error component and the fourth part 1108 represents the component of the current state. The first part 1102 shows a linear prior 1110. The linear prior 1110 is used to constrain the previous state, in particular the pose of the image capture device 1120 and the pose of the robotic device 1130. Using the reference Figure 4B Describes the transformation (e.g. T WB and T WC ) to represent each posture. The error components of the third part 1106 include geometric error terms 1140, kinematic error terms 1150, and odometer error terms 1160. For example, these error terms may correspond to Figure 4A The error terms 410 to 430 in FIG. 11 are used to constrain the pose of the robotic device 1130 in the previous state and the pose of the robotic device 1180 in the current state (as shown in the fourth portion 1108). The kinematic error term 1150 is used to constrain the pose of the robotic device 1180 in the current state and the pose of the image capture device 1170 in the current state. Finally, the geometric error term 1140 is also used to constrain the pose of the image capture device 1170 in the current state.

[0165] exist Fig.11In the example of , the previous state and the current state can relate to the previous frame and the current frame of video data obtained from the image capture device. After receiving the new "current" frame of video, the values ​​of the poses of the previous frame and the current frame can be determined via a state optimization operation. The error component of the third part 1106 can be based on measurements made by a subsystem of the robotic device (e.g., an electromechanical control system). The linear prior 1110 can be obtained from the marginalization of the state and error from the previous time iteration. The state diagram can therefore be used to construct a sliding window optimization problem that can be solved iteratively to determine the values ​​of the previous state and the current state.

[0166] Certain examples described herein allow for tight coupling of kinematic and odometer data from a mobile robotic device within a mapping system such as a dense SLAM system. These examples can be implemented in real time or near real time on a variety of robotic devices that are characterized by up to six degrees of freedom for the robotic device (e.g., the "body," "core," or "chassis" of the robot) and for an image capture device (e.g., on the "arm" or "head" of the robot) coupled to the robotic device. Certain examples described herein jointly optimize a state model to estimate the pose of the robotic device and the image capture device. The joint optimization may include one or more of a kinematic error term, an odometer error term, a geometric error term, and a linear prior representing a prior state and error.

[0167] Certain examples described herein enable building a dense three-dimensional model of an environment in real time or near real time as a robotic device navigates the environment. The use of a movable image capture device allows for more efficient and flexible mapping of the environment, particularly useful for mapping constrained environments, such as living spaces and / or disaster or conflict zones. In some examples, the three-dimensional model uses active regions to achieve tight local loop closure. This may be surface dependent when a surface metamodel is used. If the active portion of the model drifts too far from the inactive portion so that the local alignment cannot converge, an appearance-based global loop closure method may be used to guide a deformation that realigns the active portion of the model with the underlying inactive portion for tight global loop closure and model consistency, e.g., relative to the surface of the model.

[0168] The performance of embodiments of the described examples is comparable to that of comparison methods for simple and smooth camera trajectories, while providing substantial improvements for challenging motion and visual data (rapid erratic motion such as fast rotations). The examples described herein also provide improved mapping for areas lacking significant photometric and geometric variations, such as walls and floors of common colors (e.g., where the camera observes a tiled ceiling or a carpeted floor). In comparison methods, the lack of informative image data can lead to large model drift, e.g., Fig.10 shown.

[0169] Certain examples described in this paper provide improvements in quantitative tracking accuracy, e.g., compared to ground truth measurements, and qualitative reconstruction consistency. Fig.12 Exemplary test data demonstrating these improvements is shown. Fig.12 A two-dimensional top view of the environment is shown. In the test case, the environment was a room of approximately 5 meters by 5 meters. Line 1210 represents the "ground truth" path of the robotic device, i.e., the actual path of the robotic device measured independently of the systems and methods described herein. Line 1220 then represents the modeled position of the robotic device, as modeled by the state optimization example described herein, e.g., by applying Figure 3A , Figure 3B or Figure 4A . Line 1230 shows the modeled position of the robotic device using a comparison system that tracks position by minimizing the combined geometric and photometric errors, e.g., without the kinematic and odometry error terms described herein. As can be seen, line 1220 closely matches line 1210, but line 1230 differs from line 1210 in several places. In this test case, the comparison system has difficulty maintaining an accurate track during rapid rotations, especially given the lack of variation in the image data. In contrast, the examples described herein are able to utilize onboard measurements from the robotic device to aid in navigation.

[0170] When using a SLAM system without a posture graph, some examples described herein enable the generation of a robust and consistent map. For example, this example can be used to improve a SLAM system without a posture graph, where there are no constraints on kinematics and odometer measurements, and the three-dimensional model may be distorted in a manner that deviates from the actual "ground truth" geometry. Therefore, this example improves the three-dimensional structure of the mapped environment and locates the dense visual SLAM method of the robotic device within the generated structure. The examples described herein can also allow objects that are repeatedly viewed to be more accurately located in the global model of the environment. The model may also show improved local consistency.

[0171] Certain examples described herein provide improvements to comparison methods that use inertial measurements, such as those provided by an IMU. These methods are susceptible to global drift due to noise and bias in the inertial measurements. This drift may be due to reliance on accelerometer measurements and IMU biases. For example, certain comparison systems may use an extended Kalman filter to estimate the transformation between pairs of images (e.g., consecutive video frames).

[0172] Some examples described herein utilize kinematic and odometer measurements that can be obtained "for free" when using a mobile robot platform. For example, wheeled robots typically provide accessible odometer data, and manipulator kinematics can be calculated as part of the manipulator control process. Predictions made using data from a movable support (such as data based on a common manipulator) can be based on direct joint angle measurements, which can reduce the drift found in the inertial system while still providing robustness improvements. Mounting an image capture device at the end effector position on the manipulator has also been shown to provide improvements when mapping the environment. Relative odometer measurements can be used to optimize current and past states. The examples described herein can be implemented at real-time frame rates (e.g., 30 frames per second), and are therefore very suitable for fast SLAM systems, such as those without a posture graph and systems that perform flexible deformation operations on a facet model.

[0173] Certain system components and methods described herein may be implemented by computer program code that may be stored on a non-transitory storage medium. Fig.13 A specific example 1300 of a system including at least one processor 1310 arranged to retrieve data from a computer readable storage medium 1320 is shown. The system may include a portion of a mobile robotic device as described above. The computer readable storage medium 1320 includes a set of computer readable instructions 1330 stored thereon. The instructions 1330 are arranged to cause the at least one processor 1310 to perform a series of actions. These actions may include Fig. 9 Method 900 or its variants, and / or as described in reference Figure 4A The actions of state engine 440 are described.

[0174] The above examples should be understood to be illustrative. Additional examples are contemplated. Although certain components of each example have been described separately, it should be understood that the functions described with reference to one example may be suitable for implementation in another example, and certain components may be omitted depending on the implementation. It should be understood that any feature described with respect to any one example may be used alone, or in combination with other features described, and may also be used in combination with one or more features of any other example, or in combination with any combination of any other example. In addition, equivalents and modifications not described above may also be adopted without departing from the scope of the invention as defined by the appended claims.

Claims

1. A mapping system for a robotic device, the robotic device comprising one or more actuators for moving the robotic device within an environment and a movable support for an image capture device, the mapping system comprising: a state engine for jointly optimizing a current state of the robotic device and a previous state of the robotic device based on kinematic errors, odometry errors, and geometric errors, wherein each state of the robotic device comprises data indicating a pose of the robotic device relative to a model of the environment and data indicating a pose of the image capture device relative to the model, wherein the kinematic error comprises a function of kinematic measurements associated with the moveable support and a pose of the image capture device relative to the robotic device derived from the current state, wherein the odometer error comprises a function of an odometer measurement associated with the one or more actuators and an odometer difference between the current state and the previous state, wherein the geometric error comprises a comparison of image data from the capture device with the model of the environment, the comparison using a projection based on the current state, The mapping system is configured to update the model using the current state optimized by the state engine.

2. The mapping system of claim 1, wherein the movable support comprises a plurality of joints and the pose of the image capture device relative to the robotic device is determined using forward kinematics.

3. A mapping system according to claim 1 or claim 2, wherein the pose of the robotic device relative to the model and the pose of the image capture device relative to the model are defined within six degrees of freedom.

4. The mapping system according to claim 1 or 2, wherein the state engine is configured to jointly optimize the current state and the previous state using a linear prior, and the state engine is configured to update the optimized linear prior based on the kinematic error, the odometer error, the geometric error, and the marginalization of the previous state.

5. A mapping system according to claim 4, wherein the mapping system is configured to align a first set of portions of the model with a second set of portions of the model using a deformation map, the deformation map indicating a set of neighbors for a given position in the model to be used to modify the model at the given position during alignment.

6. The mapping system of claim 5, wherein the mapping system is configured to align the first set of portions of the model with the second set of portions of the model during loop closure, wherein after the loop closure, the linear prior and the previous state are reinitialized.

7. A mapping system according to claim 1 or 2, wherein the model includes a surface element model, wherein each surface element in the surface element model includes at least data defining the position of the surface element in three dimensions and data defining the normal vector of the surface element in three dimensions, wherein each surface element represents a two-dimensional area in three-dimensional space.

8. The mapping system of claim 1 or 2, wherein the mapping system is configured to generate a model without a pose graph for the image capture device.

9. A robotic device having at least one current state and a previous state, the robotic device comprising: an image capture device; a movable support on which the image capture device is movably mounted relative to the robotic device; one or more actuators for moving one or more of the robotic device and the image capture device relative to an environment; a state engine for updating the current state of the robotic device; as well as a mapping engine for generating a three-dimensional model of the environment using the current state of the robotic device, wherein the current state of the robotic device comprises at least a transformation of the image capture device relative to the three-dimensional model, wherein the state engine is configured to update the current state of the robotic device based on a comparison of the current state of the robotic device with measurements obtained from the one or more actuators, and wherein the mapping engine is configured to compare the image from the image capture device with the three-dimensional model to update the three-dimensional model based on the current state of the robotic device; wherein, to update the current state of the robotic device, the state engine is configured to jointly optimize the current state of the robotic device and the previous state of the robotic device based at least in part on a kinematic error, and Wherein the kinematic error comprises a function of kinematic measurements associated with the support and a pose of the image capture device relative to the robotic device derived from the current state.

10. The robotic device of claim 9, wherein the mapping engine is configured to determine an iterative closest point error, and wherein the state engine is configured to additionally use the iterative closest point error to update the current state of the robotic device.

11. The robotic apparatus of claim 9 or claim 10, wherein the one or more actuators comprise: a first set of actuators for moving the robotic device relative to the environment, and a second set of actuators for moving the movable support of the image capture device relative to the robotic device, The current state of the robotic device further includes a transformation of the robotic device relative to the three-dimensional model.

12. The robotic device of claim 11, wherein the first set of actuators provides multiple degrees of freedom between the robotic device and the environment, and wherein the second set of actuators provides multiple degrees of freedom between the image capture device and the robotic device.

13. The robotic device of claim 9 or 10, wherein the image capture device is configured to output color image data and a depth map, and the mapping engine is configured to update the three-dimensional model using the color image data and the depth map.

14. A method of updating a three-dimensional model of an environment using a robotic device having a movably coupled image capture device, The robot device comprises: Removable stand; a movably coupled image capture device movably coupled to the robotic device via the bracket; The method comprises: instructing at least one of the image capture device and the robotic device to move within the environment; obtaining, using the image capture device, image data representing an observation of the environment; obtaining a measurement of said movement; updating a model state of the robotic device, the model state of the robotic device comprising pose data of the robotic device and the image capture device, the updating comprising optimizing a function of the model state and the measured value of the movement, wherein the model state of the robotic device is a current model state of the robotic device, and wherein updating the model state of the robotic device comprises jointly optimizing the current model state of the robotic device and a previous model state of the robotic device based at least in part on a kinematic error, wherein the kinematic error comprises a comparison of: a spatial relationship between the image capture device and the robotic device obtained using forward kinematics applied to the support, and Kinematic measurements of the robotic device; and The three-dimensional model of the environment is updated based on a comparison of the image data to the three-dimensional model using the updated model state, the comparison including using a projection of the updated model state.

15. The method of claim 14, wherein the pose data comprises a transformation of the robotic device relative to the three-dimensional model and a transformation of the image capture device relative to at least one of the robotic device and the three-dimensional model.

16. The method of claim 14 or claim 15, wherein indicating movement comprises one or more of: instructing a first set of actuators to move the robotic device within the environment; and instructing a second set of actuators of the support to move the image capture device relative to the robotic device, and Wherein obtaining measurements of movement from the robotic device comprises one or more of: obtaining odometer data from the first set of actuators; and Kinematic data is acquired from the second set of actuators.

17. The method of claim 14 or 15, wherein updating the model states of the robotic device and the image capture device comprises: optimizing a cost function including an error term associated with the kinematic error and an error term associated with the odometry error, and Wherein the odometry error comprises a comparison of a change in the spatial relationship between the robotic device and the three-dimensional model of the environment and odometry data from the robotic device.

18. The method of claim 17, wherein the cost function includes an error term associated with a linear prior, wherein the linear prior is constructed using an error value and the model state of the robotic device from a previous update operation.

19. The method of claim 17, wherein the cost function includes an error term associated with a geometric alignment error, wherein the geometric alignment error includes a comparison of depth data derived from the acquired image data with the three-dimensional model using the model state of the robotic device prior to the updating.

20. A non-transitory computer-readable storage medium comprising computer-executable instructions which, when executed by a processor, cause a computing device to perform the method of any one of claims 14 to 19.

Images