diff --git a/Sources/SwiftFusion/Core/MathUtil.swift b/Sources/SwiftFusion/Core/MathUtil.swift new file mode 100644 index 00000000..43a2d5df --- /dev/null +++ b/Sources/SwiftFusion/Core/MathUtil.swift @@ -0,0 +1,22 @@ +//===----------------------------------------------------------------------===// +// Pseudo inverse +//===----------------------------------------------------------------------===// + +import TensorFlow + +public func pinv(_ m: Tensor) -> Tensor { + let (J_s, J_u, J_v) = m.svd(computeUV: true, fullMatrices: true) + + let m = J_v!.shape[1] + let n = J_u!.shape[0] + if (m > n) { + let J_ss = J_s.reciprocal.diagonal().concatenated(with: Tensor(repeating: 0, shape: [m-n, n]), alongAxis: 0) + return matmul(matmul(J_v!, J_ss), J_u!.transposed()) + } else if (m < n) { + let J_ss = J_s.reciprocal.diagonal().concatenated(with: Tensor(repeating: 0, shape: [m, n-m]), alongAxis: 1) + return matmul(matmul(J_v!, J_ss), J_u!.transposed()) + } else { + let J_ss = J_s.reciprocal.diagonal() + return matmul(matmul(J_v!, J_ss), J_u!.transposed()) + } +} diff --git a/Sources/SwiftFusion/Geometry/File.swift b/Sources/SwiftFusion/Geometry/File.swift new file mode 100644 index 00000000..e53d4b0b --- /dev/null +++ b/Sources/SwiftFusion/Geometry/File.swift @@ -0,0 +1,8 @@ +// +// File.swift +// +// +// Created by Fan Jiang on 2020/4/24. +// + +import Foundation diff --git a/Sources/SwiftFusion/Inference/BetweenFactor.swift b/Sources/SwiftFusion/Inference/BetweenFactor.swift new file mode 100644 index 00000000..6f9cd9de --- /dev/null +++ b/Sources/SwiftFusion/Inference/BetweenFactor.swift @@ -0,0 +1,97 @@ +// Copyright 2019 The SwiftFusion Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +import TensorFlow + +/// A `NonlinearFactor` that calculates the difference of two Values of the same type +/// +/// Input is a dictionary of `Key` to `Value` pairs, and the output is the scalar +/// error value +/// +/// Interpretation +/// ================ +/// `Input`: the input values as key-value pairs +/// +public struct BetweenFactor: NonlinearFactor { + + var key1: Int + var key2: Int + @noDerivative + public var keys: Array { + get { + [key1, key2] + } + } + public var difference: Pose2 + public typealias Output = Error + + public init (_ key1: Int, _ key2: Int, _ difference: Pose2) { + self.key1 = key1 + self.key2 = key2 + self.difference = difference + } + typealias ScalarType = Double + + /// TODO: `Dictionary` still does not conform to `Differentiable` + /// Tracking issue: https://bugs.swift.org/browse/TF-899 +// typealias Input = Dictionary> + +// I want to build a general differentiable dot product +// @differentiable(wrt: (a, b)) +// static func dot(_ a: T, _ b: T) -> Double { +// let squared = a.recursivelyAllKeyPaths(to: Double.self).map { a[keyPath: $0] * b[keyPath: $0] } +// +// return squared.differentiableReduce(0.0, {$0 + $1}) +// } +// +// @derivative(of: dot) +// static func _vjpDot(_ a: T, _ b: T) -> ( +// value: Double, +// pullback: (Double) -> (T.TangentVector, T.TangentVector) +// ) { +// return (value: dot(a, b), pullback: { v in +// ((at.scaled(by: v), bt.scaled(by: v))) +// }) +// } + + /// Returns the `error` of the factor. + @differentiable(wrt: values) + public func error(_ values: Values) -> Double { + let error = between( + between(values[key2].baseAs(Pose2.self), values[key1].baseAs(Pose2.self)), + difference + ) + + return error.t.norm + error.rot.theta * error.rot.theta + } + + @differentiable(wrt: values) + public func errorVector(_ values: Values) -> Vector3 { + let error = between( + between(values[key2].baseAs(Pose2.self), values[key1].baseAs(Pose2.self)), + difference + ) + + return Vector3(error.rot.theta, error.t.x, error.t.y) + } + + public func linearize(_ values: Values) -> JacobianFactor { + let j = jacobian(of: self.errorVector, at: values) + + let j1 = Tensor(stacking: (0..<3).map { i in (j[i]._values[values._indices[key1]!].base as! Pose2.TangentVector).tensor.reshaped(to: TensorShape([3])) }) + let j2 = Tensor(stacking: (0..<3).map { i in (j[i]._values[values._indices[key2]!].base as! Pose2.TangentVector).tensor.reshaped(to: TensorShape([3])) }) + + // TODO: remove this negative sign + return JacobianFactor(keys, [j1, j2], -errorVector(values).tensor.reshaped(to: [3, 1])) + } +} diff --git a/Sources/SwiftFusion/Inference/Factor.swift b/Sources/SwiftFusion/Inference/Factor.swift index 435f30d7..9c14c9e3 100644 --- a/Sources/SwiftFusion/Inference/Factor.swift +++ b/Sources/SwiftFusion/Inference/Factor.swift @@ -15,7 +15,7 @@ import TensorFlow /// The most general factor protocol. public protocol Factor { - var keys: Array { get set } + var keys: Array { get } } /// A `LinearFactor` corresponds to the `GaussianFactor` in GTSAM. @@ -30,11 +30,33 @@ public protocol Factor { public protocol LinearFactor: Factor { typealias ScalarType = Double + /// TODO: `Dictionary` still does not conform to `Differentiable` + /// Tracking issue: https://bugs.swift.org/browse/TF-899 +// typealias Input = Dictionary> + + /// Returns the `error` of the factor. + func error(_ values: VectorValues) -> ScalarType +} + +/// A `NonlinearFactor` corresponds to the `NonlinearFactor` in GTSAM. +/// +/// Input is a dictionary of `Key` to `Value` pairs, and the output is the scalar +/// error value +/// +/// Interpretation +/// ================ +/// `Input`: the input values as key-value pairs +/// +public protocol NonlinearFactor: Factor { + typealias ScalarType = Double + /// TODO: `Dictionary` still does not conform to `Differentiable` /// Tracking issue: https://bugs.swift.org/browse/TF-899 // typealias Input = Dictionary> /// Returns the `error` of the factor. @differentiable(wrt: values) - func error(_ indices: [Int], values: Tensor) -> ScalarType + func error(_ values: Values) -> ScalarType + + func linearize(_ values: Values) -> JacobianFactor } diff --git a/Sources/SwiftFusion/Inference/FactorGraph.swift b/Sources/SwiftFusion/Inference/FactorGraph.swift deleted file mode 100644 index 6f6ffcaa..00000000 --- a/Sources/SwiftFusion/Inference/FactorGraph.swift +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2019 The SwiftFusion Authors. All Rights Reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -import TensorFlow - -/// A Factor Graph in its very general form. -/// -/// Explanation -/// ============= -/// A factor graph is a biparite graph that connects between *Factors* and *Values*, and -/// the way they are stored does not matter here. -/// -public protocol FactorGraph { - // TODO: find a better protocol - associatedtype KeysType : Collection where KeysType.Element : SignedInteger - associatedtype FactorsType : Collection where FactorsType.Element: Factor - /// TODO(fan): Is this right? - /// Or, do we need this at all? I think this would help to register keys to descriptions and - /// help debugging and serialization, but I am not sure. - var keys: KeysType { get } - - var factors: FactorsType { get } -} diff --git a/Sources/SwiftFusion/Inference/GaussianFactorGraph.swift b/Sources/SwiftFusion/Inference/GaussianFactorGraph.swift index 8bfb11e0..f9121c1a 100644 --- a/Sources/SwiftFusion/Inference/GaussianFactorGraph.swift +++ b/Sources/SwiftFusion/Inference/GaussianFactorGraph.swift @@ -16,7 +16,7 @@ import TensorFlow /// A factor graph for linear problems /// Factors are the Jacobians between the corresponding variables and measurements /// TODO(fan): Add noise model -public struct GaussianFactorGraph: FactorGraph { +public struct GaussianFactorGraph { public typealias KeysType = Array public typealias FactorsType = Array @@ -34,11 +34,15 @@ public struct GaussianFactorGraph: FactorGraph { public init() { } /// This calculates `A*x`, where x is the collection of key-values - /// Note A is a public static func * (lhs: GaussianFactorGraph, rhs: VectorValues) -> Errors { Array(lhs.factors.map { $0 * rhs }) } + /// This calculates `A*x - b`, where x is the collection of key-values + public func residual (_ val: VectorValues) -> Errors { + Array(self.factors.map { $0 * val - $0.b }) + } + /// Convenience operator for adding factor public static func += (lhs: inout Self, rhs: JacobianFactor) { lhs.factors.append(rhs) @@ -50,7 +54,6 @@ public struct GaussianFactorGraph: FactorGraph { for i in r.indices { let JTr = factors[i].atr(r[i]) - print("JTr = \(JTr)") vv = vv + JTr } diff --git a/Sources/SwiftFusion/Inference/JacobianFactor.swift b/Sources/SwiftFusion/Inference/JacobianFactor.swift index 14c4d2ab..d5bca8c3 100644 --- a/Sources/SwiftFusion/Inference/JacobianFactor.swift +++ b/Sources/SwiftFusion/Inference/JacobianFactor.swift @@ -39,10 +39,11 @@ import TensorFlow /// and `HessianFactor` conform to this protocol instead. public struct JacobianFactor: LinearFactor { - @differentiable(wrt: values) - public func error(_ indices: [Int], values: Tensor) -> ScalarType { + // TODO(fan): correct this and add a unit test + public func error(_ values: VectorValues) -> ScalarType { ScalarType.zero } + public var dimension: Int { get { jacobians[0].shape.dimensions[0] diff --git a/Sources/SwiftFusion/Inference/NonlinearFactorGraph.swift b/Sources/SwiftFusion/Inference/NonlinearFactorGraph.swift new file mode 100644 index 00000000..fdcd2512 --- /dev/null +++ b/Sources/SwiftFusion/Inference/NonlinearFactorGraph.swift @@ -0,0 +1,49 @@ +// Copyright 2019 The SwiftFusion Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +import TensorFlow + +/// A factor graph for nonlinear problems +/// TODO(fan): Add noise model +public struct NonlinearFactorGraph { + public typealias KeysType = Array + + public typealias FactorsType = Array + + public var keys: KeysType = [] + public var factors: FactorsType = [] + + /// Default initializer + public init() { } + + /// Convenience operator for adding factor + public static func += (lhs: inout Self, rhs: NonlinearFactor) { + lhs.factors.append(rhs) + } + + /// linearize the nonlinear factor graph to a linear factor graph + public func linearize(_ values: Values) -> GaussianFactorGraph { + var gfg = GaussianFactorGraph() + + for i in factors { + let linearized = i.linearize(values) + + // Assertion for the shape of Jacobian + assert(linearized.jacobians.map { $0.shape.count == 2 }.reduce(true, { $0 && $1 })) + + gfg += linearized + } + + return gfg + } +} diff --git a/Sources/SwiftFusion/Inference/PriorFactor.swift b/Sources/SwiftFusion/Inference/PriorFactor.swift new file mode 100644 index 00000000..a54903ac --- /dev/null +++ b/Sources/SwiftFusion/Inference/PriorFactor.swift @@ -0,0 +1,69 @@ +// Copyright 2019 The SwiftFusion Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +import TensorFlow + +/// A `NonlinearFactor` that returns the difference of value and desired value +/// +/// Input is a dictionary of `Key` to `Value` pairs, and the output is the scalar +/// error value +/// +/// Interpretation +/// ================ +/// `Input`: the input values as key-value pairs +/// +public struct PriorFactor: NonlinearFactor { + @noDerivative + public var keys: Array = [] + public var difference: Pose2 + public typealias Output = Error + + public init (_ key: Int, _ difference: Pose2) { + keys = [key] + self.difference = difference + } + typealias ScalarType = Double + + /// TODO: `Dictionary` still does not conform to `Differentiable` + /// Tracking issue: https://bugs.swift.org/browse/TF-899 +// typealias Input = Dictionary> + + /// Returns the `error` of the factor. + @differentiable(wrt: values) + public func error(_ values: Values) -> Double { + let error = between( + values[keys[0]].baseAs(Pose2.self), + difference + ) + + return error.t.norm + error.rot.theta * error.rot.theta + } + + @differentiable(wrt: values) + public func errorVector(_ values: Values) -> Vector3 { + let error = between( + values[keys[0]].baseAs(Pose2.self), + difference + ) + + return Vector3(error.rot.theta, error.t.x, error.t.y) + } + + public func linearize(_ values: Values) -> JacobianFactor { + let j = jacobian(of: self.errorVector, at: values) + + let j1 = Tensor(stacking: (0..<3).map { i in (j[i]._values[0].base as! Pose2.TangentVector).tensor.reshaped(to: TensorShape([3])) }) + + return JacobianFactor(keys, [j1], -errorVector(values).tensor.reshaped(to: [3, 1])) + } +} diff --git a/Sources/SwiftFusion/Inference/Values.swift b/Sources/SwiftFusion/Inference/Values.swift new file mode 100644 index 00000000..cfd8e01b --- /dev/null +++ b/Sources/SwiftFusion/Inference/Values.swift @@ -0,0 +1,76 @@ +// Copyright 2019 The SwiftFusion Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +import TensorFlow + +/// The class that holds Key-Value pairs. +public struct Values: Differentiable & KeyPathIterable { + public typealias ScalarType = Double + var _values: [AnyDifferentiable] = [] + + /// Dictionary from Key to index + @noDerivative + var _indices: Dictionary = [:] + + public var keys: Dictionary.Keys { + get { + _indices.keys + } + } + /// Default initializer + public init() { } + + /// The subscript operator, with some indirection + /// Should be replaced after Dictionary is in + @differentiable + public subscript(key: Int) -> AnyDifferentiable { + get { + _values[_indices[key]!] + } + set(newVal) { + _values[_indices[key]!] = newVal + } + } + + /// Insert a key value pair + public mutating func insert(_ key: Int, _ val: AnyDifferentiable) { + assert(_indices[key] == nil) + + self._indices[key] = self._values.count + self._values.append(val) + } + +} + +extension Values: CustomStringConvertible { + public var description: String { + "Values(\n\(_indices.map { "Key: \($0), J: \(_values[$1])\n"}.reduce("", { $0 + $1 }) )" + } +} + +//extension Values: Equatable { +// /// Order-aware comparison +// public static func == (lhs: Values, rhs: Values) -> Bool { +// if lhs._indices.keys != rhs._indices.keys { +// return false +// } +// +// for k in lhs._indices.keys { +// if lhs._values[lhs._indices[k]!] != rhs._values[rhs._indices[k]!] { +// return false +// } +// } +// +// return true +// } +//} diff --git a/Tests/SwiftFusionTests/Applications/ManipulationTests.swift b/Tests/SwiftFusionTests/Applications/ManipulationTests.swift new file mode 100644 index 00000000..d73b1501 --- /dev/null +++ b/Tests/SwiftFusionTests/Applications/ManipulationTests.swift @@ -0,0 +1,8 @@ +// +// File.swift +// +// +// Created by Fan Jiang on 2020/4/22. +// + +import Foundation diff --git a/Tests/SwiftFusionTests/Geometry/Pose2Tests.swift b/Tests/SwiftFusionTests/Geometry/Pose2Tests.swift index 69ceda28..1c331ee6 100644 --- a/Tests/SwiftFusionTests/Geometry/Pose2Tests.swift +++ b/Tests/SwiftFusionTests/Geometry/Pose2Tests.swift @@ -30,6 +30,45 @@ final class Pose2Tests: XCTestCase { XCTAssertEqual(actual, expected) } + /// test the simplest compose (multiplication) + func testCompose() { + let pose1 = Pose2(Rot2(.pi/4.0), Vector2(sqrt(0.5), sqrt(0.5))) + let pose2 = Pose2(Rot2(.pi/2.0), Vector2(0.0, 2.0)) + + let actual = pose1 * pose2 + + let expected = Pose2(Rot2(3.0 * .pi/4.0), Vector2(-sqrt(0.5), 3.0*sqrt(0.5))) + + let _ = expected.recursivelyAllKeyPaths(to: Double.self).map { + XCTAssertEqual(expected[keyPath: $0], actual[keyPath: $0], accuracy: 1e-9) + } + } + + /// test the simplest compose (multiplication) + func testInverse() { + let gTl = Pose2(Rot2(.pi/2.0), Vector2(1.0, 2.0)) + + let actual = gTl.inverse() + + let expected = Pose2(Rot2(-.pi/2.0), Vector2(-2, 1.0)) + + let _ = expected.recursivelyAllKeyPaths(to: Double.self).map { + XCTAssertEqual(expected[keyPath: $0], actual[keyPath: $0], accuracy: 1e-9) + } + } + + /// test the between function against GTSAM + func testBetween() { + let p1 = Pose2(1.23, 2.30, 0.2) + let odo = Pose2(0.53, 0.39, 0.15) + let p2 = p1 * odo + + let expected = between(p1, p2) + let _ = expected.recursivelyAllKeyPaths(to: Double.self).map { + XCTAssertEqual(expected[keyPath: $0], odo[keyPath: $0], accuracy: 1e-9) + } + } + /// test the simplest gradient descent on Pose2 func testBetweenDerivatives() { var pT1 = Pose2(Rot2(0), Vector2(1, 0)), pT2 = Pose2(Rot2(1), Vector2(1, 1)) @@ -222,8 +261,6 @@ final class Pose2Tests: XCTestCase { /// test convergence for a simple Pose2SLAM func testPose2SLAMWithSGD() { - let pi = 3.1415926 - let dumpjson = { (p: Pose2) -> String in "[ \(p.t.x), \(p.t.y), \(p.rot.theta)]" } @@ -231,9 +268,9 @@ final class Pose2Tests: XCTestCase { // Initial estimate for poses let p1T0 = Pose2(Rot2(0.2), Vector2(0.5, 0.0)) let p2T0 = Pose2(Rot2(-0.2), Vector2(2.3, 0.1)) - let p3T0 = Pose2(Rot2(pi / 2), Vector2(4.1, 0.1)) - let p4T0 = Pose2(Rot2(pi), Vector2(4.0, 2.0)) - let p5T0 = Pose2(Rot2(-pi / 2), Vector2(2.1, 2.1)) + let p3T0 = Pose2(Rot2(.pi / 2), Vector2(4.1, 0.1)) + let p4T0 = Pose2(Rot2(.pi), Vector2(4.0, 2.0)) + let p5T0 = Pose2(Rot2(-.pi / 2), Vector2(2.1, 2.1)) var map = [p1T0, p2T0, p3T0, p4T0, p5T0] @@ -246,9 +283,9 @@ final class Pose2Tests: XCTestCase { // Odometry measurements let p2T1 = between(between(map[1], map[0]), Pose2(2.0, 0.0, 0.0)) - let p3T2 = between(between(map[2], map[1]), Pose2(2.0, 0.0, pi / 2)) - let p4T3 = between(between(map[3], map[2]), Pose2(2.0, 0.0, pi / 2)) - let p5T4 = between(between(map[4], map[3]), Pose2(2.0, 0.0, pi / 2)) + let p3T2 = between(between(map[2], map[1]), Pose2(2.0, 0.0, .pi / 2)) + let p4T3 = between(between(map[3], map[2]), Pose2(2.0, 0.0, .pi / 2)) + let p5T4 = between(between(map[4], map[3]), Pose2(2.0, 0.0, .pi / 2)) // Sum through the errors let error = self.e_pose2(p2T1) + self.e_pose2(p3T2) + self.e_pose2(p4T3) + self.e_pose2(p5T4) diff --git a/Tests/SwiftFusionTests/Geometry/Rot3Tests.swift b/Tests/SwiftFusionTests/Geometry/Rot3Tests.swift new file mode 100644 index 00000000..e53d4b0b --- /dev/null +++ b/Tests/SwiftFusionTests/Geometry/Rot3Tests.swift @@ -0,0 +1,8 @@ +// +// File.swift +// +// +// Created by Fan Jiang on 2020/4/24. +// + +import Foundation diff --git a/Tests/SwiftFusionTests/Inference/NonlinearFactorGraphTests.swift b/Tests/SwiftFusionTests/Inference/NonlinearFactorGraphTests.swift new file mode 100644 index 00000000..b9be43d8 --- /dev/null +++ b/Tests/SwiftFusionTests/Inference/NonlinearFactorGraphTests.swift @@ -0,0 +1,99 @@ +import SwiftFusion +import TensorFlow +import XCTest + +final class NonlinearFactorGraphTests: XCTestCase { + /// test ATr + func testBasicOps() { + var fg = NonlinearFactorGraph() + + let bf1 = BetweenFactor(0, 1, Pose2(0.0,0.0, 0.0)) + + fg += bf1 + + var val = Values() + val.insert(0, AnyDifferentiable(Pose2(1.0, 1.0, 0.0))) + val.insert(1, AnyDifferentiable(Pose2(1.0, 1.0, .pi))) + + let gfg = fg.linearize(val) + + var vv = VectorValues() + + vv.insert(0, Tensor(shape:[3, 1], scalars: [0.0, 0.0, 0.0])) + vv.insert(1, Tensor(shape:[3, 1], scalars: [0.0, 0.0, 0.0])) + + let expected = Tensor(shape:[3, 1], scalars: [.pi, 0.0, 0.0]) + + print("gfg = \(gfg)") + print("error = \(gfg.residual(vv).norm)") + assertEqual((gfg.residual(vv))[0], expected, accuracy: 1e-6) + } + + /// test CGLS iterative solver + func testCGLSPose2SLAM() { + // Initial estimate for poses + let p1T0 = Pose2(Rot2(0.2), Vector2(0.5, 0.0)) + let p2T0 = Pose2(Rot2(-0.2), Vector2(2.3, 0.1)) + let p3T0 = Pose2(Rot2(.pi / 2), Vector2(4.1, 0.1)) + let p4T0 = Pose2(Rot2(.pi), Vector2(4.0, 2.0)) + let p5T0 = Pose2(Rot2(-.pi / 2), Vector2(2.1, 2.1)) + + let map = [p1T0, p2T0, p3T0, p4T0, p5T0] + + var fg = NonlinearFactorGraph() + + fg += BetweenFactor(1, 0, Pose2(2.0, 0.0, .pi / 2)) + fg += BetweenFactor(2, 1, Pose2(2.0, 0.0, .pi / 2)) + fg += BetweenFactor(3, 2, Pose2(2.0, 0.0, .pi / 2)) + fg += BetweenFactor(4, 3, Pose2(2.0, 0.0, .pi / 2)) + fg += PriorFactor(0, Pose2(0.0, 0.0, 0.0)) + + var val = Values() + + for i in 0..<5 { + val.insert(i, AnyDifferentiable(map[i])) + } + + for _ in 0..<2 { + let gfg = fg.linearize(val) + + let optimizer = CGLS(precision: 1e-6, max_iteration: 500) + + var dx = VectorValues() + + for i in 0..<5 { + dx.insert(i, Tensor(shape: [3, 1], scalars: [0, 0, 0])) + } + + optimizer.optimize(gfg: gfg, initial: &dx) + + for i in 0..<5 { + var p = val[i].baseAs(Pose2.self) + p.move(along: Vector3(dx[i].reshaped(toShape: [3]))) + val[i] = AnyDifferentiable(p) + } + } + + let dumpjson = { (p: Pose2) -> String in + "[ \(p.rot.theta), \(p.t.x), \(p.t.y)]" + } + + print("map_init = [") + for v in map.indices { + print("\(dumpjson(map[v]))\({ () -> String in if v == map.indices.endIndex - 1 { return "" } else { return "," } }())") + } + print("]") + + let map_final = (0..<5).map { val[$0].baseAs(Pose2.self) } + print("map = [") + for v in map_final.indices { + print("\(dumpjson(map_final[v]))\({ () -> String in if v == map_final.indices.endIndex - 1 { return "" } else { return "," } }())") + } + print("]") + + let p5T1 = between(val[4].baseAs(Pose2.self), val[0].baseAs(Pose2.self)) + + // Test condition: P_5 should be identical to P_1 (close loop) + XCTAssertEqual(p5T1.t.norm, 0.0, accuracy: 1e-2) + } +}