Resource-aware large-scale cooperative 3D mapping using multiple mobile devices

Information

  • Patent Grant
  • 10203209
  • Patent Number
    10,203,209
  • Date Filed
    Thursday, May 25, 2017
    7 years ago
  • Date Issued
    Tuesday, February 12, 2019
    5 years ago
Abstract
A method includes: receiving, with a computing platform, respective trajectory data and map data independently generated by each of a plurality of vision-aided inertial navigation devices (VINS devices) traversing an environment, wherein the trajectory data specifies poses along a path through the environment for the respective VINS device and the map data specifies positions of observed features within the environment as determined by an estimator executed by the respective VINS device; determining, with the computing platform and based on the respective trajectory data and map data from each of the VINS devices, estimates for relative poses within the environment by determining transformations that geometrically relate the trajectory data and the map data between one or more pairs of the VINS devices; and generating, with the computing platform and based on the transformations, a composite map specifying positions within the environment for the features observed by the VINS devices.
Description
TECHNICAL FIELD

This disclosure relates to navigation and, more particularly, to vision-aided inertial navigation.


BACKGROUND

In general, a Vision-aided Inertial Navigation System (VINS) fuses data from a camera and an Inertial Measurement Unit (IMU) to track the six-degrees-of-freedom (d.o.f) position and orientation (pose) of a sensing platform through an environment. In this way, the VINS combines complementary sensing capabilities. For example, an IMU can accurately track dynamic motions over short time durations, while visual data can be used to estimate the pose displacement (up to scale) between consecutive views. For several reasons, VINS has gained popularity to address GPS-denied navigation. During the past decade, VINS have been successfully applied to robots, spacecraft, automotive, and personal localization (e.g., by use of smartphones or laptops), demonstrating real-time performance.


Creating an accurate 3D map within a GPS denied area is required in many applications, such as human (or robot) indoor navigation and localization, augmented reality, and search and rescue. However, creating a complex map with a single mobile device, such as a mobile phone, tablet or wearable computer, presents certain challenges. For example, the device used for recording data may not have sufficient resources (e.g., storage space or battery) to collect data covering a large area. Additionally, it may not be convenient for a single user to navigate the whole building at once. Furthermore, anytime a portion of the map changes (e.g., due to lighting conditions and building reformations), or is deemed of insufficient quality or accuracy, the mapping process must be repeated.


SUMMARY

In general, this disclosure describes techniques for enhanced, large-scale mapping of a 3D environment using multiple, cooperative mobile devices. In some cases, the cooperative mapping techniques described herein may be applied to process visual and inertial data collected from multiple users at different times. Moreover, the cooperative mapping techniques described herein may be applied even where transformation between the users' starting positions and orientations (poses) are not known.


In other words, the example technical solutions described herein may address the problem of cooperative mapping (CM) using datasets collected by multiple users at different times, when the transformation between the users' starting poses is unknown. As examples, CM solutions are described that formulate CM as a constrained optimization problem, where each user's independently estimated trajectory and map are merged together by imposing geometric constraints between commonly-observed point and line features. Additionally, the solutions may efficiently solve the CM problem, by taking advantage of its structure. The proposed technical solutions and implementations are proven, in examples, to be batch-least-squares (BLS) optimal over all users' datasets, while it is less memory demanding and lends itself to parallel implementations. In particular, the solutions are shown to be faster than a standard BLS solution, when the overlap between the users' data is small. Furthermore, resource-aware implementations are described that are enabled to consistently trade accuracy for lower processing cost, by retaining only an informative subset of the common-feature constraints.


In one example, the techniques of the disclosure describe a method including: receiving, with a computing platform having one or more hardware-based processors, respective trajectory data and map data independently generated by each of a plurality of vision-aided inertial navigation devices (VINS devices) traversing an environment, wherein the trajectory data specifies poses along a path through the environment for the respective VINS device and the map data specifies positions of observed features within the environment as determined by an estimator executed by the respective VINS device; determining, with the computing platform and based on the respective trajectory data and map data from each of the VINS devices, estimates for relative poses within the environment by determining transformations that geometrically relate the trajectory data and the map data between one or more pairs of the VINS devices; and generating, with the computing platform and based on the transformations, a composite map that specifies positions within the environment for the features observed by any of the VINS devices.


In another example, the techniques of the disclosure describes a vision-aided inertial navigation system including: a plurality of mobile devices, each of the mobile devices including: at least one image source to produce image data along a trajectory of the mobile device within an environment, wherein the image data contains a plurality of features observed within the environment at a plurality of poses of the mobile device along the trajectory; an inertial measurement unit (IMU) to produce IMU data indicative of motion of the vision-aided inertial navigation system; and a hardware-based processing unit including an estimator that determines, based on the image data and the IMU data, trajectory data specifying a position and orientation of the mobile device for a plurality of poses of the mobile device along the trajectory and map data specifying positions with the environment for features observed from the poses; and a cooperative mapping server configured to: receive respective trajectory data and map data independently generated by each of mobile devices; determine transformations that geometrically relate the trajectory data and the map data between one or more pairs of the mobile devices; and generating, with the computing platform and based on the transformations, a composite map that specifies positions within the environment for the features observed by any of the mobile devices.


In another example, the techniques of the disclosure describe a non-transitory computer-readable medium including instructions that, when executed, cause one or more hardware-based processors of a computing platform to: receive respective trajectory data and map data independently generated by each of a plurality of vision-aided inertial navigation devices (VINS devices) traversing an environment, wherein the trajectory data specifies poses along a path through the environment for the respective VINS device and the map data specifies positions of observed features within the environment as determined by an estimator executed by the respective VINS device; determine, based on the respective trajectory data and map data from each of the VINS devices, estimates for relative poses within the environment by determining transformations that geometrically relate the trajectory data and the map data between one or more pairs of the VINS devices; and generate, based on the transformations, a composite map that specifies positions within the environment for the features observed by any of the VINS devices.


The details of one or more embodiments of the invention are set forth in the accompanying drawings and the description below. Other features, objects, and advantages of the invention will be apparent from the description and drawings, and from the claims.





BRIEF DESCRIPTION OF DRAWINGS


FIG. 1 is a block diagram illustrating a vision-aided inertial navigation system (VINS) that navigates an environment having a plurality of features using one or more image sources and inertial measurement unit (IMUs) in accordance with the techniques of the disclosure.



FIG. 2 illustrates an example implementation of the VINS of FIG. 1 in further detail, in accordance with the techniques of the disclosure.



FIG. 3 illustrates an environment including Non-common (stars) and common (triangles) point features, as well as line features observed by mobile devices, in accordance with the techniques of the disclosure.



FIG. 4 is a flowchart illustrating an example operation in accordance with the techniques of the disclosure.



FIG. 5 illustrates the parameterization and measurement of free lines and Manhattan lines, in accordance with the techniques of the disclosure.



FIG. 6 is a depiction of line constraints in accordance with the techniques of the disclosure.



FIGS. 7A-7D are a series of diagrams illustrating the process for finding a minimum spanning tree on the user graph, in accordance with the techniques of the disclosure.



FIGS. 8A-8C depict an estimated feature 3D point cloud, in accordance with the techniques of the disclosure.



FIGS. 9A-9D depict cooperative mapping (CM) estimated trajectories, in accordance with the techniques of the disclosure.



FIGS. 10A-10C depict a merged map of the CM estimated trajectories of FIGS. 9A-9D, in accordance with the techniques of the disclosure.



FIG. 11 illustrates trajectories of all users in Keller Hall using points-only versus points, free lines, and Manhattan lines, in accordance with the techniques of the disclosure.



FIG. 12 is a graph illustrating position difference of sparse CM over the users' trajectories with respect to CM, in accordance with the techniques of the disclosure.



FIG. 13 illustrates trajectories estimated from original CM and sparse CM with common-feature sparsification using a grid, in accordance with the techniques of the disclosure.



FIGS. 14A-14D illustrate example common point features before and after sparsification using grids of different sizes, in accordance with the techniques of the disclosure.



FIG. 15 shows a detailed example of various devices that may be configured to implement some embodiments in accordance with the current disclosure, in accordance with the techniques of the disclosure.





Like reference characters refer to like elements throughout the figures and description.


DETAILED DESCRIPTION

The cooperative mapping (CM) techniques described herein provide technical solutions that take advantage of the problem structure to achieve certain technical benefits and improvements, and may utilize both consecutive and loop-closure observations of point and line features to achieve high accuracy. For example, in example implementations, CM is formulated as a constrained optimization problem, in which the cost function is expressed as the sum of the cost functions from all users, while the constraints express the geometric relationship between the features commonly observed by two or more users. Based on these formulations and approaches, technical solutions having technical improvements over conventional systems are described.


In one example, CM techniques are described that are modular (i.e., maps or submaps can be easily and efficiently added or removed), lend themselves to parallel implementation, and are more memory efficient than conventional techniques within the same technological field. In addition, the proposed technical solutions leverage each individual user's intermediate mapping results to reduce the processing cost. Moreover, the described CM techniques and implementations described herein allow for consistently trading estimation accuracy with computational-cost savings by appropriately reducing the commonly-observed-feature constraints to a number that can be analytically determined. In addition, example implementations of the described CM techniques may utilize points, “free lines” (lines not aligned with the cardinal directions of the building), and “Manhattan lines” to improve the estimation accuracy. Additionally, the described CM techniques provide robust methods for detecting and applying loop-closure line measurements in a visual-inertial mapping system. As described herein, the described CM techniques have been validated in large-scale 3D experiments using datasets collected from multiple mobile devices.



FIG. 1 is a block diagram illustrating a vision-aided inertial navigation system (VINS) 10 that navigates an environment 2 having a plurality of features 15 using one or more image sources and inertial measurement unit (IMUs). That is, VINS 10 is one example of a device that utilizes a 3D map of environment 2 to determine the position and orientation of VINS 10 as the VINS traverses the environment, where the map may be constructed in real-time by the VINS or previously constructed. Environment 2 may, for example, represent an environment where conventional GPS-signals are unavailable for navigation, such as on the moon or a different planet or even underwater. As additional examples, environment 2 may represent an indoors environment such as the interior of a building, such as a convention center, shopping mall, sporting arena, business office and the like. Features 15, also referred to as landmarks, represent objects visible within environment 2, such as rocks, trees, signs, walls, stairs, chairs, tables, and the like. Features 15 may be moving or stationary objects within environment 2.


VINS 10 represents any mobile device that implements the techniques described herein. VINS 10 may be, for example, a robot, mobile sensing platform, a mobile phone, a laptop, a tablet computer, a vehicle, an unmanned aircraft system (UAS) such as a drone, and the like. The increasing range of sensing capabilities offered by modern mobile devices, such as cell phones and tables, as well as their increasing computational resources make them ideal for applying VINS. In some implementations, the techniques described herein may be used within environments having GPS or similar signals and may provide supplemental localization and mapping information.


For purposes of example, VINS 10 is shown as an autonomous robot although, as discussed above, VINS 10 may take the form of other devices that implement the techniques described herein. While traversing environment 2, the image sources of VINS 10 produce image data at discrete time instances along the trajectory within the three-dimensional (3D) environment, where the image data captures features 15 within the 3D environment at each of the time instances. In addition, IMUs of VINS 10 produces IMU data indicative of a dynamic motion of VINS 10.


As described in detail herein, VINS 10 includes a hardware-based computing platform that implements an estimator that fuses the image data and the IMU data to perform localization of VINS 10 within environment 10. Estimator 22 process image data 14 and IMU data 18 to estimate the 3D IMU pose and velocity together with the time-varying IMU biases, camera rolling shutter and IMU-camera time synchronization and to produce, based on the captured image data, estimates for poses of VINS 10 along the trajectory and, in some cases, a position and orientation within an overall map of the environment, where the map may be constructed using the cooperative mapping techniques described herein. Utilizing these techniques, VINS 10 may navigate environment 2 and, in some cases, may construct or augment the mapping information for the environment including the positions of features 15.


The estimator of VINS 10 may operate according to different types of estimators. For example, in an example implementation, VINS 10 implements as described in U.S. Pat. No. 9,243,916, the entire contents of which are incorporated herein. VINS 10 implements an inverse, sliding-window filter (ISWF) as described in U.S. patent application Ser. 14/796,574, filed Jul. 10, 2015, entitled “INVERSE SLIDING-WINDOW FILTERS FOR VISION-AIDED INERTIAL NAVIGATION SYSTEMS,” the entire contents of which is incorporated herein by reference. In other examples, VINS 10 implements a sliding-window Iterative Kalman Smoother (IKS) as described U.S. patent application Ser. 15/130,736, filed Apr. 15, 2016, entitled “ITERATIVE KALMAN SMOOTHER FOR ROBUST 3D LOCALIZATION FOR VISION-AIDED INERTIAL NAVIGATION,” the entire contents of which are incorporated herein.



FIG. 2 illustrates an example implementation of VINS 10 in further detail. Image source 12 of VINS 10 images an environment in which VINS 10 operates so as to produce image data 14. That is, image source 12 generates image data 14 that captures a number of features visible in the environment. Image source 12 may be, for example, one or more cameras that capture 2D or 3D images, a laser scanner or other optical device that produces a stream of 1D image data, a depth sensor that produces image data indicative of ranges for features within the environment, a stereo vision system or a vision system having multiple cameras to produce 3D information, a Doppler radar and the like. In this way, image data 14 provides exteroceptive information as to the external environment in which VINS 10 operates. Moreover, image source 12 may capture and produce image data 14 at time intervals in accordance one or more clocks associated with the image source. In other words, image source 12 may produce image data 14 at each of a first set of time instances along a trajectory within the three-dimensional (3D) environment, wherein the image data captures features 15 within the 3D environment at each of the first time instances.


IMU 16 produces IMU data 18 indicative of a dynamic motion of VINS 10. IMU 16 may, for example, detect a current acceleration using one or more accelerometers as VINS 10 is translated, and detect the rotational velocity (i.e., the rate of change in rotational attributes like pitch, roll and yaw) using one or more gyroscopes as VINS 10 is rotated. IMU 14 produces IMU data 18 to specify the detected motion. In this way, IMU data 18 provides proprioceptive information as to the VINS 10 own perception of its movement and orientation within the environment. Moreover, IMU 16 may produce IMU data 18 at time intervals in accordance a clock associated with the IMU. In this way, IMU16 produces IMU data 18 for VINS 10 along the trajectory at a second set of time instances, wherein the IMU data indicates a motion of the VINS along the trajectory. In many cases, IMU 16 may produce IMU data 18 at much faster time intervals than the time intervals at which image source 12 produces image data 14. Moreover, in some cases the time instances for image source 12 and IMU 16 may not be precisely aligned such that a time offset exists between the measurements produced, and such time offset may vary over time. In such cases, VINS 10 may compensate and correct for any misalignment by applying the techniques described in U.S. patent application Ser. No. 14/733,468, entitled “EFFICIENT VISION-AIDED INERTIAL NAVIGATION USING A ROLLING-SHUTTER CAMERA WITH INACCURATE TIMESTAMPS,” incorporated herein by reference.


