Merge pull request #888 from JuliaRobotics/master

v0.13.3-rc1

Author: dehann

.github/workflows/CompatHelper.yml

@@ -1,16 +1,45 @@
 name: CompatHelper
 on:
   schedule:
-    - cron: '00 01 * * *'
+    - cron: 0 0 * * *
   workflow_dispatch:
+permissions:
+  contents: write
+  pull-requests: write
 jobs:
   CompatHelper:
     runs-on: ubuntu-latest
     steps:
-      - name: Pkg.add("CompatHelper")
-        run: julia -e 'using Pkg; Pkg.add("CompatHelper")'
-      - name: CompatHelper.main()
+      - name: Check if Julia is already available in the PATH
+        id: julia_in_path
+        run: which julia
+        continue-on-error: true
+      - name: Install Julia, but only if it is not already available in the PATH
+        uses: julia-actions/setup-julia@v1
+        with:
+          version: '1'
+          # arch: ${{ runner.arch }}
+        if: steps.julia_in_path.outcome != 'success'
+      - name: "Add the General registry via Git"
+        run: |
+          import Pkg
+          ENV["JULIA_PKG_SERVER"] = ""
+          Pkg.Registry.add("General")
+        shell: julia --color=yes {0}
+      - name: "Install CompatHelper"
+        run: |
+          import Pkg
+          name = "CompatHelper"
+          uuid = "aa819f21-2bde-4658-8897-bab36330d9b7"
+          version = "3"
+          Pkg.add(; name, uuid, version)
+        shell: julia --color=yes {0}
+      - name: "Run CompatHelper"
+        run: |
+          import CompatHelper
+          CompatHelper.main()
+        shell: julia --color=yes {0}
         env:
           GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-          COMPATHELPER_PRIV: ${{ secrets.COMPATHELPER_PRIV }}  # optional
-        run: julia -e 'using CompatHelper; CompatHelper.main()'
+          COMPATHELPER_PRIV: ${{ secrets.DOCUMENTER_KEY }}
+          # COMPATHELPER_PRIV: ${{ secrets.COMPATHELPER_PRIV }}

NEWS.md

@@ -11,3 +11,5 @@ Major changes and news in Caesar.jl.
 - Refactored `ScatterAlign` to support both `ScatterAlignPose2` and `ScatterAlignPose3`.
 - Improved `_PCL.apply` to now transform both 2D and 3D pointclouds using `Manifolds.SpecialEuclidean(2)` and `Manifolds.SpecialEuclidean(3)`.
 - Added more testing on `_PCL` conversions and transforms.
