Merge pull request #106 from JuliaRobotics/testing/addfixedlag

Testing/addfixedlag

LICENSE

@@ -1,21 +1,158 @@
-The MIT License (MIT)
-
-Copyright (c) 2016 
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
+Apache License
+
+Version 2.0, January 2004
+
+http://www.apache.org/licenses/
+
+TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+1. Definitions.
+
+"License" shall mean the terms and conditions for use, reproduction, and
+distribution as defined by Sections 1 through 9 of this document.
+
+"Licensor" shall mean the copyright owner or entity authorized by the copyright
+owner that is granting the License.
+
+"Legal Entity" shall mean the union of the acting entity and all other entities
+that control, are controlled by, or are under common control with that entity.
+For the purposes of this definition, "control" means (i) the power, direct or
+indirect, to cause the direction or management of such entity, whether by
+contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the
+outstanding shares, or (iii) beneficial ownership of such entity.
+
+"You" (or "Your") shall mean an individual or Legal Entity exercising
+permissions granted by this License.
+
+"Source" form shall mean the preferred form for making modifications, including
+but not limited to software source code, documentation source, and configuration
+files.
+
+"Object" form shall mean any form resulting from mechanical transformation or
+translation of a Source form, including but not limited to compiled object code,
+generated documentation, and conversions to other media types.
+
+"Work" shall mean the work of authorship, whether in Source or Object form, made
+available under the License, as indicated by a copyright notice that is included
+ in or attached to the work (an example is provided in the Appendix below).
+
+"Derivative Works" shall mean any work, whether in Source or Object form, that
+is based on (or derived from) the Work and for which the editorial revisions,
+annotations, elaborations, or other modifications represent, as a whole, an
+original work of authorship. For the purposes of this License, Derivative Works
+shall not include works that remain separable from, or merely link (or bind by
+name) to the interfaces of, the Work and Derivative Works thereof.
+
+"Contribution" shall mean any work of authorship, including the original version
+ of the Work and any modifications or additions to that Work or Derivative Works
+thereof, that is intentionally submitted to Licensor for inclusion in the Work
+by the copyright owner or by an individual or Legal Entity authorized to submit
+on behalf of the copyright owner. For the purposes of this definition,
+"submitted" means any form of electronic, verbal, or written communication sent
+to the Licensor or its representatives, including but not limited to
+communication on electronic mailing lists, source code control systems, and
+issue tracking systems that are managed by, or on behalf of, the Licensor for
+the purpose of discussing and improving the Work, but excluding communication
+that is conspicuously marked or otherwise designated in writing by the
+copyright owner as "Not a Contribution."
+
+"Contributor" shall mean Licensor and any individual or Legal Entity on behalf
+of whom a Contribution has been received by Licensor and subsequently
+incorporated within the Work.
+
+2. Grant of Copyright License. Subject to the terms and conditions of this
+License, each Contributor hereby grants to You a perpetual, worldwide,
+non-exclusive, no-charge, royalty-free, irrevocable copyright license to
+reproduce, prepare Derivative Works of, publicly display, publicly perform,
+sublicense, and distribute the Work and such Derivative Works in Source or
+Object form.
+
+3. Grant of Patent License. Subject to the terms and conditions of this
+License, each Contributor hereby grants to You a perpetual, worldwide,
+non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this
+section) patent license to make, have made, use, offer to sell, sell, import,
+and otherwise transfer the Work, where such license applies only to those
+patent claims licensable by such Contributor that are necessarily infringed by
+their Contribution(s) alone or by combination of their Contribution(s) with the
+Work to which such Contribution(s) was submitted. If You institute patent
+litigation against any entity (including a cross-claim or counterclaim in a
+lawsuit) alleging that the Work or a Contribution incorporated within the Work
+constitutes direct or contributory patent infringement, then any patent
+licenses granted to You under this License for that Work shall terminate as of
+the date such litigation is filed.
+
+4. Redistribution. You may reproduce and distribute copies of the Work or
+Derivative Works thereof in any medium, with or without modifications, and in
+Source or Object form, provided that You meet the following conditions:
+
+  You must give any other recipients of the Work or Derivative Works a copy of
+  this License; and
+  You must cause any modified files to carry prominent notices stating that You
+  changed the files; and
+  You must retain, in the Source form of any Derivative Works that You
+  distribute, all copyright, patent, trademark, and attribution notices from
+  the Source form of the Work, excluding those notices that do not pertain to
+  any part of the Derivative Works; and
+  If the Work includes a "NOTICE" text file as part of its distribution, then
+  any Derivative Works that You distribute must include a readable copy of the
+  attribution notices contained within such NOTICE file, excluding those
+  notices that do not pertain to any part of the Derivative Works, in at least
+  one of the following places: within a NOTICE text file distributed as part of
+  the Derivative Works; within the Source form or documentation, if provided
+  along with the Derivative Works; or, within a display generated by the
+  Derivative Works, if and wherever such third-party notices normally appear.
+  The contents of the NOTICE file are for informational purposes only and do
+  not modify the License. You may add Your own attribution notices within
+  Derivative Works that You distribute, alongside or as an addendum to the
+  NOTICE text from the Work, provided that such additional attribution notices
+  cannot be construed as modifying the License.
+
+  You may add Your own copyright statement to Your modifications and may
+  provide additional or different license terms and conditions for use,
+  reproduction, or distribution of Your modifications, or for any such
+  Derivative Works as a whole, provided Your use, reproduction, and distribution
+   of the Work otherwise complies with the conditions stated in this License.
+
+5. Submission of Contributions. Unless You explicitly state otherwise, any
+Contribution intentionally submitted for inclusion in the Work by You to the
+Licensor shall be under the terms and conditions of this License, without any
+additional terms or conditions. Notwithstanding the above, nothing herein shall
+supersede or modify the terms of any separate license agreement you may have
+executed with Licensor regarding such Contributions.
+
+6. Trademarks. This License does not grant permission to use the trade names,
+trademarks, service marks, or product names of the Licensor, except as required
+for reasonable and customary use in describing the origin of the Work and
+reproducing the content of the NOTICE file.
+
+7. Disclaimer of Warranty. Unless required by applicable law or agreed to in
+writing, Licensor provides the Work (and each Contributor provides its
+Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+KIND, either express or implied, including, without limitation, any warranties
+or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+PARTICULAR PURPOSE. You are solely responsible for determining the
+appropriateness of using or redistributing the Work and assume any risks
+associated with Your exercise of permissions under this License.
+
+8. Limitation of Liability. In no event and under no legal theory, whether in
+tort (including negligence), contract, or otherwise, unless required by
+applicable law (such as deliberate and grossly negligent acts) or agreed to in
+writing, shall any Contributor be liable to You for damages, including any
+direct, indirect, special, incidental, or consequential damages of any
+character arising as a result of this License or out of the use or inability
+to use the Work (including but not limited to damages for loss of goodwill,
+work stoppage, computer failure or malfunction, or any and all other commercial
+damages or losses), even if such Contributor has been advised of the
+possibility of such damages.
+
+9. Accepting Warranty or Additional Liability. While redistributing the Work or
+Derivative Works thereof, You may choose to offer, and charge a fee for,
+acceptance of support, warranty, indemnity, or other liability obligations
+and/or rights consistent with this License. However, in accepting such
+obligations, You may act only on Your own behalf and on Your sole
+responsibility, not on behalf of any other Contributor, and only if You agree
+to indemnify, defend, and hold each Contributor harmless for any liability
+incurred by, or claims asserted against, such Contributor by reason of your
+accepting any such warranty or additional liability.
+
+END OF TERMS AND CONDITIONS

REQUIRE

@@ -1,5 +1,5 @@
 julia 0.7 1.1
-IncrementalInference 0.4.1
+IncrementalInference 0.4.2
 Graphs 0.10.1
 TransformUtils 0.2.1
 CoordinateTransformations 0.5.0

test/runtests.jl

@@ -39,6 +39,8 @@ include("testPoint2Point2WorldBearing.jl")
 
 include("testBearingRange2D.jl")
 
+include("testFixedLagFG.jl")
+
 include("testMultimodalRangeBearing.jl")
 
 include("testDynPose2D.jl")

test/testDidsonFunctions.jl

@@ -12,8 +12,6 @@ using Distributions
 
 global meas = LinearRangeBearingElevation((3.0,3e-4),(0.2,3e-4))
 
-# Functor for efficient functional programming, avoids type_inference at each call
-# fp! = WrapParam{reuseLBRA}(zeros(3), zeros(6), zeros(3), reuseLBRA(0))
 
 global N = 100
 global X, pts = 0.01*randn(6,N), zeros(3,N);

test/testFixedLagFG.jl

@@ -0,0 +1,109 @@
+# example for fixed lag operation
+
+using Test
+using RoME
+using LinearAlgebra
+# , Distributions
+
+
+# TODO -- use this for cleanup
+function driveSomeMore!(fg::FactorGraph, idx)
+    for i in idx:(idx+5)
+      psym = Symbol("x$i")
+      nsym = Symbol("x$(i+1)")
+      addNode!(fg, nsym, Pose2)
+      pp = Pose2Pose2(MvNormal([10.0;0;pi/3], diagm([0.1;0.1;0.1].^2)))
+      addFactor!(fg, [psym;nsym], pp )
+    end
+    return Symbol("x$(idx+5)")
+end
+
+
+# start with an empty factor graph object
+global fg = initfg() # FUTURE: (quasifixedwindow=50, autosolve=true)
+
+# Set up a quasi fixed-lag horizon of 8 nodes and enable the fixed-lag solving.
+# If the graph grows over 8 nodes, the older nodes will be frozen to limit the computational window.
+fg.qfl = 8
+fg.isfixedlag = true
+
+@testset "test basic fixed lag operations..." begin
+
+## 1. Drive around in a hexagon
+# Add the first pose :x0
+println("STEP 1: Driving around a bit")
+addNode!(fg, :x0, Pose2)
+# Add at a fixed location PriorPose2 to pin :x0 to a starting location
+addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*Matrix{Float64}(LinearAlgebra.I,3,3))) )
+for i in 0:5
+  psym = Symbol("x$i")
+  nsym = Symbol("x$(i+1)")
+  addNode!(fg, nsym, Pose2)
+  pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
+  addFactor!(fg, [psym;nsym], pp )
+end
+
+# Add node linking initial pose with a bearing range measurement landmark
+addNode!(fg, :l1, Point2, labels=["LANDMARK"])
+global p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0))
+addFactor!(fg, [:x0; :l1], p2br)
+
+## 2. Solve graph when shorter than fixed length - should solve full session.
+println("STEP 2: Solve graph when shorter than fixed length")
+IIF.batchSolve!(fg)
+
+# 3. Drive a couple more, longer than fixed lag window
+println("STEP 3: Drive a couple more, longer than fixed lag window")
+for i in 6:11
+  psym = Symbol("x$i")
+  nsym = Symbol("x$(i+1)")
+  addNode!(fg, nsym, Pose2)
+  pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
+  addFactor!(fg, [psym;nsym], pp )
+end
+
+# Add another node when it comes around again, linking the node with the initial landmark
+p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0))
+addFactor!(fg, [:x6; :l1], p2br)
+
+## At this point our window is 8 nodes, but our graph consists of 13 nodes.
+## Next, freezing nodes beyond our fixed-lag horizon.
+
+# Back up the graph
+fgOriginal = deepcopy(fg)
+fgOriginal.isfixedlag = false
+
+# Back up data from these two poses so we can compare them once we solve again.
+X5 = deepcopy(getVal(fg, :x5))
+X6 = deepcopy(getVal(fg, :x6))
+
+# Now solve again, which will freeze vertices < 5
+println("STEP 4: Solve graph when shorter than fixed length, and show time to solve")
+@time IIF.batchSolve!(fg)
+# fg.isfixedlag
+# tofreeze = fg.fifo[1:(end-fg.qfl)]
+# @test length(tofreeze) > 0
+# IIF.setfreeze!.(fg, tofreeze)
+#
+# fifoFreeze!(fg)
+# global tree = IIF.wipeBuildNewTree!(fg)
+# inferOverTreeR!(fg, tree)
+
+# Confirm that the initial nodes (x0 - x5) are frozen.
+@test getData(fg, :x5).ismargin
+@test getData(fg, :x6).ismargin == false
+
+# X5 should be exactly same
+# X6 should be different
+X5cmp = deepcopy(getVal(fg, :x5))
+X6cmp = deepcopy(getVal(fg, :x6))
+@test X5 == X5cmp #Frozen
+@test X6 != X6cmp #Recalculated
+
+# Solve original graph with the to get time comparison
+@time IIF.batchSolve!(fgOriginal)
+
+end
+
+
+#