In general, estimator 22 fuses image data 14 and IMU data 18 to determine a position and orientation of VINS 10 as well as positions of features 15 as the VINS traverses environment 2. That is, estimator 22 of processing unit 20 process image data 14 and IMU data 18 to compute state estimates for the various degrees of freedom of VINS 10 and, from the state estimates, computes position, orientation, speed, locations of observable features, a map to be used for localization, an odometry or other higher order derivative information represented by VINS data 24. Processing unit 20 may, for example, comprise a hardware-based computing platform having one or more processors that execute software instructions and/or application-specific hardware for implementing the techniques described herein.


In the example of FIG. 2, estimator 22 comprises a processing pipeline 11 for measurements from image source 12 and IMU 16. In this example, processing pipeline 11 includes feature extraction and tracking module 12, outlier rejection module 13, information manager 15 and filter 23.


Feature extraction and tracking module 12 extracts features 15 from image data 14 acquired by image source 12 and stores information describing the features in feature database 25. Feature extraction and tracking module 12 may, for example, perform corner and edge detection to identify features and track features 15 across images using, for example, the Kanade-Lucas-Tomasi (KLT) techniques described in Bruce D. Lucas and Takeo Kanade, An iterative image registration technique with an application to stereo vision, In Proc. of the International Joint Conference on Artificial Intelligence, pages 674-679, Vancouver, British Columbia, Aug. 24-28 1981, the entire content of which in incorporated herein by reference.


Outlier rejection module 13 provides robust outlier rejection of measurements from image source 12 and IMU 16. For example, outlier rejection module may apply a Mahalanobis distance tests to the feature measurements to identify and reject outliers. As one example, outlier rejection module 13 may apply a 2-Point Random sample consensus (RANSAC) technique described in Laurent Kneip, Margarita Chli, and Roland Siegwart, Robust Real-Time Visual Odometry with a Single Camera and an IMU, In Proc. of the British Machine Vision Conference, pages 16.1-16.11, Dundee, Scotland, Aug. 29-Sep. 2 2011, the entire content of which in incorporated herein by reference.


Information manager 15 selects features from feature database 15 and feeds measurements for the selected features to filer 23, which may perform simultaneous localization of the position and orientation for VINS 10 within environment 2 by iteratively optimizing over measurements throughout trajectory, which can be computationally extensive. As described herein, estimator 22 implements filter 23 that iteratively updates predicted state estimates over a bounded-size sliding window of state estimates for poses of VINS 10 and positions of features 15 in real-time as new image data 14 and IMU data 18 are obtained. That is, by implementing the filtering approach, estimator 22 of VINS 10 marginalizes out past state estimates and measurements through the sliding window as VINS 10 traverses environment 2 for simultaneous localization and mapping (SLAM).


In one example implementation, filter 23 of estimator 22 recursively operates on the streams of image data 14 and IMU data 18 to compute a sliding window of predicted estimates for the state variables maintained within state vector 17 along with uncertainty data 19 representing the respective uncertainties in the form of one or more uncertainty matrices, which may take the form of covariance matrices for an extended Kalman filter (EKF). For example, at any time instant, the EKF state 17 vector comprises the evolving IMU state and a history of up to Nmax past poses of the camera state vector 17 and may take the form of:

x=[xI xIk+n−1 . . . xIk]

where xI denotes the current pose, and xIi, for I=k+n−1, . . . , k are the IMU poses in the sliding window, corresponding to the time instants of the last n camera measurements. The current robot pose may be defined as:

xI=[IqGT GvIT GpIT bαT bgT λd λr]T

where IqG is the quaternion representation of the orientation of {G} in the IMU's frame of reference {I}, GvI and GpI are the velocity and position of {I} in {G} respectively, while ba and bg correspond to gyroscope and accelerometer biases.


Estimator 22 may implement filter 23 such that uncertainty data 19 takes the form of a matrix that contains estimates of the uncertainty of each predicted state estimate in state vector 17 as well as a correlation between uncertainties. When a subsequent measurement is observed from either image data 14 or IMU data 18, filter 23 updates the sliding window of predicted state estimates with state vector 17 and the uncertainty data 19 as described herein so as to operate as an iterative Kalman smoother. In general, estimator 22 operates in real-time using the present input measurements of image data 14 and IMU data 18 and the previously calculated state estimates and its uncertainty matrix. In general, when new image data 14 or IMU data 18 is received, filter 23 projects the measurements as the data arrives onto the state estimates within state vector 17 to re-compute the predicted states and to update respective uncertainty data 19 for each state estimate. Any difference between the predicted state estimates as computed by estimator 22 and the actual feature measurements is referred to as a residual.


In some examples, estimator 22 iteratively processes measurements from image data 14 and IMU data 18 to update estimates only keyframes (key robot/device poses) and key landmarks while also exploiting information (e.g., visual observations and odometry measurements) available to the non-keyframes along the trajectory. In such example implementations, filter 23 projects new measurements onto the keyframes, by generating consistent pose (position and orientation) constraints between keyframes. As used herein, the term keyframes refers to the individual poses of the VINS 10 for which position and orientation of the VINS are to be estimated. In contrast, the term non-keyframes refers to intermediate poses between keyframes and for which, in some examples, complete state estimates of the VINS are not computed. In these example implementations, information from non-keyframes, acquired between keyframes, is not discarded. Instead, this information is projected on to estimates in the state vector associated with the keyframes, in order to generate tight constraints between the keyframes. For example, information from a non-keyframe may be projected onto a preceding keyframe to compute relative position and orientation constraints between the preceding keyframe and the non-keyframe. Further examples of such implementations are described in U.S. patent application Ser. 14/271,971, entitled “CONSTRAINED KEY FRAME LOCALIZATION AND MAPPING FOR VISION-AIDED INERTIAL NAVIGATION,” filed May 7, 2014, the entire contents of which are incorporated herein by reference.


Estimator 22 processes inertial and visual measurements to compute, based on the image data and the IMU data, state estimates for at least a position and orientation of VINS 10 for a plurality of poses of the VINS along the trajectory. That is, estimator 22 process image data 14 and IMU data 18 to update within state vector 17 estimates for the 3D IMU pose and velocity together with the time-varying IMU biases so as to determining the position and orientation of estimator 22 within the environment represented by map 21, where the map may be initially constructed using the cooperative mapping information described herein. Estimator 22 may, in accordance with the techniques described herein, apply estimation techniques that compute state estimates for 3D poses of IMU 16 at each of the first set of time instances associated with capture of the IMU data and 3D poses of image source 12 at each of the second set of time instances associated with capture of the image data along the trajectory.


In this example implementation, VINS 10 provides two sources of information: motion information (IMU data 18) from an IMU 14, and image data 14 (e.g., feature observations) from image source 12. Estimator 22 may classify the features observations into two main categories: simultaneous localization and mapping (SLAM) features for which estimates are included and updated within a complex system state vector 17 maintained by estimator 22, and multi-state constraint Kalman filter (MSCKF) features for which the estimator has determined to exclude corresponding estimates in the state vector but instead used the features to generate constraints that geometrically constrain the states for the poses of VINS 10 from which the MSCKF feature was observed. That is, rather than maintain state estimates for positions of each observed feature 15 within its internal state vector, the estimator may group the images per feature and elect to exclude state estimates for one or more of those features (i.e., MSCKF features) from its state vector that were observed from multiple poses along the trajectory. For these features excluded from the state vector, referred to as MSCKF features, estimator 22 computes geometric constraints that constrain state estimates for other poses within the sliding window state vector and that are used to compute state updates for those state estimates within the state vector. In this way, MSCKF features relate and constrain estimated poses within the sliding window. They require less computations than SLAM features since their feature states are not directly estimated. Further example details of an estimator that computes constraints for features 15 observed from multiple poses and utilizes constraints to compute the state estimates for VINS 10 while excluding the MSCKF features from the state vector are described in U.S. patent application Ser. No. 12/383,371, entitled “VISION-AIDED INERTIAL NAVIGATION,” the entire contents of which are incorporated herein by reference.


Cooperative mapping techniques are described herein for creating an accurate 3D map of an environment, including locations of features within the environment, for use with VINS navigation and localization, such as by VINS 10 of FIGS. 1 and 2. This disclosure describes techniques for enhanced, large-scale mapping of a 3D environment using multiple, cooperative mobile devices. In some cases, the cooperative mapping techniques described herein may be applied to process visual and inertial data collected from multiple users at different times. Moreover, the cooperative mapping techniques described herein may be applied event where transformation between the users' starting positions and orientations (poses) are not known.



FIG. 3 illustrates an environment including Non-common (stars) and common (triangles) point features, as well as line features observed by mobile devices 30. Consider multiple datasets consisting of visual and inertial measurements collected by several users with respective mobile devices 30 each having a camera and an IMU. We examine the most general case, where the relative transformations of the users are unknown, and no relative-pose measurements between them are provided. Furthermore, we assume that there exist enough (two or more) common point features between pairs of users to determine the transformation between all maps. Such a multi-user CM scenario is illustrated in FIG. 3.


Techniques described herein determine a BLS solution over all users' trajectories and maps. FIG. 4 is a flowchart illustrating an example operation of the technique:


1) Initially, a BLS solution for each individual user's trajectory and map is determined independently, using measurements from only their dataset (step 40). During this step, each mobile device captures measurement data, determines its respective trajectory and constructs its own local map.


2) Next, an initial estimate of the users' relative poses, using their observations of common point features (step 42). At this time, each mobile device 30 may communicate its locally determined trajectory and map to one or more cooperative mapping servers (CMS) 34. Based on this information, CMS 34 computes initial estimates for relative poses for mobile devices 30. CMS 34 may, for example, be any computing platform (system) having one or more programmable processors for executing software to implement the techniques described herein. One example computing platform is discussed in further detail below with respect to FIG. 15.


3) Finally, based on the initial estimates, CMS determines the optimal BLS solution of all users' trajectories and maps (i.e., a composite map of the environment) utilizing all available sensor data, and all, or a subset of, the constraints that arise from commonly-observed point and line features (step 44). The overall map may then be communicated to mobile device for VINS-assisted navigation through the environment.


Our formulation of the CM problem has three desirable characteristics: (i) Each user's dataset becomes a modular component of the final solution. Thus, if a user's dataset contains unreliable measurements, or we need to extend the map to a previously-unknown area, we can add or remove users to the CM problem without recomputing all BLS solutions or all initial relative-pose estimates; (ii) One example algorithm of the techniques of the disclosure can reuse the result of each individual BLS when generating the CM solution, leading to a substantial speedup. (iii) The example CM algorithm provides a convenient mechanism for trading estimation accuracy for computational-cost savings, by reducing the number of constraints imposed by commonly-observed features.


System State and Measurement Models

In this section, we first describe the system states, and then present the measurement models for processing IMU data and visual observations to point, free-line, and Manhattan-line features.


For the remainder of the paper, we denote the position and orientation of frame {F1} in frame {F2} as F2pF1 and F1F2C, respectively, where C is a 3×3 rotation matrix. We also define [e1, e2, e3]=I3, where I3 is the 3×3 identity matrix.


System State

Our CM system maintains the following state vector:

x=[1xuT . . . NxuT xτT]T

where jxu, j=1, . . . , N, denotes the state vector corresponding to each user, and xτ is the transformation between different users' poses. Furthermore, jxu is defined as:

jxu=[j{hacek over (p)}T jfT jqBT]T  (1)

where j{hacek over (p)}custom character[j{hacek over (p)}T . . . j{hacek over (p)}KT]T, j{hacek over (p)}k, k=1, . . . , K, represents the user's pose at time step k [see (2)], jf is the vector of all point, jxPi, free-line, jxLi, and Manhattan-line, jxVi, features, defined as









j


f

=


[





x

P
1

T




j













x

P

N
p


T




j






x

L
1

T




j













x

L

N
l


T




j






x

V
1

T




j













x

V

N
v


T




j





]

T






and jqB is the quaternion representation denoting the orientation of the Manhattan world. Defining the z axis of the Manhattan world frame to align with the gravity direction, jqB denotes a yaw rotation of angle jαB, i.e.,







q
B



j

=


[



0


0



sin


(


α
B



j

2

)





cos


(


α
B



j

2

)





]

T






Additionally, we assume the IMU and the camera are co-located to simplify the ensuing derivations. In our experiments, we estimate the IMU-camera extrinsic calibration parameters (6 DOF transformation) of each user concurrently with their trajectories and maps.


User Pose State and IMU Measurement Model

An IMU (i.e., gyroscope and accelerometer) provides the user's rotational velocity, ωm, and linear acceleration, am, contaminated by white Gaussian noise and time-varying biases. To model the IMU propagation process, we define the user pose at time step k as:

{hacek over (p)}k=[CkqGT bgkT GvCkT bakT GpCkT]T  (2)

where CkqG is the orientation of the global frame, {G}, in the IMU's frame of reference, {Ck}, GpCk and GvCk are the position and velocity of {Ck} in {G}, and bgk and bak are gyroscope and accelerometer biases, respectively.


The continuous-time IMU propagation model describing the time evolution of the user's pose state is:












q
.

G



C



(
t
)


=


1
2



Ω


(



ω
m



(
t
)


-


b
g



(
t
)


-


n
g



(
t
)



)





q
G



C



(
t
)







(
3
)









v
.

C



G



(
t
)


=




C


(


q
G



C



(
t
)


)


T



(



a
m



(
t
)


-


b
a



(
t
)


-


n
a



(
t
)



)


+



G


g
















p
.

C



G



(
t
)


=


v
C



G



(
t
)
















b
.

a



(
t
)


=

n
wa















b
.

g



(
t
)


=

n
wg














where Ω (ω) is defined as







[




-


ω





ω





-

ω
T




0



]







for ω∈custom character3, C(CqG(t)) denotes the rotation matrix corresponding to CqG(t), ng and na are white Gaussian measurement noise of ωm(t) and am(t), while Gg denotes the gravitational acceleration in {G}. Finally, nwa and nwg are zero-mean white Gaussian noise processes driving the gyroscope and accelerometer biases bg and ba.


