<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 PenguinStructures
import TensorFlow

/// A relaxed version of the Rot3 between that uses the Chordal (Frobenious) norm on rotation
/// Please refer to Carlone15icra (Initialization Techniques for 3D SLAM: a Survey on Rotation Estimation and its Use in Pose Graph Optimization)
/// for explanation.
public struct RelaxedRotationFactorRot3: LinearizableFactor
{
  public typealias Variables = Tuple2<Matrix3, Matrix3>
  public typealias JacobianRows = Array9<Tuple2<Matrix3.TangentVector, Matrix3.TangentVector>>
  
  public let edges: Variables.Indices
  public let difference: Matrix3
  
  public init(_ id1: TypedID<Matrix3>, _ id2: TypedID<Matrix3>, _ difference: Matrix3) {
    self.edges = Tuple2(id1, id2)
    self.difference = difference
  }
  
  public typealias ErrorVector = Vector9
  
  @differentiable
  public func errorVector(_ R1: Matrix3, _ R2: Matrix3) -> ErrorVector {
    let R12 = difference
    let R1h = matmul(R12, R2.transposed()).transposed()
    return (R1h - R1).vec
  }
  
  // Note: All the remaining code in this factor is boilerplate that we can eventually eliminate
  // with sugar.
  
  public func error(at x: Variables) -> Double {
    return 0.5 * errorVector(at: x).squaredNorm
  }
  
  @differentiable
  public func errorVector(at x: Variables) -> ErrorVector {
    return errorVector(x.head, x.tail.head)
  }
}

/// A factor for the anchor in chordal initialization
public struct RelaxedAnchorFactorRot3: LinearizableFactor
{
  public typealias Variables = Tuple1<Matrix3>
  public typealias JacobianRows = Array9<Tuple1<Matrix3.TangentVector>>
  
  public let edges: Variables.Indices
  public let prior: Matrix3
  
  public init(_ id: TypedID<Matrix3>, _ prior: Matrix3) {
    self.edges = Tuple1(id)
    self.prior = prior
  }
  
  public typealias ErrorVector = Vector9

  @differentiable
  public func errorVector(_ val: Matrix3) -> ErrorVector {
    (val - prior).vec
  }
  
  // Note: All the remaining code in this factor is boilerplate that we can eventually eliminate
  // with sugar.
  
  public func error(at x: Variables) -> Double {
    return 0.5 * errorVector(at: x).squaredNorm
  }
  
  @differentiable
  public func errorVector(at x: Variables) -> ErrorVector {
    return errorVector(x.head)
  }
}

/// Type shorthands used in the relaxed pose graph
/// NOTE: Specializations are added in `FactorsStorage.swift`
public typealias Jacobian9x3x3_1 = Array9<Tuple1<Matrix3>>
public typealias Jacobian9x3x3_2 = Array9<Tuple2<Matrix3, Matrix3>>
public typealias JacobianFactor9x3x3_1 = JacobianFactor<Jacobian9x3x3_1, Vector9>
public typealias JacobianFactor9x3x3_2 = JacobianFactor<Jacobian9x3x3_2, Vector9>
<commit_msg>update chordal factors to have less boilerplate<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 PenguinStructures
import TensorFlow

/// A relaxed version of the Rot3 between that uses the Chordal (Frobenious) norm on rotation
/// Please refer to Carlone15icra (Initialization Techniques for 3D SLAM: a Survey on Rotation Estimation and its Use in Pose Graph Optimization)
/// for explanation.
public struct RelaxedRotationFactorRot3: LinearizableFactor2
{
  public let edges: Variables.Indices
  public let difference: Matrix3
  
  public init(_ id1: TypedID<Matrix3>, _ id2: TypedID<Matrix3>, _ difference: Matrix3) {
    self.edges = Tuple2(id1, id2)
    self.difference = difference
  }
  
  public typealias ErrorVector = Vector9
  
  @differentiable
  public func errorVector(_ R1: Matrix3, _ R2: Matrix3) -> ErrorVector {
    let R12 = difference
    let R1h = matmul(R12, R2.transposed()).transposed()
    return (R1h - R1).vec
  }
}

/// A factor for the anchor in chordal initialization
public struct RelaxedAnchorFactorRot3: LinearizableFactor1
{
  public let edges: Variables.Indices
  public let prior: Matrix3
  
  public init(_ id: TypedID<Matrix3>, _ prior: Matrix3) {
    self.edges = Tuple1(id)
    self.prior = prior
  }
  
  public typealias ErrorVector = Vector9

  @differentiable
  public func errorVector(_ val: Matrix3) -> ErrorVector {
    (val - prior).vec
  }
}

/// Type shorthands used in the relaxed pose graph
/// NOTE: Specializations are added in `FactorsStorage.swift`
public typealias Jacobian9x3x3_1 = Array9<Tuple1<Matrix3>>
public typealias Jacobian9x3x3_2 = Array9<Tuple2<Matrix3, Matrix3>>
public typealias JacobianFactor9x3x3_1 = JacobianFactor<Jacobian9x3x3_1, Vector9>
public typealias JacobianFactor9x3x3_2 = JacobianFactor<Jacobian9x3x3_2, Vector9>

{'fraction_non_alphanumeric': 0.08060276852987559, 'fraction_numerical': 0.029437532854389346, 'mean_word_length': 3.959165942658558, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 35, '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': '6537929', 'n_tokens_mistral': 1765, 'n_tokens_neox': 1695, 'n_words': 603}