+- Added `ICP` for point cloud alignment, (#885, #886).
+- Separated test data to `JuliaRobotics/CaesarTestData.jl` (#885).

Project.toml

@@ -2,7 +2,7 @@ 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.2"
+version = "0.13.3"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -26,16 +26,19 @@ JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
+NearestNeighbors = "b8a86587-4115-5ab1-83bc-aa920d37bbce"
 NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56"
 Optim = "429524aa-4258-5aef-a3af-852621145aeb"
 Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
+Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"
 ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 Requires = "ae029012-a4dd-5104-9daa-d747884805df"
 RoME = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
+StatsBase = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91"
 TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
@@ -81,6 +84,8 @@ julia = "1.6"
 AprilTags = "f0fec3d5-a81e-5a6a-8c28-d2b34f3659de"
 BSON = "fbb218c0-5317-5bc6-957e-2ee96dd4b1f0"
 Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
+DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab"
+Downloads = "f43a241f-c20a-4ad4-852c-f6b1247861c6"
 FixedPointNumbers = "53c48c17-4a7d-5ca2-90c5-79b7896eea93"
 Gadfly = "c91e804a-d5a3-530f-b6f0-dfbca275c004"
 GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231"
@@ -94,4 +99,4 @@ Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 
 [targets]
-test = ["AprilTags", "BSON", "Colors", "FixedPointNumbers", "Gadfly", "GraphPlot", "Images", "PyCall", "Random", "RobotOS", "Serialization", "Test", "ZMQ"]
+test = ["AprilTags", "BSON", "Colors", "DelimitedFiles", "Downloads", "FixedPointNumbers", "Gadfly", "GraphPlot", "Images", "PyCall", "Random", "RobotOS", "Serialization", "Test", "ZMQ"]

docs/Project.toml

@@ -1,6 +1,7 @@
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 Combinatorics = "861a8166-3701-5b0c-9a16-15d98fcdc6aa"
+Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
 CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
 DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8"
 Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"
@@ -15,6 +16,7 @@ FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
 FunctionalStateMachine = "3e9e306e-7e3c-11e9-12d2-8f8f67a2f951"
 ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534"
 ImageMagick = "6218d12a-5da1-5696-b52f-db25d2ecc6d1"
+Images = "916415d5-f1e6-5110-898d-aaa5f9f070e0"
 IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480"
 JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
 JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6"
@@ -40,6 +42,7 @@ ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 [compat]
 ApproxManifoldProducts = "≥ 0.3"
 Combinatorics = "0.7, 1"
+Colors = "≥ 0.12"
 CoordinateTransformations = "≥ 0.5"
 DataStructures = "≥ 0.15"
 DistributedFactorGraphs = "≥ 0.8"
@@ -50,6 +53,7 @@ FileIO = "≥ 1.0"
 FunctionalStateMachine = "≥ 0.1"
 ImageCore = "≥ 0.7"
 ImageMagick = "≥ 0.7"
+Images = "≥ 0.24"
 IncrementalInference = "≥ 0.13"
 JLD2 = "≥ 0.1"
 JSON = "≥ 0.18"

docs/make.jl

@@ -1,5 +1,7 @@
 using Documenter, Caesar
 using RoME
+using Colors
+using Images
 
 import IncrementalInference: fmcmc!, localProduct, prodmultiplefullpartials, prodmultipleonefullpartials, setfreeze!
 import IncrementalInference: cliqGibbs, packFromLocalPotentials!, treeProductDwn, updateFGBT!, upGibbsCliqueDensity
@@ -35,7 +37,6 @@ makedocs(
             "Interact w Graphs" => "concepts/interacting_fgs.md",
             "Multi-Modal/Hypothesis" => "concepts/dataassociation.md",
             "Parallel Processing" => "concepts/parallel_processing.md",
-            "Using Manifolds.jl" => "concepts/using_manifolds.md",
             "[DEV] Parametric Solve" => "examples/parametric_solve.md",
         ],
         "Examples" => [
@@ -51,6 +52,8 @@ makedocs(
             "Variables/Factors" => "concepts/available_varfacs.md",
             "Entry=>Data Blob" => "concepts/entry_data.md",
             "Images and AprilTags" => "examples/using_images.md",
+            "Pointclouds and PCL" => "examples/using_pcl.md",
+            "Using Manifolds.jl" => "concepts/using_manifolds.md",
             "Flux (NN) Factors" => "concepts/flux_factors.md",
         ],
         "Visualization" => [

docs/src/concepts/2d_plotting.md

@@ -1,4 +1,4 @@
-# Plotting 2D
+# [Plotting 2D](@id plotting_2d)
 
 Once the graph has been built, 2D plot visualizations are provided by [RoMEPlotting.jl](http://www.github.com/JuliaRobotics/RoMEPlotting.jl) and [KernelDensityEstimatePlotting.jl](http://www.github.com/JuliaRobotics/KernelDensityEstimatePlotting.jl).  These visualizations tools are readily modifiable to highlight various aspects of mobile platform navigation.
 

docs/src/concepts/arena_visualizations.md

@@ -1,30 +1,51 @@
-# Visualization with Arena.jl
+# [Visualization 3D](@ref visualization_3d)
+
+## Introduction
+
+Over time, Caesar.jl/Arena.jl has used at various different 3D visualization technologies.  Currently work is underway to better standardize within the Julia ecosystem, with the 4th generation of Arena.jl -- note that this is work in progress.  Information about legacy generations is included below.
+
+For more formal visualization support, contact [www.NavAbility.io](http://www.navability.io) via email or slack. 
+## 4th Generation Dev Scripts using Makie.jl
+
+Working towards new [Makie.jl](https://github.com/JuliaPlots/Makie.jl).  Makie supports both GL and WGL, including 3rd party libraries such as [three.js](https://threejs.org/) (previously used via MeshCat.jl, see Legacy section below.).
+### [Visualizing Point Clouds](@id viz_pointcloud_makie)
+
+Point clouds could be massive, on the order of a million points or more.  Makie.jl has good performance for handling such large point cloud datasets.  Here is a quick example script.
+```julia
+using Makie, GLMakie
+
+# n x 3 matrix of 3D points in pointcloud
+pts1 = randn(100,3)
+pts2 = randn(100,3)
+
+# plot first and update with second
+plt = scatter(pts1[:,1],pts1[:,2],pts1[:,3], color=pts1[:,3])
+scatter!(pts2[:,1],pts2[:,2],pts2[:,3], color=-pts2[:,3])
+```
+## Visualizing with Arena.jl
+
+!!! warning
+    Arena.jl is currently out of date since the package will likely support Makie via both GL and WGL interfaces.  Makie.jl has been receiving much attention over the past years and starting to mature to a point where Arena.jl can be revived again.  2D plotting is done via [`RoMEPlotting.jl`](@ref plotting_2d).
 
 The sections below discuss 3D visualization techniques available to the Caesar.jl robot navigation system.
 Caesar.jl uses the [Arena.jl](https://github.com/dehann/Arena.jl) package for all the visualization requirements.  This part of the documentation discusses the robotic visualization aspects supported by Arena.jl.
 Arena.jl supports a wide variety of general visualization as well as developer visualization tools more focused on research and development.
 The visualizations are also intended to help with subgraph plotting for finding loop closures in data or compare two datasets.
 
-!!! warning
-
-    Arena and Amphitheater are being upgraded as part of the broader migration to DistributedFactorGraphs (1Q20)
+## Legacy Visualizers
 
-## Introduction
-
-Over time, Caesar.jl/Arena.jl has used a least three different 3D visualization technologies, with the most recent based on WebGL and [three.js](https://threejs.org/) by means of the [MeshCat.jl](https://github.com/rdeits/MeshCat.jl) package.
-The previous incarnation used a client side installation of [VTK](https://www.vtk.org/)  by means of the [DrakeVisualizer.jl](https://github.com/rdeits/DrakeVisualizer.jl) and [Director](https://github.com/RobotLocomotion/director) libraries.
+Previous generations used various technologies, including WebGL and [three.js](https://threejs.org/) by means of the [MeshCat.jl](https://github.com/rdeits/MeshCat.jl) package.
+Previous incarnations used a client side installation of [VTK](https://www.vtk.org/)  by means of the [DrakeVisualizer.jl](https://github.com/rdeits/DrakeVisualizer.jl) and [Director](https://github.com/RobotLocomotion/director) libraries.
 Different 2D plotting libraries have also been used, with evolutions to improve usability for a wider user base.
 Each epoch has been aimed at reducing dependencies and increasing multi-platform support.
 
-!!! note
+### 3rd Generation MeshCat.jl (Three.js)
 
-    See [installation page](https://juliarobotics.org/Caesar.jl/latest/installation_environment/#Install-Visualization-Tools-1) for instructions.
 
-## 3D Visualization
+For the latest work on using MeshCat.jl, see proof or concept examples in Amphitheater.jl (1Q20).  The code below inspired the Amphitheater work.
 
-!!! warning
-
-    This section is out of date, see proof or concept examples in Amphitheater.jl (1Q20).
+!!! note
+    See [installation page](https://juliarobotics.org/Caesar.jl/latest/installation_environment/#Install-Visualization-Tools-1) for instructions.
 
 Factor graphs of two or three dimensions can be visualized with the 3D visualizations provided by Arena.jl and it's dependencies.
 The 2D example above and also be visualized in a 3D space with the commands:
@@ -65,9 +86,18 @@ visualizeDensityMesh!(vc, fg, :l1)
 # visualizeallposes!(vc, fg, drawlandms=false)
 ```
 
-## Previous 3D Viewer (VTK / Director) -- no longer required
+For more information see [JuliaRobotcs/MeshCat.jl](https://github.com/rdeits/MeshCat.jl).
+
+### 2nd Generation 3D Viewer (VTK / Director)
+
+!!! note
+    This code is obsolete
 
 Previous versions used the much larger VTK based Director available via [DrakeVisualizer.jl package](https://github.com/rdeits/DrakeVisualizer.jl).  This requires the following preinstalled packages:
 ```bash
     sudo apt-get install libvtk5-qt4-dev python-vtk
 ```
+
+### 1st Generation MIT LCM Collections viewer
+
+This code has been removed.

docs/src/concepts/solving_graphs.md

@@ -27,4 +27,18 @@ When building sysmtes with limited computation resources, the out-marginalizatio
 defaultFixedLagOnTree!(fg, 50, limitfixeddown=true)
 ```
 
-This call will keep the latest 50 variables fluid for inference during Bayes tree inference.  The keyword `limitfixeddown=true` in this case will also prevent downward message passing on the Bayes tree from propagating into the out-marginalized branches on the tree.  A later page in this documentation will discuss how the inference algorithm and Bayes tree aspects are put together.
+This call will keep the latest 50 variables fluid for inference during Bayes tree inference.  The keyword `limitfixeddown=true` in this case will also prevent downward message passing on the Bayes tree from propagating into the out-marginalized branches on the tree.  A later page in this documentation will discuss how the inference algorithm and Bayes tree aspects are put together.
+
+## [Synchronizing Over a Factor Graph](@id sync_over_graph_solvable)
+
+When adding Variables and Factors, use `solvable=0` to disable the new fragments until ready for inference, for example
+```julia
+addVariable!(fg, :x45, Pose2, solvable=0)
+newfct = addFactor!(fg, [:x11,:x12], Pose2Pose2, solvable=0)
+```
+
+These parts of the factor graph can simply be activated for solving:
+```julia
+setSolvable!(fg, :x45, 1)
+setSolvable!(fg, newfct.label, 1)
+```

docs/src/concepts/using_manifolds.md

@@ -81,13 +81,20 @@ The best way to show this is just dive straight into a factor that actually uses
 ### Existing Manifolds
 
 The most popular Manifolds used in Caesar.jl related packages are:
-- [`Sphere(N)`](https://juliamanifolds.github.io/Manifolds.jl/stable/manifolds/sphere.html#Manifolds.Sphere) WORK IN PROGRESS.
+
+#### Group Manifolds
+
 - [`TranslationGroup(N)`](https://juliamanifolds.github.io/Manifolds.jl/stable/manifolds/group.html#Manifolds.TranslationGroup) (future work will relax to `Euclidean(N)`).
 - [`SpecialOrthogonal(N)`](https://juliamanifolds.github.io/Manifolds.jl/stable/manifolds/group.html#Manifolds.SpecialOrthogonal).
 - [`SpecialEuclidean(N)`](https://juliamanifolds.github.io/Manifolds.jl/stable/manifolds/group.html#Special-Euclidean-group).
 - `_CircleEuclid` LEGACY, TODO.
 - `AMP.SE2_E2` LEGACY, TODO.
 
+#### Riemannian Manifolds (Work in progress)
+
+- [`Sphere(N)`](https://juliamanifolds.github.io/Manifolds.jl/stable/manifolds/sphere.html#Manifolds.Sphere) WORK IN PROGRESS.
+
+
 !!! note
     Caesar.jl encourages the JuliaManifolds approach to defining new manifolds, and can readily be used for Caesar.jl related operations.
 

docs/src/examples/using_images.md

@@ -1,4 +1,4 @@
-# Images and Fiducials
+# [Images and Fiducials](@id images_and_fiducials)
 
 ## AprilTags
 
@@ -28,3 +28,7 @@ bytes = Caesar.toFormat(format"PNG", img)
 
 !!! note
     More details to follow.
+
+### Images enables `ScatterAlign`
+
+See [point cloud alignment page for details on `ScatterAlignPose`](@ref sec_scatter_align)

docs/src/examples/using_pcl.md

@@ -0,0 +1,73 @@
+# Pointclouds and PCL Types
+
+## Introduction Caesar._PCL
+
+A wide ranging and well used [point cloud library exists called PCL](https://pointclouds.org/) which is implemented in C++.  To get access to many of those features and bridge the Caesar.jl suite of packages, the base `PCL.PointCloud` types have been implemented in Julia and reside under `Caesar._PCL`.  The main types of interest:
+- [`Caesar._PCL.PointCloud`](@ref),
+- [`Caesar._PCL.PCLPointCloud2`](@ref),
+- [`Caesar._PCL.PointXYZ`](@ref),
+- [`Caesar._PCL.Header`](@ref),
+- [`Caesar._PCL.PointField`](@ref),
+- [`Caesar._PCL.FieldMapper`](@ref).
+
+These types are conditionally loaded once `Colors.jl` is included (see [Requires.jl](https://github.com/JuliaPackaging/Requires.jl)),
+```julia
+using Colors, Caesar
+using StaticArrays
+
+# one point
+x,y,z,intens = 1f0,0,0,1
+pt = Caesar._PCL.PointXYZ(;data=SA[x,y,z,intens])
+
+# etc.
+```
+
+```@docs
+Caesar._PCL.PointCloud
+```
+
+## Conversion with `ROS.PointCloud2`
+
+Strong integration between PCL and [ROS](http://www.ros.org) predominantly through the message types
+- `@rosimport std_msgs.msg: Header`, `@rosimport sensor_msgs.msg: PointField`, `@rosimport sensor_msgs.msg: PointCloud2`.
+
+These have been integrated through conversions to equivalent Julian types already listed above.  ROS conversions requires RobotOS.jl be loaded, see page on using [ROS Direct](@ref ros_direct).
+
+```@docs
+Caesar._PCL.PCLPointCloud2
+Caesar._PCL.PointXYZ
+Caesar._PCL.Header
+Caesar._PCL.PointField
+Caesar._PCL.FieldMapper
+```
+
+## Aligning Point Clouds
+
+Caesar.jl is currently growing support for two related point cloud alignment methods, namely:
+- Continuous density function alignment [`ScatterAlignPose2`](@ref), [`ScatterAlignPose3`](@ref),
+- Traditional Iterated Closest Point (with normals) [`alignICP_Simple`](@ref).
+
+### [`ScatterAlign` for `Pose2` and `Pose3`](@id sec_scatter_align)
+
+These factors use minimum mean distance embeddings to cost the alignment between pointclouds and supports various other interesting function alignment cases.  These functions require `Images.jl`, see page [Using Images](@ref images_and_fiducials).
+
+```@docs
+Caesar.ScatterAlign
+Caesar.ScatterAlignPose2
+Caesar.ScatterAlignPose3
+```
+
+!!! note
+    Future work may include `ScatterAlignPose2z`, please open issues at Caesar.jl if this is of interest.
+
+### Iterative Closest Point
+
+Ongoing work is integrating ICP into a factor similar to `ScatterAlign`.
+
+```@docs
+Caesar._PCL.alignICP_Simple
+```
+
+## Visualizing Point Clouds
+
+See work in progress on alng with example code on the page [3D Visualization](@ref viz_pointcloud_makie).