The corresponding discrete-time system is determined by integrating (3) between time steps k and k+1. This process can be described by the following equation:

{hacek over (p)}k+1=g({hacek over (p)}k, uk)+wk  (4)

where g({hacek over (p)}k, uk) is a nonlinear function corresponding to the IMU measurement model, ukcustom charactermkT amkT]T, and wk is the IMU measurement noise, which is assumed to be zero mean, Gaussian with covariance Qk, computed through IMU characterization.


Point-Feature Measurement Model

By defining the position of a point feature with respect to the first camera pose, {Cj}, that observes it as CjxPi, the camera pose {Ck} measures the bearing angle to the feature as:

Ckz=π(CkpCj+CjCkCCjxPi)+nk  (5)

where







π


(


[



x


y


z



]

T

)




=
Δ





1
z



[



x


y



]


T






represents the camera perspective-projection model, nk is the measurement noise, CjCkC and CkpCj are expressed as:

CjCkC=CkGCCjGCT
CkpCj=CkGC(GpCjGpCk)  (6)


Free-Line Feature State and Measurement Model


FIG. 5 illustrates the parameterization and measurement of free lines (left) and Manhattan lines (right). In this work, we use 4 DOF free-line parameterization. Consider the line li in FIG. 5 which is first observed by camera pose {Cj}. We define a coordinate frame {Li} for this line whose origin, pLi, is the point on the line at minimum distance, dLi, from {Cj}, x-axis is aligned with the line's direction li, and z-axis points away from the origin of {Cj}. Then, the line is represented with respect to {Cj} by the parameter vector CjxLi=[CjqLiT dLi]T. Defining LiCjCcustom characterC(CjqLi), the origin of the line frame in the camera pose {Cj} can be written as CjpLi=dLi·LiCjCe3.


In the absence of noise, any line measurement sk in frame {Ck}, which is defined as a 2-DOF unit vector perpendicular to the plane passing through the line li and the origin of {Ck}, imposes two constraints on the line and the observing camera: sk is perpendicular to both the line direction and the displacement between the origins of {Ck} and {Li}, i.e.,

skTCiCkCLiCjCe1=0
skT(CiCkCCjpLi+CkpCj)=0  (7)

where CiCkC and CkpCj are expressed in terms of the camera poses {Cj} and {Ck} in (6). In the presence of noise, the measured normal vector is








s
k


=


C


(


s






k


,

n
2


)




C


(


s



k


,

n
1


)




s
k



;





where the rotational matrices







C


(


s



k


,

n
1


)







and






C


(


s






k


,

n
2


)







express the effect of the noise perturbing the true unit vector sk about two perpendicular axes,







s



k






and





s






k


,





by angles of magnitude n1 and n2, respectively.


Manhattan-Line Feature State and Measurement Model

Manhattan lines are aligned with one of the building's cardinal directions, and thus have only 2 DOF. Assuming a line aligns with the x-axis of the Manhattan world {B} (i. e., vi=e1), as in FIG. 5, the Manhattan line with respect to its first observing camera pose {Cj} is represented by the parameter vector CjxVi=[θVi dVi]T, where θVi is the angle between CjpVi and the y-axis of the Manhattan world (i.e., e2), and dVi is the distance between the origins of {Cj} and the Manhattan-line's frame {Vi}. Using this parameterization, CjpVi is expressed as:

CjpVi=dViCjGCBGC(cos θVie2+sin θVie3)  (8)

where BGC is the rotation matrix expressing the orientation of the Manhattan world frame {B} with respect to the global frame {G}. Similar to (7), the geometric constraints corresponding to Manhattan lines are:

skTCiCkCCjGCBGCe1=0
skT(CiCkCCjpVi+CkpCj)=0  (9)

where CiCkC and CkpCj are defined in (6).


Common-Feature Constraints

In one example problem formulation, if a feature is observed by multiple users, we first define it as a different feature in each user's map but then ensure geometric consistency by imposing constraints on the common features to be the same physical point or line. In what follows, we present the geometric constraints for common point, free-line, and Manhattan-line features.


Consider a point feature xPi observed by two users whose global frames of reference are {G1} and {G2}, respectively, and expressed as







x

P
i





C

j
1








and






x

P
i





C

j
2








with respect to the first observing camera poses in the two users' maps. The geometric constraint between them is:











x

P
i





C

j
1



-





C

j
2



C

j
1




C



x

P
i





C

j
2




-



p

C

j
2






C

j
1





=
0




(
10
)








where










C

j
2



C

j
1




C






and






p

C

j
2






C

j
1








are then expressed as:













C

j
2



C

j
1




C

=


C

G
2


G
1



G
1


C

j
1





C

G
2


C

j
2





C
T






(
11
)







p

C

j
2






C

j
1



=





G
1


C

j
1




C



(


p

G
2





G
1


-

p

C

j
1






G
1


+


C


G
2



G
2


G
1




p

C

j
2





)






(
12
)








FIG. 6 is a depiction of line constraints in accordance with the techniques of the disclosure. Consider a free-line feature l observed by two users, and expressed with respect to the first observing camera poses {Cj1} and {Cj2} in their maps, respectively. As evident from FIG. 6, the common free line is represented in the two maps using frames of different origins. For deriving the geometric constraint between two free lines, we employ the following relation between frames {Cj1}, {Li1}, {Cj2}, and {Li2}:










p

L

i
1






L

i
2



=


C

T


L

i
2



C

j
2





[



C

T


C

j
2



C

j
1





(


p

L

i
1






C

j
1



-

p

C

j
2






C

j
1




)


-

p

L

i
2






C

j
2




]






(
13
)








where







p

L

i
1






L

i
2



=


-

d
c





e
1

.







To remove dc from (13), we define E23custom character[e2 e3]T and multiply with it both sides of (13) to obtain the 2 DOF constraint:











E
23




C

T


L

i
2



C

j
2





(



C

T


C

j
2



C

j
1





(


p

L

i
1






C

j
1



-

p

C

j
2






C

j
1




)


-

p

L

i
2






C

j
2




)



=
0




(
14
)







Then, since the x-axes of frames {Li1} and {Li2} are both defined according to the same line direction, we have the additional 2 DOF constraint:











E
23



(


Ce
1



L

i
1



C

j
1



-





C

j
2



C

j
1




C



Ce
1



L

i
2



C

j
2





)


=
0




(
15
)







The common Manhattan-line features also satisfy (13), with the additional information that a Manhattan line is aligned with one of the building's cardinal directions. For example, if the line's direction is e1, we have:











C




V

i
2




L

i
2



C

j
2





p

V

i
1




=


-

d
c




C






e
1




B

C

j
2








(
16
)







Similar to (14), the 2-DOF translational common-Manhattan-line constraint can be written as:











E
23




C



T

B

C

j
2





(



C



T


C

j
2



C

j
1





(


p

V

i
1








C

j
1



-

p

C

j
2








C

j
1




)


-

p

V

i
2








C

j
2




)



=
0




(
17
)







Note also since the Manhattan lines align with the building's cardinal directions, the orientation constraint corresponding to (15) is automatically satisfied and need not be considered.


Example Algorithm Description

In what follows, we first briefly review the BLS method for determining the trajectory and map of each user based on only its own measurements, and then describe in detail an approach to find an initial estimate for the relative poses between users. Subsequently, we introduce our example CM algorithm and present a method for “sparsifying” (i.e., reducing their number and thus spatial density) the commonly-observed-feature constraints.


Single-User Batch Least-Squares

For a user j, computing the BLS estimate requires minimizing the following non-linear cost function:

custom characterj=∥j{hacek over (p)}g(j{hacek over (p)}, ju)∥jQ2+∥jz−h(jxu)∥jR2  (18)

where the first term corresponds to the cost function arising from IMU measurements (4), while the second term is due to visual observations of point (5), free-line (7), and Manhattan-line (9) features. Also, in (18) j{hacek over (p)} and jxu denote the user' poses and entire state [see (1)], respectively, ju includes all IMU measurements, jz comprises all visual observations, jQ and jR are the covariance matrices of the corresponding measurement noises.


The cost function (18) can be minimized by employing Gauss-Newton iterative minimization. In particular, by expressing the error states of jxu with δjxu, we have the linearized cost function:

custom characterj′=∥Jjδjxu−bj2  (19)

where Jj and bj are the Jacobian and residual, respectively. In each Gauss-Newton iteration, (19) is solved very efficiently by using the Cholmod algorithm. Specifically, defining Gj as the Cholesky factor of the Hessian, i.e., GjGjT=JjTJj, we minimize (19) with respect to δjxu as follows:

JjTJjδjxu=JjTbj⇔GjGjTδjxu=JjTbj
⇔Gjδjyu=JjTbj, with δjyu=GjTδjxu  (20)

which involves consecutively solving two triangular systems. Once δjxu is computed, it is used to update the estimates for jxu, and initiate a new Gauss-Newton iteration until convergence (∥δjxu∥<size(δjxu)×10−5).


Note the Gauss-Newton minimization described above can be performed by each user independently (in parallel or at different times) to compute an estimate of each user's state jxu. These estimates and the Choleskey factor, Gj, will be provided to the CM algorithm for merging all maps. Before computing the merged map, however, an initial estimate of the transformation between the users' reference frames is needed. This initialization process using visual observations of common, amongst the different users' maps, point features is described in the next section.


Initial Estimate of the Users' Relative Poses

In what follows, we first describe our algorithm for computing the transformation between two users, which can be used in a minimal solver in conjunction with RANSAC for outlier rejection and/or to find an approximate least-squares solution. Then, we explain our approach for computing the transformation between all users.


1) Transformation between pairs of users: When using visual and inertial measurements, the roll and pitch angles of each user's orientation are observable in the inertial frame of reference. Therefore, the transformation between any two users has four DOF: One corresponding to their relative yaw angle and three corresponding to their relative position. By defining the two users' frames of reference as {G1} and {G2}, we seek to estimate the position and orientation of {G2} with respect to {G1}, denoted as G1pG2 and G2G1C, respectively. Note that G2G1C corresponds to a rotation about the global z-axis, which is aligned with gravity, and thus equals:













G
2


G
1



C

=

[




cos





θ





-
sin






θ



0





sin





θ




cos





θ



0




0


0


1



]





(
21
)







When two users observe the same point feature xPj, j=1, . . . , M, the geometric constraint between them is:

G1xPj=G1pG2+G2G1CG2xPj  (22)

where G1xPj, G2xPj are the point feature xPj's 3D positions expressed in {G1} and {G2}, respectively.


Subtracting the constraint (22) corresponding to feature xP1 from the constraints (22) corresponding to xPj, j=2, . . . , M, results in:

G1xPjG1xP1=G2G1C(G2xPjG2xP1)  (23)

which can be rewritten as:












A
j



[




cos





θ






sin





θ




]




=
Δ





A
j


w

=



b

j
,







j

=
2



,





,
M




(
24
)








where Aj and bj are a 3×2 matrix and a 3×1 vector respectively, and both of them are functions of G1xPj, G1xP1, G2xPj, and G2xP1. By defining A=[A2T, . . . , AMT]T and b=[b2T, . . . , bMT]T, w can be obtained by solving the following minimization problem:

w*=argmin∥Aw−b∥2
s.t.∥w∥2=1  (25)


Note that (25) is a least-squares problem with a quadratic constraint, which can be solved by following the general methodology of [31]. Instead, in this work we provide a more efficient solution herein, which takes advantage of the fact that w is a 2×1 vector.


After solving for the yaw angle θ, we substitute G2G1C in (22) and obtain G1pG2 as:










p

G
2





G
1


=


1
M






j
=
1

M



(


x

P
j





G
1


-


C


G
2



G
2


G
1




x

P
j




)







(
26
)







Both G1pG2 in (26) and G2G1C corresponding to the w calculated in (25) will be used for initializing the example CM algorithm. Note also this transformation will be estimated by CM so as to improve the mapping accuracy.


The aforementioned process assumes that all matched point features are inliers and their 3D position estimates G1{circumflex over (x)}Pj, G2{circumflex over (x)}Pj, j=1, . . . , M, are accurate. In practice, we employ RANSAC to remove outliers. Specifically, the solution of (25) and (26) for M=2 are used as the minimal solver for RANSAC.


2) Initial transformation between multiple users: A naive approach to obtain the relative pose between any two users is to compute the relative transformation between all possible pairs. Instead, we seek to compute the initial transformation between any two users indirectly by employing a chain of pairwise transformations. To do so, we select the “best” pairwise transformations (in the sense that they are computed using the maximum number of common features) that form a chain connecting all users. In order to solve this problem, we first construct a graph whose vertices correspond to each user and edges are assigned weights proportional to the number of commonly-observed features between the corresponding users, and then compute the minimum spanning tree (MST).



FIGS. 7A-7D are a series of diagrams illustrating the process for finding a minimum spanning tree (marked in bold) on the user graph. The numbers of commonly-observed features between pairs of users are written on the edges in FIG. 7. In this way, FIG. 7 illustrates an example of finding the chain connection between users, where the transformations between fives users are obtained by combining the transformation pairs (5, 4), (4, 1), (1, 2), (2, 3). Finally, we estimate the transformation between user pairs corresponding to the edges in the resulting MST.


Cooperative Mapping Constructing the Overall Map

In this section, we first present the standard BLS formulation of the CM problem and briefly discuss its drawbacks. Subsequently, we reformulate CM as a constrained optimization problem, and show the equivalence of its solution to that of the BLS formulation. The solution of the CM formulation is then described, and then its advantages as compared to the BLS formulation are described.


1) BLS Formulation: As previously mentioned, linking the different users' maps requires using observations of common features. The standard BLS formulation when applied to the CM problem achieves this by modifying the camera measurement model for all three types of features to explicitly consider the transformation between the reference frames of the users observing the same features. Specifically, for a feature first observed by user {G1} at camera pose {Cj1}, its observation from user {G2} at camera pose {Ck2} can be still expressed with original point (5), free-line (7), and Manhattan-line (9) measurement models, but










C

j
1



C

k
2




C






and






p

C

j
1








C

k
2








need to be redefined including the transformation between the two users as:














C

j
1



C

k
2




C

=





G
1


C

k
2




C






G
2


G
1



C



C

T


G
2


C

j
1












p

C

j
1






C

k
2



=





G
1


C

k
2




C



(


p

G
2





G
1


-

p

C

k
2






G
1


+





G
2


G
1



C



p

C

j
1






G
2




)







(
27
)







This new camera model, for all common observations, can be written in a compact form as:

zc=s(xafcxτ)+nc  (28)

