System and method for image capture device pose estimation

1. A method for estimating a plurality of image capture device poses, comprising:

using at least one hardware processor executing a code for:

extracting a plurality of observed image features of a plurality of landmarks from a plurality of images captured by at least one image capture device from at least one pose, said plurality of landmarks calibrated with respect to a certain coordinate system;

identifying among said plurality of observed image features at least one common observed image feature documented in at least some of said images;

producing image feature scale values of said at least one common observed image feature by analyzing said at least some of said images;

determining a plurality of estimated poses of said at least one image capture device with respect to said certain coordinate system by using said image feature scale values in calculating a minimal re-projection error between said plurality of observed image features and a plurality of predicted image features by using a scale re-projection error term in a bundle adjustment optimization calculation comprising at least one cost function which comprises at least one scale error term expressing a scale re-projection error using image feature scale information;

wherein said at least one scale error term is computed by computing a difference between a measured image feature scale calculated by a feature detector, and a predicted feature scale computed using an image capture device focal length, a distance from said common observed image feature's landmark to one of a plurality of estimated camera poses and a size estimate of said common observed image feature's landmark; and

outputting said plurality of estimated poses.

2. The method of claim 1 , wherein said scale re-projection error term is computed using said image feature scale values.

3. The method of claim 2 , wherein said bundle adjustment optimization calculation comprises:

producing a plurality of estimated image capture device pose sets, each of said plurality of sets comprising a plurality of estimated image capture device poses, each of said plurality of estimated image capture device poses associated with one of said plurality of images;

producing a plurality of estimated costs by applying said at least one cost function to each of said plurality of estimated capture device pose sets;

identifying a minimum estimated cost of said plurality of estimated costs; and

selecting the estimated capture device pose set used to produce said minimum estimated cost.

4. The method of claim 3 , wherein said producing a plurality of estimated image capture device pose sets comprises applying sparsity-aware iterative optimization technique;

wherein said sparsity-aware iterative optimization technique is a member of a group comprising: g2o, Georgia Tech smoothing and mapping (gtsam) and Ceres Solver.

5. The method of claim 4 , wherein said feature detector is Scale Invariant Feature Transform (SIFT).

6. The method of claim 4 , wherein computing said difference comprises calculating a Squared Mahalanobis Distance between said measured image feature scale and said predicted feature scale, having a measurement covariance matrix of a non zero-mean Gaussian distribution of noise elements in said image feature scale values.

7. The method of claim 6 , further comprising producing said measurement covariance matrix by computing a covariance matrix of a zero-mean Gaussian distribution of said noise elements in said image feature scale values.

8. The method of claim 1 ,

wherein computing said predicted feature scale comprises:

dividing said size estimate by said distance to produce a ratio; and

multiplying said ratio by said focal length; and

wherein said computing a difference between a measured image feature scale and a predicted feature scale comprises subtracting said predicted feature scale from said measured image feature scale.

9. The method of claim 1 , wherein said identifying among said plurality of observed image features at least one common observed image feature is by applying image matching algorithms.

10. The method of claim 9 , wherein said image matching algorithms include Scale Invariant Feature Transform (SIFT) and Random Sample Consensus (RANSAC).

11. The method of claim 1 , wherein said image feature scale values are produced while said identifying among said plurality of observed image features at least one common observed image feature.

12. The method of claim 1 , wherein said certain coordinate system is a world coordinate system.

13. The method of claim 1 , wherein the origin of said certain coordinate system is calibrated to a pose of one of said at least one image capture devices when capturing a first image of said plurality of images.

14. A system for estimating a plurality of image capture device poses, comprising:

at least one hardware processor executing a code for:

extracting a plurality of observed image features of a plurality of landmarks from a plurality of images captured by at least one image capture device from at least one pose, said plurality of landmarks calibrated with respect to a certain coordinate system;

identifying among said plurality of observed image features at least one common observed image feature documented in at least some of said images;

producing image feature scale values of said at least one common observed image feature by analyzing said at least some of said images;

determining a plurality of estimated poses of said at least one image capture device with respect to said certain coordinate system by using said image feature scale values in calculating a minimal re-projection error between said plurality of observed image features and a plurality of predicted image features by using a scale re-projection error term in a bundle adjustment optimization calculation comprising at least one cost function which comprises at least one scale error term expressing a scale re-projection error using image feature scale information;

wherein said at least one scale error term is computed by computing a difference between a measured image feature scale and a predicted feature scale;

wherein said measured image feature scale is calculated by a feature detector; and wherein said predicted feature scale is computed using an image capture device focal length, a distance from said common observed image feature's landmark to one of said plurality of estimated camera poses and a size estimate of said common observed image feature's landmark; and

outputting said plurality of estimated poses.

15. The system of claim 14 , further comprising at least one digital storage hardware, electrically connected to said at least one hardware processor.

16. The system of claim 14 , further comprising at least one non-volatile data storage;

wherein said at least one hardware processor is connected to said at least one non-volatile data storage via a network.

17. The system of claim 16 , wherein said non-volatile data storage is a cloud database.

18. The system of claim 14 , wherein said image capture device is installed on a moving arm.

19. The system of claim 14 , wherein said image capture device is installed on a vehicle.

20. The system of claim 19 , wherein said vehicle is a member of a group including: an airplane, a drone, a helicopter, a floating vehicle, a motorized vehicle, a car, a bicycle, a motorcycle, an electric vehicle, a balloon, a blimp and a cart.