<commit_before>// Copyright 2020 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 Foundation
import TensorFlow
import XCTest

import PenguinStructures
import SwiftFusion

class FactorGraphTests: XCTestCase {
  /// Tests that we can read factors from a factor graph.
  func testGetFactors() {
    let pose1ID = TypedID<Pose2>(0)
    let pose2ID = TypedID<Pose2>(1)

    var graph = FactorGraph()
    graph.store(PriorFactor(pose1ID, Pose2(1, 2, 3)))
    graph.store(PriorFactor(pose2ID, Pose2(4, 5, 6)))
    graph.store(BetweenFactor(pose1ID, pose2ID, Pose2(7, 8, 9)))

    XCTAssertEqual(
      graph.factors(type: PriorFactor<Pose2>.self).map { $0.edges },
      [Tuple1(pose1ID), Tuple1(pose2ID)]
    )
    XCTAssertEqual(
      graph.factors(type: PriorFactor<Pose2>.self).map { $0.prior },
      [Pose2(1, 2, 3), Pose2(4, 5, 6)]
    )

    XCTAssertEqual(
      graph.factors(type: BetweenFactor<Pose2>.self).map { $0.edges },
      [Tuple2(pose1ID, pose2ID)]
    )
    XCTAssertEqual(
      graph.factors(type: BetweenFactor<Pose2>.self).map { $0.difference },
      [Pose2(7, 8, 9)]
    )

    XCTAssertEqual(graph.factors(type: PriorFactor<Pose3>.self).count, 0)
  }

  func testSimplePose2SLAM() {
    var x = VariableAssignments()
    let pose1ID = x.store(Pose2(Rot2(0.2), Vector2(0.5, 0.0)))
    let pose2ID = x.store(Pose2(Rot2(-0.2), Vector2(2.3, 0.1)))
    let pose3ID = x.store(Pose2(Rot2(.pi / 2), Vector2(4.1, 0.1)))
    let pose4ID = x.store(Pose2(Rot2(.pi), Vector2(4.0, 2.0)))
    let pose5ID = x.store(Pose2(Rot2(-.pi / 2), Vector2(2.1, 2.1)))

    var graph = FactorGraph()
    graph.store(BetweenFactor(pose2ID, pose1ID, Pose2(2.0, 0.0, .pi / 2)))
    graph.store(BetweenFactor(pose3ID, pose2ID, Pose2(2.0, 0.0, .pi / 2)))
    graph.store(BetweenFactor(pose4ID, pose3ID, Pose2(2.0, 0.0, .pi / 2)))
    graph.store(BetweenFactor(pose5ID, pose4ID, Pose2(2.0, 0.0, .pi / 2)))
    graph.store(PriorFactor(pose1ID, Pose2(0, 0, 0)))

    for _ in 0..<3 {
      let linearized = graph.linearized(at: x)
      var dx = x.tangentVectorZeros
      var optimizer = GenericCGLS(precision: 1e-6, max_iteration: 500)
      optimizer.optimize(gfg: linearized, initial: &dx)
      x.move(along: dx)
    }

    // Test condition: pose 5 should be identical to pose 1 (close loop).
    XCTAssertEqual(between(x[pose1ID], x[pose5ID]).t.norm, 0.0, accuracy: 1e-2)
  }
  <commit_msg>Fix tests<commit_after>// Copyright 2020 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 Foundation
import TensorFlow
import XCTest

import PenguinStructures
import SwiftFusion

class FactorGraphTests: XCTestCase {
  /// Tests that we can read factors from a factor graph.
  func testGetFactors() {
    let pose1ID = TypedID<Pose2>(0)
    let pose2ID = TypedID<Pose2>(1)

    var graph = FactorGraph()
    graph.store(PriorFactor(pose1ID, Pose2(1, 2, 3)))
    graph.store(PriorFactor(pose2ID, Pose2(4, 5, 6)))
    graph.store(BetweenFactor(pose1ID, pose2ID, Pose2(7, 8, 9)))

    XCTAssertEqual(
      graph.factors(type: PriorFactor<Pose2>.self).map { $0.edges },
      [Tuple1(pose1ID), Tuple1(pose2ID)]
    )
    XCTAssertEqual(
      graph.factors(type: PriorFactor<Pose2>.self).map { $0.prior },
      [Pose2(1, 2, 3), Pose2(4, 5, 6)]
    )

    XCTAssertEqual(
      graph.factors(type: BetweenFactor<Pose2>.self).map { $0.edges },
      [Tuple2(pose1ID, pose2ID)]
    )
    XCTAssertEqual(
      graph.factors(type: BetweenFactor<Pose2>.self).map { $0.difference },
      [Pose2(7, 8, 9)]
    )

    XCTAssertEqual(graph.factors(type: PriorFactor<Pose3>.self).count, 0)
  }

  func testSimplePose2SLAM() {
    var x = VariableAssignments()
    let pose1ID = x.store(Pose2(Rot2(0.2), Vector2(0.5, 0.0)))
    let pose2ID = x.store(Pose2(Rot2(-0.2), Vector2(2.3, 0.1)))
    let pose3ID = x.store(Pose2(Rot2(.pi / 2), Vector2(4.1, 0.1)))
    let pose4ID = x.store(Pose2(Rot2(.pi), Vector2(4.0, 2.0)))
    let pose5ID = x.store(Pose2(Rot2(-.pi / 2), Vector2(2.1, 2.1)))

    var graph = FactorGraph()
    graph.store(BetweenFactor(pose2ID, pose1ID, Pose2(2.0, 0.0, .pi / 2)))
    graph.store(BetweenFactor(pose3ID, pose2ID, Pose2(2.0, 0.0, .pi / 2)))
    graph.store(BetweenFactor(pose4ID, pose3ID, Pose2(2.0, 0.0, .pi / 2)))
    graph.store(BetweenFactor(pose5ID, pose4ID, Pose2(2.0, 0.0, .pi / 2)))
    graph.store(PriorFactor(pose1ID, Pose2(0, 0, 0)))

    for _ in 0..<5 {
      let linearized = graph.linearized(at: x)
      var dx = x.tangentVectorZeros
      var optimizer = GenericCGLS(precision: 1e-6, max_iteration: 500)
      optimizer.optimize(gfg: linearized, initial: &dx)
      x.move(along: dx)
    }

    // Test condition: pose 5 should be identical to pose 1 (close loop).
    XCTAssertEqual(between(x[pose1ID], x[pose5ID]).t.norm, 0.0, accuracy: 1e-2)
  }
  
{'fraction_non_alphanumeric': 0.1425661914460285, 'fraction_numerical': 0.05363204344874406, 'mean_word_length': 3.3587278106508878, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 17, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6033225', 'n_tokens_mistral': 2290, 'n_tokens_neox': 2131, 'n_words': 537}