where xτ is a vector of size 4(N−1) comprising the pairwise transformations between users computed as described in Section V-B, xacustom character[1{hacek over (p)}T . . . 1{hacek over (p)}T]T is the vector comprising all users' poses, fc is the set of common features observed by two or more users, and nc is the corresponding measurement noise of covariance Rc.


Following the standard BLS formulation, we can construct the cost function for the CM problem by summing the following terms: (i) custom characteri, the cost function of each individual user after removing all cost terms involving common features [see (18)]. (ii) The cost terms arising from each user's observations to common features [see (28)]. Thus, our objective function becomes:

xτ*, xa*, fa*, fc*=argmin (Σi=1Ncustom characteri+∥zc−s(xafcxτ)∥Rc2)  (29)

where fa are the features observed by only one user. The Hessian of problem (29) has very clear structure: Different users' states are uncorrelated from Σi=1Ncustom characteri, comprising of all IMU and most of camera measurements, and their correlations all come from a small number of common-feature observations, ∥zc−s(xafcxτ)∥Rc2.


A standard BLS solution can be obtained following the same procedure as (20), by applying Cholesky factorization on the Hessian matrix arising from measurements corresponding to all users. This strategy treats the problem as one user collecting a large dataset, ignoring the fact there exist few correlations between different users' states. In the next section, we will present an example algorithm of the techniques of the disclosure that constructs a Hessian matrix corresponding to each user, and enforces their correlations through common-feature constraints. As shown later, this design will result in a solution that is parallelizable, efficient, and modular.


2) CM Formulation: To introduce the proposed CM formulation, we employ the following theorem:


Theorem 1: The optimization problem (29) is equivalent to the following constrained optimization problem:











x
τ
*

,

x
a
*

,

f
a
*

,

f

c
1

*

,





,


f

c
N

*

=

arg





min





i
=
1

N



i













s
.
t
.





𝒦


(


x
a

,

x
τ

,

f

c
i


,

f

c
j



)



=
0

,




i
,

j
=
1

,





,
N
,

i

j






(
30
)








where custom characteri denotes the cost function for user i [see (18)], fci is defined as the subset of fc observed by user i, and custom character (xa, xτ, fci, fcj) denotes common-feature constraints as defined in (10), (14), (15), and (17). Note that both fci and fcj are optimization variables here.


Proof: We will first prove the theorem for a simple case when a point feature is observed by two users. Then, we can easily extend this case to multiple users and line features.


Consider a point feature,







x

P
i







C

j
1



,





defined with respect to the camera pose {Cj1} of user {G1}, and also observed by user {G2} at camera pose {Ck2}. The BLS formulation of this problem is written as:










x
τ
*

,

x
a
*

,







f
a
*



x

P
i

*



C

j
1




=





arg






min


(





i
=
1

N



i


+








C

k
2




z

-

π


(


p

C

j
1






C

k
2



+





C

j
1



C

k
2




C



x

P
i





C

j
1





)







R
i



)










(
31
)







On the other hand, the proposed CM formulation is:










x
τ
*

,

x
a
*

,



f
a
*



x

P
i

*





C

j
1







x

P
i

*





C

j
2





=

argmin





i
=
1

N



i








(
32
)







s
.
t
.





x

P
i







C

j
2




=






C

j
1



C

j
2




C



x

P
i







C

j
1




+

p

C

j
1








C

j
2








(
33
)








where the feature is redefined as






x

P
i







C

j
2







with respect to a camera pose {Cj2} of user {G2}. Note the regular camera measurement model [see (5)] is employed to express the feature's observation from user {G2}, and the corresponding cost term is included in custom character2 as:
















C

k
2




z

-

π


(


p

C

j
2






C

k
2



+





C

j
2



C

k
2




C



x

P
i





C

j
2





)







R
i





(
34
)







Substituting constraint (33) into cost term (34) results in:

















C

k
2




z

-

π


(


p

C

j
2






C

k
2



+





C

j
2



C

k
2




C



x

P
i





C

j
2





)







R
i











C

k
2




z

-

π


(


p

C

j
2






C

k
2



+





C

j
2



C

k
2




C



(






C

j
1



C

j
2




C



x

P
i





C

j
1




+

p

C

j
1






C

j
2




)



)







R
i











C

k
2




z

-

π


(


p

C

j
1






C

k
2



+





C

j
1



C

k
2




C



x

P
i





C

j
1





)







R
i






(
35
)








which is the same as the corresponding cost term in the BLS formulation (31). Therefore, the CM formulation (30) and BLS formulation (29) are equivalent in this case.


When multiple features commonly observed by multiple users, a constraint is added in the CM formulation for each common feature observation, and the equivalence between BLS and CM formulations is established by substituting the constraints in the cost function. By replacing the corresponding measurement model and common-feature constraint, this proof can also be extended to common free-line and Manhattan-line features straightforwardly.


Note the Hessian matrix corresponding to the cost function in the CM formulation (30) has a block-diagonal structure, where each block comprises the state of a user. Solving CM as a constrained optimization problem has several advantages.


3) CM Solution: Since problem (30) is nonlinear, we solve it employing Gauss-Newton iterative minimization [33]. At each iteration, we focus on the following (linearized) constrained optimization problem:











δ






x
τ
*


,


δ
1



x
u
*


,





,



δ
N



x
u
*


=

arg





min





i
=
1

N








J
i



δ
i



x
u


-

b
i




2













s
.
t
.








i
=
1

N




A
i



δ
i



x
u




+


A
τ


δ






x
τ



=
r





(
36
)








where δixu and δxτ are the error states of user state, ixu, and transformations between users, xτ, respectively, while Ji and bi are the corresponding Jacobian and residual of custom character2 in (30). Ai, Aτ, and r are the Jacobians (corresponding to ixu and xτ) and residual of the constraints.


The KKT optimality conditions [34] for (36) are:

JiT(Jiδixu−bi)+AiTλ=0, i=1, . . . , N
Σi=1NAiδixu+Aτδxτ=r=0
AiTλ=0  (37)

where λ is the Lagrange-multiplier vector.


To simplify notation, in what follows we present an example algorithm for solving (37) for the case of two users, while its extension to three or more users is straightforward.


Writing (37) in a compact form yields:













[





J
1
T



J
1










A
1
T

















J
2
T



J
2





A
2
T









A
τ







A
1




A
2


























A
τ
T









]









H
CM





[




x
u



1






x
u



2





λ





δ






x
τ





]


=

[





J
1
T



b
1








J
2
T



b
2






r




0



]





(
38
)







Note that due to the zeros in the (3, 3) and (4, 4) block-diagonal elements, HCM is not positive definite. Thus, Cholesky factorization cannot be applied. Although other methods, such as diagonal pivoting [35], can be employed to solve (38), we propose an alternative approach that takes advantage of HCM's structure and the Cholesky factors previously computed by each user based on the following theorem:


Theorem 2: HCM can be factorized into the product of a lower-triangular matrix and an upper-triangular matrix as:










H
CM

=


[




G
1


























G
2
















K
1
T




K
2
T




T
11





















T
21




T
22




]



[




G
1
T









K
1
















G
2
T




K
2





















-

T
11
T





T
21
T





















-

T
22
T





]






(
39
)








where G1 and G2 are the Cholesky factors of the users' Hessian matrices J1TJ1 and J2TJ2 respectively, and T11 and T22 are lower-triangular matrices.


Proof: Multiplying the two triangular matrices in (39), and employing the structure of HCM in (38) yields the following system of equations:

GiKi=AiT, i=1,2  (40)
T11T11Ti=12KiTKi  (41)
T11T21T=Aτ  (42)
T22T22T=T21T21T  (43)


To find matrices K1, K1, T11, T21 and T22 that satisfy (40)-(43), we first compute Ki, i=1,2, by solving a linear equation corresponding to each of the columns of Ki [see (40)].


Defining K=[K1TK2T]T, it is easy to see that










j
=
1

2




K
j
T



K
j



=



K
T


K

=



[




A
1




A
2




]



[





(


G
1



G
1
T


)


-
1




0




0




(


G
2



G
2
T


)


-
1





]




[




A
1
T






A
2
T




]








is a positive definite matrix because [A1 A2] has full row rank (i.e., each common feature constraint appears only once). Thus, we compute T11 as the Cholesky factor of KTK that satisfies (41). Given T11, we determine T21 using triangular back-substitution according to (42).


Lastly, T21T21T is also positive definite, and thus T22 is selected as the Cholesky factor of T21T21T [see (43)].


Once all the block matrices in (39) are obtained, (38) is efficiently solved by employing two back-substitutions involving triangular matrices.


Now, we briefly discuss the computational complexity of computing each block in (39). The Cholesky factors Gi do not require any calculation in the first Gauss-Newton iteration, because they have already been computed by each user. Starting from the second Gauss-Newton iteration, the Gi matrices need to be re-computed, which can be done in parallel, at a cost that depends on the structure of the Hessian.


Computing the Ki matrices involves triangular back substitution according to (40), which has low computational cost for two reasons: (i) the Ai matrices are very sparse (less than 0:01% nonzero elements); (ii) each column of the Ki matrices can be computed in parallel. Note also that since the number of columns of K is equal to the number of commonly-observed feature constraints, the time for computing K grows linearly with the number of constraints.


Defining each row in K as kj, KTK can be computed as ΣkjTkj, where all the terms in the summation can be calculated in parallel. Since the size of kj equals to the dimension of the commonly-observed feature constraints, the overall computational complexity increases quadratically with the number of constraints.


The T11 matrix is the Cholesky factor of KTK. Although K is sparse (about 1% nonzero elements), since it is typically a tall matrix, KTK is generally a small dense square matrix with size equal to the number of commonly-observed feature constraints. Thus, computing T11 has cubic processing cost with respect to the number of constraints.


Lastly, both T21 and T22 are very small matrices, and thus take very little time to compute. Once all the block matrices are computed, solving the linear system requires only two sparse back triangular substitutions.


As we will show in the experimental results, the most computationally demanding parts of the example CM algorithm is either computing Gi or KTK depending on the number of commonly-observed feature constraints. Fortunately, both these operations are parallelizable.


CM Solution Advantages: Formulating and solving CM as a constrained optimization problem has the following advantages:


Parallelization: In (29), due to the features observed by multiple users, many of the off-diagonal blocks in the resulting Hessian matrix are nonzero. Thus, there is no straightforward way to parallelize computations. In contrast, and as described above, most operations required for solving (30) are parallelizable (e.g., computing the Ki and Gi matrices). This is of particular importance when mapping very large areas such as airports, museums, shopping malls, etc.


Efficiency: The BLS formulation (29) requires to apply Cholesky factorization on the Hessian created from all the users' data, while the proposed formulation (30) applies Cholesky factorization on the smaller-size Hessians created from each user's data. Since the memory requirements of Cholesky factorization directly relates to the problem size, solving (29) is much more memory demanding. In terms of processing, the Cholesky factor of each individual user's Hessian matrix can be reused in an example algorithm of the proposed technical solutions of the disclosure to save computational cost.


Modularity: In (29), the feature measurement model changes if a common feature is already defined in another map, in which case the transformation between the maps needs to be involved. In contrast, in (30) common features always use the same measurement model as the rest of features which does not involve the transformation between maps, thus the feature measurement model is uniform. Moreover, adding or removing users' trajectories and maps does not affect the Jacobian matrices of the other users. Instead, we simply add the corresponding constraints. This is especially convenient when expanding the map or updating pre-existing maps.


Covariance of CM estimates: For a standard BLS solution, we can find the uncertainty of the estimate, i.e., covariance matrix, by inverting the system's Hessian matrix. Since we formulate CM as a constrained optimization problem, the estimate's covariance cannot be computed following the same procedure.


Commonly-Observed Feature Sparsification: Resource-Aware CM

As analyzed in the end of Section V-C3, the computational cost of the example CM algorithm increases quadratically to cubically with the number of commonly-observed features. Therefore, the example CM algorithm is ideal for scenarios where there are a small number of inter-dataset loop-closures. On the other hand, if users view a large number of common features, the example CM algorithm will become expensive. Such a scenario motivates us to reduce the number of common features by selecting an informative subset of common feature constraints to include in the CM formulation. Specifically, retaining only a subset of common features in (30), expressed as fc1, . . . , fcN, yields:











x
τ
*

,

x
a
*

,

f
a
*

,

f

c
1

*

,





,


f

c
N

*

=

arg





min









i
=
1

N



i













s
.
t
.





𝒦


(


x
a

,

x
τ

,


f
_


c
1


,


f
_


c
j



)



=
0

,




i
,

j
=
1

,





,
N
,

i

j






(
44
)







By doing so we do not drop or change any feature measurement. Instead, we make an approximation that a feature common in two or more maps corresponds to different physical features in each user's map. Additionally, since no spurious information is gained through this approximation, this feature sparsification method is consistent.


The optimal solution to the problem of selecting the M most informative (in terms of expected CM accuracy) out of N commonly-observed features has computational cost significantly higher than computing the CM using all available features. For this reason, we introduce a heuristic method to efficiently select commonly-observed features which are (i) evenly distributed across the physical space; and (ii) accurately estimated in each map, so as to improve rigidity and accuracy of the resulting merged map.


The first requirement is satisfied by evenly partitioning the physical space into different regions using a fixed-size grid. Since the resulting map comprises of 3D point and line features, and in order to improve efficiency, we first partition the map into 3D levels, and then project all features belonging to a certain level on the corresponding x-y plane for that level. The process for splitting the 3D map into separate levels, based on the histogram of the features' density when projected on the z-axis, is automated. Typically, large feature concentrations appear on the ceiling and floor of each level with several meters' gaps between them, which makes the level-separation problem fairly easy to solve. Subsequently, we “sparsify” the corresponding map of commonly-observed features by selecting two point features per cell, which is the minimum number of features required for determining the transformation between two maps. These two features are selected according to the second requirement. Specifically, we pick two common features with most number of observations, because the accuracy of feature estimate increases with growing number of camera measurements.



FIGS. 14A-14D illustrate example common point features before and after sparsification using grids of different sizes, in accordance with the techniques of the disclosure. In particular, FIG. 14 illustrates common point features: (a) Original CM, sparse CM using grids of size (b) 1×1 m2, (c) 4×4 m2, (d) 8×8 m2. This common feature sparsification method is shown to be effective in our experimental results. For example, in a building-sized dataset, after reducing the number of common feature from more than four thousand to about two hundred, the root-mean-square difference of estimated user trajectory is only 13.5 cm.


Experiment Results

In this section, we first describe our experiment setup, followed by introducing the point and line feature tracking methods within a single and across multiple user maps. Then, we provide experimental results demonstrating the accuracy improvement by employing line features in addition to point features. Finally, we show that employing the feature-sparsification of the CM results in significant processing and memory savings with only limited accuracy loss.


