From c969f18fba1a073757271e5da104e4f8b83f332e Mon Sep 17 00:00:00 2001 From: David OK Date: Thu, 11 Jan 2024 21:49:22 +0000 Subject: [PATCH 01/49] ENH: show the compiled statement by Halide. --- doc/book/random/separable_conv_2d.stmt.html | 19874 ++++++++++++++++++ doc/book/random/vector_intrinsics.Rmd | 10 + 2 files changed, 19884 insertions(+) create mode 100644 doc/book/random/separable_conv_2d.stmt.html diff --git a/doc/book/random/separable_conv_2d.stmt.html b/doc/book/random/separable_conv_2d.stmt.html new file mode 100644 index 000000000..3c011333f --- /dev/null +++ b/doc/book/random/separable_conv_2d.stmt.html @@ -0,0 +1,19874 @@ + + +Visualizing Module: conv_y + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
module name=conv_y, target=x86-64-linux-avx-avx2-f16c-fma-sse41 {
func conv_y_par_for_conv_y_s0_x_xo_tile(__user_context, conv_y.s0.x.xo.tile, closure_arg){
Op Count: 1
Bits Moved: 0

let closure_prototype = make_struct((void *)((uint64)0), (void *)((uint64)0), 0, 0, 0, 0, 0, 0, 0)

Op Count: 1
Bits Moved: 0

let conv_y = load_typed_struct_member(closure_arg, closure_prototype, 0)

Op Count: 1
Bits Moved: 0

let kernel = load_typed_struct_member(closure_arg, closure_prototype, 1)

Op Count: 1
Bits Moved: 0

let conv_y.extent.0 = load_typed_struct_member(closure_arg, closure_prototype, 2)

Op Count: 1
Bits Moved: 0

let conv_y.extent.1 = load_typed_struct_member(closure_arg, closure_prototype, 3)

Op Count: 1
Bits Moved: 0

let conv_y.min.1 = load_typed_struct_member(closure_arg, closure_prototype, 4)

Op Count: 1
Bits Moved: 0

let conv_y.stride.1 = load_typed_struct_member(closure_arg, closure_prototype, 5)

Op Count: 1
Bits Moved: 0

let t139 = load_typed_struct_member(closure_arg, closure_prototype, 6)

Op Count: 1
Bits Moved: 0

let t140 = load_typed_struct_member(closure_arg, closure_prototype, 7)

Op Count: 1
Bits Moved: 0

let t141 = load_typed_struct_member(closure_arg, closure_prototype, 8)

Op Count: 4
Bits Moved: 0

let conv_y.s0.x.xi.base.s = min(((conv_y.s0.x.xo.tile % t139) * 64), (conv_y.extent.0 + -64))

Op Count: 4
Bits Moved: 0

let conv_y.s0.y.yi.base.s = min(((conv_y.s0.x.xo.tile / t139) * 64), (conv_y.extent.1 + -64))

Op Count: 2
Bits Moved: 0

let t142 = ((conv_y.s0.x.xi.base.s + t140) + conv_y.s0.y.yi.base.s)

Op Count: 1
Bits Moved: 0

let t143 = (conv_y.s0.x.xi.base.s + t141)

Op Count: 1
Bits Moved: 0

let t144 = (conv_y.min.1 + conv_y.s0.y.yi.base.s)

Op Count: 0
Bits Moved: 0
for (conv_y.s0.y.yi, 0, 64){
Op Count: 3
Bits Moved: 0

let t146 = (((conv_y.s0.y.yi + t144) * conv_y.stride.1) + t143)

Op Count: 1
Bits Moved: 0

let t145 = (conv_y.s0.y.yi + t142)

Op Count: 0
Bits Moved: 0
for (conv_y.s0.x.xi.xi, 0, 16){
Op Count: 0
Bits Moved: 0
allocate conv_x[float32 * 4 * 20]
Op Count: 0
Bits Moved: 0
produce conv_x{
Op Count: 2
Bits Moved: 0

let t147 = ((conv_y.s0.x.xi.xi * 4) + t145)

Op Count: 0
Bits Moved: 0
for (conv_x.s0.y.rebased, 0, 20){
Op Count: 0
Bits Moved: 0
allocate conv_x$1[float32 * 4]
Op Count: 0
Bits Moved: 0
produce conv_x$1{
Op Count: 2
Bits Moved: 128
conv_x$1[ramp(0, 1, 4)] = x4(0.000000f)
Op Count: 1
Bits Moved: 0

let t148 = (conv_x.s0.y.rebased + t147)

Op Count: 0
Bits Moved: 0
for (conv_x$1.s1.k$x, 0, 20){
Op Count: 17
Bits Moved: 288
conv_x$1[ramp(0, 1, 4)] = (conv_x$1[ramp(0, 1, 4)] + (float32x4(ramp(((conv_x$1.s1.k$x + t148) + -20), 1, 4)) * x4(kernel[conv_x$1.s1.k$x])))
}
}
Op Count: 0
Bits Moved: 0
consume conv_x$1{
Op Count: 3
Bits Moved: 256
conv_x[ramp((conv_x.s0.y.rebased * 4), 1, 4)] = conv_x$1[ramp(0, 1, 4)]
}
Op Count: 0
Bits Moved: 0
free conv_x$1
}
}
Op Count: 0
Bits Moved: 0
consume conv_x{
Op Count: 0
Bits Moved: 0
allocate conv_y$1[float32 * 4]
Op Count: 0
Bits Moved: 0
produce conv_y$1{
Op Count: 2
Bits Moved: 128
conv_y$1[ramp(0, 1, 4)] = x4(0.000000f)
Op Count: 0
Bits Moved: 0
for (conv_y$1.s1.k$x, 0, 20){
Op Count: 13
Bits Moved: 416
conv_y$1[ramp(0, 1, 4)] = (conv_y$1[ramp(0, 1, 4)] + (conv_x[ramp((conv_y$1.s1.k$x * 4), 1, 4)] * x4(kernel[conv_y$1.s1.k$x])))
}
Op Count: 0
Bits Moved: 0
free conv_x
}
Op Count: 0
Bits Moved: 0
consume conv_y$1{
Op Count: 4
Bits Moved: 256
conv_y[ramp(((conv_y.s0.x.xi.xi * 4) + t146), 1, 4)] = conv_y$1[ramp(0, 1, 4)]
}
Op Count: 0
Bits Moved: 0
free conv_y$1
}
}
}
}
func conv_y(conv_y){
Op Count: 2
Bits Moved: 0
assert((uint64(conv_y.buffer) != (uint64)0), halide_error_buffer_argument_is_null("conv_y"))
Op Count: 1
Bits Moved: 0

let conv_y = _halide_buffer_get_host(conv_y.buffer)

Op Count: 1
Bits Moved: 0

let conv_y.type = _halide_buffer_get_type(conv_y.buffer)

Op Count: 1
Bits Moved: 0

let conv_y.device_dirty = _halide_buffer_get_device_dirty(conv_y.buffer)

Op Count: 1
Bits Moved: 0

let conv_y.dimensions = _halide_buffer_get_dimensions(conv_y.buffer)

Op Count: 1
Bits Moved: 0

let conv_y.min.0 = _halide_buffer_get_min(conv_y.buffer, 0)

Op Count: 1
Bits Moved: 0

let conv_y.extent.0 = _halide_buffer_get_extent(conv_y.buffer, 0)

Op Count: 1
Bits Moved: 0

let conv_y.stride.0 = _halide_buffer_get_stride(conv_y.buffer, 0)

Op Count: 1
Bits Moved: 0

let conv_y.min.1 = _halide_buffer_get_min(conv_y.buffer, 1)

Op Count: 1
Bits Moved: 0

let conv_y.extent.1 = _halide_buffer_get_extent(conv_y.buffer, 1)

Op Count: 1
Bits Moved: 0

let conv_y.stride.1 = _halide_buffer_get_stride(conv_y.buffer, 1)

Op Count: 8
Bits Moved: 0

let conv_y.extent.0.required.s = min((max((max((-64 - conv_y.extent.0), (conv_y.extent.0 + -1)) / 64), 0) * 64), (conv_y.extent.0 + -64))

Op Count: 19
Bits Moved: 0

let conv_y.extent.1.required.s = (let t149 = (0 < conv_y.extent.0) in (let t150 = (((((conv_y.extent.0 + 63) / 64) * ((conv_y.extent.1 + 63) / 64)) + -1) / ((conv_y.extent.0 + 63) / 64)) in (min((select(t149, t150, 0) * 64), (conv_y.extent.1 + -64)) - min((select(t149, 0, t150) * 64), (conv_y.extent.1 + -64)))))

Op Count: 14
Bits Moved: 0

let conv_y.min.1.required.s = min((select((0 < conv_y.extent.0), 0, (((((conv_y.extent.0 + 63) / 64) * ((conv_y.extent.1 + 63) / 64)) + -1) / ((conv_y.extent.0 + 63) / 64))) * 64), (conv_y.extent.1 + -64))

Op Count: 2
Bits Moved: 0
if (_halide_buffer_is_bounds_query(conv_y.buffer)) {
Op Count: 11
Bits Moved: 0
(let t151 = max(conv_y.extent.0.required.s, 0) in _halide_buffer_init(conv_y.buffer, _halide_buffer_get_shape(conv_y.buffer), (void *)((uint64)0), (uint64)0, (struct halide_device_interface_t *)((uint64)0), 2, 32, 2, make_struct(((min(conv_y.extent.0, 64) + conv_y.min.0) + -64), (t151 + 64), 1, 0, (conv_y.min.1 + conv_y.min.1.required.s), (conv_y.extent.1.required.s + 64), (t151 + 64), 0), (uint64)0))
}
Op Count: 3
Bits Moved: 0
if (!_halide_buffer_is_bounds_query(conv_y.buffer)) {
Op Count: 2
Bits Moved: 0
assert((conv_y.type == (uint32)73730), halide_error_bad_type("Output buffer conv_y", conv_y.type, (uint32)73730))
Op Count: 2
Bits Moved: 0
assert((conv_y.dimensions == 2), halide_error_bad_dimensions("Output buffer conv_y", conv_y.dimensions, 2))
Op Count: 6
Bits Moved: 0
assert((max((max(conv_y.extent.0.required.s, 0) + min(conv_y.extent.0, 64)), 64) <= conv_y.extent.0), (let t152 = min(conv_y.extent.0, 64) in halide_error_access_out_of_bounds("Output buffer conv_y", 0, ((t152 + conv_y.min.0) + -64), (((max(conv_y.extent.0.required.s, 0) + t152) + conv_y.min.0) + -1), conv_y.min.0, ((conv_y.extent.0 + conv_y.min.0) + -1))))
Op Count: 8
Bits Moved: 0
assert(((0 <= conv_y.min.1.required.s) && ((((conv_y.min.1 + conv_y.min.1.required.s) + conv_y.extent.1.required.s) + 64) <= (conv_y.extent.1 + conv_y.min.1))), (let t153 = (conv_y.min.1 + conv_y.min.1.required.s) in halide_error_access_out_of_bounds("Output buffer conv_y", 1, t153, ((t153 + conv_y.extent.1.required.s) + 63), conv_y.min.1, ((conv_y.extent.1 + conv_y.min.1) + -1))))
Op Count: 2
Bits Moved: 0
assert((0 <= conv_y.extent.1), halide_error_buffer_extents_negative("Output buffer conv_y", 1, conv_y.extent.1))
Op Count: 2
Bits Moved: 0
assert((conv_y.stride.0 == 1), halide_error_constraint_violated("conv_y.stride.0", conv_y.stride.0, "1", 1))
Op Count: 3
Bits Moved: 0

let conv_y.total_extent.1 = (int64(conv_y.extent.1) * int64(conv_y.extent.0))

Op Count: 3
Bits Moved: 0
assert((uint64(conv_y.extent.0) <= (uint64)2147483647), halide_error_buffer_allocation_too_large("conv_y", uint64(conv_y.extent.0), (uint64)2147483647))
Op Count: 6
Bits Moved: 0
assert((abs((int64(conv_y.extent.1) * int64(conv_y.stride.1))) <= (uint64)2147483647), halide_error_buffer_allocation_too_large("conv_y", abs((int64(conv_y.extent.1) * int64(conv_y.stride.1))), (uint64)2147483647))
Op Count: 2
Bits Moved: 0
assert((conv_y.total_extent.1 <= (int64)2147483647), halide_error_buffer_extents_too_large("conv_y", conv_y.total_extent.1, (int64)2147483647))
Op Count: 2
Bits Moved: 0
assert(!conv_y.device_dirty, halide_error_device_dirty_with_no_device_support("Output buffer conv_y"))
Op Count: 2
Bits Moved: 0
assert((conv_y != (void *)((uint64)0)), halide_error_host_is_null("Output buffer conv_y"))
Op Count: 0
Bits Moved: 0
allocate kernel[float32 * 20]
Op Count: 0
Bits Moved: 0
produce kernel{
Op Count: 0
Bits Moved: 0
for (kernel.s0.x, 0, 20){
Op Count: 3
Bits Moved: 32
kernel[kernel.s0.x] = exp_f32(float32((0 - kernel.s0.x)))
}
}
Op Count: 5
Bits Moved: 0

let conv_y.s0.x.xo.tile.loop_extent = (((conv_y.extent.0 + 63) / 64) * ((conv_y.extent.1 + 63) / 64))

Op Count: 0
Bits Moved: 0
produce conv_y{
Op Count: 0
Bits Moved: 0
consume kernel{
Op Count: 2
Bits Moved: 0

let t139 = ((conv_y.extent.0 + 63) / 64)

Op Count: 2
Bits Moved: 0

let t141 = (0 - (conv_y.min.1 * conv_y.stride.1))

Op Count: 1
Bits Moved: 0

let t140 = (conv_y.min.0 + conv_y.min.1)

Op Count: 1
Bits Moved: 0

let parallel_closure = make_struct(conv_y, kernel, conv_y.extent.0, conv_y.extent.1, conv_y.min.1, conv_y.stride.1, t139, t140, t141)

Op Count: 2
Bits Moved: 0

let closure_result = halide_do_par_for(::conv_y_par_for_conv_y_s0_x_xo_tile, 0, conv_y.s0.x.xo.tile.loop_extent, (uint8_t *)(parallel_closure))

Op Count: 2
Bits Moved: 0
assert((closure_result == 0), closure_result)
}
}
Op Count: 0
Bits Moved: 0
free kernel
}
}
}
+
+
+
+ +
+
+ +
+
+
+
Func: conv_y_par_for_conv_y_s0_x_xo_tile
For: conv_y.s0.y.yi
Op Count: 0
Bits Moved: 0
Min0
Extent64
For: conv_y.s0.x.xi.xi
Op Count: 0
Bits Moved: 0
Min0
Extent16
Allocate: conv_x
Op Count: 0
Bits Moved: 0
Memory TypeAuto
Data Typefloat32
Dim-04
Dim-120
Produce: conv_x
Op Count: 0
Bits Moved: 0
For: conv_x.s0.y.rebased
Op Count: 0
Bits Moved: 0
Min0
Extent20
Allocate: conv_x$1
Op Count: 0
Bits Moved: 0
Memory TypeAuto
Data Typefloat32
Dim-04
Produce: conv_x$1
Op Count: 0
Bits Moved: 0
Store: conv_x$1
Op Count: 2
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
For: conv_x$1.s1.k$x
Op Count: 0
Bits Moved: 0
Min0
Extent20
Load: conv_x$1
Op Count: 1
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
Load: kernel
Op Count: 0
Bits Moved: 32
TypeScalar
Outputfloat32
Store: conv_x$1
Op Count: 17
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
Consume: conv_x$1
Op Count: 0
Bits Moved: 0
Load: conv_x$1
Op Count: 1
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
Store: conv_x
Op Count: 3
Bits Moved: 128
Alignmentaligned(4, 0)
TypeDense Vector
Output Tilefloat32x4
Consume: conv_x
Op Count: 0
Bits Moved: 0
Allocate: conv_y$1
Op Count: 0
Bits Moved: 0
Memory TypeAuto
Data Typefloat32
Dim-04
Produce: conv_y$1
Op Count: 0
Bits Moved: 0
Store: conv_y$1
Op Count: 2
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
For: conv_y$1.s1.k$x
Op Count: 0
Bits Moved: 0
Min0
Extent20
Load: conv_y$1
Op Count: 1
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
Load: conv_x
Op Count: 2
Bits Moved: 128
Alignmentaligned(4, 0)
TypeDense Vector
Output Tilefloat32x4
Load: kernel
Op Count: 0
Bits Moved: 32
TypeScalar
Outputfloat32
Store: conv_y$1
Op Count: 13
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
Consume: conv_y$1
Op Count: 0
Bits Moved: 0
Load: conv_y$1
Op Count: 1
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
Store: conv_y
Op Count: 4
Bits Moved: 128
TypeDense Vector
Output Tilefloat32x4
Func: conv_y
  • If: _halide_buffer_is_bounds_query(conv_y.buffer)
    Op Count: 11
    Bits Moved: 0
    _halide_buffer_init(...)
  • If: !_halide_buffer_is_bounds_query(conv_y.buffer)
    Op Count: 0
    Bits Moved: 0
    Allocate: kernel
    Op Count: 0
    Bits Moved: 0
    Memory TypeAuto
    Data Typefloat32
    Dim-020
    Produce: kernel
    Op Count: 0
    Bits Moved: 0
    For: kernel.s0.x
    Op Count: 0
    Bits Moved: 0
    Min0
    Extent20
    Store: kernel
    Op Count: 3
    Bits Moved: 32
    TypeScalar
    Outputfloat32
    Produce: conv_y
    Op Count: 0
    Bits Moved: 0
    Consume: kernel
    Op Count: 0
    Bits Moved: 0
    halide_do_par_for(...)
+
+
+
+ +
+
+ +
+
+
+ +
+
+
+ \ No newline at end of file diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index 78f5bf4ab..2f5bed0a0 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -453,6 +453,16 @@ After a bit of inspection, by reinstating progressively all the different parallelisms, we start to understand how the parallel patterns are implemented on assembly code. +This is how it looks like. + + +It is not practical to view the embedded HTML page, so go to this link below to fully explore the +compiled statement as a full page: + + ### Vectorizing in the Convolution Eventually, this was not so hard and this gives a very tangible hands-on From c17f01c78f02bd130a50100a28636ceaa13e6ab4 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Fri, 12 Jan 2024 20:53:08 +0000 Subject: [PATCH 02/49] DOC: fix typo. --- doc/book/random/bilinear_interpolation.Rmd | 12 ++++++------ doc/book/random/python_code_that_suck_less.Rmd | 16 ++++++++-------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/doc/book/random/bilinear_interpolation.Rmd b/doc/book/random/bilinear_interpolation.Rmd index e0d9299e4..57b4d4fb9 100644 --- a/doc/book/random/bilinear_interpolation.Rmd +++ b/doc/book/random/bilinear_interpolation.Rmd @@ -10,12 +10,12 @@ This is a technical exercise I do find interesting because it helps me to know whether someone can fix the technical insides of an existing pipeline. It's not simple but it's not that hard either. -Eventually I lowered my expectations by just asking to complete a simple machine -learning training pipeline. And everybody just jumped their guns and were happy -to impress you about how much they know about the bleeding edge neural -architecture and what not.^[The world never changes... Just shows how people -most of the time likes to mansplain by displaying their knowledge more than -their ability to think. So much ego all in all...] +Eventually I lowered my expectations by just asking to complete applicants a +simple machine learning training pipeline. And everybody just jumped their guns +and were happy to impress you about how much they know about the bleeding edge +neural architecture and what not.^[The world never changes... Just shows how +people most of the time likes to mansplain by displaying their knowledge more +than their ability to think. So much ego all in all...] Let's see how we can do this in Python in a NumPy-like style. I leave it as an exercise in C++. diff --git a/doc/book/random/python_code_that_suck_less.Rmd b/doc/book/random/python_code_that_suck_less.Rmd index 4d81825cf..976e3b7a3 100644 --- a/doc/book/random/python_code_that_suck_less.Rmd +++ b/doc/book/random/python_code_that_suck_less.Rmd @@ -108,8 +108,8 @@ Non-Maximum Suppression (NMS) algorithm. ### The algorithm described -The NMS aims at filtering the best object boxes. The best object boxes must have -the highest objectness score and each one of them can't overlap too much with +The NMS aims at keeping the best object boxes. The best object boxes are those +with the highest objectness score and each one of them overlap very little with each other. The objectness score is the probability that the object box contains an object @@ -120,13 +120,13 @@ according to this definition.] objectness score. - Then we process greedily by - 1. first keeping the first object box of the list, since it has the highest object - box. + 1. first keeping the first object box of the list, since it has the highest + objectness score. 2. Next we examine the second best object box and we keep if it does not - overlap with the first box or if it overlaps, it should overlap very - little. A good metric is the ratio of the **intersection** area between the - first box and second box *over the union* area, the so-called IoU score. + overlap with the first box or if it overlaps, it should do so very little. + A good metric is the ratio of the **intersection** area between the first + box and second box **over the union** area, the so-called IoU score. Let's imagine the second box does not overlap with the first one. @@ -327,7 +327,7 @@ This is aesthetically better and more meaningful than ```{cpp} const auto& xijk = ...; // too crammed -// The following is fine too as typing the underscore symbol can be a hassle. +// The following is fine as typing the underscore symbol can be a hassle. // I am not a snake_case zealot either... const auto xi = x[i]; ``` From d4d54cd13d89d527643ebb3de1469ff946a776bf Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sat, 20 Jan 2024 01:52:11 +0000 Subject: [PATCH 03/49] Bump vite from 4.3.9 to 4.5.2 in /svelte/sara-app Bumps [vite](https://github.com/vitejs/vite/tree/HEAD/packages/vite) from 4.3.9 to 4.5.2. - [Release notes](https://github.com/vitejs/vite/releases) - [Changelog](https://github.com/vitejs/vite/blob/v4.5.2/packages/vite/CHANGELOG.md) - [Commits](https://github.com/vitejs/vite/commits/v4.5.2/packages/vite) --- updated-dependencies: - dependency-name: vite dependency-type: direct:development ... Signed-off-by: dependabot[bot] --- svelte/sara-app/package.json | 2 +- svelte/sara-app/pnpm-lock.yaml | 183 +++++++++++++++++---------------- 2 files changed, 94 insertions(+), 91 deletions(-) diff --git a/svelte/sara-app/package.json b/svelte/sara-app/package.json index a0cdf2925..4c2fdbaf8 100644 --- a/svelte/sara-app/package.json +++ b/svelte/sara-app/package.json @@ -20,7 +20,7 @@ "svelte-check": "^3.0.1", "tslib": "^2.4.1", "typescript": "^5.0.0", - "vite": "^4.3.0", + "vite": "^4.5.2", "autoprefixer": "^10.4.14", "daisyui": "^3.0.2", "postcss": "^8.4.31", diff --git a/svelte/sara-app/pnpm-lock.yaml b/svelte/sara-app/pnpm-lock.yaml index d2b1e2850..5885f4d62 100644 --- a/svelte/sara-app/pnpm-lock.yaml +++ b/svelte/sara-app/pnpm-lock.yaml @@ -10,7 +10,7 @@ devDependencies: version: 2.1.0(@sveltejs/kit@1.20.2) '@sveltejs/kit': specifier: ^1.5.0 - version: 1.20.2(svelte@3.59.1)(vite@4.3.9) + version: 1.20.2(svelte@3.59.1)(vite@4.5.2) autoprefixer: specifier: ^10.4.14 version: 10.4.14(postcss@8.4.31) @@ -42,8 +42,8 @@ devDependencies: specifier: ^5.0.0 version: 5.1.3 vite: - specifier: ^4.3.0 - version: 4.3.9 + specifier: ^4.5.2 + version: 4.5.2 packages: @@ -52,8 +52,8 @@ packages: engines: {node: '>=10'} dev: true - /@esbuild/android-arm64@0.17.19: - resolution: {integrity: sha512-KBMWvEZooR7+kzY0BtbTQn0OAYY7CsiydT63pVEaPtVYF0hXbUaOyZog37DKxK7NF3XacBJOpYT4adIJh+avxA==} + /@esbuild/android-arm64@0.18.20: + resolution: {integrity: sha512-Nz4rJcchGDtENV0eMKUNa6L12zz2zBDXuhj/Vjh18zGqB44Bi7MBMSXjgunJgjRhCmKOjnPuZp4Mb6OKqtMHLQ==} engines: {node: '>=12'} cpu: [arm64] os: [android] @@ -61,8 +61,8 @@ packages: dev: true optional: true - /@esbuild/android-arm@0.17.19: - resolution: {integrity: sha512-rIKddzqhmav7MSmoFCmDIb6e2W57geRsM94gV2l38fzhXMwq7hZoClug9USI2pFRGL06f4IOPHHpFNOkWieR8A==} + /@esbuild/android-arm@0.18.20: + resolution: {integrity: sha512-fyi7TDI/ijKKNZTUJAQqiG5T7YjJXgnzkURqmGj13C6dCqckZBLdl4h7bkhHt/t0WP+zO9/zwroDvANaOqO5Sw==} engines: {node: '>=12'} cpu: [arm] os: [android] @@ -70,8 +70,8 @@ packages: dev: true optional: true - /@esbuild/android-x64@0.17.19: - resolution: {integrity: sha512-uUTTc4xGNDT7YSArp/zbtmbhO0uEEK9/ETW29Wk1thYUJBz3IVnvgEiEwEa9IeLyvnpKrWK64Utw2bgUmDveww==} + /@esbuild/android-x64@0.18.20: + resolution: {integrity: sha512-8GDdlePJA8D6zlZYJV/jnrRAi6rOiNaCC/JclcXpB+KIuvfBN4owLtgzY2bsxnx666XjJx2kDPUmnTtR8qKQUg==} engines: {node: '>=12'} cpu: [x64] os: [android] @@ -79,8 +79,8 @@ packages: dev: true optional: true - /@esbuild/darwin-arm64@0.17.19: - resolution: {integrity: sha512-80wEoCfF/hFKM6WE1FyBHc9SfUblloAWx6FJkFWTWiCoht9Mc0ARGEM47e67W9rI09YoUxJL68WHfDRYEAvOhg==} + /@esbuild/darwin-arm64@0.18.20: + resolution: {integrity: sha512-bxRHW5kHU38zS2lPTPOyuyTm+S+eobPUnTNkdJEfAddYgEcll4xkT8DB9d2008DtTbl7uJag2HuE5NZAZgnNEA==} engines: {node: '>=12'} cpu: [arm64] os: [darwin] @@ -88,8 +88,8 @@ packages: dev: true optional: true - /@esbuild/darwin-x64@0.17.19: - resolution: {integrity: sha512-IJM4JJsLhRYr9xdtLytPLSH9k/oxR3boaUIYiHkAawtwNOXKE8KoU8tMvryogdcT8AU+Bflmh81Xn6Q0vTZbQw==} + /@esbuild/darwin-x64@0.18.20: + resolution: {integrity: sha512-pc5gxlMDxzm513qPGbCbDukOdsGtKhfxD1zJKXjCCcU7ju50O7MeAZ8c4krSJcOIJGFR+qx21yMMVYwiQvyTyQ==} engines: {node: '>=12'} cpu: [x64] os: [darwin] @@ -97,8 +97,8 @@ packages: dev: true optional: true - /@esbuild/freebsd-arm64@0.17.19: - resolution: {integrity: sha512-pBwbc7DufluUeGdjSU5Si+P3SoMF5DQ/F/UmTSb8HXO80ZEAJmrykPyzo1IfNbAoaqw48YRpv8shwd1NoI0jcQ==} + /@esbuild/freebsd-arm64@0.18.20: + resolution: {integrity: sha512-yqDQHy4QHevpMAaxhhIwYPMv1NECwOvIpGCZkECn8w2WFHXjEwrBn3CeNIYsibZ/iZEUemj++M26W3cNR5h+Tw==} engines: {node: '>=12'} cpu: [arm64] os: [freebsd] @@ -106,8 +106,8 @@ packages: dev: true optional: true - /@esbuild/freebsd-x64@0.17.19: - resolution: {integrity: sha512-4lu+n8Wk0XlajEhbEffdy2xy53dpR06SlzvhGByyg36qJw6Kpfk7cp45DR/62aPH9mtJRmIyrXAS5UWBrJT6TQ==} + /@esbuild/freebsd-x64@0.18.20: + resolution: {integrity: sha512-tgWRPPuQsd3RmBZwarGVHZQvtzfEBOreNuxEMKFcd5DaDn2PbBxfwLcj4+aenoh7ctXcbXmOQIn8HI6mCSw5MQ==} engines: {node: '>=12'} cpu: [x64] os: [freebsd] @@ -115,8 +115,8 @@ packages: dev: true optional: true - /@esbuild/linux-arm64@0.17.19: - resolution: {integrity: sha512-ct1Tg3WGwd3P+oZYqic+YZF4snNl2bsnMKRkb3ozHmnM0dGWuxcPTTntAF6bOP0Sp4x0PjSF+4uHQ1xvxfRKqg==} + /@esbuild/linux-arm64@0.18.20: + resolution: {integrity: sha512-2YbscF+UL7SQAVIpnWvYwM+3LskyDmPhe31pE7/aoTMFKKzIc9lLbyGUpmmb8a8AixOL61sQ/mFh3jEjHYFvdA==} engines: {node: '>=12'} cpu: [arm64] os: [linux] @@ -124,8 +124,8 @@ packages: dev: true optional: true - /@esbuild/linux-arm@0.17.19: - resolution: {integrity: sha512-cdmT3KxjlOQ/gZ2cjfrQOtmhG4HJs6hhvm3mWSRDPtZ/lP5oe8FWceS10JaSJC13GBd4eH/haHnqf7hhGNLerA==} + /@esbuild/linux-arm@0.18.20: + resolution: {integrity: sha512-/5bHkMWnq1EgKr1V+Ybz3s1hWXok7mDFUMQ4cG10AfW3wL02PSZi5kFpYKrptDsgb2WAJIvRcDm+qIvXf/apvg==} engines: {node: '>=12'} cpu: [arm] os: [linux] @@ -133,8 +133,8 @@ packages: dev: true optional: true - /@esbuild/linux-ia32@0.17.19: - resolution: {integrity: sha512-w4IRhSy1VbsNxHRQpeGCHEmibqdTUx61Vc38APcsRbuVgK0OPEnQ0YD39Brymn96mOx48Y2laBQGqgZ0j9w6SQ==} + /@esbuild/linux-ia32@0.18.20: + resolution: {integrity: sha512-P4etWwq6IsReT0E1KHU40bOnzMHoH73aXp96Fs8TIT6z9Hu8G6+0SHSw9i2isWrD2nbx2qo5yUqACgdfVGx7TA==} engines: {node: '>=12'} cpu: [ia32] os: [linux] @@ -142,8 +142,8 @@ packages: dev: true optional: true - /@esbuild/linux-loong64@0.17.19: - resolution: {integrity: sha512-2iAngUbBPMq439a+z//gE+9WBldoMp1s5GWsUSgqHLzLJ9WoZLZhpwWuym0u0u/4XmZ3gpHmzV84PonE+9IIdQ==} + /@esbuild/linux-loong64@0.18.20: + resolution: {integrity: sha512-nXW8nqBTrOpDLPgPY9uV+/1DjxoQ7DoB2N8eocyq8I9XuqJ7BiAMDMf9n1xZM9TgW0J8zrquIb/A7s3BJv7rjg==} engines: {node: '>=12'} cpu: [loong64] os: [linux] @@ -151,8 +151,8 @@ packages: dev: true optional: true - /@esbuild/linux-mips64el@0.17.19: - resolution: {integrity: sha512-LKJltc4LVdMKHsrFe4MGNPp0hqDFA1Wpt3jE1gEyM3nKUvOiO//9PheZZHfYRfYl6AwdTH4aTcXSqBerX0ml4A==} + /@esbuild/linux-mips64el@0.18.20: + resolution: {integrity: sha512-d5NeaXZcHp8PzYy5VnXV3VSd2D328Zb+9dEq5HE6bw6+N86JVPExrA6O68OPwobntbNJ0pzCpUFZTo3w0GyetQ==} engines: {node: '>=12'} cpu: [mips64el] os: [linux] @@ -160,8 +160,8 @@ packages: dev: true optional: true - /@esbuild/linux-ppc64@0.17.19: - resolution: {integrity: sha512-/c/DGybs95WXNS8y3Ti/ytqETiW7EU44MEKuCAcpPto3YjQbyK3IQVKfF6nbghD7EcLUGl0NbiL5Rt5DMhn5tg==} + /@esbuild/linux-ppc64@0.18.20: + resolution: {integrity: sha512-WHPyeScRNcmANnLQkq6AfyXRFr5D6N2sKgkFo2FqguP44Nw2eyDlbTdZwd9GYk98DZG9QItIiTlFLHJHjxP3FA==} engines: {node: '>=12'} cpu: [ppc64] os: [linux] @@ -169,8 +169,8 @@ packages: dev: true optional: true - /@esbuild/linux-riscv64@0.17.19: - resolution: {integrity: sha512-FC3nUAWhvFoutlhAkgHf8f5HwFWUL6bYdvLc/TTuxKlvLi3+pPzdZiFKSWz/PF30TB1K19SuCxDTI5KcqASJqA==} + /@esbuild/linux-riscv64@0.18.20: + resolution: {integrity: sha512-WSxo6h5ecI5XH34KC7w5veNnKkju3zBRLEQNY7mv5mtBmrP/MjNBCAlsM2u5hDBlS3NGcTQpoBvRzqBcRtpq1A==} engines: {node: '>=12'} cpu: [riscv64] os: [linux] @@ -178,8 +178,8 @@ packages: dev: true optional: true - /@esbuild/linux-s390x@0.17.19: - resolution: {integrity: sha512-IbFsFbxMWLuKEbH+7sTkKzL6NJmG2vRyy6K7JJo55w+8xDk7RElYn6xvXtDW8HCfoKBFK69f3pgBJSUSQPr+4Q==} + /@esbuild/linux-s390x@0.18.20: + resolution: {integrity: sha512-+8231GMs3mAEth6Ja1iK0a1sQ3ohfcpzpRLH8uuc5/KVDFneH6jtAJLFGafpzpMRO6DzJ6AvXKze9LfFMrIHVQ==} engines: {node: '>=12'} cpu: [s390x] os: [linux] @@ -187,8 +187,8 @@ packages: dev: true optional: true - /@esbuild/linux-x64@0.17.19: - resolution: {integrity: sha512-68ngA9lg2H6zkZcyp22tsVt38mlhWde8l3eJLWkyLrp4HwMUr3c1s/M2t7+kHIhvMjglIBrFpncX1SzMckomGw==} + /@esbuild/linux-x64@0.18.20: + resolution: {integrity: sha512-UYqiqemphJcNsFEskc73jQ7B9jgwjWrSayxawS6UVFZGWrAAtkzjxSqnoclCXxWtfwLdzU+vTpcNYhpn43uP1w==} engines: {node: '>=12'} cpu: [x64] os: [linux] @@ -196,8 +196,8 @@ packages: dev: true optional: true - /@esbuild/netbsd-x64@0.17.19: - resolution: {integrity: sha512-CwFq42rXCR8TYIjIfpXCbRX0rp1jo6cPIUPSaWwzbVI4aOfX96OXY8M6KNmtPcg7QjYeDmN+DD0Wp3LaBOLf4Q==} + /@esbuild/netbsd-x64@0.18.20: + resolution: {integrity: sha512-iO1c++VP6xUBUmltHZoMtCUdPlnPGdBom6IrO4gyKPFFVBKioIImVooR5I83nTew5UOYrk3gIJhbZh8X44y06A==} engines: {node: '>=12'} cpu: [x64] os: [netbsd] @@ -205,8 +205,8 @@ packages: dev: true optional: true - /@esbuild/openbsd-x64@0.17.19: - resolution: {integrity: sha512-cnq5brJYrSZ2CF6c35eCmviIN3k3RczmHz8eYaVlNasVqsNY+JKohZU5MKmaOI+KkllCdzOKKdPs762VCPC20g==} + /@esbuild/openbsd-x64@0.18.20: + resolution: {integrity: sha512-e5e4YSsuQfX4cxcygw/UCPIEP6wbIL+se3sxPdCiMbFLBWu0eiZOJ7WoD+ptCLrmjZBK1Wk7I6D/I3NglUGOxg==} engines: {node: '>=12'} cpu: [x64] os: [openbsd] @@ -214,8 +214,8 @@ packages: dev: true optional: true - /@esbuild/sunos-x64@0.17.19: - resolution: {integrity: sha512-vCRT7yP3zX+bKWFeP/zdS6SqdWB8OIpaRq/mbXQxTGHnIxspRtigpkUcDMlSCOejlHowLqII7K2JKevwyRP2rg==} + /@esbuild/sunos-x64@0.18.20: + resolution: {integrity: sha512-kDbFRFp0YpTQVVrqUd5FTYmWo45zGaXe0X8E1G/LKFC0v8x0vWrhOWSLITcCn63lmZIxfOMXtCfti/RxN/0wnQ==} engines: {node: '>=12'} cpu: [x64] os: [sunos] @@ -223,8 +223,8 @@ packages: dev: true optional: true - /@esbuild/win32-arm64@0.17.19: - resolution: {integrity: sha512-yYx+8jwowUstVdorcMdNlzklLYhPxjniHWFKgRqH7IFlUEa0Umu3KuYplf1HUZZ422e3NU9F4LGb+4O0Kdcaag==} + /@esbuild/win32-arm64@0.18.20: + resolution: {integrity: sha512-ddYFR6ItYgoaq4v4JmQQaAI5s7npztfV4Ag6NrhiaW0RrnOXqBkgwZLofVTlq1daVTQNhtI5oieTvkRPfZrePg==} engines: {node: '>=12'} cpu: [arm64] os: [win32] @@ -232,8 +232,8 @@ packages: dev: true optional: true - /@esbuild/win32-ia32@0.17.19: - resolution: {integrity: sha512-eggDKanJszUtCdlVs0RB+h35wNlb5v4TWEkq4vZcmVt5u/HiDZrTXe2bWFQUez3RgNHwx/x4sk5++4NSSicKkw==} + /@esbuild/win32-ia32@0.18.20: + resolution: {integrity: sha512-Wv7QBi3ID/rROT08SABTS7eV4hX26sVduqDOTe1MvGMjNd3EjOz4b7zeexIR62GTIEKrfJXKL9LFxTYgkyeu7g==} engines: {node: '>=12'} cpu: [ia32] os: [win32] @@ -241,8 +241,8 @@ packages: dev: true optional: true - /@esbuild/win32-x64@0.17.19: - resolution: {integrity: sha512-lAhycmKnVOuRYNtRtatQR1LPQf2oYCkRGkSFnseDAKPl8lu5SOsK/e1sXe5a0Pc5kHIHe6P2I/ilntNv2xf3cA==} + /@esbuild/win32-x64@0.18.20: + resolution: {integrity: sha512-kTdfRcSiDfQca/y9QIkng02avJ+NCaQvrMejlsB3RRv5sE9rRoeBPISaZpKxHELzRxZyLvNts1P27W3wV+8geQ==} engines: {node: '>=12'} cpu: [x64] os: [win32] @@ -314,11 +314,11 @@ packages: peerDependencies: '@sveltejs/kit': ^1.0.0 dependencies: - '@sveltejs/kit': 1.20.2(svelte@3.59.1)(vite@4.3.9) + '@sveltejs/kit': 1.20.2(svelte@3.59.1)(vite@4.5.2) import-meta-resolve: 3.0.0 dev: true - /@sveltejs/kit@1.20.2(svelte@3.59.1)(vite@4.3.9): + /@sveltejs/kit@1.20.2(svelte@3.59.1)(vite@4.5.2): resolution: {integrity: sha512-MtR1i+HtmYWcRgtubw1GQqT/+CWXL/z24PegE0xYAdObbhdr7YtEfmoe705D/JZMtMmoPXrmSk4W0MfL5A3lYw==} engines: {node: ^16.14 || >=18} hasBin: true @@ -327,7 +327,7 @@ packages: svelte: ^3.54.0 || ^4.0.0-next.0 vite: ^4.0.0 dependencies: - '@sveltejs/vite-plugin-svelte': 2.4.1(svelte@3.59.1)(vite@4.3.9) + '@sveltejs/vite-plugin-svelte': 2.4.1(svelte@3.59.1)(vite@4.5.2) '@types/cookie': 0.5.1 cookie: 0.5.0 devalue: 4.3.2 @@ -341,12 +341,12 @@ packages: svelte: 3.59.1 tiny-glob: 0.2.9 undici: 5.22.1 - vite: 4.3.9 + vite: 4.5.2 transitivePeerDependencies: - supports-color dev: true - /@sveltejs/vite-plugin-svelte-inspector@1.0.2(@sveltejs/vite-plugin-svelte@2.4.1)(svelte@3.59.1)(vite@4.3.9): + /@sveltejs/vite-plugin-svelte-inspector@1.0.2(@sveltejs/vite-plugin-svelte@2.4.1)(svelte@3.59.1)(vite@4.5.2): resolution: {integrity: sha512-Cy1dUMcYCnDVV/hPLXa43YZJ2jGKVW5rA0xuNL9dlmYhT0yoS1g7+FOFSRlgk0BXKk/Oc7grs+8BVA5Iz2fr8A==} engines: {node: ^14.18.0 || >= 16} peerDependencies: @@ -354,30 +354,30 @@ packages: svelte: ^3.54.0 || ^4.0.0-next.0 vite: ^4.0.0 dependencies: - '@sveltejs/vite-plugin-svelte': 2.4.1(svelte@3.59.1)(vite@4.3.9) + '@sveltejs/vite-plugin-svelte': 2.4.1(svelte@3.59.1)(vite@4.5.2) debug: 4.3.4 svelte: 3.59.1 - vite: 4.3.9 + vite: 4.5.2 transitivePeerDependencies: - supports-color dev: true - /@sveltejs/vite-plugin-svelte@2.4.1(svelte@3.59.1)(vite@4.3.9): + /@sveltejs/vite-plugin-svelte@2.4.1(svelte@3.59.1)(vite@4.5.2): resolution: {integrity: sha512-bNNKvoRY89ptY7udeBSCmTdCVwkjmMcZ0j/z9J5MuedT8jPjq0zrknAo/jF1sToAza4NVaAgR9AkZoD9oJJmnA==} engines: {node: ^14.18.0 || >= 16} peerDependencies: svelte: ^3.54.0 || ^4.0.0-next.0 vite: ^4.0.0 dependencies: - '@sveltejs/vite-plugin-svelte-inspector': 1.0.2(@sveltejs/vite-plugin-svelte@2.4.1)(svelte@3.59.1)(vite@4.3.9) + '@sveltejs/vite-plugin-svelte-inspector': 1.0.2(@sveltejs/vite-plugin-svelte@2.4.1)(svelte@3.59.1)(vite@4.5.2) debug: 4.3.4 deepmerge: 4.3.1 kleur: 4.1.5 magic-string: 0.30.0 svelte: 3.59.1 svelte-hmr: 0.15.2(svelte@3.59.1) - vite: 4.3.9 - vitefu: 0.2.4(vite@4.3.9) + vite: 4.5.2 + vitefu: 0.2.4(vite@4.5.2) transitivePeerDependencies: - supports-color dev: true @@ -584,34 +584,34 @@ packages: resolution: {integrity: sha512-SOp9Phqvqn7jtEUxPWdWfWoLmyt2VaJ6MpvP9Comy1MceMXqE6bxvaTu4iaxpYYPzhny28Lc+M87/c2cPK6lDg==} dev: true - /esbuild@0.17.19: - resolution: {integrity: sha512-XQ0jAPFkK/u3LcVRcvVHQcTIqD6E2H1fvZMA5dQPSOWb3suUbWbfbRf94pjc0bNzRYLfIrDRQXr7X+LHIm5oHw==} + /esbuild@0.18.20: + resolution: {integrity: sha512-ceqxoedUrcayh7Y7ZX6NdbbDzGROiyVBgC4PriJThBKSVPWnnFHZAkfI1lJT8QFkOwH4qOS2SJkS4wvpGl8BpA==} engines: {node: '>=12'} hasBin: true requiresBuild: true optionalDependencies: - '@esbuild/android-arm': 0.17.19 - '@esbuild/android-arm64': 0.17.19 - '@esbuild/android-x64': 0.17.19 - '@esbuild/darwin-arm64': 0.17.19 - '@esbuild/darwin-x64': 0.17.19 - '@esbuild/freebsd-arm64': 0.17.19 - '@esbuild/freebsd-x64': 0.17.19 - '@esbuild/linux-arm': 0.17.19 - '@esbuild/linux-arm64': 0.17.19 - '@esbuild/linux-ia32': 0.17.19 - '@esbuild/linux-loong64': 0.17.19 - '@esbuild/linux-mips64el': 0.17.19 - '@esbuild/linux-ppc64': 0.17.19 - '@esbuild/linux-riscv64': 0.17.19 - '@esbuild/linux-s390x': 0.17.19 - '@esbuild/linux-x64': 0.17.19 - '@esbuild/netbsd-x64': 0.17.19 - '@esbuild/openbsd-x64': 0.17.19 - '@esbuild/sunos-x64': 0.17.19 - '@esbuild/win32-arm64': 0.17.19 - '@esbuild/win32-ia32': 0.17.19 - '@esbuild/win32-x64': 0.17.19 + '@esbuild/android-arm': 0.18.20 + '@esbuild/android-arm64': 0.18.20 + '@esbuild/android-x64': 0.18.20 + '@esbuild/darwin-arm64': 0.18.20 + '@esbuild/darwin-x64': 0.18.20 + '@esbuild/freebsd-arm64': 0.18.20 + '@esbuild/freebsd-x64': 0.18.20 + '@esbuild/linux-arm': 0.18.20 + '@esbuild/linux-arm64': 0.18.20 + '@esbuild/linux-ia32': 0.18.20 + '@esbuild/linux-loong64': 0.18.20 + '@esbuild/linux-mips64el': 0.18.20 + '@esbuild/linux-ppc64': 0.18.20 + '@esbuild/linux-riscv64': 0.18.20 + '@esbuild/linux-s390x': 0.18.20 + '@esbuild/linux-x64': 0.18.20 + '@esbuild/netbsd-x64': 0.18.20 + '@esbuild/openbsd-x64': 0.18.20 + '@esbuild/sunos-x64': 0.18.20 + '@esbuild/win32-arm64': 0.18.20 + '@esbuild/win32-ia32': 0.18.20 + '@esbuild/win32-x64': 0.18.20 dev: true /escalade@3.1.1: @@ -1075,8 +1075,8 @@ packages: glob: 7.2.3 dev: true - /rollup@3.25.1: - resolution: {integrity: sha512-tywOR+rwIt5m2ZAWSe5AIJcTat8vGlnPFAv15ycCrw33t6iFsXZ6mzHVFh2psSjxQPmI+xgzMZZizUAukBI4aQ==} + /rollup@3.29.4: + resolution: {integrity: sha512-oWzmBZwvYrU0iJHtDmhsm662rC15FRXmcjCk1xD771dFDx5jJ02ufAQQTn0etB2emNk4J9EZg/yWKpsn9BWGRw==} engines: {node: '>=14.18.0', npm: '>=8.0.0'} hasBin: true optionalDependencies: @@ -1353,13 +1353,14 @@ packages: resolution: {integrity: sha512-EPD5q1uXyFxJpCrLnCc1nHnq3gOa6DZBocAIiI2TaSCA7VCJ1UJDMagCzIkXNsUYfD1daK//LTEQ8xiIbrHtcw==} dev: true - /vite@4.3.9: - resolution: {integrity: sha512-qsTNZjO9NoJNW7KnOrgYwczm0WctJ8m/yqYAMAK9Lxt4SoySUfS5S8ia9K7JHpa3KEeMfyF8LoJ3c5NeBJy6pg==} + /vite@4.5.2: + resolution: {integrity: sha512-tBCZBNSBbHQkaGyhGCDUGqeo2ph8Fstyp6FMSvTtsXeZSPpSMGlviAOav2hxVTqFcx8Hj/twtWKsMJXNY0xI8w==} engines: {node: ^14.18.0 || >=16.0.0} hasBin: true peerDependencies: '@types/node': '>= 14' less: '*' + lightningcss: ^1.21.0 sass: '*' stylus: '*' sugarss: '*' @@ -1369,6 +1370,8 @@ packages: optional: true less: optional: true + lightningcss: + optional: true sass: optional: true stylus: @@ -1378,14 +1381,14 @@ packages: terser: optional: true dependencies: - esbuild: 0.17.19 + esbuild: 0.18.20 postcss: 8.4.31 - rollup: 3.25.1 + rollup: 3.29.4 optionalDependencies: fsevents: 2.3.2 dev: true - /vitefu@0.2.4(vite@4.3.9): + /vitefu@0.2.4(vite@4.5.2): resolution: {integrity: sha512-fanAXjSaf9xXtOOeno8wZXIhgia+CZury481LsDaV++lSvcU2R9Ch2bPh3PYFyoHW+w9LqAeYRISVQjUIew14g==} peerDependencies: vite: ^3.0.0 || ^4.0.0 @@ -1393,7 +1396,7 @@ packages: vite: optional: true dependencies: - vite: 4.3.9 + vite: 4.5.2 dev: true /wrappy@1.0.2: From d34eee1c5d3cf598f8a89a4407b639f75f1d30a1 Mon Sep 17 00:00:00 2001 From: David OK Date: Fri, 29 Mar 2024 13:10:43 +0000 Subject: [PATCH 04/49] WIP: rewording. --- doc/book/random/vector_intrinsics.Rmd | 126 +++++++++++++++----------- 1 file changed, 74 insertions(+), 52 deletions(-) diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index 2f5bed0a0..9ea3439ff 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -1,51 +1,51 @@ # Super Fast Separable Convolutions Sounds boring? I promise you this is going to be way more interesting than you -might think. There's quite a bit interesting things to learn. +might think. While this is a common operation in low-level operation, there is +quite a few interesting things to learn how to leverage the hardware to +accelerate computing. Story time before we dive into the implementation of the Gaussian blur. ## Story Time -Once I applied for a C++ technical leadership role for some company. I was -considered for the role after a preliminary behavioral screening by the CTO. He -then told me who I would be interviewing with in the next interviews. A few days -later, he called me back for a quick chat. He ended up telling me they would not -move forward after examining sara's image processing code. Without really -explaining why, I guessed that probably one of their senior engineers -disqualified me as he did care more about my ability in understanding and -manipulating CPU vector instructions. +Once, I applied for a C++ technical leadership role for some company. I did a +behavioral interview with their CTO. He then told me who I would be interviewing +with in the next interviews. A few days later, he called me back for a quick +chat. He ended up telling me they would not move forward after examining sara's +image processing code. Without really explaining why, the most likely reason I +could guess was that one of their senior engineers disqualified me because all +he cared about was whether I could understand and write code with CPU vector +instructions. The CTO profusedly apologized to me. He said politely that I certainly was -gifted but my C++ code was not to their standard. From what I guessed, they -probably found that my image processing code was implemented too naively. +gifted but my code was not up to their standard. Indeed they must have deemed that +my image processing code was implemented too naively. -This was based on the fact that the CTO told me they were doing lots of +During the behavioral interview, the CTO told me they were doing lots of optimization involving CPU vector intrinsics in order to calculate data as fast as possible. That it really was not that difficult and that I could get up to -speed fairly quickly. - -I have mostly an applied math background, so it did sound unfair to me. It did -made me feel that you are never enough whatever you achieve. Like you do need to -know every single aspect of engineering from high level to low level. In -hindsight I would not have been happy with the job anyways. Still frustrating... -In that moment, I was telling myself: what can you do when you are already being -shut the door? Going by the same logic, if David Lowe showcased his SIFT -research code, he would not qualify for the job either: I learnt from studying -his code. - -Right, back to the topic: today how can we achieve that? Nowadays, we have some -answers with Halide. And we can do it very elegantly without coding in assembly -directly. +speed fairly quickly. That was the main reason I believe they disqualified me. + +Having mostly an applied math background, it did sound unfair and hypocritical +to me. Hypocritical because, if it really was easy, then why can you not learn +on the job? +So it did make me feel that you are never enough whatever you achieve in life. +Oh, so you're supposed to master every single damn thing of software engineering +from high level to low level when you start a job? +Going by the same logic, if David Lowe showcased his SIFT research code, he +would not qualify for the job either since I learnt from studying his code. + +End of the rant... In hindsight I would not have been happy with the job +anyways. Back to the topic: today how can we achieve that? Nowadays, we have +some answers with Halide. And we can do it very elegantly without coding in +assembly code directly. ## What is Halide? -With Halide you can write a an image processing filter and optimize the way it is run. - -You write sets of arithmetic instructions that operates on image buffers with -specific parallelism patterns (multicore and vectorization). Then you can tell -to compile with a C++ method to generate the optimize the image filter as a C++ -static library. +With Halide you can write a an image processing filter and optimize for each +specific hardware and architecture. All of this in a *very few lines of code*. +Then Halide will compile the resulting code into usable assembly code. The main beauty with Halide is that you can decouple: @@ -57,17 +57,39 @@ Halide can check and provide guarantees that your algorithm remains correct for the schedule you are implementing. With Halide, you won't write any nested `for` loops and multi-threaded -processing with bound-checking. So you can express ideas at a higher level. +processing with bound-checking. Instead you express these ideas at a higher +level. + +Halide abstracts the different kind of parallelisms for you and supports a wide +range of platforms. You don't have to be an expert in CPU vector intrinsics, but +you do need to know the schedule strategies to say optimize the speed at which +your convolutional operations run. -Halide abstracts these parallelisms for you and supports a wide range of -platforms. You don't have to be an expert in CPU vector intrinsics, but you do -need to know the schedule strategies to say optimize the speed at which your -convolutional operations run. Halide has identified the most common schedule -patterns that are used to optimize image processing code. +You still need to skim through the publications, the presentations and practise. +But in my experience, it is still difficult for the layman or the novice to +identify the schedule patterns that work and those that don't. -You still need to skim the publications and the presentations, practise. But in -my experience, it is still difficult for the layman or the novice to identify -the schedule patterns that work and those that don't. +### Halide vs Explicit CPU intrisic code + +Naysayers be like: "but Halide is too high level and does too much magic!" + +Optimizing algorithms by writing explicitly CPU intrinsic instructions can +certainly be done. But you would have to pay a very costly engineering price. +You have to optimize for different CPU platforms: x86-64, ARM, RISC-V and learn +about their different intrinsic API. The resulting code is much harder to +maintain than using a unified language that allows you to write these in a very +lines of codes. + +Unless this is your full-time job and something you really want to learn, that's +not something a computer vision scientist like me would like to spend time to... +Halide has done an excellent job in abstracting this at the least on the CPU +side. So know who you are and decide what you want to do. + +Let's conclude this paragraph with a few words regarding the GPU acceleration +with Halide. On the GPU side, Halide is indeed not yet mature. For one thing, +the documentation is still lacking regarding the memory model at this time of +writing. You will be much better off writing code in CUDA, Vulkan, or OpenCL to +fully control your GPU. ## Naive C++ Implementation of the Gaussian Blur @@ -81,11 +103,11 @@ the big lie: don't prematurely optimize and just let the compiler do its job. And just make things work. I did learn in class that the Gaussian filter is separable: Don't write the 2D -convolution naively, exploit its separable property. I am proudly exhibiting a -naive implementation in C++ in my Sara repo, something along the lines: - -Icing on the cake, I have discovered multicore parallelism via OpenMP. +convolution naively, exploit its separable property. Icing on the cake, I have +discovered multicore parallelism via OpenMP. I am proudly exhibiting a naive +implementation in C++ in my Sara repo, something along the lines below. +Below is the first step, that is the x-convolution. ```{Rcpp} auto conv_x(const float *in, const float *kernel, float *out, const int w, const int h, const int ksz) -> void @@ -121,7 +143,7 @@ auto conv_x(const float *in, const float *kernel, float *out, } ``` -We execute the same dance for the y-convolution: +Then we perform the same dance for the y-convolution: ```{Rcpp} auto conv_y(const float *in, const float *kernel, float *out, @@ -173,13 +195,13 @@ enough without elaborating why and what he was expecting to see. Fine! let's see how we could optimize the code... -OK How? I vaguely understand you have to write the code with CPU intrinsics? -Should I write in C-style or in ASM. How to do it with limited bandwidth after -finishing your day job and wanting to learn? +I vaguely understand we have to rewrite the code explicitly with CPU intrinsics? +Should I write it in C-style or in ASM code? How to do it with limited bandwidth +after finishing your day job and wanting to learn? -Use Halide? Yes! It is a very elegant language. It can also compile your code -directly in OpenCL and Vulkan, Direct3D bit code. Here is how we could rewrite -the 2D separable convolution. +Can we use Halide? Yes! It is a very elegant language. It can also compile your +code directly in OpenCL and Vulkan, Direct3D bit code. Here is how we could +rewrite the 2D separable convolution. Let us write the implementation first. From 4b02615497e71a470bf99fd76fadbd6e220b85dd Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 31 Mar 2024 15:31:40 +0100 Subject: [PATCH 05/49] MAINT: update to Halide 17 and LLVM 17 for MacOS. --- build.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build.py b/build.py index 512a9eefb..59507912f 100755 --- a/build.py +++ b/build.py @@ -28,7 +28,7 @@ CUDA_VERSION = "12.1.0" TRT_VERSION = "8.6" SWIFT_VERSION = "5.9.1" -HALIDE_VERSION = "16.0.0" +HALIDE_VERSION = "17.0.0" # Docker SARA_SOURCE_DIR = pathlib.Path(__file__).parent.resolve() @@ -186,7 +186,7 @@ def generate_project( my_cmake_prefix_paths.append(HALIDE_ROOT_PATH) elif SYSTEM == "Darwin": cmake_options.append("-D SARA_USE_HALIDE:BOOL=ON") - llvm_dir = subprocess.check_output(["brew", "--prefix", "llvm@16"]) + llvm_dir = subprocess.check_output(["brew", "--prefix", "llvm"]) llvm_dir = llvm_dir.decode(sys.stdout.encoding).strip() llvm_cmake_dir = pathlib.Path(llvm_dir) / "lib" / "cmake" / "llvm" cmake_options.append(f"-D LLVM_DIR={llvm_cmake_dir}") From b04a7f42e4b7c00e055b9fa7fc1c7ca11f434536 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 1 Apr 2024 20:30:29 +0100 Subject: [PATCH 06/49] DOC: rewrite. --- doc/book/random/vector_intrinsics.Rmd | 282 +++++++++++++++----------- 1 file changed, 168 insertions(+), 114 deletions(-) diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index 9ea3439ff..b658348b7 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -19,8 +19,8 @@ he cared about was whether I could understand and write code with CPU vector instructions. The CTO profusedly apologized to me. He said politely that I certainly was -gifted but my code was not up to their standard. Indeed they must have deemed that -my image processing code was implemented too naively. +gifted but my code was not up to their standard. They must have deemed that my +image processing code was implemented too naively. During the behavioral interview, the CTO told me they were doing lots of optimization involving CPU vector intrinsics in order to calculate data as fast @@ -36,54 +36,66 @@ from high level to low level when you start a job? Going by the same logic, if David Lowe showcased his SIFT research code, he would not qualify for the job either since I learnt from studying his code. -End of the rant... In hindsight I would not have been happy with the job -anyways. Back to the topic: today how can we achieve that? Nowadays, we have -some answers with Halide. And we can do it very elegantly without coding in -assembly code directly. +In hindsight I would not have been happy with the job anyways. End of the rant +and back to the topic: today how can we achieve that? Nowadays, we have some +answers with Halide. And we can do it very elegantly without coding in assembly +code directly. ## What is Halide? -With Halide you can write a an image processing filter and optimize for each -specific hardware and architecture. All of this in a *very few lines of code*. -Then Halide will compile the resulting code into usable assembly code. +In a technical jargon, Halide is an domain specific language embedded in C++. +The specific domain we are dealing is image processing. It is a real language +because it compile your optimized algorithm into assembly code. -The main beauty with Halide is that you can decouple: +With Halide, you can write an image processing filter and optimize for each +specific hardware and architecture. All of this elegangtly in a *very few lines +of code*. Then Halide will compile the C++ code into usable assembly code. -1. The algorithm: the separable convolution and, -2. the scheduling strategy: the parallelism strategy to make it as fast as the -baseline if not faster than OpenCV's Gaussian blur. +The main beauty with Halide is that you decouple: + +1. the algorithm: in our case, the separable convolution and, +2. the scheduling strategy: which exploits the different kinds of + parallelism that the hardware offers, + +so that our implementation is as fast as the baseline implementation. In our +case, the baseline implementation is OpenCV's Gaussian blur. Halide can check and provide guarantees that your algorithm remains correct for the schedule you are implementing. -With Halide, you won't write any nested `for` loops and multi-threaded -processing with bound-checking. Instead you express these ideas at a higher -level. +Halide is trying to unify the common programming patterns from CPU to GPU +programming in CUDA, OpenCL, Metal, Vulkan. Similarly to CUDA, an image +processing algorithm can often expressed as a succesion of CUDA kernels. +Therfore you don't need to write any nested loops. Halide abstracts the different kind of parallelisms for you and supports a wide range of platforms. You don't have to be an expert in CPU vector intrinsics, but -you do need to know the schedule strategies to say optimize the speed at which -your convolutional operations run. - -You still need to skim through the publications, the presentations and practise. -But in my experience, it is still difficult for the layman or the novice to -identify the schedule patterns that work and those that don't. +you do need to know some scheduling strategies to best optimize the +convolutional operation. +When I began learning Halide, I knew very little about optimal schedules. +Skimming through publications and the presentations by Halide authors was the +only way for me to really learn. And of course practise, practise, practise. So +all in all, in my experience, the entry barrier is still very high for the +average programmer to identify which schedule patterns work best and those that +don't. ### Halide vs Explicit CPU intrisic code Naysayers be like: "but Halide is too high level and does too much magic!" -Optimizing algorithms by writing explicitly CPU intrinsic instructions can -certainly be done. But you would have to pay a very costly engineering price. -You have to optimize for different CPU platforms: x86-64, ARM, RISC-V and learn -about their different intrinsic API. The resulting code is much harder to -maintain than using a unified language that allows you to write these in a very -lines of codes. +Certainly! Optimizing algorithms by writing explicitly CPU intrinsic +instructions can be done. But you would have to pay a very costly engineering +price. + +You would have to optimize for different CPU platforms: x86-64, ARM, RISC-V and +learn about their C API to utilize SIMD instructions. The resulting code would +be much harder to maintain than using a unified language that allows you to +write these in a very few lines of codes. -Unless this is your full-time job and something you really want to learn, that's -not something a computer vision scientist like me would like to spend time to... -Halide has done an excellent job in abstracting this at the least on the CPU -side. So know who you are and decide what you want to do. +Unless this is your full-time job or it's something you really want to learn, +personally I don't want to spend time into this. Halide has done an excellent +job in abstracting this at the least on the CPU side. So know who you are and +decide what you want to do. Let's conclude this paragraph with a few words regarding the GPU acceleration with Halide. On the GPU side, Halide is indeed not yet mature. For one thing, @@ -92,7 +104,7 @@ writing. You will be much better off writing code in CUDA, Vulkan, or OpenCL to fully control your GPU. -## Naive C++ Implementation of the Gaussian Blur +## A Naive Implementation of the Gaussian Blur in C++ Right, let's rewind back in time and imagine myself more than 15 years ago. Freshly graduated with a Master's degree looking to showcase some work @@ -187,23 +199,40 @@ I am proudly showcasing my code on GitHub. Never complained about it as I bothered about real-time issues as later I learnt about in CUDA and would write in CUDA anyways. +### Issues in the C++ code + +Let's enumerate some issues and ideas that we will address later on. + +1. The only parallelism we are using is the **multi-threading**. +2. It is not clear how the **CPU vector intrinsics** can be applied in the convolution. + - For one thing, the boundary checking does not easily allow the compiler to + vectorize the C++ code. + - There is actually a better way to exploit the CPU vector instructions for + the convolutional operation and the C++ code does not do this way. +3. **Data locality** is very important aspect, which we are not exploiting. + +It certainly wasn't the fault of my younger self who did not know any better. +Let us now address these issues and ideas in Halide. + -## Halide Implementation of the Algorithm +## A Very Fast Implementation in Halide -Back to the time where the CTO and his minions tell you that you are not good -enough without elaborating why and what he was expecting to see. +As we said it earlier, Halide splits an algorithm into two parts: -Fine! let's see how we could optimize the code... +1. the implementation and +2. the schedule for any algorithm. -I vaguely understand we have to rewrite the code explicitly with CPU intrinsics? -Should I write it in C-style or in ASM code? How to do it with limited bandwidth -after finishing your day job and wanting to learn? +We will now detail each part. This section will divided in 3 parts. -Can we use Halide? Yes! It is a very elegant language. It can also compile your -code directly in OpenCL and Vulkan, Direct3D bit code. Here is how we could -rewrite the 2D separable convolution. +1. First we implement the separable convolution in Halide. +2. Then we explain the different kinds of parallelisms that the CPU offers. +3. Finally we explain the schedule part of the algorithm in Halide and we will + explore a few different schedules. -Let us write the implementation first. +### The Algorithm + +First, let us write the implementation that exploits the separability of the +Gaussian filter. ```{cpp} #include @@ -219,7 +248,11 @@ static constexpr auto truncation_factor = 4.f; static constexpr auto ksz = static_cast(2 * sigma * truncation_factor); static constexpr auto kr = ksz / 2; +// N.B.: we just need to define something for the input... but just think of it +// as an image. input(x, y) = x + y; + +// The 1D Gaussian filter. kernel(x) = Halide::exp(-x * x / (0.5f * sigma * sigma)); auto conv_x = Halide::Func{"conv_x"}; @@ -232,73 +265,92 @@ conv_x(x, y) = Halide::sum(input(x + k - kr, y) * kernel(k)); conv_y(x, y) = Halide::sum(conv_x(x, y + k - kr / 2) * kernel(k)); ``` -## Shedule Optimization - ### Two Types of Parallelisms on CPUs -There are two types of parallelisms on the CPU. +There are two types of parallelisms on the CPU which we can exploit altogether. 1. Multicore processing: - This is straightforward to understand and is about keep all the CPU cores as - busy as possible with minimal data sharing. OpenMP is simple and helps to - parallelize image filter quite easily once we identify the parts of the - algorithm that operate independently. + + This is straightforward to understand. A CPU can be thought as a factory of + workers, each one of them being called a CPU core. The multicore processing + consists in keeping each CPU core as busy as possible with minimal data + sharing. + + OpenMP is one implementation of multicore processing among others to + parallelise our image filter. 2. Vector instructions: - Until I implemented filters with Halide, I could not understand what CPU - instrutions were really about. - I am not going to pretend to be an expert in CPU optimization but this little - paragraph should convince you why it is so interesting to apply vector - instructions wherever possible. - So, as a first approximation, a CPU vector instruction typically enables the - programmer to perform arithmetic operations on small vectors in a single CPU - cycle. Typically arithmetic operations such addition, multiplication and more - can operate on 4D vectors. That is where we can observe additional 4x speed - up or more if your CPU can process those operations on bigger vectors. -For more accurate and more comprehensive information, I will simply encourage -you to do your own research and share what you have learnt. + Until I implemented filters with Halide, I really did not understand what CPU + vector instrutions were really about. + + Mainly, a CPU vector instruction enables a CPU core to perform arithmetic + operations on small vectors in a **single** CPU cycle. + That means that additions, subtractions, dot products on 4D float vectors can + be executed in a single CPU cycle instead of 4 CPU cycles (7 CPU cycles for + the dot product). + + That is very significant on a very large scale as we can observe additional + 4x speed up or more on very very large arrays and therefore image data. -Like image filter, BLAS routines makes extensive use of the two types -parallelism on CPU platforms. +Nowadays, an Intel CPU that supports AVX-512 vector instructions can perform +operations on 16D vectors of 32-bit floating point data in a single CPU +instruction. -### Schedule 1 +So when we combine parallelism 1 and 2 on an 12-core Intel CPU with AVX-512 +instructions, an optimized algorithm could theoretically be sped up by a factor +of $12 \times 16 = 192$ on 32-bit floating point array. This is huge. + +Now, for more accurate and more comprehensive information, I will simply +encourage you to do your own research on ARM, RISC-V CPUs, and share what you +have learnt. + +Likewise, optimized linear algebra routines like OpenBLAS makes extensive use of +the two types parallelism on CPU platforms. + +We are now moving to the schedule, which is the most difficult part of the +implementation. We will explore 3 schedules with Halide. + +### Schedule 1: Naive Strategy The obvious strategy is to start from the idea of separable convolution and vectorize the convolution wherever possible. We parallelize the computation of image rows. We apply Halide's magic invocation -to vectorize the convolution without really understanding why but thinks it -works. +to vectorize the convolution without trying to really understand why it works. -``` +```{cpp} +// Precompute the kernel all at once. +// +// This is irrelevant in the schedule but necessary when you want to understand +// the assembly code generated by Halide. kernel.compute_root(); +// First, compute the x-convolution in a separate memory buffer and use CPU +// vector instructions. conv_x .compute_root() .parallel(y) .vectorize(x, 32, Halide::TailStrategy::GuardWithIf) ; + +// Second, compute the y-convolution as a second step. conv_y .parallel(y) - .vectorize(x, 32, Halide::TailStrategy::GuardWithIf) - ; + .vectorize(x, 32, Halide::TailStrategy::GuardWithIf) ; ``` -Nice improvements over your naive implementation but then you decide to compete -with OpenCV just to see that it still crushes your implementation by being 2x -faster. +Thanks to the CPU vectorization, this schedule is a nice improvement over the +naive C++ implementation as we exploit the CPU data vectorization. Yet OpenCV's +implementation is still better than this schedule by a factor 2. -### Schedule 2: found in one Halide publication. +We will worry about how the CPU vectorization is implemented later by examining +a better schedule. Let's move on to a better schedule: schedule #2. -In fact, the optimal schedule is really not obvious in CPU as exposed in Halide -presentations. Until you dig into Halide publications, you start to understand -how much work and expertise it is to optimize a typical image processing filter -in Photoshop and took 3 months of hard work to correctly invoke CPU vector -instructions. +### Schedule 2: From a Publication -After digging in the publications, I find this schedule in one of Halide's -publications: +In fact, the optimal schedule is really not obvious in CPU. After digging in the +publications, I find this schedule in one of Halide publications: ```{cpp} auto xo = Halide::Var{"xo"}; @@ -313,21 +365,29 @@ conv_y.tile(x, y, xo, yo, xi, yi, 64, 64, Halide::TailStrategy::GuardWithIf) .vectorize(xi, 16, Halide::TailStrategy::GuardWithIf) ``` -This crushes OpenCV's implementation but I don't understand why. +This literally crushes OpenCV's implementation but I didn't understand why at +that time. However the main idea in this schedule is to better exploit data +locality but splitting the image into smaller images. + +As argued in a presentation by Halide authors, without Halide, the optimal +schedule is definitely not obvious and would have necessitated at least 3 months +of work in order how to exploit CPU SIMD instructions and the data locality in +the memory cache. -The first step to achieve this to divide the final convolved image into tiles of -64 by 64 pixels. The set of tiles can be seen as an input batch of many smaller -images. The output is another batch of image tiles with the same image sizes -(let's just assume that the image width and height are multiple of 64.) +Specifically this schedule the final convolved image into square tiles of 64 by +64 pixels. The set of input tiles can be seen as an input batch of many smaller +images and processed in parallel. The output image is obtained by reassembling +the batch of output image tiles. +We will detail later how the vectorization of the convolution is done. -### Schedule 3: A better one found by myself +### Schedule 3: An Improved Version by Myself Because each output image tile is independent of each other, we can calculate -smaller x-convolution and y-convolution. For each tile we can fit the -x-convolution in the CPU cache and it improves data locality in the memory -cache. Then we explot the CPU vector instructions to calculate convolution in -batch. +smaller x-convolution and y-convolution that can fit in the memory cache. For +each tile we can fit the x-convolution in the CPU cache and it improves data +locality in the memory cache. Then we explot the CPU vector instructions to +calculate convolution in batch. ```{cpp} @@ -340,8 +400,7 @@ auto yi = Halide::Var{"yi"}; auto tile = Halide::Var{"tile"}; // Precalculate the kernel. -// We want this and this will avoid cluttering Halide's compiled statement later -// on. +// We want this to avoid cluttering Halide's compiled statement later on. kernel.compute_root(); // The schedule @@ -357,7 +416,7 @@ conv_x ; ``` -#### Second-Guessing what Halide does +### Second-Guessing what Halide does There is a lot to unpack here. Let's try to break it down bit by bit the schedule below: @@ -371,12 +430,11 @@ schedule below: According to how I understand it: ```{cpp} -#pragma omp parallel for // .parallel(y) +#pragma omp parallel for for (auto tile_index = 0; tile_index < T; ++T) { - // Process the tile (xo, yo). - const auto yo = tile_index / T; - const auto xo = tile_index % T; + // Process the tile (xo, yo). const auto yo = tile_index / T; const auto xo = + tile_index % T; for (auto yi = 0; yi < 64; ++yi) { @@ -390,30 +448,26 @@ for (auto tile_index = 0; tile_index < T; ++T) const auto x = xo * 64 + xi; - // Inferring from: + // compute_at(...) allocates memory on the stack as explained in: // https://halide-lang.org/docs/class_halide_1_1_func.html#a800cbcc3ca5e3d3fa1707f6e1990ec83. // - // Halide must allocate some storage on the stack - // // I understand that the storage **has** to be 2D, if we want to calculate // conv_y to ensure maximum data locality. float conv_x[ksz][ksz]; - // TODO: verify this. - // - // This is trivially vectorizable. + // Trivially vectorizable. for (auto k = 0; k < ksz; ++k) conv_x[yi][xi] = 0.f; - // Vectorizable in xi? I don't understand how. + // Vectorizable in xi? How? for (auto k = 0; k < ksz; ++k) conv_x[yi][xi] += in[y][x + k] * kernel[k]; - // Vectorizable in xi? I don't understand how. + // Vectorizable in xi? How? for (auto k = 0; k < ksz; ++ksz) conv[y][x] = 0.f; - // Vectorizable in xi? I don't understand how. + // Vectorizable in xi? How? for (auto k = 0; k < ksz; ++ksz) conv_y[y][x] += conv_x[yi+k][xi] * kernel[k]; } @@ -421,16 +475,16 @@ for (auto tile_index = 0; tile_index < T; ++T) } ``` -I still don't understand how the convolutions are vectorized in the variable +It is not yet clear to me how the convolutions are vectorized in the variable `xi`. This is the goal of the next paragraph. ### Understanding with Halide Compiled Statement -OK the second-guessing turns out to be not that bad but we really understand -what Halide does for us. +The second-guessing turns out to be not that bad but we really need to +understand what Halide does for us. How is the CPU vectorization done? -To get a more definite answer, let's pluck up the courage to actually inspect -the assembly code generated by Halide. +To get a more definite answer, let's actually inspect the assembly code +generated by Halide. ```{cpp} conv_y.compile_to_stmt( @@ -481,7 +535,7 @@ style="border: 1px solid #464646;" allowfullscreen> It is not practical to view the embedded HTML page, so go to this link below to fully explore the +href="./random/separable_conv_2d.stmt.html">link to fully explore the compiled statement as a full page: From 0d57d836eadba3b790b4cef57304423577925674 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 1 Apr 2024 21:23:02 +0100 Subject: [PATCH 07/49] DOC: fix code. --- doc/book/random/vector_intrinsics.Rmd | 35 ++++++++++++++------------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index b658348b7..18f38d72b 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -384,10 +384,10 @@ We will detail later how the vectorization of the convolution is done. ### Schedule 3: An Improved Version by Myself Because each output image tile is independent of each other, we can calculate -smaller x-convolution and y-convolution that can fit in the memory cache. For -each tile we can fit the x-convolution in the CPU cache and it improves data -locality in the memory cache. Then we explot the CPU vector instructions to -calculate convolution in batch. +their x-convolution and y-convolution that can fit in the memory cache. For each +tile we can fit the x-convolution in the CPU cache and it improves data locality +in the memory cache. Then we exploit the CPU vector instructions to calculate +convolution in batch. ```{cpp} @@ -416,7 +416,7 @@ conv_x ; ``` -### Second-Guessing what Halide does +### Second-Guessing What Halide Does There is a lot to unpack here. Let's try to break it down bit by bit the schedule below: @@ -426,6 +426,7 @@ schedule below: .fuse(xo, yo, tile_index) .parallel(tile_index); .vectorize(xi, 16, Halide::TailStrategy::GuardWithIf) + ; ``` According to how I understand it: @@ -541,27 +542,27 @@ compiled statement as a full page: ### Vectorizing in the Convolution -Eventually, this was not so hard and this gives a very tangible hands-on -introduction to assembly code. +Upon inspection of the compiled statement, it turns out that the convolution +operation is implemented by batch where we calculate 4, 8 or 16 convolved values +at the same time by repeating the vectorized fused multiply-add `fmla.4s` +operation. -Then it becomes clear that the convolution operation is implemented by batch -where we calculate 4, 8 or 16 convolved values at the same time by repeating -the vectorized fused multiply-add `fmla.4s` operation. +For example, the vectorization the x-convolution can be translated equivalently +in NumPy code as -The vectorization the x-convolution can be translated in terms of NumPy equivalent code - -``` +```{python} import numpy as np -def convolve_vectorized(conv_x, in, kernel, tile, xi, yi): - # Trivially vectorizable initialization. +def convolve_vectorized(conv_x, input, kernel, tile, xi, yi): + # Trivially vectorized initialization. conv_x[tile, yi, xi:xi+ksz] = 0 # Repeat the fused multiply-add operation as follows. ksz = kernel.shape[0] for k in range(ksz): - conv_x[tile, yi, xi:xi+32] = conv_x[tile, yi, xi:xi+32] \ - + in[tile, yi, xi:xi+32] * kernel[k] + conv_x[tile, yi, xi: xi+ 32] = \ + conv_x[tile, yi, xi:xi + 32] + \ + input[tile, yi, xi + k:xi + 32 + k] * kernel[k] ``` From 1e3ef2a60b34413d14f03163387baa96130fd42d Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 3 Apr 2024 13:20:29 +0100 Subject: [PATCH 08/49] MAINT: update swift compiler and docker images. --- build.py | 4 ++-- docker/Dockerfile | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/build.py b/build.py index 59507912f..66a791a88 100755 --- a/build.py +++ b/build.py @@ -27,8 +27,8 @@ UBUNTU_VERSION = "22.04" CUDA_VERSION = "12.1.0" TRT_VERSION = "8.6" -SWIFT_VERSION = "5.9.1" -HALIDE_VERSION = "17.0.0" +SWIFT_VERSION = "5.10" +HALIDE_VERSION = "17.0.1" # Docker SARA_SOURCE_DIR = pathlib.Path(__file__).parent.resolve() diff --git a/docker/Dockerfile b/docker/Dockerfile index 862d826c1..052be2c91 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -75,14 +75,14 @@ RUN pip3 install \ pybind11 # Install Swift toolchain. -RUN wget https://download.swift.org/swift-5.9.1-release/ubuntu2204/swift-5.9.1-RELEASE/swift-5.9.1-RELEASE-ubuntu22.04.tar.gz -RUN tar xvzf swift-5.9.1-RELEASE-ubuntu22.04.tar.gz \ - && mv swift-5.9.1-RELEASE-ubuntu22.04 /opt +RUN wget https://download.swift.org/swift-5.10-release/ubuntu2204/swift-5.10-RELEASE/swift-5.10-RELEASE-ubuntu22.04.tar.gz +RUN tar xvzf swift-5.10-RELEASE-ubuntu22.04.tar.gz \ + && mv swift-5.10-RELEASE-ubuntu22.04 /opt # Install Halide. -RUN wget https://github.com/halide/Halide/releases/download/v16.0.0/Halide-16.0.0-x86-64-linux-1e963ff817ef0968cc25d811a25a7350c8953ee6.tar.gz -RUN tar xvzf Halide-16.0.0-x86-64-linux-1e963ff817ef0968cc25d811a25a7350c8953ee6.tar.gz && \ - mv Halide-16.0.0-x86-64-linux /opt +RUN wget https://github.com/halide/Halide/releases/download/v17.0.1/Halide-17.0.1-x86-64-linux-52541176253e74467dabc42eeee63d9a62c199f6.tar.gz +RUN tar xvzf Halide-17.0.1-x86-64-linux-52541176253e74467dabc42eeee63d9a62c199f6.tar.gz && \ + mv Halide-17.0.1-x86-64-linux /opt # Please make my life easier # TODO: install neovim, etc. From c325a692b3c141ce9ae6e3e7638c0dbab3902ead Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 3 Apr 2024 13:24:02 +0100 Subject: [PATCH 09/49] MAINT: update docker image. --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6a0db6f3a..5fcb971a2 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -18,7 +18,7 @@ jobs: # os: [macos-latest, windows-latest] include: - os: ubuntu-latest - container: 'oddkiva/sara-devel:cuda12.1.0-ubuntu22.04-trt8.6-swift5.9.1-halide16.0.0' + container: 'oddkiva/sara-devel:cuda12.1.0-ubuntu22.04-trt8.6-swift5.10-halide17.0.1' - os: ubuntu-latest container: 'oddkiva/sara-emsdk-devel:latest' From 29cf35c7410d8e0ee51ebf84f9ebd4bf871da69a Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Wed, 3 Apr 2024 17:17:24 +0100 Subject: [PATCH 10/49] DOC: reword. --- doc/book/random/vector_intrinsics.Rmd | 107 ++++++++++++++------------ 1 file changed, 59 insertions(+), 48 deletions(-) diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index 18f38d72b..b8df9152c 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -12,40 +12,41 @@ Story time before we dive into the implementation of the Gaussian blur. Once, I applied for a C++ technical leadership role for some company. I did a behavioral interview with their CTO. He then told me who I would be interviewing with in the next interviews. A few days later, he called me back for a quick -chat. He ended up telling me they would not move forward after examining sara's -image processing code. Without really explaining why, the most likely reason I -could guess was that one of their senior engineers disqualified me because all -he cared about was whether I could understand and write code with CPU vector -instructions. +chat. He ended up telling me they would not move forward after examining +*Sara*'s image processing code. Without really explaining why, the most likely +reason I could come up with was that one of their senior engineers disqualified +me because all he cared about was whether I could understand and write code with +CPU vector instructions. The CTO profusedly apologized to me. He said politely that I certainly was gifted but my code was not up to their standard. They must have deemed that my image processing code was implemented too naively. -During the behavioral interview, the CTO told me they were doing lots of -optimization involving CPU vector intrinsics in order to calculate data as fast -as possible. That it really was not that difficult and that I could get up to -speed fairly quickly. That was the main reason I believe they disqualified me. +Indeed I remembered that during the behavioral interview, the CTO told me they +were doing lots of optimization involving CPU vector instructions in order to +process data as fast as possible. That it really was not that difficult and that +I could get up to speed fairly quickly, and blah blah blah... That was the only +reason for which, I believe, they disqualified me. Having mostly an applied math background, it did sound unfair and hypocritical to me. Hypocritical because, if it really was easy, then why can you not learn -on the job? -So it did make me feel that you are never enough whatever you achieve in life. +it on the job? +So, yes, it did make me feel that you are never enough whatever you achieve in +life. Oh, so you're supposed to master every single damn thing of software engineering -from high level to low level when you start a job? +when you start a job? Going by the same logic, if David Lowe showcased his SIFT research code, he would not qualify for the job either since I learnt from studying his code. In hindsight I would not have been happy with the job anyways. End of the rant and back to the topic: today how can we achieve that? Nowadays, we have some -answers with Halide. And we can do it very elegantly without coding in assembly -code directly. +answers with Halide, which allows us to do it very elegantly. ## What is Halide? In a technical jargon, Halide is an domain specific language embedded in C++. -The specific domain we are dealing is image processing. It is a real language -because it compile your optimized algorithm into assembly code. +The specific domain we are dealing is image processing. It is a language because +it compiles your optimized algorithm into assembly code. With Halide, you can write an image processing filter and optimize for each specific hardware and architecture. All of this elegangtly in a *very few lines @@ -60,18 +61,16 @@ The main beauty with Halide is that you decouple: so that our implementation is as fast as the baseline implementation. In our case, the baseline implementation is OpenCV's Gaussian blur. -Halide can check and provide guarantees that your algorithm remains correct for -the schedule you are implementing. +Halide is trying to unify common programming patterns that arise from CPU to GPU +programming in CUDA, OpenCL, Metal, Vulkan. In Halide, an image processing +algorithm can often expressed as a succesion of CUDA kernels. Therefore you +don't need to write any nested loops. -Halide is trying to unify the common programming patterns from CPU to GPU -programming in CUDA, OpenCL, Metal, Vulkan. Similarly to CUDA, an image -processing algorithm can often expressed as a succesion of CUDA kernels. -Therfore you don't need to write any nested loops. +Halide abstracts different kinds of parallelism and supports a wide range of +platforms. While we don't have to know about the C API to call the vector +instructions, we still need to know the common programming patterns that +involves CPU vector instructions. -Halide abstracts the different kind of parallelisms for you and supports a wide -range of platforms. You don't have to be an expert in CPU vector intrinsics, but -you do need to know some scheduling strategies to best optimize the -convolutional operation. When I began learning Halide, I knew very little about optimal schedules. Skimming through publications and the presentations by Halide authors was the only way for me to really learn. And of course practise, practise, practise. So @@ -81,11 +80,10 @@ don't. ### Halide vs Explicit CPU intrisic code -Naysayers be like: "but Halide is too high level and does too much magic!" +Naysayers will argue: "But Halide is too high level and does too much magic!" -Certainly! Optimizing algorithms by writing explicitly CPU intrinsic -instructions can be done. But you would have to pay a very costly engineering -price. +Certainly, optimizing algorithms by writing explicitly CPU vector instructions +can be done. But we would have to pay a very costly engineering price. You would have to optimize for different CPU platforms: x86-64, ARM, RISC-V and learn about their C API to utilize SIMD instructions. The resulting code would @@ -117,7 +115,7 @@ And just make things work. I did learn in class that the Gaussian filter is separable: Don't write the 2D convolution naively, exploit its separable property. Icing on the cake, I have discovered multicore parallelism via OpenMP. I am proudly exhibiting a naive -implementation in C++ in my Sara repo, something along the lines below. +implementation in C++ in my *Sara* repo, something along the lines below. Below is the first step, that is the x-convolution. ```{Rcpp} @@ -272,9 +270,11 @@ There are two types of parallelisms on the CPU which we can exploit altogether. 1. Multicore processing: This is straightforward to understand. A CPU can be thought as a factory of - workers, each one of them being called a CPU core. The multicore processing - consists in keeping each CPU core as busy as possible with minimal data - sharing. + workers, each one of them being called a CPU core. + + The multicore processing becomes most effective when each CPU core is being + kept as busy as possible and the CPU cores don't need to communicate with + each other via data synchronization. OpenMP is one implementation of multicore processing among others to parallelise our image filter. @@ -298,16 +298,25 @@ operations on 16D vectors of 32-bit floating point data in a single CPU instruction. So when we combine parallelism 1 and 2 on an 12-core Intel CPU with AVX-512 -instructions, an optimized algorithm could theoretically be sped up by a factor -of $12 \times 16 = 192$ on 32-bit floating point array. This is huge. +instructions, an optimized algorithm could in principle be sped up by a factor +of + +\begin{equation} + \#\{\text{CPUs}\} \times \text{SIMD max dim} = 12 \times 16 = 192 +\end{equation} -Now, for more accurate and more comprehensive information, I will simply -encourage you to do your own research on ARM, RISC-V CPUs, and share what you -have learnt. +on single-precision floating point data. + +While we should instead use Amdahl's law instead of this naive formula, applying +this formula is not so simple and we will be content to just benchmark and +measure the performance gain. Likewise, optimized linear algebra routines like OpenBLAS makes extensive use of the two types parallelism on CPU platforms. +For more accurate and more comprehensive information, I will simply encourage +you to do your own research on ARM, RISC-V CPUs, and share what you have learnt. + We are now moving to the schedule, which is the most difficult part of the implementation. We will explore 3 schedules with Halide. @@ -340,17 +349,18 @@ conv_y .vectorize(x, 32, Halide::TailStrategy::GuardWithIf) ; ``` -Thanks to the CPU vectorization, this schedule is a nice improvement over the -naive C++ implementation as we exploit the CPU data vectorization. Yet OpenCV's -implementation is still better than this schedule by a factor 2. +This schedule is a nice improvement over the naive C++ implementation as we +exploit the CPU data vectorization. Yet OpenCV's implementation is still better +than this schedule by a factor 2. -We will worry about how the CPU vectorization is implemented later by examining -a better schedule. Let's move on to a better schedule: schedule #2. +Later we will examine how the CPU vectorization is implemented by inspecting a +better schedule. So let's move on to a better schedule: schedule #2. ### Schedule 2: From a Publication -In fact, the optimal schedule is really not obvious in CPU. After digging in the -publications, I find this schedule in one of Halide publications: +The optimal schedule is really not obvious in CPU, at least for me. After +digging in the publications, I found this schedule in one of Halide +publications: ```{cpp} auto xo = Halide::Var{"xo"}; @@ -359,7 +369,8 @@ auto yo = Halide::Var{"yo"}; auto xi = Halide::Var{"xi"}; auto yi = Halide::Var{"yi"}; -conv_y.tile(x, y, xo, yo, xi, yi, 64, 64, Halide::TailStrategy::GuardWithIf) +conv_y.tile(x, y, xo, yo, xi, yi, 64, 64, + Halide::TailStrategy::GuardWithIf) .fuse(xo, yo, tile_index) .parallel(tile_index); .vectorize(xi, 16, Halide::TailStrategy::GuardWithIf) @@ -554,7 +565,7 @@ in NumPy code as import numpy as np def convolve_vectorized(conv_x, input, kernel, tile, xi, yi): - # Trivially vectorized initialization. + # Trivially vectorized. conv_x[tile, yi, xi:xi+ksz] = 0 # Repeat the fused multiply-add operation as follows. From c453fe80dfd7b9cd2c8f8f03500ad8619eca3c97 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Wed, 3 Apr 2024 19:18:54 +0100 Subject: [PATCH 11/49] DOC: reword. --- doc/book/random/vector_intrinsics.Rmd | 94 +++++++++++++++------------ 1 file changed, 53 insertions(+), 41 deletions(-) diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index b8df9152c..c24e2df62 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -54,29 +54,28 @@ of code*. Then Halide will compile the C++ code into usable assembly code. The main beauty with Halide is that you decouple: -1. the algorithm: in our case, the separable convolution and, -2. the scheduling strategy: which exploits the different kinds of - parallelism that the hardware offers, +1. the **algorithm**: in our case, the **separable convolution** and, +2. the **schedule**: which exploits the different **kinds of parallelism** that + the hardware offers, so that our implementation is as fast as the baseline implementation. In our case, the baseline implementation is OpenCV's Gaussian blur. -Halide is trying to unify common programming patterns that arise from CPU to GPU -programming in CUDA, OpenCL, Metal, Vulkan. In Halide, an image processing -algorithm can often expressed as a succesion of CUDA kernels. Therefore you +Halide abstracts and unifies the **common programming patterns** that arise from +CPU to GPU programming in CUDA, OpenCL, Metal, Vulkan. Besides, an image +processing algorithm can often expressed as a sequence of CUDA kernels, you don't need to write any nested loops. Halide abstracts different kinds of parallelism and supports a wide range of -platforms. While we don't have to know about the C API to call the vector -instructions, we still need to know the common programming patterns that -involves CPU vector instructions. +platforms. While we don't have to know the C API to invoke vector instructions, +we still need to know those common programming patterns that involves CPU vector +instructions. When I began learning Halide, I knew very little about optimal schedules. Skimming through publications and the presentations by Halide authors was the only way for me to really learn. And of course practise, practise, practise. So all in all, in my experience, the entry barrier is still very high for the -average programmer to identify which schedule patterns work best and those that -don't. +average programmer to identify which schedules work best and those that don't. ### Halide vs Explicit CPU intrisic code @@ -85,10 +84,10 @@ Naysayers will argue: "But Halide is too high level and does too much magic!" Certainly, optimizing algorithms by writing explicitly CPU vector instructions can be done. But we would have to pay a very costly engineering price. -You would have to optimize for different CPU platforms: x86-64, ARM, RISC-V and -learn about their C API to utilize SIMD instructions. The resulting code would -be much harder to maintain than using a unified language that allows you to -write these in a very few lines of codes. +You would have to optimize for different CPU platforms: *x86-64*, *ARM*, +*RISC-V* and learn their respective C API to utilize hardware SIMD instructions. +The resulting code would be much lengthier and harder to maintain than using a +unified language that allows you to write these in a very few lines of codes. Unless this is your full-time job or it's something you really want to learn, personally I don't want to spend time into this. Halide has done an excellent @@ -117,8 +116,12 @@ convolution naively, exploit its separable property. Icing on the cake, I have discovered multicore parallelism via OpenMP. I am proudly exhibiting a naive implementation in C++ in my *Sara* repo, something along the lines below. +### The x-convolution + Below is the first step, that is the x-convolution. ```{Rcpp} +#include + auto conv_x(const float *in, const float *kernel, float *out, const int w, const int h, const int ksz) -> void { @@ -132,15 +135,12 @@ auto conv_x(const float *in, const float *kernel, float *out, for (auto x = 0; x < w; ++x) { auto val = 0.f; + + // Calculate the convolved value. for (auto k = 0; k < ksz; ++k) { - auto xk = x - r + k; - // Check the boundary conditions. - if (xk < 0) - xk = 0; - else if (xk >= w) - xk = w - 1; + const auto xk = std::clamp(x - r + k, 0, w - 1); // Accumulate. const auto xy = y * w + xk; @@ -153,9 +153,13 @@ auto conv_x(const float *in, const float *kernel, float *out, } ``` -Then we perform the same dance for the y-convolution: +### The y-convolution + +Then we perform the same dance for the y-convolution in a similar fashion. ```{Rcpp} +#include + auto conv_y(const float *in, const float *kernel, float *out, const int w, const int h, const int ksz) -> void { @@ -169,15 +173,12 @@ auto conv_y(const float *in, const float *kernel, float *out, for (auto x = 0; x < w; ++x) { auto val = 0.f; + + // Calculate the convolved value. for (auto k = 0; k < ksz; ++k) { - auto yk = y - r + k; - // Check the boundary conditions. - if (yk < 0) - yk = 0; - else if (yk >= h) - yk = h - 1; + const auto yk = std::clamp(y - r + k, 0, w - 1); // Accumulate. const auto xy = yk * w + x; @@ -193,21 +194,27 @@ auto conv_y(const float *in, const float *kernel, float *out, I diligently write unit tests, validate on synthetic tests, check the boundary conditions and try it on a real image with a Gaussian kernel. I am happy it works reasonably fast when compiling in Release mode on Visual Studio. Job done! -I am proudly showcasing my code on GitHub. Never complained about it as I -bothered about real-time issues as later I learnt about in CUDA and would write -in CUDA anyways. +I am proudly showcasing my code on GitHub. Never complained about it as I never +bothered about real-time considerations. Instead I would port the code to make +the algorithm run faster. ### Issues in the C++ code Let's enumerate some issues and ideas that we will address later on. 1. The only parallelism we are using is the **multi-threading**. -2. It is not clear how the **CPU vector intrinsics** can be applied in the convolution. + + Each row is processed in parallel but can we do better than parallel row + processing? + +2. It is not clear how the **CPU vector instructions** can be applied in the + convolution. - For one thing, the boundary checking does not easily allow the compiler to vectorize the C++ code. - There is actually a better way to exploit the CPU vector instructions for the convolutional operation and the C++ code does not do this way. -3. **Data locality** is very important aspect, which we are not exploiting. +3. **Data locality** is a very important aspect, which the parallel row + processing does not easily allow to leverage. It certainly wasn't the fault of my younger self who did not know any better. Let us now address these issues and ideas in Halide. @@ -308,7 +315,7 @@ of on single-precision floating point data. While we should instead use Amdahl's law instead of this naive formula, applying -this formula is not so simple and we will be content to just benchmark and +Amdahl's law is not so simple and we will be content to just benchmark and measure the performance gain. Likewise, optimized linear algebra routines like OpenBLAS makes extensive use of @@ -369,6 +376,8 @@ auto yo = Halide::Var{"yo"}; auto xi = Halide::Var{"xi"}; auto yi = Halide::Var{"yi"}; +auto tile_index = Halide::Var{"t"}; + conv_y.tile(x, y, xo, yo, xi, yi, 64, 64, Halide::TailStrategy::GuardWithIf) .fuse(xo, yo, tile_index) @@ -433,7 +442,8 @@ There is a lot to unpack here. Let's try to break it down bit by bit the schedule below: ```{cpp} - conv_y.tile(x, y, xo, yo, xi, yi, 64, 64, Halide::TailStrategy::GuardWithIf) + conv_y.tile(x, y, xo, yo, xi, yi, 64, 64, + Halide::TailStrategy::GuardWithIf) .fuse(xo, yo, tile_index) .parallel(tile_index); .vectorize(xi, 16, Halide::TailStrategy::GuardWithIf) @@ -445,8 +455,9 @@ According to how I understand it: #pragma omp parallel for for (auto tile_index = 0; tile_index < T; ++T) { - // Process the tile (xo, yo). const auto yo = tile_index / T; const auto xo = - tile_index % T; + // Process the tile (xo, yo). + const auto yo = tile_index / T; + const auto xo = tile_index % T; for (auto yi = 0; yi < 64; ++yi) { @@ -531,11 +542,12 @@ parallelisms in the schedule: // vectorize(xi, 32, GuardWithIf); ``` -This will help us to map out mentally the algorithmic roadmap and understand how -the HTML visually maps out the **three-way** correspondence between: -1. each part of the pseudo-code +This will help us to break down the algorithmic roadmap and understand how the +HTML visually maps out the **three-way** correspondence between: + +1. each part of the pseudo-code, 2. each diagram block and -3. each of the assembly code. +3. each part of the assembly code. After a bit of inspection, by reinstating progressively all the different parallelisms, we start to understand how the parallel patterns are implemented From c64fe4c65ce68970ea2bfc138f74b71c4c646118 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Wed, 3 Apr 2024 19:20:02 +0100 Subject: [PATCH 12/49] DOC: reformat code. --- doc/book/random/vector_intrinsics.Rmd | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index c24e2df62..2ff98df46 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -424,15 +424,15 @@ auto tile = Halide::Var{"tile"}; kernel.compute_root(); // The schedule -conv_y // +conv_y .tile(x, y, xo, yo, xi, yi, 64, 64) .fuse(xo, yo, tile) .parallel(tile) - .vectorize(xi, 32, Halide::TailStrategy::GuardWithIf) // + .vectorize(xi, 32, Halide::TailStrategy::GuardWithIf) ; conv_x - .compute_at(conv_y, xi) // - .vectorize(x, 32, Halide::TailStrategy::GuardWithIf) // + .compute_at(conv_y, xi) + .vectorize(x, 32, Halide::TailStrategy::GuardWithIf) ; ``` From bb4c915a5f1fd06fa0372c5d7b9fdd6590195d21 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 4 Apr 2024 01:39:38 +0000 Subject: [PATCH 13/49] Bump vite from 4.5.2 to 4.5.3 in /svelte/sara-app Bumps [vite](https://github.com/vitejs/vite/tree/HEAD/packages/vite) from 4.5.2 to 4.5.3. - [Release notes](https://github.com/vitejs/vite/releases) - [Changelog](https://github.com/vitejs/vite/blob/v4.5.3/packages/vite/CHANGELOG.md) - [Commits](https://github.com/vitejs/vite/commits/v4.5.3/packages/vite) --- updated-dependencies: - dependency-name: vite dependency-type: direct:development ... Signed-off-by: dependabot[bot] --- svelte/sara-app/package.json | 2 +- svelte/sara-app/pnpm-lock.yaml | 36 +++++++++++++++++----------------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/svelte/sara-app/package.json b/svelte/sara-app/package.json index 4c2fdbaf8..792f8e3d0 100644 --- a/svelte/sara-app/package.json +++ b/svelte/sara-app/package.json @@ -20,7 +20,7 @@ "svelte-check": "^3.0.1", "tslib": "^2.4.1", "typescript": "^5.0.0", - "vite": "^4.5.2", + "vite": "^4.5.3", "autoprefixer": "^10.4.14", "daisyui": "^3.0.2", "postcss": "^8.4.31", diff --git a/svelte/sara-app/pnpm-lock.yaml b/svelte/sara-app/pnpm-lock.yaml index 5885f4d62..f45e45005 100644 --- a/svelte/sara-app/pnpm-lock.yaml +++ b/svelte/sara-app/pnpm-lock.yaml @@ -10,7 +10,7 @@ devDependencies: version: 2.1.0(@sveltejs/kit@1.20.2) '@sveltejs/kit': specifier: ^1.5.0 - version: 1.20.2(svelte@3.59.1)(vite@4.5.2) + version: 1.20.2(svelte@3.59.1)(vite@4.5.3) autoprefixer: specifier: ^10.4.14 version: 10.4.14(postcss@8.4.31) @@ -42,8 +42,8 @@ devDependencies: specifier: ^5.0.0 version: 5.1.3 vite: - specifier: ^4.5.2 - version: 4.5.2 + specifier: ^4.5.3 + version: 4.5.3 packages: @@ -314,11 +314,11 @@ packages: peerDependencies: '@sveltejs/kit': ^1.0.0 dependencies: - '@sveltejs/kit': 1.20.2(svelte@3.59.1)(vite@4.5.2) + '@sveltejs/kit': 1.20.2(svelte@3.59.1)(vite@4.5.3) import-meta-resolve: 3.0.0 dev: true - /@sveltejs/kit@1.20.2(svelte@3.59.1)(vite@4.5.2): + /@sveltejs/kit@1.20.2(svelte@3.59.1)(vite@4.5.3): resolution: {integrity: sha512-MtR1i+HtmYWcRgtubw1GQqT/+CWXL/z24PegE0xYAdObbhdr7YtEfmoe705D/JZMtMmoPXrmSk4W0MfL5A3lYw==} engines: {node: ^16.14 || >=18} hasBin: true @@ -327,7 +327,7 @@ packages: svelte: ^3.54.0 || ^4.0.0-next.0 vite: ^4.0.0 dependencies: - '@sveltejs/vite-plugin-svelte': 2.4.1(svelte@3.59.1)(vite@4.5.2) + '@sveltejs/vite-plugin-svelte': 2.4.1(svelte@3.59.1)(vite@4.5.3) '@types/cookie': 0.5.1 cookie: 0.5.0 devalue: 4.3.2 @@ -341,12 +341,12 @@ packages: svelte: 3.59.1 tiny-glob: 0.2.9 undici: 5.22.1 - vite: 4.5.2 + vite: 4.5.3 transitivePeerDependencies: - supports-color dev: true - /@sveltejs/vite-plugin-svelte-inspector@1.0.2(@sveltejs/vite-plugin-svelte@2.4.1)(svelte@3.59.1)(vite@4.5.2): + /@sveltejs/vite-plugin-svelte-inspector@1.0.2(@sveltejs/vite-plugin-svelte@2.4.1)(svelte@3.59.1)(vite@4.5.3): resolution: {integrity: sha512-Cy1dUMcYCnDVV/hPLXa43YZJ2jGKVW5rA0xuNL9dlmYhT0yoS1g7+FOFSRlgk0BXKk/Oc7grs+8BVA5Iz2fr8A==} engines: {node: ^14.18.0 || >= 16} peerDependencies: @@ -354,30 +354,30 @@ packages: svelte: ^3.54.0 || ^4.0.0-next.0 vite: ^4.0.0 dependencies: - '@sveltejs/vite-plugin-svelte': 2.4.1(svelte@3.59.1)(vite@4.5.2) + '@sveltejs/vite-plugin-svelte': 2.4.1(svelte@3.59.1)(vite@4.5.3) debug: 4.3.4 svelte: 3.59.1 - vite: 4.5.2 + vite: 4.5.3 transitivePeerDependencies: - supports-color dev: true - /@sveltejs/vite-plugin-svelte@2.4.1(svelte@3.59.1)(vite@4.5.2): + /@sveltejs/vite-plugin-svelte@2.4.1(svelte@3.59.1)(vite@4.5.3): resolution: {integrity: sha512-bNNKvoRY89ptY7udeBSCmTdCVwkjmMcZ0j/z9J5MuedT8jPjq0zrknAo/jF1sToAza4NVaAgR9AkZoD9oJJmnA==} engines: {node: ^14.18.0 || >= 16} peerDependencies: svelte: ^3.54.0 || ^4.0.0-next.0 vite: ^4.0.0 dependencies: - '@sveltejs/vite-plugin-svelte-inspector': 1.0.2(@sveltejs/vite-plugin-svelte@2.4.1)(svelte@3.59.1)(vite@4.5.2) + '@sveltejs/vite-plugin-svelte-inspector': 1.0.2(@sveltejs/vite-plugin-svelte@2.4.1)(svelte@3.59.1)(vite@4.5.3) debug: 4.3.4 deepmerge: 4.3.1 kleur: 4.1.5 magic-string: 0.30.0 svelte: 3.59.1 svelte-hmr: 0.15.2(svelte@3.59.1) - vite: 4.5.2 - vitefu: 0.2.4(vite@4.5.2) + vite: 4.5.3 + vitefu: 0.2.4(vite@4.5.3) transitivePeerDependencies: - supports-color dev: true @@ -1353,8 +1353,8 @@ packages: resolution: {integrity: sha512-EPD5q1uXyFxJpCrLnCc1nHnq3gOa6DZBocAIiI2TaSCA7VCJ1UJDMagCzIkXNsUYfD1daK//LTEQ8xiIbrHtcw==} dev: true - /vite@4.5.2: - resolution: {integrity: sha512-tBCZBNSBbHQkaGyhGCDUGqeo2ph8Fstyp6FMSvTtsXeZSPpSMGlviAOav2hxVTqFcx8Hj/twtWKsMJXNY0xI8w==} + /vite@4.5.3: + resolution: {integrity: sha512-kQL23kMeX92v3ph7IauVkXkikdDRsYMGTVl5KY2E9OY4ONLvkHf04MDTbnfo6NKxZiDLWzVpP5oTa8hQD8U3dg==} engines: {node: ^14.18.0 || >=16.0.0} hasBin: true peerDependencies: @@ -1388,7 +1388,7 @@ packages: fsevents: 2.3.2 dev: true - /vitefu@0.2.4(vite@4.5.2): + /vitefu@0.2.4(vite@4.5.3): resolution: {integrity: sha512-fanAXjSaf9xXtOOeno8wZXIhgia+CZury481LsDaV++lSvcU2R9Ch2bPh3PYFyoHW+w9LqAeYRISVQjUIew14g==} peerDependencies: vite: ^3.0.0 || ^4.0.0 @@ -1396,7 +1396,7 @@ packages: vite: optional: true dependencies: - vite: 4.5.2 + vite: 4.5.3 dev: true /wrappy@1.0.2: From cbe81d6e6c8a4faf1cd807d62bb46779983fe754 Mon Sep 17 00:00:00 2001 From: David OK Date: Thu, 4 Apr 2024 18:39:01 +0100 Subject: [PATCH 14/49] DOC: reword. --- doc/book/random/vector_intrinsics.Rmd | 86 +++++++++++++++++---------- 1 file changed, 53 insertions(+), 33 deletions(-) diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index 2ff98df46..f915a08d9 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -1,9 +1,15 @@ # Super Fast Separable Convolutions -Sounds boring? I promise you this is going to be way more interesting than you -might think. While this is a common operation in low-level operation, there is -quite a few interesting things to learn how to leverage the hardware to -accelerate computing. +Sounds boring? + +I promise you this is going to be way more interesting than you might think. +While this is a common operation in low-level operation, there is quite a few +interesting things to learn how to use the hardware to accelerate computing. +This topic can be read nonlinearly and you can directly move to the sections of +interest. + +In a nutshell, I can give you some tools to reimplement *algorithms that run +much faster than OpenCV*. Story time before we dive into the implementation of the Gaussian blur. @@ -13,10 +19,10 @@ Once, I applied for a C++ technical leadership role for some company. I did a behavioral interview with their CTO. He then told me who I would be interviewing with in the next interviews. A few days later, he called me back for a quick chat. He ended up telling me they would not move forward after examining -*Sara*'s image processing code. Without really explaining why, the most likely -reason I could come up with was that one of their senior engineers disqualified -me because all he cared about was whether I could understand and write code with -CPU vector instructions. +*Sara*'s image processing code. He did not really explain why. Nevertheless, the +most likely explanation I could come up with was that one of their senior +engineers disqualified me because all he seemed to care about was whether I +could understand and write code with CPU vector instructions. The CTO profusedly apologized to me. He said politely that I certainly was gifted but my code was not up to their standard. They must have deemed that my @@ -28,19 +34,20 @@ process data as fast as possible. That it really was not that difficult and that I could get up to speed fairly quickly, and blah blah blah... That was the only reason for which, I believe, they disqualified me. -Having mostly an applied math background, it did sound unfair and hypocritical -to me. Hypocritical because, if it really was easy, then why can you not learn -it on the job? -So, yes, it did make me feel that you are never enough whatever you achieve in -life. +Having mostly an applied math background and being a self-taught programmer, it +did sound unfair and hypocritical to me. Hypocritical because, if it really was +easy, then why can you not learn it on the job? So, yes, it did make me feel +that you are never enough whatever you achieve in life. Oh, so you're supposed to master every single damn thing of software engineering when you start a job? Going by the same logic, if David Lowe showcased his SIFT research code, he would not qualify for the job either since I learnt from studying his code. -In hindsight I would not have been happy with the job anyways. End of the rant -and back to the topic: today how can we achieve that? Nowadays, we have some -answers with Halide, which allows us to do it very elegantly. +In hindsight I would not have been happy with the job anyways. + +End of my angry rant and back to the topic: today how can we achieve that? +Nowadays, we have some answers with Halide, which allows us to do it very +elegantly. ## What is Halide? @@ -55,7 +62,7 @@ of code*. Then Halide will compile the C++ code into usable assembly code. The main beauty with Halide is that you decouple: 1. the **algorithm**: in our case, the **separable convolution** and, -2. the **schedule**: which exploits the different **kinds of parallelism** that +2. the **schedule**: which leverages the different **kinds of parallelism** that the hardware offers, so that our implementation is as fast as the baseline implementation. In our @@ -82,17 +89,20 @@ average programmer to identify which schedules work best and those that don't. Naysayers will argue: "But Halide is too high level and does too much magic!" Certainly, optimizing algorithms by writing explicitly CPU vector instructions -can be done. But we would have to pay a very costly engineering price. +can be done. But we would have to pay a very costly engineering price. If I were +the head of a small business, this would be hard to justify the heavy +engineering time and money, that it entails to. -You would have to optimize for different CPU platforms: *x86-64*, *ARM*, -*RISC-V* and learn their respective C API to utilize hardware SIMD instructions. -The resulting code would be much lengthier and harder to maintain than using a -unified language that allows you to write these in a very few lines of codes. +Nowadays, would be ready spend time on optimizing for different CPU platforms: +*x86-64*, *ARM*, *RISC-V* and learn their respective C API to invoke SIMD +instructions? On top of that, the resulting code would be much lengthier and +harder to maintain than using a unified language that allows you to write these +in a very few lines of codes. Unless this is your full-time job or it's something you really want to learn, -personally I don't want to spend time into this. Halide has done an excellent -job in abstracting this at the least on the CPU side. So know who you are and -decide what you want to do. +personally I don't want to spend time into this. Either way... Halide has done +an excellent job in making CPU vector instructions *platform-agnostic* and it +will help you learn the C API faster. Let's conclude this paragraph with a few words regarding the GPU acceleration with Halide. On the GPU side, Halide is indeed not yet mature. For one thing, @@ -178,7 +188,7 @@ auto conv_y(const float *in, const float *kernel, float *out, for (auto k = 0; k < ksz; ++k) { // Check the boundary conditions. - const auto yk = std::clamp(y - r + k, 0, w - 1); + const auto yk = std::clamp(y - r + k, 0, h - 1); // Accumulate. const auto xy = yk * w + x; @@ -195,8 +205,8 @@ I diligently write unit tests, validate on synthetic tests, check the boundary conditions and try it on a real image with a Gaussian kernel. I am happy it works reasonably fast when compiling in Release mode on Visual Studio. Job done! I am proudly showcasing my code on GitHub. Never complained about it as I never -bothered about real-time considerations. Instead I would port the code to make -the algorithm run faster. +bothered about real-time considerations. Instead I would port the code in CUDA +to make the algorithm run faster. ### Issues in the C++ code @@ -209,15 +219,25 @@ Let's enumerate some issues and ideas that we will address later on. 2. It is not clear how the **CPU vector instructions** can be applied in the convolution. - - For one thing, the boundary checking does not easily allow the compiler to - vectorize the C++ code. + - For one thing, the boundary check via clamping does not easily allow the + compiler to vectorize the C++ code. - There is actually a better way to exploit the CPU vector instructions for the convolutional operation and the C++ code does not do this way. + 3. **Data locality** is a very important aspect, which the parallel row - processing does not easily allow to leverage. + processing may not easily allow to do so. + + Just like in CUDA programming, processing the image into tiles on CPU is much + more efficient in practice. + + Let's note that processing tiles in parallel is a pattern that keeps + repeating in optimized linear algebra routines (OpenBLAS etc.), in particular + the matrix-matrix multiplication *gemm* operation, which is one fundamental + operation in deep learning. -It certainly wasn't the fault of my younger self who did not know any better. -Let us now address these issues and ideas in Halide. +There are probably more issues. Hopefully I pointed out the most problematic +ones. It certainly wasn't the fault of my younger self who did not know any +better. Let us now address these issues and ideas in Halide. ## A Very Fast Implementation in Halide From f07a2d5c38c5442ce69201183f280c5bc458addb Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Fri, 5 Apr 2024 22:50:14 +0100 Subject: [PATCH 15/49] DOC: save work. --- doc/book/random/vector_intrinsics.Rmd | 36 +++++++++++++-------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/doc/book/random/vector_intrinsics.Rmd b/doc/book/random/vector_intrinsics.Rmd index 2ff98df46..0d24b3edf 100644 --- a/doc/book/random/vector_intrinsics.Rmd +++ b/doc/book/random/vector_intrinsics.Rmd @@ -274,7 +274,7 @@ conv_y(x, y) = Halide::sum(conv_x(x, y + k - kr / 2) * kernel(k)); There are two types of parallelisms on the CPU which we can exploit altogether. -1. Multicore processing: +1. *Multicore processing*: This is straightforward to understand. A CPU can be thought as a factory of workers, each one of them being called a CPU core. @@ -286,25 +286,26 @@ There are two types of parallelisms on the CPU which we can exploit altogether. OpenMP is one implementation of multicore processing among others to parallelise our image filter. -2. Vector instructions: +2. *Vector instructions*: - Until I implemented filters with Halide, I really did not understand what CPU - vector instrutions were really about. + Until I implemented filters with Halide, I did not understand what CPU vector + instructions were really about. Mainly, a CPU vector instruction enables a CPU core to perform arithmetic operations on small vectors in a **single** CPU cycle. That means that additions, subtractions, dot products on 4D float vectors can be executed in a single CPU cycle instead of 4 CPU cycles (7 CPU cycles for - the dot product). + the dot product). **(PLEASE RESEARCH THIS MORE: this is not true although the + idea is there...)** - That is very significant on a very large scale as we can observe additional - 4x speed up or more on very very large arrays and therefore image data. + The performance is very significant on a large scale as we can observe + additional 4x speed up or more. Nowadays, an Intel CPU that supports AVX-512 vector instructions can perform operations on 16D vectors of 32-bit floating point data in a single CPU instruction. -So when we combine parallelism 1 and 2 on an 12-core Intel CPU with AVX-512 +By combining parallelisms 1 and 2 on an 12-core Intel CPU with AVX-512 instructions, an optimized algorithm could in principle be sped up by a factor of @@ -314,9 +315,9 @@ of on single-precision floating point data. -While we should instead use Amdahl's law instead of this naive formula, applying -Amdahl's law is not so simple and we will be content to just benchmark and -measure the performance gain. +While we should instead use *Amdahl's law* instead of this naive formula, +applying Amdahl's law is not so simple and we will be content to just benchmark +and measure the performance gain. Likewise, optimized linear algebra routines like OpenBLAS makes extensive use of the two types parallelism on CPU platforms. @@ -518,10 +519,9 @@ conv_y.compile_to_stmt( ``` Halide generates a nicely illustrated HTML document. Still The generated code is -overwhelming as there is a lot to unpack. How are we going to learn how to read -a bit of assembly code. +overwhelming as there is a lot to unpack. How are we going to learn how to read a bit of assembly code? -To digest the algorithmic flow, start by commenting out the different +To digest the algorithmic flow, I started by commenting out the different parallelisms in the schedule: 1. the multicore parallelism @@ -558,12 +558,12 @@ This is how it looks like. style="border: 1px solid #464646;" allowfullscreen> -It is not practical to view the embedded HTML page, so go to this link to fully explore the -compiled statement as a full page: +It is not practical to view the embedded HTML page, so you can click on this +link link to fully explore +the compiled statement as a full page: -### Vectorizing in the Convolution +### Vectorizing the Convolution Upon inspection of the compiled statement, it turns out that the convolution operation is implemented by batch where we calculate 4, 8 or 16 convolved values From 4b9e1cfbc43a1cf3f340619792d9ee51b3797adb Mon Sep 17 00:00:00 2001 From: Odd Kiva Date: Sun, 7 Apr 2024 15:50:55 +0100 Subject: [PATCH 16/49] MAINT: update to Halide 17.0.1 --- build.ps1 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.ps1 b/build.ps1 index 04b14386f..4c5632bd1 100644 --- a/build.ps1 +++ b/build.ps1 @@ -13,7 +13,7 @@ $cmake_toolset = $cmake_vsver[$vsver] $qt_dir = "C:\local\qt-everywhere-src-6.1.2\qtbase" $cudnn_dir = "C:\local\C:\local\cudnn-windows-x86_64-8.8.0.121_cuda12-archive" -$halide_dir = "C:\local\Halide-16.0.0-x86-64-windows" +$halide_dir = "C:\local\Halide-17.0.1-x86-64-windows" $nvidia_codec_sdk_dir = "C:\local\Video_Codec_SDK_12.1.14" $tensorrt_dir = "C:\local\TensorRT-8.6.0.12.Windows10.x86_64.cuda-12.0" From 5a466ca8e49687d4231c0ca7bfb9ed8d1e4aede6 Mon Sep 17 00:00:00 2001 From: Odd Kiva Date: Sun, 7 Apr 2024 15:51:11 +0100 Subject: [PATCH 17/49] WIP: fix compile errors with MSVC. --- cpp/src/DO/Sara/Graphics/ImageDraw.cpp | 2 +- .../DO/Sara/NeuralNetworks/Darknet/Debug.hpp | 28 +++++++++++-------- .../DO/Shakti/Cuda/TensorRT/DarknetParser.cpp | 4 +++ ...st_neuralnetworks_tensorrt_onnx_parser.cpp | 2 ++ 4 files changed, 23 insertions(+), 13 deletions(-) diff --git a/cpp/src/DO/Sara/Graphics/ImageDraw.cpp b/cpp/src/DO/Sara/Graphics/ImageDraw.cpp index 0a0d6db80..85843493c 100644 --- a/cpp/src/DO/Sara/Graphics/ImageDraw.cpp +++ b/cpp/src/DO/Sara/Graphics/ImageDraw.cpp @@ -186,7 +186,7 @@ namespace DO { namespace Sara { const auto fm = QFontMetrics{font}; const auto qstr = QString::QString::fromLocal8Bit(text.c_str()); - const auto qstr_bbox = fm.boundingRect(qstr); + const auto qstr_bbox = QRectF{fm.boundingRect(qstr)}; return { qstr_bbox.x(), diff --git a/cpp/src/DO/Sara/NeuralNetworks/Darknet/Debug.hpp b/cpp/src/DO/Sara/NeuralNetworks/Darknet/Debug.hpp index 286f20cfa..17027f4cf 100644 --- a/cpp/src/DO/Sara/NeuralNetworks/Darknet/Debug.hpp +++ b/cpp/src/DO/Sara/NeuralNetworks/Darknet/Debug.hpp @@ -24,11 +24,12 @@ namespace DO::Sara::Darknet { // CAVEAT: this is sensitive to the CPU architecture endianness. template inline auto write_tensor(const TensorView_& x, - const std::string& filepath) -> void + const std::filesystem::path& filepath) -> void { - auto file = std::ofstream{filepath, std::ios::binary}; + auto file = std::ofstream{filepath.string(), std::ios::binary}; if (!file.is_open()) - throw std::runtime_error{"Error: could not open file: " + filepath + "!"}; + throw std::runtime_error{ + "Error: could not open file: " + filepath.string() + "!"}; // Write the tensor dimension. file.write(reinterpret_cast(x.sizes().data()), @@ -39,11 +40,13 @@ namespace DO::Sara::Darknet { } // CAVEAT: this is sensitive to the CPU architecture endianness. - inline auto read_tensor(const std::string& filepath) -> Tensor_ + inline auto read_tensor(const std::filesystem::path& filepath) + -> Tensor_ { - auto file = std::ifstream{filepath, std::ios::binary}; + auto file = std::ifstream{filepath.string(), std::ios::binary}; if (!file.is_open()) - throw std::runtime_error{"Error: could not open file: " + filepath + "!"}; + throw std::runtime_error{ + "Error: could not open file: " + filepath.string() + "!"}; auto sizes = Eigen::Vector4i{}; file.read(reinterpret_cast(sizes.data()), sizeof(int) * 4); @@ -58,7 +61,8 @@ namespace DO::Sara::Darknet { } // CAVEAT: this is sensitive to the CPU architecture endianness. - inline auto read_all_intermediate_outputs(const std::string& dir_path) + inline auto + read_all_intermediate_outputs(const std::filesystem::path& dir_path) { namespace fs = std::filesystem; @@ -157,9 +161,9 @@ namespace DO::Sara::Darknet { } } - inline auto check_convolutional_weights(const Network& model, - const std::string& data_dirpath) - -> void + inline auto + check_convolutional_weights(const Network& model, + const std::filesystem::path& data_dirpath) -> void { const auto stringify = [](int n) { std::ostringstream ss; @@ -175,9 +179,9 @@ namespace DO::Sara::Darknet { SARA_DEBUG << "Checking convolution weights " << i << std::endl; const auto weights_fp = - data_dirpath + "/kernel-" + stringify(i - 1) + ".bin"; + data_dirpath / ("/kernel-" + stringify(i - 1) + ".bin"); const auto biases_fp = - data_dirpath + "/bias-" + stringify(i - 1) + ".bin"; + data_dirpath / ("/bias-" + stringify(i - 1) + ".bin"); const auto w = read_tensor(weights_fp).reshape(conv->weights.w.sizes()); const auto b = read_tensor(biases_fp); diff --git a/cpp/src/DO/Shakti/Cuda/TensorRT/DarknetParser.cpp b/cpp/src/DO/Shakti/Cuda/TensorRT/DarknetParser.cpp index a2f378ebb..117405767 100644 --- a/cpp/src/DO/Shakti/Cuda/TensorRT/DarknetParser.cpp +++ b/cpp/src/DO/Shakti/Cuda/TensorRT/DarknetParser.cpp @@ -9,6 +9,10 @@ // you can obtain one at http://mozilla.org/MPL/2.0/. // ========================================================================== // +#if (defined(_WIN32) || defined(_WIN32_WCE)) && !defined(NOMINMAX) +# define NOMINMAX +#endif + #include #include #include diff --git a/cpp/test/Shakti/Cuda/TensorRT/test_neuralnetworks_tensorrt_onnx_parser.cpp b/cpp/test/Shakti/Cuda/TensorRT/test_neuralnetworks_tensorrt_onnx_parser.cpp index b04251f1d..88ee67a18 100644 --- a/cpp/test/Shakti/Cuda/TensorRT/test_neuralnetworks_tensorrt_onnx_parser.cpp +++ b/cpp/test/Shakti/Cuda/TensorRT/test_neuralnetworks_tensorrt_onnx_parser.cpp @@ -84,6 +84,8 @@ auto serialize_onnx_model_as_tensort_engine( throw std::runtime_error{"Failed to serialize the ONNX model!"}; if (plan->size() == 0) throw std::runtime_error{"The byte size of the serialized engine is 0!"}; + + return plan; } From 7a99a71ef5d7183fb35d53432e97c2dc53872690 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 7 Apr 2024 16:55:01 +0100 Subject: [PATCH 18/49] WIP: save work. --- .../Sara/MultiViewGeometry/FeatureGraph.hpp | 24 ++++++++++++++----- cpp/src/DO/Sara/SfM/Graph/BundleAdjuster.hpp | 2 +- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp | 11 +++++---- 3 files changed, 26 insertions(+), 11 deletions(-) diff --git a/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.hpp b/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.hpp index c0c5d8a87..264102d73 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.hpp @@ -30,11 +30,17 @@ namespace DO::Sara { //! @defgroup FeatureGraph Feature Correspondence Graph //! @{ - //! @brief Feature global ID (GID). + //! @brief Feature Global ID (GID). struct FeatureGID { - int image_id{-1}; - int local_id{-1}; + using image_id_t = int; + using local_id_t = int; + + static constexpr auto undefined_image_id = -1; + static constexpr auto undefined_local_id = -1; + + image_id_t image_id = undefined_image_id; + local_id_t local_id = undefined_local_id; auto operator==(const FeatureGID& other) const -> bool { @@ -49,11 +55,17 @@ namespace DO::Sara { }; - //! @brief Match global ID (GID). + //! @brief Match Global ID (GID). struct MatchGID { - int ij; - int m; + using image_pair_t = int; + using match_t = int; + + static constexpr auto undefined_image_pair = -1; + static constexpr auto undefined_match = -1; + + image_pair_t ij = undefined_image_pair; + match_t m = undefined_match; auto operator==(const MatchGID& other) const -> bool { diff --git a/cpp/src/DO/Sara/SfM/Graph/BundleAdjuster.hpp b/cpp/src/DO/Sara/SfM/Graph/BundleAdjuster.hpp index 673b9c948..2a0cdba0f 100644 --- a/cpp/src/DO/Sara/SfM/Graph/BundleAdjuster.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/BundleAdjuster.hpp @@ -24,7 +24,7 @@ namespace DO::Sara { private: CameraPoseGraph& _camera_pose_graph; const FeatureGraph& _feature_graph; - PointCloud& _point_cloud; + PointCloudManipulator::PointCloud& _point_cloud; }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp index 9202f4a41..7d90fb4fa 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -28,13 +29,15 @@ namespace DO::Sara { struct RelativePoseEdge { - int src_camera = -1; - int dst_camera = -1; + using camera_id_t = int; + static constexpr auto undefined_camera_id = -1; + + camera_id_t src_camera = undefined_camera_id; + camera_id_t dst_camera = undefined_camera_id; std::vector> _matches; std::vector _inliers; - Eigen::Matrix3d R; - Eigen::Vector3d t; + Motion _motion; }; struct CameraPoseGraph From 32c1b619922d984b36eed4bed8d13b87292772fa Mon Sep 17 00:00:00 2001 From: Odd Kiva Date: Sun, 7 Apr 2024 16:42:21 +0100 Subject: [PATCH 19/49] MAINT: fix compile error with MSVC. --- cpp/examples/Shakti/TensorRT/CMakeLists.txt | 14 +++- cpp/examples/Shakti/TensorRT/downsample.cu | 61 +++++++++++++++++ cpp/examples/Shakti/TensorRT/downsample.hpp | 23 +++++++ ...example.cu => tensorrt_yolov4_example.cpp} | 66 +------------------ 4 files changed, 97 insertions(+), 67 deletions(-) create mode 100644 cpp/examples/Shakti/TensorRT/downsample.cu create mode 100644 cpp/examples/Shakti/TensorRT/downsample.hpp rename cpp/examples/Shakti/TensorRT/{tensorrt_yolov4_example.cu => tensorrt_yolov4_example.cpp} (82%) diff --git a/cpp/examples/Shakti/TensorRT/CMakeLists.txt b/cpp/examples/Shakti/TensorRT/CMakeLists.txt index 85105247b..582b25ea1 100644 --- a/cpp/examples/Shakti/TensorRT/CMakeLists.txt +++ b/cpp/examples/Shakti/TensorRT/CMakeLists.txt @@ -2,9 +2,16 @@ if(NOT CMAKE_CUDA_COMPILER OR NOT TensorRT_FOUND) return() endif() -file(GLOB TRT_SOURCE_FILES FILES *.cu) +# This is a workaround with MSVC to avoid fighting with CMake and CUDA. +add_library(tensorrt_yolov4_utilities downsample.hpp downsample.cu) +target_link_libraries(tensorrt_yolov4_utilities PRIVATE DO::Shakti::Cuda::TensorRT) +set_property(TARGET tensorrt_yolov4_utilities + PROPERTY FOLDER "Examples/Shakti/NeuralNetworks") -foreach(file ${TRT_SOURCE_FILES}) + +list(APPEND TRT_EXAMPLE_FILES FILES tensorrt_yolov4_example.cpp) + +foreach(file ${TRT_EXAMPLE_FILES}) get_filename_component(filename ${file} NAME_WE) add_executable(${filename} ${file}) @@ -24,7 +31,8 @@ foreach(file ${TRT_SOURCE_FILES}) DO::Shakti::Cuda::Utilities DO::Shakti::Cuda::TensorRT tinycolormap - fmt::fmt) + fmt::fmt + tensorrt_yolov4_utilities) set_target_properties( ${filename} PROPERTIES COMPILE_FLAGS ${SARA_DEFINITIONS} diff --git a/cpp/examples/Shakti/TensorRT/downsample.cu b/cpp/examples/Shakti/TensorRT/downsample.cu new file mode 100644 index 000000000..ec2585b54 --- /dev/null +++ b/cpp/examples/Shakti/TensorRT/downsample.cu @@ -0,0 +1,61 @@ +#include "downsample.hpp" + +__global__ auto +naive_downsample_and_transpose_impl(float* out_chw, const std::uint8_t* in_hwc, + const int wout, const int hout, + const int win, const int hin) -> void +{ + const int c = blockIdx.x * blockDim.x + threadIdx.x; + const int yout = blockIdx.y * blockDim.y + threadIdx.y; + const int xout = blockIdx.z * blockDim.z + threadIdx.z; + + if (xout >= wout || yout >= hout || c >= 3) + return; + + const float sx = float(win) / float(wout); + const float sy = float(hin) / float(hout); + + int xin = int(xout * sx + 0.5f); + int yin = int(yout * sy + 0.5f); + + if (xin >= win) + xin = win - 1; + if (yin >= hin) + yin = hin - 1; + + const int gi_out = c * hout * wout + yout * wout + xout; + const int gi_in = yin * win * 3 + xin * 3 + c; + + static constexpr auto normalize_factor = 1 / 255.f; + out_chw[gi_out] = static_cast(in_hwc[gi_in]) * normalize_factor; +} + +auto naive_downsample_and_transpose(CudaManagedTensor3f& tensor_chw_resized_32f, + const CudaManagedTensor3ub& tensor_hwc_8u) + -> void +{ + // Data order: H W C + // 0 1 2 + const auto in_hwc = tensor_hwc_8u.data(); + const auto win = tensor_hwc_8u.sizes()(1); + const auto hin = tensor_hwc_8u.sizes()(0); + + // Data order: C H W + // 0 1 2 + auto out_chw = tensor_chw_resized_32f.data(); + const auto hout = tensor_chw_resized_32f.sizes()(1); + const auto wout = tensor_chw_resized_32f.sizes()(2); + + static const auto threads_per_block = dim3{4, 16, 16}; + static const auto num_blocks = dim3{ + 1, // + (hout + threads_per_block.y - 1) / threads_per_block.y, + (wout + threads_per_block.z - 1) / threads_per_block.z // + }; + + naive_downsample_and_transpose_impl<<>>( + out_chw, in_hwc, // + wout, hout, // + win, hin // + ); +} \ No newline at end of file diff --git a/cpp/examples/Shakti/TensorRT/downsample.hpp b/cpp/examples/Shakti/TensorRT/downsample.hpp new file mode 100644 index 000000000..b6d7bab3a --- /dev/null +++ b/cpp/examples/Shakti/TensorRT/downsample.hpp @@ -0,0 +1,23 @@ +#pragma once + +#if (defined(_WIN32) || defined(_WIN32_WCE)) && !defined(NOMINMAX) +# define NOMINMAX +#endif + +#include +#include + +#include + + +namespace trt = DO::Shakti::TensorRT; + + +using CudaManagedTensor3ub = + trt::InferenceEngine::ManagedTensor; +using CudaManagedTensor3f = trt::InferenceEngine::ManagedTensor; + + +auto naive_downsample_and_transpose(CudaManagedTensor3f& tensor_chw_resized_32f, + const CudaManagedTensor3ub& tensor_hwc_8u) + -> void; \ No newline at end of file diff --git a/cpp/examples/Shakti/TensorRT/tensorrt_yolov4_example.cu b/cpp/examples/Shakti/TensorRT/tensorrt_yolov4_example.cpp similarity index 82% rename from cpp/examples/Shakti/TensorRT/tensorrt_yolov4_example.cu rename to cpp/examples/Shakti/TensorRT/tensorrt_yolov4_example.cpp index 96ea96a46..70f61ccd4 100644 --- a/cpp/examples/Shakti/TensorRT/tensorrt_yolov4_example.cu +++ b/cpp/examples/Shakti/TensorRT/tensorrt_yolov4_example.cpp @@ -9,10 +9,10 @@ // you can obtain one at http://mozilla.org/MPL/2.0/. // ========================================================================== // -#include +#include "downsample.hpp" + #include #include -#include #include #include @@ -41,68 +41,6 @@ using CudaManagedTensor3ub = using CudaManagedTensor3f = trt::InferenceEngine::ManagedTensor; -__global__ auto naive_downsample_and_transpose(float* out_chw, - const std::uint8_t* in_hwc, - const int wout, const int hout, - const int win, const int hin) - -> void -{ - const int c = blockIdx.x * blockDim.x + threadIdx.x; - const int yout = blockIdx.y * blockDim.y + threadIdx.y; - const int xout = blockIdx.z * blockDim.z + threadIdx.z; - - if (xout >= wout || yout >= hout || c >= 3) - return; - - const float sx = float(win) / float(wout); - const float sy = float(hin) / float(hout); - - int xin = int(xout * sx + 0.5f); - int yin = int(yout * sy + 0.5f); - - if (xin >= win) - xin = win - 1; - if (yin >= hin) - yin = hin - 1; - - const int gi_out = c * hout * wout + yout * wout + xout; - const int gi_in = yin * win * 3 + xin * 3 + c; - - static constexpr auto normalize_factor = 1 / 255.f; - out_chw[gi_out] = static_cast(in_hwc[gi_in]) * normalize_factor; -} - -auto naive_downsample_and_transpose(CudaManagedTensor3f& tensor_chw_resized_32f, - const CudaManagedTensor3ub& tensor_hwc_8u) - -> void -{ - // Data order: H W C - // 0 1 2 - const auto in_hwc = tensor_hwc_8u.data(); - const auto win = tensor_hwc_8u.sizes()(1); - const auto hin = tensor_hwc_8u.sizes()(0); - - // Data order: C H W - // 0 1 2 - auto out_chw = tensor_chw_resized_32f.data(); - const auto hout = tensor_chw_resized_32f.sizes()(1); - const auto wout = tensor_chw_resized_32f.sizes()(2); - - static const auto threads_per_block = dim3{4, 16, 16}; - static const auto num_blocks = dim3{ - 1, // - (hout + threads_per_block.y - 1) / threads_per_block.y, - (wout + threads_per_block.z - 1) / threads_per_block.z // - }; - - naive_downsample_and_transpose<<>>( - out_chw, in_hwc, // - wout, hout, // - win, hin // - ); -} - - class Yolo { public: From 18e9f2c6cbd93c2e71a890bee11482c9e8fcc9da Mon Sep 17 00:00:00 2001 From: David OK Date: Sun, 7 Apr 2024 18:28:37 +0100 Subject: [PATCH 20/49] MAINT: fix script. --- cpp/examples/Shakti/TensorRT/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cpp/examples/Shakti/TensorRT/CMakeLists.txt b/cpp/examples/Shakti/TensorRT/CMakeLists.txt index 582b25ea1..ab3f41d93 100644 --- a/cpp/examples/Shakti/TensorRT/CMakeLists.txt +++ b/cpp/examples/Shakti/TensorRT/CMakeLists.txt @@ -4,12 +4,12 @@ endif() # This is a workaround with MSVC to avoid fighting with CMake and CUDA. add_library(tensorrt_yolov4_utilities downsample.hpp downsample.cu) -target_link_libraries(tensorrt_yolov4_utilities PRIVATE DO::Shakti::Cuda::TensorRT) +target_link_libraries(tensorrt_yolov4_utilities + PRIVATE DO::Shakti::Cuda::TensorRT) set_property(TARGET tensorrt_yolov4_utilities PROPERTY FOLDER "Examples/Shakti/NeuralNetworks") - -list(APPEND TRT_EXAMPLE_FILES FILES tensorrt_yolov4_example.cpp) +list(APPEND TRT_EXAMPLE_FILES tensorrt_yolov4_example.cpp) foreach(file ${TRT_EXAMPLE_FILES}) get_filename_component(filename ${file} NAME_WE) From fb91dd58b203ef7ffe1e460e0802508897f6295f Mon Sep 17 00:00:00 2001 From: Odd Kiva Date: Mon, 8 Apr 2024 11:16:59 +0100 Subject: [PATCH 21/49] MAINT: don't force OpenCL usage with NVIDIA hardware. Not worth fighting... --- cpp/src/DO/Shakti/OpenCL/CMakeLists.txt | 4 ++++ cpp/test/Shakti/OpenCL/CMakeLists.txt | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/cpp/src/DO/Shakti/OpenCL/CMakeLists.txt b/cpp/src/DO/Shakti/OpenCL/CMakeLists.txt index 214a04cca..477dcc2d4 100644 --- a/cpp/src/DO/Shakti/OpenCL/CMakeLists.txt +++ b/cpp/src/DO/Shakti/OpenCL/CMakeLists.txt @@ -1,3 +1,7 @@ +if (NOT OpenCL_FOUND) + return() +endif () + file(GLOB DO_Sara_OpenCL_FILES FILES *.hpp) set_source_files_properties(${DO_Sara_OpenCL_FILES} PROPERTIES LANGUAGE CXX) diff --git a/cpp/test/Shakti/OpenCL/CMakeLists.txt b/cpp/test/Shakti/OpenCL/CMakeLists.txt index adab7653f..21d001d70 100644 --- a/cpp/test/Shakti/OpenCL/CMakeLists.txt +++ b/cpp/test/Shakti/OpenCL/CMakeLists.txt @@ -1,3 +1,7 @@ +if (NOT OpenCL_FOUND) + return() +endif () + file(GLOB test_OpenCL_SOURCE_FILES FILES test_*.cpp) foreach(file IN LISTS test_OpenCL_SOURCE_FILES) From 7b9c95ea2c46651563bfb675a50aa0d66301a132 Mon Sep 17 00:00:00 2001 From: Odd Kiva Date: Mon, 8 Apr 2024 12:21:34 +0100 Subject: [PATCH 22/49] MAINT: fix CMake script. --- cpp/examples/Shakti/OpenCL/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cpp/examples/Shakti/OpenCL/CMakeLists.txt b/cpp/examples/Shakti/OpenCL/CMakeLists.txt index b3f945c69..e73e8efc6 100644 --- a/cpp/examples/Shakti/OpenCL/CMakeLists.txt +++ b/cpp/examples/Shakti/OpenCL/CMakeLists.txt @@ -1,2 +1,6 @@ +if (NOT OpenCL_FOUND) + return() +endif () + add_subdirectory(hello_opencl) add_subdirectory(image_processing) From 0f2143a3ffec4889e6201957c8f8e530dbda140b Mon Sep 17 00:00:00 2001 From: Odd Kiva Date: Mon, 8 Apr 2024 12:42:10 +0100 Subject: [PATCH 23/49] MAINT: fix compile warning. --- cpp/test/Sara/Core/test_core_tensor.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/cpp/test/Sara/Core/test_core_tensor.cpp b/cpp/test/Sara/Core/test_core_tensor.cpp index f37afe00a..ea0bdb58a 100644 --- a/cpp/test/Sara/Core/test_core_tensor.cpp +++ b/cpp/test/Sara/Core/test_core_tensor.cpp @@ -113,16 +113,17 @@ BOOST_AUTO_TEST_CASE(test_matrix_case) auto img = Image>{W, H}; // Indexed by (y, x, j, i). auto img_elem = Matrix{}; - std::iota(img_elem.data(), img_elem.data() + M * N, 0); + for (auto i = 0; i < M * N; ++i) + img_elem.data()[i] = static_cast(i); img.flat_array().fill(img_elem); auto m = img.matrix(); - m(0, 0) *= 0; - m(0, 1) *= 1; - m(1, 0) *= 2; - m(1, 1) *= 3; - m(2, 0) *= 4; - m(2, 1) *= 5; + // clang-format off + m(0, 0) *= 0; m(0, 1) *= 1; + m(1, 0) *= 2; m(1, 1) *= 3; + m(2, 0) *= 4; m(2, 1) *= 5; + // clang-format on + /* * [[0, 0, 0], [[0, 2, 4], * [0, 0, 0]] [1, 3, 5]] From f07b4b0bf0625c02892ba82e2de009bbfaa515a2 Mon Sep 17 00:00:00 2001 From: Odd Kiva Date: Mon, 8 Apr 2024 13:09:10 +0100 Subject: [PATCH 24/49] MAINT: fix compile errors with MSVC. --- .../Calibration/OmnidirectionalCameraReprojectionError.hpp | 6 ++++++ .../Calibration/PinholeCameraReprojectionError.hpp | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp index 94fc0c5c4..8d1f17789 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp @@ -9,6 +9,12 @@ // you can obtain one at http://mozilla.org/MPL/2.0/. // ========================================================================== // +#pragma once + +#ifndef GLOG_USE_GLOG_EXPORT +# define GLOG_USE_GLOG_EXPORT +#endif + #include #include diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Calibration/PinholeCameraReprojectionError.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Calibration/PinholeCameraReprojectionError.hpp index 666b13c73..fec158066 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Calibration/PinholeCameraReprojectionError.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Calibration/PinholeCameraReprojectionError.hpp @@ -9,6 +9,12 @@ // you can obtain one at http://mozilla.org/MPL/2.0/. // ========================================================================== // +#pragma once + +#ifndef GLOG_USE_GLOG_EXPORT +# define GLOG_USE_GLOG_EXPORT +#endif + #include #include From cb938e3667fdc7d48005dc8e10b76c94e562e396 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 8 Apr 2024 14:47:56 +0100 Subject: [PATCH 25/49] MAINT: invoke dark magic for CMake and CUDA. --- cpp/src/DO/Sara/UseDOSaraCore.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/src/DO/Sara/UseDOSaraCore.cmake b/cpp/src/DO/Sara/UseDOSaraCore.cmake index 5fed5b867..fbdae9708 100644 --- a/cpp/src/DO/Sara/UseDOSaraCore.cmake +++ b/cpp/src/DO/Sara/UseDOSaraCore.cmake @@ -19,8 +19,8 @@ if(NOT DO_Sara_Core_ADDED) target_compile_options( DO_Sara_Core PUBLIC - $<$,$>:-Xcudafe - "--diag_suppress=20236 --diag_suppress=20012"> + "$<$,$>:SHELL:-Xcudafe --diag_suppress=20236>" + "$<$,$>:SHELL:-Xcudafe --diag_suppress=20012>" $<$:--expt-relaxed-constexpr> ) endif() From f0209b1f9bbc8807fab7da51fdd13095b805a6f0 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 8 Apr 2024 19:55:50 +0100 Subject: [PATCH 26/49] WIP: save work on visual odometry. --- .../visual_odometry_example.cpp | 58 ++++++++------ .../SfM/BuildingBlocks/v2/FeatureTracker.hpp | 39 +++++++++ .../v2/RelativePoseEstimator.hpp | 80 +++++++++++++++++++ cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp | 77 ++++++++++++++++++ cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp | 57 +++++++++---- cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp | 1 + cpp/src/DO/Sara/SfM/Graph/ImageFeatures.hpp | 40 ---------- cpp/src/DO/Sara/UseDOSaraSfM.cmake | 3 +- 8 files changed, 274 insertions(+), 81 deletions(-) create mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/v2/FeatureTracker.hpp create mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/v2/RelativePoseEstimator.hpp create mode 100644 cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp delete mode 100644 cpp/src/DO/Sara/SfM/Graph/ImageFeatures.hpp diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index a6e891510..365e59c04 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -73,11 +73,18 @@ class SingleWindowApp glfwSetWindowSizeCallback(_window, window_size_callback); } - //! @brief Note: RAII does not work on OpenGL applications. - //! - //! So the destructor gets a default implementation and we neeed to explicitly - //! call the terminate method. - ~SingleWindowApp() = default; + ~SingleWindowApp() + { + // Destroy GL objects. + deinit_gl_resources(); + + // Destroy GLFW. + if (_window != nullptr) + glfwDestroyWindow(_window); + + if (_glfw_initialized) + glfwTerminate(); + } auto set_config(const fs::path& video_path, const sara::v2::BrownConradyDistortionModel& camera) @@ -125,17 +132,6 @@ class SingleWindowApp } } - auto terminate() -> void - { - // Destroy GL objects. - deinit_gl_resources(); - - // Destroy GLFW. - if (_window != nullptr) - glfwDestroyWindow(_window); - glfwTerminate(); - } - private: auto init_opengl() -> void { @@ -264,8 +260,13 @@ class SingleWindowApp private: static auto init_glfw() -> void { + if (_glfw_initialized) + throw std::runtime_error{ + "Error: cannot instantiate more than one GLFW application!"}; + // Initialize the windows manager. - if (!glfwInit()) + _glfw_initialized = glfwInit(); + if (!_glfw_initialized) throw std::runtime_error{"Error: failed to initialize GLFW!"}; glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); @@ -292,6 +293,8 @@ class SingleWindowApp } private: + static bool _glfw_initialized; + GLFWwindow* _window = nullptr; //! @brief Framebuffer sizes //! We want to use this and not the window sizes because of MacOS retina @@ -327,6 +330,8 @@ class SingleWindowApp float _point_size = 5.f; }; +bool SingleWindowApp::_glfw_initialized = false; + auto main(int const argc, char** const argv) -> int { @@ -340,7 +345,7 @@ auto main(int const argc, char** const argv) -> int { std::cout << fmt::format("Usage: {} VIDEO_PATH\n", std::string_view{argv[0]}); - return 1; + return EXIT_FAILURE; } const auto video_path = fs::path{argv[1]}; @@ -360,10 +365,17 @@ auto main(int const argc, char** const argv) -> int camera.p() << -0.0003137658969742134, 0.00021943576376532096; } - auto app = SingleWindowApp{{800, 600}, "Odometry: " + video_path.string()}; - app.set_config(video_path, camera); - app.run(); - app.terminate(); + try + { + auto app = SingleWindowApp{{800, 600}, "Odometry: " + video_path.string()}; + app.set_config(video_path, camera); + app.run(); + } + catch (std::exception& e) + { + std::cerr << e.what() << std::endl; + return EXIT_FAILURE; + } - return 0; + return EXIT_SUCCESS; } diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/FeatureTracker.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/FeatureTracker.hpp new file mode 100644 index 000000000..fdac7975e --- /dev/null +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/FeatureTracker.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include + +#include +#include + + +namespace DO::Sara::v2 { + + struct FeatureTracker + { + ImagePyramidParams image_pyr_params = ImagePyramidParams(0); + float sift_nn_ratio = 0.6f; + std::size_t num_matches_max = 1000u; + + auto detect_features(const ImageView& image, + KeypointList& keypoints) const -> void + { + keypoints = compute_sift_keypoints(image, image_pyr_params); + } + + auto match_features(const KeypointList& src_keys, + const KeypointList& dst_keys) const + -> std::vector + { + if (features(src_keys).empty() || features(dst_keys).empty()) + return {}; + + auto matches = match(src_keys, dst_keys, sift_nn_ratio); + if (matches.size() > num_matches_max) + matches.resize(num_matches_max); + + return matches; + } + }; + +} // namespace DO::Sara::v2 diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/RelativePoseEstimator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/RelativePoseEstimator.hpp new file mode 100644 index 000000000..ff3829a3a --- /dev/null +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/RelativePoseEstimator.hpp @@ -0,0 +1,80 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + + +namespace DO::Sara::v2 { + + struct RelativePoseEstimator + { + int ransac_iterations_max = 1000; + double ransac_confidence = 0.999; + double err_thres = 4.; + + // Use Stewenius' algorithm instead of Nister's for now. The polynomial + // solver must have some convergence problems. + const RelativePoseSolver _solver; + CheiralAndEpipolarConsistency _inlier_predicate; + + Eigen::Matrix3d _K; + Eigen::Matrix3d _K_inv; + + RelativePoseEstimator(const v2::BrownConradyDistortionModel& camera) + { + configure(camera); + } + + auto configure(const v2::BrownConradyDistortionModel& camera) + -> void + { + _K = camera.calibration_matrix(); + _K_inv = _K.inverse(); + + _inlier_predicate.distance.K1_inv = _K_inv; + _inlier_predicate.distance.K2_inv = _K_inv; + _inlier_predicate.err_threshold = err_thres; + } + + auto estimate_relative_pose(const KeypointList& src_keys, + const KeypointList& dst_keys, + std::vector& matches) const + -> std::tuple, Tensor_> + { + print_stage("Estimating the relative pose..."); + if (matches.empty()) + { + SARA_DEBUG << "Skipping relative pose estimation\n"; + return {}; + } + + const auto& f0 = features(src_keys); + const auto& f1 = features(dst_keys); + const auto u = std::array{ + homogeneous(extract_centers(f0)).cast(), + homogeneous(extract_centers(f1)).cast() // + }; + // List the matches as a 2D-tensor where each row encodes a match 'm' as a + // pair of point indices (i, j). + const auto M = to_tensor(matches); + + const auto X = PointCorrespondenceList{M, u[0], u[1]}; + auto data_normalizer = + std::make_optional(Normalizer{_K, _K}); + + return v2::ransac(X, _solver, _inlier_predicate, ransac_iterations_max, + ransac_confidence, data_normalizer, true); + } + }; + +} // namespace DO::Sara::v2 diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp new file mode 100644 index 000000000..a77edd418 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp @@ -0,0 +1,77 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2023-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + + +#include + +#include +#include +#include + + +using namespace DO::Sara; + +auto CameraPoseGraph::detect_keypoints( + const v2::FeatureTracker& feature_tracker, + const ImageView& image, // + const int frame_index) -> void + +{ + auto& logger = Logger::get(); + + SARA_LOGI(logger, "Detecting keypoints for image frame {}", frame_index); + + // Grow the pose graph by creating a new camera vertex. + const auto v = boost::add_vertex(_g); + + // Store the camera pose data. + auto& camera_pose_data = _g[v]; + camera_pose_data.frame_index = frame_index; + camera_pose_data.keypoints = compute_sift_keypoints(image); + + const auto& f = features(camera_pose_data.keypoints); + SARA_LOGI(logger, "Camera vertex: {} keypoints", f.size()); +} + +auto CameraPoseGraph::estimate_relative_motion( + const v2::FeatureTracker& feature_tracker, // + const v2::RelativePoseEstimator& relative_pose_estimator, // + const Vertex u, const Vertex v) -> void +{ + auto& logger = Logger::get(); + + SARA_LOGI(logger, "Match features..."); + const auto& src_keys = _g[u].keypoints; + const auto& dst_keys = _g[v].keypoints; + auto matches = feature_tracker.match_features(src_keys, dst_keys); + if (matches.empty()) + return; + + SARA_LOGI(logger, "Estimating relative pose..."); + auto [geometry, inliers, sample_best] = + relative_pose_estimator.estimate_relative_pose(src_keys, dst_keys, + matches); + const auto num_inliers = inliers.flat_array().count(); + SARA_LOGI(logger, "inlier count: {}", num_inliers); + + const auto success = num_inliers > 100; + auto e = Edge{}; + auto edge_added = false; + if (success) + { + std::tie(e, edge_added) = boost::add_edge(u, v, _g); + auto& relative_motion_data = _g[e]; + relative_motion_data.matches = std::move(matches); + relative_motion_data.inliers = std::move(inliers); + relative_motion_data.src_camera = u; + relative_motion_data.dst_camera = v; + } +} diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp index 7d90fb4fa..aa7b4f99b 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp @@ -11,23 +11,33 @@ #pragma once -#include - #include -#include -#include +#include +#include #include -#include +#include -#include +#include +#include -#include -#include +#include namespace DO::Sara { - struct RelativePoseEdge + struct CameraPoseData + { + //! @brief The corresponding image frame index. + int frame_index; + + //! @brief The keypoints detected in the image. + KeypointList keypoints; + + //! @brief "Absolute" pose w.r.t. some reference frame. + QuaternionBasedPose pose; + }; + + struct RelativeMotionData { using camera_id_t = int; static constexpr auto undefined_camera_id = -1; @@ -35,22 +45,35 @@ namespace DO::Sara { camera_id_t src_camera = undefined_camera_id; camera_id_t dst_camera = undefined_camera_id; - std::vector> _matches; - std::vector _inliers; - Motion _motion; + std::vector matches; + Tensor_ inliers; + + Motion motion; }; - struct CameraPoseGraph + class CameraPoseGraph { + public: using GraphImpl = boost::adjacency_list, RelativePoseEdge>; + CameraPoseData, RelativeMotionData>; + + public: using Vertex = boost::graph_traits::vertex_descriptor; using Edge = boost::graph_traits::edge_descriptor; - GraphImpl _pose_graph; - std::vector>> _images; - ImageKeypoints _image_keypoints; + auto detect_keypoints(const v2::FeatureTracker& feature_tracker, + const ImageView& image, // + const int frame_index) -> void; + + auto estimate_relative_motion( + const v2::FeatureTracker& feature_tracker, // + const v2::RelativePoseEstimator& relative_pose_estimator, // + const Vertex src, const Vertex dst) -> void; + + private: + //! @brief The graph data structure shortened as g. + GraphImpl _g; }; } /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp index 96baa8ca3..afd29e130 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp @@ -12,6 +12,7 @@ #pragma once #include + #include diff --git a/cpp/src/DO/Sara/SfM/Graph/ImageFeatures.hpp b/cpp/src/DO/Sara/SfM/Graph/ImageFeatures.hpp deleted file mode 100644 index 83c2ae340..000000000 --- a/cpp/src/DO/Sara/SfM/Graph/ImageFeatures.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include -#include -#include - -#include - - -namespace DO::Sara { - - struct ImageKeypoints - { - auto num_images() const -> int - { - return _num_images; - }; - - auto descriptor_dimension() const -> int - { - return _descriptor_dimension; - } - - auto features(const std::size_t camera_vertex) const - -> std::span; - - auto descriptors(const std::size_t camera_vertex) const - -> TensorView_; - - //! @brief Feature data. - //! @{ - std::vector> _features; - std::vector _descriptors; - //! @} - - int _descriptor_dimension = -1; - int _num_images = -1; - }; - -} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/UseDOSaraSfM.cmake b/cpp/src/DO/Sara/UseDOSaraSfM.cmake index d85d4798f..53c63eec4 100644 --- a/cpp/src/DO/Sara/UseDOSaraSfM.cmake +++ b/cpp/src/DO/Sara/UseDOSaraSfM.cmake @@ -14,7 +14,8 @@ if(SARA_USE_FROM_SOURCE) target_link_libraries( DO_Sara_SfM PRIVATE tinyply Boost::filesystem - PUBLIC DO::Sara::Features + PUBLIC DO::Sara::Logging + DO::Sara::Features DO::Sara::FeatureDetectors DO::Sara::FeatureDescriptors DO::Sara::FeatureMatching From 0d8555e6ca00c04fc5abe475fb6515f818ebdd01 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 8 Apr 2024 20:04:54 +0100 Subject: [PATCH 27/49] WIP: save work. WIP: save work. MAINT: reorganize files. MAINT: reorganize files and trim number of functions. MAINT: fix compile errors. MAINT: clean up code. MAINT: clean up code. MAINT: fix compile errors. MAINT: refactor code. WIP: clean up code. WIP: save work. WIP: save work. WIP: save work. --- .../Sara/MultiViewGeometry/CMakeLists.txt | 7 +- .../essential_5_point_example.cpp | 10 +- .../fundamental_7_point_example.cpp | 5 +- .../relative_pose_estimation_example.cpp | 10 +- .../two_view_bundle_adjustment_example.cpp | 13 +- .../visual_odometry_example_v2.cpp | 381 ++++++++++++++++++ .../SIFT/V2/halide_sift_pyramid_example.cpp | 2 +- .../BundleAdjustmentProblem.hpp | 2 +- .../Sara/MultiViewGeometry/FeatureGraph.cpp | 311 -------------- .../{ => Graph}/EpipolarGraph.cpp | 3 +- .../{ => Graph}/EpipolarGraph.hpp | 0 .../MultiViewGeometry/Graph/FeatureGraph.cpp | 318 +++++++++++++++ .../{ => Graph}/FeatureGraph.hpp | 2 +- cpp/src/DO/Sara/MultiViewGeometry/HDF5.hpp | 2 +- .../BundleAdjuster.hpp | 0 .../EssentialMatrixEstimation.cpp | 345 ---------------- .../Sara/SfM/BuildingBlocks/FeatureParams.hpp | 16 + .../FundamentalMatrixEstimation.cpp | 312 -------------- .../SfM/BuildingBlocks/KeypointDetection.cpp | 91 ----- .../SfM/BuildingBlocks/KeypointMatching.cpp | 109 ----- .../PointCloudManipulator.hpp | 2 +- .../BuildingBlocks/RelativePoseEstimator.cpp | 59 +++ .../BuildingBlocks/RelativePoseEstimator.hpp | 41 ++ .../RgbColoredPoint.hpp | 0 .../SfM/BuildingBlocks/v2/FeatureTracker.hpp | 39 -- .../v2/RelativePoseEstimator.hpp | 80 ---- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp | 36 +- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp | 84 ++-- .../DO/Sara/SfM/Graph/FeatureDisjointSets.hpp | 57 +++ cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp | 41 -- cpp/src/DO/Sara/SfM/Graph/FeatureGraph.cpp | 6 +- cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp | 73 +++- cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp | 189 +++++++++ cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp | 53 +++ .../SfM/{BuildingBlocks.hpp => Helpers.hpp} | 9 +- .../SfM/Helpers/EssentialMatrixEstimation.cpp | 55 +++ .../EssentialMatrixEstimation.hpp | 16 +- .../Helpers/FundamentalMatrixEstimation.cpp | 122 ++++++ .../FundamentalMatrixEstimation.hpp | 14 +- .../KeypointMatching.cpp} | 26 +- .../KeypointMatching.hpp | 6 - .../Triangulation.cpp | 2 +- .../Triangulation.hpp | 0 .../DO/Sara/SfM/Odometry/FeatureTracker.hpp | 2 +- .../SfM/Odometry/RelativePoseEstimator.hpp | 2 +- cpp/src/DO/Sara/SfM/Odometry/Triangulator.hpp | 2 +- .../DO/Sara/SfM/Odometry/VideoStreamer.hpp | 5 + .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 120 ++++++ .../Sara/SfM/OdometryV2/OdometryPipeline.hpp | 76 ++++ .../test_multiviewgeometry_feature_graph.cpp | 31 +- 50 files changed, 1697 insertions(+), 1490 deletions(-) create mode 100644 cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp delete mode 100644 cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.cpp rename cpp/src/DO/Sara/MultiViewGeometry/{ => Graph}/EpipolarGraph.cpp (99%) rename cpp/src/DO/Sara/MultiViewGeometry/{ => Graph}/EpipolarGraph.hpp (100%) create mode 100644 cpp/src/DO/Sara/MultiViewGeometry/Graph/FeatureGraph.cpp rename cpp/src/DO/Sara/MultiViewGeometry/{ => Graph}/FeatureGraph.hpp (98%) rename cpp/src/DO/Sara/SfM/{Graph => BuildingBlocks}/BundleAdjuster.hpp (100%) delete mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.cpp create mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp delete mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.cpp delete mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointDetection.cpp delete mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointMatching.cpp rename cpp/src/DO/Sara/SfM/{Graph => BuildingBlocks}/PointCloudManipulator.hpp (95%) create mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp create mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp rename cpp/src/DO/Sara/SfM/{Graph => BuildingBlocks}/RgbColoredPoint.hpp (100%) delete mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/v2/FeatureTracker.hpp delete mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/v2/RelativePoseEstimator.hpp create mode 100644 cpp/src/DO/Sara/SfM/Graph/FeatureDisjointSets.hpp delete mode 100644 cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp create mode 100644 cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp create mode 100644 cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp rename cpp/src/DO/Sara/SfM/{BuildingBlocks.hpp => Helpers.hpp} (62%) create mode 100644 cpp/src/DO/Sara/SfM/Helpers/EssentialMatrixEstimation.cpp rename cpp/src/DO/Sara/SfM/{BuildingBlocks => Helpers}/EssentialMatrixEstimation.hpp (59%) create mode 100644 cpp/src/DO/Sara/SfM/Helpers/FundamentalMatrixEstimation.cpp rename cpp/src/DO/Sara/SfM/{BuildingBlocks => Helpers}/FundamentalMatrixEstimation.hpp (73%) rename cpp/src/DO/Sara/SfM/{BuildingBlocks/KeypointDetection.hpp => Helpers/KeypointMatching.cpp} (58%) rename cpp/src/DO/Sara/SfM/{BuildingBlocks => Helpers}/KeypointMatching.hpp (82%) rename cpp/src/DO/Sara/SfM/{BuildingBlocks => Helpers}/Triangulation.cpp (99%) rename cpp/src/DO/Sara/SfM/{BuildingBlocks => Helpers}/Triangulation.hpp (100%) create mode 100644 cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp create mode 100644 cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp diff --git a/cpp/examples/Sara/MultiViewGeometry/CMakeLists.txt b/cpp/examples/Sara/MultiViewGeometry/CMakeLists.txt index 91fb31c21..258c5278a 100644 --- a/cpp/examples/Sara/MultiViewGeometry/CMakeLists.txt +++ b/cpp/examples/Sara/MultiViewGeometry/CMakeLists.txt @@ -49,8 +49,11 @@ target_link_libraries(essential_5_point_example PRIVATE tinyply) # Visual odometry. sara_add_example(visual_odometry_example) target_link_libraries(visual_odometry_example PRIVATE DO::Kalpana::EasyGL # - fmt::fmt - glfw) + fmt::fmt glfw) + +sara_add_example(visual_odometry_example_v2) +target_link_libraries(visual_odometry_example_v2 PRIVATE DO::Kalpana::EasyGL # + fmt::fmt glfw) # Bundle adjustment. sara_add_example(two_view_bundle_adjustment_example) diff --git a/cpp/examples/Sara/MultiViewGeometry/essential_5_point_example.cpp b/cpp/examples/Sara/MultiViewGeometry/essential_5_point_example.cpp index e5dafa087..8094bfed0 100644 --- a/cpp/examples/Sara/MultiViewGeometry/essential_5_point_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/essential_5_point_example.cpp @@ -16,14 +16,14 @@ #include #include #include -#include +#include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include diff --git a/cpp/examples/Sara/MultiViewGeometry/fundamental_7_point_example.cpp b/cpp/examples/Sara/MultiViewGeometry/fundamental_7_point_example.cpp index 36943ae7f..888fc6f5a 100644 --- a/cpp/examples/Sara/MultiViewGeometry/fundamental_7_point_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/fundamental_7_point_example.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include using namespace std; @@ -89,7 +89,8 @@ auto estimate_fundamental_matrix(const KeypointList& keys1, const auto data_normalizer = std::make_optional(Normalizer{X}); - auto inlier_predicate = InlierPredicate{}; + auto inlier_predicate = + InlierPredicate{}; inlier_predicate.err_threshold = f_err_thres; const auto [F, inliers, sample_best] = diff --git a/cpp/examples/Sara/MultiViewGeometry/relative_pose_estimation_example.cpp b/cpp/examples/Sara/MultiViewGeometry/relative_pose_estimation_example.cpp index 163e553f3..3a5da0b7f 100644 --- a/cpp/examples/Sara/MultiViewGeometry/relative_pose_estimation_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/relative_pose_estimation_example.cpp @@ -23,16 +23,16 @@ #include #include #include -#include +#include #include #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include using namespace std::string_literals; diff --git a/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp b/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp index a2e7eefec..539b6920e 100644 --- a/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp @@ -15,15 +15,14 @@ #include #include #include -#include -#include +#include +#include #include #include - -#include -#include -#include -#include +#include +#include +#include +#include #if defined(_WIN32) # pragma warning(push, 0) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp new file mode 100644 index 000000000..1c7f50309 --- /dev/null +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp @@ -0,0 +1,381 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2023 David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#if defined(_WIN32) +# include +#endif + +#include + +#include + +#include + +#if defined(_OPENMP) +# include +#endif + + +namespace fs = std::filesystem; +namespace sara = DO::Sara; +namespace k = DO::Kalpana; +namespace kgl = DO::Kalpana::GL; + + +class SingleWindowApp +{ +public: + SingleWindowApp(const Eigen::Vector2i& sizes, const std::string& title) + { + // Init GLFW. + init_glfw(); + + // Create a window. + _window = glfwCreateWindow(sizes.x(), sizes.y(), // + title.c_str(), // + nullptr, nullptr); + + _fb_sizes = get_framebuffer_sizes(); + + // Initialize the point cloud viewport. + _point_cloud_viewport.top_left().setZero(); + _point_cloud_viewport.sizes() << _fb_sizes.x() / 2, _fb_sizes.y(); + + // Initialize the video viewport. + _video_viewport.top_left() << _fb_sizes.x() / 2, 0; + _video_viewport.sizes() << _fb_sizes.x() / 2, _fb_sizes.y(); + + // Prepare OpenGL first before any OpenGL calls. + init_opengl(); + + // The magic function. + glfwSetWindowUserPointer(_window, this); + // Register callbacks. + glfwSetWindowSizeCallback(_window, window_size_callback); + } + + ~SingleWindowApp() + { + // Destroy GL objects. + deinit_gl_resources(); + + // Destroy GLFW. + if (_window != nullptr) + glfwDestroyWindow(_window); + + if (_glfw_initialized) + glfwTerminate(); + } + + auto set_config(const fs::path& video_path, + const sara::v2::BrownConradyDistortionModel& camera) + -> void + { + _pipeline.set_config(video_path, camera); + init_gl_resources(); + } + + auto run() -> void + { + // Current projection matrix + _projection = _video_viewport.orthographic_projection(); + _point_cloud_projection = _point_cloud_viewport.orthographic_projection(); + + // Background color. + glClearColor(0.2f, 0.3f, 0.3f, 1.0f); + + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glEnable(GL_PROGRAM_POINT_SIZE); + + // You absolutely need this for 3D objects! + glEnable(GL_DEPTH_TEST); + + // Display image. + glfwSwapInterval(1); + while (!glfwWindowShouldClose(_window)) + { + if (!_pipeline.read()) + break; + + _pipeline.process(); + // Load data to OpenGL. + upload_point_cloud_data_to_opengl(); + + // Clear the color buffer and the buffer testing. + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + render_video(); + render_point_cloud(); + + glfwSwapBuffers(_window); + glfwPollEvents(); + } + } + +private: + auto init_opengl() -> void + { + glfwMakeContextCurrent(_window); + init_glew(); + } + + auto init_gl_resources() -> void + { + // Video texture rendering + _texture.initialize(_pipeline._video_streamer.frame_rgb8(), 0); + + const auto& w = _pipeline._video_streamer.width(); + const auto& h = _pipeline._video_streamer.height(); + const auto aspect_ratio = static_cast(w) / h; + auto vertices = _quad.host_vertices().matrix(); + vertices.col(0) *= aspect_ratio; + _quad.initialize(); + + _texture_renderer.initialize(); + + // Point cloud rendering + _point_cloud.initialize(); + _point_cloud_renderer.initialize(); + } + + auto deinit_gl_resources() -> void + { + _texture.destroy(); + _quad.destroy(); + _texture_renderer.destroy(); + + _point_cloud.destroy(); + _point_cloud_renderer.destroy(); + } + + auto upload_point_cloud_data_to_opengl() -> void + { + // _point_cloud.upload_host_data_to_gl( + // _pipeline._triangulator->_colored_point_cloud); + } + + auto render_video() -> void + { + // Render on the right half of the window surface. + glViewport(_video_viewport.x(), _video_viewport.y(), // + _video_viewport.width(), _video_viewport.height()); + // Transfer the CPU image frame data to the OpenGL texture. + // _texture.reset(_pipeline._video_stream.frame_rgb8()); + _texture.reset(_pipeline.make_display_frame()); + // Render the texture on the quad. + auto model_view = Eigen::Transform{}; + model_view.setIdentity(); + _texture_renderer.render(_texture, _quad, model_view.matrix(), _projection); + } + + auto render_point_cloud() -> void + { + glViewport(_point_cloud_viewport.x(), _point_cloud_viewport.y(), + _point_cloud_viewport.width(), _point_cloud_viewport.height()); + + // CAVEAT: re-express the point cloud in OpenGL axis convention. + auto from_cam_to_gl = Eigen::Transform{}; + from_cam_to_gl.setIdentity(); + // clang-format off + from_cam_to_gl.matrix().topLeftCorner<3, 3>() << + 1, 0, 0, + 0, -1, 0, + 0, 0, -1; + // clang-format on + + // Update the model view matrix. + const Eigen::Matrix4f model_view = Eigen::Matrix4f::Identity(); + + // Render the point cloud. + _point_cloud_renderer.render(_point_cloud, _point_size, + from_cam_to_gl.matrix(), // + model_view, _point_cloud_projection); + } + + auto get_framebuffer_sizes() const -> Eigen::Vector2i + { + auto sizes = Eigen::Vector2i{}; + glfwGetFramebufferSize(_window, &sizes.x(), &sizes.y()); + return sizes; + } + +private: + static auto get_self(GLFWwindow* const window) -> SingleWindowApp& + { + const auto app_void_ptr = glfwGetWindowUserPointer(window); + if (app_void_ptr == nullptr) + throw std::runtime_error{ + "Please call glfwSetWindowUserPointer to register this window!"}; + const auto app_ptr = reinterpret_cast(app_void_ptr); + return *app_ptr; + } + + static auto window_size_callback(GLFWwindow* window, const int, const int) + -> void + { + auto& self = get_self(window); + + auto& fb_sizes = self._fb_sizes; + fb_sizes = self.get_framebuffer_sizes(); + + // Point cloud viewport rectangle geometry. + self._point_cloud_viewport.top_left().setZero(); + self._point_cloud_viewport.sizes() << fb_sizes.x() / 2, fb_sizes.y(); + + // Video viewport rectangle geometry. + self._video_viewport.top_left() << fb_sizes.x() / 2, 0; + self._video_viewport.sizes() << fb_sizes.x() / 2, fb_sizes.y(); + + // Update the current projection matrices. + auto scale = 0.5f; + if (self._video_viewport.width() < self._pipeline._video_streamer.width()) + scale *= static_cast(self._pipeline._video_streamer.width()) / + self._video_viewport.width(); + self._projection = self._video_viewport.orthographic_projection(scale); + + // Point cloud projection matrix. + self._point_cloud_projection = self._point_cloud_viewport.perspective(); + } + +private: + static auto init_glfw() -> void + { + if (_glfw_initialized) + throw std::runtime_error{ + "Error: cannot instantiate more than one GLFW application!"}; + + // Initialize the windows manager. + _glfw_initialized = glfwInit(); + if (!_glfw_initialized) + throw std::runtime_error{"Error: failed to initialize GLFW!"}; + + glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); + glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); + glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); +#if defined(__APPLE__) + glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); +#endif + } + + static auto init_glew() -> void + { +#if !defined(__APPLE__) + // Initialize GLEW. + const auto err = glewInit(); + if (err != GLEW_OK) + { + const auto err_str = + reinterpret_cast(glewGetErrorString(err)); + throw std::runtime_error{fmt::format( + "Error: failed to initialize GLEW: {}", std::string_view{err_str})}; + } +#endif + } + +private: + static bool _glfw_initialized; + + GLFWwindow* _window = nullptr; + //! @brief Framebuffer sizes + //! We want to use this and not the window sizes because of MacOS retina + //! display. + Eigen::Vector2i _fb_sizes = -Eigen::Vector2i::Ones(); + + sara::v2::OdometryPipeline _pipeline; + + //! Video rendering + //! + //! The viewport + k::Viewport _video_viewport; + //! @brief the video texture. + kgl::TexturedImage2D _texture; + //! @brief the video quad. + kgl::TexturedQuad _quad; + //! @brief Texture renderer. + kgl::TextureRenderer _texture_renderer; + //! @brief Model-view-projection matrices. + Eigen::Matrix4f _projection; + + //! Point cloud rendering + //! + //! @brief The viewport. + k::Viewport _point_cloud_viewport; + //! @brief Point cloud GPU data. + kgl::ColoredPointCloud _point_cloud; + //! @brief Point cloud GPU renderer. + kgl::ColoredPointCloudRenderer _point_cloud_renderer; + //! @brief Point cloud rendering options. + Eigen::Matrix4f _point_cloud_projection; + // kgl::Camera _point_cloud_camera; + float _point_size = 5.f; +}; + +bool SingleWindowApp::_glfw_initialized = false; + + +auto main(int const argc, char** const argv) -> int +{ +#if defined(_OPENMP) + const auto num_threads = omp_get_max_threads(); + omp_set_num_threads(num_threads); + Eigen::setNbThreads(num_threads); +#endif + + if (argc < 2) + { + std::cout << fmt::format("Usage: {} VIDEO_PATH\n", + std::string_view{argv[0]}); + return EXIT_FAILURE; + } + + const auto video_path = fs::path{argv[1]}; + auto camera = sara::v2::BrownConradyDistortionModel{}; + { + camera.fx() = 917.2878392016245; + camera.fy() = 917.2878392016245; + camera.shear() = 0.; + camera.u0() = 960.; + camera.v0() = 540.; + // clang-format off + camera.k() << + -0.2338367557617234, + 0.05952465745165465, + -0.007947847982157091; + // clang-format on + camera.p() << -0.0003137658969742134, 0.00021943576376532096; + } + + try + { + auto app = SingleWindowApp{{800, 600}, "Odometry: " + video_path.string()}; + app.set_config(video_path, camera); + app.run(); + } + catch (std::exception& e) + { + std::cerr << e.what() << std::endl; + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} diff --git a/cpp/examples/Shakti/Halide/SIFT/V2/halide_sift_pyramid_example.cpp b/cpp/examples/Shakti/Halide/SIFT/V2/halide_sift_pyramid_example.cpp index 1c3c0c0ae..79fbfb429 100644 --- a/cpp/examples/Shakti/Halide/SIFT/V2/halide_sift_pyramid_example.cpp +++ b/cpp/examples/Shakti/Halide/SIFT/V2/halide_sift_pyramid_example.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include diff --git a/cpp/src/DO/Sara/MultiViewGeometry/BundleAdjustmentProblem.hpp b/cpp/src/DO/Sara/MultiViewGeometry/BundleAdjustmentProblem.hpp index 92ee6f733..c296da689 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/BundleAdjustmentProblem.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/BundleAdjustmentProblem.hpp @@ -12,7 +12,7 @@ #pragma once #include -#include +#include #include diff --git a/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.cpp b/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.cpp deleted file mode 100644 index 41bc34dcb..000000000 --- a/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.cpp +++ /dev/null @@ -1,311 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2019 David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#include -#include -#include - - -namespace DO::Sara { - -auto populate_feature_gids( - const std::vector>& keypoints) - -> std::vector -{ - const auto image_ids = range(static_cast(keypoints.size())); - - auto populate_gids = [&](auto image_id) { - const auto num_features = - static_cast(features(keypoints[image_id]).size()); - auto lids = range(num_features); - auto gids = std::vector(lids.size()); - std::transform(std::begin(lids), std::end(lids), std::begin(gids), - [&](auto lid) -> FeatureGID { - return {image_id, lid}; - }); - return gids; - }; - - const auto gids = - std::accumulate(std::begin(image_ids), std::end(image_ids), // - std::vector{}, // - [&](const auto& gids, const auto image_id) { - auto gids_union = gids; - ::append(gids_union, populate_gids(image_id)); - return gids_union; - }); - - return gids; -} - -auto calculate_feature_id_offsets( - const std::vector>& keypoints) - -> std::vector -{ - auto fid_offsets = std::vector(keypoints.size(), 0); - std::transform(std::begin(keypoints), std::end(keypoints) - 1, - std::begin(fid_offsets) + 1, [](const auto& keypoints) { - return static_cast(features(keypoints).size()); - }); - - std::partial_sum(std::begin(fid_offsets), std::end(fid_offsets), - std::begin(fid_offsets)); - - return fid_offsets; -} - -auto populate_feature_tracks(const ViewAttributes& view_attributes, - const EpipolarEdgeAttributes& epipolar_edges) - -> std::pair>> -{ - const auto& keypoints = view_attributes.keypoints; - - const auto gids = populate_feature_gids(keypoints); - const auto num_keypoints = gids.size(); - - // Populate the vertices. - const auto feature_ids = range(static_cast(num_keypoints)); - auto graph = FeatureGraph{num_keypoints}; - // Fill the GID attribute for each vertex. - std::for_each(std::begin(feature_ids), std::end(feature_ids), - [&](auto v) { graph[v] = gids[v]; }); - - const auto feature_id_offset = calculate_feature_id_offsets(keypoints); - - // Incremental connected components. - using ICC = IncrementalConnectedComponentsHelper; - auto rank = ICC::initialize_ranks(graph); - auto parent = ICC::initialize_parents(graph); - auto ds = ICC::initialize_disjoint_sets(rank, parent); - ICC::initialize_incremental_components(graph, ds); - - auto add_edge = [&](auto u, auto v) { - boost::add_edge(u, v, graph); - ds.union_set(u, v); - }; - - const auto& edge_ids = epipolar_edges.edge_ids; - const auto& edges = epipolar_edges.edges; - const auto& matches = epipolar_edges.matches; - const auto& E_inliers = epipolar_edges.E_inliers; - const auto& two_view_geometries = epipolar_edges.two_view_geometries; - - // Populate the edges. - std::for_each(std::begin(edge_ids), std::end(edge_ids), [&](const auto& ij) { - const auto& eij = edges[ij]; - const auto i = eij.first; - const auto j = eij.second; - const auto& Mij = matches[ij]; - const auto& inliers_ij = E_inliers[ij]; - const auto& cheirality_ij = two_view_geometries[ij].cheirality; - - std::cout << std::endl; - SARA_DEBUG << "Processing image pair " << i << " " << j << std::endl; - - SARA_DEBUG << "Checking if there are inliers..." << std::endl; - SARA_CHECK(cheirality_ij.count()); - SARA_CHECK(inliers_ij.flat_array().count()); - if (inliers_ij.flat_array().count() == 0) - return; - - SARA_DEBUG << "Calculating cheiral inliers..." << std::endl; - SARA_CHECK(cheirality_ij.size()); - SARA_CHECK(inliers_ij.size()); - static_assert(std::is_same_v); - if (static_cast(cheirality_ij.size()) != inliers_ij.size()) - throw std::runtime_error{"cheirality_ij.size() != inliers_ij.size()"}; - - const Array cheiral_inliers = - inliers_ij.row_vector().array() && cheirality_ij; - SARA_CHECK(cheiral_inliers.size()); - SARA_CHECK(cheiral_inliers.count()); - - // Convert each match 'm' to a pair of point indices '(p, q)'. - SARA_DEBUG << "Transforming matches..." << std::endl; - const auto pq_tensor = to_tensor(Mij); - SARA_CHECK(Mij.size()); - SARA_CHECK(pq_tensor.size(0)); - - if (pq_tensor.empty()) - return; - - SARA_DEBUG << "Updating disjoint sets..." << std::endl; - for (int m = 0; m < pq_tensor.size(0); ++m) - { - if (!cheiral_inliers(m)) - continue; - - const auto p = pq_tensor(m, 0); - const auto q = pq_tensor(m, 1); - - const auto &p_off = feature_id_offset[i]; - const auto &q_off = feature_id_offset[j]; - - const auto vp = p_off + p; - const auto vq = q_off + q; - - // Runtime checks. - if (graph[vp].image_id != i) - throw std::runtime_error{"image_id[vp] != i"}; - if (graph[vp].local_id != p) - throw std::runtime_error{"local_id[vp] != p"}; - - if (graph[vq].image_id != j) - throw std::runtime_error{"image_id[vq] != j"}; - if (graph[vq].local_id != q) - throw std::runtime_error{"local_id[vq] != q"}; - - // Update the graph and the disjoint sets. - add_edge(vp, vq); - } - }); - - // Calculate the connected components. - auto components = std::vector>{}; - { - const auto components_tmp = ICC::get_components(parent); - components.resize(components_tmp.size()); - for (auto c : components_tmp) - for (auto [child, child_end] = components_tmp[c]; child != child_end; ++child) - components[c].push_back(static_cast(*child)); - } - - - return {graph, components}; -} - -auto filter_feature_tracks(const FeatureGraph& graph, - const std::vector>& components, - ViewAttributes& views) - -> std::set> -{ - // Populate the feature tracks regardless of their cardinality. - auto feature_tracks = std::set>{}; - for (const auto& component : components) - { - auto feature_track = std::set{}; - std::transform(component.begin(), component.end(), - std::inserter(feature_track, std::begin(feature_track)), - [&](const auto v) { return graph[v]; }); - - feature_tracks.insert(feature_track); - } - - // Remove redundant features across images. - using ImageID = int; - using FeatureID = int; - - auto filtered_feature_tracks_dict = std::set>{}; - for (const auto& track : feature_tracks) - { - auto filtered_track = std::map{}; - for (const auto& f : track) - { - const auto current_feature = filtered_track.find(f.image_id); - const auto current_feature_found = - current_feature != filtered_track.end(); - - if (!current_feature_found) - filtered_track[f.image_id] = f.local_id; - - // Replace the feature if the response is stronger. - else // image_id == f.image_id - { - const auto& features_list = features(views.keypoints[f.image_id]); - - // The feature local IDs. - const auto& current_feature_id = current_feature->second; - const auto current_feature_response = - std::abs(features_list[current_feature_id].extremum_value); - - const auto feature_response = - std::abs(features_list[f.local_id].extremum_value); - - if (feature_response > current_feature_response) - filtered_track[f.image_id] = f.local_id; - } - } - - filtered_feature_tracks_dict.insert(filtered_track); - } - - // Transform the filtered feature tracks again. - auto filtered_feature_tracks = std::set>{}; - for (const auto& track_dict : filtered_feature_tracks_dict) - { - if (track_dict.size() == 1) - continue; - - auto track_set = std::set{}; - for (const auto& gid : track_dict) - track_set.insert({gid.first, gid.second}); - filtered_feature_tracks.insert(track_set); - } - - // Replace the feature tracks. - feature_tracks.swap(filtered_feature_tracks); - - return feature_tracks; -} - - -template <> -struct CalculateH5Type -{ - static inline auto value() -> H5::CompType - { - auto h5_comp_type = H5::CompType{sizeof(FeatureGID)}; - INSERT_MEMBER(h5_comp_type, FeatureGID, image_id); - INSERT_MEMBER(h5_comp_type, FeatureGID, local_id); - return h5_comp_type; - } -}; - - -auto write_feature_graph(const FeatureGraph& graph, H5File& file, - const std::string& group_name) -> void -{ - auto features = std::vector(boost::num_vertices(graph)); - for (auto [v, v_end] = boost::vertices(graph); v != v_end; ++v) - features[*v] = {graph[*v].image_id, graph[*v].local_id}; - - auto matches = std::vector{}; - for (auto [e, e_end] = boost::edges(graph); e != e_end; ++e) - matches.push_back({boost::source(*e, graph), boost::target(*e, graph)}); - - file.get_group(group_name); - file.write_dataset(group_name + "/" + "features", tensor_view(features)); - file.write_dataset(group_name + "/" + "matches", tensor_view(matches)); -} - - -auto read_feature_graph(H5File& file, const std::string& group_name) - -> FeatureGraph -{ - auto features = std::vector{}; - auto matches = std::vector{}; - - file.read_dataset(group_name + "/" + "features", features); - file.read_dataset(group_name + "/" + "matches", matches); - - // Reconstruct the graph. - auto g = FeatureGraph{}; - - for (const auto& v : features) - boost::add_vertex(v, g); - - for (const auto& e : matches) - boost::add_edge(e(0), e(1), g); - - return g; -} - -} /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/MultiViewGeometry/EpipolarGraph.cpp b/cpp/src/DO/Sara/MultiViewGeometry/Graph/EpipolarGraph.cpp similarity index 99% rename from cpp/src/DO/Sara/MultiViewGeometry/EpipolarGraph.cpp rename to cpp/src/DO/Sara/MultiViewGeometry/Graph/EpipolarGraph.cpp index b303d61d1..45d62a58c 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/EpipolarGraph.cpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Graph/EpipolarGraph.cpp @@ -9,11 +9,12 @@ // you can obtain one at http://mozilla.org/MPL/2.0/. // ========================================================================== // +#include + #include #include #include #include -#include #include diff --git a/cpp/src/DO/Sara/MultiViewGeometry/EpipolarGraph.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Graph/EpipolarGraph.hpp similarity index 100% rename from cpp/src/DO/Sara/MultiViewGeometry/EpipolarGraph.hpp rename to cpp/src/DO/Sara/MultiViewGeometry/Graph/EpipolarGraph.hpp diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Graph/FeatureGraph.cpp b/cpp/src/DO/Sara/MultiViewGeometry/Graph/FeatureGraph.cpp new file mode 100644 index 000000000..3b9e4c23c --- /dev/null +++ b/cpp/src/DO/Sara/MultiViewGeometry/Graph/FeatureGraph.cpp @@ -0,0 +1,318 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2019 David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#include + +#include +#include + + +namespace DO::Sara { + + auto populate_feature_gids( + const std::vector>& keypoints) + -> std::vector + { + const auto image_ids = range(static_cast(keypoints.size())); + + auto populate_gids = [&](auto image_id) { + const auto num_features = + static_cast(features(keypoints[image_id]).size()); + auto lids = range(num_features); + auto gids = std::vector(lids.size()); + std::transform(std::begin(lids), std::end(lids), std::begin(gids), + [&](auto lid) -> FeatureGID { + return {image_id, lid}; + }); + return gids; + }; + + const auto gids = + std::accumulate(std::begin(image_ids), std::end(image_ids), // + std::vector{}, // + [&](const auto& gids, const auto image_id) { + auto gids_union = gids; + ::append(gids_union, populate_gids(image_id)); + return gids_union; + }); + + return gids; + } + + auto calculate_feature_id_offsets( + const std::vector>& keypoints) + -> std::vector + { + auto fid_offsets = std::vector(keypoints.size(), 0); + std::transform(std::begin(keypoints), std::end(keypoints) - 1, + std::begin(fid_offsets) + 1, [](const auto& keypoints) { + return static_cast(features(keypoints).size()); + }); + + std::partial_sum(std::begin(fid_offsets), std::end(fid_offsets), + std::begin(fid_offsets)); + + return fid_offsets; + } + + auto populate_feature_tracks(const ViewAttributes& view_attributes, + const EpipolarEdgeAttributes& epipolar_edges) + -> std::pair>> + { + const auto& keypoints = view_attributes.keypoints; + + const auto gids = populate_feature_gids(keypoints); + const auto num_keypoints = gids.size(); + + // Populate the vertices. + const auto feature_ids = range(static_cast(num_keypoints)); + auto graph = FeatureGraph{num_keypoints}; + // Fill the GID attribute for each vertex. + std::for_each(std::begin(feature_ids), std::end(feature_ids), + [&](auto v) { graph[v] = gids[v]; }); + + const auto feature_id_offset = calculate_feature_id_offsets(keypoints); + + // Incremental connected components. + using ICC = IncrementalConnectedComponentsHelper; + auto rank = ICC::initialize_ranks(graph); + auto parent = ICC::initialize_parents(graph); + auto ds = ICC::initialize_disjoint_sets(rank, parent); + ICC::initialize_incremental_components(graph, ds); + + auto add_edge = [&](auto u, auto v) { + boost::add_edge(u, v, graph); + ds.union_set(u, v); + }; + + const auto& edge_ids = epipolar_edges.edge_ids; + const auto& edges = epipolar_edges.edges; + const auto& matches = epipolar_edges.matches; + const auto& E_inliers = epipolar_edges.E_inliers; + const auto& two_view_geometries = epipolar_edges.two_view_geometries; + + // Populate the edges. + std::for_each( + std::begin(edge_ids), std::end(edge_ids), [&](const auto& ij) { + const auto& eij = edges[ij]; + const auto i = eij.first; + const auto j = eij.second; + const auto& Mij = matches[ij]; + const auto& inliers_ij = E_inliers[ij]; + const auto& cheirality_ij = two_view_geometries[ij].cheirality; + + std::cout << std::endl; + SARA_DEBUG << "Processing image pair " << i << " " << j << std::endl; + + SARA_DEBUG << "Checking if there are inliers..." << std::endl; + SARA_CHECK(cheirality_ij.count()); + SARA_CHECK(inliers_ij.flat_array().count()); + if (inliers_ij.flat_array().count() == 0) + return; + + SARA_DEBUG << "Calculating cheiral inliers..." << std::endl; + SARA_CHECK(cheirality_ij.size()); + SARA_CHECK(inliers_ij.size()); + static_assert( + std::is_same_v); + if (static_cast(cheirality_ij.size()) != + inliers_ij.size()) + throw std::runtime_error{ + "cheirality_ij.size() != inliers_ij.size()"}; + + const Array cheiral_inliers = + inliers_ij.row_vector().array() && cheirality_ij; + SARA_CHECK(cheiral_inliers.size()); + SARA_CHECK(cheiral_inliers.count()); + + // Convert each match 'm' to a pair of point indices '(p, q)'. + SARA_DEBUG << "Transforming matches..." << std::endl; + const auto pq_tensor = to_tensor(Mij); + SARA_CHECK(Mij.size()); + SARA_CHECK(pq_tensor.size(0)); + + if (pq_tensor.empty()) + return; + + SARA_DEBUG << "Updating disjoint sets..." << std::endl; + for (int m = 0; m < pq_tensor.size(0); ++m) + { + if (!cheiral_inliers(m)) + continue; + + const auto p = pq_tensor(m, 0); + const auto q = pq_tensor(m, 1); + + const auto& p_off = feature_id_offset[i]; + const auto& q_off = feature_id_offset[j]; + + const auto vp = p_off + p; + const auto vq = q_off + q; + + // Runtime checks. + if (graph[vp].image_id != i) + throw std::runtime_error{"image_id[vp] != i"}; + if (graph[vp].local_id != p) + throw std::runtime_error{"local_id[vp] != p"}; + + if (graph[vq].image_id != j) + throw std::runtime_error{"image_id[vq] != j"}; + if (graph[vq].local_id != q) + throw std::runtime_error{"local_id[vq] != q"}; + + // Update the graph and the disjoint sets. + add_edge(vp, vq); + } + }); + + // Calculate the connected components. + auto components = std::vector>{}; + { + const auto components_tmp = ICC::get_components(parent); + components.resize(components_tmp.size()); + for (auto c : components_tmp) + for (auto [child, child_end] = components_tmp[c]; child != child_end; + ++child) + components[c].push_back(static_cast(*child)); + } + + + return {graph, components}; + } + + auto filter_feature_tracks(const FeatureGraph& graph, + const std::vector>& components, + ViewAttributes& views) + -> std::set> + { + // Populate the feature tracks regardless of their cardinality. + auto feature_tracks = std::set>{}; + for (const auto& component : components) + { + auto feature_track = std::set{}; + std::transform(component.begin(), component.end(), + std::inserter(feature_track, std::begin(feature_track)), + [&](const auto v) { return graph[v]; }); + + feature_tracks.insert(feature_track); + } + + // Remove redundant features across images. + using ImageID = int; + using FeatureID = int; + + auto filtered_feature_tracks_dict = + std::set>{}; + for (const auto& track : feature_tracks) + { + auto filtered_track = std::map{}; + for (const auto& f : track) + { + const auto current_feature = filtered_track.find(f.image_id); + const auto current_feature_found = + current_feature != filtered_track.end(); + + if (!current_feature_found) + filtered_track[f.image_id] = f.local_id; + + // Replace the feature if the response is stronger. + else // image_id == f.image_id + { + const auto& features_list = features(views.keypoints[f.image_id]); + + // The feature local IDs. + const auto& current_feature_id = current_feature->second; + const auto current_feature_response = + std::abs(features_list[current_feature_id].extremum_value); + + const auto feature_response = + std::abs(features_list[f.local_id].extremum_value); + + if (feature_response > current_feature_response) + filtered_track[f.image_id] = f.local_id; + } + } + + filtered_feature_tracks_dict.insert(filtered_track); + } + + // Transform the filtered feature tracks again. + auto filtered_feature_tracks = std::set>{}; + for (const auto& track_dict : filtered_feature_tracks_dict) + { + if (track_dict.size() == 1) + continue; + + auto track_set = std::set{}; + for (const auto& gid : track_dict) + track_set.insert({gid.first, gid.second}); + filtered_feature_tracks.insert(track_set); + } + + // Replace the feature tracks. + feature_tracks.swap(filtered_feature_tracks); + + return feature_tracks; + } + + + template <> + struct CalculateH5Type + { + static inline auto value() -> H5::CompType + { + auto h5_comp_type = H5::CompType{sizeof(FeatureGID)}; + INSERT_MEMBER(h5_comp_type, FeatureGID, image_id); + INSERT_MEMBER(h5_comp_type, FeatureGID, local_id); + return h5_comp_type; + } + }; + + + auto write_feature_graph(const FeatureGraph& graph, H5File& file, + const std::string& group_name) -> void + { + auto features = std::vector(boost::num_vertices(graph)); + for (auto [v, v_end] = boost::vertices(graph); v != v_end; ++v) + features[*v] = {graph[*v].image_id, graph[*v].local_id}; + + auto matches = std::vector{}; + for (auto [e, e_end] = boost::edges(graph); e != e_end; ++e) + matches.push_back({boost::source(*e, graph), boost::target(*e, graph)}); + + file.get_group(group_name); + file.write_dataset(group_name + "/" + "features", tensor_view(features)); + file.write_dataset(group_name + "/" + "matches", tensor_view(matches)); + } + + + auto read_feature_graph(H5File& file, const std::string& group_name) + -> FeatureGraph + { + auto features = std::vector{}; + auto matches = std::vector{}; + + file.read_dataset(group_name + "/" + "features", features); + file.read_dataset(group_name + "/" + "matches", matches); + + // Reconstruct the graph. + auto g = FeatureGraph{}; + + for (const auto& v : features) + boost::add_vertex(v, g); + + for (const auto& e : matches) + boost::add_edge(e(0), e(1), g); + + return g; + } + +} /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Graph/FeatureGraph.hpp similarity index 98% rename from cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.hpp rename to cpp/src/DO/Sara/MultiViewGeometry/Graph/FeatureGraph.hpp index 264102d73..44bfae98d 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/FeatureGraph.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Graph/FeatureGraph.hpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include diff --git a/cpp/src/DO/Sara/MultiViewGeometry/HDF5.hpp b/cpp/src/DO/Sara/MultiViewGeometry/HDF5.hpp index 35a3f54e8..d8a09f453 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/HDF5.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/HDF5.hpp @@ -12,9 +12,9 @@ #pragma once #include -#include #include #include +#include namespace DO::Sara { diff --git a/cpp/src/DO/Sara/SfM/Graph/BundleAdjuster.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.hpp similarity index 100% rename from cpp/src/DO/Sara/SfM/Graph/BundleAdjuster.hpp rename to cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.hpp diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.cpp deleted file mode 100644 index 1e2ae7892..000000000 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.cpp +++ /dev/null @@ -1,345 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2019 David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#include -#include -#include -#include -#include - -#include - - -namespace fs = boost::filesystem; - - -namespace DO::Sara { - - using ESolver = NisterFivePointAlgorithm; - - auto estimate_essential_matrix(const std::vector& Mij, // - const KeypointList& ki, // - const KeypointList& kj, // - const Eigen::Matrix3d& Ki_inv, // - const Eigen::Matrix3d& Kj_inv, // - int num_samples, // - double err_thres) - -> std::tuple, Tensor_> - { - const auto& fi = features(ki); - const auto& fj = features(kj); - const auto ui = extract_centers(fi).cast(); - const auto uj = extract_centers(fj).cast(); - - const auto uni = apply_transform(Ki_inv, homogeneous(ui)); - const auto unj = apply_transform(Kj_inv, homogeneous(uj)); - - const auto Mij_tensor = to_tensor(Mij); - const auto Xij = PointCorrespondenceList{Mij_tensor, uni, unj}; - - auto inlier_predicate = InlierPredicate{}; - inlier_predicate.err_threshold = err_thres; - - const auto [E, inliers, sample_best] = - ransac(Xij, ESolver{}, inlier_predicate, num_samples); - - SARA_CHECK(E); - SARA_CHECK(inliers.row_vector()); - SARA_CHECK(Mij.size()); - - return std::make_tuple(E, inliers, sample_best); - } - - auto estimate_essential_matrices(const std::string& dirpath, // - const std::string& h5_filepath, // - int num_samples, // - double noise, // - int min_F_inliers, // - bool overwrite, // - bool debug, // - bool wait_key) -> void - { - // Create a backup. - if (!fs::exists(h5_filepath + ".bak")) - cp(h5_filepath, h5_filepath + ".bak"); - - SARA_DEBUG << "Opening file " << h5_filepath << "..." << std::endl; - auto h5_file = H5File{h5_filepath, H5F_ACC_RDWR}; - - auto view_attributes = ViewAttributes{}; - - // Load images (optional). - SARA_DEBUG << "Listing images from:\n\t" << dirpath << std::endl; - view_attributes.list_images(dirpath); - if (debug) - view_attributes.read_images(); - - // Load the internal camera matrices from Strecha dataset. - // N.B.: this is an ad-hoc code. - SARA_DEBUG << "Reading internal camera matrices in Strecha's data format" - << std::endl; - std::for_each(std::begin(view_attributes.image_paths), - std::end(view_attributes.image_paths), - [&](const auto& image_path) { - const auto K_filepath = - dirpath + "/" + basename(image_path) + ".png.K"; - SARA_DEBUG << "Reading internal camera matrix from:\n\t" - << K_filepath << std::endl; - view_attributes.cameras.push_back(normalized_camera()); - view_attributes.cameras.back().K = - read_internal_camera_parameters(K_filepath); - }); - - // Load keypoints. - SARA_DEBUG << "Reading keypoints from HDF5 file:\n\t" << h5_filepath - << std::endl; - view_attributes.read_keypoints(h5_file); - const auto& keypoints = view_attributes.keypoints; - - // Initialize the epipolar graph. - const auto num_vertices = int(view_attributes.image_paths.size()); - SARA_CHECK(num_vertices); - - auto edge_attributes = EpipolarEdgeAttributes{}; - SARA_DEBUG << "Initializing the epipolar edges..." << std::endl; - edge_attributes.initialize_edges(num_vertices); - - SARA_DEBUG << "Reading matches from HDF5 file:\n\t" << h5_filepath - << std::endl; - edge_attributes.read_matches(h5_file, view_attributes); - - SARA_DEBUG << "Reading the fundamental matrices..." << std::endl; - edge_attributes.resize_fundamental_edge_list(); - edge_attributes.read_fundamental_matrices(view_attributes, h5_file); - // TODO: we will use the meta data later to decide if we want to estimate an - // essential matrix because it is a lot slower than the fundamental - // matrix estimation. - - SARA_DEBUG << "Preallocate the E data structures..." << std::endl; - edge_attributes.resize_essential_edge_list(); - - const auto& edge_ids = edge_attributes.edge_ids; - const auto& edges = edge_attributes.edges; - const auto& matches = edge_attributes.matches; - SARA_CHECK(edge_ids.size()); - SARA_CHECK(edges.size()); - SARA_CHECK(matches.size()); - - // Mutate these. - auto& E = edge_attributes.E; - auto& E_num_samples = edge_attributes.E_num_samples; - auto& E_noise = edge_attributes.E_noise; - auto& E_inliers = edge_attributes.E_inliers; - auto& E_best_samples = edge_attributes.E_best_samples; - - // Use this data to decide if we want to estimate an essential matrix. - const auto& F_inliers = edge_attributes.F_inliers; - auto F_num_inliers = [&](const auto& ij) { - return F_inliers[ij].vector().count(); - }; - // auto F_inlier_ratio = [&](const auto& ij) { - // return double(F_num_inliers(ij)) / F_inliers[ij].size(); - // }; - - std::for_each( - std::begin(edge_ids), std::end(edge_ids), [&](const auto& ij) { - const auto& eij = edges[ij]; - const auto i = eij.first; - const auto j = eij.second; - const auto& Mij = matches[ij]; - const auto& ki = keypoints[i]; - const auto& kj = keypoints[j]; - const auto& Ki = view_attributes.cameras[i].K; - const auto& Kj = view_attributes.cameras[j].K; - const auto Ki_inv = Ki.inverse(); - const auto Kj_inv = Kj.inverse(); - - SARA_DEBUG << "Calculating essential matrices between images:\n" - << "- image[" << i << "] = " // - << view_attributes.group_names[i] << "\n" - << "- image[" << j << "] = " // - << view_attributes.group_names[j] << "\n"; - - SARA_DEBUG << "Internal camera matrices :\n" - << "- K[" << i << "] =\n" - << Ki << "\n" - << "- K[" << j << "] =\n" - << Kj << "\n"; - std::cout.flush(); - - auto Eij = EssentialMatrix{}; - auto E_best_sample_ij = Tensor_{ESolver::num_points}; - auto E_inliers_ij = Tensor_{static_cast(Mij.size())}; - auto Fij = FundamentalMatrix{}; - if (F_num_inliers(ij) >= min_F_inliers) - { - // Estimate the fundamental matrix. - std::tie(Eij, E_inliers_ij, E_best_sample_ij) = - estimate_essential_matrix(Mij, ki, kj, Ki_inv, Kj_inv, - num_samples, noise); - - Eij.matrix() = Eij.matrix().normalized(); - - Fij.matrix() = - Kj.inverse().transpose() * Eij.matrix() * Ki.inverse(); - - SARA_DEBUG << "Eij = \n" << Eij << std::endl; - SARA_DEBUG << "Fij = \n" << Fij << std::endl; - SARA_CHECK(E_inliers_ij.row_vector()); - SARA_CHECK(E_best_sample_ij.row_vector()); - } - else - { - Eij.matrix().setZero(); - E_best_sample_ij.flat_array().setZero(); - E_inliers_ij.flat_array().setZero(); - } - - if (debug) - { - const int display_step = 20; - const auto& Ii = view_attributes.images[i]; - const auto& Ij = view_attributes.images[j]; - check_epipolar_constraints(Ii, Ij, Fij, Mij, E_best_sample_ij, - E_inliers_ij, display_step, wait_key); - } - - // Update. - E[ij] = Eij; - E_inliers[ij] = E_inliers_ij; - E_best_samples[ij] = E_best_sample_ij; - // Useful if we use MLESAC and variants. - E_noise[ij] = noise; - // Useful if we use PROSAC sampling strategy. - E_num_samples[ij] = num_samples; - - SARA_CHECK(E_num_samples[ij]); - SARA_CHECK(E_noise[ij]); - }); - - h5_file.write_dataset("E", tensor_view(E), overwrite); - h5_file.write_dataset("E_num_samples", tensor_view(E_num_samples), - overwrite); - h5_file.write_dataset("E_noise", tensor_view(E_noise), overwrite); - h5_file.write_dataset("E_best_samples", E_best_samples, overwrite); - // Save E-edges. - h5_file.get_group("E_inliers"); - std::for_each(std::begin(edge_ids), std::end(edge_ids), - [&](const auto& ij) { - const auto i = edges[ij].first; - const auto j = edges[ij].second; - h5_file.write_dataset(format("E_inliers/%d_%d", i, j), - E_inliers[ij], overwrite); - }); - } - - auto inspect_essential_matrices(const std::string& dirpath, - const std::string& h5_filepath, - int display_step, bool wait_key) -> void - { - SARA_DEBUG << "Opening file " << h5_filepath << "..." << std::endl; - auto h5_file = H5File{h5_filepath, H5F_ACC_RDONLY}; - - auto view_attributes = ViewAttributes{}; - - // Load images (optional). - SARA_DEBUG << "Listing images from:\n\t" << dirpath << std::endl; - view_attributes.list_images(dirpath); - view_attributes.read_images(); - - // Load keypoints. - SARA_DEBUG << "Reading keypoints from HDF5 file:\n\t" << h5_filepath - << std::endl; - view_attributes.read_keypoints(h5_file); - const auto& images = view_attributes.images; - - // Load the internal camera matrices from Strecha dataset. - // N.B.: this is an ad-hoc code. - SARA_DEBUG << "Reading internal camera matrices in Strecha's data format" - << std::endl; - std::for_each(std::begin(view_attributes.image_paths), - std::end(view_attributes.image_paths), - [&](const auto& image_path) { - const auto K_filepath = - dirpath + "/" + basename(image_path) + ".png.K"; - SARA_DEBUG << "Reading internal camera matrix from:\n\t" - << K_filepath << std::endl; - view_attributes.cameras.push_back(normalized_camera()); - view_attributes.cameras.back().K = - read_internal_camera_parameters(K_filepath); - }); - - // Initialize the epipolar graph. - const auto num_vertices = int(view_attributes.image_paths.size()); - SARA_CHECK(num_vertices); - - auto edge_attributes = EpipolarEdgeAttributes{}; - SARA_DEBUG << "Initializing the epipolar edges..." << std::endl; - edge_attributes.initialize_edges(num_vertices); - - SARA_DEBUG << "Reading matches from HDF5 file:\n\t" << h5_filepath - << std::endl; - edge_attributes.read_matches(h5_file, view_attributes); - - SARA_DEBUG << "Reading the essential matrices..." << std::endl; - edge_attributes.resize_essential_edge_list(); - edge_attributes.read_essential_matrices(view_attributes, h5_file); - - // Convenient references. - const auto& edge_ids = edge_attributes.edge_ids; - const auto& edges = edge_attributes.edges; - const auto& matches = edge_attributes.matches; - - const auto& E = edge_attributes.E; - const auto& E_num_samples = edge_attributes.E_num_samples; - const auto& E_noise = edge_attributes.E_noise; - const auto& E_best_samples = edge_attributes.E_best_samples; - const auto& E_inliers = edge_attributes.E_inliers; - - std::for_each( - std::begin(edge_ids), std::end(edge_ids), [&](const auto& ij) { - const auto& eij = edges[ij]; - const auto i = eij.first; - const auto j = eij.second; - const auto& Mij = matches[ij]; - - const auto& Eij = E[ij]; - - const auto& Ki = view_attributes.cameras[i].K; - const auto& Kj = view_attributes.cameras[j].K; - - SARA_DEBUG << "Internal camera matrices :\n" - << "- K[" << i << "] =\n" - << Ki << "\n" - << "- K[" << j << "] =\n" - << Kj << "\n"; - - SARA_DEBUG - << "Forming the fundamental matrix from the essential matrix:\n"; - std::cout.flush(); - auto Fij = FundamentalMatrix{}; - Fij.matrix() = Kj.inverse().transpose() * Eij.matrix() * Ki.inverse(); - - SARA_DEBUG << "Fij = \n" << E[ij] << std::endl; - SARA_CHECK(E_num_samples[ij]); - SARA_CHECK(E_noise[ij]); - SARA_CHECK(E_inliers[ij].row_vector()); - SARA_CHECK(E_inliers[ij].row_vector().count()); - SARA_CHECK(E_best_samples[ij].row_vector()); - - const auto& Ii = images[i]; - const auto& Ij = images[j]; - check_epipolar_constraints(Ii, Ij, Fij, Mij, E_best_samples[ij], - E_inliers[ij], display_step, wait_key); - }); - } - -} /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp new file mode 100644 index 000000000..7ee194712 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + + +namespace DO::Sara { + + struct FeatureParams + { + ImagePyramidParams image_pyr_params = ImagePyramidParams(0); + float sift_nn_ratio = 0.6f; + std::size_t num_matches_max = 1000u; + std::size_t num_inliers_min = 100u; + }; + +} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.cpp deleted file mode 100644 index a799231ed..000000000 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2019 David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#include -#include -#include -#include -#include -#include -#include - -#include - - -namespace fs = boost::filesystem; - - -namespace DO::Sara { - - using FSolver = SevenPointAlgorithmDoublePrecision; - - auto estimate_fundamental_matrix(const std::vector& Mij, - const KeypointList& ki, - const KeypointList& kj, - const int num_samples, - const double err_thres) - -> std::tuple, Tensor_> - { - const auto& fi = features(ki); - const auto& fj = features(kj); - const auto pi = homogeneous(extract_centers(fi).cast()); - const auto pj = homogeneous(extract_centers(fj).cast()); - const auto Mij_tensor = to_tensor(Mij); - - const auto Xij = PointCorrespondenceList{Mij_tensor, pi, pj}; - const auto data_normalizer = - std::make_optional(Normalizer{Xij}); - - auto inlier_predicate = InlierPredicate{}; - inlier_predicate.err_threshold = err_thres; - - static constexpr auto confidence = 0.99; - const auto [F, inliers, sample_best] = v2::ransac( // - Xij, // - FSolver{}, // - inlier_predicate, // - num_samples, confidence, // - data_normalizer, // - true); - - return std::make_tuple(F, inliers, sample_best); - } - - auto estimate_fundamental_matrices(const std::string& dirpath, - const std::string& h5_filepath, - bool overwrite, bool debug, bool wait_key) - -> void - { - // Create a backup. - if (!fs::exists(h5_filepath + ".bak")) - cp(h5_filepath, h5_filepath + ".bak"); - - SARA_DEBUG << "Opening file " << h5_filepath << "..." << std::endl; - auto h5_file = H5File{h5_filepath, H5F_ACC_RDWR}; - - auto view_attributes = ViewAttributes{}; - - // Load images (optional). - SARA_DEBUG << "Listing images from:\n\t" << dirpath << std::endl; - view_attributes.list_images(dirpath); - if (debug) - view_attributes.read_images(); - - // Load keypoints (optional). - SARA_DEBUG << "Reading keypoints from HDF5 file:\n\t" << h5_filepath - << std::endl; - view_attributes.read_keypoints(h5_file); - const auto& keypoints = view_attributes.keypoints; - - // Initialize the epipolar graph. - const auto num_vertices = int(view_attributes.image_paths.size()); - SARA_CHECK(num_vertices); - - auto edge_attributes = EpipolarEdgeAttributes{}; - SARA_DEBUG << "Initializing the epipolar edges..." << std::endl; - edge_attributes.initialize_edges(num_vertices); - - SARA_DEBUG << "Reading matches from HDF5 file:\n\t" << h5_filepath - << std::endl; - edge_attributes.read_matches(h5_file, view_attributes); - - SARA_DEBUG << "Preallocate the F data structures..." << std::endl; - edge_attributes.resize_fundamental_edge_list(); - - const auto& edge_ids = edge_attributes.edge_ids; - const auto& edges = edge_attributes.edges; - const auto& matches = edge_attributes.matches; - SARA_CHECK(edge_ids.size()); - SARA_CHECK(edges.size()); - SARA_CHECK(matches.size()); - - - // Mutate these. - auto& F = edge_attributes.F; - auto& F_num_samples = edge_attributes.F_num_samples; - auto& F_noise = edge_attributes.F_noise; - auto& F_inliers = edge_attributes.F_inliers; - auto& F_best_samples = edge_attributes.F_best_samples; - - const auto num_samples = 1000; - const auto f_err_thres = 5e-3; - std::for_each( - std::begin(edge_ids), std::end(edge_ids), [&](const auto& ij) { - const auto& eij = edges[ij]; - const auto i = eij.first; - const auto j = eij.second; - const auto& Mij = matches[ij]; - const auto& ki = keypoints[i]; - const auto& kj = keypoints[j]; - - SARA_DEBUG << "Calculating fundamental matrices between images:\n" - << "- image[" << i << "] = " // - << view_attributes.group_names[i] << "\n" - << "- image[" << j << "] = " // - << view_attributes.group_names[j] << "\n"; - std::cout.flush(); - - // Estimate the fundamental matrix. - const auto [Fij, F_inliers_ij, F_best_sample_ij] = - estimate_fundamental_matrix(Mij, ki, kj, num_samples, - f_err_thres); - SARA_DEBUG << "Fij = \n" << Fij << std::endl; - SARA_CHECK(F_inliers_ij.row_vector()); - SARA_CHECK(F_best_sample_ij.row_vector()); - - if (debug) - { - const int display_step = 20; - const auto& Ii = view_attributes.images[i]; - const auto& Ij = view_attributes.images[j]; - check_epipolar_constraints(Ii, Ij, Fij, Mij, F_best_sample_ij, - F_inliers_ij, display_step, wait_key); - } - - // Update. - F[ij] = Fij; - F_inliers[ij] = F_inliers_ij; - F_best_samples[ij] = F_best_sample_ij; - F_noise[ij] = f_err_thres; - }); - - // Save fundamental matrices and additional info from RANSAC. - h5_file.write_dataset("F", tensor_view(F), overwrite); - h5_file.write_dataset("F_num_samples", tensor_view(F_num_samples), - overwrite); - h5_file.write_dataset("F_noise", tensor_view(F_noise), overwrite); - h5_file.write_dataset("F_best_samples", F_best_samples, overwrite); - - h5_file.get_group("F_inliers"); - std::for_each(std::begin(edge_ids), std::end(edge_ids), - [&](const auto& ij) { - const auto i = edges[ij].first; - const auto j = edges[ij].second; - h5_file.write_dataset(format("F_inliers/%d_%d", i, j), - F_inliers[ij], overwrite); - }); - } - - auto check_epipolar_constraints(const Image& Ii, const Image& Ij, - const FundamentalMatrix& F, - const std::vector& Mij, - const TensorView_& sample_best, - const TensorView_& inliers, - int display_step, bool wait_key) -> void - { - const auto scale = 0.25f; - const auto w = int((Ii.width() + Ij.width()) * scale + 0.5f); - const auto h = int(std::max(Ii.height(), Ij.height()) * scale + 0.5f); - - if (!active_window()) - { - create_window(w, h); - set_antialiasing(); - } - - if (get_sizes(active_window()) != Eigen::Vector2i(w, h)) - resize_window(w, h); - - PairWiseDrawer drawer(Ii, Ij); - drawer.set_viz_params(scale, scale, PairWiseDrawer::CatH); - - drawer.display_images(); - - for (auto m = 0; m < static_cast(Mij.size()); ++m) - { - const Eigen::Vector3d X1 = Mij[m].x_pos().cast().homogeneous(); - const Eigen::Vector3d X2 = Mij[m].y_pos().cast().homogeneous(); - - if (!inliers(m)) - continue; - - if (m % display_step == 0) - { - drawer.draw_match(Mij[m], Blue8, false); - - const auto proj_X1 = F.right_epipolar_line(X1); - const auto proj_X2 = F.left_epipolar_line(X2); - - drawer.draw_line_from_eqn(0, proj_X2.cast(), Cyan8, 1); - drawer.draw_line_from_eqn(1, proj_X1.cast(), Cyan8, 1); - } - } - - for (auto m = 0; m < static_cast(sample_best.size()); ++m) - { - // Draw the best elemental subset drawn by RANSAC. - drawer.draw_match(Mij[sample_best(m)], Red8, true); - - const Eigen::Vector3d X1 = - Mij[sample_best(m)].x_pos().cast().homogeneous(); - const Eigen::Vector3d X2 = - Mij[sample_best(m)].y_pos().cast().homogeneous(); - - const auto proj_X1 = F.right_epipolar_line(X1); - const auto proj_X2 = F.left_epipolar_line(X2); - - // Draw the corresponding epipolar lines. - drawer.draw_line_from_eqn(1, proj_X1.cast(), Magenta8, 1); - drawer.draw_line_from_eqn(0, proj_X2.cast(), Magenta8, 1); - } - - if (wait_key) - get_key(); - } - - auto inspect_fundamental_matrices(const std::string& dirpath, - const std::string& h5_filepath, - int display_step, bool wait_key) -> void - { - SARA_DEBUG << "Opening file " << h5_filepath << "..." << std::endl; - auto h5_file = H5File{h5_filepath, H5F_ACC_RDONLY}; - - auto view_attributes = ViewAttributes{}; - - // Load images (optional). - SARA_DEBUG << "Listing images from:\n\t" << dirpath << std::endl; - view_attributes.list_images(dirpath); - view_attributes.read_images(); - - // Load keypoints (optional). - SARA_DEBUG << "Reading keypoints from HDF5 file:\n\t" << h5_filepath - << std::endl; - view_attributes.read_keypoints(h5_file); - const auto& images = view_attributes.images; - - // Initialize the epipolar graph. - const auto num_vertices = int(view_attributes.image_paths.size()); - SARA_CHECK(num_vertices); - - auto edge_attributes = EpipolarEdgeAttributes{}; - SARA_DEBUG << "Initializing the epipolar edges..." << std::endl; - edge_attributes.initialize_edges(num_vertices); - - SARA_DEBUG << "Reading matches from HDF5 file:\n\t" << h5_filepath - << std::endl; - edge_attributes.read_matches(h5_file, view_attributes); - - SARA_DEBUG << "Reading the fundamental matrices..." << std::endl; - edge_attributes.resize_fundamental_edge_list(); - edge_attributes.read_fundamental_matrices(view_attributes, h5_file); - - // Convenient references. - const auto& edge_ids = edge_attributes.edge_ids; - const auto& edges = edge_attributes.edges; - const auto& matches = edge_attributes.matches; - - const auto& F = edge_attributes.F; - const auto& F_num_samples = edge_attributes.F_num_samples; - const auto& F_noise = edge_attributes.F_noise; - const auto& F_best_samples = edge_attributes.F_best_samples; - const auto& F_inliers = edge_attributes.F_inliers; - - std::for_each( - std::begin(edge_ids), std::end(edge_ids), [&](const auto& ij) { - const auto& eij = edges[ij]; - const auto i = eij.first; - const auto j = eij.second; - const auto& Mij = matches[ij]; - - SARA_DEBUG << "Fij = \n" << F[ij] << std::endl; - SARA_CHECK(F_num_samples[ij]); - SARA_CHECK(F_noise[ij]); - SARA_CHECK(F_inliers[ij].row_vector()); - SARA_CHECK(F_inliers[ij].row_vector().count()); - SARA_CHECK(F_best_samples[ij].row_vector()); - - const auto& Ii = images[i]; - const auto& Ij = images[j]; - check_epipolar_constraints(Ii, Ij, F[ij], Mij, F_best_samples[ij], - F_inliers[ij], display_step, wait_key); - }); - } - -} /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointDetection.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointDetection.cpp deleted file mode 100644 index 3bf50c42b..000000000 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointDetection.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2019 David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#include - -#include -#include -#include -#include - - -namespace DO::Sara { - - auto detect_keypoints(const std::string& dirpath, - const std::string& h5_filepath, // - bool overwrite) -> void - { - auto h5_file = H5File{h5_filepath, H5F_ACC_TRUNC}; - - auto image_paths = std::vector{}; - append(image_paths, ls(dirpath, ".png")); - append(image_paths, ls(dirpath, ".jpg")); - - std::for_each( - std::begin(image_paths), std::end(image_paths), [&](const auto& path) { - SARA_DEBUG << "Reading image " << path << "..." << std::endl; - const auto image = imread(path); - - SARA_DEBUG << "Computing SIFT keypoints " << path << "..." - << std::endl; - const auto keys = compute_sift_keypoints(image); - - const auto group_name = basename(path); - h5_file.get_group(group_name); - - SARA_DEBUG << "Saving SIFT keypoints of " << path << "..." - << std::endl; - write_keypoints(h5_file, group_name, keys, overwrite); - }); - } - - - auto read_keypoints(const std::string& dirpath, - const std::string& h5_filepath) -> void - { - auto h5_file = H5File{h5_filepath, H5F_ACC_RDONLY}; - auto image_paths = std::vector{}; - append(image_paths, ls(dirpath, ".png")); - append(image_paths, ls(dirpath, ".jpg")); - - std::for_each( - std::begin(image_paths), std::end(image_paths), [&](const auto& path) { - SARA_DEBUG << "Reading image " << path << "..." << std::endl; - const auto image = imread(path); - - const auto group_name = basename(path); - - SARA_DEBUG << "Read keypoints for " << group_name << "..." - << std::endl; - const auto keys = read_keypoints(h5_file, group_name); - - const auto& features = std::get<0>(keys); - - // Visual inspection. - if (!active_window()) - { - create_window(image.sizes() / 2, group_name); - set_antialiasing(); - } - - if (get_sizes(active_window()) != image.sizes() / 2) - resize_window(image.sizes() / 2); - - display(image, Point2i::Zero(), 0.5); - draw_oe_regions(features, Red8, 0.5f); - get_key(); - }); - - if (active_window()) - close_window(); - } - -} /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointMatching.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointMatching.cpp deleted file mode 100644 index bc08bc368..000000000 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointMatching.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2019 David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#include -#include -#include -#include - -#include - - -namespace fs = boost::filesystem; - - -namespace DO::Sara { - -auto match(const KeypointList& keys1, - const KeypointList& keys2, - float lowe_ratio) - -> std::vector -{ - AnnMatcher matcher{keys1, keys2, lowe_ratio}; - return matcher.compute_matches(); -} - - -auto match_keypoints(const std::string& dirpath, const std::string& h5_filepath, - bool overwrite) -> void -{ - // Create a backup. - if (!fs::exists(h5_filepath + ".bak")) - cp(h5_filepath, h5_filepath + ".bak"); - - auto h5_file = H5File{h5_filepath, H5F_ACC_RDWR}; - - auto image_paths = std::vector{}; - append(image_paths, ls(dirpath, ".png")); - append(image_paths, ls(dirpath, ".jpg")); - std::sort(image_paths.begin(), image_paths.end()); - - auto group_names = std::vector{}; - group_names.reserve(image_paths.size()); - std::transform(std::begin(image_paths), std::end(image_paths), - std::back_inserter(group_names), - [&](const std::string& image_path) { - return basename(image_path); - }); - - auto keypoints = std::vector>{}; - keypoints.reserve(image_paths.size()); - std::transform(std::begin(group_names), std::end(group_names), - std::back_inserter(keypoints), - [&](const std::string& group_name) { - return read_keypoints(h5_file, group_name); - }); - - const auto N = int(image_paths.size()); - auto edges = std::vector>{}; - edges.reserve(N * (N - 1) / 2); - for (int i = 0; i < N; ++i) - for (int j = i + 1; j < N; ++j) - edges.emplace_back(i, j); - - auto matches = std::vector>{}; - matches.reserve(edges.size()); - std::transform(std::begin(edges), std::end(edges), - std::back_inserter(matches), - [&](const auto& edge) { - const auto i = edge.first; - const auto j = edge.second; - return match(keypoints[i], keypoints[j]); - }); - - // Save matches to HDF5. - auto edge_ids = range(edges.size()); - std::for_each( - std::begin(edge_ids), std::end(edge_ids), [&](const auto& e) { - const auto& ij = edges[e]; - const auto i = ij.first; - const auto j = ij.second; - const auto& matches_ij = matches[e]; - - // Transform the data. - auto Mij = std::vector{}; - std::transform( - std::begin(matches_ij), std::end(matches_ij), - std::back_inserter(Mij), [](const auto& m) { - return IndexMatch{m.x_index(), m.y_index(), m.score()}; - }); - - // Save the keypoints to HDF5 - const auto group_name = std::string{"matches"}; - h5_file.get_group(group_name); - - const auto match_dataset = - group_name + "/" + std::to_string(i) + "_" + std::to_string(j); - h5_file.write_dataset(match_dataset, tensor_view(Mij), overwrite); - }); -} - -} /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/Graph/PointCloudManipulator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp similarity index 95% rename from cpp/src/DO/Sara/SfM/Graph/PointCloudManipulator.hpp rename to cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp index 69e91dcd6..139dece09 100644 --- a/cpp/src/DO/Sara/SfM/Graph/PointCloudManipulator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp @@ -1,8 +1,8 @@ #pragma once +#include #include #include -#include namespace DO::Sara { diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp new file mode 100644 index 000000000..4c7be664e --- /dev/null +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp @@ -0,0 +1,59 @@ +#include + +#include + +#include +#include +#include + +using namespace DO::Sara::v2; + +RelativePoseEstimator::RelativePoseEstimator( + const v2::BrownConradyDistortionModel& camera) +{ + configure(camera); +} + +auto RelativePoseEstimator::configure( + const v2::BrownConradyDistortionModel& camera) -> void +{ + _K = camera.calibration_matrix(); + _K_inv = _K.inverse(); + + _inlier_predicate.distance.K1_inv = _K_inv; + _inlier_predicate.distance.K2_inv = _K_inv; + _inlier_predicate.err_threshold = err_thres; +} + +auto RelativePoseEstimator::estimate_relative_pose( + const KeypointList& src_keys, + const KeypointList& dst_keys, + std::vector& matches) const + -> std::tuple, Tensor_> +{ + auto logger = Logger::get(); + + SARA_LOGD(logger, "Estimating the relative pose..."); + if (matches.empty()) + { + SARA_LOGD(logger, "Skipping relative pose estimation"); + return {}; + } + + const auto& f0 = features(src_keys); + const auto& f1 = features(dst_keys); + const auto u = std::array{ + homogeneous(extract_centers(f0)).cast(), + homogeneous(extract_centers(f1)).cast() // + }; + // List the matches as a 2D-tensor where each row encodes a match 'm' as a + // pair of point indices (i, j). + const auto M = to_tensor(matches); + + const auto X = PointCorrespondenceList{M, u[0], u[1]}; + auto data_normalizer = + std::make_optional(Normalizer{_K, _K}); + + return v2::ransac(X, _solver, _inlier_predicate, ransac_iterations_max, + ransac_confidence, data_normalizer, true); +} diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp new file mode 100644 index 000000000..914ad0b55 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include + +#include +#include +#include +#include + +#include + + +namespace DO::Sara::v2 { + + struct RelativePoseEstimator + { + int ransac_iterations_max = 1000; + double ransac_confidence = 0.999; + double err_thres = 4.; + + // Use Stewenius' algorithm instead of Nister's for now. The polynomial + // solver must have some convergence problems. + const RelativePoseSolver _solver; + CheiralAndEpipolarConsistency _inlier_predicate; + + Eigen::Matrix3d _K; + Eigen::Matrix3d _K_inv; + + RelativePoseEstimator( + const v2::BrownConradyDistortionModel& camera); + + auto configure(const v2::BrownConradyDistortionModel& camera) + -> void; + + auto estimate_relative_pose(const KeypointList& src_keys, + const KeypointList& dst_keys, + std::vector& matches) const + -> std::tuple, Tensor_>; + }; + +} // namespace DO::Sara::v2 diff --git a/cpp/src/DO/Sara/SfM/Graph/RgbColoredPoint.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp similarity index 100% rename from cpp/src/DO/Sara/SfM/Graph/RgbColoredPoint.hpp rename to cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/FeatureTracker.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/FeatureTracker.hpp deleted file mode 100644 index fdac7975e..000000000 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/FeatureTracker.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - - -namespace DO::Sara::v2 { - - struct FeatureTracker - { - ImagePyramidParams image_pyr_params = ImagePyramidParams(0); - float sift_nn_ratio = 0.6f; - std::size_t num_matches_max = 1000u; - - auto detect_features(const ImageView& image, - KeypointList& keypoints) const -> void - { - keypoints = compute_sift_keypoints(image, image_pyr_params); - } - - auto match_features(const KeypointList& src_keys, - const KeypointList& dst_keys) const - -> std::vector - { - if (features(src_keys).empty() || features(dst_keys).empty()) - return {}; - - auto matches = match(src_keys, dst_keys, sift_nn_ratio); - if (matches.size() > num_matches_max) - matches.resize(num_matches_max); - - return matches; - } - }; - -} // namespace DO::Sara::v2 diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/RelativePoseEstimator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/RelativePoseEstimator.hpp deleted file mode 100644 index ff3829a3a..000000000 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/v2/RelativePoseEstimator.hpp +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include - - -namespace DO::Sara::v2 { - - struct RelativePoseEstimator - { - int ransac_iterations_max = 1000; - double ransac_confidence = 0.999; - double err_thres = 4.; - - // Use Stewenius' algorithm instead of Nister's for now. The polynomial - // solver must have some convergence problems. - const RelativePoseSolver _solver; - CheiralAndEpipolarConsistency _inlier_predicate; - - Eigen::Matrix3d _K; - Eigen::Matrix3d _K_inv; - - RelativePoseEstimator(const v2::BrownConradyDistortionModel& camera) - { - configure(camera); - } - - auto configure(const v2::BrownConradyDistortionModel& camera) - -> void - { - _K = camera.calibration_matrix(); - _K_inv = _K.inverse(); - - _inlier_predicate.distance.K1_inv = _K_inv; - _inlier_predicate.distance.K2_inv = _K_inv; - _inlier_predicate.err_threshold = err_thres; - } - - auto estimate_relative_pose(const KeypointList& src_keys, - const KeypointList& dst_keys, - std::vector& matches) const - -> std::tuple, Tensor_> - { - print_stage("Estimating the relative pose..."); - if (matches.empty()) - { - SARA_DEBUG << "Skipping relative pose estimation\n"; - return {}; - } - - const auto& f0 = features(src_keys); - const auto& f1 = features(dst_keys); - const auto u = std::array{ - homogeneous(extract_centers(f0)).cast(), - homogeneous(extract_centers(f1)).cast() // - }; - // List the matches as a 2D-tensor where each row encodes a match 'm' as a - // pair of point indices (i, j). - const auto M = to_tensor(matches); - - const auto X = PointCorrespondenceList{M, u[0], u[1]}; - auto data_normalizer = - std::make_optional(Normalizer{_K, _K}); - - return v2::ransac(X, _solver, _inlier_predicate, ransac_iterations_max, - ransac_confidence, data_normalizer, true); - } - }; - -} // namespace DO::Sara::v2 diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp index a77edd418..0a551fd31 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp @@ -9,41 +9,42 @@ // you can obtain one at http://mozilla.org/MPL/2.0/. // ========================================================================== // - #include #include #include #include +#include using namespace DO::Sara; -auto CameraPoseGraph::detect_keypoints( - const v2::FeatureTracker& feature_tracker, - const ImageView& image, // - const int frame_index) -> void +auto CameraPoseGraph::add_absolute_pose( + KeypointList&& keypoints, // + const int image_id) -> CameraPoseGraph::Vertex { auto& logger = Logger::get(); - SARA_LOGI(logger, "Detecting keypoints for image frame {}", frame_index); + SARA_LOGI(logger, "Detecting keypoints for image frame {}", image_id); // Grow the pose graph by creating a new camera vertex. const auto v = boost::add_vertex(_g); // Store the camera pose data. - auto& camera_pose_data = _g[v]; - camera_pose_data.frame_index = frame_index; - camera_pose_data.keypoints = compute_sift_keypoints(image); + auto& pose_data = _g[v]; + pose_data.image_id = image_id; + pose_data.keypoints = std::move(keypoints); + + const auto& f = features(pose_data.keypoints); + SARA_LOGI(logger, "Camera {}: {} keypoints", v, f.size()); - const auto& f = features(camera_pose_data.keypoints); - SARA_LOGI(logger, "Camera vertex: {} keypoints", f.size()); + return v; } -auto CameraPoseGraph::estimate_relative_motion( - const v2::FeatureTracker& feature_tracker, // +auto CameraPoseGraph::add_relative_pose( const v2::RelativePoseEstimator& relative_pose_estimator, // + const FeatureParams& feature_params, // const Vertex u, const Vertex v) -> void { auto& logger = Logger::get(); @@ -51,9 +52,14 @@ auto CameraPoseGraph::estimate_relative_motion( SARA_LOGI(logger, "Match features..."); const auto& src_keys = _g[u].keypoints; const auto& dst_keys = _g[v].keypoints; - auto matches = feature_tracker.match_features(src_keys, dst_keys); + if (features(src_keys).empty() || features(dst_keys).empty()) + return; + + auto matches = match(src_keys, dst_keys, feature_params.sift_nn_ratio); if (matches.empty()) return; + if (matches.size() > feature_params.num_matches_max) + matches.resize(feature_params.num_matches_max); SARA_LOGI(logger, "Estimating relative pose..."); auto [geometry, inliers, sample_best] = @@ -71,7 +77,5 @@ auto CameraPoseGraph::estimate_relative_motion( auto& relative_motion_data = _g[e]; relative_motion_data.matches = std::move(matches); relative_motion_data.inliers = std::move(inliers); - relative_motion_data.src_camera = u; - relative_motion_data.dst_camera = v; } } diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp index aa7b4f99b..701ef40a5 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp @@ -17,63 +17,87 @@ #include #include -#include -#include +#include +#include #include namespace DO::Sara { - struct CameraPoseData + struct AbsolutePoseData { - //! @brief The corresponding image frame index. - int frame_index; - + //! @brief The corresponding image frame index or image ID. + int image_id; //! @brief The keypoints detected in the image. KeypointList keypoints; - //! @brief "Absolute" pose w.r.t. some reference frame. QuaternionBasedPose pose; }; - struct RelativeMotionData + struct RelativePoseData { - using camera_id_t = int; - static constexpr auto undefined_camera_id = -1; - - camera_id_t src_camera = undefined_camera_id; - camera_id_t dst_camera = undefined_camera_id; - std::vector matches; Tensor_ inliers; - Motion motion; }; class CameraPoseGraph { public: - using GraphImpl = - boost::adjacency_list; - - public: - using Vertex = boost::graph_traits::vertex_descriptor; - using Edge = boost::graph_traits::edge_descriptor; - - auto detect_keypoints(const v2::FeatureTracker& feature_tracker, - const ImageView& image, // - const int frame_index) -> void; - - auto estimate_relative_motion( - const v2::FeatureTracker& feature_tracker, // + using Impl = boost::adjacency_list< // + boost::vecS, boost::vecS, boost::undirectedS, // + AbsolutePoseData, RelativePoseData>; + using Vertex = boost::graph_traits::vertex_descriptor; + using VertexIndex = boost::graph_traits::vertices_size_type; + using Edge = boost::graph_traits::edge_descriptor; + + operator Impl&() + { + return _g; + } + + operator const Impl&() const + { + return _g; + } + + auto operator[](const Vertex u) -> AbsolutePoseData& + { + return _g[u]; + } + + auto operator[](const Vertex u) const -> const AbsolutePoseData& + { + return _g[u]; + } + + auto operator[](const Edge e) -> RelativePoseData& + { + return _g[e]; + } + + auto operator[](const Edge e) const -> const RelativePoseData& + { + return _g[e]; + } + + auto num_vertices() const -> VertexIndex + { + return boost::num_vertices(_g); + } + + auto add_absolute_pose(KeypointList&& keypoints, + const int image_id) -> Vertex; + + auto add_relative_pose( const v2::RelativePoseEstimator& relative_pose_estimator, // + const FeatureParams& feature_params, // const Vertex src, const Vertex dst) -> void; private: //! @brief The graph data structure shortened as g. - GraphImpl _g; + Impl _g; }; } /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureDisjointSets.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureDisjointSets.hpp new file mode 100644 index 000000000..998b2a209 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureDisjointSets.hpp @@ -0,0 +1,57 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#pragma once + +#include + +#include +#include +#include + + +namespace DO::Sara { + + //! @brief Feature disjoint sets for feature tracking. + struct FeatureDisjointSets + { + using Rank = FeatureGraph::Vertex*; + using Parent = FeatureGraph::Vertex*; + using Components = boost::component_index; + + FeatureDisjointSets() = default; + + explicit FeatureDisjointSets(const FeatureGraph& graph) + { + const FeatureGraph::Impl& g = graph; + + const auto n = graph.num_vertices(); + _rank.resize(n); + _parent.resize(n); + _ds.reset(new boost::disjoint_sets(&_rank[0], &_parent[0])); + if (_ds.get() == nullptr) + throw std::runtime_error{ + "Failed to allocate and initialize the feature disjoint sets"}; + boost::initialize_incremental_components(g, *_ds); + boost::incremental_components(g, *_ds); + } + + auto components() const -> Components + { + return {_parent.begin(), _parent.end()}; + } + + std::vector _rank; + std::vector _parent; + std::unique_ptr> _ds; + }; + +} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp deleted file mode 100644 index afd29e130..000000000 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureGID.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2023-present David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#pragma once - -#include - -#include - - -namespace DO::Sara { - - //! @brief Feature Global ID (GID). - struct FeatureGID - { - CameraPoseGraph::Vertex pose_vertex; - std::size_t feature_index; - - auto operator==(const FeatureGID& other) const -> bool - { - return pose_vertex == other.pose_vertex && - feature_index == other.feature_index; - } - - auto operator<(const FeatureGID& other) const -> bool - { - return (pose_vertex < other.pose_vertex) || - (pose_vertex == other.pose_vertex && - feature_index < other.feature_index); - } - }; - -} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.cpp b/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.cpp index 9315d363b..b53d35fe0 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.cpp @@ -12,8 +12,8 @@ #include #include -#include #include +#include using namespace DO::Sara; @@ -22,13 +22,9 @@ using namespace DO::Sara; auto FeatureGraph::calculate_feature_tracks() const -> std::vector { - using VertexIndex = boost::graph_traits::vertices_size_type; - using Rank = VertexIndex*; using Parent = Vertex*; - // using DisjointSets = boost::disjoint_sets; - using Components = boost::component_index; const auto num_vertices = boost::num_vertices(_feature_graph); diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp index 06ef1b36a..c5dd624dd 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp @@ -12,22 +12,74 @@ #pragma once #include -#include +#include namespace DO::Sara { - class FeatureGraph + //! @brief Feature Global ID (GID). + struct FeatureGID { - using GraphImpl = boost::adjacency_list< // - boost::vecS, boost::vecS, boost::undirectedS, // - FeatureGID>; + CameraPoseGraph::Vertex pose_vertex; + int feature_index; + + auto operator==(const FeatureGID& other) const -> bool + { + return pose_vertex == other.pose_vertex && + feature_index == other.feature_index; + } + + auto operator<(const FeatureGID& other) const -> bool + { + return (pose_vertex < other.pose_vertex) || + (pose_vertex == other.pose_vertex && + feature_index < other.feature_index); + } + }; + + //! @brief Match global ID (GID). + struct MatchGID + { + //! @brief Index of the epipolar edge connecting camera i and camera j. + CameraPoseGraph::Vertex i; + CameraPoseGraph::Vertex j; + //! @brief Local match index. + std::size_t index; + + auto operator==(const MatchGID& other) const -> bool + { + return i == other.i && j == other.j && index == other.index; + } + + auto operator<(const MatchGID& other) const -> bool + { + return (i < other.i) || (i == other.i && j < other.j) || + (i == other.i && j == other.j && index < other.index); + } + }; + //! @brief Feature Graph. + class FeatureGraph + { public: - using Vertex = boost::graph_traits::vertex_descriptor; - using Edge = boost::graph_traits::edge_descriptor; + using Impl = boost::adjacency_list< // + boost::vecS, boost::vecS, boost::undirectedS, // + FeatureGID, MatchGID>; + using Vertex = boost::graph_traits::vertex_descriptor; + using VertexIndex = boost::graph_traits::vertices_size_type; + using Edge = boost::graph_traits::edge_descriptor; using Track = std::vector; + operator Impl&() + { + return _feature_graph; + } + + operator const Impl&() const + { + return _feature_graph; + } + auto operator[](Vertex v) -> FeatureGID& { return _feature_graph[v]; @@ -38,6 +90,11 @@ namespace DO::Sara { return _feature_graph[v]; } + auto num_vertices() const -> VertexIndex + { + return boost::num_vertices(_feature_graph); + } + auto calculate_feature_tracks() const -> std::vector; auto filter_by_non_max_suppression(const Track&, const CameraPoseGraph&) const -> Track; @@ -46,7 +103,7 @@ namespace DO::Sara { -> Vertex; private: - GraphImpl _feature_graph; + Impl _feature_graph; }; } /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp new file mode 100644 index 000000000..497627f90 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp @@ -0,0 +1,189 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#include + +#include + +#include + + +using namespace DO::Sara; + +auto FeatureTracker::update_feature_tracks( + const CameraPoseGraph& camera_pose_graph, + const CameraPoseGraph::Edge relative_pose_edge) -> void +{ + auto& logger = Logger::get(); + + const CameraPoseGraph::Impl& cg = camera_pose_graph; + FeatureGraph::Impl& fg = _feature_graph; + + // Retrieve the two camera vertices from the relative pose edge. + const auto pose_i = boost::source(relative_pose_edge, cg); + const auto pose_j = boost::target(relative_pose_edge, cg); + // The relative pose edge contains the set of all feature correspondences. + const auto& matches = cg[relative_pose_edge].matches; + // Which of these feature correspondences are marked as inliers? + const auto& inliers = cg[relative_pose_edge].inliers; + + // Loop over the feature correspondence and add the feature graph edges. + SARA_LOGD(logger, "Pose {} <-> Pose {}", pose_i, pose_j); + SARA_LOGD(logger, "Add feature correspondences..."); + for (auto m = 0u; m < matches.size(); ++m) + { + if (!inliers(m)) + continue; + + const auto& match = matches[m]; + + // Local feature indices. + const auto& f1 = match.x_index(); + const auto& f2 = match.y_index(); + + // Create their corresponding feature GIDs. + const auto gid1 = FeatureGID{ + .pose_vertex = pose_i, // + .feature_index = f1 // + }; + const auto gid2 = FeatureGID{ + .pose_vertex = pose_j, // + .feature_index = f2 // + }; + + // Locate their corresponding pair of vertices (u, v) in the graph? + // Do they exist yet in the first place? + const auto u_it = _feature_vertex.find(gid1); + const auto v_it = _feature_vertex.find(gid2); + + const auto u_does_not_exist_yet = u_it == _feature_vertex.end(); + const auto v_does_not_exist_yet = v_it == _feature_vertex.end(); + + // If not, add them if necessary. + const auto u = u_does_not_exist_yet ? boost::add_vertex(fg) : u_it->second; + const auto v = v_does_not_exist_yet ? boost::add_vertex(fg) : v_it->second; + + if (u_does_not_exist_yet) + { + fg[u] = gid1; + _feature_vertex[gid1] = u; + } + if (v_does_not_exist_yet) + { + fg[v] = gid2; + _feature_vertex[gid2] = v; + } + + // Finally, store the feature match as an edge in the feature graph. + const auto [uv, uv_added] = boost::add_edge(u, v, fg); + auto& uv_attrs = fg[uv]; + uv_attrs.i = boost::source(relative_pose_edge, cg); + uv_attrs.j = boost::target(relative_pose_edge, cg); + uv_attrs.index = m; + } + + // Update the feature disjoint-sets + SARA_LOGD(logger, "[Feature-Tracks] Recalculating connected components..."); + const auto _feature_ds = FeatureDisjointSets{_feature_graph}; + const auto feature_components = _feature_ds.components(); + SARA_LOGD(logger, "[Feature-Tracks] num feature components = {}", + feature_components.size()); + + // Update the list of feature tracks. + _feature_tracks.clear(); + _feature_tracks.reserve(feature_components.size()); + + BOOST_FOREACH (FeatureGraph::VertexIndex current_index, feature_components) + { + // Iterate through the child vertex indices for [current_index] + auto component_size = 0; + BOOST_FOREACH (FeatureGraph::VertexIndex child_index, + feature_components[current_index]) + { + (void) child_index; + ++component_size; + } + + if (component_size == 1) + continue; + + auto track = std::vector{}; + track.reserve(component_size); + BOOST_FOREACH (FeatureGraph::VertexIndex child_index, + feature_components[current_index]) + track.push_back(child_index); + + _feature_tracks.emplace_back(std::move(track)); + } + + SARA_LOGD(logger, "[Feature-Tracks] num feature tracks = {}", + _feature_tracks.size()); +} + +auto FeatureTracker::calculate_alive_feature_tracks( + const CameraPoseGraph::Vertex camera_vertex_curr) const + -> std::tuple +{ + auto& logger = Logger::get(); + + // Find the feature tracks that are still alive. + const FeatureGraph::Impl& fgraph = _feature_graph; + + const auto& ftracks = _feature_tracks; + auto tracks_alive = TrackArray{}; + auto track_visibility_count = TrackVisibilityCountArray{}; + + for (const auto& ftrack : ftracks) + { + // Do we still see the track in the image. + const auto is_alive = + std::find_if(ftrack.begin(), ftrack.end(), + [&fgraph, camera_vertex_curr](const auto& v) { + return fgraph[v].pose_vertex == camera_vertex_curr; + }) != ftrack.end(); + + if (!is_alive) + continue; + + // Add the newly found alive track. + tracks_alive.push_back(ftrack); + + // Carefully count the track life, it's not the number of vertices, but + // the number of camera views in which the feature reappears. + auto camera_vertices_where_present = + std::unordered_set{}; + std::transform(ftrack.begin(), ftrack.end(), + std::inserter(camera_vertices_where_present, + camera_vertices_where_present.end()), + [&fgraph](const auto& v) { return fgraph[v].pose_vertex; }); + track_visibility_count.push_back(camera_vertices_where_present.size()); + } + SARA_LOGD(logger, "Num tracks alive: {}", tracks_alive.size()); + + const auto longest_track_alive = std::max_element( + track_visibility_count.begin(), track_visibility_count.end()); + if (longest_track_alive != track_visibility_count.end()) + { + SARA_LOGD(logger, "Longest track life: {}", *longest_track_alive); +#if 0 + const auto longest_track_index = + longest_track_alive - track_visibility_count.begin(); + const auto& longest_track = tracks_alive[longest_track_index]; + for (const auto& v : longest_track) + std::cout << fmt::format("(cam:{},ind:{})", fgraph[v].camera_vertex, + fgraph[v].index) + << " "; + std::cout << std::endl; +#endif + } + + return std::make_tuple(tracks_alive, track_visibility_count); +} diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp new file mode 100644 index 000000000..45323fb72 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp @@ -0,0 +1,53 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#pragma once + +#include + +#include + + +namespace DO::Sara { + + //! @brief Feature tracker + struct FeatureTracker + { + using Track = std::vector; + using TrackArray = std::vector; + using TrackVisibilityCountArray = std::vector; + + //! @brief The graph of 2D image features + //! + //! Image features are connected if there exists a relative pose that + //! explains it. + FeatureGraph _feature_graph; + FeatureDisjointSets _feature_ds; + std::vector _feature_tracks; + + //! @brief Retrieve the feature vertex from its pair (camera pose vertex, + //! keypoint index). + std::map _feature_vertex; + + //! @brief Retrieve the feature edge from its pair (camera pose edge, + //! match index). + std::map _feature_match; + + auto update_feature_tracks( // + const CameraPoseGraph& camera_pose_graph, + const CameraPoseGraph::Edge relative_pose_edge_id) -> void; + + auto calculate_alive_feature_tracks( + const CameraPoseGraph::Vertex camera_vertex_curr) const + -> std::tuple; + }; + +} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks.hpp b/cpp/src/DO/Sara/SfM/Helpers.hpp similarity index 62% rename from cpp/src/DO/Sara/SfM/BuildingBlocks.hpp rename to cpp/src/DO/Sara/SfM/Helpers.hpp index fece2c527..c2134ef25 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks.hpp +++ b/cpp/src/DO/Sara/SfM/Helpers.hpp @@ -11,8 +11,7 @@ #pragma once -#include -#include -#include -#include -#include +#include +#include +#include +#include diff --git a/cpp/src/DO/Sara/SfM/Helpers/EssentialMatrixEstimation.cpp b/cpp/src/DO/Sara/SfM/Helpers/EssentialMatrixEstimation.cpp new file mode 100644 index 000000000..af744f710 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/Helpers/EssentialMatrixEstimation.cpp @@ -0,0 +1,55 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2019 David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#include + +#include +#include + + +namespace DO::Sara { + + using ESolver = NisterFivePointAlgorithm; + + auto estimate_essential_matrix(const std::vector& Mij, // + const KeypointList& ki, // + const KeypointList& kj, // + const Eigen::Matrix3d& Ki_inv, // + const Eigen::Matrix3d& Kj_inv, // + int num_samples, // + double err_thres) + -> std::tuple, Tensor_> + { + const auto& fi = features(ki); + const auto& fj = features(kj); + const auto ui = extract_centers(fi).cast(); + const auto uj = extract_centers(fj).cast(); + + const auto uni = apply_transform(Ki_inv, homogeneous(ui)); + const auto unj = apply_transform(Kj_inv, homogeneous(uj)); + + const auto Mij_tensor = to_tensor(Mij); + const auto Xij = PointCorrespondenceList{Mij_tensor, uni, unj}; + + auto inlier_predicate = InlierPredicate{}; + inlier_predicate.err_threshold = err_thres; + + const auto [E, inliers, sample_best] = + ransac(Xij, ESolver{}, inlier_predicate, num_samples); + + SARA_CHECK(E); + SARA_CHECK(inliers.row_vector()); + SARA_CHECK(Mij.size()); + + return std::make_tuple(E, inliers, sample_best); + } + +} /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.hpp b/cpp/src/DO/Sara/SfM/Helpers/EssentialMatrixEstimation.hpp similarity index 59% rename from cpp/src/DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.hpp rename to cpp/src/DO/Sara/SfM/Helpers/EssentialMatrixEstimation.hpp index ed9bdf87c..6961f6cbc 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.hpp +++ b/cpp/src/DO/Sara/SfM/Helpers/EssentialMatrixEstimation.hpp @@ -11,7 +11,6 @@ #pragma once -#include #include @@ -20,8 +19,7 @@ namespace DO::Sara { //! @addtogroup SfM //! @{ - //! @{ - //! @brief Essential matrix estimation. + //! @brief Helper to estimate the essential matrix. auto estimate_essential_matrix(const std::vector& Mij, const KeypointList& ki, const KeypointList& kj, @@ -30,18 +28,6 @@ namespace DO::Sara { double err_thres) -> std::tuple, Tensor_>; - auto estimate_essential_matrices(const std::string& dirpath, // - const std::string& h5_filepath, // - int num_samples, // - double noise, // - int min_F_inliers, // - bool overwrite, bool debug, - bool wait_key = false) -> void; - - auto inspect_essential_matrices(const std::string& dirpath, - const std::string& h5_filepath, - int display_step, bool wait_key) -> void; - //! @} //! @} diff --git a/cpp/src/DO/Sara/SfM/Helpers/FundamentalMatrixEstimation.cpp b/cpp/src/DO/Sara/SfM/Helpers/FundamentalMatrixEstimation.cpp new file mode 100644 index 000000000..21c84aa59 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/Helpers/FundamentalMatrixEstimation.cpp @@ -0,0 +1,122 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2019 David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#include + +#include +#include +#include + + +namespace DO::Sara { + + using FSolver = SevenPointAlgorithmDoublePrecision; + + auto estimate_fundamental_matrix(const std::vector& Mij, + const KeypointList& ki, + const KeypointList& kj, + const int num_samples, + const double err_thres) + -> std::tuple, Tensor_> + { + const auto& fi = features(ki); + const auto& fj = features(kj); + const auto pi = homogeneous(extract_centers(fi).cast()); + const auto pj = homogeneous(extract_centers(fj).cast()); + const auto Mij_tensor = to_tensor(Mij); + + const auto Xij = PointCorrespondenceList{Mij_tensor, pi, pj}; + const auto data_normalizer = + std::make_optional(Normalizer{Xij}); + + auto inlier_predicate = InlierPredicate{}; + inlier_predicate.err_threshold = err_thres; + + static constexpr auto confidence = 0.99; + const auto [F, inliers, sample_best] = v2::ransac( // + Xij, // + FSolver{}, // + inlier_predicate, // + num_samples, confidence, // + data_normalizer, // + true); + + return std::make_tuple(F, inliers, sample_best); + } + + auto check_epipolar_constraints(const Image& Ii, const Image& Ij, + const FundamentalMatrix& F, + const std::vector& Mij, + const TensorView_& sample_best, + const TensorView_& inliers, + int display_step, bool wait_key) -> void + { + const auto scale = 0.25f; + const auto w = int((Ii.width() + Ij.width()) * scale + 0.5f); + const auto h = int(std::max(Ii.height(), Ij.height()) * scale + 0.5f); + + if (!active_window()) + { + create_window(w, h); + set_antialiasing(); + } + + if (get_sizes(active_window()) != Eigen::Vector2i(w, h)) + resize_window(w, h); + + PairWiseDrawer drawer(Ii, Ij); + drawer.set_viz_params(scale, scale, PairWiseDrawer::CatH); + + drawer.display_images(); + + for (auto m = 0; m < static_cast(Mij.size()); ++m) + { + const Eigen::Vector3d X1 = Mij[m].x_pos().cast().homogeneous(); + const Eigen::Vector3d X2 = Mij[m].y_pos().cast().homogeneous(); + + if (!inliers(m)) + continue; + + if (m % display_step == 0) + { + drawer.draw_match(Mij[m], Blue8, false); + + const auto proj_X1 = F.right_epipolar_line(X1); + const auto proj_X2 = F.left_epipolar_line(X2); + + drawer.draw_line_from_eqn(0, proj_X2.cast(), Cyan8, 1); + drawer.draw_line_from_eqn(1, proj_X1.cast(), Cyan8, 1); + } + } + + for (auto m = 0; m < static_cast(sample_best.size()); ++m) + { + // Draw the best elemental subset drawn by RANSAC. + drawer.draw_match(Mij[sample_best(m)], Red8, true); + + const Eigen::Vector3d X1 = + Mij[sample_best(m)].x_pos().cast().homogeneous(); + const Eigen::Vector3d X2 = + Mij[sample_best(m)].y_pos().cast().homogeneous(); + + const auto proj_X1 = F.right_epipolar_line(X1); + const auto proj_X2 = F.left_epipolar_line(X2); + + // Draw the corresponding epipolar lines. + drawer.draw_line_from_eqn(1, proj_X1.cast(), Magenta8, 1); + drawer.draw_line_from_eqn(0, proj_X2.cast(), Magenta8, 1); + } + + if (wait_key) + get_key(); + } + +} /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.hpp b/cpp/src/DO/Sara/SfM/Helpers/FundamentalMatrixEstimation.hpp similarity index 73% rename from cpp/src/DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.hpp rename to cpp/src/DO/Sara/SfM/Helpers/FundamentalMatrixEstimation.hpp index 846143171..ee054761a 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.hpp +++ b/cpp/src/DO/Sara/SfM/Helpers/FundamentalMatrixEstimation.hpp @@ -20,8 +20,7 @@ namespace DO::Sara { //! @addtogroup SfM //! @{ - //! @{ - //! @brief Fundamental matrix estimation. + //! @brief Helper to estimate the fundamental matrix. auto estimate_fundamental_matrix(const std::vector& Mij, const KeypointList& ki, const KeypointList& kj, @@ -29,11 +28,7 @@ namespace DO::Sara { const double err_thres) -> std::tuple, Tensor_>; - auto estimate_fundamental_matrices(const std::string& dirpath, - const std::string& h5_filepath, - bool overwrite, bool debug, - bool wait_key = false) -> void; - + //! @brief Inspect visually the epipolar constraints. auto check_epipolar_constraints(const Image& Ii, const Image& Ij, const FundamentalMatrix& F, const std::vector& Mij, @@ -42,11 +37,6 @@ namespace DO::Sara { int display_step, bool wait_key = true) -> void; - auto inspect_fundamental_matrices(const std::string& dirpath, - const std::string& h5_filepath, - int display_step, bool wait_key) -> void; - //! @} - //! @} } /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointDetection.hpp b/cpp/src/DO/Sara/SfM/Helpers/KeypointMatching.cpp similarity index 58% rename from cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointDetection.hpp rename to cpp/src/DO/Sara/SfM/Helpers/KeypointMatching.cpp index 9a163b565..f02c2da4e 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointDetection.hpp +++ b/cpp/src/DO/Sara/SfM/Helpers/KeypointMatching.cpp @@ -9,27 +9,19 @@ // you can obtain one at http://mozilla.org/MPL/2.0/. // ========================================================================== // -#pragma once +#include -#include - -#include +#include namespace DO::Sara { - //! @addtogroup SfM - //! @{ - - //! @{ - //! @brief Keypoint detection. - auto detect_keypoints(const std::string& dirpath, - const std::string& h5_filepath, bool overwrite) -> void; - - auto read_keypoints(const std::string& dirpath, - const std::string& h5_filepath) -> void; - //! @} - - //! @} + auto match(const KeypointList& keys1, + const KeypointList& keys2, float lowe_ratio) + -> std::vector + { + AnnMatcher matcher{keys1, keys2, lowe_ratio}; + return matcher.compute_matches(); + } } /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointMatching.hpp b/cpp/src/DO/Sara/SfM/Helpers/KeypointMatching.hpp similarity index 82% rename from cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointMatching.hpp rename to cpp/src/DO/Sara/SfM/Helpers/KeypointMatching.hpp index cd5232079..1d8b421c1 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/KeypointMatching.hpp +++ b/cpp/src/DO/Sara/SfM/Helpers/KeypointMatching.hpp @@ -11,7 +11,6 @@ #pragma once -#include #include @@ -20,16 +19,11 @@ namespace DO::Sara { //! @addtogroup SfM //! @{ - //! @{ //! @brief Keypoint matching. auto match(const KeypointList& keys1, const KeypointList& keys2, float lowe_ratio = 0.6f) -> std::vector; - auto match_keypoints(const std::string& dirpath, - const std::string& h5_filepath, bool overwrite) -> void; - //! @} - //! @} } /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/Triangulation.cpp b/cpp/src/DO/Sara/SfM/Helpers/Triangulation.cpp similarity index 99% rename from cpp/src/DO/Sara/SfM/BuildingBlocks/Triangulation.cpp rename to cpp/src/DO/Sara/SfM/Helpers/Triangulation.cpp index 5053450e4..9ff32f174 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/Triangulation.cpp +++ b/cpp/src/DO/Sara/SfM/Helpers/Triangulation.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include using namespace std; diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/Triangulation.hpp b/cpp/src/DO/Sara/SfM/Helpers/Triangulation.hpp similarity index 100% rename from cpp/src/DO/Sara/SfM/BuildingBlocks/Triangulation.hpp rename to cpp/src/DO/Sara/SfM/Helpers/Triangulation.hpp diff --git a/cpp/src/DO/Sara/SfM/Odometry/FeatureTracker.hpp b/cpp/src/DO/Sara/SfM/Odometry/FeatureTracker.hpp index c52d0c252..2fd475342 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/FeatureTracker.hpp +++ b/cpp/src/DO/Sara/SfM/Odometry/FeatureTracker.hpp @@ -3,7 +3,7 @@ #include #include -#include +#include namespace DO::Sara { diff --git a/cpp/src/DO/Sara/SfM/Odometry/RelativePoseEstimator.hpp b/cpp/src/DO/Sara/SfM/Odometry/RelativePoseEstimator.hpp index 3361cf3c6..9090e0ea1 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/RelativePoseEstimator.hpp +++ b/cpp/src/DO/Sara/SfM/Odometry/RelativePoseEstimator.hpp @@ -10,7 +10,7 @@ #include -#include +#include namespace DO::Sara { diff --git a/cpp/src/DO/Sara/SfM/Odometry/Triangulator.hpp b/cpp/src/DO/Sara/SfM/Odometry/Triangulator.hpp index 4c910ba1b..41993c0e1 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/Triangulator.hpp +++ b/cpp/src/DO/Sara/SfM/Odometry/Triangulator.hpp @@ -2,7 +2,7 @@ #include #include -#include +#include namespace DO::Sara { diff --git a/cpp/src/DO/Sara/SfM/Odometry/VideoStreamer.hpp b/cpp/src/DO/Sara/SfM/Odometry/VideoStreamer.hpp index 984d9a608..fb1cee361 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/VideoStreamer.hpp +++ b/cpp/src/DO/Sara/SfM/Odometry/VideoStreamer.hpp @@ -48,6 +48,11 @@ namespace DO::Sara { return _video_stream.height(); } + auto frame_number() const -> int + { + return _frame_index; + } + auto skip() const -> bool { return _frame_index % (_num_skips + 1) != 0; diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp new file mode 100644 index 000000000..a89fa9303 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -0,0 +1,120 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#include + +#include +#include + + +using namespace DO::Sara; + + +auto v2::OdometryPipeline::set_config( + const std::filesystem::path& video_path, + const v2::BrownConradyDistortionModel& camera) -> void +{ + // Build the dependency graph. + _video_streamer.open(video_path); + _camera = camera; + + // Computer vision tasks. + _distortion_corrector = std::make_unique( + _video_streamer.frame_rgb8(), // + _video_streamer.frame_gray32f(), // + _camera // + ); + _relative_pose_estimator.configure(_camera); +} + +auto v2::OdometryPipeline::read() -> bool +{ + return _video_streamer.read(); +} + +auto v2::OdometryPipeline::process() -> void +{ + if (_video_streamer.skip()) + return; + + _distortion_corrector->undistort(); +} + +auto v2::OdometryPipeline::make_display_frame() const -> Image +{ + return _distortion_corrector->frame_rgb8(); +} + +auto v2::OdometryPipeline::detect_keypoints() const + -> KeypointList +{ + return compute_sift_keypoints(_distortion_corrector->frame_gray32f(), + _feature_params.image_pyr_params); +} + +auto v2::OdometryPipeline::estimate_relative_pose( + const CameraPoseGraph::Vertex u, // + const CameraPoseGraph::Vertex v) const + -> std::pair +{ + auto& logger = Logger::get(); + + SARA_LOGI(logger, "Matching features..."); + const auto& keys_u = _pose_graph[u].keypoints; + const auto& keys_v = _pose_graph[v].keypoints; + if (features(keys_u).empty() || features(keys_v).empty()) + return; + + auto matches = match(keys_u, keys_v, _feature_params.sift_nn_ratio); + if (matches.empty()) + return; + if (matches.size() > _feature_params.num_matches_max) + matches.resize(_feature_params.num_matches_max); + + SARA_LOGI(logger, "Estimating relative pose..."); + auto [geometry, inliers, sample_best] = + _relative_pose_estimator.estimate_relative_pose(keys_u, keys_v, matches); + const auto num_inliers = inliers.flat_array().count(); + SARA_LOGI(logger, "inlier count: {}", num_inliers); + + const auto num_inliers = inliers.flat_array().count(); + SARA_LOGI(logger, "inlier count: {}", num_inliers); + + return std::make_pair(RelativePoseData{.matches = std::move(matches), + .inliers = std::move(inliers), + .motion = {} + + }, + geometry); +} + +auto v2::OdometryPipeline::add_camera_pose_and_grow_point_cloud() -> bool +{ + auto& logger = Logger::get(); + + // Detect and describe the local features. + _pose_prev = _pose_curr; + const auto frame = _distortion_corrector->frame_gray32f(); + const auto frame_number = _video_streamer.frame_number(); + auto keypoints = detect_keypoints(frame); + _pose_curr = _pose_graph.add_absolute_pose(std::move(keypoints), // + frame_number); + + const auto& pose_data = _pose_graph[_pose_curr]; + SARA_LOGI(logger, "Camera [frame:{}]: {} keypoints", // + pose_data.image_id, features(pose_data.keypoints).size()); + + // We need two frames at least for the epipolar geometry. + if (_pose_graph.num_vertices() < 2) + return false; + + return false; +} diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp new file mode 100644 index 000000000..50d1f324e --- /dev/null +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp @@ -0,0 +1,76 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace DO::Sara::v2 { + + class OdometryPipeline + { + public: + auto set_config(const std::filesystem::path& video_path, + const v2::BrownConradyDistortionModel& camera) + -> void; + + auto read() -> bool; + + auto process() -> void; + + auto make_display_frame() const -> Image; + + private: /* computer vision tasks */ + auto detect_keypoints(const ImageView&) const + -> KeypointList; + + auto estimate_relative_pose(const CameraPoseGraph::Vertex u, + const CameraPoseGraph::Vertex v) const + -> std::pair; + + private: /* graph update tasks */ + auto add_camera_pose_and_grow_point_cloud() -> bool; + + private: /* data members */ + VideoStreamer _video_streamer; + v2::BrownConradyDistortionModel _camera; + + std::unique_ptr _distortion_corrector; + v2::RelativePoseEstimator _relative_pose_estimator; + + + //! @brief SfM data. + //! @{ + FeatureParams _feature_params; + FeatureTracker _feature_tracker; + CameraPoseGraph _pose_graph; + //! @} + + //! @brief SfM state. + //! @{ + CameraPoseGraph::Vertex _pose_prev; + CameraPoseGraph::Vertex _pose_curr; + CameraPoseGraph::Edge _relative_pose_edge; + FeatureTracker::TrackArray _tracks_alive; + FeatureTracker::TrackVisibilityCountArray _track_visibility_count; + Eigen::Matrix3d _current_global_rotation = Eigen::Matrix3d::Identity(); + //! @} + }; + +} // namespace DO::Sara::v2 diff --git a/cpp/test/Sara/MultiViewGeometry/test_multiviewgeometry_feature_graph.cpp b/cpp/test/Sara/MultiViewGeometry/test_multiviewgeometry_feature_graph.cpp index d10de268a..b4590a55b 100644 --- a/cpp/test/Sara/MultiViewGeometry/test_multiviewgeometry_feature_graph.cpp +++ b/cpp/test/Sara/MultiViewGeometry/test_multiviewgeometry_feature_graph.cpp @@ -12,14 +12,15 @@ #define BOOST_TEST_MODULE "MultiViewGeometry/Geometry/Feature Graph" #include -#include -#include +#include +#include -#include #include +#include -namespace fs = boost::filesystem; + +namespace fs = std::filesystem; using namespace DO::Sara; @@ -72,10 +73,10 @@ BOOST_AUTO_TEST_CASE(test_incremental_connected_components) auto ds = ICC::initialize_disjoint_sets(rank, parent); ICC::initialize_incremental_components(graph, ds); - for (auto r: rank) + for (auto r : rank) std::cout << "rank = " << r << std::endl; - for (auto p: parent) + for (auto p : parent) std::cout << "p = " << p << std::endl; auto add_edge = [&](auto u, auto v) { @@ -187,11 +188,9 @@ BOOST_AUTO_TEST_CASE(test_read_write_feature_graph_to_hdf5) BOOST_AUTO_TEST_CASE(test_populate_feature_gids) { - auto keys = std::vector{ - KeypointList{}, - KeypointList{}, - KeypointList{} - }; + auto keys = std::vector{KeypointList{}, + KeypointList{}, + KeypointList{}}; features(keys[0]).resize(3); features(keys[1]).resize(1); @@ -209,11 +208,9 @@ BOOST_AUTO_TEST_CASE(test_populate_feature_gids) BOOST_AUTO_TEST_CASE(test_calculate_of_feature_id_offset) { - auto keys = std::vector{ - KeypointList{}, - KeypointList{}, - KeypointList{} - }; + auto keys = std::vector{KeypointList{}, + KeypointList{}, + KeypointList{}}; features(keys[0]).resize(3); features(keys[1]).resize(1); @@ -300,7 +297,7 @@ BOOST_AUTO_TEST_CASE(test_populate_feature_tracks) const auto& component = components[c]; std::cout << "Component " << c << " : "; - for (const auto& v: component) + for (const auto& v : component) std::cout << "GID[" << v << "] = {" << graph[v].image_id << ", " << graph[v].local_id << "}, "; std::cout << std::endl; From 1b8c491449869a45db6ecce575834700a293d282 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Thu, 11 Apr 2024 19:18:19 +0100 Subject: [PATCH 28/49] MAINT: fix compile errors. --- .../BuildingBlocks/RelativePoseEstimator.cpp | 6 --- .../BuildingBlocks/RelativePoseEstimator.hpp | 7 +--- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp | 8 ++-- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp | 2 +- cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp | 1 - .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 39 ++++++++++--------- .../Sara/SfM/OdometryV2/OdometryPipeline.hpp | 10 ++--- 7 files changed, 33 insertions(+), 40 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp index 4c7be664e..42b7a44db 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.cpp @@ -8,12 +8,6 @@ using namespace DO::Sara::v2; -RelativePoseEstimator::RelativePoseEstimator( - const v2::BrownConradyDistortionModel& camera) -{ - configure(camera); -} - auto RelativePoseEstimator::configure( const v2::BrownConradyDistortionModel& camera) -> void { diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp index 914ad0b55..2dd996b07 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/RelativePoseEstimator.hpp @@ -26,11 +26,8 @@ namespace DO::Sara::v2 { Eigen::Matrix3d _K; Eigen::Matrix3d _K_inv; - RelativePoseEstimator( - const v2::BrownConradyDistortionModel& camera); - - auto configure(const v2::BrownConradyDistortionModel& camera) - -> void; + auto + configure(const v2::BrownConradyDistortionModel& camera) -> void; auto estimate_relative_pose(const KeypointList& src_keys, const KeypointList& dst_keys, diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp index 0a551fd31..5e8fc1fb5 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp @@ -45,7 +45,7 @@ auto CameraPoseGraph::add_absolute_pose( auto CameraPoseGraph::add_relative_pose( const v2::RelativePoseEstimator& relative_pose_estimator, // const FeatureParams& feature_params, // - const Vertex u, const Vertex v) -> void + const Vertex u, const Vertex v) -> std::pair { auto& logger = Logger::get(); @@ -53,11 +53,11 @@ auto CameraPoseGraph::add_relative_pose( const auto& src_keys = _g[u].keypoints; const auto& dst_keys = _g[v].keypoints; if (features(src_keys).empty() || features(dst_keys).empty()) - return; + return {{}, false}; auto matches = match(src_keys, dst_keys, feature_params.sift_nn_ratio); if (matches.empty()) - return; + return {{}, false}; if (matches.size() > feature_params.num_matches_max) matches.resize(feature_params.num_matches_max); @@ -78,4 +78,6 @@ auto CameraPoseGraph::add_relative_pose( relative_motion_data.matches = std::move(matches); relative_motion_data.inliers = std::move(inliers); } + + return {e, edge_added}; } diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp index 701ef40a5..2792d5e42 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp @@ -93,7 +93,7 @@ namespace DO::Sara { auto add_relative_pose( const v2::RelativePoseEstimator& relative_pose_estimator, // const FeatureParams& feature_params, // - const Vertex src, const Vertex dst) -> void; + const Vertex src, const Vertex dst) -> std::pair; private: //! @brief The graph data structure shortened as g. diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp index c5dd624dd..6d76ccdbe 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp @@ -12,7 +12,6 @@ #pragma once #include -#include namespace DO::Sara { diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index a89fa9303..a7f1d4734 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -11,7 +11,12 @@ #include -#include +#include + +#include +#include + +#include #include @@ -53,29 +58,28 @@ auto v2::OdometryPipeline::make_display_frame() const -> Image return _distortion_corrector->frame_rgb8(); } -auto v2::OdometryPipeline::detect_keypoints() const +auto v2::OdometryPipeline::detect_keypoints(const ImageView& image) const -> KeypointList { - return compute_sift_keypoints(_distortion_corrector->frame_gray32f(), - _feature_params.image_pyr_params); + return compute_sift_keypoints(image, _feature_params.image_pyr_params); } auto v2::OdometryPipeline::estimate_relative_pose( - const CameraPoseGraph::Vertex u, // - const CameraPoseGraph::Vertex v) const + const CameraPoseGraph::Vertex pose_u, // + const CameraPoseGraph::Vertex pose_v) const -> std::pair { auto& logger = Logger::get(); SARA_LOGI(logger, "Matching features..."); - const auto& keys_u = _pose_graph[u].keypoints; - const auto& keys_v = _pose_graph[v].keypoints; + const auto& keys_u = _pose_graph[pose_u].keypoints; + const auto& keys_v = _pose_graph[pose_v].keypoints; if (features(keys_u).empty() || features(keys_v).empty()) - return; + return {}; auto matches = match(keys_u, keys_v, _feature_params.sift_nn_ratio); if (matches.empty()) - return; + return {}; if (matches.size() > _feature_params.num_matches_max) matches.resize(_feature_params.num_matches_max); @@ -85,15 +89,14 @@ auto v2::OdometryPipeline::estimate_relative_pose( const auto num_inliers = inliers.flat_array().count(); SARA_LOGI(logger, "inlier count: {}", num_inliers); - const auto num_inliers = inliers.flat_array().count(); - SARA_LOGI(logger, "inlier count: {}", num_inliers); - - return std::make_pair(RelativePoseData{.matches = std::move(matches), - .inliers = std::move(inliers), - .motion = {} + return { + RelativePoseData{.matches = std::move(matches), + .inliers = std::move(inliers), + .motion = {} - }, - geometry); + }, + geometry // + }; } auto v2::OdometryPipeline::add_camera_pose_and_grow_point_cloud() -> bool diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp index 50d1f324e..7ca06be46 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp @@ -12,10 +12,8 @@ #pragma once #include -#include #include #include -#include #include #include @@ -26,9 +24,9 @@ namespace DO::Sara::v2 { class OdometryPipeline { public: - auto set_config(const std::filesystem::path& video_path, - const v2::BrownConradyDistortionModel& camera) - -> void; + auto + set_config(const std::filesystem::path& video_path, + const v2::BrownConradyDistortionModel& camera) -> void; auto read() -> bool; @@ -47,7 +45,7 @@ namespace DO::Sara::v2 { private: /* graph update tasks */ auto add_camera_pose_and_grow_point_cloud() -> bool; - private: /* data members */ + public: /* data members */ VideoStreamer _video_streamer; v2::BrownConradyDistortionModel _camera; From 4530b2d72ea61a49ffbdf0d160229e82f699106c Mon Sep 17 00:00:00 2001 From: David OK Date: Sat, 13 Apr 2024 11:09:14 +0100 Subject: [PATCH 29/49] WIP: save work. --- .../Sara/SfM/BuildingBlocks/FeatureParams.hpp | 2 +- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp | 41 +------- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp | 11 ++- .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 94 +++++++++++++++++-- .../Sara/SfM/OdometryV2/OdometryPipeline.hpp | 10 +- 5 files changed, 105 insertions(+), 53 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp index 7ee194712..17303cc6b 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp @@ -10,7 +10,7 @@ namespace DO::Sara { ImagePyramidParams image_pyr_params = ImagePyramidParams(0); float sift_nn_ratio = 0.6f; std::size_t num_matches_max = 1000u; - std::size_t num_inliers_min = 100u; + int num_inliers_min = 100; }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp index 5e8fc1fb5..6cff045be 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp @@ -43,41 +43,10 @@ auto CameraPoseGraph::add_absolute_pose( } auto CameraPoseGraph::add_relative_pose( - const v2::RelativePoseEstimator& relative_pose_estimator, // - const FeatureParams& feature_params, // - const Vertex u, const Vertex v) -> std::pair + const RelativePoseData& relative_pose_data, // + const Vertex u, const Vertex v) -> bool { - auto& logger = Logger::get(); - - SARA_LOGI(logger, "Match features..."); - const auto& src_keys = _g[u].keypoints; - const auto& dst_keys = _g[v].keypoints; - if (features(src_keys).empty() || features(dst_keys).empty()) - return {{}, false}; - - auto matches = match(src_keys, dst_keys, feature_params.sift_nn_ratio); - if (matches.empty()) - return {{}, false}; - if (matches.size() > feature_params.num_matches_max) - matches.resize(feature_params.num_matches_max); - - SARA_LOGI(logger, "Estimating relative pose..."); - auto [geometry, inliers, sample_best] = - relative_pose_estimator.estimate_relative_pose(src_keys, dst_keys, - matches); - const auto num_inliers = inliers.flat_array().count(); - SARA_LOGI(logger, "inlier count: {}", num_inliers); - - const auto success = num_inliers > 100; - auto e = Edge{}; - auto edge_added = false; - if (success) - { - std::tie(e, edge_added) = boost::add_edge(u, v, _g); - auto& relative_motion_data = _g[e]; - relative_motion_data.matches = std::move(matches); - relative_motion_data.inliers = std::move(inliers); - } - - return {e, edge_added}; + const auto [e, edge_added] = boost::add_edge(u, v, _g); + _g[e] = relative_pose_data; + return edge_added; } diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp index 2792d5e42..aeee5ed0c 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp @@ -82,6 +82,11 @@ namespace DO::Sara { return _g[e]; } + auto edge(const Vertex u, const Vertex v) const -> std::pair + { + return boost::edge(u, v, _g); + } + auto num_vertices() const -> VertexIndex { return boost::num_vertices(_g); @@ -90,10 +95,8 @@ namespace DO::Sara { auto add_absolute_pose(KeypointList&& keypoints, const int image_id) -> Vertex; - auto add_relative_pose( - const v2::RelativePoseEstimator& relative_pose_estimator, // - const FeatureParams& feature_params, // - const Vertex src, const Vertex dst) -> std::pair; + auto add_relative_pose(const RelativePoseData& relative_pose_data, // + const Vertex src, const Vertex dst) -> bool; private: //! @brief The graph data structure shortened as g. diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index a7f1d4734..5befdcb94 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -61,6 +61,8 @@ auto v2::OdometryPipeline::make_display_frame() const -> Image auto v2::OdometryPipeline::detect_keypoints(const ImageView& image) const -> KeypointList { + auto& logger = Logger::get(); + SARA_LOGI(logger, "[Feature Detection] Matching image keypoints..."); return compute_sift_keypoints(image, _feature_params.image_pyr_params); } @@ -71,34 +73,86 @@ auto v2::OdometryPipeline::estimate_relative_pose( { auto& logger = Logger::get(); - SARA_LOGI(logger, "Matching features..."); const auto& keys_u = _pose_graph[pose_u].keypoints; const auto& keys_v = _pose_graph[pose_v].keypoints; if (features(keys_u).empty() || features(keys_v).empty()) + { + SARA_LOGI(logger, "[Relative Pose] Skipped image matching..."); return {}; + } auto matches = match(keys_u, keys_v, _feature_params.sift_nn_ratio); + SARA_LOGI(logger, "[Relative Pose] Matched image keypoints..."); if (matches.empty()) return {}; if (matches.size() > _feature_params.num_matches_max) matches.resize(_feature_params.num_matches_max); - SARA_LOGI(logger, "Estimating relative pose..."); - auto [geometry, inliers, sample_best] = + auto [two_view_geometry, inliers, sample_best] = _relative_pose_estimator.estimate_relative_pose(keys_u, keys_v, matches); - const auto num_inliers = inliers.flat_array().count(); - SARA_LOGI(logger, "inlier count: {}", num_inliers); + SARA_LOGI(logger, "[Relative Pose] Estimated relative pose..."); - return { + const auto res = std::pair{ RelativePoseData{.matches = std::move(matches), .inliers = std::move(inliers), - .motion = {} + .motion = + { + .R = two_view_geometry.C2.R, // + .t = two_view_geometry.C2.t // + } }, - geometry // + two_view_geometry // }; + + return res; +} + +auto v2::OdometryPipeline::update_absolute_pose_from_latest_relative_pose_data( + const RelativePoseData& relative_pose_data, + const TwoViewGeometry& two_view_geometry) -> bool +{ + auto& logger = Logger::get(); + + const auto num_inliers = relative_pose_data.inliers.flat_array().count(); + SARA_LOGI(logger, "[SfM] Relative pose inliers: {} 3D points", num_inliers); + if (num_inliers < _feature_params.num_inliers_min) + { + SARA_LOGI(logger, "[SfM] Relative pose failed!"); + return false; + } + SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); + + if (_pose_graph.num_vertices() == 2) + { + SARA_LOGI(logger, "Initializing the first two camera poses..."); + // Set the absolute pose of the first camera which is the identity rigid + // body transformation. + auto& initial_pose = _pose_graph[_pose_prev].pose; + { + initial_pose.q.setIdentity(); + initial_pose.t.setZero(); + } + + // Set the absolute pose of the first camera as the first relative pose. + auto& second_pose = _pose_graph[_pose_curr].pose; + { + // HEURISTICS: Advance from only 5 cm at most to view something nice. + const auto [e, edge_exists] = _pose_graph.edge(_pose_prev, _pose_curr); + if (!edge_exists) + throw std::runtime_error{"Edge must exist!"}; + + _pose_graph[e] = relative_pose_data; + } + + // TODO: make a new function for this. + // SARA_LOGI(logger, "Initializing the point cloud..."); + // _point_cloud_operator->init_point_cloud(_tracks_alive, current_image, + // _relative_pose_edge_id, _camera); + } } + auto v2::OdometryPipeline::add_camera_pose_and_grow_point_cloud() -> bool { auto& logger = Logger::get(); @@ -112,12 +166,34 @@ auto v2::OdometryPipeline::add_camera_pose_and_grow_point_cloud() -> bool frame_number); const auto& pose_data = _pose_graph[_pose_curr]; - SARA_LOGI(logger, "Camera [frame:{}]: {} keypoints", // + SARA_LOGI(logger, + "[SfM] Initialized new camera pose[frame:{}]: {} keypoints", // pose_data.image_id, features(pose_data.keypoints).size()); // We need two frames at least for the epipolar geometry. if (_pose_graph.num_vertices() < 2) return false; + const auto [relative_pose_data, two_view_geometry] = + this->estimate_relative_pose(_pose_prev, _pose_curr); + const auto num_inliers = relative_pose_data.inliers.flat_array().count(); + SARA_LOGI(logger, "[SfM] Relative pose inliers: {} 3D points", num_inliers); + if (num_inliers < _feature_params.num_inliers_min) + { + SARA_LOGI(logger, "[SfM] Relative pose failed!"); + return false; + } + SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); + + if (_pose_graph.num_vertices() == 2) + { + // Init point cloud. + } + else + { + // Grow point cloud by triangulation. + } + + return false; } diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp index 7ca06be46..455095179 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp @@ -24,9 +24,9 @@ namespace DO::Sara::v2 { class OdometryPipeline { public: - auto - set_config(const std::filesystem::path& video_path, - const v2::BrownConradyDistortionModel& camera) -> void; + auto set_config(const std::filesystem::path& video_path, + const v2::BrownConradyDistortionModel& camera) + -> void; auto read() -> bool; @@ -42,6 +42,10 @@ namespace DO::Sara::v2 { const CameraPoseGraph::Vertex v) const -> std::pair; + auto update_absolute_pose_from_latest_relative_pose_data( + const RelativePoseData& relative_pose_data, + const TwoViewGeometry& two_view_geometry) -> bool; + private: /* graph update tasks */ auto add_camera_pose_and_grow_point_cloud() -> bool; From fd7c4091f774b45c82180ab697f646cd63364a7d Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 14 Apr 2024 18:43:16 +0100 Subject: [PATCH 30/49] WIP: refactor code. --- .../Geometry/QuaternionBasedPose.hpp | 15 +- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp | 30 ++-- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp | 24 ++- .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 143 ++++++++---------- .../Sara/SfM/OdometryV2/OdometryPipeline.hpp | 21 ++- 5 files changed, 119 insertions(+), 114 deletions(-) diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/QuaternionBasedPose.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/QuaternionBasedPose.hpp index ac331c983..be3af6040 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/QuaternionBasedPose.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/QuaternionBasedPose.hpp @@ -12,8 +12,7 @@ namespace DO::Sara { Eigen::Quaternion q; Eigen::Vector3 t; - inline auto operator()(const Eigen::Vector3& x) const - -> Eigen::Vector3 + inline auto operator*(const Eigen::Vector3& x) const -> Eigen::Vector3 { return q * x + t; } @@ -25,9 +24,15 @@ namespace DO::Sara { inline auto matrix4() const -> Eigen::Matrix4 { - return (Eigen::Matrix{} << - matrix34(), Eigen::RowVector3::Zero().homogeneous()) - .finished(); + auto r = Eigen::Matrix{}; + r << matrix34(), Eigen::RowVector3::Zero().homogeneous(); + return r; + } + + static inline auto identity() -> QuaternionBasedPose + { + return {.q = Eigen::Quaternion::Identity(), + .t = Eigen::Vector3::Zero()}; } }; diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp index 6cff045be..37b3b784f 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp @@ -20,33 +20,33 @@ using namespace DO::Sara; -auto CameraPoseGraph::add_absolute_pose( - KeypointList&& keypoints, // - const int image_id) -> CameraPoseGraph::Vertex +auto CameraPoseGraph::add_absolute_pose(AbsolutePoseData&& data) + -> CameraPoseGraph::Vertex { auto& logger = Logger::get(); - SARA_LOGI(logger, "Detecting keypoints for image frame {}", image_id); - // Grow the pose graph by creating a new camera vertex. const auto v = boost::add_vertex(_g); // Store the camera pose data. - auto& pose_data = _g[v]; - pose_data.image_id = image_id; - pose_data.keypoints = std::move(keypoints); + _g[v] = std::move(data); - const auto& f = features(pose_data.keypoints); - SARA_LOGI(logger, "Camera {}: {} keypoints", v, f.size()); + SARA_LOGI(logger, + "[SfM] Added camera absolute pose[frame:{}]:\n" + "Keypoints: {} points\n" + "Absolute pose: {}\n", // + _g[v].image_id, // + features(_g[v].keypoints).size(), // + _g[v].pose.matrix34()); return v; } -auto CameraPoseGraph::add_relative_pose( - const RelativePoseData& relative_pose_data, // - const Vertex u, const Vertex v) -> bool +auto CameraPoseGraph::add_relative_pose(const Vertex u, const Vertex v, + RelativePoseData&& relative_pose_data) + -> CameraPoseGraph::Edge { const auto [e, edge_added] = boost::add_edge(u, v, _g); - _g[e] = relative_pose_data; - return edge_added; + _g[e] = std::move(relative_pose_data); + return e; } diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp index aeee5ed0c..5ba8584ce 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp @@ -33,6 +33,23 @@ namespace DO::Sara { KeypointList keypoints; //! @brief "Absolute" pose w.r.t. some reference frame. QuaternionBasedPose pose; + + AbsolutePoseData() = default; + + AbsolutePoseData(const AbsolutePoseData&) = default; + + AbsolutePoseData(AbsolutePoseData&&) = default; + + AbsolutePoseData(const int image_id, + KeypointList&& keypoints, + QuaternionBasedPose&& pose) + : image_id{image_id} + , keypoints{std::move(keypoints)} + , pose{std::move(pose)} + { + } + + auto operator=(AbsolutePoseData&&) -> AbsolutePoseData& = default; }; struct RelativePoseData @@ -92,11 +109,10 @@ namespace DO::Sara { return boost::num_vertices(_g); } - auto add_absolute_pose(KeypointList&& keypoints, - const int image_id) -> Vertex; + auto add_absolute_pose(AbsolutePoseData&& data) -> Vertex; - auto add_relative_pose(const RelativePoseData& relative_pose_data, // - const Vertex src, const Vertex dst) -> bool; + auto add_relative_pose(const Vertex src, const Vertex dst, + RelativePoseData&& relative_pose_data) -> Edge; private: //! @brief The graph data structure shortened as g. diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index 5befdcb94..92006bce8 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -51,6 +51,8 @@ auto v2::OdometryPipeline::process() -> void return; _distortion_corrector->undistort(); + + add_camera_pose(); } auto v2::OdometryPipeline::make_display_frame() const -> Image @@ -67,21 +69,19 @@ auto v2::OdometryPipeline::detect_keypoints(const ImageView& image) const } auto v2::OdometryPipeline::estimate_relative_pose( - const CameraPoseGraph::Vertex pose_u, // - const CameraPoseGraph::Vertex pose_v) const + const KeypointList& keys_src, + const KeypointList& keys_dst) const -> std::pair { auto& logger = Logger::get(); - const auto& keys_u = _pose_graph[pose_u].keypoints; - const auto& keys_v = _pose_graph[pose_v].keypoints; - if (features(keys_u).empty() || features(keys_v).empty()) + if (features(keys_src).empty() || features(keys_dst).empty()) { SARA_LOGI(logger, "[Relative Pose] Skipped image matching..."); return {}; } - auto matches = match(keys_u, keys_v, _feature_params.sift_nn_ratio); + auto matches = match(keys_src, keys_dst, _feature_params.sift_nn_ratio); SARA_LOGI(logger, "[Relative Pose] Matched image keypoints..."); if (matches.empty()) return {}; @@ -89,10 +89,11 @@ auto v2::OdometryPipeline::estimate_relative_pose( matches.resize(_feature_params.num_matches_max); auto [two_view_geometry, inliers, sample_best] = - _relative_pose_estimator.estimate_relative_pose(keys_u, keys_v, matches); + _relative_pose_estimator.estimate_relative_pose(keys_src, keys_dst, + matches); SARA_LOGI(logger, "[Relative Pose] Estimated relative pose..."); - const auto res = std::pair{ + const auto res = std::make_pair( // RelativePoseData{.matches = std::move(matches), .inliers = std::move(inliers), .motion = @@ -102,98 +103,82 @@ auto v2::OdometryPipeline::estimate_relative_pose( } }, - two_view_geometry // - }; + std::move(two_view_geometry) // + ); return res; } -auto v2::OdometryPipeline::update_absolute_pose_from_latest_relative_pose_data( - const RelativePoseData& relative_pose_data, - const TwoViewGeometry& two_view_geometry) -> bool +auto v2::OdometryPipeline::add_camera_pose() -> bool { auto& logger = Logger::get(); - const auto num_inliers = relative_pose_data.inliers.flat_array().count(); - SARA_LOGI(logger, "[SfM] Relative pose inliers: {} 3D points", num_inliers); - if (num_inliers < _feature_params.num_inliers_min) + // Detect and describe the local features. + _pose_prev = _pose_curr; + + const auto frame = _distortion_corrector->frame_gray32f(); + const auto frame_number = _video_streamer.frame_number(); + auto keys_curr = detect_keypoints(frame); + + if (_pose_graph.num_vertices() == 1) { - SARA_LOGI(logger, "[SfM] Relative pose failed!"); - return false; + // Initialize the new camera pose from the latest image frame. + auto abs_pose_curr = QuaternionBasedPose::identity(); + auto abs_pose_data = AbsolutePoseData{ + frame_number, // + std::move(keys_curr), // + std::move(abs_pose_curr) // + }; + _pose_curr = _pose_graph.add_absolute_pose(std::move(abs_pose_data)); + + return true; } - SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); - - if (_pose_graph.num_vertices() == 2) + else { - SARA_LOGI(logger, "Initializing the first two camera poses..."); - // Set the absolute pose of the first camera which is the identity rigid - // body transformation. - auto& initial_pose = _pose_graph[_pose_prev].pose; + const auto& keys_prev = _pose_graph[_pose_prev].keypoints; + auto [rel_pose_data, two_view_geometry] = + estimate_relative_pose(keys_prev, keys_curr); + const auto num_inliers = rel_pose_data.inliers.flat_array().count(); + SARA_LOGI(logger, "[SfM] Relative pose inliers: {} 3D points", num_inliers); + if (num_inliers < _feature_params.num_inliers_min) { - initial_pose.q.setIdentity(); - initial_pose.t.setZero(); + SARA_LOGI(logger, "[SfM] Relative pose failed!"); + return false; } + SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); - // Set the absolute pose of the first camera as the first relative pose. - auto& second_pose = _pose_graph[_pose_curr].pose; + if (_pose_graph.num_vertices() == 2) { - // HEURISTICS: Advance from only 5 cm at most to view something nice. - const auto [e, edge_exists] = _pose_graph.edge(_pose_prev, _pose_curr); - if (!edge_exists) - throw std::runtime_error{"Edge must exist!"}; + auto abs_pose_curr = QuaternionBasedPose{ + .q = Eigen::Quaterniond{rel_pose_data.motion.R}, + .t = rel_pose_data.motion.t // + }; - _pose_graph[e] = relative_pose_data; - } + auto abs_pose_data = AbsolutePoseData{ + frame_number, // + std::move(keys_curr), // + std::move(abs_pose_curr) // + }; - // TODO: make a new function for this. - // SARA_LOGI(logger, "Initializing the point cloud..."); - // _point_cloud_operator->init_point_cloud(_tracks_alive, current_image, - // _relative_pose_edge_id, _camera); - } -} + // 1. Add the absolute pose vertex. + _pose_graph.add_absolute_pose(std::move(abs_pose_data)); + // 2. Add the pose edge, which will invalidate the relative pose data. + _pose_graph.add_relative_pose(_pose_prev, _pose_curr, + std::move(rel_pose_data)); -auto v2::OdometryPipeline::add_camera_pose_and_grow_point_cloud() -> bool -{ - auto& logger = Logger::get(); + // 3. TODO: Init point cloud - // Detect and describe the local features. - _pose_prev = _pose_curr; - const auto frame = _distortion_corrector->frame_gray32f(); - const auto frame_number = _video_streamer.frame_number(); - auto keypoints = detect_keypoints(frame); - _pose_curr = _pose_graph.add_absolute_pose(std::move(keypoints), // - frame_number); - - const auto& pose_data = _pose_graph[_pose_curr]; - SARA_LOGI(logger, - "[SfM] Initialized new camera pose[frame:{}]: {} keypoints", // - pose_data.image_id, features(pose_data.keypoints).size()); - - // We need two frames at least for the epipolar geometry. - if (_pose_graph.num_vertices() < 2) - return false; - - const auto [relative_pose_data, two_view_geometry] = - this->estimate_relative_pose(_pose_prev, _pose_curr); - const auto num_inliers = relative_pose_data.inliers.flat_array().count(); - SARA_LOGI(logger, "[SfM] Relative pose inliers: {} 3D points", num_inliers); - if (num_inliers < _feature_params.num_inliers_min) - { - SARA_LOGI(logger, "[SfM] Relative pose failed!"); - return false; - } - SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); + return true; + } + else + { + // 1. Add the absolute pose vertex. - if (_pose_graph.num_vertices() == 2) - { - // Init point cloud. - } - else - { - // Grow point cloud by triangulation. + // TODO: Grow point cloud by triangulation. + return false; + } } - return false; } diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp index 455095179..acead5370 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp @@ -24,9 +24,9 @@ namespace DO::Sara::v2 { class OdometryPipeline { public: - auto set_config(const std::filesystem::path& video_path, - const v2::BrownConradyDistortionModel& camera) - -> void; + auto + set_config(const std::filesystem::path& video_path, + const v2::BrownConradyDistortionModel& camera) -> void; auto read() -> bool; @@ -34,20 +34,19 @@ namespace DO::Sara::v2 { auto make_display_frame() const -> Image; + auto detect_keypoints() -> const KeypointList&; + auto estimate_relative_pose() -> const RelativePoseData&; + private: /* computer vision tasks */ auto detect_keypoints(const ImageView&) const -> KeypointList; - auto estimate_relative_pose(const CameraPoseGraph::Vertex u, - const CameraPoseGraph::Vertex v) const - -> std::pair; - - auto update_absolute_pose_from_latest_relative_pose_data( - const RelativePoseData& relative_pose_data, - const TwoViewGeometry& two_view_geometry) -> bool; + auto estimate_relative_pose(const KeypointList& keys_src, + const KeypointList& keys_dst) + const -> std::pair; private: /* graph update tasks */ - auto add_camera_pose_and_grow_point_cloud() -> bool; + auto add_camera_pose() -> bool; public: /* data members */ VideoStreamer _video_streamer; From 24a7ebd06e69db5dada5708caa7e012bfb7c73fd Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 14 Apr 2024 19:28:44 +0100 Subject: [PATCH 31/49] MAINT: remove audio stream code. It does not compile anymore with FFmpeg 7.0 and we are not encoding any audio data anyways. --- cpp/src/DO/Sara/VideoIO/VideoWriter.cpp | 325 ------------------------ cpp/src/DO/Sara/VideoIO/VideoWriter.hpp | 3 - 2 files changed, 328 deletions(-) diff --git a/cpp/src/DO/Sara/VideoIO/VideoWriter.cpp b/cpp/src/DO/Sara/VideoIO/VideoWriter.cpp index b6620f213..789cffdb4 100644 --- a/cpp/src/DO/Sara/VideoIO/VideoWriter.cpp +++ b/cpp/src/DO/Sara/VideoIO/VideoWriter.cpp @@ -51,7 +51,6 @@ av_always_inline char* av_err2str(int errnum) } #endif -static constexpr auto STREAM_FRAME_RATE = 25; /* 25 images/s */ static constexpr auto STREAM_PIX_FMT = AV_PIX_FMT_YUV420P; /* default pix_fmt */ @@ -112,90 +111,6 @@ namespace DO::Sara { // ======================================================================== // // Add an output stream. - static void add_stream(OutputStream* out_stream, AVFormatContext* out_context, - const AVCodec** codec, enum AVCodecID codec_id) - { - AVCodecContext* c; - /* find the encoder */ - *codec = avcodec_find_encoder(codec_id); - if (!(*codec)) - throw std::runtime_error{format("Could not find encoder for '%s'", - avcodec_get_name(codec_id))}; - out_stream->stream = avformat_new_stream(out_context, nullptr); - if (!out_stream->stream) - throw std::runtime_error{"Could not allocate stream"}; - - out_stream->stream->id = out_context->nb_streams - 1; - c = avcodec_alloc_context3(*codec); - if (!c) - throw std::runtime_error{"Could not alloc an encoding context"}; - - out_stream->encoding_context = c; - switch ((*codec)->type) - { - case AVMEDIA_TYPE_AUDIO: - c->sample_fmt = - (*codec)->sample_fmts ? (*codec)->sample_fmts[0] : AV_SAMPLE_FMT_FLTP; - c->bit_rate = 64000; - c->sample_rate = 44100; - if ((*codec)->supported_samplerates) - { - c->sample_rate = (*codec)->supported_samplerates[0]; - for (int i = 0; (*codec)->supported_samplerates[i]; ++i) - { - if ((*codec)->supported_samplerates[i] == 44100) - c->sample_rate = 44100; - } - } - c->channels = av_get_channel_layout_nb_channels(c->channel_layout); - c->channel_layout = AV_CH_LAYOUT_STEREO; - if ((*codec)->channel_layouts) - { - c->channel_layout = (*codec)->channel_layouts[0]; - for (int i = 0; (*codec)->channel_layouts[i]; ++i) - { - if ((*codec)->channel_layouts[i] == AV_CH_LAYOUT_STEREO) - c->channel_layout = AV_CH_LAYOUT_STEREO; - } - } - c->channels = av_get_channel_layout_nb_channels(c->channel_layout); - out_stream->stream->time_base = AVRational{1, c->sample_rate}; - break; - case AVMEDIA_TYPE_VIDEO: - c->codec_id = codec_id; - c->bit_rate = 400000; - /* Resolution must be a multiple of two. */ - c->width = 352; - c->height = 288; - /* timebase: This is the fundamental unit of time (in seconds) in terms - * of which frame timestamps are represented. For fixed-fps content, - * timebase should be 1/framerate and timestamp increments should be - * identical to 1. */ - out_stream->stream->time_base = {1, STREAM_FRAME_RATE}; - c->time_base = out_stream->stream->time_base; - c->gop_size = 12; /* emit one intra frame every twelve frames at most */ - c->pix_fmt = STREAM_PIX_FMT; - if (c->codec_id == AV_CODEC_ID_MPEG2VIDEO) - { - /* just for testing, we also add B-frames */ - c->max_b_frames = 2; - } - if (c->codec_id == AV_CODEC_ID_MPEG1VIDEO) - { - /* Needed to avoid using macroblocks in which some coeffs overflow. - * This does not happen with normal video, it just happens here as - * the motion of the chroma plane does not match the luma plane. */ - c->mb_decision = 2; - } - break; - default: - break; - } - /* Some formats want stream headers to be separate. */ - if (out_context->oformat->flags & AVFMT_GLOBALHEADER) - c->flags |= AV_CODEC_FLAG_GLOBAL_HEADER; - } - static void add_video_stream(OutputStream* ostream, AVFormatContext* format_context, const AVCodec** codec, @@ -252,155 +167,6 @@ namespace DO::Sara { } - // ======================================================================== // - // Audio output - // - static AVFrame* alloc_audio_frame(enum AVSampleFormat sample_fmt, - uint64_t channel_layout, int sample_rate, - int nb_samples) - { - AVFrame* frame = av_frame_alloc(); - if (!frame) - throw std::runtime_error{"Error allocating an audio frame"}; - - frame->format = sample_fmt; - frame->channel_layout = channel_layout; - frame->sample_rate = sample_rate; - frame->nb_samples = nb_samples; - if (nb_samples) - { - const auto ret = av_frame_get_buffer(frame, 0); - if (ret < 0) - throw std::runtime_error{"Error allocating an audio buffer"}; - } - return frame; - } - - static void open_audio(AVFormatContext*, - const AVCodec* codec, - OutputStream* ost, - AVDictionary* opt_arg) - { - AVCodecContext* c; - int nb_samples; - int ret; - AVDictionary* opt = nullptr; - c = ost->encoding_context; - /* open it */ - av_dict_copy(&opt, opt_arg, 0); - ret = avcodec_open2(c, codec, &opt); - av_dict_free(&opt); - if (ret < 0) - throw std::runtime_error{ - format("Could not open audio codec: %s", av_err2str(ret))}; - - /* init signal generator */ - ost->t = 0; - ost->tincr = static_cast(2 * M_PI * 110.0 / c->sample_rate); - /* increment frequency by 110 Hz per second */ - ost->tincr2 = - static_cast(2 * M_PI * 110.0 / c->sample_rate / c->sample_rate); - if (c->codec->capabilities & AV_CODEC_CAP_VARIABLE_FRAME_SIZE) - nb_samples = 10000; - else - nb_samples = c->frame_size; - ost->frame = alloc_audio_frame(c->sample_fmt, c->channel_layout, - c->sample_rate, nb_samples); - ost->tmp_frame = alloc_audio_frame(AV_SAMPLE_FMT_S16, c->channel_layout, - c->sample_rate, nb_samples); - /* copy the stream parameters to the muxer */ - ret = avcodec_parameters_from_context(ost->stream->codecpar, c); - if (ret < 0) - throw std::runtime_error{"Could not copy the stream parameters"}; - - /* create resampler context */ - ost->swr_ctx = swr_alloc(); - if (!ost->swr_ctx) - throw std::runtime_error{"Could not allocate resampler context"}; - - /* set options */ - av_opt_set_int(ost->swr_ctx, "in_channel_count", c->channels, 0); - av_opt_set_int(ost->swr_ctx, "in_sample_rate", c->sample_rate, 0); - av_opt_set_sample_fmt(ost->swr_ctx, "in_sample_fmt", AV_SAMPLE_FMT_S16, 0); - av_opt_set_int(ost->swr_ctx, "out_channel_count", c->channels, 0); - av_opt_set_int(ost->swr_ctx, "out_sample_rate", c->sample_rate, 0); - av_opt_set_sample_fmt(ost->swr_ctx, "out_sample_fmt", c->sample_fmt, 0); - /* initialize the resampling context */ - if ((ret = swr_init(ost->swr_ctx)) < 0) - throw std::runtime_error{"Failed to initialize the resampling context\n"}; - } - - -#if 0 - /* Prepare a 16 bit dummy audio frame of 'frame_size' samples and - * 'nb_channels' channels. */ - static AVFrame* get_audio_frame(OutputStream* ost) - { - AVFrame* frame = ost->tmp_frame; - int j, i, v; - int16_t* q = reinterpret_cast(frame->data[0]); - /* check if we want to generate more frames */ - if (av_compare_ts(ost->next_pts, ost->encoding_context->time_base, - static_cast(STREAM_DURATION), {1, 1}) > 0) - return nullptr; - for (j = 0; j < frame->nb_samples; j++) - { - v = (int) (sin(ost->t) * 10000); - for (i = 0; i < ost->encoding_context->channels; i++) - *q++ = v; - ost->t += ost->tincr; - ost->tincr += ost->tincr2; - } - frame->pts = ost->next_pts; - ost->next_pts += frame->nb_samples; - return frame; - } - - /* - * encode one audio frame and send it to the muxer - * return 1 when encoding is finished, 0 otherwise - */ - static int write_audio_frame(AVFormatContext* oc, OutputStream* ost) - { - AVCodecContext* c; - AVFrame* frame; - int ret; - int dst_nb_samples; - c = ost->encoding_context; - frame = get_audio_frame(ost); - if (frame) - { - /* convert samples from native format to destination codec format, using - * the resampler */ - /* compute destination number of samples */ - dst_nb_samples = static_cast(av_rescale_rnd( - swr_get_delay(ost->swr_ctx, c->sample_rate) + frame->nb_samples, - c->sample_rate, c->sample_rate, AV_ROUND_UP)); - av_assert0(dst_nb_samples == frame->nb_samples); - /* when we pass a frame to the encoder, it may keep a reference to it - * internally; - * make sure we do not overwrite it here - */ - ret = av_frame_make_writable(ost->frame); - if (ret < 0) - throw std::runtime_error{"Could not make frame writable!"}; - - /* convert to destination format */ - ret = swr_convert(ost->swr_ctx, ost->frame->data, dst_nb_samples, - (const uint8_t**) frame->data, frame->nb_samples); - if (ret < 0) - throw std::runtime_error{"Error while converting audio frame!"}; - - frame = ost->frame; - frame->pts = - av_rescale_q(ost->samples_count, {1, c->sample_rate}, c->time_base); - ost->samples_count += dst_nb_samples; - } - return write_frame(oc, c, ost->stream, frame); - } -#endif - - // ======================================================================== // // Video output // @@ -458,81 +224,6 @@ namespace DO::Sara { throw std::runtime_error{"Could not copy the stream parameters!"}; } -#if 0 - /* Prepare a dummy image. */ - static void fill_yuv_image(AVFrame* pict, int frame_index, int width, - int height) - { - int x, y, i; - i = frame_index; - /* Y */ - for (y = 0; y < height; y++) - for (x = 0; x < width; x++) - pict->data[0][y * pict->linesize[0] + x] = x + y + i * 3; - /* Cb and Cr */ - for (y = 0; y < height / 2; y++) - { - for (x = 0; x < width / 2; x++) - { - pict->data[1][y * pict->linesize[1] + x] = 128 + y + i * 2; - pict->data[2][y * pict->linesize[2] + x] = 64 + x + i * 5; - } - } - } - - static AVFrame* get_video_frame(OutputStream* ostream) - { - AVCodecContext* c = ostream->encoding_context; - /* check if we want to generate more frames */ - if (av_compare_ts(ostream->next_pts, c->time_base, - static_cast(STREAM_DURATION), {1, 1}) > 0) - return nullptr; - /* when we pass a frame to the encoder, it may keep a reference to it - * internally; make sure we do not overwrite it here */ - if (av_frame_make_writable(ostream->frame) < 0) - throw std::runtime_error{"Could not make frame writable!"}; - - if (c->pix_fmt != AV_PIX_FMT_YUV420P) - { - /* as we only generate a YUV420P picture, we must convert it - * to the codec pixel format if needed */ - if (!ostream->sws_ctx) - { - ostream->sws_ctx = sws_getContext( - c->width, c->height, AV_PIX_FMT_YUV420P, c->width, c->height, - c->pix_fmt, SCALE_FLAGS, nullptr, nullptr, nullptr); - if (!ostream->sws_ctx) - throw std::runtime_error{ - "Could not initialize the conversion context"}; - } - fill_yuv_image(ostream->tmp_frame, static_cast(ostream->next_pts), - c->width, c->height); - sws_scale(ostream->sws_ctx, - (const uint8_t* const*) ostream->tmp_frame->data, - ostream->tmp_frame->linesize, 0, c->height, - ostream->frame->data, ostream->frame->linesize); - } - else - fill_yuv_image(ostream->frame, static_cast(ostream->next_pts), - c->width, c->height); - - ostream->frame->pts = ostream->next_pts++; - - return ostream->frame; - } - - /* - * encode one video frame and send it to the muxer - * return 1 when encoding is finished, 0 otherwise - */ - static int write_video_frame(AVFormatContext* format_context, - OutputStream* ostream) - { - return write_frame(format_context, ostream->encoding_context, - ostream->stream, get_video_frame(ostream)); - } -#endif - static void close_stream(AVFormatContext*, OutputStream* os) { avcodec_free_context(&os->encoding_context); @@ -575,20 +266,11 @@ namespace DO::Sara { _have_video = 1; _encode_video = 1; } - if (_output_format->audio_codec != AV_CODEC_ID_NONE) - { - add_stream(&_audio_stream, _format_context, &_audio_codec, - _output_format->audio_codec); - _have_audio = 1; - _encode_audio = 1; - } /* Now that all the parameters are set, we can open the audio and * video codecs and allocate the necessary encode buffers. */ if (_have_video) open_video(_format_context, _video_codec, &_video_stream, _options); - if (_have_audio) - open_audio(_format_context, _audio_codec, &_audio_stream, _options); av_dump_format(_format_context, 0, filepath.c_str(), 1); auto ret = int{}; @@ -681,13 +363,6 @@ namespace DO::Sara { _have_video = 0; _video_stream = {}; } - // Close the audio codec. - if (_have_audio) - { - close_stream(_format_context, &_audio_stream); - _have_audio = 0; - _audio_stream = {}; - } // Close the output file. if (_output_format) diff --git a/cpp/src/DO/Sara/VideoIO/VideoWriter.hpp b/cpp/src/DO/Sara/VideoIO/VideoWriter.hpp index 7a3621e35..12fa8331c 100644 --- a/cpp/src/DO/Sara/VideoIO/VideoWriter.hpp +++ b/cpp/src/DO/Sara/VideoIO/VideoWriter.hpp @@ -67,13 +67,10 @@ namespace DO::Sara { OutputStream _audio_stream; const AVOutputFormat* _output_format = nullptr; AVFormatContext* _format_context; - const AVCodec* _audio_codec = nullptr; const AVCodec* _video_codec = nullptr; AVDictionary* _options = nullptr; - int _have_audio = 0; int _have_video = 0; - int _encode_audio = 0; int _encode_video = 0; }; From e502b4ef0ba08e98b779905b521aa6ec7de39843 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 14 Apr 2024 20:48:36 +0100 Subject: [PATCH 32/49] WIP: add notes. --- cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp | 18 ++-- cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp | 85 ++++++++------- cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp | 7 +- .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 101 ++++++++++-------- 4 files changed, 114 insertions(+), 97 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp index 6d76ccdbe..9fddb0292 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureGraph.hpp @@ -40,20 +40,23 @@ namespace DO::Sara { struct MatchGID { //! @brief Index of the epipolar edge connecting camera i and camera j. - CameraPoseGraph::Vertex i; - CameraPoseGraph::Vertex j; + CameraPoseGraph::Vertex pose_src; + CameraPoseGraph::Vertex pose_dst; //! @brief Local match index. std::size_t index; auto operator==(const MatchGID& other) const -> bool { - return i == other.i && j == other.j && index == other.index; + return pose_src == other.pose_src && pose_dst == other.pose_dst && + index == other.index; } auto operator<(const MatchGID& other) const -> bool { - return (i < other.i) || (i == other.i && j < other.j) || - (i == other.i && j == other.j && index < other.index); + return (pose_src < other.pose_src) || + (pose_src == other.pose_src && pose_dst < other.pose_dst) || + (pose_src == other.pose_src && pose_dst == other.pose_dst && + index < other.index); } }; @@ -97,9 +100,8 @@ namespace DO::Sara { auto calculate_feature_tracks() const -> std::vector; auto filter_by_non_max_suppression(const Track&, const CameraPoseGraph&) const -> Track; - auto find_vertex_from_camera_view(const Track&, - const CameraPoseGraph::Vertex&) const - -> Vertex; + auto find_vertex_from_camera_view( + const Track&, const CameraPoseGraph::Vertex&) const -> Vertex; private: Impl _feature_graph; diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp index 497627f90..88095b0d3 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp @@ -19,75 +19,78 @@ using namespace DO::Sara; auto FeatureTracker::update_feature_tracks( - const CameraPoseGraph& camera_pose_graph, - const CameraPoseGraph::Edge relative_pose_edge) -> void + const CameraPoseGraph& pose_graph, + const CameraPoseGraph::Edge pose_edge) -> void { auto& logger = Logger::get(); - const CameraPoseGraph::Impl& cg = camera_pose_graph; + const CameraPoseGraph::Impl& pg = pose_graph; FeatureGraph::Impl& fg = _feature_graph; - // Retrieve the two camera vertices from the relative pose edge. - const auto pose_i = boost::source(relative_pose_edge, cg); - const auto pose_j = boost::target(relative_pose_edge, cg); + // Retrieve the camera poses from the relative pose edge. + const auto pose_u = boost::source(pose_edge, pg); + const auto pose_v = boost::target(pose_edge, pg); // The relative pose edge contains the set of all feature correspondences. - const auto& matches = cg[relative_pose_edge].matches; + const auto& matches = pg[pose_edge].matches; // Which of these feature correspondences are marked as inliers? - const auto& inliers = cg[relative_pose_edge].inliers; + const auto& inliers = pg[pose_edge].inliers; - // Loop over the feature correspondence and add the feature graph edges. - SARA_LOGD(logger, "Pose {} <-> Pose {}", pose_i, pose_j); + // Add the feature graph edges. + // + // They are feature matches that are deemed inliers according the relative + // pose estimation task. + SARA_LOGD(logger, "Pose {} <-> Pose {}", pose_u, pose_v); SARA_LOGD(logger, "Add feature correspondences..."); for (auto m = 0u; m < matches.size(); ++m) { if (!inliers(m)) continue; + // The feature match is 'm = (ix, iy)' + // where 'ix' and 'iy' are the local IDs of feature 'x' and 'y'. const auto& match = matches[m]; - // Local feature indices. - const auto& f1 = match.x_index(); - const auto& f2 = match.y_index(); - - // Create their corresponding feature GIDs. - const auto gid1 = FeatureGID{ - .pose_vertex = pose_i, // - .feature_index = f1 // + // 'x' and 'y' are respectively identified by their GID 'gid_x' and 'gid_y', + // which are defined as follows. + const auto gid_x = FeatureGID{ + .pose_vertex = pose_u, // + .feature_index = match.x_index() // }; - const auto gid2 = FeatureGID{ - .pose_vertex = pose_j, // - .feature_index = f2 // + const auto gid_y = FeatureGID{ + .pose_vertex = pose_v, // + .feature_index = match.y_index() // }; - // Locate their corresponding pair of vertices (u, v) in the graph? - // Do they exist yet in the first place? - const auto u_it = _feature_vertex.find(gid1); - const auto v_it = _feature_vertex.find(gid2); + // Are features 'x' and 'y' already added in the graph, i.e., + // are vertex 'gid_x' and 'gid_y' already added in the graph? + const auto it_x = _feature_vertex.find(gid_x); + const auto it_y = _feature_vertex.find(gid_y); - const auto u_does_not_exist_yet = u_it == _feature_vertex.end(); - const auto v_does_not_exist_yet = v_it == _feature_vertex.end(); + const auto x_does_not_exist_yet = it_x == _feature_vertex.end(); + const auto y_does_not_exist_yet = it_y == _feature_vertex.end(); // If not, add them if necessary. - const auto u = u_does_not_exist_yet ? boost::add_vertex(fg) : u_it->second; - const auto v = v_does_not_exist_yet ? boost::add_vertex(fg) : v_it->second; + const auto x = x_does_not_exist_yet ? boost::add_vertex(fg) : it_x->second; + const auto y = y_does_not_exist_yet ? boost::add_vertex(fg) : it_y->second; - if (u_does_not_exist_yet) + if (x_does_not_exist_yet) { - fg[u] = gid1; - _feature_vertex[gid1] = u; + fg[x] = gid_x; + _feature_vertex[gid_x] = x; } - if (v_does_not_exist_yet) + if (y_does_not_exist_yet) { - fg[v] = gid2; - _feature_vertex[gid2] = v; + fg[y] = gid_y; + _feature_vertex[gid_y] = y; } - // Finally, store the feature match as an edge in the feature graph. - const auto [uv, uv_added] = boost::add_edge(u, v, fg); - auto& uv_attrs = fg[uv]; - uv_attrs.i = boost::source(relative_pose_edge, cg); - uv_attrs.j = boost::target(relative_pose_edge, cg); - uv_attrs.index = m; + // Finally, store the feature match as an edge in the feature graph to + // navigate between the feature graph to the pose graph. + const auto [xy, xy_added] = boost::add_edge(x, y, fg); + auto& xy_attrs = fg[xy]; + xy_attrs.pose_src = boost::source(pose_edge, pg); + xy_attrs.pose_dst = boost::target(pose_edge, pg); + xy_attrs.index = m; } // Update the feature disjoint-sets diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp index 45323fb72..9ad4da9be 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp @@ -41,12 +41,11 @@ namespace DO::Sara { //! match index). std::map _feature_match; - auto update_feature_tracks( // - const CameraPoseGraph& camera_pose_graph, - const CameraPoseGraph::Edge relative_pose_edge_id) -> void; + auto update_feature_tracks(const CameraPoseGraph&, + const CameraPoseGraph::Edge) -> void; auto calculate_alive_feature_tracks( - const CameraPoseGraph::Vertex camera_vertex_curr) const + const CameraPoseGraph::Vertex last_pose_vertex) const -> std::tuple; }; diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index 92006bce8..b62cbd2db 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -120,6 +120,7 @@ auto v2::OdometryPipeline::add_camera_pose() -> bool const auto frame_number = _video_streamer.frame_number(); auto keys_curr = detect_keypoints(frame); + // Boundary case. if (_pose_graph.num_vertices() == 1) { // Initialize the new camera pose from the latest image frame. @@ -133,52 +134,64 @@ auto v2::OdometryPipeline::add_camera_pose() -> bool return true; } - else + + const auto& keys_prev = _pose_graph[_pose_prev].keypoints; + auto [rel_pose_data, two_view_geometry] = + estimate_relative_pose(keys_prev, keys_curr); + const auto num_inliers = rel_pose_data.inliers.flat_array().count(); + SARA_LOGI(logger, "[SfM] Relative pose inliers: {} 3D points", num_inliers); + if (num_inliers < _feature_params.num_inliers_min) { - const auto& keys_prev = _pose_graph[_pose_prev].keypoints; - auto [rel_pose_data, two_view_geometry] = - estimate_relative_pose(keys_prev, keys_curr); - const auto num_inliers = rel_pose_data.inliers.flat_array().count(); - SARA_LOGI(logger, "[SfM] Relative pose inliers: {} 3D points", num_inliers); - if (num_inliers < _feature_params.num_inliers_min) - { - SARA_LOGI(logger, "[SfM] Relative pose failed!"); - return false; - } - SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); - - if (_pose_graph.num_vertices() == 2) - { - auto abs_pose_curr = QuaternionBasedPose{ - .q = Eigen::Quaterniond{rel_pose_data.motion.R}, - .t = rel_pose_data.motion.t // - }; - - auto abs_pose_data = AbsolutePoseData{ - frame_number, // - std::move(keys_curr), // - std::move(abs_pose_curr) // - }; - - // 1. Add the absolute pose vertex. - _pose_graph.add_absolute_pose(std::move(abs_pose_data)); - - // 2. Add the pose edge, which will invalidate the relative pose data. - _pose_graph.add_relative_pose(_pose_prev, _pose_curr, - std::move(rel_pose_data)); - - // 3. TODO: Init point cloud - - return true; - } - else - { - // 1. Add the absolute pose vertex. - - // TODO: Grow point cloud by triangulation. - return false; - } + SARA_LOGI(logger, "[SfM] Relative pose failed!"); + return false; + } + SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); + + if (_pose_graph.num_vertices() == 2) + { + auto abs_pose_curr = QuaternionBasedPose{ + .q = Eigen::Quaterniond{rel_pose_data.motion.R}, + .t = rel_pose_data.motion.t // + }; + + auto abs_pose_data = AbsolutePoseData{ + frame_number, // + std::move(keys_curr), // + std::move(abs_pose_curr) // + }; + + // 1. Add the absolute pose vertex. + _pose_graph.add_absolute_pose(std::move(abs_pose_data)); + + // 2. Add the pose edge, which will invalidate the relative pose data. + const auto pose_edge = _pose_graph.add_relative_pose( + _pose_prev, _pose_curr, std::move(rel_pose_data)); + + // 3. Grow the feature graph by adding the feature matches. + _feature_tracker.update_feature_tracks(_pose_graph, pose_edge); + + // 4. TODO: Init point cloud + + // 5. TODO: don't add 3D scene points that are too far, like point in the + // sky + + return true; } + // 1. Grow the feature graph first by adding the feature matches that are + // deemed reliable from the relative pose estimation. + // 2. Recalculate the feature tracks. + // 3. Get the feature tracks that are still alive. + // 4. For each feature track still alive, get the corresponding scene + // points. + // Each alive feature track still has the same old feature IDs in the + // previous image frames, and we know their scene points. + // Use triangulation computer vision task, to calculate the new camera + // absolute pose. + // 5. With the camera absolute pose, add the new scene points. + // Specifically, they are the alive feature tracks (with cardinality 2) + // for which we don't know the scene points yet. + + // TODO: Grow point cloud by triangulation. return false; } From 3a5fb2078ebcbd79ccfe3fef214ae9f8acd29f2c Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Mon, 15 Apr 2024 01:03:31 +0100 Subject: [PATCH 33/49] WIP: save work. --- .../SfM/BuildingBlocks/BundleAdjuster.hpp | 11 + .../BuildingBlocks/PointCloudManipulator.cpp | 295 ++++++++++++++++++ .../BuildingBlocks/PointCloudManipulator.hpp | 71 ++++- .../SfM/BuildingBlocks/RgbColoredPoint.hpp | 12 +- 4 files changed, 372 insertions(+), 17 deletions(-) create mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.cpp diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.hpp index 2a0cdba0f..feeeeb003 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/BundleAdjuster.hpp @@ -1,3 +1,14 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + #pragma once #include diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.cpp new file mode 100644 index 000000000..c03a59629 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.cpp @@ -0,0 +1,295 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#include "DO/Sara/Core/Pixel/SmartColorConversion.hpp" +#include + +#include + +#include + + +using namespace DO::Sara; + + +auto PointCloudManipulator::list_scene_point_indices( + const FeatureTrack& track) const -> std::vector +{ + auto index_set = std::unordered_set{}; + for (const auto& v : track) + { + const auto scene_point_it = _from_vertex_to_scene_point_index.find(v); + if (scene_point_it != _from_vertex_to_scene_point_index.end()) + index_set.emplace(scene_point_it->second); + } + + const auto index_list = std::vector( // + index_set.begin(), index_set.end()); + + return index_list; +} + +auto PointCloudManipulator::filter_by_non_max_suppression( + const FeatureTrack& track) const -> FeatureTrack +{ + struct VertexScorePair + { + FeatureVertex vertex; + float score; + auto operator<(const VertexScorePair& other) const -> bool + { + return score < other.score; + } + }; + + auto filtered_set = std::unordered_map{}; + for (const auto& v : track) + { + const auto& f = feature(v); + const auto& pose_vertex = _feature_graph[v].pose_vertex; + const auto pose_vertex_it = filtered_set.find(pose_vertex); + if (pose_vertex_it == filtered_set.end()) + { + filtered_set[pose_vertex] = {.vertex = v, .score = f.extremum_value}; + continue; + } + + auto& vertex_score = pose_vertex_it->second; + if (vertex_score.score < f.extremum_value) + vertex_score = {.vertex = v, .score = f.extremum_value}; + } + + auto filtered_list = FeatureTrack(filtered_set.size()); + std::transform(filtered_set.begin(), filtered_set.end(), + filtered_list.begin(), + [](const auto& v) { return v.second.vertex; }); + + // Order feature vertices in a chronological order. + // + // The camera vertex ID is incremented as time goes on and can be seen as a + // timestep. + std::sort(filtered_list.begin(), filtered_list.end(), + [this](const auto u, const auto v) { + return _feature_graph[u].pose_vertex < + _feature_graph[v].pose_vertex; + }); + + return filtered_list; +} + +auto PointCloudManipulator::find_feature_vertex_at_pose( + const FeatureTrack& track, + const PoseVertex pose_vertex) const -> std::optional +{ + auto v = std::find_if(track.begin(), track.end(), + [this, pose_vertex](const auto& v) { + return this->gid(v).pose_vertex == pose_vertex; + }); + return v == track.end() ? std::nullopt : std::make_optional(*v); +} + + +auto PointCloudManipulator::barycenter( + const std::vector& scene_point_indices) const -> ScenePoint +{ + if (scene_point_indices.empty()) + throw std::runtime_error{"Error: cannot calculate a barycentric scene " + "point from an empty list of scene point indices"}; + static const ScenePoint::Value zero = ScenePoint::Value::Zero(); + auto bary = std::accumulate( // + scene_point_indices.begin(), scene_point_indices.end(), zero, + [this](const ScenePoint::Value& a, + const ScenePointIndex bi) -> ScenePoint::Value { + const ScenePoint::Value& b = _point_cloud[bi]; + return a + b; + }); + bary /= scene_point_indices.size(); + + auto scene_point = ScenePoint{}; + static_cast(scene_point) = bary; + return scene_point; +} + +auto PointCloudManipulator::split_by_scene_point_knowledge( + const std::vector& tracks) const + -> std::pair, std::vector> +{ + auto& logger = Logger::get(); + + auto tracks_with_known_scene_point = std::vector{}; + auto tracks_with_unknown_scene_point = std::vector{}; + tracks_with_known_scene_point.reserve(tracks.size()); + tracks_with_unknown_scene_point.reserve(tracks.size()); + + SARA_LOGD(logger, "Splitting feature tracks by knowledge of scene point..."); + + for (const auto& track : tracks) + { + const auto scene_point_indices = list_scene_point_indices(track); + if (scene_point_indices.empty()) + tracks_with_unknown_scene_point.emplace_back(track); + else + tracks_with_known_scene_point.emplace_back(track); + } + + SARA_LOGD(logger, "Tracks: {}", tracks.size()); + SARA_LOGD(logger, "Tracks with known scene point: {}", + tracks_with_known_scene_point.size()); + SARA_LOGD(logger, "Tracks with unknown scene point: {}", + tracks_with_unknown_scene_point.size()); + + return std::make_pair(tracks_with_known_scene_point, + tracks_with_unknown_scene_point); +} + +auto PointCloudManipulator::retrieve_scene_point_color( + const Eigen::Vector3d& scene_point, // + const ImageView& image, // + const QuaternionBasedPose& pose, + const v2::BrownConradyDistortionModel& camera) const -> Rgb64f +{ + const auto& w = image.width(); + const auto& h = image.height(); + + // Its coordinates in the camera frame. + const auto camera_point = pose * scene_point; + + // Its corresponding pixel coordinates in the image. + const Eigen::Vector2i u = camera + .project(camera_point) // + .array() + .round() + .cast(); + + // Clamp for safety + // TODO: do bilinear interpolation. + const auto x = std::clamp(u.x(), 0, w - 1); + const auto y = std::clamp(u.y(), 0, h - 1); + + // N.B.: the image is an array of BGR values. + const auto& rgb8 = image(x, y); + // We store RGB values. + static constexpr auto normalization_factor = 1 / 255.; + const Rgb64f rgb64f = rgb8.cast() * normalization_factor; + + return rgb64f; +} + + +auto PointCloudManipulator::init_point_cloud( + const std::vector& feature_tracks, + const ImageView& image, // + const PoseEdge pose_edge, + const v2::BrownConradyDistortionModel& camera) -> void +{ + auto& logger = Logger::get(); + + SARA_LOGD(logger, "Transform feature tracks into best feature pairs..."); + const auto& pose_u = boost::source(pose_edge, _pose_graph); + const auto& pose_v = boost::target(pose_edge, _pose_graph); + const auto& pose_data_u = _camera_pose_graph[pose_u].pose; + const auto& pose_data_v = _camera_pose_graph[pose_v].pose; + SARA_LOGD(logger, "Pose[from]:\n{}", pose_from.matrix34()); + SARA_LOGD(logger, "Pose[to ]:\n{}", pose_to.matrix34()); + +#if 0 + const auto num_feature_tracks = + static_cast(feature_tracks.size()); + + using FeatureVertexPair = std::array; + auto best_feature_pairs = std::vector(num_feature_tracks); + std::transform( + feature_tracks.begin(), feature_tracks.end(), best_feature_pairs.begin(), + [this, camera_from, camera_to](const auto& track) -> FeatureVertexPair { + // Non-max suppression. + const auto track_filtered = filter_by_non_max_suppression(track); + if (track_filtered.size() != 2) + throw std::runtime_error{"Error: the feature track filtered by NMS " + "is not a feature pair!"}; + + // Retrieve the cleaned up feature correspondence. + const auto v_from = find_vertex_at_camera_view(track_filtered, // + camera_from); + const auto v_to = find_vertex_at_camera_view(track_filtered, // + camera_to); + if (!v_from.has_value() || !v_to.has_value()) + throw std::runtime_error{ + "Error: the feature pair is not a valid feature correspondence!"}; + + return {*v_from, *v_to}; + }); + + SARA_LOGD(logger, "Calculating ray pairs from feature pairs..."); + auto rays_from = Eigen::MatrixXd{3, num_feature_tracks}; + auto rays_to = Eigen::MatrixXd{3, num_feature_tracks}; + for (auto t = 0u; t < num_feature_tracks; ++t) + { + const auto& feature_pair = best_feature_pairs[t]; + const auto coords = std::array{pixel_coords(feature_pair[0]), + pixel_coords(feature_pair[1])}; + rays_from.col(t) = camera.backproject(coords[0]); + rays_to.col(t) = camera.backproject(coords[1]); + } + + // Calculate the associated triangulation. + SARA_LOGD(logger, "Initialization the point cloud by 3D triangulation from " + "the relative pose..."); + const auto& relative_pose_attr = _pose_graph[pose_edge]; + if (!relative_pose_attr.relative_pose.has_value()) + throw std::runtime_error{ + "Error: tried triangulating but there is no relative pose!"}; + const auto& motion = *relative_pose_attr.relative_pose; + if ((pose_to.matrix34() - motion.projection_matrix()).norm() > 1e-6) + throw std::runtime_error{ + "Error: the absolute pose is not initialized from the relative pose!"}; + + const auto triangulation = triangulate_linear_eigen( // + pose_from.matrix34(), pose_to.matrix34(), // + rays_from, rays_to); + + // Allocate the mapping from the feature vertices to the scene point index. + if (!_from_vertex_to_scene_point_index.empty()) + _from_vertex_to_scene_point_index.clear(); + + // Calculate the initial point cloud. + if (!_point_cloud.empty()) + _point_cloud.clear(); + + const auto& X = triangulation.X; + const auto& scales_from = triangulation.scales[0]; + const auto& scales_to = triangulation.scales[1]; + + auto scene_point_index = scene_point_index_t{}; + for (auto j = 0; j < X.cols(); ++j) + { + // Only consider **cheiral** inliers! + if (!(scales_from(j) > 0 && scales_to(j) > 0)) + continue; + + const Eigen::Vector3d scene_coords = X.col(j).hnormalized(); + const auto rgb = retrieve_scene_point_color(scene_coords, image, // + pose_to, camera); + const auto colored_point = (ColoredPoint{} << scene_coords, rgb).finished(); + + // Add a new point to the point cloud. + _point_cloud.emplace_back(colored_point); + + const auto& [u, v] = best_feature_pairs[j]; + + // Assign a scene point index. + _from_vertex_to_scene_point_index[u] = scene_point_index; + _from_vertex_to_scene_point_index[v] = scene_point_index; + ++scene_point_index; + } + + SARA_LOGD(logger, "point cloud: {} 3D points", _point_cloud.size()); +#endif +} diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp index 139dece09..d73575256 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp @@ -1,8 +1,19 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + #pragma once #include #include -#include +#include namespace DO::Sara { @@ -10,32 +21,70 @@ namespace DO::Sara { class PointCloudManipulator { public: - using scene_point_index_t = std::size_t; + using PoseVertex = CameraPoseGraph::Vertex; + using PoseEdge = CameraPoseGraph::Edge; + using FeatureVertex = FeatureGraph::Vertex; + using ScenePointIndex = std::size_t; + using ScenePoint = RgbColoredPoint; + using PointCloud = std::vector>; + using FeatureTrack = FeatureTracker::Track; PointCloudManipulator(const CameraPoseGraph& camera_pose_graph, const FeatureGraph& feature_graph, PointCloud& point_cloud) - : _camera_pose_graph{camera_pose_graph} + : _pose_graph{camera_pose_graph} , _feature_graph{feature_graph} , _point_cloud{point_cloud} { } - auto grow_point_cloud(const std::vector&, - const CameraPoseGraph::Edge&, // - const ImageView&) -> void; + auto list_scene_point_indices(const FeatureTrack&) const + -> std::vector; + + auto filter_by_non_max_suppression(const FeatureTrack&) const // + -> FeatureTrack; + + auto split_by_scene_point_knowledge(const std::vector&) const + -> std::pair, std::vector>; + + auto + init_point_cloud(const std::vector&, // + const ImageView&, // + const PoseEdge, + const v2::BrownConradyDistortionModel&) -> void; + + public: /* utility methods */ + auto gid(const FeatureVertex u) const -> const FeatureGID& + { + return _feature_graph[u]; + } + + auto feature(const FeatureVertex u) const -> const OERegion& + { + const auto& [pose_vertex, feature_index] = gid(u); + const auto& f = features(_pose_graph[pose_vertex].keypoints); + return f[feature_index]; + } + + auto barycenter(const std::vector&) const -> ScenePoint; + + auto find_feature_vertex_at_pose(const FeatureTrack&, // + const PoseVertex) const + -> std::optional; - auto compact_point_cloud(const std::vector&, - const CameraPoseGraph::Edge&, - const ImageView&) -> void; + auto retrieve_scene_point_color( + const Eigen::Vector3d& scene_point, // + const ImageView& image, // + const QuaternionBasedPose& pose, + const v2::BrownConradyDistortionModel& camera) const -> Rgb64f; private: - const CameraPoseGraph& _camera_pose_graph; + const CameraPoseGraph& _pose_graph; const FeatureGraph& _feature_graph; PointCloud& _point_cloud; - std::unordered_map + std::unordered_map _from_vertex_to_scene_point_index; }; diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp index b393fe16f..8da8cecac 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp @@ -12,32 +12,32 @@ namespace DO::Sara { using Coords = Eigen::Vector; using Color = Eigen::Vector; - operator Value&() + inline operator Value&() { return value; } - operator const Value&() const + inline operator const Value&() const { return value; } - auto coords() -> Eigen::Map + inline auto coords() -> Eigen::Map { return Eigen::Map>{value.data()}; } - auto coords() const -> Eigen::Map + inline auto coords() const -> Eigen::Map { return Eigen::Map{value.data()}; } - auto color() -> Eigen::Map + inline auto color() -> Eigen::Map { return Eigen::Map{value.data() + 3}; } - auto color() const -> Eigen::Map + inline auto color() const -> Eigen::Map { return Eigen::Map{value.data() + 3}; } From f6e8fb8baf428c81b739b9da2d9fd01ebcc6cf56 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Mon, 15 Apr 2024 13:12:34 +0100 Subject: [PATCH 34/49] WIP: pursue work on point cloud generation. --- .../Geometry/PinholeCamera.hpp | 6 +- .../BuildingBlocks/PointCloudGenerator.cpp | 97 ++++++ ...anipulator.hpp => PointCloudGenerator.hpp} | 8 +- .../BuildingBlocks/PointCloudManipulator.cpp | 295 ------------------ .../SfM/BuildingBlocks/RgbColoredPoint.hpp | 24 +- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp | 11 + 6 files changed, 131 insertions(+), 310 deletions(-) create mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp rename cpp/src/DO/Sara/SfM/BuildingBlocks/{PointCloudManipulator.hpp => PointCloudGenerator.hpp} (93%) delete mode 100644 cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.cpp diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp index 9ccc2b426..848013e01 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp @@ -37,9 +37,9 @@ namespace DO::Sara { return K * Rt; } - Eigen::Matrix3d K{Eigen::Matrix3d::Identity()}; - Eigen::Matrix3d R{Eigen::Matrix3d::Identity()}; - Eigen::Vector3d t{Eigen::Vector3d::Zero()}; + Eigen::Matrix3d K = Eigen::Matrix3d::Identity(); + Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); + Eigen::Vector3d t = Eigen::Vector3d::Zero(); }; inline auto normalized_camera(const Eigen::Matrix3d& R, diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp new file mode 100644 index 000000000..61d52ed06 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -0,0 +1,97 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#pragma once + +#include +#include +#include + + +namespace DO::Sara { + + class PointCloudGenerator + { + public: + using PoseVertex = CameraPoseGraph::Vertex; + using PoseEdge = CameraPoseGraph::Edge; + using FeatureVertex = FeatureGraph::Vertex; + using ScenePointIndex = std::size_t; + using ScenePoint = RgbColoredPoint; + + using PointCloud = std::vector>; + using FeatureTrack = FeatureTracker::Track; + + PointCloudGenerator(const CameraPoseGraph& camera_pose_graph, + const FeatureGraph& feature_graph, + PointCloud& point_cloud) + : _pose_graph{camera_pose_graph} + , _feature_graph{feature_graph} + , _point_cloud{point_cloud} + { + } + + auto list_scene_point_indices(const FeatureTrack&) const + -> std::vector; + + auto filter_by_non_max_suppression(const FeatureTrack&) const // + -> FeatureTrack; + + auto split_by_scene_point_knowledge(const std::vector&) const + -> std::pair, std::vector>; + + auto seed_point_cloud_from_two_views( + const std::vector&, // + const ImageView&, // + const PoseEdge, // + const v2::BrownConradyDistortionModel&) -> void; + + public: /* utility methods */ + auto gid(const FeatureVertex u) const -> const FeatureGID& + { + return _feature_graph[u]; + } + + auto feature(const FeatureVertex u) const -> const OERegion& + { + const auto& [pose_vertex, feature_index] = gid(u); + const auto& f = features(_pose_graph[pose_vertex].keypoints); + return f[feature_index]; + } + + auto pixel_coords(const FeatureVertex u) const -> const Eigen::Vector2f& + { + return feature(u).center(); + } + + auto barycenter(const std::vector&) const -> ScenePoint; + + auto find_feature_vertex_at_pose(const FeatureTrack&, // + const PoseVertex) const + -> std::optional; + + auto retrieve_scene_point_color( + const ScenePoint::Coords& scene_point_coords, // + const ImageView& image, // + const QuaternionBasedPose& absolute_pose, // + const v2::BrownConradyDistortionModel& camera) const + -> ScenePoint::Color; + + private: + const CameraPoseGraph& _pose_graph; + const FeatureGraph& _feature_graph; + PointCloud& _point_cloud; + + std::unordered_map + _from_vertex_to_scene_point_index; + }; + +} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp similarity index 93% rename from cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp rename to cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index d73575256..2ce69a063 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -18,7 +18,7 @@ namespace DO::Sara { - class PointCloudManipulator + class PointCloudGenerator { public: using PoseVertex = CameraPoseGraph::Vertex; @@ -30,9 +30,9 @@ namespace DO::Sara { using PointCloud = std::vector>; using FeatureTrack = FeatureTracker::Track; - PointCloudManipulator(const CameraPoseGraph& camera_pose_graph, - const FeatureGraph& feature_graph, - PointCloud& point_cloud) + PointCloudGenerator(const CameraPoseGraph& camera_pose_graph, + const FeatureGraph& feature_graph, + PointCloud& point_cloud) : _pose_graph{camera_pose_graph} , _feature_graph{feature_graph} , _point_cloud{point_cloud} diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.cpp deleted file mode 100644 index c03a59629..000000000 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudManipulator.cpp +++ /dev/null @@ -1,295 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2024-present David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#include "DO/Sara/Core/Pixel/SmartColorConversion.hpp" -#include - -#include - -#include - - -using namespace DO::Sara; - - -auto PointCloudManipulator::list_scene_point_indices( - const FeatureTrack& track) const -> std::vector -{ - auto index_set = std::unordered_set{}; - for (const auto& v : track) - { - const auto scene_point_it = _from_vertex_to_scene_point_index.find(v); - if (scene_point_it != _from_vertex_to_scene_point_index.end()) - index_set.emplace(scene_point_it->second); - } - - const auto index_list = std::vector( // - index_set.begin(), index_set.end()); - - return index_list; -} - -auto PointCloudManipulator::filter_by_non_max_suppression( - const FeatureTrack& track) const -> FeatureTrack -{ - struct VertexScorePair - { - FeatureVertex vertex; - float score; - auto operator<(const VertexScorePair& other) const -> bool - { - return score < other.score; - } - }; - - auto filtered_set = std::unordered_map{}; - for (const auto& v : track) - { - const auto& f = feature(v); - const auto& pose_vertex = _feature_graph[v].pose_vertex; - const auto pose_vertex_it = filtered_set.find(pose_vertex); - if (pose_vertex_it == filtered_set.end()) - { - filtered_set[pose_vertex] = {.vertex = v, .score = f.extremum_value}; - continue; - } - - auto& vertex_score = pose_vertex_it->second; - if (vertex_score.score < f.extremum_value) - vertex_score = {.vertex = v, .score = f.extremum_value}; - } - - auto filtered_list = FeatureTrack(filtered_set.size()); - std::transform(filtered_set.begin(), filtered_set.end(), - filtered_list.begin(), - [](const auto& v) { return v.second.vertex; }); - - // Order feature vertices in a chronological order. - // - // The camera vertex ID is incremented as time goes on and can be seen as a - // timestep. - std::sort(filtered_list.begin(), filtered_list.end(), - [this](const auto u, const auto v) { - return _feature_graph[u].pose_vertex < - _feature_graph[v].pose_vertex; - }); - - return filtered_list; -} - -auto PointCloudManipulator::find_feature_vertex_at_pose( - const FeatureTrack& track, - const PoseVertex pose_vertex) const -> std::optional -{ - auto v = std::find_if(track.begin(), track.end(), - [this, pose_vertex](const auto& v) { - return this->gid(v).pose_vertex == pose_vertex; - }); - return v == track.end() ? std::nullopt : std::make_optional(*v); -} - - -auto PointCloudManipulator::barycenter( - const std::vector& scene_point_indices) const -> ScenePoint -{ - if (scene_point_indices.empty()) - throw std::runtime_error{"Error: cannot calculate a barycentric scene " - "point from an empty list of scene point indices"}; - static const ScenePoint::Value zero = ScenePoint::Value::Zero(); - auto bary = std::accumulate( // - scene_point_indices.begin(), scene_point_indices.end(), zero, - [this](const ScenePoint::Value& a, - const ScenePointIndex bi) -> ScenePoint::Value { - const ScenePoint::Value& b = _point_cloud[bi]; - return a + b; - }); - bary /= scene_point_indices.size(); - - auto scene_point = ScenePoint{}; - static_cast(scene_point) = bary; - return scene_point; -} - -auto PointCloudManipulator::split_by_scene_point_knowledge( - const std::vector& tracks) const - -> std::pair, std::vector> -{ - auto& logger = Logger::get(); - - auto tracks_with_known_scene_point = std::vector{}; - auto tracks_with_unknown_scene_point = std::vector{}; - tracks_with_known_scene_point.reserve(tracks.size()); - tracks_with_unknown_scene_point.reserve(tracks.size()); - - SARA_LOGD(logger, "Splitting feature tracks by knowledge of scene point..."); - - for (const auto& track : tracks) - { - const auto scene_point_indices = list_scene_point_indices(track); - if (scene_point_indices.empty()) - tracks_with_unknown_scene_point.emplace_back(track); - else - tracks_with_known_scene_point.emplace_back(track); - } - - SARA_LOGD(logger, "Tracks: {}", tracks.size()); - SARA_LOGD(logger, "Tracks with known scene point: {}", - tracks_with_known_scene_point.size()); - SARA_LOGD(logger, "Tracks with unknown scene point: {}", - tracks_with_unknown_scene_point.size()); - - return std::make_pair(tracks_with_known_scene_point, - tracks_with_unknown_scene_point); -} - -auto PointCloudManipulator::retrieve_scene_point_color( - const Eigen::Vector3d& scene_point, // - const ImageView& image, // - const QuaternionBasedPose& pose, - const v2::BrownConradyDistortionModel& camera) const -> Rgb64f -{ - const auto& w = image.width(); - const auto& h = image.height(); - - // Its coordinates in the camera frame. - const auto camera_point = pose * scene_point; - - // Its corresponding pixel coordinates in the image. - const Eigen::Vector2i u = camera - .project(camera_point) // - .array() - .round() - .cast(); - - // Clamp for safety - // TODO: do bilinear interpolation. - const auto x = std::clamp(u.x(), 0, w - 1); - const auto y = std::clamp(u.y(), 0, h - 1); - - // N.B.: the image is an array of BGR values. - const auto& rgb8 = image(x, y); - // We store RGB values. - static constexpr auto normalization_factor = 1 / 255.; - const Rgb64f rgb64f = rgb8.cast() * normalization_factor; - - return rgb64f; -} - - -auto PointCloudManipulator::init_point_cloud( - const std::vector& feature_tracks, - const ImageView& image, // - const PoseEdge pose_edge, - const v2::BrownConradyDistortionModel& camera) -> void -{ - auto& logger = Logger::get(); - - SARA_LOGD(logger, "Transform feature tracks into best feature pairs..."); - const auto& pose_u = boost::source(pose_edge, _pose_graph); - const auto& pose_v = boost::target(pose_edge, _pose_graph); - const auto& pose_data_u = _camera_pose_graph[pose_u].pose; - const auto& pose_data_v = _camera_pose_graph[pose_v].pose; - SARA_LOGD(logger, "Pose[from]:\n{}", pose_from.matrix34()); - SARA_LOGD(logger, "Pose[to ]:\n{}", pose_to.matrix34()); - -#if 0 - const auto num_feature_tracks = - static_cast(feature_tracks.size()); - - using FeatureVertexPair = std::array; - auto best_feature_pairs = std::vector(num_feature_tracks); - std::transform( - feature_tracks.begin(), feature_tracks.end(), best_feature_pairs.begin(), - [this, camera_from, camera_to](const auto& track) -> FeatureVertexPair { - // Non-max suppression. - const auto track_filtered = filter_by_non_max_suppression(track); - if (track_filtered.size() != 2) - throw std::runtime_error{"Error: the feature track filtered by NMS " - "is not a feature pair!"}; - - // Retrieve the cleaned up feature correspondence. - const auto v_from = find_vertex_at_camera_view(track_filtered, // - camera_from); - const auto v_to = find_vertex_at_camera_view(track_filtered, // - camera_to); - if (!v_from.has_value() || !v_to.has_value()) - throw std::runtime_error{ - "Error: the feature pair is not a valid feature correspondence!"}; - - return {*v_from, *v_to}; - }); - - SARA_LOGD(logger, "Calculating ray pairs from feature pairs..."); - auto rays_from = Eigen::MatrixXd{3, num_feature_tracks}; - auto rays_to = Eigen::MatrixXd{3, num_feature_tracks}; - for (auto t = 0u; t < num_feature_tracks; ++t) - { - const auto& feature_pair = best_feature_pairs[t]; - const auto coords = std::array{pixel_coords(feature_pair[0]), - pixel_coords(feature_pair[1])}; - rays_from.col(t) = camera.backproject(coords[0]); - rays_to.col(t) = camera.backproject(coords[1]); - } - - // Calculate the associated triangulation. - SARA_LOGD(logger, "Initialization the point cloud by 3D triangulation from " - "the relative pose..."); - const auto& relative_pose_attr = _pose_graph[pose_edge]; - if (!relative_pose_attr.relative_pose.has_value()) - throw std::runtime_error{ - "Error: tried triangulating but there is no relative pose!"}; - const auto& motion = *relative_pose_attr.relative_pose; - if ((pose_to.matrix34() - motion.projection_matrix()).norm() > 1e-6) - throw std::runtime_error{ - "Error: the absolute pose is not initialized from the relative pose!"}; - - const auto triangulation = triangulate_linear_eigen( // - pose_from.matrix34(), pose_to.matrix34(), // - rays_from, rays_to); - - // Allocate the mapping from the feature vertices to the scene point index. - if (!_from_vertex_to_scene_point_index.empty()) - _from_vertex_to_scene_point_index.clear(); - - // Calculate the initial point cloud. - if (!_point_cloud.empty()) - _point_cloud.clear(); - - const auto& X = triangulation.X; - const auto& scales_from = triangulation.scales[0]; - const auto& scales_to = triangulation.scales[1]; - - auto scene_point_index = scene_point_index_t{}; - for (auto j = 0; j < X.cols(); ++j) - { - // Only consider **cheiral** inliers! - if (!(scales_from(j) > 0 && scales_to(j) > 0)) - continue; - - const Eigen::Vector3d scene_coords = X.col(j).hnormalized(); - const auto rgb = retrieve_scene_point_color(scene_coords, image, // - pose_to, camera); - const auto colored_point = (ColoredPoint{} << scene_coords, rgb).finished(); - - // Add a new point to the point cloud. - _point_cloud.emplace_back(colored_point); - - const auto& [u, v] = best_feature_pairs[j]; - - // Assign a scene point index. - _from_vertex_to_scene_point_index[u] = scene_point_index; - _from_vertex_to_scene_point_index[v] = scene_point_index; - ++scene_point_index; - } - - SARA_LOGD(logger, "point cloud: {} 3D points", _point_cloud.size()); -#endif -} diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp index 8da8cecac..8e94875ab 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp @@ -6,44 +6,52 @@ namespace DO::Sara { template - struct RgbColoredPoint + class RgbColoredPoint { + public: using Value = Eigen::Vector; using Coords = Eigen::Vector; using Color = Eigen::Vector; + RgbColoredPoint() = default; + + RgbColoredPoint(const Value& v) + : _v{v} + { + } + inline operator Value&() { - return value; + return _v; } inline operator const Value&() const { - return value; + return _v; } inline auto coords() -> Eigen::Map { - return Eigen::Map>{value.data()}; + return Eigen::Map>{_v.data()}; } inline auto coords() const -> Eigen::Map { - return Eigen::Map{value.data()}; + return Eigen::Map{_v.data()}; } inline auto color() -> Eigen::Map { - return Eigen::Map{value.data() + 3}; + return Eigen::Map{_v.data() + 3}; } inline auto color() const -> Eigen::Map { - return Eigen::Map{value.data() + 3}; + return Eigen::Map{_v.data() + 3}; } private: - Value value; + Value _v; }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp index 5ba8584ce..982c5b0cb 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.hpp @@ -99,6 +99,17 @@ namespace DO::Sara { return _g[e]; } + auto source(const Edge e) const -> Vertex + { + return boost::source(e, _g); + } + + auto target(const Edge e) const -> Vertex + { + return boost::target(e, _g); + } + + auto edge(const Vertex u, const Vertex v) const -> std::pair { return boost::edge(u, v, _g); From 9a8ad445f714295e762c42109e3634c2cd603e88 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 15 Apr 2024 16:01:01 +0100 Subject: [PATCH 35/49] WIP: save work. --- .../visual_odometry_example_v2.cpp | 2 +- .../BuildingBlocks/PointCloudGenerator.hpp | 15 +++++------ .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 16 ++++++++---- .../Sara/SfM/OdometryV2/OdometryPipeline.hpp | 25 +++++++++++-------- 4 files changed, 35 insertions(+), 23 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp index 1c7f50309..4d4d24aa0 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp @@ -359,7 +359,7 @@ auto main(int const argc, char** const argv) -> int // clang-format off camera.k() << -0.2338367557617234, - 0.05952465745165465, + +0.05952465745165465, -0.007947847982157091; // clang-format on camera.p() << -0.0003137658969742134, 0.00021943576376532096; diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index 2ce69a063..910006d0c 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -29,6 +29,8 @@ namespace DO::Sara { using PointCloud = std::vector>; using FeatureTrack = FeatureTracker::Track; + using FeatureToScenePointMap = std::unordered_map; PointCloudGenerator(const CameraPoseGraph& camera_pose_graph, const FeatureGraph& feature_graph, @@ -48,11 +50,11 @@ namespace DO::Sara { auto split_by_scene_point_knowledge(const std::vector&) const -> std::pair, std::vector>; - auto - init_point_cloud(const std::vector&, // - const ImageView&, // - const PoseEdge, - const v2::BrownConradyDistortionModel&) -> void; + auto init_point_cloud(const std::vector&, // + const ImageView&, // + const PoseEdge, + const v2::BrownConradyDistortionModel&) + -> void; public: /* utility methods */ auto gid(const FeatureVertex u) const -> const FeatureGID& @@ -84,8 +86,7 @@ namespace DO::Sara { const FeatureGraph& _feature_graph; PointCloud& _point_cloud; - std::unordered_map - _from_vertex_to_scene_point_index; + FeatureToScenePointMap _from_vertex_to_scene_point_index; }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index b62cbd2db..5cc32068a 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -38,6 +38,8 @@ auto v2::OdometryPipeline::set_config( _camera // ); _relative_pose_estimator.configure(_camera); + _point_cloud_generator = std::make_unique( + _pose_graph, _feature_tracker._feature_graph, _point_cloud); } auto v2::OdometryPipeline::read() -> bool @@ -64,8 +66,10 @@ auto v2::OdometryPipeline::detect_keypoints(const ImageView& image) const -> KeypointList { auto& logger = Logger::get(); - SARA_LOGI(logger, "[Feature Detection] Matching image keypoints..."); - return compute_sift_keypoints(image, _feature_params.image_pyr_params); + const auto keys = compute_sift_keypoints(image, // + _feature_params.image_pyr_params); + SARA_LOGI(logger, "[Feature Detection] {} keypoints", features(keys).size()); + return keys; } auto v2::OdometryPipeline::estimate_relative_pose( @@ -120,8 +124,8 @@ auto v2::OdometryPipeline::add_camera_pose() -> bool const auto frame_number = _video_streamer.frame_number(); auto keys_curr = detect_keypoints(frame); - // Boundary case. - if (_pose_graph.num_vertices() == 1) + // Boundary case: the graphs are empty. + if (_pose_graph.num_vertices() == 0) { // Initialize the new camera pose from the latest image frame. auto abs_pose_curr = QuaternionBasedPose::identity(); @@ -147,7 +151,7 @@ auto v2::OdometryPipeline::add_camera_pose() -> bool } SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); - if (_pose_graph.num_vertices() == 2) + if (_pose_graph.num_vertices() == 1) { auto abs_pose_curr = QuaternionBasedPose{ .q = Eigen::Quaterniond{rel_pose_data.motion.R}, @@ -169,6 +173,8 @@ auto v2::OdometryPipeline::add_camera_pose() -> bool // 3. Grow the feature graph by adding the feature matches. _feature_tracker.update_feature_tracks(_pose_graph, pose_edge); + std::tie(_tracks_alive, _track_visibility_count) = + _feature_tracker.calculate_alive_feature_tracks(_pose_curr); // 4. TODO: Init point cloud diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp index acead5370..90c6a2ab3 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp @@ -12,21 +12,21 @@ #pragma once #include -#include -#include - +#include #include #include #include +#include +#include namespace DO::Sara::v2 { class OdometryPipeline { public: - auto - set_config(const std::filesystem::path& video_path, - const v2::BrownConradyDistortionModel& camera) -> void; + auto set_config(const std::filesystem::path& video_path, + const v2::BrownConradyDistortionModel& camera) + -> void; auto read() -> bool; @@ -41,9 +41,10 @@ namespace DO::Sara::v2 { auto detect_keypoints(const ImageView&) const -> KeypointList; - auto estimate_relative_pose(const KeypointList& keys_src, - const KeypointList& keys_dst) - const -> std::pair; + auto + estimate_relative_pose(const KeypointList& keys_src, + const KeypointList& keys_dst) const + -> std::pair; private: /* graph update tasks */ auto add_camera_pose() -> bool; @@ -52,15 +53,19 @@ namespace DO::Sara::v2 { VideoStreamer _video_streamer; v2::BrownConradyDistortionModel _camera; + //! @brief Data mutators. + //! @{ std::unique_ptr _distortion_corrector; v2::RelativePoseEstimator _relative_pose_estimator; - + std::unique_ptr _point_cloud_generator; + //! @} //! @brief SfM data. //! @{ FeatureParams _feature_params; FeatureTracker _feature_tracker; CameraPoseGraph _pose_graph; + PointCloudGenerator::PointCloud _point_cloud; //! @} //! @brief SfM state. From ca520e5efd89840efb41a7920573ddb7451480db Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 15 Apr 2024 17:10:15 +0100 Subject: [PATCH 36/49] WIP: salvage lost work. --- .../BuildingBlocks/PointCloudGenerator.cpp | 326 ++++++++++++++---- .../BuildingBlocks/PointCloudGenerator.hpp | 7 +- 2 files changed, 266 insertions(+), 67 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index 61d52ed06..836ebe003 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -9,89 +9,283 @@ // you can obtain one at http://mozilla.org/MPL/2.0/. // ========================================================================== // -#pragma once +#include -#include -#include -#include +#include -namespace DO::Sara { +using namespace DO::Sara; - class PointCloudGenerator - { - public: - using PoseVertex = CameraPoseGraph::Vertex; - using PoseEdge = CameraPoseGraph::Edge; - using FeatureVertex = FeatureGraph::Vertex; - using ScenePointIndex = std::size_t; - using ScenePoint = RgbColoredPoint; - - using PointCloud = std::vector>; - using FeatureTrack = FeatureTracker::Track; - - PointCloudGenerator(const CameraPoseGraph& camera_pose_graph, - const FeatureGraph& feature_graph, - PointCloud& point_cloud) - : _pose_graph{camera_pose_graph} - , _feature_graph{feature_graph} - , _point_cloud{point_cloud} - { - } - auto list_scene_point_indices(const FeatureTrack&) const - -> std::vector; - - auto filter_by_non_max_suppression(const FeatureTrack&) const // - -> FeatureTrack; +auto PointCloudGenerator::list_scene_point_indices( + const FeatureTrack& track) const -> std::vector +{ + auto index_set = std::unordered_set{}; + for (const auto& v : track) + { + const auto scene_point_it = _from_vertex_to_scene_point_index.find(v); + if (scene_point_it != _from_vertex_to_scene_point_index.end()) + index_set.emplace(scene_point_it->second); + } - auto split_by_scene_point_knowledge(const std::vector&) const - -> std::pair, std::vector>; + const auto index_list = std::vector( // + index_set.begin(), index_set.end()); - auto seed_point_cloud_from_two_views( - const std::vector&, // - const ImageView&, // - const PoseEdge, // - const v2::BrownConradyDistortionModel&) -> void; + return index_list; +} - public: /* utility methods */ - auto gid(const FeatureVertex u) const -> const FeatureGID& +auto PointCloudGenerator::filter_by_non_max_suppression( + const FeatureTrack& track) const -> FeatureTrack +{ + struct VertexScorePair + { + FeatureVertex vertex; + float score; + auto operator<(const VertexScorePair& other) const -> bool { - return _feature_graph[u]; + return score < other.score; } + }; - auto feature(const FeatureVertex u) const -> const OERegion& + auto filtered_set = std::unordered_map{}; + for (const auto& v : track) + { + const auto& f = feature(v); + const auto& pose_vertex = _feature_graph[v].pose_vertex; + const auto pose_vertex_it = filtered_set.find(pose_vertex); + if (pose_vertex_it == filtered_set.end()) { - const auto& [pose_vertex, feature_index] = gid(u); - const auto& f = features(_pose_graph[pose_vertex].keypoints); - return f[feature_index]; + filtered_set[pose_vertex] = {.vertex = v, .score = f.extremum_value}; + continue; } - auto pixel_coords(const FeatureVertex u) const -> const Eigen::Vector2f& - { - return feature(u).center(); - } + auto& vertex_score = pose_vertex_it->second; + if (vertex_score.score < f.extremum_value) + vertex_score = {.vertex = v, .score = f.extremum_value}; + } - auto barycenter(const std::vector&) const -> ScenePoint; + auto filtered_list = FeatureTrack(filtered_set.size()); + std::transform(filtered_set.begin(), filtered_set.end(), + filtered_list.begin(), + [](const auto& v) { return v.second.vertex; }); - auto find_feature_vertex_at_pose(const FeatureTrack&, // - const PoseVertex) const - -> std::optional; + // Order feature vertices in a chronological order. + // + // The camera vertex ID is incremented as time goes on and can be seen as a + // timestep. + std::sort(filtered_list.begin(), filtered_list.end(), + [this](const auto u, const auto v) { + return _feature_graph[u].pose_vertex < + _feature_graph[v].pose_vertex; + }); - auto retrieve_scene_point_color( - const ScenePoint::Coords& scene_point_coords, // - const ImageView& image, // - const QuaternionBasedPose& absolute_pose, // - const v2::BrownConradyDistortionModel& camera) const - -> ScenePoint::Color; + return filtered_list; +} - private: - const CameraPoseGraph& _pose_graph; - const FeatureGraph& _feature_graph; - PointCloud& _point_cloud; +auto PointCloudGenerator::find_feature_vertex_at_pose( + const FeatureTrack& track, const PoseVertex pose_vertex) const + -> std::optional +{ + auto v = std::find_if(track.begin(), track.end(), + [this, pose_vertex](const auto& v) { + return this->gid(v).pose_vertex == pose_vertex; + }); + return v == track.end() ? std::nullopt : std::make_optional(*v); +} - std::unordered_map - _from_vertex_to_scene_point_index; - }; -} // namespace DO::Sara +auto PointCloudGenerator::barycenter( + const std::vector& scene_point_indices) const -> ScenePoint +{ + if (scene_point_indices.empty()) + throw std::runtime_error{"Error: cannot calculate a barycentric scene " + "point from an empty list of scene point indices"}; + static const ScenePoint::Value zero = ScenePoint::Value::Zero(); + auto bary = std::accumulate( // + scene_point_indices.begin(), scene_point_indices.end(), zero, + [this](const ScenePoint::Value& a, + const ScenePointIndex bi) -> ScenePoint::Value { + const ScenePoint::Value& b = _point_cloud[bi]; + return a + b; + }); + bary /= scene_point_indices.size(); + + return bary; +} + +auto PointCloudGenerator::split_by_scene_point_knowledge( + const std::vector& tracks) const + -> std::pair, std::vector> +{ + auto& logger = Logger::get(); + + auto tracks_with_known_scene_point = std::vector{}; + auto tracks_with_unknown_scene_point = std::vector{}; + tracks_with_known_scene_point.reserve(tracks.size()); + tracks_with_unknown_scene_point.reserve(tracks.size()); + + SARA_LOGD(logger, "Splitting feature tracks by knowledge of scene point..."); + + for (const auto& track : tracks) + { + const auto scene_point_indices = list_scene_point_indices(track); + if (scene_point_indices.empty()) + tracks_with_unknown_scene_point.emplace_back(track); + else + tracks_with_known_scene_point.emplace_back(track); + } + + SARA_LOGD(logger, "Tracks: {}", tracks.size()); + SARA_LOGD(logger, "Tracks with known scene point: {}", + tracks_with_known_scene_point.size()); + SARA_LOGD(logger, "Tracks with unknown scene point: {}", + tracks_with_unknown_scene_point.size()); + + return std::make_pair(tracks_with_known_scene_point, + tracks_with_unknown_scene_point); +} + +auto PointCloudGenerator::retrieve_scene_point_color( + const Eigen::Vector3d& scene_point, // + const ImageView& image, // + const QuaternionBasedPose& pose, + const v2::BrownConradyDistortionModel& camera) const -> Rgb64f +{ + const auto& w = image.width(); + const auto& h = image.height(); + + // Its coordinates in the camera frame. + const auto camera_point = pose * scene_point; + + // Its corresponding pixel coordinates in the image. + const Eigen::Vector2i u = camera + .project(camera_point) // + .array() + .round() + .cast(); + + // Clamp for safety + // TODO: do bilinear interpolation. + const auto x = std::clamp(u.x(), 0, w - 1); + const auto y = std::clamp(u.y(), 0, h - 1); + + // N.B.: the image is an array of BGR values. + const auto& rgb8 = image(x, y); + // We store RGB values. + static constexpr auto normalization_factor = 1 / 255.; + const Rgb64f rgb64f = rgb8.cast() * normalization_factor; + + return rgb64f; +} + + +auto PointCloudGenerator::seed_point_cloud( + const std::vector& tracks, + const ImageView& image, // + const PoseEdge pose_edge, + const v2::BrownConradyDistortionModel& camera) -> void +{ + auto& logger = Logger::get(); + + SARA_LOGD(logger, "Transform feature tracks into best feature pairs..."); + const auto pose_u = _pose_graph.source(pose_edge); + const auto pose_v = _pose_graph.target(pose_edge); + const auto& tsfm_u = _pose_graph[pose_u].pose; + const auto& tsfm_v = _pose_graph[pose_v].pose; + SARA_LOGD(logger, "Pose[{}]:\n{}", pose_u, tsfm_u.matrix34()); + SARA_LOGD(logger, "Pose[{}]:\n{}", pose_v, tsfm_v.matrix34()); + + const auto num_tracks = static_cast(tracks.size()); + + using FeatureVertexPair = std::array; + auto matches = std::vector(num_tracks); + std::transform( + tracks.begin(), tracks.end(), matches.begin(), + [this, pose_u, pose_v](const FeatureTrack& track) -> FeatureVertexPair { + // Non-maximum suppression. + // + // We do need to filter the feature tracks by non-maximum suppression. + // + // Even in the case where the pose graph contains only 2 views, feature + // matches can be merged into components of cardinality larger than 2. + const auto track_filtered = filter_by_non_max_suppression(track); + if (track_filtered.size() != 2) + throw std::runtime_error{ + "Error: the NMS-filtered feature track must have cardinality 2!"}; + + // Retrieve the cleaned up feature correspondence. + const auto fu = find_feature_vertex_at_pose(track_filtered, pose_u); + const auto fv = find_feature_vertex_at_pose(track_filtered, pose_v); + if (!fu.has_value() || !fv.has_value()) + throw std::runtime_error{ + "Error: the feature match must exist in the graph!"}; + + return {*fu, *fv}; + }); + + SARA_LOGD(logger, "Calculating ray pairs from feature pairs..."); + auto rays_u = Eigen::MatrixXd{3, num_tracks}; + auto rays_v = Eigen::MatrixXd{3, num_tracks}; + for (auto t = 0u; t < num_tracks; ++t) + { + // Collect the feature match '(x, y)'. + const auto& [x, y] = matches[t]; + const auto x_coords = pixel_coords(x).cast(); + const auto y_coords = pixel_coords(y).cast(); + + // Backproject the pixel coordinates to their corresponding incident rays on + // the camera plane. + rays_u.col(t) = camera.backproject(x_coords); + rays_v.col(t) = camera.backproject(y_coords); + } + + // Calculate the associated triangulation. + SARA_LOGD(logger, "Initialization the point cloud by 3D triangulation from " + "the relative pose..."); + const auto motion = normalized_camera(_pose_graph[pose_edge].motion).matrix(); + if ((tsfm_v.matrix34() - motion).norm() > 1e-6) + throw std::runtime_error{"Error: the target abs pose must be initialized " + "as the relative motion!"}; + + const auto [X, scales_u, scales_v] = triangulate_linear_eigen( // + tsfm_u.matrix34(), tsfm_v.matrix34(), // + rays_u, rays_v); + + // Allocate the mapping from the feature vertices to the scene point index. + if (!_from_vertex_to_scene_point_index.empty()) + _from_vertex_to_scene_point_index.clear(); + + // Calculate the initial point cloud. + if (!_point_cloud.empty()) + _point_cloud.clear(); + + auto scene_point_index = ScenePointIndex{}; + for (auto j = 0; j < X.cols(); ++j) + { + // Only consider **cheiral** inliers: + // + // The triangulated 3D points must be in front of the two cameras! + if (!(scales_u(j) > 0 && scales_v(j) > 0)) + continue; + + // Calculate the scene point. + const Eigen::Vector3d coords = X.col(j).hnormalized(); + const auto color = retrieve_scene_point_color(coords, image, // + tsfm_v, camera); + + // Store the scene point to the point cloud. + auto scene_point_value = ScenePoint::Value{}; + scene_point_value << coords, color; + _point_cloud.emplace_back(scene_point_value); + + // Recall that a match is a pair of feature vertex. + const auto& [x, y] = matches[j]; + + // Assign a scene point index to the two feature vertices. + _from_vertex_to_scene_point_index[x] = scene_point_index; + _from_vertex_to_scene_point_index[y] = scene_point_index; + ++scene_point_index; + } + + SARA_LOGD(logger, "point cloud: {} 3D points", _point_cloud.size()); +} diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index 910006d0c..81aff7072 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -50,7 +50,7 @@ namespace DO::Sara { auto split_by_scene_point_knowledge(const std::vector&) const -> std::pair, std::vector>; - auto init_point_cloud(const std::vector&, // + auto seed_point_cloud(const std::vector&, // const ImageView&, // const PoseEdge, const v2::BrownConradyDistortionModel&) @@ -69,6 +69,11 @@ namespace DO::Sara { return f[feature_index]; } + auto pixel_coords(const FeatureVertex u) const -> const Eigen::Vector2f& + { + return feature(u).center(); + } + auto barycenter(const std::vector&) const -> ScenePoint; auto find_feature_vertex_at_pose(const FeatureTrack&, // From f06936ca5f801f04cae88cc6c5c766d4a124cbe6 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 15 Apr 2024 20:44:09 +0100 Subject: [PATCH 37/49] WIP: save work. --- .../BuildingBlocks/PointCloudGenerator.cpp | 89 +++++++++++++++++++ .../BuildingBlocks/PointCloudGenerator.hpp | 42 ++++++--- cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp | 20 ++--- .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 52 +++++++---- 4 files changed, 160 insertions(+), 43 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index 836ebe003..56b478af6 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -289,3 +289,92 @@ auto PointCloudGenerator::seed_point_cloud( SARA_LOGD(logger, "point cloud: {} 3D points", _point_cloud.size()); } + +auto PointCloudGenerator::propagate_scene_point_indices( + const std::vector& tracks) -> void +{ + auto& logger = Logger::get(); + + SARA_LOGI(logger, + "Propagating scene point indices to new feature vertices..."); + + for (const auto& track : tracks) + { + const auto scene_point_indices = list_scene_point_indices(track); + if (scene_point_indices.empty()) + continue; + +#if defined(DEBUG_ME) + if (scene_point_indices.size() > 1) + { + SARA_LOGT(logger, "Found a fused feature track..."); + + using ScenePointIndexVector = Eigen::RowVector< // + ScenePointIndex, Eigen::Dynamic>; + using FeatureVerticesAsVector = Eigen::Map< + const Eigen::RowVector>; + using ScenePointIndicesAsVector = Eigen::Map< // + const Eigen::RowVector>; + + const ScenePointIndexVector track_vector = + FeatureVerticesAsVector(track.data(), track.size()) + .cast(); + + const ScenePointIndexVector scene_index_vector = + ScenePointIndicesAsVector(scene_point_indices.data(), + scene_point_indices.size()); + SARA_LOGT(logger, "track indices: {}", track_vector); + SARA_LOGT(logger, "scene point indices: {}", scene_index_vector); + + for (const auto& i : scene_point_indices) + SARA_LOGT(logger, "scene coords[{}]: {}", i, + Eigen::RowVector3d(_point_cloud[i].coords().transpose())); + } +#endif + + // 1. Calculating the barycentric scene point coordinates to disambiguate + // the cluster of scene points. + const auto scene_point = barycenter(scene_point_indices); + for (const auto& i : scene_point_indices) + _point_cloud[i] = scene_point; + + // 2. Assigning a unique scene point index for each vertex of the feature + // track. + const auto& scene_point_index = scene_point_indices.front(); + for (const auto& v : track) + _from_vertex_to_scene_point_index[v] = scene_point_index; + } +} + +auto PointCloudOperator::compress_point_cloud( + const std::vector& tracks) -> bool +{ + auto& logger = Logger::get(); + SARA_LOGI(logger, "Compressing the point cloud..."); + + // Calculate the barycentric scene point for a given feature track. + auto point_cloud_compressed = std::vector{}; + point_cloud_compressed.reserve(tracks.size()); + + // Reset the scene point index for each feature track. + for (auto t = ScenePointIndex{}; t < tracks.size(); ++t) + { + const auto scene_point_indices = list_scene_point_indices(tracks[t]); + if (scene_point_indices.empty()) + continue; + + // Reassign the scene point index for the given feature track. + for (const auto& v : track) + _from_vertex_to_scene_point_index[v] = t; + + // Recalculate the scene point index as a barycenter. + const auto scene_point = + barycenter(scene_point_indices) + point_cloud_compressed.emplace_back(scene_point); + } + + // Swap the point cloud with the set of barycenters. + std::swap(_point_cloud, point_cloud_compressed); + + return true; +} diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index 81aff7072..fea06f4fe 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -20,10 +20,13 @@ namespace DO::Sara { class PointCloudGenerator { - public: + public: /* aliases */ using PoseVertex = CameraPoseGraph::Vertex; using PoseEdge = CameraPoseGraph::Edge; + using FeatureVertex = FeatureGraph::Vertex; + using FeatureVertexIndex = FeatureGraph::VertexIndex; + using ScenePointIndex = std::size_t; using ScenePoint = RgbColoredPoint; @@ -32,6 +35,7 @@ namespace DO::Sara { using FeatureToScenePointMap = std::unordered_map; + public: /* main interface */ PointCloudGenerator(const CameraPoseGraph& camera_pose_graph, const FeatureGraph& feature_graph, PointCloud& point_cloud) @@ -41,22 +45,13 @@ namespace DO::Sara { { } - auto list_scene_point_indices(const FeatureTrack&) const - -> std::vector; - - auto filter_by_non_max_suppression(const FeatureTrack&) const // - -> FeatureTrack; - - auto split_by_scene_point_knowledge(const std::vector&) const - -> std::pair, std::vector>; - auto seed_point_cloud(const std::vector&, // const ImageView&, // const PoseEdge, const v2::BrownConradyDistortionModel&) -> void; - public: /* utility methods */ + public: /* helper feature retrieval methods */ auto gid(const FeatureVertex u) const -> const FeatureGID& { return _feature_graph[u]; @@ -74,7 +69,9 @@ namespace DO::Sara { return feature(u).center(); } - auto barycenter(const std::vector&) const -> ScenePoint; + public: /* helper query methods */ + auto list_scene_point_indices(const FeatureTrack&) const + -> std::vector; auto find_feature_vertex_at_pose(const FeatureTrack&, // const PoseVertex) const @@ -86,7 +83,26 @@ namespace DO::Sara { const QuaternionBasedPose& pose, const v2::BrownConradyDistortionModel& camera) const -> Rgb64f; - private: + public: /* data transformation methods */ + auto barycenter(const std::vector&) const -> ScenePoint; + + auto filter_by_non_max_suppression(const FeatureTrack&) const // + -> FeatureTrack; + + auto split_by_scene_point_knowledge(const std::vector&) const + -> std::pair, std::vector>; + + auto propagate_scene_point_indices(const std::vector&) + -> void; + + //! - The point cloud compression reassigns a unique scene point cloud to + //! each feature tracks. + //! - The scene point is recalculated as a the barycenter of the + //! possibly multiple scene points we have found after recalculating the + //! feature tracks. + auto compress_point_cloud(const std::vector&) -> bool; + + private: /* data members */ const CameraPoseGraph& _pose_graph; const FeatureGraph& _feature_graph; PointCloud& _point_cloud; diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp index 88095b0d3..611a8c298 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp @@ -19,21 +19,18 @@ using namespace DO::Sara; auto FeatureTracker::update_feature_tracks( - const CameraPoseGraph& pose_graph, - const CameraPoseGraph::Edge pose_edge) -> void + const CameraPoseGraph& pose_graph, const CameraPoseGraph::Edge pose_edge) + -> void { auto& logger = Logger::get(); - const CameraPoseGraph::Impl& pg = pose_graph; - FeatureGraph::Impl& fg = _feature_graph; - // Retrieve the camera poses from the relative pose edge. - const auto pose_u = boost::source(pose_edge, pg); - const auto pose_v = boost::target(pose_edge, pg); + const auto pose_u = pose_graph.source(pose_edge); + const auto pose_v = pose_graph.target(pose_edge); // The relative pose edge contains the set of all feature correspondences. - const auto& matches = pg[pose_edge].matches; + const auto& matches = pose_graph[pose_edge].matches; // Which of these feature correspondences are marked as inliers? - const auto& inliers = pg[pose_edge].inliers; + const auto& inliers = pose_graph[pose_edge].inliers; // Add the feature graph edges. // @@ -70,6 +67,7 @@ auto FeatureTracker::update_feature_tracks( const auto y_does_not_exist_yet = it_y == _feature_vertex.end(); // If not, add them if necessary. + FeatureGraph::Impl& fg = _feature_graph; const auto x = x_does_not_exist_yet ? boost::add_vertex(fg) : it_x->second; const auto y = y_does_not_exist_yet ? boost::add_vertex(fg) : it_y->second; @@ -88,8 +86,8 @@ auto FeatureTracker::update_feature_tracks( // navigate between the feature graph to the pose graph. const auto [xy, xy_added] = boost::add_edge(x, y, fg); auto& xy_attrs = fg[xy]; - xy_attrs.pose_src = boost::source(pose_edge, pg); - xy_attrs.pose_dst = boost::target(pose_edge, pg); + xy_attrs.pose_src = pose_graph.source(pose_edge); + xy_attrs.pose_dst = pose_graph.target(pose_edge); xy_attrs.index = m; } diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index 5cc32068a..9dfcf3359 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -120,9 +120,9 @@ auto v2::OdometryPipeline::add_camera_pose() -> bool // Detect and describe the local features. _pose_prev = _pose_curr; - const auto frame = _distortion_corrector->frame_gray32f(); + const auto frame_gray32f = _distortion_corrector->frame_gray32f(); const auto frame_number = _video_streamer.frame_number(); - auto keys_curr = detect_keypoints(frame); + auto keys_curr = detect_keypoints(frame_gray32f); // Boundary case: the graphs are empty. if (_pose_graph.num_vertices() == 0) @@ -176,27 +176,41 @@ auto v2::OdometryPipeline::add_camera_pose() -> bool std::tie(_tracks_alive, _track_visibility_count) = _feature_tracker.calculate_alive_feature_tracks(_pose_curr); - // 4. TODO: Init point cloud - - // 5. TODO: don't add 3D scene points that are too far, like point in the - // sky + // 4. Initialize the point cloud. + // + // TODO: don't add 3D scene points that are too far, like point in the + // sky + const auto frame_rgb8 = _distortion_corrector->frame_rgb8(); + _point_cloud_generator->seed_point_cloud(_tracks_alive, frame_rgb8, + pose_edge, _camera); return true; } - // 1. Grow the feature graph first by adding the feature matches that are - // deemed reliable from the relative pose estimation. - // 2. Recalculate the feature tracks. - // 3. Get the feature tracks that are still alive. - // 4. For each feature track still alive, get the corresponding scene - // points. - // Each alive feature track still has the same old feature IDs in the - // previous image frames, and we know their scene points. - // Use triangulation computer vision task, to calculate the new camera - // absolute pose. - // 5. With the camera absolute pose, add the new scene points. - // Specifically, they are the alive feature tracks (with cardinality 2) - // for which we don't know the scene points yet. + // 1. Update the feature tracks by adding the feature matches that are + // verified by the relative pose estimation. + const auto pose_edge = _pose_graph.add_relative_pose( // + _pose_prev, _pose_curr, // + std::move(rel_pose_data)); + _feature_tracker.update_feature_tracks(_pose_graph, pose_edge); + + // 2. Recalculate the feature tracks that are still alive. + std::tie(_tracks_alive, _track_visibility_count) = + _feature_tracker.calculate_alive_feature_tracks(_pose_curr); + + // 2. Propagate the scene point to the feature tracks that grew longer. + // The feature tracks that grew longer can only be those among the tracks + // still alive. + SARA_LOGI(logger, "Propagating the scene points to new features..."); + _point_cloud_generator->propagate_scene_point_indices(_tracks_alive); + + // 3. Reassign a unique scene point cloud to each feature tracks by + // compressing the point cloud. + SARA_LOGI(logger, "Compressing the point cloud..."); + _point_cloud_generator->compress_point_cloud( + _feature_tracker._feature_tracks); + + // 4. Determine the current absolute pose from the alive tracks. // TODO: Grow point cloud by triangulation. return false; From 44a55c4bb2e1c1ca9d9288e344c4f8b68643dd64 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Mon, 15 Apr 2024 21:58:12 +0100 Subject: [PATCH 38/49] WIP: save work. --- .../BuildingBlocks/PointCloudGenerator.cpp | 17 +++++------ .../BuildingBlocks/PointCloudGenerator.hpp | 29 ++++++++++++++----- .../SfM/BuildingBlocks/RgbColoredPoint.hpp | 5 ++++ 3 files changed, 35 insertions(+), 16 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index 56b478af6..5f4925b59 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -83,8 +83,8 @@ auto PointCloudGenerator::filter_by_non_max_suppression( } auto PointCloudGenerator::find_feature_vertex_at_pose( - const FeatureTrack& track, const PoseVertex pose_vertex) const - -> std::optional + const FeatureTrack& track, + const PoseVertex pose_vertex) const -> std::optional { auto v = std::find_if(track.begin(), track.end(), [this, pose_vertex](const auto& v) { @@ -93,7 +93,6 @@ auto PointCloudGenerator::find_feature_vertex_at_pose( return v == track.end() ? std::nullopt : std::make_optional(*v); } - auto PointCloudGenerator::barycenter( const std::vector& scene_point_indices) const -> ScenePoint { @@ -178,7 +177,6 @@ auto PointCloudGenerator::retrieve_scene_point_color( return rgb64f; } - auto PointCloudGenerator::seed_point_cloud( const std::vector& tracks, const ImageView& image, // @@ -346,7 +344,7 @@ auto PointCloudGenerator::propagate_scene_point_indices( } } -auto PointCloudOperator::compress_point_cloud( +auto PointCloudGenerator::compress_point_cloud( const std::vector& tracks) -> bool { auto& logger = Logger::get(); @@ -359,7 +357,9 @@ auto PointCloudOperator::compress_point_cloud( // Reset the scene point index for each feature track. for (auto t = ScenePointIndex{}; t < tracks.size(); ++t) { - const auto scene_point_indices = list_scene_point_indices(tracks[t]); + const auto& track = tracks[t]; + + const auto scene_point_indices = list_scene_point_indices(track); if (scene_point_indices.empty()) continue; @@ -368,9 +368,8 @@ auto PointCloudOperator::compress_point_cloud( _from_vertex_to_scene_point_index[v] = t; // Recalculate the scene point index as a barycenter. - const auto scene_point = - barycenter(scene_point_indices) - point_cloud_compressed.emplace_back(scene_point); + const auto scene_point = barycenter(scene_point_indices); + point_cloud_compressed.emplace_back(scene_point); } // Swap the point cloud with the set of barycenters. diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index fea06f4fe..9d011df5f 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -45,11 +45,11 @@ namespace DO::Sara { { } - auto seed_point_cloud(const std::vector&, // - const ImageView&, // - const PoseEdge, - const v2::BrownConradyDistortionModel&) - -> void; + auto + seed_point_cloud(const std::vector&, // + const ImageView&, // + const PoseEdge, + const v2::BrownConradyDistortionModel&) -> void; public: /* helper feature retrieval methods */ auto gid(const FeatureVertex u) const -> const FeatureGID& @@ -84,16 +84,31 @@ namespace DO::Sara { const v2::BrownConradyDistortionModel& camera) const -> Rgb64f; public: /* data transformation methods */ + //! @brief Calculate the barycentric scene point. + //! + //! We expect the array of scene point indices to be originated from a + //! feature track. auto barycenter(const std::vector&) const -> ScenePoint; + //! @brief A track is a sequence of feature. auto filter_by_non_max_suppression(const FeatureTrack&) const // -> FeatureTrack; + //! @brief Split the list of feature tracks into two lists. + //! + //! The first list contains the tracks for which a scene point is calculated. + //! The second list contains the tracks for which a scene point is not yet + //! calculated. auto split_by_scene_point_knowledge(const std::vector&) const -> std::pair, std::vector>; - auto propagate_scene_point_indices(const std::vector&) - -> void; + //! - The point cloud compression reassigns a unique scene point cloud to + //! each feature tracks. + //! - The scene point is recalculated as a the barycenter of the + //! possibly multiple scene points we have found after recalculating the + //! feature tracks. + auto + propagate_scene_point_indices(const std::vector&) -> void; //! - The point cloud compression reassigns a unique scene point cloud to //! each feature tracks. diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp index 8e94875ab..b57fac448 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/RgbColoredPoint.hpp @@ -15,6 +15,11 @@ namespace DO::Sara { RgbColoredPoint() = default; + RgbColoredPoint(const Coords& coords, const Color& color) + { + _v << coords, color; + } + RgbColoredPoint(const Value& v) : _v{v} { From 4f1d0524d99df6842b3e1972d05867bf73c980e6 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 15 Apr 2024 22:41:20 +0100 Subject: [PATCH 39/49] WIP: save work. --- .../visual_odometry_example_v2.cpp | 18 ++- .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 115 +++++++++--------- .../Sara/SfM/OdometryV2/OdometryPipeline.hpp | 8 +- 3 files changed, 79 insertions(+), 62 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp index 4d4d24aa0..0c51d4f60 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #if defined(_WIN32) @@ -170,8 +171,21 @@ class SingleWindowApp auto upload_point_cloud_data_to_opengl() -> void { - // _point_cloud.upload_host_data_to_gl( - // _pipeline._triangulator->_colored_point_cloud); + const auto& point_cloud = _pipeline.point_cloud(); + const auto ptr = + const_cast(point_cloud.data()); + const auto ptrd = reinterpret_cast(ptr); + + const auto num_points = static_cast(point_cloud.size()); + static constexpr auto dim = 6; + const auto pc_tview = sara::TensorView_{ + ptrd, // + {num_points, dim} // + }; + + auto& logger = sara::Logger::get(); + SARA_LOGW(logger, "point cloud dimensions: {} ", pc_tview.sizes()); + _point_cloud.upload_host_data_to_gl(pc_tview.cast()); } auto render_video() -> void diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index 9dfcf3359..a814cd946 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -54,7 +54,7 @@ auto v2::OdometryPipeline::process() -> void _distortion_corrector->undistort(); - add_camera_pose(); + grow_geometry(); } auto v2::OdometryPipeline::make_display_frame() const -> Image @@ -113,7 +113,7 @@ auto v2::OdometryPipeline::estimate_relative_pose( return res; } -auto v2::OdometryPipeline::add_camera_pose() -> bool +auto v2::OdometryPipeline::grow_geometry() -> bool { auto& logger = Logger::get(); @@ -151,67 +151,68 @@ auto v2::OdometryPipeline::add_camera_pose() -> bool } SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); - if (_pose_graph.num_vertices() == 1) - { - auto abs_pose_curr = QuaternionBasedPose{ - .q = Eigen::Quaterniond{rel_pose_data.motion.R}, - .t = rel_pose_data.motion.t // - }; - - auto abs_pose_data = AbsolutePoseData{ - frame_number, // - std::move(keys_curr), // - std::move(abs_pose_curr) // - }; + // if (_pose_graph.num_vertices() == 1) + // { + auto abs_pose_curr = QuaternionBasedPose{ + .q = Eigen::Quaterniond{rel_pose_data.motion.R}, + .t = rel_pose_data.motion.t // + }; - // 1. Add the absolute pose vertex. - _pose_graph.add_absolute_pose(std::move(abs_pose_data)); + auto abs_pose_data = AbsolutePoseData{ + frame_number, // + std::move(keys_curr), // + std::move(abs_pose_curr) // + }; - // 2. Add the pose edge, which will invalidate the relative pose data. - const auto pose_edge = _pose_graph.add_relative_pose( - _pose_prev, _pose_curr, std::move(rel_pose_data)); + // 1. Add the absolute pose vertex. + _pose_graph.add_absolute_pose(std::move(abs_pose_data)); - // 3. Grow the feature graph by adding the feature matches. - _feature_tracker.update_feature_tracks(_pose_graph, pose_edge); - std::tie(_tracks_alive, _track_visibility_count) = - _feature_tracker.calculate_alive_feature_tracks(_pose_curr); + // 2. Add the pose edge, which will invalidate the relative pose data. + const auto pose_edge = _pose_graph.add_relative_pose( + _pose_prev, _pose_curr, std::move(rel_pose_data)); - // 4. Initialize the point cloud. - // - // TODO: don't add 3D scene points that are too far, like point in the - // sky - const auto frame_rgb8 = _distortion_corrector->frame_rgb8(); - _point_cloud_generator->seed_point_cloud(_tracks_alive, frame_rgb8, - pose_edge, _camera); - - return true; - } - - // 1. Update the feature tracks by adding the feature matches that are - // verified by the relative pose estimation. - const auto pose_edge = _pose_graph.add_relative_pose( // - _pose_prev, _pose_curr, // - std::move(rel_pose_data)); + // 3. Grow the feature graph by adding the feature matches. _feature_tracker.update_feature_tracks(_pose_graph, pose_edge); - - // 2. Recalculate the feature tracks that are still alive. std::tie(_tracks_alive, _track_visibility_count) = _feature_tracker.calculate_alive_feature_tracks(_pose_curr); - // 2. Propagate the scene point to the feature tracks that grew longer. - // The feature tracks that grew longer can only be those among the tracks - // still alive. - SARA_LOGI(logger, "Propagating the scene points to new features..."); - _point_cloud_generator->propagate_scene_point_indices(_tracks_alive); - - // 3. Reassign a unique scene point cloud to each feature tracks by - // compressing the point cloud. - SARA_LOGI(logger, "Compressing the point cloud..."); - _point_cloud_generator->compress_point_cloud( - _feature_tracker._feature_tracks); - - // 4. Determine the current absolute pose from the alive tracks. - - // TODO: Grow point cloud by triangulation. - return false; + // 4. Initialize the point cloud. + // + // TODO: don't add 3D scene points that are too far, like point in the + // sky + const auto frame_rgb8 = _distortion_corrector->frame_rgb8(); + _point_cloud_generator->seed_point_cloud(_tracks_alive, frame_rgb8, pose_edge, + _camera); + + return true; + // } + + // // 1. Update the feature tracks by adding the feature matches that are + // // verified by the relative pose estimation. + // const auto pose_edge = _pose_graph.add_relative_pose( // + // _pose_prev, _pose_curr, // + // std::move(rel_pose_data)); + // _feature_tracker.update_feature_tracks(_pose_graph, pose_edge); + + // // 2. Recalculate the feature tracks that are still alive. + // std::tie(_tracks_alive, _track_visibility_count) = + // _feature_tracker.calculate_alive_feature_tracks(_pose_curr); + + // // 2. Propagate the scene point to the feature tracks that grew longer. + // // The feature tracks that grew longer can only be those among the + // tracks + // // still alive. + // SARA_LOGI(logger, "Propagating the scene points to new features..."); + // _point_cloud_generator->propagate_scene_point_indices(_tracks_alive); + + // // 3. Reassign a unique scene point cloud to each feature tracks by + // // compressing the point cloud. + // SARA_LOGI(logger, "Compressing the point cloud..."); + // _point_cloud_generator->compress_point_cloud( + // _feature_tracker._feature_tracks); + + // // 4. Determine the current absolute pose from the alive tracks. + + // // TODO: Grow point cloud by triangulation. + // return false; } diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp index 90c6a2ab3..e71e30b57 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp @@ -34,8 +34,10 @@ namespace DO::Sara::v2 { auto make_display_frame() const -> Image; - auto detect_keypoints() -> const KeypointList&; - auto estimate_relative_pose() -> const RelativePoseData&; + auto point_cloud() const -> const PointCloudGenerator::PointCloud& + { + return _point_cloud; + } private: /* computer vision tasks */ auto detect_keypoints(const ImageView&) const @@ -47,7 +49,7 @@ namespace DO::Sara::v2 { -> std::pair; private: /* graph update tasks */ - auto add_camera_pose() -> bool; + auto grow_geometry() -> bool; public: /* data members */ VideoStreamer _video_streamer; From cfbb7409f867b009bdd9396c897100e7b4f96eff Mon Sep 17 00:00:00 2001 From: David OK Date: Tue, 16 Apr 2024 12:17:16 +0100 Subject: [PATCH 40/49] WIP: deduplicated code. --- .../BuildingBlocks/PointCloudGenerator.cpp | 241 ++++++++++-------- .../BuildingBlocks/PointCloudGenerator.hpp | 30 ++- .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 4 +- 3 files changed, 149 insertions(+), 126 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index 5f4925b59..1dfaf5054 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -83,8 +83,8 @@ auto PointCloudGenerator::filter_by_non_max_suppression( } auto PointCloudGenerator::find_feature_vertex_at_pose( - const FeatureTrack& track, - const PoseVertex pose_vertex) const -> std::optional + const FeatureTrack& track, const PoseVertex pose_vertex) const + -> std::optional { auto v = std::find_if(track.begin(), track.end(), [this, pose_vertex](const auto& v) { @@ -177,117 +177,6 @@ auto PointCloudGenerator::retrieve_scene_point_color( return rgb64f; } -auto PointCloudGenerator::seed_point_cloud( - const std::vector& tracks, - const ImageView& image, // - const PoseEdge pose_edge, - const v2::BrownConradyDistortionModel& camera) -> void -{ - auto& logger = Logger::get(); - - SARA_LOGD(logger, "Transform feature tracks into best feature pairs..."); - const auto pose_u = _pose_graph.source(pose_edge); - const auto pose_v = _pose_graph.target(pose_edge); - const auto& tsfm_u = _pose_graph[pose_u].pose; - const auto& tsfm_v = _pose_graph[pose_v].pose; - SARA_LOGD(logger, "Pose[{}]:\n{}", pose_u, tsfm_u.matrix34()); - SARA_LOGD(logger, "Pose[{}]:\n{}", pose_v, tsfm_v.matrix34()); - - const auto num_tracks = static_cast(tracks.size()); - - using FeatureVertexPair = std::array; - auto matches = std::vector(num_tracks); - std::transform( - tracks.begin(), tracks.end(), matches.begin(), - [this, pose_u, pose_v](const FeatureTrack& track) -> FeatureVertexPair { - // Non-maximum suppression. - // - // We do need to filter the feature tracks by non-maximum suppression. - // - // Even in the case where the pose graph contains only 2 views, feature - // matches can be merged into components of cardinality larger than 2. - const auto track_filtered = filter_by_non_max_suppression(track); - if (track_filtered.size() != 2) - throw std::runtime_error{ - "Error: the NMS-filtered feature track must have cardinality 2!"}; - - // Retrieve the cleaned up feature correspondence. - const auto fu = find_feature_vertex_at_pose(track_filtered, pose_u); - const auto fv = find_feature_vertex_at_pose(track_filtered, pose_v); - if (!fu.has_value() || !fv.has_value()) - throw std::runtime_error{ - "Error: the feature match must exist in the graph!"}; - - return {*fu, *fv}; - }); - - SARA_LOGD(logger, "Calculating ray pairs from feature pairs..."); - auto rays_u = Eigen::MatrixXd{3, num_tracks}; - auto rays_v = Eigen::MatrixXd{3, num_tracks}; - for (auto t = 0u; t < num_tracks; ++t) - { - // Collect the feature match '(x, y)'. - const auto& [x, y] = matches[t]; - const auto x_coords = pixel_coords(x).cast(); - const auto y_coords = pixel_coords(y).cast(); - - // Backproject the pixel coordinates to their corresponding incident rays on - // the camera plane. - rays_u.col(t) = camera.backproject(x_coords); - rays_v.col(t) = camera.backproject(y_coords); - } - - // Calculate the associated triangulation. - SARA_LOGD(logger, "Initialization the point cloud by 3D triangulation from " - "the relative pose..."); - const auto motion = normalized_camera(_pose_graph[pose_edge].motion).matrix(); - if ((tsfm_v.matrix34() - motion).norm() > 1e-6) - throw std::runtime_error{"Error: the target abs pose must be initialized " - "as the relative motion!"}; - - const auto [X, scales_u, scales_v] = triangulate_linear_eigen( // - tsfm_u.matrix34(), tsfm_v.matrix34(), // - rays_u, rays_v); - - // Allocate the mapping from the feature vertices to the scene point index. - if (!_from_vertex_to_scene_point_index.empty()) - _from_vertex_to_scene_point_index.clear(); - - // Calculate the initial point cloud. - if (!_point_cloud.empty()) - _point_cloud.clear(); - - auto scene_point_index = ScenePointIndex{}; - for (auto j = 0; j < X.cols(); ++j) - { - // Only consider **cheiral** inliers: - // - // The triangulated 3D points must be in front of the two cameras! - if (!(scales_u(j) > 0 && scales_v(j) > 0)) - continue; - - // Calculate the scene point. - const Eigen::Vector3d coords = X.col(j).hnormalized(); - const auto color = retrieve_scene_point_color(coords, image, // - tsfm_v, camera); - - // Store the scene point to the point cloud. - auto scene_point_value = ScenePoint::Value{}; - scene_point_value << coords, color; - _point_cloud.emplace_back(scene_point_value); - - // Recall that a match is a pair of feature vertex. - const auto& [x, y] = matches[j]; - - // Assign a scene point index to the two feature vertices. - _from_vertex_to_scene_point_index[x] = scene_point_index; - _from_vertex_to_scene_point_index[y] = scene_point_index; - ++scene_point_index; - } - - SARA_LOGD(logger, "point cloud: {} 3D points", _point_cloud.size()); -} - auto PointCloudGenerator::propagate_scene_point_indices( const std::vector& tracks) -> void { @@ -377,3 +266,129 @@ auto PointCloudGenerator::compress_point_cloud( return true; } + +auto PointCloudGenerator::grow_point_cloud( + const std::vector& ftracks_without_scene_point, + const ImageView& image, // + const PoseEdge pose_edge, + const v2::BrownConradyDistortionModel& camera) -> void +{ + auto& logger = Logger::get(); + + SARA_LOGD(logger, + "Extracting the pairwise matches from the feature tracks..."); + const auto& pose_u = _pose_graph.source(pose_edge); + const auto& pose_v = _pose_graph.target(pose_edge); + const auto& tsfm_u = _pose_graph[pose_u].pose; + const auto& tsfm_v = _pose_graph[pose_v].pose; + SARA_LOGD(logger, "Pose[{}]:\n{}", pose_u, tsfm_u.matrix34()); + SARA_LOGD(logger, "Pose[{}]:\n{}", pose_v, tsfm_v.matrix34()); + + const auto num_tracks = + static_cast(ftracks_without_scene_point.size()); + + using FeatureVertexPair = std::array; + auto fmatches = std::vector(num_tracks); + + SARA_LOGD(logger, "Calculating feature matches..."); + std::transform( + ftracks_without_scene_point.begin(), + ftracks_without_scene_point.end(), // + fmatches.begin(), + [this, pose_u, pose_v](const FeatureTrack& ftrack) -> FeatureVertexPair { + // Non-maximum suppression. + // + // We do need to filter the feature tracks by non-maximum suppression. + // + // Even in the case where the pose graph contains only 2 views, feature + // matches can be merged into components of cardinality larger than 2. + const auto ftrack_nms = filter_by_non_max_suppression(ftrack); + + // N.B.: the feature track cannot have any scene point indices at this + // point. + const auto scene_point_indices = list_scene_point_indices(ftrack_nms); + if (!scene_point_indices.empty()) + throw std::runtime_error{ + "Error: the feature track cannot have any scene point index!"}; + + // N.B.: at this point a track of visibility count >= 3 can possibly + // have no scene point. + // + // This happens when the cheirality is not satisifed. When the + // cheirality is not satisfied, we choose not to assign a scene point to + // this feature track. +#if defined(DEBUG_ME) + const Eigen::RowVector feature_vector = + Eigen::Map< + const Eigen::RowVector>( + track_filtered.data(), track_filtered.size()); + SARA_LOGD(logger, "track indices: {}", feature_vector); +#endif + + + // Retrieve the cleaned up feature correspondence. + const auto fu = find_feature_vertex_at_pose(ftrack_nms, pose_u); + const auto fv = find_feature_vertex_at_pose(ftrack_nms, pose_v); + if (!fu.has_value() || !fv.has_value()) + throw std::runtime_error{ + "Error: the feature match must exist in the feature graph!"}; + + return {*fu, *fv}; + }); + + SARA_LOGD(logger, + "Calculating the backprojected rays from the feature matches..."); + auto rays_u = Eigen::MatrixXd{3, num_tracks}; + auto rays_v = Eigen::MatrixXd{3, num_tracks}; + for (auto t = 0u; t < num_tracks; ++t) + { + // Collect the feature match '(x, y)'. + const auto& [x, y] = fmatches[t]; + const auto x_coords = pixel_coords(x).cast(); + const auto y_coords = pixel_coords(y).cast(); + + // Backproject the pixel coordinates to their corresponding incident rays on + // the camera plane. + rays_u.col(t) = camera.backproject(x_coords); + rays_v.col(t) = camera.backproject(y_coords); + } + + // Calculate the associated triangulation. + SARA_LOGD(logger, "Triangulating from the backprojected rays..."); + const auto [X, scales_u, scales_v] = triangulate_linear_eigen( // + tsfm_u.matrix34(), tsfm_v.matrix34(), // + rays_u, rays_v); + + SARA_LOGD(logger, "Adding new scene points to point cloud..."); + SARA_LOGD(logger, "[BEFORE] point cloud: {} 3D points", _point_cloud.size()); + + // N.B.: start with the right offset for the scene point index. + auto scene_point_index = _point_cloud.size(); + for (auto j = 0; j < X.cols(); ++j) + { + // Only consider **cheiral** inliers: + // + // The triangulated 3D points must be in front of the two cameras! + if (!(scales_u(j) > 0 && scales_v(j) > 0)) + continue; + + // Calculate the scene point. + const Eigen::Vector3d coords = X.col(j).hnormalized(); + const auto color = retrieve_scene_point_color(coords, image, // + tsfm_v, camera); + + // Store the scene point to the point cloud. + auto scene_point_value = ScenePoint{coords, color}; + _point_cloud.emplace_back(std::move(scene_point_value)); + + // Recall that a match is a pair of feature vertex. + const auto& [x, y] = fmatches[j]; + + // Assign a scene point index to the two feature vertices. + _from_vertex_to_scene_point_index[x] = scene_point_index; + _from_vertex_to_scene_point_index[y] = scene_point_index; + ++scene_point_index; + } + + SARA_LOGD(logger, "[AFTER ] point cloud: {} 3D points", _point_cloud.size()); +} diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index 9d011df5f..4865f9a94 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -35,7 +35,7 @@ namespace DO::Sara { using FeatureToScenePointMap = std::unordered_map; - public: /* main interface */ + public: /* constructor */ PointCloudGenerator(const CameraPoseGraph& camera_pose_graph, const FeatureGraph& feature_graph, PointCloud& point_cloud) @@ -45,12 +45,6 @@ namespace DO::Sara { { } - auto - seed_point_cloud(const std::vector&, // - const ImageView&, // - const PoseEdge, - const v2::BrownConradyDistortionModel&) -> void; - public: /* helper feature retrieval methods */ auto gid(const FeatureVertex u) const -> const FeatureGID& { @@ -96,9 +90,9 @@ namespace DO::Sara { //! @brief Split the list of feature tracks into two lists. //! - //! The first list contains the tracks for which a scene point is calculated. - //! The second list contains the tracks for which a scene point is not yet - //! calculated. + //! The first list contains the tracks for which a scene point is + //! calculated. The second list contains the tracks for which a scene point + //! is not yet calculated. auto split_by_scene_point_knowledge(const std::vector&) const -> std::pair, std::vector>; @@ -107,8 +101,8 @@ namespace DO::Sara { //! - The scene point is recalculated as a the barycenter of the //! possibly multiple scene points we have found after recalculating the //! feature tracks. - auto - propagate_scene_point_indices(const std::vector&) -> void; + auto propagate_scene_point_indices(const std::vector&) + -> void; //! - The point cloud compression reassigns a unique scene point cloud to //! each feature tracks. @@ -117,6 +111,18 @@ namespace DO::Sara { //! feature tracks. auto compress_point_cloud(const std::vector&) -> bool; + //! Grow the point cloud becomes possible when the most recent absolute pose + //! is known. + //! + //! This calculates the new 3D scene points. Specifically the new 3D scene + //! points are those calculated from the feature tracks for which we didn't + //! know their scene point values. + auto grow_point_cloud( + const std::vector& feature_tracks_without_scene_point, + const ImageView& image, // + const PoseEdge pose_edge, + const v2::BrownConradyDistortionModel& camera) -> void; + private: /* data members */ const CameraPoseGraph& _pose_graph; const FeatureGraph& _feature_graph; diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index a814cd946..644bc8228 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -181,7 +181,7 @@ auto v2::OdometryPipeline::grow_geometry() -> bool // TODO: don't add 3D scene points that are too far, like point in the // sky const auto frame_rgb8 = _distortion_corrector->frame_rgb8(); - _point_cloud_generator->seed_point_cloud(_tracks_alive, frame_rgb8, pose_edge, + _point_cloud_generator->grow_point_cloud(_tracks_alive, frame_rgb8, pose_edge, _camera); return true; @@ -214,5 +214,7 @@ auto v2::OdometryPipeline::grow_geometry() -> bool // // 4. Determine the current absolute pose from the alive tracks. // // TODO: Grow point cloud by triangulation. + // _point_cloud_generator->grow_point_cloud(_ftracks_without_scene_point, + // frame_rgb8, pose_edge, _camera); // return false; } From 471059b635422aeeb4380c50f68d238061dd29fc Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 17 Apr 2024 13:35:22 +0100 Subject: [PATCH 41/49] ENH: inspect the pipeline step by step. --- .../visual_odometry_example_v2.cpp | 53 +++++++++++++++---- .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 16 +++++- 2 files changed, 59 insertions(+), 10 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp index 0c51d4f60..6063c8e39 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp @@ -72,6 +72,7 @@ class SingleWindowApp glfwSetWindowUserPointer(_window, this); // Register callbacks. glfwSetWindowSizeCallback(_window, window_size_callback); + glfwSetKeyCallback(_window, key_callback); } ~SingleWindowApp() @@ -115,12 +116,24 @@ class SingleWindowApp glfwSwapInterval(1); while (!glfwWindowShouldClose(_window)) { - if (!_pipeline.read()) - break; + if (!_pause) + { + if (!_pipeline.read()) + break; - _pipeline.process(); - // Load data to OpenGL. - upload_point_cloud_data_to_opengl(); + if (!_pipeline._video_streamer.skip()) + { + _pipeline.process(); + + // Load data to OpenGL. + // + // TODO: upload only if we have a new image frame to process and only + // if the absolute pose estimation is successful. + upload_point_cloud_data_to_opengl(); + + _pause = true; + } + } // Clear the color buffer and the buffer testing. glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); @@ -172,19 +185,22 @@ class SingleWindowApp auto upload_point_cloud_data_to_opengl() -> void { const auto& point_cloud = _pipeline.point_cloud(); + + static constexpr auto dim = 6; + const auto num_points = static_cast(point_cloud.size()); + if (num_points == 0) + return; + const auto ptr = const_cast(point_cloud.data()); const auto ptrd = reinterpret_cast(ptr); - - const auto num_points = static_cast(point_cloud.size()); - static constexpr auto dim = 6; const auto pc_tview = sara::TensorView_{ ptrd, // {num_points, dim} // }; auto& logger = sara::Logger::get(); - SARA_LOGW(logger, "point cloud dimensions: {} ", pc_tview.sizes()); + SARA_LOGI(logger, "point cloud dimensions: {} ", pc_tview.sizes()); _point_cloud.upload_host_data_to_gl(pc_tview.cast()); } @@ -271,6 +287,22 @@ class SingleWindowApp self._point_cloud_projection = self._point_cloud_viewport.perspective(); } + static auto key_callback(GLFWwindow* window, // + int key, // + int /* scancode */, // + int action, // + int /* mods */) -> void + { + auto& app = get_self(window); + if (app._pause && key == GLFW_KEY_SPACE && + (action == GLFW_RELEASE || action == GLFW_REPEAT)) + { + app._pause = false; + std::cout << "RESUME" << std::endl; + return; + } + } + private: static auto init_glfw() -> void { @@ -342,6 +374,9 @@ class SingleWindowApp Eigen::Matrix4f _point_cloud_projection; // kgl::Camera _point_cloud_camera; float _point_size = 5.f; + + //! @brief User interaction. + bool _pause = false; }; bool SingleWindowApp::_glfw_initialized = false; diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index 644bc8228..9ef89a9b3 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -52,6 +52,12 @@ auto v2::OdometryPipeline::process() -> void if (_video_streamer.skip()) return; + auto& logger = Logger::get(); + SARA_LOGI(logger, "[Video Stream] Processing image frame {}", + _video_streamer.frame_number()); + + SARA_LOGI(logger, "[Image Distortion] Undistort image frame {}", + _video_streamer.frame_number()); _distortion_corrector->undistort(); grow_geometry(); @@ -68,7 +74,7 @@ auto v2::OdometryPipeline::detect_keypoints(const ImageView& image) const auto& logger = Logger::get(); const auto keys = compute_sift_keypoints(image, // _feature_params.image_pyr_params); - SARA_LOGI(logger, "[Feature Detection] {} keypoints", features(keys).size()); + SARA_LOGI(logger, "[Keypoint Detection] {} keypoints", features(keys).size()); return keys; } @@ -124,6 +130,10 @@ auto v2::OdometryPipeline::grow_geometry() -> bool const auto frame_number = _video_streamer.frame_number(); auto keys_curr = detect_keypoints(frame_gray32f); + // TODO: CHECK EVERYTHING UNTIL HERE. + return true; + + // Boundary case: the graphs are empty. if (_pose_graph.num_vertices() == 0) { @@ -176,10 +186,14 @@ auto v2::OdometryPipeline::grow_geometry() -> bool std::tie(_tracks_alive, _track_visibility_count) = _feature_tracker.calculate_alive_feature_tracks(_pose_curr); + // 4. Initialize the point cloud. // // TODO: don't add 3D scene points that are too far, like point in the // sky + // + // TODO: don't clear next time we just need to debug at this time. + _point_cloud.clear(); const auto frame_rgb8 = _distortion_corrector->frame_rgb8(); _point_cloud_generator->grow_point_cloud(_tracks_alive, frame_rgb8, pose_edge, _camera); From 1fc07d48c8922dd8af547a313e27f9171239ccd0 Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 17 Apr 2024 14:05:40 +0100 Subject: [PATCH 42/49] MAINT: fix bug. --- .../BuildingBlocks/PointCloudGenerator.cpp | 4 +- cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp | 7 ++-- .../Sara/SfM/OdometryV2/OdometryPipeline.cpp | 40 ++++++++++++++----- 3 files changed, 37 insertions(+), 14 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index 1dfaf5054..6ef5e789c 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -360,7 +360,7 @@ auto PointCloudGenerator::grow_point_cloud( rays_u, rays_v); SARA_LOGD(logger, "Adding new scene points to point cloud..."); - SARA_LOGD(logger, "[BEFORE] point cloud: {} 3D points", _point_cloud.size()); + SARA_LOGD(logger, "[BEFORE] {} scene points", _point_cloud.size()); // N.B.: start with the right offset for the scene point index. auto scene_point_index = _point_cloud.size(); @@ -390,5 +390,5 @@ auto PointCloudGenerator::grow_point_cloud( ++scene_point_index; } - SARA_LOGD(logger, "[AFTER ] point cloud: {} 3D points", _point_cloud.size()); + SARA_LOGD(logger, "[AFTER ] {} scene points", _point_cloud.size()); } diff --git a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp index 37b3b784f..cfd2cf8ab 100644 --- a/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/CameraPoseGraph.cpp @@ -32,9 +32,10 @@ auto CameraPoseGraph::add_absolute_pose(AbsolutePoseData&& data) _g[v] = std::move(data); SARA_LOGI(logger, - "[SfM] Added camera absolute pose[frame:{}]:\n" - "Keypoints: {} points\n" - "Absolute pose: {}\n", // + "[SfM] Added camera absolute pose:\n" + "- Frame : {}\n" + "- Keypoints : {}\n" + "- Absolute pose:\n{}\n", // _g[v].image_id, // features(_g[v].keypoints).size(), // _g[v].pose.matrix34()); diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp index 9ef89a9b3..6bfec0ae9 100644 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp @@ -13,11 +13,11 @@ #include -#include -#include - +#include #include +#include #include +#include using namespace DO::Sara; @@ -130,10 +130,6 @@ auto v2::OdometryPipeline::grow_geometry() -> bool const auto frame_number = _video_streamer.frame_number(); auto keys_curr = detect_keypoints(frame_gray32f); - // TODO: CHECK EVERYTHING UNTIL HERE. - return true; - - // Boundary case: the graphs are empty. if (_pose_graph.num_vertices() == 0) { @@ -161,6 +157,7 @@ auto v2::OdometryPipeline::grow_geometry() -> bool } SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); + // if (_pose_graph.num_vertices() == 1) // { auto abs_pose_curr = QuaternionBasedPose{ @@ -168,6 +165,32 @@ auto v2::OdometryPipeline::grow_geometry() -> bool .t = rel_pose_data.motion.t // }; + // The rotation is expressed in the camera coordinates. + // But the calculation is done in the automotive/aeronautics coordinate + // system. + // + // The z-coordinate of the camera coordinates is the x-axis of the automotive + // coordinates + // + // clang-format off + static const auto P = (Eigen::Matrix3d{} << + 0, 0, 1, + -1, 0, 0, + 0, -1, 0 + ).finished(); + // clang-format on + + const auto& R = rel_pose_data.motion.R; + const Eigen::Matrix3d R_delta_abs = P * R.transpose() * P.transpose(); + _current_global_rotation = R_delta_abs * _current_global_rotation; + + const auto q_global = Eigen::Quaterniond{_current_global_rotation}; + auto angles = calculate_yaw_pitch_roll(q_global); + static constexpr auto degrees = 180. / M_PI; + SARA_LOGI(logger, "Global yaw = {} deg", angles(0) * degrees); + SARA_LOGI(logger, "Global pitch = {} deg", angles(1) * degrees); + SARA_LOGI(logger, "Global roll = {} deg", angles(2) * degrees); + auto abs_pose_data = AbsolutePoseData{ frame_number, // std::move(keys_curr), // @@ -175,7 +198,7 @@ auto v2::OdometryPipeline::grow_geometry() -> bool }; // 1. Add the absolute pose vertex. - _pose_graph.add_absolute_pose(std::move(abs_pose_data)); + _pose_curr = _pose_graph.add_absolute_pose(std::move(abs_pose_data)); // 2. Add the pose edge, which will invalidate the relative pose data. const auto pose_edge = _pose_graph.add_relative_pose( @@ -186,7 +209,6 @@ auto v2::OdometryPipeline::grow_geometry() -> bool std::tie(_tracks_alive, _track_visibility_count) = _feature_tracker.calculate_alive_feature_tracks(_pose_curr); - // 4. Initialize the point cloud. // // TODO: don't add 3D scene points that are too far, like point in the From acc11667027e6ab5cd5e6d70bc21247fc04a2ff2 Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 17 Apr 2024 15:57:05 +0100 Subject: [PATCH 43/49] MAINT: copyright notice, less includes. --- .../MinimalSolvers/SevenPointAlgorithm.hpp | 2 -- .../MultiViewGeometry/PointCorrespondenceList.hpp | 11 +++++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/SevenPointAlgorithm.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/SevenPointAlgorithm.hpp index ec425d85f..4905aeb9f 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/SevenPointAlgorithm.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/SevenPointAlgorithm.hpp @@ -6,8 +6,6 @@ #include #include -#include - namespace DO::Sara { diff --git a/cpp/src/DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp b/cpp/src/DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp index 49d0b2dda..17b01e650 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp @@ -1,3 +1,14 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024 David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + #pragma once #include From c12a2355f4d617bee1c1665b2073f662a74b288b Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 17 Apr 2024 16:14:37 +0100 Subject: [PATCH 44/49] WIP: save work. --- .../MinimalSolvers/P3PSolver.hpp | 96 +++++++++++++++++++ .../PointRayCorrespondenceList.hpp | 26 +++++ 2 files changed, 122 insertions(+) create mode 100644 cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp create mode 100644 cpp/src/DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp new file mode 100644 index 000000000..8166f3c0c --- /dev/null +++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp @@ -0,0 +1,96 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024 David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#pragma once + +#include "DO/Sara/Core/Math/UsualFunctions.hpp" +#include +#include + + +namespace DO::Sara { + + template + struct P3PSolver + { + static constexpr auto num_points = 3; + static constexpr auto num_models = 4; + + using data_point_type = TensorView_; + using model_type = Eigen::Matrix; + + inline auto operator()(const data_point_type& x) const + -> std::vector + { + const Eigen::Matrix3 scene_points = x.matrix().leftCols(3); + const Eigen::Matrix3 backprojected_rays = x.matrix().rightCols(3); + return solve_p3p(scene_points, backprojected_rays); + } + }; + + //! @brief Joint cheirality and epipolar consistency for RANSAC. + template + struct CheiralPnPConsistency + { + using Model = Eigen::Matrix; + + CameraModel camera; + Model pose; + double pixel_reprojection_error; + + CheiralPnPConsistency() = default; + + CheiralPnPConsistency(const Model& pose) + { + set_model(pose); + } + + auto set_model(const Model& p) -> void + { + pose = p; + } + + template + auto operator()(const Eigen::MatrixBase& scene_points, + const Eigen::MatrixBase& rays) const + -> Eigen::Array + { + Eigen::MatrixXd u1n = pose * scene_points; + Eigen::MatrixXd u2{rays.rows(), rays.cols()}; + + auto u1 = Eigen::MatrixXd{2, scene_points.cols()}; + for (auto i = 0; i < u1.cols(); ++i) + u1.col(i) = camera.project(u1.col(i)); + + for (auto i = 0; i < u2.cols(); ++i) + u2.col(i) = camera.project(rays.col(i)); + + const auto err_max = square(pixel_reprojection_error); + + const auto small_reproj_error = + (u2 - u1).colwise().squaredNorm().array() < err_max; + const auto cheiral = scene_points.row(2).array() > 0; + + return small_reproj_error && cheiral; + } + + //! @brief Check the inlier predicate on a list of correspondences. + template + inline auto operator()(const PointRayCorrespondenceSubsetList& m) const + -> Array + { + const auto& scene_points = m._p1.colmajor_view().matrix(); + const auto& backprojected_rays = m._p2.colmajor_view().matrix(); + return this->operator()(scene_points, backprojected_rays); + } + }; + +} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp b/cpp/src/DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp new file mode 100644 index 000000000..e507a49f4 --- /dev/null +++ b/cpp/src/DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp @@ -0,0 +1,26 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024 David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#pragma once + +#include "DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp" +#include + + +namespace DO::Sara { + + template + using PointRayCorrespondenceList = PointCorrespondenceList; + + template + using PointRayCorrespondenceSubsetList = PointCorrespondenceSubsetList; + +} // namespace DO::Sara From 6bd8750d41413225b03827885b8e6b799bf39b8f Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 17 Apr 2024 17:37:49 +0100 Subject: [PATCH 45/49] WIP: save work. --- .../MinimalSolvers/P3PSolver.hpp | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp index 8166f3c0c..77541f522 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp @@ -42,9 +42,9 @@ namespace DO::Sara { { using Model = Eigen::Matrix; - CameraModel camera; + const CameraModel* camera = nullptr; Model pose; - double pixel_reprojection_error; + double image_reproj_err_max; CheiralPnPConsistency() = default; @@ -63,21 +63,26 @@ namespace DO::Sara { const Eigen::MatrixBase& rays) const -> Eigen::Array { - Eigen::MatrixXd u1n = pose * scene_points; - Eigen::MatrixXd u2{rays.rows(), rays.cols()}; + if (camera == nullptr) + throw std::runtime_error{ + "Error: you must initialize the intrinsic camera parameters!"}; + Eigen::MatrixXd u1n = pose * scene_points; auto u1 = Eigen::MatrixXd{2, scene_points.cols()}; for (auto i = 0; i < u1.cols(); ++i) - u1.col(i) = camera.project(u1.col(i)); + u1.col(i) = camera->project(u1.col(i)); + Eigen::MatrixXd u2{rays.rows(), rays.cols()}; for (auto i = 0; i < u2.cols(); ++i) - u2.col(i) = camera.project(rays.col(i)); + u2.col(i) = camera->project(rays.col(i)); - const auto err_max = square(pixel_reprojection_error); + // Check the cheirality w.r.t. the candidate pose. + const auto cheiral = u1n.row(2).array() > 0; + // Checka the image reprojection errors. + const auto err_max = square(image_reproj_err_max); const auto small_reproj_error = (u2 - u1).colwise().squaredNorm().array() < err_max; - const auto cheiral = scene_points.row(2).array() > 0; return small_reproj_error && cheiral; } From aeabbc0041405ff78a6aa252f8af51f5c2325acc Mon Sep 17 00:00:00 2001 From: David OK Date: Thu, 18 Apr 2024 13:19:43 +0100 Subject: [PATCH 46/49] MAINT: rename variables for more natural usage. --- .../FeatureDetectors/EdgePostProcessing.hpp | 4 +- .../Algorithms/RobustEstimation/PointList.hpp | 10 ++-- cpp/src/DO/Sara/Geometry/Tools/Normalizer.hpp | 2 +- .../MultiViewGeometry/Geometry/Normalizer.hpp | 42 +++++++------- .../MinimalSolvers/InlierPredicates.hpp | 7 ++- .../PointCorrespondenceList.hpp | 57 ++++++++++++------- .../PointRayCorrespondenceList.hpp | 3 + cpp/src/DO/Sara/RANSAC/RANSAC.hpp | 6 +- cpp/src/DO/Sara/RANSAC/Utility.hpp | 6 +- ...test_multiviewgeometry_vanishing_point.cpp | 2 +- 10 files changed, 81 insertions(+), 58 deletions(-) diff --git a/cpp/src/DO/Sara/FeatureDetectors/EdgePostProcessing.hpp b/cpp/src/DO/Sara/FeatureDetectors/EdgePostProcessing.hpp index feb02db32..8926c2969 100644 --- a/cpp/src/DO/Sara/FeatureDetectors/EdgePostProcessing.hpp +++ b/cpp/src/DO/Sara/FeatureDetectors/EdgePostProcessing.hpp @@ -194,8 +194,8 @@ namespace DO::Sara { const auto num_curve_points = static_cast(curve_points.size()); auto point_list = PointList{}; - point_list._data.resize(num_curve_points, 3); - auto& points = point_list._data; + point_list.data.resize(num_curve_points, 3); + auto& points = point_list.data; auto point_matrix = points.matrix(); for (auto r = 0; r < num_curve_points; ++r) point_matrix.row(r) = curve_points[r] // diff --git a/cpp/src/DO/Sara/Geometry/Algorithms/RobustEstimation/PointList.hpp b/cpp/src/DO/Sara/Geometry/Algorithms/RobustEstimation/PointList.hpp index 8204f7f41..1f69d5cdf 100644 --- a/cpp/src/DO/Sara/Geometry/Algorithms/RobustEstimation/PointList.hpp +++ b/cpp/src/DO/Sara/Geometry/Algorithms/RobustEstimation/PointList.hpp @@ -23,25 +23,25 @@ namespace DO::Sara { operator TensorView&() { - return _data; + return data; } operator const TensorView&() const { - return _data; + return data; } auto operator[](const int n) const -> value_type { - return _data[n]; + return data[n]; } auto size() const -> int { - return _data.size(0); + return data.size(0); } - Tensor_ _data; + Tensor_ data; }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/Geometry/Tools/Normalizer.hpp b/cpp/src/DO/Sara/Geometry/Tools/Normalizer.hpp index 3952445a9..080118fff 100644 --- a/cpp/src/DO/Sara/Geometry/Tools/Normalizer.hpp +++ b/cpp/src/DO/Sara/Geometry/Tools/Normalizer.hpp @@ -96,7 +96,7 @@ namespace DO::Sara { inline auto normalize(const PointList& X) const { auto Xn = PointList{}; - Xn._data = apply_transform(T, X._data); + Xn.data = apply_transform(T, X.data); return Xn; } diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp index fb32a281a..3e44cafff 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp @@ -38,8 +38,8 @@ namespace DO::Sara { } Normalizer(const PointCorrespondenceList& matches) - : T1{compute_normalizer(matches._p1)} - , T2{compute_normalizer(matches._p2)} + : T1{compute_normalizer(matches.x)} + , T2{compute_normalizer(matches.y)} { T1_inv = T1.inverse(); T2_inv = T2.inverse(); @@ -51,12 +51,12 @@ namespace DO::Sara { return std::make_tuple(apply_transform(T1, p1), apply_transform(T2, p2)); } - auto normalize(const PointCorrespondenceList& X) const + auto normalize(const PointCorrespondenceList& M) const -> PointCorrespondenceList { - auto Xn = PointCorrespondenceList{}; - std::tie(Xn._p1, Xn._p2) = this->normalize(X._p1, X._p2); - return Xn; + auto Mn = PointCorrespondenceList{}; + std::tie(Mn.x, Mn.y) = this->normalize(M.x, M.y); + return Mn; } inline auto denormalize(Eigen::Matrix3d& H) const -> void @@ -81,9 +81,9 @@ namespace DO::Sara { { } - Normalizer(const PointCorrespondenceList& matches) - : T1{compute_normalizer(matches._p1)} - , T2{compute_normalizer(matches._p2)} + Normalizer(const PointCorrespondenceList& M) + : T1{compute_normalizer(M.x)} + , T2{compute_normalizer(M.y)} { } @@ -93,12 +93,12 @@ namespace DO::Sara { return std::make_tuple(apply_transform(T1, p1), apply_transform(T2, p2)); } - auto normalize(const PointCorrespondenceList& X) const + auto normalize(const PointCorrespondenceList& M) const -> PointCorrespondenceList { - auto Xn = PointCorrespondenceList{}; - std::tie(Xn._p1, Xn._p2) = this->normalize(X._p1, X._p2); - return Xn; + auto Mn = PointCorrespondenceList{}; + std::tie(Mn.x, Mn.y) = this->normalize(M.x, M.y); + return Mn; } auto denormalize(Eigen::Matrix3d& F) const -> void @@ -133,12 +133,12 @@ namespace DO::Sara { return std::make_tuple(p1n, p2n); } - auto normalize(const PointCorrespondenceList& X) const + auto normalize(const PointCorrespondenceList& M) const -> PointCorrespondenceList { - auto Xn = PointCorrespondenceList{}; - std::tie(Xn._p1, Xn._p2) = this->normalize(X._p1, X._p2); - return Xn; + auto Mn = PointCorrespondenceList{}; + std::tie(Mn.x, Mn.y) = this->normalize(M.x, M.y); + return Mn; } //! @brief Dummy implementation. @@ -172,12 +172,12 @@ namespace DO::Sara { return std::make_tuple(p1n, p2n); } - auto normalize(const PointCorrespondenceList& X) const + auto normalize(const PointCorrespondenceList& M) const -> PointCorrespondenceList { - auto Xn = PointCorrespondenceList{}; - std::tie(Xn._p1, Xn._p2) = this->normalize(X._p1, X._p2); - return Xn; + auto Mn = PointCorrespondenceList{}; + std::tie(Mn.x, Mn.y) = this->normalize(M.x, M.y); + return Mn; } //! @brief Dummy implementation. diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/InlierPredicates.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/InlierPredicates.hpp index 3cd5a4c03..8451ccdb4 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/InlierPredicates.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/InlierPredicates.hpp @@ -43,7 +43,8 @@ namespace DO::Sara { // THE BUG IS HERE: we need to pass backprojected rays and instead we are // giving pixel coordinates. const auto [X, s1, s2] = triangulate_linear_eigen(P1, P2, u1, u2); - const auto cheirality = (s1.transpose().array()) > 0 && (s2.transpose().array() > 0); + const auto cheirality = + (s1.transpose().array()) > 0 && (s2.transpose().array() > 0); return epipolar_consistent && cheirality; #else @@ -57,8 +58,8 @@ namespace DO::Sara { inline auto operator()(const PointCorrespondenceList& m) const -> Array { - return this->operator()(m._p1.colmajor_view().matrix(), - m._p2.colmajor_view().matrix()); + return this->operator()(m.x.colmajor_view().matrix(), + m.y.colmajor_view().matrix()); } }; diff --git a/cpp/src/DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp b/cpp/src/DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp index 17b01e650..08a5e8864 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/PointCorrespondenceList.hpp @@ -16,6 +16,22 @@ namespace DO::Sara { + //! @brief The data structure to store the list of point correspondences. + //! + //! A point correspondence is denoted as $(x[i], y[i])$. + //! + //! In my opinion, 'x' and 'y' are the lightest and most agnostic notations I + //! could find. + //! We do lose expressivity because they may be too neutral, but it still + //! feels natural and mathematical at the same time. + //! + //! Notice that we are not assuming anythin about the dimensions of x[i] and + //! y[i]. + //! - x[i] and y[i] don't have to have the same dimensions. + //! - (x[i], y[i]) can be 2D point <-> 2D point correspondence in two + //! different images, or + //! - (x[i], y[i]) can be a 3D point <-> 2D point correspondence, or + //! - (x[i], y[i]) can be a 3D scene point <-> 3D ray correspondence. template struct PointCorrespondenceList { @@ -24,37 +40,40 @@ namespace DO::Sara { PointCorrespondenceList() = default; PointCorrespondenceList(const TensorView_& M, - const TensorView_& p1, - const TensorView_& p2) - : _p1{M.size(0), p1.size(1)} - , _p2{M.size(0), p2.size(1)} + const TensorView_& x_all, + const TensorView_& y_all) + : x{M.size(0), x_all.size(1)} + , y{M.size(0), y_all.size(1)} { - auto p1_mat = p1.matrix(); - auto p2_mat = p2.matrix(); - auto p1_matched = _p1.matrix(); - auto p2_matched = _p2.matrix(); + auto x_all_mat = x_all.matrix(); + auto y_all_mat = y_all.matrix(); + auto x_matched = x.matrix(); + auto y_matched = y.matrix(); for (auto m = 0; m < M.size(0); ++m) { - const auto& i1 = M(m, 0); - const auto& i2 = M(m, 1); + const auto& x_idx = M(m, 0); + const auto& y_idx = M(m, 1); - p1_matched.row(m) = p1_mat.row(i1); - p2_matched.row(m) = p2_mat.row(i2); + x_matched.row(m) = x_all_mat.row(x_idx); + y_matched.row(m) = y_all_mat.row(y_idx); } } auto size() const -> int { - return _p1.size(0); + return x.size(0); } auto operator[](const int n) const -> value_type { - return {_p1[n], _p2[n]}; + return {x[n], y[n]}; } - Tensor_ _p1; - Tensor_ _p2; + //! @brief The correspondences are: (x[i], y[i]). + //! @{ + Tensor_ x; + Tensor_ y; + //! @} }; template @@ -64,11 +83,11 @@ namespace DO::Sara { auto operator[](const int n) const -> value_type { - return {_p1[n], _p2[n]}; + return {x[n], y[n]}; } - Tensor_ _p1; - Tensor_ _p2; + Tensor_ x; + Tensor_ y; }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp b/cpp/src/DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp index e507a49f4..e166d7a02 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp @@ -17,10 +17,13 @@ namespace DO::Sara { + //! @brief Convenient aliases. + //! @{ template using PointRayCorrespondenceList = PointCorrespondenceList; template using PointRayCorrespondenceSubsetList = PointCorrespondenceSubsetList; + //! @} } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/RANSAC/RANSAC.hpp b/cpp/src/DO/Sara/RANSAC/RANSAC.hpp index 986dfd263..124af43a3 100644 --- a/cpp/src/DO/Sara/RANSAC/RANSAC.hpp +++ b/cpp/src/DO/Sara/RANSAC/RANSAC.hpp @@ -151,7 +151,7 @@ namespace DO::Sara { inline auto operator()(const PointList& X) const -> Array { - return distance(X._data.colmajor_view().matrix()).array() < + return distance(X.data.colmajor_view().matrix()).array() < static_cast(err_threshold); } @@ -160,8 +160,8 @@ namespace DO::Sara { inline auto operator()(const PointCorrespondenceList& m) const -> Array { - return distance(m._p1.colmajor_view().matrix(), - m._p2.colmajor_view().matrix()) + return distance(m.x.colmajor_view().matrix(), + m.y.colmajor_view().matrix()) .array() < err_threshold; } }; diff --git a/cpp/src/DO/Sara/RANSAC/Utility.hpp b/cpp/src/DO/Sara/RANSAC/Utility.hpp index 18e531e60..55a179fb7 100644 --- a/cpp/src/DO/Sara/RANSAC/Utility.hpp +++ b/cpp/src/DO/Sara/RANSAC/Utility.hpp @@ -62,7 +62,7 @@ namespace DO::Sara { auto from_index_to_point(const TensorView_& point_indices, const PointList& points) -> Tensor_ { - return from_index_to_point(point_indices, points._data); + return from_index_to_point(point_indices, points.data); }; template @@ -71,8 +71,8 @@ namespace DO::Sara { -> PointCorrespondenceSubsetList { auto res = PointCorrespondenceSubsetList{}; - res._p1 = from_index_to_point(point_indices, correspondences._p1); - res._p2 = from_index_to_point(point_indices, correspondences._p2); + res.x = from_index_to_point(point_indices, correspondences.x); + res.y = from_index_to_point(point_indices, correspondences.y); return res; }; diff --git a/cpp/test/Sara/MultiViewGeometry/test_multiviewgeometry_vanishing_point.cpp b/cpp/test/Sara/MultiViewGeometry/test_multiviewgeometry_vanishing_point.cpp index 91cdca9b3..6c4c7bb1c 100644 --- a/cpp/test/Sara/MultiViewGeometry/test_multiviewgeometry_vanishing_point.cpp +++ b/cpp/test/Sara/MultiViewGeometry/test_multiviewgeometry_vanishing_point.cpp @@ -29,7 +29,7 @@ BOOST_AUTO_TEST_CASE(test_vp_detection) const auto vp = Eigen::Vector3f(500, 500, 1); auto line_list = PointList{}; - auto& lines = line_list._data; + auto& lines = line_list.data; lines = Tensor_{6, 3}; auto lines_as_matrix = lines.matrix(); From d7143d86ffce687c4209efd65e2c487806301e59 Mon Sep 17 00:00:00 2001 From: David OK Date: Thu, 18 Apr 2024 13:22:58 +0100 Subject: [PATCH 47/49] MAINT: fix compile errors. --- .../MultiViewGeometry/orthogonal_vanishing_point_detection.cpp | 2 +- cpp/test/Sara/RANSAC/test_ransac_line_fit.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/orthogonal_vanishing_point_detection.cpp b/cpp/examples/Sara/MultiViewGeometry/orthogonal_vanishing_point_detection.cpp index 5e4d49d09..2fa26e34e 100644 --- a/cpp/examples/Sara/MultiViewGeometry/orthogonal_vanishing_point_detection.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/orthogonal_vanishing_point_detection.cpp @@ -354,7 +354,7 @@ int sara_graphics_main(int argc, char** argv) tic(); const Eigen::MatrixXf lines_as_matrix = lines.matrix().transpose(); auto plane_list = PointList{}; - auto& plane_tensor = plane_list._data; + auto& plane_tensor = plane_list.data; plane_tensor.resize(static_cast(line_segments.size()), 4); plane_tensor.colmajor_view().matrix() = (Pt * lines_as_matrix) // .colwise() // diff --git a/cpp/test/Sara/RANSAC/test_ransac_line_fit.cpp b/cpp/test/Sara/RANSAC/test_ransac_line_fit.cpp index 2781ceafc..617e7d77e 100644 --- a/cpp/test/Sara/RANSAC/test_ransac_line_fit.cpp +++ b/cpp/test/Sara/RANSAC/test_ransac_line_fit.cpp @@ -27,7 +27,7 @@ BOOST_AUTO_TEST_CASE(test_robust_line_fit) { auto points = PointList{Tensor_{6, 3}}; // clang-format off - points._data.matrix() << 0.00, 0.00, 1, + points.data.matrix() << 0.00, 0.00, 1, 1.00, 1.10, 1, 3.40, 3.46, 1, 9.80, 10.10, 1, From 2a7ca1e63dc46d03782b3e1179a87659aae77f47 Mon Sep 17 00:00:00 2001 From: David OK Date: Mon, 22 Apr 2024 12:51:08 +0100 Subject: [PATCH 48/49] MAINT: rewview code. --- .../MinimalSolvers/P3PSolver.hpp | 45 ++++++++++--------- 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp index 77541f522..5c3fadf76 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp @@ -40,22 +40,26 @@ namespace DO::Sara { template struct CheiralPnPConsistency { - using Model = Eigen::Matrix; + using PoseMatrix = Eigen::Matrix; + using Model = PoseMatrix; + //! @brief The camera model for the image. const CameraModel* camera = nullptr; - Model pose; - double image_reproj_err_max; + //! @brief The pose matrix. + PoseMatrix T; + //! @brief Image reprojection error in pixel. + double ε; - CheiralPnPConsistency() = default; + inline CheiralPnPConsistency() = default; - CheiralPnPConsistency(const Model& pose) + inline CheiralPnPConsistency(const PoseMatrix& pose_matrix) { - set_model(pose); + set_model(pose_matrix); } - auto set_model(const Model& p) -> void + inline auto set_model(const PoseMatrix& pose_matrix) -> void { - pose = p; + T = pose_matrix; } template @@ -67,33 +71,34 @@ namespace DO::Sara { throw std::runtime_error{ "Error: you must initialize the intrinsic camera parameters!"}; - Eigen::MatrixXd u1n = pose * scene_points; + const auto& X_world = scene_points; + const Eigen::MatrixXd X_camera = T * X_world; + auto u1 = Eigen::MatrixXd{2, scene_points.cols()}; for (auto i = 0; i < u1.cols(); ++i) - u1.col(i) = camera->project(u1.col(i)); + u1.col(i) = camera->project(X_camera.col(i)); - Eigen::MatrixXd u2{rays.rows(), rays.cols()}; + auto u2 = Eigen::MatrixXd{rays.rows(), rays.cols()}; for (auto i = 0; i < u2.cols(); ++i) u2.col(i) = camera->project(rays.col(i)); // Check the cheirality w.r.t. the candidate pose. - const auto cheiral = u1n.row(2).array() > 0; + const auto cheiral = X_camera.row(2).array() > 0; - // Checka the image reprojection errors. - const auto err_max = square(image_reproj_err_max); - const auto small_reproj_error = - (u2 - u1).colwise().squaredNorm().array() < err_max; + // Check the **squared** image reprojection errors. + const auto ε_max = square(ε); + const auto ε_small = (u2 - u1).colwise().squaredNorm().array() < ε_max; - return small_reproj_error && cheiral; + return ε_small && cheiral; } //! @brief Check the inlier predicate on a list of correspondences. template inline auto operator()(const PointRayCorrespondenceSubsetList& m) const - -> Array + -> Eigen::Array { - const auto& scene_points = m._p1.colmajor_view().matrix(); - const auto& backprojected_rays = m._p2.colmajor_view().matrix(); + const auto& scene_points = m.x.colmajor_view().matrix(); + const auto& backprojected_rays = m.y.colmajor_view().matrix(); return this->operator()(scene_points, backprojected_rays); } }; From 51c865a45faabd0e95308073ed504b82e536c1dc Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 24 Apr 2024 17:18:10 +0100 Subject: [PATCH 49/49] ENH: add robust estimation API for the PnP problem. --- cpp/src/DO/Sara/Core/PhysicalQuantities.hpp | 2 - .../MultiViewGeometry/Geometry/Normalizer.hpp | 30 ++++++++++- .../Geometry/PinholeCamera.hpp | 2 - .../MinimalSolvers/P3PSolver.hpp | 50 +++++++++++++++---- cpp/src/DO/Sara/RANSAC/RANSAC.hpp | 4 +- cpp/src/DO/Sara/RANSAC/RANSACv2.hpp | 8 +-- .../SyntheticDataUtilities.hpp | 7 ++- cpp/test/Sara/RANSAC/CMakeLists.txt | 2 +- 8 files changed, 79 insertions(+), 26 deletions(-) diff --git a/cpp/src/DO/Sara/Core/PhysicalQuantities.hpp b/cpp/src/DO/Sara/Core/PhysicalQuantities.hpp index 39f2d4d43..bb2be3837 100644 --- a/cpp/src/DO/Sara/Core/PhysicalQuantities.hpp +++ b/cpp/src/DO/Sara/Core/PhysicalQuantities.hpp @@ -252,8 +252,6 @@ namespace DO::Sara { { } - auto operator=(const self_type&) -> self_type& = default; - inline constexpr operator scalar_type() const { return value; diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp index 3e44cafff..92043f845 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp @@ -25,6 +25,8 @@ namespace DO::Sara { //! @ingroup GeometryDataNormalizer //! @{ + + //! @brief Normalizer for the two-view homography estimation. template <> struct Normalizer { @@ -70,7 +72,7 @@ namespace DO::Sara { Eigen::Matrix3d T2_inv; }; - + //! @brief Normalizer for the two-view fundamental matrix estimation. template <> struct Normalizer { @@ -150,6 +152,7 @@ namespace DO::Sara { Eigen::Matrix3d K2_inv; }; + //! @brief Normalizer for the two-view relative pose estimation. template <> struct Normalizer { @@ -189,6 +192,31 @@ namespace DO::Sara { Eigen::Matrix3d K2_inv; }; + //! @brief Normalizer for the PnP estimation. + template <> + struct Normalizer> + { + using PoseMatrix = Eigen::Matrix; + + Normalizer() = default; + + auto normalize(const TensorView_& scene_points, + const TensorView_& backprojected_rays) const + { + return std::make_tuple(scene_points, backprojected_rays); + } + + auto normalize(const PointCorrespondenceList& M) const + -> PointCorrespondenceList + { + return M; + } + + auto denormalize(const PoseMatrix&) const -> void + { + } + }; + //! @} } /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp index 848013e01..7ecdbfe16 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp @@ -11,8 +11,6 @@ #pragma once -#include - #include #include diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp index 5c3fadf76..07d4c4a2b 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp @@ -2,7 +2,7 @@ // This file is part of Sara, a basic set of libraries in C++ for computer // vision. // -// Copyright (C) 2024 David Ok +// Copyright (C) 2024-present David Ok // // This Source Code Form is subject to the terms of the Mozilla Public // License v. 2.0. If a copy of the MPL was not distributed with this file, @@ -11,7 +11,7 @@ #pragma once -#include "DO/Sara/Core/Math/UsualFunctions.hpp" +#include #include #include @@ -24,15 +24,28 @@ namespace DO::Sara { static constexpr auto num_points = 3; static constexpr auto num_models = 4; - using data_point_type = TensorView_; + using tensor_view_type = TensorView_; + using data_point_type = std::array, 2>; using model_type = Eigen::Matrix; - inline auto operator()(const data_point_type& x) const + inline auto operator()(const tensor_view_type& scene_points, + const tensor_view_type& rays) const -> std::vector { - const Eigen::Matrix3 scene_points = x.matrix().leftCols(3); - const Eigen::Matrix3 backprojected_rays = x.matrix().rightCols(3); - return solve_p3p(scene_points, backprojected_rays); + const auto sp_mat_ = scene_points.colmajor_view().matrix(); + + Eigen::Matrix3 sp_mat = sp_mat_.topRows(3); + if (sp_mat_.cols() == 4) + sp_mat.array().rowwise() /= sp_mat_.array().row(3); + + const Eigen::Matrix3 ray_mat = rays.colmajor_view().matrix(); + return solve_p3p(sp_mat, ray_mat); + } + + inline auto operator()(const data_point_type& X) -> std::vector + { + const auto& [scene_points, backprojected_rays] = X; + return this->operator()(scene_points, backprojected_rays); } }; @@ -47,7 +60,7 @@ namespace DO::Sara { const CameraModel* camera = nullptr; //! @brief The pose matrix. PoseMatrix T; - //! @brief Image reprojection error in pixel. + //! @brief Image reprojection error in pixels. double ε; inline CheiralPnPConsistency() = default; @@ -71,14 +84,26 @@ namespace DO::Sara { throw std::runtime_error{ "Error: you must initialize the intrinsic camera parameters!"}; + if (scene_points.cols() != rays.cols()) + throw std::runtime_error{ + "Error: the number of scene points and rays must be equal!"}; + const auto& X_world = scene_points; - const Eigen::MatrixXd X_camera = T * X_world; + auto X_camera = Eigen::MatrixXd{}; + if (X_world.rows() == 3) + X_camera = T * X_world.colwise().homogeneous(); + else if (X_world.rows() == 4) + X_camera = T * X_world; + else + throw std::runtime_error{ + "The dimension of scene points is incorrect. They must either 3D " + "(Euclidean) or 4D (homogeneous)!"}; auto u1 = Eigen::MatrixXd{2, scene_points.cols()}; for (auto i = 0; i < u1.cols(); ++i) u1.col(i) = camera->project(X_camera.col(i)); - auto u2 = Eigen::MatrixXd{rays.rows(), rays.cols()}; + auto u2 = Eigen::MatrixXd{2, rays.cols()}; for (auto i = 0; i < u2.cols(); ++i) u2.col(i) = camera->project(rays.col(i)); @@ -94,7 +119,7 @@ namespace DO::Sara { //! @brief Check the inlier predicate on a list of correspondences. template - inline auto operator()(const PointRayCorrespondenceSubsetList& m) const + inline auto operator()(const PointRayCorrespondenceList& m) const -> Eigen::Array { const auto& scene_points = m.x.colmajor_view().matrix(); @@ -103,4 +128,7 @@ namespace DO::Sara { } }; + + //! @} + } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/RANSAC/RANSAC.hpp b/cpp/src/DO/Sara/RANSAC/RANSAC.hpp index 124af43a3..fe6250bd8 100644 --- a/cpp/src/DO/Sara/RANSAC/RANSAC.hpp +++ b/cpp/src/DO/Sara/RANSAC/RANSAC.hpp @@ -186,9 +186,9 @@ namespace DO::Sara { double confidence = 0.99) -> std::uint64_t { // Check the range of values... - if (!(0 <= inlier_ratio && inlier_ratio < 1)) + if (!(0 <= inlier_ratio && inlier_ratio <= 1)) throw std::runtime_error{ - "Error: the inlier ratio must be in the open interval [0, 1["}; + "Error: the inlier ratio must be in the open interval [0, 1]"}; if (!(0 <= confidence && confidence < 1)) throw std::runtime_error{ "Error: the confidence value must be in the open interval [0, 1["}; diff --git a/cpp/src/DO/Sara/RANSAC/RANSACv2.hpp b/cpp/src/DO/Sara/RANSAC/RANSACv2.hpp index cb4fd1a16..6a7c377ee 100644 --- a/cpp/src/DO/Sara/RANSAC/RANSACv2.hpp +++ b/cpp/src/DO/Sara/RANSAC/RANSACv2.hpp @@ -13,6 +13,8 @@ #include +#include + namespace DO::Sara::v2 { @@ -100,9 +102,9 @@ namespace DO::Sara::v2 { inliers_best.flat_array() = inliers; subset_best = minimal_index_subsets[n]; - // - inlier_ratio_current = - num_inliers / static_cast(data_points.size()); + inlier_ratio_current = std::clamp( + num_inliers / static_cast(data_points.size()), // + 0., 1.); update_num_iterations(); if (verbose) diff --git a/cpp/test/Sara/MultiViewGeometry/SyntheticDataUtilities.hpp b/cpp/test/Sara/MultiViewGeometry/SyntheticDataUtilities.hpp index 7798a696c..47103ea36 100644 --- a/cpp/test/Sara/MultiViewGeometry/SyntheticDataUtilities.hpp +++ b/cpp/test/Sara/MultiViewGeometry/SyntheticDataUtilities.hpp @@ -34,7 +34,8 @@ inline auto make_cube_vertices() return cube; } -inline auto make_planar_chessboard_corners(int rows, int cols, double square_size) +inline auto make_planar_chessboard_corners(int rows, int cols, + double square_size) { auto corners = Eigen::MatrixXd{4, rows * cols}; for (auto y = 0; y < rows; ++y) @@ -47,10 +48,8 @@ inline auto make_planar_chessboard_corners(int rows, int cols, double square_siz inline auto make_relative_motion(double x = 0.1, double y = 0.3, double z = 0.2) -> DO::Sara::Motion { - using namespace DO::Sara; - // Euler composite rotation. - const Eigen::Matrix3d R = rotation(z, y, x); + const Eigen::Matrix3d R = DO::Sara::rotation(z, y, x); // - The axes of the world coordinate system has turned by the following // rotational quantity. // - The columns of R are the vector coordinates of the world axes w.r.t. diff --git a/cpp/test/Sara/RANSAC/CMakeLists.txt b/cpp/test/Sara/RANSAC/CMakeLists.txt index 8dee67c92..3a76f3b67 100644 --- a/cpp/test/Sara/RANSAC/CMakeLists.txt +++ b/cpp/test/Sara/RANSAC/CMakeLists.txt @@ -5,6 +5,6 @@ foreach (file ${test_SOURCE_FILES}) sara_add_test( NAME ${filename} SOURCES ${file} - DEPENDENCIES DO::Sara::MultiViewGeometry DO::Sara::RANSAC + DEPENDENCIES DO::Sara::MultiViewGeometry DO::Sara::RANSAC fmt::fmt FOLDER RANSAC) endforeach ()