Pose3Pose3Offset

Sonar.jl

# BEGIN:
"""
    $(SIGNATURES)

A factor for sensor offset calibration, connecting three variable nodes: platform, sensor, and offset. The measurement should be centered around zero.

# Examples
```julia-repl
```
"""
mutable struct Pose3Pose3Offset <: FunctorPairwise
  diff::IIF.SamplableBelief
  Pose3Pose3Offset = new()
  Pose3Pose3Offset(z::SamplableBelief) = new(z)
end

# get samples from the factor
function getSample(f::Pose3Pose3Offset, N::Int=1)::Tuple{Array{Float64,2}}
  y = rand(diff, N)
  return (y,)
end


# compute the residual based on the current estimates
function( f::Pose3Pose3Offset)(
          res::Vector{Float64},
          userdata::FactorMetadata,
          idx::Int,
          meas::Tuple{Array{Float64,2}},
          platform::Array{Float64,2},
          sensor::Array{Float64,2},
          offset::Array{Float64,2})
  # platform pose
  wTp = compose(Translation(platform[1:3,idx]),RotXYZ(platform[4:6,idx]))
  # sensor pose
  wTs = compose(Translation(sensor[1:3,idx]),RotXYZ(sensor[4:6,idx]))
  # offset
  pTs = compose(Translation(offset[1:3,idx]),RotXYZ(offset[4:6,idx]))
  # identity
  pTp = compose(wTp,compose(inv(wTs),pTs))
  res = [pTp.translation; RotXYZ(pTp.linear)]
end

FunctorPairwise returns ::Void, so you want to do this instead:

function( f::Pose3Pose3Offset)(
          res::Vector{Float64},
          userdata::FactorMetadata,
          idx::Int,
          meas::Tuple{Array{Float64,2}},
          platform::Array{Float64,2},
          sensor::Array{Float64,2},
          offset::Array{Float64,2} )::Void
  # platform pose
  wTp = compose(Translation(platform[1:3,idx]),RotXYZ(platform[4:6,idx]))
  # sensor pose
  wTs = compose(Translation(sensor[1:3,idx]),RotXYZ(sensor[4:6,idx]))
  # offset
  pTs = compose(Translation(offset[1:3,idx]),RotXYZ(offset[4:6,idx]))
  # identity
  pTp = compose(wTp,compose(inv(wTs),pTs))
  res[1] = pTp.translation[1]
  res[2] = pTp.translation[2]
  res[3] = pTp.translation[3]
  Er = RotXYZ(pTp.linear)
  res[4] = Er.theta1
  res[5] = Er.theta2
  res[6] = Er.theta3
  nothing
end

FunctorPairwiseMinimize requires you return one Float with the objective value