Dataset Collection and Algorithm Implementation: The visual and inertial measurements used in our CM tests were collected using a Project Tango developer phone and tablet. Greyscale images, with resolution 640×480, were saved at 15 Hz, along with consumer-grade, MEMS-based, IMU data at 100 Hz. To evaluate the example algorithm, four datasets were acquired while navigating through a large building, the Keller Hall at the University of Minnesota, over a period of three days.



FIGS. 9A-9D depict cooperative mapping (CM) estimated trajectories, in accordance with the techniques of the disclosure. FIGS. 10A-10C depict a merged map of the CM estimated trajectories of FIGS. 9A-9D, in accordance with the techniques of the disclosure. Each of the CM estimated trajectories of the four datasets are shown separately in FIG. 9. FIG. 10 illustrates merged trajectories of all users from (a) 3D, (b) y-x, and (c) x-z views.


Datasets of FIG. 9A-9D correspond to trajectories of approximately 1300, 1000, 800, and 500 m, from which 181,140 points, 945 free lines, and 4,511 Manhattan lines are processed in CM. Of these features, 4,243 points, 18 free lines, and 148 Manhattan lines are common to two or more datasets



FIGS. 8A-8C depict an estimated feature 3D point cloud, in accordance with the techniques of the disclosure. That is, FIG. 8A-8B depict a 3D point cloud from two different views. FIG. 8C depicts 3D Manhattan-line features following x, y, z directions.


Note in order to test the performance of the example CM algorithm with different number of commonly-observed features selected by the proposed feature-sparsification method, we collect these four datasets with significant trajectory overlaps in purpose. For datasets containing only a few hundred of common features, the feature-sparsification method does not have to be applied.


All the reported timing results are obtained by running the example CM algorithm on a desktop computer with an Intel® Xeon® E5-1650 v2 Processor (3.5 GHz). The parallelization is implemented using Intel's threading building blocks (TBB) library. All the matrix operations are performed utilizing the Eigen library [38], and the Cholesky factors, matrices Gi, are computed employing the Cholmod algorithm from the SuiteSparse library, which is also used in Google's standard non-linear least-squares solver, Ceres. Based on our tests, Cholmod is faster than other existing Cholesky factorization algorithms, such as the Simplicial Cholesky algorithm in Eigen and CSparse also from SuiteSparse. Cholmod is a sequential algorithm, but it is able to utilize a basic linear algebra subprograms (BLAS) library to process low-level matrix operations (e.g., matrix multiplication) in parallel. We tested the speed of three most widely used BLAS libraries: OpenBLAS (developed from GotoBLAS2), EigenBLAS (BLAS supported by the Eigen library), and Intel MKLBLAS, and MKLBLAS turns out to be the fastest one. Therefore, we utilize MKLBLAS for reporting the best timing results.


Data Preparation and Initial Estimate: In the example implementations of the techniques described herein, each user device first solves its own single-map (SM) estimation problem via BLS to determine its local trajectory and map. Each user device may utilize the following information:


(SM 1) An initial estimate for its trajectory and map: In our case, this is computed using a variant of the multi-state constrained Kalman filter (MSC-KF). Each MSC-KF operates on the IMU measurements and feature tracks collected by each user. Feature tracks correspond to Harris-corners extracted from each image and tracked using the Kanade-Lucas-Tomasi (KLT) algorithm. The subset of these feature tracks that pass the 2pt-RANSAC, are used by the MSC-KF to improve the estimated trajectory of each user. These tracks, however, are not used to detect loop closures.


(SM 2) Point Loop-closure detection (intra-dataset): To determine if a user has revisited an area, we follow a bag-of-words approach using ORB feature descriptors and employ our implementation of Nistér's vocabulary tree. These matches are confirmed after they pass a 3pt+1-RANSAC geometric-consistency test.


(SM 3) Line tracking: Line segments are extracted from images using a Line Segment Detection (LSD) algorithm. The line tracking process first creates hypotheses for each 3D line's orientation and position by considering all possible line vector pairs (1si, 1sj) between the first and the second image. Then, line segments from the third image are used to determine valid hypotheses. Line segment triplets that satisfy the three-image constraints are then considered as a line track, and are used to estimate a 3D line's parameters. Lastly, the line segments from image 1 that were not assigned to any 3D lines are discarded, while the unassigned line segments from images 2 and 3 are used to create new hypotheses that are validated using the segments from image 4. Note that the line segments of image 4 are first tested against the current set of tracks, and the remaining segments are used for hypothesis testing. This process is then repeated as new images are considered. The example algorithm below presents the details of one example of the line tracking procedure.












Algorithm 1: Line tracking procedure. Each line track


consists of a set of line normals viewed in different poses.



















Input:  custom characteri, i = 1,...,K the set of line segment normals




  viewed by each pose i, the camera poses {hacek over (p)}




Output: The set of line tracks  custom character



1
custom character   = { }; // Set of line tracks



2
for i = 3:K do




 // Check in current line tracks



3
 foreach l ∈  custom character  , s ∈  custom characteri do



4
  if ReprojectionLineTest(l, s, {hacek over (p)}) then



5
   l ← l ∪ {s};



6
    custom characteri ←  custom characteri − {s};



7
  end



8
 end




 // Hypotheses generation



9
  custom character   = { }; // Set of line hypotheses



10
 foreach 1s ∈  custom characteri−1, 2s ∈  custom characteri−2 do



11
   custom character   ←  custom character   ∪ {1s, 2s};



12
 end




 // Hypotheses validation



13
 foreach l ∈  custom character  , s ∈  custom characteri do



14
  if ReprojectionLineTest(l, s, {hacek over (p)}) then



15
   l ← l ∪ {s};



16
    custom characteri ←  custom characteri − {s};



17
     custom character   ←  custom character   ∪ {l};



18
  end



19
 end



20
end









A second example line tracking and intra-dataset loop closure detection process (SM 3) is shown below.


















Input  custom character  k = l,...,K the set of images viewed by




  each pose k of the user j; the camera poses j{hacek over (p)} of




  the user j




Output: The set of line tracks  custom character



1
custom character   ← { }; // Initially empty set of 3D




 lines




// Extract line normals of images



2
for k = 1 : K do



3
  custom character   ← LSD_Extract( custom character  )



4
end



5
for k = 3 : K do




 // Test current line measurements




  against previous hypotheses



6
 foreach h ∈  custom character  , s ∈  custom character   do



7
  if ReprojectionLineTest(h, s, j{hacek over (p)}) then



8
   Meas(h) ← Meas(h) ∪{(k,s)}



9
   Param(h) ← TriangulateLine(Meas(h), j{hacek over (p)})



10
    custom character   ←  custom character   − {s}



11
  end



12
 end




 // New hypotheses generation



13
custom character  ← { }; // Potential line hypotheses



14
 foreach sk−1 ∈  custom character−1, sk−2 ∈  custom character−2 do



15
  h ← Ø



16
  Meas(h) ← {(k − 2,sk−2),(k − 1,sk−1)}



17
  Param(h) ← TriangulateLine(Meas(h), j{hacek over (p)})



18
   custom character   ←  custom character   ∪ {h}



19
 end




 // Test current line measurements




  against new hypotheses



20
 foreach h ∈  custom character  , s ∈  custom character   do



21
  if ReprojectionLineTest(h, s, j{hacek over (p)}) then



22
   Meas(h) ← Meas(h) ∪{(k,s)}



23
   Param(h) ← TriangulateLine(Meas(h), j{hacek over (p)})



24
    custom character   ←  custom character   − {s}



25
    custom character   ←  custom character   ∪ {h}



26
  end



27
 end



28
end










First, from each image custom characterk; k=1; : : : ; K, line segments are extracted using a line segment detection (LSD) algorithm, and the corresponding line normals are assigned to the set custom characterk (see lines 2-4 of the above example algorithm). Then, for k≥3, each line normal skcustom characterk is tested against the current (accepted as valid) 3D line hypotheses custom character (initially an empty set) by computing the re-projection errors. Note that each hypothesis comprises the triangulated line parameters and the corresponding set of pairs of image indices and line-normal measurements. The line normals which pass the test are added to the list of their accepting hypothesis and are removed from custom characterk to avoid re-processing (see lines 6-12 of the above example algorithm).


Afterwards, as shown in lines 13-19 of Algorithm 1, the line-tracking process creates a new set custom character of candidate 3D lines by assuming each possible pair of line normals sk−2custom characterk−2 and sk−1custom characterk−1 corresponds to measurements of a single 3D line observed at poses k−2 and k−1, respectively.


Subsequently, each hypothesis h∈custom character is validated by computing the re-projection error of each line normal skcustom characterk. If this is smaller than a threshold (currently set to 0.01), h is updated with sk and is added to custom character. These steps are shown in lines 20-27 of the example algorithm.


Once the line-tracking process is completed, the subset of line tracks corresponding to the cardinal directions of the building (environment) may be identified using a RANSAC-based vanishing point estimator. Lastly, we use the trajectory and line estimates of each user's BLS to find loop-closure line features by accepting free or Manhattan lines at poses where loop-closure point features have previously been found. In particular, if any of these free or Manhattan lines are close to each other (i.e., the difference of their distance and direction parameters are within one degree and 15 cm, respectively), they are accepted as loop-closure measurements.


Once each user has solved its own SM problem, they communicate to the CM their estimated trajectories, maps, Cholesky factors, and all available visual-inertial measurements. At this point, another step of preprocessing is required to compute the following quantities:


(CM 1) Inter-dataset point feature loop-closure detection: To achieve this, we follow the same procedure as in (SM 2), and determine the matched images and corresponding common landmarks across all datasets.


(CM 2) Inter-dataset line feature loop closure detection: We follow a process similar to (SM 3) using the resulting CM trajectory.


(CM 3) Relative transformation initialization: Once common point features are identified, we compute an initial estimate for the unknown 4 DOF transformation between user pairs.


Once the above pre-processing steps are complete, the CM techniques described herein are employed to estimate the trajectories of all users as well as the combined map. The results from our experiments are summarized in the following section.


Evaluation Results for using Line Features in addition to Point Features: The achieved accuracy of the example CM algorithm can be qualitatively assessed by observing the CM estimated trajectories of all users for the Keller Hall datasets shown in FIG. 10. Note that we intentionally instructed the users collecting data to keep the camera at about the same height, and walk in the middle of fairly narrow corridors. Correspondingly, the z (height) estimated for all users' trajectories remains about the same in the x-z view of the trajectory estimates, FIG. 10 (c), despite the fact that they have travelled for hundreds of meters across multiple floors. Moreover, in the x-y view, i.e., FIG. 10 (b), the user trajectories on different floors overlap almost exactly.


In addition to the qualitative results, we also present a ground-truth comparison. Specifically, we placed four April-Tags in the far corners of a single floor within the building and used the building's blueprints to find the true distance (ground truth) between any pair of AprilTags. Then, to compute the estimated distance between AprilTags, we employ a PnP algorithm to find an observed AprilTag's position expressed with respect to the camera's frame, and use the CM estimate of the camera frame to express the AprilTag with respect to the global frame. Specifically, defining the PnP estimated position of an AprilTag {Ai}, in the reference of the observing camera frame {Ck}, as CkpAi, this AprilTag's position in the global frame of reference is expressed as:

GpAi=CkGCCkpAi+GpCk  (45A)

Alternatively, to compute the estimated distance between AprilTags, we employ a PnP algorithm to find an observed camera's pose, (AnpCk, CkAnC), with respect to each AprilTag's frame, and then use the CM estimate of the camera's global pose, (GpCk, CkGC), to express each AprilTag with respect to the global frame {G} as:

GpAn=GpCkCkGCCkAnCTAnpCk  (45B)


Then, we average the estimated positions of each AprilTag across all camera observations and compute the estimated pairwise distances between AprilTags. For example, the estimated distance between two AprilTags, {Ai} and {Aj}, are computed as ∥GpAiGpAj∥. Lastly, we report the algorithm accuracy by comparing the difference between the estimated and true distances between all AprilTags. Note the distance from the camera to the AprilTag is relatively small as compared to the distance between AprilTags (about 0.4 m compared to over 80 m) so any error in the PnP estimate will negligibly affect the result.



FIG. 11 illustrates trajectories of all users in Keller Hall using points-only versus points, free lines, and Manhattan lines, in accordance with the techniques of the disclosure. The effect of using free and Manhattan lines can be observed in FIG. 11, where the absolute and relative errors in the pairwise distance between AprilTags found from this method when using only points versus when using points, free-lines, and Manhattan-lines are 63 cm (0.84%) and 48 cm (0.64%), respectively. In addition, at the position of the trajectory shown at bottom-left of FIG. 11, no loop-closure occurs, and when only points are used the estimated user trajectory is about 1.7 m off, while this error is corrected by processing line features. Moreover, the trajectory estimated when employing only point features has a small, but noticeable yaw error. This error, however, is corrected when Manhattan lines are also used since they provide attitude information between the user's frame and the building frame, which makes yaw observable.


Evaluation Results for Sparse CM: We compare the following four approximate algorithms with the CM exact solution: (i) The initialization of the example CM algorithm: Align the BLS estimated single-user maps using the inter-dataset transformation computed by the method described in Section V-B; (ii)-(iv) Sparse CM, i.e., CM with commonly-observed feature sparsification as described in Section V-D, using grids of size 8×8 m2, 4×4 m2, and 1×1 m2, respectively. The common point features before and after sparsification are shown in FIG. 14.


The resulting number of common point features, dimension of total common point, free-line and Manhattan-line feature constraint, and the position RMSE (with respect to CM estimates) of (i)-(iv) and CM are shown in Table I.









TABLE I







Number of common point features, the dimension of common point,


free-line, and Manhattan-line feature constraint, and position


RMSE of sparse CM compared to exact CM










Algorithms
Feature count
Constraint dimension
Position RMSE (cm)













Initialization
0
0
258.3


8 m grid
223
1050
13.5


4 m grid
488
2063
8.8


1 m grid
1,507
5877
5.7


Exact
4,243
16912
0









