JuliaRobotics · dehann · Sep 28, 2022 · Aug 1, 2022 · Aug 2, 2022 · Aug 8, 2022
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -7,7 +7,7 @@ on:
       - develop
       - release**
 jobs:
-  test:
+  basic-functional:
     name: Julia ${{ matrix.version }} - ${{ matrix.arch }} - ${{ matrix.group }} - ${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     env:
@@ -16,16 +16,14 @@ jobs:
       fail-fast: false
       matrix:
         version:
-          - '1.6'
-          - '1.7'
+          - '1.8'
           - 'nightly'
         os:
           - ubuntu-latest
         arch:
           - x64
         group:
           - 'basic_functional_group'
-          - 'test_cases_group'
     steps:
       - uses: actions/checkout@v2
       - uses: julia-actions/setup-julia@v1
@@ -55,7 +53,53 @@ jobs:
         with:
           file: lcov.info
 
-  test-masters:
+  test-cases:
+    name: Julia ${{ matrix.version }} - ${{ matrix.arch }} - ${{ matrix.group }} - ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    env:
+      JULIA_PKG_SERVER: ""
+    strategy:
+      fail-fast: false
+      matrix:
+        version:
+          - '1.8'
+        os:
+          - ubuntu-latest
+        arch:
+          - x64
+        group:
+          - 'test_cases_group'
+    continue-on-error: true
+    steps:
+      - uses: actions/checkout@v2
+      - uses: julia-actions/setup-julia@v1
+        with:
+          version: ${{ matrix.version }}
+          arch: ${{ matrix.arch }}
+      - uses: actions/cache@v1
+        env:
+          cache-name: cache-artifacts
+        with:
+          path: ~/.julia/artifacts
+          key: ${{ runner.os }}-test-${{ env.cache-name }}-${{ hashFiles('**/Project.toml') }}
+          restore-keys: |
+            ${{ runner.os }}-test-${{ env.cache-name }}-
+            ${{ runner.os }}-test-
+            ${{ runner.os }}-
+      - uses: julia-actions/julia-buildpkg@latest
+      - run: |
+          git config --global user.name Tester
+          git config --global user.email te@st.er
+      - uses: julia-actions/julia-runtest@latest
+        continue-on-error: ${{ matrix.version == 'nightly' }}
+        env:
+          IIF_TEST_GROUP: ${{ matrix.group }}
+      # - uses: julia-actions/julia-processcoverage@v1
+      # - uses: codecov/codecov-action@v1
+      #   with:
+      #     file: lcov.info
+
+  test-dev-main:
     #if: github.ref != 'refs/heads/release**'
     name: Upstream Dev
     runs-on: ubuntu-latest
@@ -66,7 +110,7 @@ jobs:
 
       - uses: julia-actions/setup-julia@v1
         with:
-          version: 1.7
+          version: 1.8
           arch: x64
 
       - uses: actions/cache@v1
@@ -107,7 +151,7 @@ jobs:
       - uses: actions/checkout@v2
       - uses: julia-actions/setup-julia@v1
         with:
-          version: 1.7
+          version: 1.8
       - name: 'Docs on ${{ github.head_ref }}'
         run: |
           export JULIA_PKG_SERVER=""

diff --git a/Project.toml b/Project.toml
@@ -2,10 +2,11 @@ name = "Caesar"
 uuid = "62eebf14-49bc-5f46-9df9-f7b7ef379406"
 keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics", "ROS"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.13.4"
+version = "0.13.5"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
+Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
 Combinatorics = "861a8166-3701-5b0c-9a16-15d98fcdc6aa"
 CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
 DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8"
@@ -59,7 +60,7 @@ FFTW = "1"
 FileIO = "1"
 ImageCore = "0.8, 0.9"
 ImageMagick = "1"
-IncrementalInference = "0.30"
+IncrementalInference = "0.30, 0.31"
 JLD2 = "0.3, 0.4"
 JSON = "0.20, 0.21"
 JSON2 = "0.3, 0.4"
@@ -71,7 +72,7 @@ Optim = "1"
 ProgressMeter = "1"
 Reexport = "1"
 Requires = "1"
-RoME = "0.20"
+RoME = "0.20, 0.21"
 Rotations = "1.1"
 StaticArrays = "1"
 StatsBase = "0.33"

diff --git a/docs/src/examples/custom_factor_features.md b/docs/src/examples/custom_factor_features.md
@@ -20,8 +20,8 @@ The MM-iSAMv2 algorithm relies on the Kolmogorov-Criteria as well as uncorrelate
 
 At present `cfo` contains three main fields:
 - `cfo.factor::MyFactor` the factor object as defined in the `struct` definition,
-- `cfo.metadata::FactorMetadata`, which is currently under development and likely to change.
-  - This contains references to the connected variables to the factor and more, and is useful for large data retrieval such as used in Terrain Relative Navigation (TRN).
+- `cfo.fullvariables`, which can be used for large data blob retrieval such as used in Terrain Relative Navigation (TRN).
+  - Also see [Stashing and Caching](@ref section_stash_and_cache)
 - `cfo._sampleIdx` is the index of which computational sample is currently being calculated.
 
 

diff --git a/docs/src/refs/literature.md b/docs/src/refs/literature.md
@@ -10,7 +10,7 @@ Newly created page to list related references and additional literature pertaini
 
 [1.3]  Fourie, D., Claassens, S., Pillai, S., Mata, R., Leonard, J.: ["SLAMinDB: Centralized graph databases for mobile robotics"](http://people.csail.mit.edu/spillai/projects/cloud-graphs/2017-icra-cloudgraphs.pdf), IEEE Intl. Conf. on Robotics and Automation (ICRA), Singapore, 2017.
 
-[1.4]  Cheung, M., Fourie, D., Rypkema, N., Vaz Teixeira, P., Schmidt, H., and Leonard, J.: ["Non-Gaussian SLAM utilizing Synthetic Aperture Sonar"](https://marinerobotics.mit.edu/sites/default/files/cheung_icra2019.pdf), Intl. Conf. On Robotics and Automation (ICRA), IEEE, Montreal, 2019.
+[1.4]  Cheung, M., Fourie, D., Rypkema, N., Vaz Teixeira, P., Schmidt, H., and Leonard, J.: ["Non-Gaussian SLAM utilizing Synthetic Aperture Sonar"](https://ieeexplore.ieee.org/document/8793536), Intl. Conf. On Robotics and Automation (ICRA), IEEE, Montreal, 2019.
 
 [1.5]  Doherty, K., Fourie, D., Leonard, J.: ["Multimodal Semantic SLAM with Probabilistic Data Association"](https://dspace.mit.edu/bitstream/handle/1721.1/137995.2/doherty_icra2019_revised.pdf?sequence=4&isAllowed=y), Intl. Conf. On Robotics and Automation (ICRA), IEEE, Montreal, 2019.
 
@@ -58,7 +58,7 @@ Newly created page to list related references and additional literature pertaini
 
 [2.14] Arnborg, S., Corneil, D.G. and Proskurowski, A., 1987. ["Complexity of finding embeddings in a k-tree"](https://epubs.siam.org/doi/pdf/10.1137/0608024). SIAM Journal on Algebraic Discrete Methods, 8(2), pp.277-284.
 
-[2.15a] Sola, J., Deray, J. and Atchuthan, D., 2018. ["A micro Lie theory for state estimation in robotics". arXiv preprint arXiv:1812.01537](https://arxiv.org/pdf/1812.01537), and [tech report](https://upcommons.upc.edu/bitstream/handle/2117/179757/2089-A-micro-Lie-theory-for-state-estimation-in-robotics%20(3).pdf).
+[2.15a] Sola, J., Deray, J. and Atchuthan, D., 2018. ["A micro Lie theory for state estimation in robotics". arXiv preprint arXiv:1812.01537](https://arxiv.org/pdf/1812.01537), and [tech report](https://upcommons.upc.edu/bitstream/handle/2117/179757/2089-A-micro-Lie-theory-for-state-estimation-in-robotics%20(3).pdf). And [cheatsheet w/ suspected typos](https://github.com/artivis/manif/blob/devel/paper/Lie_theory_cheat_sheet.pdf).
 
 [2.15b] Delleart F., 2012. [Lie Groups for Beginners](https://raw.githubusercontent.com/devbharat/gtsam/master/doc/LieGroups.pdf).
 

diff --git a/src/3rdParty/_PCL/_PCL.jl b/src/3rdParty/_PCL/_PCL.jl
@@ -40,6 +40,7 @@ include("services/ICP_Simple.jl")
 
 function __init__()
   @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("services/ROSConversions.jl")
+  @require Gadfly="c91e804a-d5a3-530f-b6f0-dfbca275c004" include("services/PlottingUtilsPCL.jl")
 end
 
 

diff --git a/src/3rdParty/_PCL/entities/PCLTypes.jl b/src/3rdParty/_PCL/entities/PCLTypes.jl
@@ -4,6 +4,7 @@
   Point Cloud Library (PCL) - www.pointclouds.org
   Copyright (c) 2010, Willow Garage, Inc.
   Copyright (c) 2012-, Open Perception, Inc.
+  Copyright (c) 2022, NavAbility, Inc.
 
   All rights reserved.
 
@@ -277,4 +278,4 @@ end
 
 
 
-#
+#
diff --git a/src/3rdParty/_PCL/services/ConsolidateRigidTransform.jl b/src/3rdParty/_PCL/services/ConsolidateRigidTransform.jl
@@ -46,16 +46,28 @@ end
 ## FIXME, not-yet-consolidated rigid transform code that must be deprecated below
 ## ============================================================
 
+const _SO3_MANI = SpecialOrthogonal(3)
+const _SE3_MANI = SpecialEuclidean(3)
+
 # name euler here is very ambiguous, these are Lie algebra elements
 # used to manipulate cartesian coordinates in a TranslationGroup(3) space.
-function euler_angles_to_linearized_rotation_matrix(α1, α2, α3)
-  dR = [  1 -α3  α2
-         α3   1 -α1
-        -α2  α1   1]
+function euler_angles_to_linearized_rotation_matrix(α1, α2, α3, rigid::Bool=true)
+  dR = if rigid
+    # TODO likely faster performance by using a retraction instead of expmap
+    exp_lie(_SO3_MANI, hat(_SO3_MANI, SMatrix{3,3, Float64}(I), SA[α1, α2, α3]))
+  else
+    SMatrix{3,3,Float64}(1.0,0,0,0,1,0,0,0,1) + 
+      hat(_SO3_MANI, Identity(_SO3_MANI), SA[α1, α2, α3])
+    # [ 1 -α3  α2
+    #   α3   1 -α1
+    #  -α2  α1   1]
+  end
 end
+
 function create_homogeneous_transformation_matrix(R, t)
-  H = [R          t
-       zeros(1,3) 1]
+  H = affine_matrix(_SE3_MANI, ArrayPartition(t, R))
+  # H = [R          t
+  #      zeros(1,3) 1]
 end
 function euler_coord_to_homogeneous_coord(XE)
   no_points = size(XE, 1)

diff --git a/src/3rdParty/_PCL/services/ICP_Simple.jl b/src/3rdParty/_PCL/services/ICP_Simple.jl
@@ -221,7 +221,7 @@ Downloads.download(lidar2_url, io2)
 X_fix = readdlm(io1)
 X_mov = readdlm(io2)
 
-H, HX_mov = Caesar._PCL.alignICP_Simple(X_fix, X_mov; verbose=true)
+H, HX_mov, stat = Caesar._PCL.alignICP_Simple(X_fix, X_mov; verbose=true)
 ```
 
 Notes
@@ -244,16 +244,17 @@ function alignICP_Simple(
     max_overlap_distance::Number=Inf,
     min_change::Number=3,
     max_iterations::Integer=100,
-    verbose::Bool=false 
+    verbose::Bool=false,
+    H = Matrix{Float64}(I,4,4),
   )
   #
   size(X_fix)[2] == 3 || error(""""X_fix" must have 3 columns""")
   size(X_mov)[2] == 3 || error(""""X_mov" must have 3 columns""")
-  correspondences >= 10 || error(""""correspondences" must be >= 10""")
-  min_planarity >= 0 && min_planarity < 1 || error(""""min_planarity" must be >= 0 and < 1""")
-  neighbors >= 2 || error(""""neighbors" must be >= 2""")
-  min_change > 0 || error(""""min_change" must be > 0""")
-  max_iterations > 0 || error(""""max_iterations" must be > 0""")
+  10 <= correspondences || error(""""correspondences" must be >= 10""")
+  0 <= min_planarity < 1 || error(""""min_planarity" must be >= 0 and < 1""")
+  2 <= neighbors || error(""""neighbors" must be >= 2""")
+  0 < min_change || error(""""min_change" must be > 0""")
+  0 < max_iterations || error(""""max_iterations" must be > 0""")
 
   dt = @elapsed begin
     verbose && @info "Create point cloud objects ..."
@@ -276,11 +277,12 @@ function alignICP_Simple(
     verbose && @info "Estimate normals of selected points ..."
     estimate_normals!(pcfix, neighbors)
 
-    H = Matrix{Float64}(I,4,4)
     residual_distances = Any[]
 
     verbose && @info "Start iterations ..."
+    count = 0
     for i in 1:max_iterations
+      count += 1
       initial_distances = matching!(pcmov, pcfix)
       reject!(pcmov, pcfix, min_planarity, initial_distances)
       dH, residuals = estimate_rigid_body_transformation(
@@ -290,7 +292,7 @@ function alignICP_Simple(
 
       push!(residual_distances, residuals)
       transform!(pcmov, dH)
-      H = dH*H
+      H .= dH*H
       pcfix.sel = sel_orig
       if i > 1
         if check_convergence_criteria(residual_distances[i], residual_distances[i-1],
@@ -324,7 +326,8 @@ function alignICP_Simple(
 
   X_mov_transformed = [pcmov.x pcmov.y pcmov.z]
 
-  return H, X_mov_transformed
+  stat = (;residual_mean=mean(residual_distances[end]), correspondences=length(residual_distances[end]),residual_std=std(residual_distances[end]), iterations=count)
+  return H, X_mov_transformed, stat
 end
 
 

diff --git a/src/3rdParty/_PCL/services/PlottingUtilsPCL.jl b/src/3rdParty/_PCL/services/PlottingUtilsPCL.jl
@@ -0,0 +1,16 @@
+
+@info "Caesar._PCL is loading tools related to Gadfly.jl."
+
+# import ..Gadfly as GF
+
+## =====================================================================================
+## _PCL plotting utils
+## =====================================================================================
+
+# NOTE moved pointcloud plotting to and see PR Arena.jl#94
+function plotPointCloud(pc::PointCloud)
+  x = (s->s.data[1]).(pc.points)
+  y = (s->s.data[2]).(pc.points)
+
+  Main.Gadfly.plot(x=x,y=y, Main.Gadfly.Geom.point)
+end
diff --git a/src/3rdParty/_PCL/services/PointCloud.jl b/src/3rdParty/_PCL/services/PointCloud.jl
@@ -359,6 +359,21 @@ function PCLPointCloud2(cloud::PointCloud{T,P,R}; datatype = _PCL_FLOAT32) where
   )
 end
 
+## =========================================================================================================
+## Useful utils
+## =========================================================================================================
+
+
+function _filterMinRange(
+  pts::AbstractVector{<:AbstractVector{<:Real}}, 
+  minrange::Real, 
+  maxrange::Real
+)
+  # filter pointcloud section of interest
+  msk = findall(x-> minrange < norm(x) < maxrange, pts)
+  pts[msk]
+end
+
 
 ## =========================================================================================================
 ## Custom printing

diff --git a/src/Caesar.jl b/src/Caesar.jl
@@ -31,6 +31,7 @@ using
   CoordinateTransformations,
   JSON,
   JSON2,
+  Base64,
   FileIO,
   DataStructures,
   ProgressMeter,
@@ -100,6 +101,7 @@ function __init__()
     include("images/ScatterAlignPose2.jl")
 
     @require Gadfly="c91e804a-d5a3-530f-b6f0-dfbca275c004" include("plotting/ScatterAlignPlotting.jl")
+    @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("images/ROSConversions.jl")
   end
   @require Distributed="8ba89e20-285c-5b6f-9357-94700520ee1b" include("images/DistributedUtils.jl")
 end

diff --git a/src/beamforming/SASBearing2D.jl b/src/beamforming/SASBearing2D.jl
@@ -120,14 +120,14 @@ function (cfo::CalcFactor{<:SASBearing2D})( meas,
                                             XX...  )
   #
   sas2d = cfo.factor
-  userdata = cfo.metadata
+  # userdata = cfo.metadata # OUT OF DATE, use CalcFactor.fullvariables
   idx = cfo._sampleIdx
   #
   dx, dy, azi = 0.0, 0.0, 0.0
   thread_data = sas2d.threadreuse[Threads.threadid()]
   res = [0.0;]
 
-  if string(userdata.solvefor)[1] == 'l'
+  if cf.solvefor == 1
     # Solving all poses to Landmark
     # Reference the filter positions locally (first element)
 
@@ -140,8 +140,8 @@ function (cfo::CalcFactor{<:SASBearing2D})( meas,
       # ccall(:jl_, Void, (Any,), "idx=$(idx)\n")
       thread_data.oncebackidx[1] = idx
       #run for eacn new idx
-      thread_data.workvarlist = userdata.variablelist[2:end] # skip landmark element
-      thread_data.looelement = findfirst(thread_data.workvarlist .== userdata.solvefor)
+      thread_data.workvarlist = cfo.variablelist[2:end] # skip landmark element
+      thread_data.looelement = findfirst(thread_data.workvarlist .== cfo.solvefor)
       thread_data.elementset = setdiff(Int[1:length(thread_data.workvarlist);],   [thread_data.looelement;])
 
       # Reference first or second element
-Original file line number
+Diff line change
@@ Expand Up / @@ -40,6 +40,7 @@ include("services/ICP_Simple.jl") @@
     function __init__()
       @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("services/ROSConversions.jl")
+      @require Gadfly="c91e804a-d5a3-530f-b6f0-dfbca275c004" include("services/PlottingUtilsPCL.jl")
     end
@@ Expand Down @@