Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

use ApproxManiProds.skew instead #747

Merged
merged 2 commits into from
Aug 13, 2024
Merged

use ApproxManiProds.skew instead #747

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8cad499
use ApproxManiProds.skew instead
dehann Aug 8, 2024
f8a79ad
Update src/factors/Inertial/IMUDeltaFactor.jl
dehann Aug 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Project.toml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ RoMEFluxExt = "Flux"
RoMEImageIOExt = "ImageIO"

[compat]
ApproxManifoldProducts = "0.8"
ApproxManifoldProducts = "0.8.5"
CameraModels = "0.2"
CoordinateTransformations = "0.5, 0.6"
Dates = "1.10"
Expand Down
33 changes: 11 additions & 22 deletions src/factors/Inertial/IMUDeltaFactor.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,20 +5,6 @@ using LinearAlgebra
using DistributedFactorGraphs
using Dates

# FIXME use ApproxManifoldsProducts version instead
Affie marked this conversation as resolved.
Show resolved Hide resolved
function TransformUtils.skew(v::SVector{3,T}) where T<:Real
return SMatrix{3,3,T}(
0,
v[3],
-v[2],
-v[3],
0,
v[1],
v[2],
-v[1],
0
)
end

struct IMUDeltaManifold <: AbstractManifold{ℝ} end

Expand Down Expand Up @@ -101,7 +87,7 @@ end

function Manifolds.hat(M::IMUDeltaGroup, Xⁱ::SVector{10, T}) where T<:Real
return ArrayPartition(
skew(Xⁱ[SA[7:9...]]), # θ ωΔt
ApproxManifoldProducts.skew(Xⁱ[SA[7:9...]]), # θ ωΔt
Xⁱ[SA[4:6...]], # ν aΔt
Xⁱ[SA[1:3...]], # ρ vΔt
Xⁱ[10], # Δt
Expand Down Expand Up @@ -131,7 +117,7 @@ function _Q(θ⃗)
else
u = θ⃗/θ
sθ, cθ = sincos(θ)
uₓ = skew(u)
uₓ = ApproxManifoldProducts.skew(u)
# NOTE difference in references here --- (θ - sθ)/θ^2 vs (θ - sθ)/θ
# with no ^2 looking correct when compared to exp of SE3
return SMatrix{3,3,T}(I) + (1 - cθ)/θ * uₓ + (θ - sθ)/θ * uₓ^2
Expand All @@ -146,7 +132,7 @@ function _P(θ⃗)
else
u = θ⃗/θ
sθ, cθ = sincos(θ)
uₓ = skew(u)
uₓ = ApproxManifoldProducts.skew(u)
return 1/2*SMatrix{3,3,T}(I) + (θ - sθ)/θ^2 * uₓ + (cθ + 1/2*θ^2 - 1)/θ^2 * uₓ^2
end
end
Expand Down Expand Up @@ -255,8 +241,8 @@ function adjointMatrix(::IMUDeltaGroup, X::ArrayPartition{T}) where T

IΔt = SMatrix{3,3,T}(X.x[4]*I)

ρₓ = skew(ρ)
νₓ = skew(ν)
ρₓ = ApproxManifoldProducts.skew(ρ)
νₓ = ApproxManifoldProducts.skew(ν)

m0 = zeros(3,3)
v0 = zeros(3)
Expand All @@ -277,8 +263,8 @@ function AdjointMatrix(::IMUDeltaGroup, p::ArrayPartition{T}) where T
Δp = p.x[3]
Δt = p.x[4]

Δvₓ = skew(Δv)
pmvtₓ = skew(Δp - Δv*Δt)
Δvₓ = ApproxManifoldProducts.skew(Δv)
pmvtₓ = ApproxManifoldProducts.skew(Δp - Δv*Δt)

m0 = zeros(3,3)
v0 = zeros(3)
Expand Down Expand Up @@ -324,7 +310,7 @@ Base.@kwdef struct IMUDeltaFactor{T <: SamplableBelief} <: AbstractManifoldMinim
J_b::SMatrix{10,6,Float64} = zeros(SMatrix{10,6,Float64})
# accelerometer bias, gyroscope bias
b::SVector{6, Float64} = zeros(SVector{6, Float64})
#optional raw measurements
#optional raw measurements, FIXME replace with BlobEntry -- dont do abstract types here, or raw data if not needed in hotloop
Affie marked this conversation as resolved.
Show resolved Hide resolved
raw_measurements::Union{Nothing,IMUMeasurements} = nothing
end

Expand All @@ -340,6 +326,9 @@ end
IIF.getManifold(::IMUDeltaFactor) = IMUDeltaGroup()

function IIF.preambleCache(fg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, ::IMUDeltaFactor)
# FIXME, nsec should only contain fractional second information, i.e. < 1.0s. Use together with `trunc(timestamp) + 1e-9*nsec`
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That is not how I use it and not how its documented.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Again, we should do what ROS does and not our own special thing. By the way, the suggested code here works for both cases. We should also update our documentation to reflect the same as ROS.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We had this discussion before and also didn't agree then.
If you want to do what ros does:
Split the timestamp in two 32 bit timestamps, one for your ns and the other for the second part. The 2 combined as a single 64bit timestamp is basically what you have here.

By the way, the suggested code here works for both cases. We should also update our documentation to reflect the same as ROS.

I feel like a parrot, but it doesn't, I've had multiple cases where there are 2 different timestamps. Not all timestamps are synced, and not all timestamps are unix epoch.

This comment was marked as duplicate.

Sign in to view

Copy link
Member Author

@dehann dehann Aug 8, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Member Author

@dehann dehann Aug 8, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I feel like a parrot, but it doesn't, I've had multiple cases where there are 2 different timestamps. Not all timestamps are synced, and not all timestamps are unix epoch.

So what I'm looking to get right here is that this should never be allowed to happen. Sounds like imposing a restriction (along with .missionnanosec) would be the only way to avoid this?

Synchronization may well be a session-delta (or clone of session to resolve). Ultimately the unsync'd graph variables (or whole graph fragment) should be replaced so that only consistent timestamps remain. We should not kick the can down and have users struggle with timestamps downstream.

Copy link
Member Author

@dehann dehann Aug 8, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

32 bit timestamps

I mean that we stay with 64 bit (obviously).

Note some web stacks only support 32 bit at this time. Some of the json libraries was the example we bumped into before. In that case we should then still be good, but would not be our "fault" if it happens somewhere.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

updated the FIXME comment to be inline with missionnanosec.

dehann marked this conversation as resolved.
Show resolved Hide resolved
# pt = floor(Float64, datetime2unix(getTimestamp(vars[1]))) + (1e-9*vars[1].nstime % 1.0)
# qt = floor(Float64, datetime2unix(getTimestamp(vars[2]))) + (1e-9*vars[2].nstime % 1.0)
(timestams=(vars[1].nstime,vars[2].nstime),)
end

Expand Down
Loading