Additionally, we present the position difference of sparse CM (ii)-(iv) with respect to CM over all user trajectories in FIG. 12. FIG. 12 is a graph illustrating position difference of sparse CM over the users' trajectories with respect to CM, in accordance with the techniques of the disclosure. Note that since the position error of the CM initialization algorithm is much bigger, it is not plotted to keep the difference between the rest of algorithms visible. According to FIG. 12, after the common-feature sparsification process, the position difference is at maximum 0.82 m, and typically below 0.2 m for all sparse CM algorithms considered. To visually evaluate how much this level of position RMSE affects the user trajectories, we depict the estimated trajectory from the sparse CM using grids of size 8×8 m2 versus CM in FIG. 13. FIG. 13 illustrates trajectories estimated from original CM and sparse CM with common-feature sparsification using a grid size of 8×8 m2, in accordance with the techniques of the disclosure. Notice that the sparse CM preserves about 5% of the common-feature constraints, while obtains an estimate that has very small position difference from the exact CM estimate. The intuition behind this result is that since only two point features are required to compute the transformation between two maps, a few hundred (instead of several thousand) common-feature constraints are adequate to create an accurate merged map.


The timing results of the aforementioned example algorithms are reported in Table II.









TABLE II







Timing comparison between CM and BLS with different number of


commonly-observed features (in seconds)











Algorithms
8 m grid
4 m grid
1 m grid
Exact














Gi
13.5
13.5
13.5
13.5


Ki
4.8
9.7
29.3
85.7


KTK
1.2
4.5
36.4
334.2


T11
0.04
0.2
5.0
114.5


CM total
19.5
27.9
84.2
547.9


BLS
32.9
34.4
37.4
41.6










The timings for CM initialization is not listed because no computation is required for merging maps from different users. In general, all the example algorithms converge after 5-10 Gauss-Newton iterations. For comparison, we also report the timing results for solving the equivalent BLS formulation [see (29)] using the Cholmod Cholesky-factorization algorithm. As shown in the table, the sparse CM takes significantly less time compared to the exact CM solution. Moreover, when the number of common features are in the level of hundreds (as shown in Table I, 233 for 8 m grid and 488 for 4 m grid), the proposed CM solver is much faster than the equivalent BLS solver. Furthermore, since the Cholesky factors, Gi matrices, can be reused from each single-user BLS in the first iteration, additional time can be saved in the proposed CM solution. Lastly, note the time for computing matrices Gi, Ki, KTK, and T11 grows constantly, linearly, quadratically, and cubically with increasing dimension of the common-feature constraint (as shown in Table I). This result verifies our computational complexity analysis, which indicates the computational cost of the CM solution can be controlled by adjusting the number of commonly-observed features.


We also compared the peak memory usage between the proposed CM solver and the equivalent BLS solver in Table III.









TABLE III







Peak memory usage comparison between the example CM algorithm


and the BLS formulation (in GB)











Algorithms
8 m grid
4 m grid
1 m grid
Exact





CM algorithm
1.37
1.37
1.37
1.37


Equivalent BLS
4.41
4.72
4.97
5.60










The peak memory usage of the CM solver is reached when computing the Cholesky factorization, Gi, of the Hessian matrix corresponding to the largest dataset, which does not change with the number of commonly-observed features. Note that computing Ki and KTK matrices in parallel may consume more memory, but it can be reduced by limiting the number of tasks processed in parallel. The extreme case would be to compute Ki and KTK matrices in a single thread which requires very limited memory. On the other hand, the peak memory usage of the BLS solver happens during the Cholesky factorization of the Hessian corresponding to the merged dataset. Since the Cholesky factorization consumes memory proportional to the size of the Hessian matrix, the memory usage of the BLS solution is about 4 times larger. In addition, this memory usage grows with increasing number of common features, because they introduce correlations between different datasets. Note the memory usage is particularly important for running the mapping algorithm on mobile devices, which have very limited available RAM (e.g., iPhone 6 has only 1 GB RAM) and need to run other processes concurrently (e.g., the android operating system).


In summary, when the number of common features is in the order of hundreds, the proposed CM solution is much more efficient than the standard BLS solution in terms of both computational cost and memory usage. If the number of common features increases to the order of thousands, the CM solution is still less memory demanding but requires significantly more computations. In such case, we can speed up the CM solution by selecting a informative subset of the common features whose number can be analytically determined according to the affordable computational cost, and the estimates from the resulting sparse CM have very small difference from exact one.


This disclosure describes cooperative mapping (CM) algorithms for combining visual and inertial measurements collected using mobile devices by multiple users at different times across large indoor spaces. We considered the most general case, where the users' relative transformation is not known and cannot be inferred by directly observing each other. Our formulation of CM as a Batch Least Squares (BLS) equivalent constrained-optimization problem offers significant advantages when processing multiple maps: (i) Resource awareness, as the computational cost can be adjusted by changing the number of commonly-observed feature constraints; (ii) Computational gains, as the most computational intensive operations can be processed in parallel, and partial results regarding the trajectory and map of each user can be re-used; (iii) Memory savings, as the Cholesky factorization is applied on the Hessian of each user's data sequential, but not on the Hessian of the merged data from all users. Furthermore, we utilize free-line and Manhattan-line features in addition to point features, which improves the estimation accuracy and particularly corrects the “yaw” error. A large-scale CM experiment was conducted demonstrating the performance of the example algorithm in terms of accuracy and processing speed when using point, free-line, and Manhattan-line visual measurements.



FIG. 15 shows a detailed example of various devices (generally referred to as a computing platform) that may be configured to implement some embodiments, in accordance with the techniques of the disclosure. For example, device 500 may be a robot, mobile sensing platform, a mobile phone, a wearable device such as a smartphone or smart watch, a workstation, a cloud-based computing center, a cluster of servers or other example embodiments of a computing environment, centrally located or distributed, capable of executing the techniques described herein. Any or all of the devices may, for example, implement portions of the techniques described herein for vision-aided inertial navigation systems.


In this example, a computer 500 includes a hardware-based processor 510 that may be incorporated into VINS 10 or any device or system to execute program instructions or software, causing the computer to perform various methods or tasks, such as performing the techniques described herein. Processor 510 may be a general-purpose processor, a digital signal processor (DSP), a core processor within an Application Specific Integrated Circuit (ASIC) and the like. Processor 510 is coupled via bus 520 to a memory 530, which is used to store information such as program instructions and other data while the computer is in operation. A storage device 540, such as a hard disk drive, nonvolatile memory, or other non-transient storage device stores information such as program instructions, data files of the multidimensional data and the reduced data set, and other information. As another example, computer 500 may provide an operating environment for execution of one or more virtual machines that, in turn, provide an execution environment for software for implementing the techniques described herein.


The computer also includes various input-output elements 550, including parallel or serial ports, USB, Firewire or IEEE 1394, Ethernet, and other such ports to connect the computer to external device such a printer, video camera, surveillance equipment or the like. Other input-output elements include wireless communication interfaces such as Bluetooth, Wi-Fi, and cellular data networks.


The computer itself may be a traditional personal computer, a rack-mount or business computer or server, or any other type of computerized system. The computer in a further example may include fewer than all elements listed above, such as a thin client or mobile device having only some of the shown elements. In another example, the computer is distributed among multiple computer systems, such as a distributed server that has many computers working together to provide various functions.


The techniques described herein may be implemented in hardware, software, firmware, or any combination thereof. Various features described as modules, units or components may be implemented together in an integrated logic device or separately as discrete but interoperable logic devices or other hardware devices. In some cases, various features of electronic circuitry may be implemented as one or more integrated circuit devices, such as an integrated circuit chip or chipset.


If implemented in hardware, this disclosure may be directed to an apparatus such a processor or an integrated circuit device, such as an integrated circuit chip or chipset. Alternatively or additionally, if implemented in software or firmware, the techniques may be realized at least in part by a computer readable data storage medium comprising instructions that, when executed, cause one or more processors to perform one or more of the methods described above. For example, the computer-readable data storage medium or device may store such instructions for execution by a processor. Any combination of one or more computer-readable medium(s) may be utilized.


A computer-readable storage medium (device) may form part of a computer program product, which may include packaging materials. A computer-readable storage medium (device) may comprise a computer data storage medium such as random access memory (RAM), read-only memory (ROM), non-volatile random access memory (NVRAM), electrically erasable programmable read-only memory (EEPROM), flash memory, magnetic or optical data storage media, and the like. In general, a computer-readable storage medium may be any tangible medium that can contain or store a program for use by or in connection with an instruction execution system, apparatus, or device. Additional examples of computer readable medium include computer-readable storage devices, computer-readable memory, and tangible computer-readable medium. In some examples, an article of manufacture may comprise one or more computer-readable storage media.


In some examples, the computer-readable storage media may comprise non-transitory media. The term “non-transitory” may indicate that the storage medium is not embodied in a carrier wave or a propagated signal. In certain examples, a non-transitory storage medium may store data that can, over time, change (e.g., in RAM or cache).


The code or instructions may be software and/or firmware executed by processing circuitry including one or more processors, such as one or more digital signal processors (DSPs), general purpose microprocessors, application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other equivalent integrated or discrete logic circuitry. Accordingly, the term “processor,” as used herein may refer to any of the foregoing structure or any other processing circuitry suitable for implementation of the techniques described herein. In addition, in some aspects, functionality described in this disclosure may be provided within software modules or hardware modules.


Various embodiments of the invention have been described. Further examples are contemplated herein. These and other embodiments are within the scope of the following claims.

Claims
  • 1. A method comprising: receiving, with a computing platform having one or more hardware-based processors, respective trajectory data and map data independently generated by each of a plurality of vision-aided inertial navigation devices (VINS devices) traversing an environment, wherein the trajectory data specifies poses along a path through the environment for the respective VINS device and the map data specifies positions of observed features within the environment as determined by an estimator executed by the respective VINS device;determining, with the computing platform and based on the respective trajectory data and map data from each of the VINS devices, estimates for relative poses within the environment by determining transformations that geometrically relate the trajectory data and the map data between one or more pairs of the VINS devices; andgenerating, with the computing platform and based on the transformations, a composite map that specifies positions within the environment for the features observed by any of the VINS devices.
  • 2. The method of claim 1, further comprising: identifying features that are commonly observed by two or more of the VINS devices;for each of the commonly observed features, determining, with the computing platform, a geometric constraint that imposes geometric consistency for the feature based on the map data from each of the two or more VINS devices that observed the feature; andgenerating composite maps based on the transformations geometrically relating the map data received from the plurality of VINS devices and by imposing the geometric constraints computed from the commonly observed features.
  • 3. The method of claim 2, further comprising selecting, based on computing resources available within the computing platform, only a subset of the commonly observed features for which to compute respective geometric constraints to be applied when generating the composite map.
  • 4. The method of claim 3, wherein, while generating the composite map, excluding computation of respective geometric constraints for any unselected commonly observed features and instead treating any unselected commonly observed feature as different features within the map data for the two or more VINS that observed the feature.
  • 5. The method of claim 3, wherein selecting only the subset of the commonly observed features for which to compute the respective geometric constraints further comprises: partitioning the composite map into a plurality of regions; andselecting an equal number subset of the features within each of the regions, wherein the number of features for inclusion in the subsets is determined based on the computing resources within the computing platform.
  • 6. The method of claim 5, wherein selecting the equal number subset of the features comprises: ranking, within each of the regions, the features based on the number of VINS that commonly observed the feature; andselecting, for each of the regions, the features that were commonly observed by the most number of the VINS devices until the number of features for inclusion within the subset has been selected.
  • 7. The method of claim 5, wherein selecting the subset of the features for each of the regions comprises: projecting each of the features within the respective region onto a two dimensional X-Y plane for that region such that features within the region having the same X-Y positions are combined on the X-Y plane;selecting the features for the subset from the projected features on the X-Y plane.
  • 8. The method of claim 1, wherein generating the relative estimates further comprises: constructing a tree in which each node represents a different one of the VINS devices and each link between pairs of nodes is assigned a weight representing the number of commonly observed features between the pair of VINS devices;selecting, based on the weights of the links, a chain through the tree that links all of the nodes; anddetermining the transformations only between pairs of the VINS devices that are neighboring nodes along the selected chain.
  • 9. The method of claim 1, further comprising communicating the composite map to a VINS device for navigation within the environment.
  • 10. The method of claim 1, wherein the features represent objects visible within the environment, andwherein the vision-aided inertial navigation system is integrated within a tablet computer, a laptop computer, a mobile phone, a wearable computing device, a robot, a vehicle, or an unmanned aircraft system (UAS).
  • 11. A vision-aided inertial navigation system comprising: a plurality of mobile devices, each of the mobile devices comprising: at least one image source to produce image data along a trajectory of the mobile device within an environment, wherein the image data contains a plurality of features observed within the environment at a plurality of poses of the mobile device along the trajectory;an inertial measurement unit (IMU) to produce IMU data indicative of motion of the vision-aided inertial navigation system; anda hardware-based processing unit comprising an estimator that determines, based on the image data and the IMU data, trajectory data specifying a position and orientation of the mobile device for a plurality of poses of the mobile device along the trajectory and map data specifying positions with the environment for features observed from the poses; anda cooperative mapping server configured to: receive respective trajectory data and map data independently generated by each of mobile devices;determine transformations that geometrically relate the trajectory data and the map data between one or more pairs of the mobile devices; andgenerating, with the computing platform and based on the transformations, a composite map that specifies positions within the environment for the features observed by any of the mobile devices.
  • 12. The vision-aided inertial navigation system of claim 11, wherein the cooperative mapping server is further configured to: identify features that are commonly observed by two or more of the mobile devices;for each of the commonly observed features, determine a geometric constraint that imposes geometric consistency for the feature based on the map data from each of the two or more mobile devices that observed the feature; andgenerate composite maps based on the transformations geometrically relating the map data received from the plurality of mobile devices and by imposing the geometric constraints computed from the commonly observed features.
  • 13. The vision-aided inertial navigation system of claim 12, wherein the cooperative mapping server is further configured to select, based on computing resources available within the cooperative mapping serve, only a subset of the commonly observed features for which to compute respective geometric constraints to be applied when generating the composite map.
  • 14. The vision-aided inertial navigation system of claim 13, wherein, while generating the composite map, the cooperative mapping server is further configured to exclude computation of respective geometric constraints for any unselected commonly observed features and instead treat any unselected commonly observed feature as different features within the map data for the two or more mobile devices that observed the feature.
  • 15. The vision-aided inertial navigation system of claim 13, wherein, to select only the subset of the commonly observed features for which to compute the respective geometric constraints, the cooperative mapping server is further configured to: partition the composite map into a plurality of regions; andselect an equal number subset of the features within each of the regions, wherein the number of features for inclusion in the subsets is determined based on the computing resources within the cooperative mapping server.
  • 16. The vision-aided inertial navigation system of claim 15, wherein to select the equal number subset of the features, the cooperative mapping server is further configured to: rank, within each of the regions, the features based on the number of VINS that commonly observed the feature; andselect, for each of the regions, the features that were commonly observed by the most number of the mobile devices until the number of features for inclusion within the subset has been selected.
  • 17. The vision-aided inertial navigation system of claim 15, wherein to select the subset of the features for each of the region, the cooperative mapping server is further configured to: project each of the features within the respective region onto a two dimensional X-Y plane for that region such that features within the region having the same X-Y positions are combined on the X-Y plane;select the features for the subset from the projected features on the X-Y plane.
  • 18. The vision-aided inertial navigation system of claim 11, wherein to generate the relative estimates, the cooperative mapping server is further configured to: construct a tree in which each node represents a different one of the mobile devices and each link between pairs of nodes is assigned a weight representing the number of commonly observed features between the pair of mobile devices;select, based on the weights of the links, a chain through the tree that links all of the nodes; anddetermine the transformations only between pairs of the mobile devices that are neighboring nodes along the selected chain.
  • 19. The vision-aided inertial navigation system of claim 11, wherein the cooperative mapping server is further configured to communicate the composite map to a mobile device for navigation within the environment.
  • 20. The vision-aided inertial navigation system of claim 11, wherein the mobile devices comprise any of a tablet computer, a laptop computer, a mobile phone, a wearable computing device, a robot, a vehicle, or an unmanned aircraft system (UAS).
  • 21. A non-transitory computer-readable medium comprising instructions that, when executed, cause one or more hardware-based processors of a computing platform to: receive respective trajectory data and map data independently generated by each of a plurality of vision-aided inertial navigation devices (VINS devices) traversing an environment, wherein the trajectory data specifies poses along a path through the environment for the respective VINS device and the map data specifies positions of observed features within the environment as determined by an estimator executed by the respective VINS device;determine, based on the respective trajectory data and map data from each of the VINS devices, estimates for relative poses within the environment by determining transformations that geometrically relate the trajectory data and the map data between one or more pairs of the VINS devices; andgenerate, based on the transformations, a composite map that specifies positions within the environment for the features observed by any of the VINS devices.
