Computationally efficient method for computing a composite representation of a 3D environment
Methods and apparatus for providing a representation of an environment, for example, in an XR system, and any suitable computer vision and robotics applications. A representation of an environment may include one or more planar features. The representation of the environment may be provided by jointly optimizing plane parameters of the planar features and sensor poses that the planar features are observed at. The joint optimization may be based on a reduced matrix and a reduced residual vector in lieu of the Jacobian matrix and the original residual vector.
1. A method of operating a computing system to generate a representation of an environment, the method comprising:
obtaining sensor captured information, the sensor captured information comprising a plurality of images;
providing an initial representation of the environment, the initial representation comprising N initial poses and initial parameters of M planar features based at least in part on the plurality of images, the initial parameters of the M planar features indicating normals of planes represented by the M planar features;
for each of the M planar features at each pose corresponding to an image of the plurality of images comprising one or more observations of the planar feature:
computing a matrix indicating the one or more observations of the planar feature, and
factorizing the matrix into two or more matrices, the two or more matrices comprising one matrix having reduced rows compared with the matrix; and
computing N refined poses and refined parameters of the M planar features by jointly adjusting the N initial poses and the initial parameters of the M planar features based at least in part on the matrices having reduced rows,
wherein the representation of the environment comprises N refined poses and the refined parameters of the M planar features.
2. The method of claim 1 , wherein for each of the M planar features at each pose corresponding to an image of the plurality of images comprising one or more observations of the planar feature:
factorizing the matrix into two or more matrices comprises computing an orthogonal matrix and an upper triangular matrix.
3. The method of claim 2 , wherein computing the N refined poses and refined parameters of the M planar features is based at least in part on the upper triangular matrices.
4. The method of claim 1 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having reduced rows comprises
computing reduced Jacobian matrix blocks based at least in part on the matrices having reduced rows.
5. The method of claim 4 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having reduced rows comprises
stacking the reduced Jacobian matrix blocks to form a reduced Jacobian matrix.
6. The method of claim 5 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having reduced rows comprises
providing the reduced Jacobian matrix to an algorithm that solves least-squares problem to update current estimate of the N refined poses and refined parameters of the M planar features.
7. The method of claim 1 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having reduced rows comprises
computing reduced residual blocks based at least in part on the matrices having reduced rows.
8. The method of claim 7 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having reduced rows comprises
stacking the reduced residual blocks to form a reduced residual vector.
9. The method of claim 8 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having reduced rows comprises
providing the reduced residual vector to an algorithm that solves least-squares problem to update current estimate of the N refined poses and refined parameters of the M planar features.
10. The method of claim 1 , wherein for each of the M planar features at each pose corresponding to an image of the plurality of images comprising one or more observations of the planar feature, computing the matrix indicating the one or more observations of the planar feature comprises
for each of the one or more observations of the planar feature, computing a matrix block indicating said observation, and
stacking the matrix blocks into the matrix indicating the one or more observations of the planar feature.
11. A method of operating a computing system to generate a representation of an environment, the method comprising:
obtaining sensor captured information, the sensor captured information comprising a plurality of images;
providing an initial representation of the environment, the initial representation comprising N initial poses and initial parameters of M planar features based at least in part on the plurality of images, the initial parameters of the M planar features indicating normals of planes represented by the M planar features;
for each of the M planar features at each pose corresponding to an image of the plurality of images comprising one or more observations of the planar feature:
computing a matrix having P rows, P being less than a number of the one or more observations of the planar feature; and
computing N refined poses and refined parameters of the M planar features by jointly adjusting the N initial poses and the initial parameters of the M planar features based at least in part on the matrices having P rows,
wherein the representation of the environment comprises the N refined poses and the refined parameters of the M planar features.
12. The method of claim 11 , wherein for each of the M planar features at each pose corresponding to an image of the plurality of images comprising one or more observations of the planar feature, computing the matrix having P rows comprises
computing a matrix indicating the one or more observations of the planar feature, and
factorizing the matrix into two or more matrices, the two or more matrices comprising the matrix having P rows.
13. The method of claim 12 , wherein factorizing the matrix into two or more matrices comprises computing an orthogonal matrix and an upper triangular matrix.
14. The method of claim 12 , wherein for each of the M planar features at each pose corresponding to an image of the plurality of images comprising one or more observations of the planar feature, computing the matrix indicating the one or more observations of the planar feature comprises
for each of the one or more observations of the planar feature, computing a matrix block indicating said observation, and
stacking the matrix blocks into the matrix indicating the one or more observations of the planar feature.
15. The method of claim 11 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having P rows comprises
computing reduced Jacobian matrix blocks based at least in part on the matrices P rows.
16. The method of claim 15 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices P rows comprises
stacking the reduced Jacobian matrix blocks to form a reduced Jacobian matrix.
17. The method of claim 16 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having P rows comprises
providing the reduced Jacobian matrix to an algorithm that solves least-squares problem to update current estimate of the N refined poses and refined parameters of the M planar features.
18. The method of claim 11 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having P rows comprises
computing reduced residual blocks based at least in part on the matrices having P rows.
19. The method of claim 18 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having P rows comprises
stacking the reduced residual blocks to form a reduced residual vector.
20. The method of claim 19 , wherein computing the N refined poses and refined parameters of the M planar features based at least in part on the matrices having P rows comprises
providing the reduced residual vector to an algorithm that solves least-squares problem to update current estimate of the N refined poses and refined parameters of the M planar features.