Parent Case Info

This application claims the benefit of U.S. Provisional Application No. 62/341,237 by Roumeliotis, entitled, “RESOURCE-AWARE LARGE-SCALE COOPERATIVE 3D MAPPING USING MULTIPLE MOBILE DEVICES” and filed on May 25, 2016, the entire content of which is incorporated herein by reference.

US Referenced Citations (36)
Number Name Date Kind
5847755 Wixson et al. Dec 1998 A
6104861 Tsukagoshi Aug 2000 A
7015831 Karisson et al. Mar 2006 B2
7162338 Goncalves et al. Jan 2007 B2
7991576 Roumeliotis Aug 2011 B2
8577539 Morrison et al. Nov 2013 B1
8965682 Tangirala et al. Feb 2015 B2
8996311 Morin et al. Mar 2015 B1
9031809 Kumar et al. May 2015 B1
9243916 Roumeliotis et al. Jan 2016 B2
9607401 Roumeliotis et al. Mar 2017 B2
9658070 Roumeliotis et al. May 2017 B2
9709404 Roumeliotis et al. Jul 2017 B2
20020198632 Breed et al. Dec 2002 A1
20040073360 Foxlin Apr 2004 A1
20040167667 Goncalves et al. Aug 2004 A1
20050013583 Itoh Jan 2005 A1
20080167814 Samarasekera et al. Jul 2008 A1
20080265097 Stecko et al. Oct 2008 A1
20080279421 Hamza et al. Nov 2008 A1
20090248304 Roumeliotis et al. Oct 2009 A1
20100110187 von Flowtow et al. May 2010 A1
20100220176 Ziemeck et al. Sep 2010 A1
20120121161 Eade May 2012 A1
20120194517 Izadi et al. Aug 2012 A1
20130335562 Ramanandan et al. Dec 2013 A1
20140316698 Roumeliotis et al. Oct 2014 A1
20140333741 Roumeliotis et al. Nov 2014 A1
20150356357 McManus et al. Dec 2015 A1
20150369609 Roumeliotis et al. Dec 2015 A1
20160005164 Roumeliotis et al. Jan 2016 A1
20160161260 Mourikis Jun 2016 A1
20160305784 Roumeliotis et al. Oct 2016 A1
20160327395 Roumeliotis et al. Nov 2016 A1
20170176189 D'Aquila Jun 2017 A1
20180082137 Melvin Mar 2018 A1
Foreign Referenced Citations (2)
Number Date Country
WO 2015013418 Jan 2015 WO
WO 2015013534 Jan 2015 WO
Non-Patent Literature Citations (124)
Entry
Guo et al., “Resource-Aware Large-Scale Cooperative 3D Mapping from Multiple Cell Phones,” Poster submitted to The International Robotics and Automation Conference, May 26-31, 2015, 1 pp.
Golub et al., “Matrix Multiplication Problems,” Chapter 1, Matrix Computations, Third Edition, ISBN 0-8018-5413-X, 1996, (Applicant points out, in accordance with MPEP 609.04(a), that the year of publication, 1996, is sufficiently earlier than the effective U.S. filing date so that the particular month of publication is not in issue.).
“Kalman filter,” Wikipedia, the Free Encyclopedia, accessed from https://en.wikipedia.org/w/index.php?title=Kalman_filter&oldid=615383582, drafted Jul. 3, 2014, 27 pp.
Thorton et al., “Triangular Covariance Factorizations for Kalman Filtering,” Technical Memorandum 33-798, National Aeronautics and Space Administration, Oct. 15, 1976, 212 pp.
Shalom et al., “Estimation with Applications to Tracking and Navigation,” Chapter 7, Estimation with Applications to Tracking and Navigation, ISBN 0-471-41655-X, Jul. 2001, 20 pp.
Higham, “Matrix Inversion,” Chapter 14, Accuracy and Stability of Numerical Algorithms, Second Edition, ISBN 0-89871-521-0, 2002, 29 pp. (Applicant points out, in accordance with MPEP 609.04(a), that the year of publication, 2002, is sufficiently earlier than the effective U.S. filing date, so that the particular month of publication is not in issue.).
Kottas et al., “An Iterative Kalman Smoother for Robust 3D Localization and mapping,” ISRR, Tech Report, Oct. 16, 2014, 15 pp.
“Project Tango,” retrieved from https://www.google.com/atap/projecttango on Nov. 2, 2015, 4 pp.
Ait-Aider et al., “Simultaneous object pose and velocity computation using a single view from a rolling shutter camera,” Proceedings of the IEEE European Conference on Computer Vision, May 7-13, 2006, pp. 56-68.
Ayache et al., “Maintaining Representations of the Environment of a Mobile Robot,” IEEE Transactions on Robotics and Automation, vol. 5, No. 6, Dec. 1989, pp. 804-819.
Agarwal et al., “A Survey of Geodetic Approaches to Mapping and the Relationship to Graph-Based SLAM,” IEEE Robotics and Automation Magazine, vol. 31, Sep. 2014, 17 pp.
Baker et al., “Removing rolling shutter wobble,” Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, Jun. 13-18, 2010, 8 pp.
Bartoli et al., “Structure from Motion Using Lines: Representation, Triangulation and Bundle Adjustment,” Computer Vision and Image Understanding, vol. 100, Aug. 11, 2005, pp. 416-441.
Bayard et al., “An Estimation Algorithm for Vision-Based Exploration of Small Bodies in Space,” 2005 American Control Conference, Jun. 8-10, 2005, pp. 4589-4595.
Bierman, “Factorization Methods for Discrete Sequential Estimation,” Mathematics in Science and Engineering, Academic Press, vol. 128, 1977, 259 pp. (Applicant points out that, in accordance with MPEP 609.04(a), the 2012 year of publication is sufficiently earlier than the effective U.S. filing date and any foreign priority date of Apr. 17, 2016 so that the particular month of publication is not in issue.).
Bouguet, “Camera Calibration Toolbox for Matlab,” retrieved from http://www.vision.caltech.edu/bouguetj/calib_doc/., Oct. 14, 2015, 5 pp.
Boyd et al., “Convex Optimization,” Cambridge University Press, 2004, 730 pp. (Applicant points out that, in accordance with MPEP 609.04(a), the 2004 year of publication is sufficiently earlier than the effective U.S. filing date and any foreign priority date of Apr. 17, 2016 so that the particular month of publication is not in issue.).
Breckenridge, “Interoffice Memorandum to T. K. Brown, Quaternions—Proposed Standard Conventions,” I0M 343-79-1199, Oct. 31, 1979, 12 pp.
Canny, “A Computational Approach to Edge Detection,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 8, No. 6, Nov. 1986, pp. 679-698.
Chen, “Pose Determination from Line-to-Plane Correspondences: Existence Condition and Closed-Form Solutions,” Proc. 3rd. Int. Coni. Comp. Vision, Dec. 4-7, 1990, pp. 374-378.
Chiu et al., “Robust vision-aided navigation using sliding-window factor graphs,” 2013 IEEE International Conference on Robotics and Automation, May 6-10, 2013, pp. 46-53.
Chiuso et al., “Structure from Motion Causally Integrated Over Time,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 24, No. 4, Apr. 2002, pp. 523-535.
Davison et al., “Simultaneous Localisation and Map-Building Using Active Vision,” Jun. 2001, 18 pp.
Deans., “Maximally Informative Statistics for Localization and Mapping”, Proceedings of the 2002 IEEE International Conference on Robotics & Automation, (Washington, D.C.), May 2002, 1824-1829.
Dellaert et al., “Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing,” International Journal of Robotics and Research, vol. 25, No. 12, Dec. 2006, pp. 1181-1203.
Diel, “Stochastic Constraints for Vision-Aided Inertial Navigation,” Massachusetts Institute of Technology, Department of Mechanical Engineering, Master Thesis, Jan. 2005, 106 pp.
Dong-Si et al., “Motion Tracking with Fixed-lag Smoothing: Algorithm and Consistency Analysis,” Proceedings of the IEEE International Conference on Robotics and Automation, May 9-13, 2011, 8 pp.
Eade et al., “Scalable Monocular SLAM,” Proceedings of the 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR '06), vol. 1, Jun. 17-22, 2006, 8 pp.
Erdogan et al., “Planar Segmentation of RGBD Images Using Fast Linear Filling and Markov Chain Monte Carlo,” Proceedings of the IEEE International Conference on Computer and Robot Vision, May 27-30, 2012, pp. 32-39.
Eustice et al., “Exactly Sparse Delayed-slate Filters for View-based SLAM,” IEEE Transactions on Robotics, vol. 22 (6), Dec. 2006, pp. 1100-1114.
Eustice et al., “Visually Navigating the RMS Titanic With Slam Information Filters,” Proceedings of Robotics Science and Systems, Jun. 2005, 9 pp.
Furgale et al., “Unified temporal and spatial calibration for multi-sensor systems,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Nov. 3-7, 2013, pp. 1280-1286.
Garcia et al., “Augmented State Kalman Filtering for AUV Navigation.” Proceedings of the 2002 IEEE International Conference on Robotics & Automation, May 2002, 6 pp.
Golub et al., “Matrix Computations, Third Edition,” The Johns Hopkins University Press, 2012, 723 pp. (Applicant points out that, in accordance with MPEP 609.04(a), the 2012 year of publication is sufficiently earlier than the effective U.S. filing date and any foreign priority date of Apr. 17, 2016 so that the particular month of publication is not in issue.).
Golub et al., “Matrix Computations, Fourth Edition,” The Johns Hopkins University Press, 2013, 780 pp. (Applicant points out that, in accordance with MPEP 609.04(a), the 2013 year of publication is sufficiently earlier than the effective U.S. filing date and any foreign priority date of Apr. 17, 2016 so that the particular month of publication is not in issue.).
Guo et al., “Observability-constrained EKF Implementation of the IMU-RGBD Camera Navigation Using Point and Plane Features,” Technical Report. University of Minnesota, Mar. 2013, 6 pp.
Guo et al., “IMU-RGBD Camera 3D Pose Estimation and Extrinsic Calibration: Observability Analysis and Consistency Improvement,” Proceedings of the IEEE International Conference on Robotics and Automation, May 6-10, 2013, pp. 2920-2927.
U.S. Appl. No. 15/601,261, filed May 22, 2017, Stergios I. Roumeliotis.
Guo et al., “Efficient Visual-Inertial Navigation using a Rolling-Shutter Camera with Inaccurate Timestamps,” Proceedings of Robotics: Science and Systems, Jul. 2014, 9 pp.
Harris et al., “A combined corner and edge detector,” Proceedings of the Alvey Vision Conference, Aug. 31-Sep. 2, 1988, pp. 147-151.
Hermann et al., “Nonlinear Controllability and Observability,” IEEE Transactions on Automatic Control, vol. 22, No. 5, Oct. 1977, pp. 728-740.
Herrera et al., “Joint Depth and Color Camera Calibration with Distortion Correction,” IEEE Trans. on Pattern Analysis and Machine Intelligence, vol. 34(10), Oct. 2012, pp. 2058-2064.
Hesch et al., “Consistency analysis and improvement of vision-aided inertial navigation,” IEEE Transactions on Robotics, vol. 30, No. 1, Feb. 2014, pp. 158-176.
Hesch et al., “Observability-constrained Vision-aided Inertial Navigation,” University of Minnesota, Department of Computer Science and Engineering, MARS Lab, Feb. 2012, 24 pp.
Hesch et al., “Towards Consistent Vision-aided Inertial Navigation,” Proceedings of the 10th International Workshop on the Algorithmic Foundations of Robotics, Jun. 13-15, 2012, 16 pp.
Horn et al., “Closed-form solution of absolute orientation using orthonormal matrices,” Journal of the Optical Society of America A, vol. 5, No. 7, Jul. 1988, pp. 1127-1135.
Huang et al., “Observability-based rules for designing consistent EKF slam estimators,” International Journal of Robotics Research, vol. 29, No. 5, Apr. 2010, pp. 502-528.
Huang et al., “Visual Odometry and Mapping for Autonomous Flight Using an RGB-D Camera,” Proceedings of the International Symposium on Robotics Research, Aug. 28-Sep. 1, 2011, 16 pp.
Huster, “Relative Position Sensing by Fusing Monocular Vision and Inertial Rate Sensors,” Stanford University, Department of Electrical Engineering, Dissertation, Jul. 2003, 158 pp.
Jia et al., “Probabilistic 3-D motion estimation for rolling shutter video rectification from visual and inertial measurements,” Proceedings of the IEEE International Workshop on Multimedia Signal Processing, Sep. 2012, pp. 203-208.
Johannsson et al., “Temporally Scalable Visual Slam Using a Reduced Pose Graph,” in Proceedings of the IEEE International Conference on Robotics and Automation, May 6-10, 2013, 8 pp.
Jones et al., “Visual-inertial Navigation, Mapping and Localization: A Scalable Real-time Causal Approach,” International Journal of Robotics Research, vol. 30, No. 4, Mar. 31, 2011, pp. 407-430.
Kaess et al., “iSAM: Incremental Smoothing and Mapping,” IEEE Transactions on Robotics, Manuscript, Sep. 7, 2008, 14 pp.
Kaess et al., “iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree,” International Journal of Robotics Research, vol. 31, No. 2, Feb. 2012, pp. 216-235.
Kelly et al., “A general framework for temporal calibration of multiple proprioceptive and exteroceptive sensors,” Proceedings of International Symposium on Experimental Robotics, Dec. 18-21, 2010, 15 pp.
Kelly et al., “Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration,” International Journal of Robotics Research, vol. 30, No. 1, Jan. 2011, pp. 56-79.
Klein et al., “Parallel Tracking and Mapping for Small AR Workspaces,” Proceedings of the IEEE and ACM International Symposium on Mixed and Augmented Reality, Nov. 13-16, 2007, pp. 225-234.
Kneip et al., “Robust Real-Time Visual Odometry with a Single Camera and an IMU,” Proceedings of the British Machine Vision Conference, Aug. 29-Sep. 2, 2011, pp. 16.1-16.11.
Konolige et al., “View-based Maps,” International Journal of Robotics Research, vol. 29, No. 8, Jul. 2010, 14 pp.
Konolige et al., “Efficient Sparse Pose Adjustment for 2D Mapping,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Oct. 18-22, 2010, pp. 22-29.
Konolige et al., “FrameSLAM: From Bundle Adjustment to Real-Time Visual Mapping,” IEEE Transactions on Robotics, vol. 24, No. 5, Oct. 2008, pp. 1066-1077.
Kottas et al., “An iterative Kalman smoother for robust 3D localization on mobile and wearable devices,” Proceedings of the IEEE International Conference on Robotics and Automation, May 26-30, 2015, pp. 6336-6343.
Kottas et al., “Detecting and dealing with hovering maneuvers in vision-aided inertial navigation systems,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Nov. 3-7, 2013, pp. 3172-3179.
Kottas et al., “Efficient and Consistent Vision-aided Inertial Navigation using Line Observations,” Department of Computer Science & Engineering, University of Minnesota, MARS Lab, TR-2012-002, Sep. 2012, 14 pp.
Kottas et al., “On the Consistency of Vision-aided Inertial Navigation,” Proceedings of the International Symposium on Experimental Robotics, Jun. 17-20, 2012, 15 pp.
Kummerle et al., “g2o: A General Framework for Graph Optimization,” in Proceedings of the IEEE International Conference on Robotics and Automation, May 9-13, 2011, pp. 3607-3613.
Langelaan, “State Estimation for Autonomous Flight in Cluttered Environments,” Stanford University, Department of Aeronautics and Astronautics, Dissertation, Mar. 2006, 128 pp.
Leutenegger et al., “Keyframe-based visual-inertial odometry using nonlinear optimization,” The International Journal of Robotics Research, vol. 34, No. 3, Mar. 2015, pp. 314-334.
Li et al., “3-D Motion Estimation and Online Temporal Calibration for Camera-IMU Systems,” Proceedings of the IEEE International Conference on Robotics and Automation, May 6-10, 2013, pp. 5709-5716.
Li et al., “Improving the Accuracy of EKF-based Visual-Inertial Odometry,” 2012 IEEE International Conference on Robotics and Automation, May 14-18, 2012, pp. 828-835.
Li et al., “Optimization-Based Estimator Design for Vision-Aided Inertial Navigation,” Proceedings of the Robotics: Science and Systems Conference, Jul. 9-13, 2012, 8 pp.
Li et al., “Real-time Motion Tracking on a Cellphone using Inertial Sensing and a Rolling-Shutter Camera,” 2013 IEEE International Conference on Robotics and Automation (ICRA), May 6-10, 2013, 8 pp.
Li et al., “Vision-aided inertial navigation with rolling-shutter cameras,” The International Journal of Robotics Research, retrieved from ijr.sagepub.com on May 22, 2015, 18 pp.
Liu et al., “Estimation of Rigid Body Motion Using Straight Line Correspondences,” Computer Vision, Graphics, and Image Processing, vol. 43, No. 1, Jul. 1988, pp. 37-52.
Liu et al., “Multi-aided Inertial Navigation for Ground Vehicles in Outdoor Uneven Environments,” Proceedings of the IEEE International Conference on Robotics and Automation, Apr. 18-22, 2005, pp. 4703-4708.
Kottas et al., “A Resource-aware Vision-aided Inertial Navigation System for Wearable and Portable Computers,” IEEE International Conference on Robotics and Automation, Accepted Apr. 18, 2014, available online May 6, 2014, 3 pp.
Lowe, “Distinctive Image Features from Scale-Invariant Keypoints,” International Journal of Computer Vision, Jan. 5, 2004, 28 pp.
Lucas et al., “An Iterative Image Registration Technique with an Application to Stereo Vision,” Proceedings of the 7th International Joint Conference on Artificial Intelligence, Aug. 24-28, 1981, pp. 674-679.
Lupton et al., “Visual-inertial-aided Navigation for High-dynamic Motion in Built Environments Without Initial Conditions,” IEEE Transactions on Robotics, vol. 28, No. 1, Feb. 2012, pp. 61-76.
Martinelli, “Vision and IMU Data Fusion: Closed-form Solutions for Attitude, Speed, Absolute Scale, and Bias Determination,” IEEE Transactions on Robotics, vol. 28, No. 1, Feb. 2012, pp. 44-60.
Matas et al., “Robust Detection of Lines Using the Progressive Probabilistic Hough Transformation,” Computer Vision and Image Understanding, vol. 78, No. 1, Apr. 2000, pp. 119-137.
McLauchlan, “The Variable State Dimension Filter Applied to Surface-Based Structure from Motion CVSSP Technical Report VSSP-TR-4/99,” University of Surrey, Department of Electrical Engineering, Dec. 1999, 52 pp.
Meltzer et al., “Edge Descriptors for Robust Wide-baseline Correspondence,” Proc. IEEE Conf. Comp. Vision Pall. Recognition., Jun. 23-28, 2008, pp. 1-8.
Mirzaei et al., “A Kalman Filter-based Algorithm for IMU-Camera Calibration: Observability Analysis and Performance Evaluation,” IEEE Trans. Robot., vol. 24 No. 5, Oct. 2008, pp. 1143-1156.
Mirzaei et al., “Optimal Estimation of Vanishing Points in a Manhattan World,” IEEE Int. Conference on Computer Vision, Nov. 6-13, 2011, pp. 2454-2461.
Mirzaei et al., “Globally Optimal Pose Estimation from Line Correspondences,” IEEE International Conference on Robotics and Automation, May 9-13, 2011, pp. 5581-5588.
Montiel et al., “Unified Inverse Depth Parametrization for Monocular SLAM,” Proceedings of Robotics: Science and Systems II (RSS-06), Aug. 16-19, 2006, 8 pp.
Mourikis et al., “On the Treatment of Relative-Pose Measurements for Mobile Robot Localization,” IEEE International Conference on Robotics and Automation, Conference Date May 15-19, 2006, Jun. 26, 2006, 8 pp.
Mourikis et al., “A Dual-Layer Estimator Architecture for Long-term Localization,” Proceedings of the Workshop on Visual Localization for Mobile Platforms, Jun. 24-26, 2008, 8 pp.
Mourikis et al., “A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation,” IEEE International Conference on Robotics and Automation, Apr. 10-14, 2007, pp. 3565-3572.
Mourikis et al., “A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation,” IEEE International Conference on Robotics and Automation, Sep. 28, 2006, 20 pp.
Mourikis et al., “Vision-Aided Inertial Navigation for Spacecraft Entry, Descent, and Landing,” IEEE Transactions on Robotics, vol. 25, No. 2, Apr. 2009, pp. 264-280.
Nerurkar et al., “C-KLAM: Constrained Keyframe-Based Localization and Mapping,” Proceedings of the IEEE International Conference on Robotics and Automation, May 31-Jun. 7, 2014, 6 pp.
Nister et al., “Visual Odometry for Ground Vehicle Applications,” Journal of Field Robotics, vol. 23, No. 1, Jan. 2006, 35 pp.
Oliensis, “A New Structure from Motion Ambiguity,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 22, No. 7, Jul. 2000, 30 pp.
Ong et al., “Six DoF Decentralised SLAM,” Proceedings of the Australasian Conference on Robotics and Automation, 2003, 10 pp. (Applicant points out that, in accordance with MPEP 609.04(a), the 2003 year of publication is sufficiently earlier than the effective U.S. filing date and any foreign priority date of Apr. 17, 2016 so that the particular month of publication is not in issue.).
Oth et al., “Rolling shutter camera calibration,” Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, Jun. 23-28, 2013, pp. 1360-1367.
Prazenica et al., “Vision-Based Kalman Filtering for Aircraft State Estimation and Structure from Motion,” AIAA Guidance, Navigation, and Control Conference and Exhibit, Aug. 15-18, 2005, 13 pp.
Roumeliotis et al., “Augmenting Inertial Navigation With Image-Based Motion Estimation,” IEEE International Conference on Robotics and Automation, vol. 4, 2002, 8 pp. (Applicant points out that, in accordance with MPEP 609.04(a), the 2002 year of publication is sufficiently earlier than the effective U.S. filing date and any foreign priority date of Apr. 17, 2016 so that the particular month of publication is not in issue.).
Roumeliotis et al., “Stochastic Cloning: A Generalized Framework for Processing Relative State Measurements,” Proceedings of the 2002 IEEE International Conference on Robotics and Automation, May 11-15, 2002, pp. 1788-1795.
Schmid et al., “Automatic Line Matching Across Views,” Proceedings of the IEEE Computer Science Conference on Computer Vision and Pattern Recognition, Jun. 17-19, 1997, pp. 666-671.
Servant et al., “Improving Monocular Plane-based SLAM with Inertial Measurements,” 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, Oct. 18-22, 2010, pp. 3810-3815.
Shoemake et al., “Animating rotation with quaternion curves,” ACM SIGGRAPH Computer Graphics, vol. 19, No. 3, Jul. 22-26, 1985, pp. 245-254.
Sibley et al., “Sliding Window Filter with Application to Planetary Landing,” Journal of Field Robotics, vol. 27, No. 5, Sep./Oct. 2010, pp. 587-608.
Smith et al., “On the Representation and Estimation of Spatial Uncertainty,” International Journal of Robotics Research, vol. 5(4), 1986, pp. 56-68 (Note: Applicant points out in accordance with MPEP 609.04(a) that the 1986 year of publication is sufficiently earlier than the effective U.S. filing date and any foreign priority date of Apr. 17, 2016 so that the particular month of publication is not in issue.).
Smith et al., “Real-time Monocular Slam with Straight Lines,” British Machine vision Conference, vol. 1, Sep. 2006, pp. 17-26.
Soatto et al., “Motion Estimation via Dynamic Vision,” IEEE Transactions on Automatic Control, vol. 41, No. 3, Mar. 1996, pp. 393-413.
Soatto et al., “Recursive 3-D Visual Motion Estimation Using Subspace Constraints,” International Journal of Computer Vision, vol. 22, No. 3, Mar. 1997, pp. 235-259.
Spetsakis et al., “Structure from Motion Using Line Correspondences,” International Journal of Computer Vision, vol. 4, No. 3), Jun. 1990, pp. 171-183.
Strelow, D. W., “Motion Estimation from Image and Inertial Measurements”, CMU-CS-04-174, (Nov. 2004), 164 pgs.
Taylor et al., “Structure and Motion from Line Segments in Multiple Images,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 17, No. 11, Nov. 1995, pp. 1021-1032.
Trawny et al., “Indirect Kalman Filter for 3D Attitude Estimation,” University of Minnesota, Department of Computer Science & Engineering, MARS Lab, Mar. 2005, 25 pp.
Triggs et al., “Bundle Adjustment—A Modern Synthesis,” Proceedings of the International Workshop on Vision Algorithms: Theory and Practice, Lecture Notes in Computer Science, vol. 1883, Sep. 21-22, 1999, pp. 298-372.
Triggs et al., “Bundle Adjustment—A Modern Synthesis,” Vision Algorithms: Theory & Practice, LNCS 1883, Apr. 12, 2002, 71 pp.
Weiss et al., “Real-time Metric State Estimation for Modular Vision-Inertial Systems,” Proceedings of the IEEE International Conference on Robotics and Automation, May 9-13, 2011, pp. 4531-4537.
Weiss et al., “Versatile Distributed Pose Estimation and sensor Self-Calibration for an Autonomous MAV,” Proceedings of IEEE International Conference on Robotics and Automations, May 14-18, 2012, pp. 31-38.
Weng et al., “Motion and Structure from Line Correspondences: Closed-Form Solution, Uniqueness, and Optimization,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, No. 3, Mar. 1992, pp. 318-336.
Williams et al., “Feature and Pose Constrained Visual Aided Inertial Navigation for Computationally Constrained Aerial Vehicles,” 2011 IEEE International Conference on Robotics and Automation, May 9-13, 2011, pp. 431-438.
Zhou et al., “Determining 3d Relative Transformations for any Combination of Range and Bearing Measurements,” IEEE Trans. on Robotics, vol. 29 No. 2, Apr. 2013, pp. 458-474.
Kottas et al., “An Iterative Kalman Smoother for Robust 3D Localization on Mobile and Wearable devices,” Submitted confidentially to International Conference on Robotics & Automation, ICRA '15, May 5, 2015, 8 pp.
Weiss et al., “Real-time Onboard Visual-inertial State Estimation and Self-calibration of MAVs in Unknown Environment,” Proceedings of the IEEE International Conference on Robotics and Automation, May 14-18, 2012, 957-964 pp.
Perea et al., “Sliding Windows and Persistence: An Application of Topological Methods to Signal Analysis,” Foundations of Computational Mathematics, Nov. 25, 2013, 34 pp.
U.S. Appl. No. 15/470,595, filed Mar. 27, 2017, by Stergios I. Roumeliotis.
U.S. Appl. No. 15/706,149, filed Sep. 15, 2015, Stergios I. Roumeliotis.
Related Publications (1)
Number Date Country
20170343356 A1 Nov 2017 US
Provisional Applications (1)
Number Date Country
62341237 May 2016 US