├── .github └── workflows │ └── ci.yml ├── .gitignore ├── .swiftformat ├── Examples ├── BeeTrackingTool │ └── main.swift ├── OISTVisualizationTool │ ├── Util.swift │ └── main.swift └── Pose3SLAMG2O │ └── main.swift ├── LICENSE ├── Package.resolved ├── Package.swift ├── README.md ├── Scripts ├── Andrew01.swift ├── Fan01.swift ├── Fan02.swift ├── Fan03.swift ├── Fan04.swift ├── Fan05.swift ├── Fan10.swift ├── Fan12.swift ├── Fan13.swift ├── Fan14.swift ├── Frank01.swift ├── Frank02.swift ├── Frank03.swift ├── Frank04.swift ├── README.md └── main.swift ├── Sources ├── BeeDataset │ ├── BeeFrames.swift │ ├── BeeOrientedBoundingBoxes.swift │ ├── BeeVideo.swift │ ├── Download.swift │ ├── OISTBeeVideo.swift │ ├── VOTVideo.swift │ └── Visualization.swift ├── BeeTracking │ ├── AppearanceRAE+Serialization.swift │ ├── AppearanceRAE.swift │ ├── FrameStatistics.swift │ ├── NaiveBayesAETracker.swift │ ├── OISTBeeVideo+Batches.swift │ ├── PPCATracker.swift │ ├── ProbabilisticTracker.swift │ ├── RAETracker.swift │ ├── RandomProjectionTracker.swift │ ├── RawPixelTracker.swift │ ├── TrackingFactorGraph.swift │ ├── TrackingLikelihoodModel.swift │ ├── TrackingMetrics.swift │ └── Visualizations.swift ├── ModelSupport │ ├── DatasetUtilities.swift │ ├── FileManagement.swift │ └── Image.swift ├── STBImage │ ├── CMakeLists.txt │ ├── include │ │ ├── module.modulemap │ │ ├── stb_image.h │ │ └── stb_image_write.h │ ├── stb_image.c │ └── stb_image_write.c ├── SwiftFusion │ ├── Core │ │ ├── DataTypes.swift │ │ ├── Dictionary+Differentiable.swift │ │ ├── FixedSizeMatrix.swift │ │ ├── LieGroup.swift │ │ ├── Manifold.swift │ │ ├── MathUtil.swift │ │ ├── TensorVector.swift │ │ ├── Timer.swift │ │ ├── TrappingDouble.swift │ │ ├── Tuple+Vector.swift │ │ ├── TypeKeyedArrayBuffers.swift │ │ ├── Vector.swift │ │ ├── VectorN.swift │ │ └── VectorN.swift.gyb │ ├── Datasets │ │ ├── DatasetCache.swift │ │ └── G2OReader.swift │ ├── Geometry │ │ ├── Cal3_S2.swift │ │ ├── CameraCalibration.swift │ │ ├── PinholeCamera.swift │ │ ├── Pose2.swift │ │ ├── Pose3.swift │ │ ├── Rot2.swift │ │ └── Rot3.swift │ ├── Image │ │ ├── ArrayImage.swift │ │ ├── OrientedBoundingBox.swift │ │ └── Patch.swift │ ├── Inference │ │ ├── AllVectors.swift │ │ ├── AnyArrayBuffer+Differentiable.swift │ │ ├── AnyArrayBuffer+Vector.swift │ │ ├── AppearanceTrackingFactor.swift │ │ ├── ArrayBuffer+Differentiable.swift │ │ ├── ArrayBuffer+Tensor.swift │ │ ├── ArrayBuffer+Vector.swift │ │ ├── ArrayStorage+Tensor.swift │ │ ├── BearingRangeFactor.swift │ │ ├── BetweenFactor.swift │ │ ├── BetweenFactorAlternative.swift │ │ ├── ChordalInitialization.swift │ │ ├── DiscreteTransitionFactor.swift │ │ ├── Factor.swift │ │ ├── FactorBoilerplate.swift │ │ ├── FactorBoilerplate.swift.gyb │ │ ├── FactorGraph.swift │ │ ├── FactorsStorage.swift │ │ ├── FlattenedScalars.swift │ │ ├── GaussianFactorGraph.swift │ │ ├── IdentityLinearizationFactor.swift │ │ ├── IdentityProjection.swift │ │ ├── JacobianFactor.swift │ │ ├── LatentAppearanceTrackingFactor.swift │ │ ├── MonteCarloEM.swift │ │ ├── PCAEncoder.swift │ │ ├── PPCA.swift │ │ ├── PPCATrackingFactor.swift │ │ ├── PenguinExtensions.swift │ │ ├── PriorFactor.swift │ │ ├── ProbablisticTrackingFactor.swift │ │ ├── RandomProjection.swift │ │ ├── RawPixelTrackingFactor.swift │ │ ├── ScalarJacobianFactor.swift │ │ ├── SwitchingBetweenFactor.swift │ │ └── VariableAssignments.swift │ ├── MCMC │ │ ├── RandomWalkMetropolis.swift │ │ ├── TransitionKernel.swift │ │ └── sample.swift │ ├── Optimizers │ │ ├── CGLS.swift │ │ ├── GradientDescent.swift │ │ └── LM.swift │ └── Probability │ │ ├── MultivariateGaussian.swift │ │ ├── NaiveBayes.swift │ │ └── ProbabilityModel.swift └── SwiftFusionBenchmarks │ ├── PPCATracking.swift │ ├── Patch.swift │ ├── Pose2SLAM.swift │ ├── Pose3SLAM.swift │ └── main.swift ├── Tests ├── BeeDatasetTests │ ├── BeeDatasetTests.swift │ ├── BeePPCATests.swift │ ├── OISTDatasetTests.swift │ └── fakeDataset │ │ ├── downsampled │ │ ├── frame_30fps_001515.png │ │ └── frame_30fps_001530.png │ │ ├── downsampled_txt │ │ ├── frame_30fps_001515.txt │ │ └── frame_30fps_001530.txt │ │ ├── frames │ │ └── seq1 │ │ │ ├── frame1.png │ │ │ ├── frame2.png │ │ │ └── index.txt │ │ ├── obbs │ │ └── seq1.txt │ │ └── tracks │ │ └── track000.txt ├── BeeTrackingTests │ ├── AppearanceRAETests.swift │ ├── LikelihoodModelEMTests.swift │ ├── OISTBeeVideo+BatchesTests.swift │ ├── TrackingFactorGraphTests.swift │ └── TrackingMetricsTests.swift ├── LinuxMain.swift └── SwiftFusionTests │ ├── Applications │ └── ManipulationTests.swift │ ├── Core │ ├── Dictionary+DifferentiableTests.swift │ ├── FixedSizeMatrixTests.swift │ ├── MatrixNTests.swift │ ├── TensorFlowMatrixTests.swift │ ├── TrappingDoubleTests.swift │ ├── VectorNTests.swift │ ├── VectorNTests.swift.gyb │ └── VectorTests.swift │ ├── Datasets │ ├── Data │ │ ├── malformed.g2o │ │ ├── simple2d.g2o │ │ └── simple3d.g2o │ └── G2OReader.swift │ ├── Geometry │ ├── Cal3_S2Tests.swift │ ├── PinholeCameraTests.swift │ ├── Pose2Tests.swift │ ├── Pose3Tests.swift │ ├── Rot2Tests.swift │ └── Rot3Tests.swift │ ├── Image │ ├── ArrayImageTests.swift │ ├── OrientedBoundingBoxTests.swift │ ├── PatchTests.swift │ └── data │ │ ├── cropped.png │ │ └── test.png │ ├── Inference │ ├── AnyArrayBufferTests.swift │ ├── BearingRangeFactorTests.swift │ ├── ChordalInitializationTests.swift │ ├── FactorGraphTests.swift │ ├── FactorTests.swift │ ├── GaussianFactorGraphTests.swift │ ├── IdentityLinearizationFactorTests.swift │ ├── PCAEncoderTests.swift │ ├── PPCATests.swift │ ├── ProbablisticTrackingFactorTests.swift │ ├── RandomProjectionTests.swift │ ├── SwitchingMCMCTests.swift │ ├── ValuesStorageTests.swift │ └── VariableAssignmentsTests.swift │ ├── MCMC │ └── MCMCTests.swift │ ├── Optimizers │ ├── CGLSTests.swift │ ├── GradientDescentTests.swift │ └── LMTests.swift │ ├── Probability │ ├── MonteCarloEMTests.swift │ ├── MultivariateGaussianTests.swift │ └── NaiveBayesTests.swift │ └── TestUtilities.swift ├── Util └── BeeAnnotations │ ├── README.md │ ├── annotator.html │ └── app.js ├── doc ├── DifferentiableManifoldRecipe.md ├── ImageOperations.md └── TensorBoardUsage.md └── generate.sh /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | on: [pull_request, push] 2 | name: Standard Testset 3 | jobs: 4 | test: 5 | name: macOS Default 6 | runs-on: macOS-latest 7 | steps: 8 | - name: Checkout 9 | uses: actions/checkout@master 10 | - name: Install toolchain 11 | run: | 12 | brew install geos 13 | pip3 install --user numpy matplotlib shapely 14 | sudo xcode-select -s /Applications/Xcode_12.2.app 15 | wget https://storage.googleapis.com/swift-tensorflow-artifacts/macos-toolchains/swift-tensorflow-DEVELOPMENT-2020-08-26-a-osx.pkg 16 | sudo installer -pkg swift-tensorflow-DEVELOPMENT-2020-08-26-a-osx.pkg -target / 17 | echo "PATH=/Library/Developer/Toolchains/swift-latest/usr/bin:${PATH}" >> $GITHUB_ENV 18 | - name: Build 19 | run: swift build -v 20 | - name: Test 21 | run: swift test -v 22 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | /.build 3 | /Packages 4 | /*.xcodeproj 5 | xcuserdata/ 6 | /.swiftpm 7 | /.vscode 8 | /.idea 9 | /OIST_Data/ 10 | /Results/ 11 | *.npy 12 | *.json -------------------------------------------------------------------------------- /.swiftformat: -------------------------------------------------------------------------------- 1 | --binarygrouping none 2 | --closingparen same-line 3 | --decimalgrouping none 4 | --hexgrouping none 5 | --ifdef no-indent 6 | --indent 2 7 | --octalgrouping none 8 | --ranges no-space 9 | --semicolons never 10 | --wraparguments before-first 11 | --wrapcollections before-first -------------------------------------------------------------------------------- /Package.resolved: -------------------------------------------------------------------------------- 1 | { 2 | "object": { 3 | "pins": [ 4 | { 5 | "package": "CSV.swift", 6 | "repositoryURL": "https://github.com/yaslab/CSV.swift.git", 7 | "state": { 8 | "branch": null, 9 | "revision": "81d2874c51db364d7e1d71b0d99018a294c87ac1", 10 | "version": "2.4.3" 11 | } 12 | }, 13 | { 14 | "package": "Penguin", 15 | "repositoryURL": "https://github.com/saeta/penguin.git", 16 | "state": { 17 | "branch": "main", 18 | "revision": "2ed8978cfc91384f98c245cce3513f4345eec239", 19 | "version": null 20 | } 21 | }, 22 | { 23 | "package": "Plotly", 24 | "repositoryURL": "https://github.com/vojtamolda/Plotly.swift", 25 | "state": { 26 | "branch": null, 27 | "revision": "6e80119ba37b913e5460459556e2bf58f02eba67", 28 | "version": "0.4.0" 29 | } 30 | }, 31 | { 32 | "package": "swift-argument-parser", 33 | "repositoryURL": "https://github.com/apple/swift-argument-parser.git", 34 | "state": { 35 | "branch": null, 36 | "revision": "92646c0cdbaca076c8d3d0207891785b3379cbff", 37 | "version": "0.3.1" 38 | } 39 | }, 40 | { 41 | "package": "Benchmark", 42 | "repositoryURL": "https://github.com/google/swift-benchmark.git", 43 | "state": { 44 | "branch": null, 45 | "revision": "8e0ef8bb7482ab97dcd2cd1d6855bd38921c345d", 46 | "version": "0.1.0" 47 | } 48 | }, 49 | { 50 | "package": "swift-models", 51 | "repositoryURL": "https://github.com/tensorflow/swift-models.git", 52 | "state": { 53 | "branch": null, 54 | "revision": "b2fc0325bf9d476bf2d7a4cd0a09d36486c506e4", 55 | "version": null 56 | } 57 | }, 58 | { 59 | "package": "SwiftProtobuf", 60 | "repositoryURL": "https://github.com/apple/swift-protobuf.git", 61 | "state": { 62 | "branch": null, 63 | "revision": "da9a52be9cd36c63993291ce3f1b65dafcd1e826", 64 | "version": "1.14.0" 65 | } 66 | }, 67 | { 68 | "package": "swift-tools-support-core", 69 | "repositoryURL": "https://github.com/apple/swift-tools-support-core.git", 70 | "state": { 71 | "branch": "swift-5.2-branch", 72 | "revision": "7ecf17a83eab20cbd700d7e45d66c03409bc72d0", 73 | "version": null 74 | } 75 | }, 76 | { 77 | "package": "TensorBoardX", 78 | "repositoryURL": "https://github.com/ProfFan/tensorboardx-s4tf.git", 79 | "state": { 80 | "branch": null, 81 | "revision": "02838220694f4236ba84a83856581b76ee9cf1bc", 82 | "version": "0.1.3" 83 | } 84 | } 85 | ] 86 | }, 87 | "version": 1 88 | } 89 | -------------------------------------------------------------------------------- /Scripts/Fan01.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeDataset 5 | import BeeTracking 6 | import PythonKit 7 | import Foundation 8 | 9 | /// Fan01: AE Tracker, with sampling-based initialization 10 | struct Fan01: ParsableCommand { 11 | @Option(help: "Run on track number x") 12 | var trackId: Int = 0 13 | 14 | @Option(help: "Run for number of frames") 15 | var trackLength: Int = 80 16 | 17 | @Option(help: "Size of feature space") 18 | var featureSize: Int = 30 19 | 20 | @Option(help: "Pretrained weights") 21 | var weightsFile: String? 22 | 23 | // Make sure you have a folder `Results/frank02` before running 24 | func run() { 25 | let np = Python.import("numpy") 26 | let kHiddenDimension = 100 27 | 28 | let dataDir = URL(fileURLWithPath: "./OIST_Data") 29 | let (imageHeight, imageWidth, imageChannels) = 30 | (40, 70, 1) 31 | var rae = DenseRAE( 32 | imageHeight: imageHeight, imageWidth: imageWidth, imageChannels: imageChannels, 33 | hiddenDimension: kHiddenDimension, latentDimension: featureSize 34 | ) 35 | 36 | if let weightsFile = weightsFile { 37 | rae.load(weights: np.load(weightsFile, allow_pickle: true)) 38 | } else { 39 | rae.load(weights: np.load("./oist_rae_weight_\(featureSize).npy", allow_pickle: true)) 40 | } 41 | let (fig, _, _) = runProbabilisticTracker( 42 | directory: dataDir, 43 | encoder: rae, 44 | onTrack: trackId, forFrames: trackLength, withSampling: true, 45 | withFeatureSize: featureSize, 46 | savePatchesIn: "Results/fan01" 47 | ) 48 | 49 | /// Actual track v.s. ground truth track 50 | fig.savefig("Results/fan01/fan01_track\(trackId)_\(featureSize).pdf", bbox_inches: "tight") 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /Scripts/Fan02.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeDataset 5 | import BeeTracking 6 | import PythonKit 7 | import Foundation 8 | import TensorFlow 9 | 10 | /// Fan02: Random Projections Error Landscape 11 | struct Fan02: ParsableCommand { 12 | @Option(help: "Size of feature space") 13 | var featureSize: Int = 100 14 | 15 | // Visualize error landscape of PCA 16 | // Make sure you have a folder `Results/fan02` before running 17 | func run() { 18 | let dataDir = URL(fileURLWithPath: "./OIST_Data") 19 | 20 | let np = Python.import("numpy") 21 | let plt = Python.import("matplotlib.pyplot") 22 | 23 | // train foreground and background model and create tracker 24 | let trainingData = OISTBeeVideo(directory: dataDir, length: 100)! 25 | // let testData = OISTBeeVideo(directory: dataDir, afterIndex: 100, length: forFrames)! 26 | 27 | // let (imageHeight, imageWidth, imageChannels) = (40, 70, 1) 28 | // let encoder = RandomProjection(fromShape: TensorShape([imageHeight, imageWidth, imageChannels]), toFeatureSize: featureSize) 29 | let encoder = PCAEncoder( 30 | withBasis: Tensor(numpy: np.load("./pca_U_\(featureSize).npy"))!, 31 | andMean: Tensor(numpy: np.load("./pca_mu_\(featureSize).npy"))! 32 | ) 33 | 34 | let (fg, _, _) = getTrainingBatches( 35 | dataset: trainingData, boundingBoxSize: (40, 70), 36 | fgBatchSize: 3000, 37 | bgBatchSize: 3000, 38 | fgRandomFrameCount: 100, 39 | bgRandomFrameCount: 100, 40 | useCache: true 41 | ) 42 | 43 | let batchPositive = encoder.encode(fg) 44 | // let foregroundModel = MultivariateGaussian(from:batchPositive, regularizer: 1e-3) 45 | // 46 | // let batchNegative = encoder.encode(bg) 47 | // let backgroundModel = GaussianNB(from: batchNegative, regularizer: 1e-3) 48 | 49 | // print(foregroundModel.covariance_inv!.diagonalPart()) 50 | // print(backgroundModel.sigmas!) 51 | 52 | // for i in 0.. PCAEncoder in 33 | /// Training mode, trains the PPCA model and save to cache file 34 | if training { 35 | let pcaTrainingData = OISTBeeVideo(directory: dataDir, length: 100)! 36 | var statistics = FrameStatistics(Tensor(0.0)) 37 | statistics.mean = Tensor(62.26806976644069) 38 | statistics.standardDeviation = Tensor(37.44683834503672) 39 | let trainingBatch = pcaTrainingData.makeBatch(statistics: statistics, appearanceModelSize: (imageHeight, imageWidth), batchSize: 3000) 40 | let encoder = PCAEncoder(from: trainingBatch, given: featureSize) 41 | np.save("./pca_U_\(featureSize)", encoder.U.makeNumpyArray()) 42 | np.save("./pca_mu_\(featureSize)", encoder.mu.makeNumpyArray()) 43 | return encoder 44 | } else { 45 | /// Just load the cached weight matrix 46 | return PCAEncoder( 47 | withBasis: Tensor(numpy: np.load("./pca_U_\(featureSize).npy"))!, 48 | andMean: Tensor(numpy: np.load("./pca_mu_\(featureSize).npy"))! 49 | ) 50 | } 51 | }() 52 | 53 | let (fig, track, gt) = runProbabilisticTracker( 54 | directory: dataDir, 55 | encoder: pca, 56 | onTrack: trackId, forFrames: trackLength, withSampling: true, 57 | withFeatureSize: featureSize, 58 | savePatchesIn: "Results/fan04" 59 | ) 60 | 61 | /// Actual track v.s. ground truth track 62 | fig.savefig("Results/fan04/fan04_track\(trackId)_\(featureSize).pdf", bbox_inches: "tight") 63 | fig.savefig("Results/fan04/fan04_track\(trackId)_\(featureSize).png", bbox_inches: "tight") 64 | 65 | let json = JSONEncoder() 66 | json.outputFormatting = .prettyPrinted 67 | 68 | let track_data = try! json.encode(track) 69 | try! track_data.write(to: URL(fileURLWithPath: "Results/fan04/fan04_track_\(trackId)_\(featureSize).json")) 70 | 71 | let gt_data = try! json.encode(gt) 72 | try! gt_data.write(to: URL(fileURLWithPath: "Results/fan04/fan04_gt_\(trackId)_\(featureSize).json")) 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /Scripts/Fan05.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeDataset 5 | import BeeTracking 6 | import PythonKit 7 | import Foundation 8 | import TensorFlow 9 | 10 | /// Fan05: Error Landscape 11 | struct Fan05: ParsableCommand { 12 | @Option(help: "Size of feature space") 13 | var featureSize: Int = 100 14 | 15 | @Option(help: "Which frame to show") 16 | var frameId: Int = 0 17 | 18 | @Option(help: "Which track to show") 19 | var trackId: Int = 0 20 | 21 | typealias LikelihoodModel = TrackingLikelihoodModel 22 | 23 | func getTrainingDataEM( 24 | from dataset: OISTBeeVideo, 25 | numberForeground: Int = 300, 26 | numberBackground: Int = 300 27 | ) -> [LikelihoodModel.Datum] { 28 | let bgBoxes = dataset.makeBackgroundBoundingBoxes(patchSize: (40, 70), batchSize: numberBackground).map { 29 | (frame: $0.frame, type: LikelihoodModel.PatchType.bg, obb: $0.obb) 30 | } 31 | let fgBoxes = dataset.makeForegroundBoundingBoxes(patchSize: (40, 70), batchSize: numberForeground).map { 32 | (frame: $0.frame, type: LikelihoodModel.PatchType.fg, obb: $0.obb) 33 | } 34 | 35 | return fgBoxes + bgBoxes 36 | } 37 | 38 | // Visualize error landscape of PCA 39 | // Make sure you have a folder `Results/fan05` before running 40 | func run() { 41 | let dataDir = URL(fileURLWithPath: "./OIST_Data") 42 | 43 | let np = Python.import("numpy") 44 | 45 | // train foreground and background model and create tracker 46 | let trainingDataset = OISTBeeVideo(directory: dataDir, length: 100)! 47 | let trainingData = getTrainingDataEM(from: trainingDataset) 48 | 49 | let generator = ARC4RandomNumberGenerator(seed: 42) 50 | var em = MonteCarloEM(sourceOfEntropy: generator) 51 | 52 | let kHiddenDimension = 100 53 | let trackingModel = em.run( 54 | with: trainingData, 55 | iterationCount: 3, 56 | hook: { i, _, _ in 57 | print("EM run iteration \(i)") 58 | }, 59 | given: LikelihoodModel.HyperParameters( 60 | encoder: PretrainedDenseRAE.HyperParameters(hiddenDimension: kHiddenDimension, latentDimension: featureSize, weightFile: "./oist_rae_weight_\(featureSize).npy") 61 | ) 62 | ) 63 | 64 | var statistics = FrameStatistics(Tensor([1.0])) 65 | statistics.mean = Tensor(0.0) 66 | statistics.standardDeviation = Tensor(1.0) 67 | 68 | let deltaXRange = Array(-60..<60).map { Double($0) } 69 | let deltaYRange = Array(-40..<40).map { Double($0) } 70 | 71 | let datasetToShow = OISTBeeVideo(directory: dataDir, afterIndex: frameId - 1, length: 2)! 72 | let frame = datasetToShow.frames[1] 73 | let pose = datasetToShow.tracks[trackId].boxes[0].center 74 | let (fig, _) = plotErrorPlaneTranslation( 75 | frame: frame, 76 | at: pose, 77 | deltaXs: deltaXRange, 78 | deltaYs: deltaYRange, 79 | statistics: statistics, 80 | encoder: trackingModel.encoder, 81 | foregroundModel: trackingModel.foregroundModel, 82 | backgroundModel: trackingModel.backgroundModel 83 | ) 84 | fig.savefig("Results/fan05/fan05_em_ae_mg_mg_\(trackId)_\(frameId)_\(featureSize).pdf", bbox_inches: "tight") 85 | fig.savefig("Results/fan05/fan05_em_ae_mg_mg_\(trackId)_\(frameId)_\(featureSize).png", bbox_inches: "tight") 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /Scripts/Fan10.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeDataset 5 | import BeeTracking 6 | import TensorFlow 7 | import PythonKit 8 | import Foundation 9 | 10 | /// Fan10: RP Tracker, using the new tracking model 11 | struct Fan10: ParsableCommand { 12 | 13 | typealias LikelihoodModel = TrackingLikelihoodModel 14 | 15 | @Option(help: "Run on track number x") 16 | var trackId: Int = 3 17 | 18 | @Option(help: "Run for number of frames") 19 | var trackLength: Int = 80 20 | 21 | @Option(help: "Size of feature space") 22 | var featureSize: Int = 20 23 | 24 | @Flag(help: "Training mode") 25 | var training: Bool = false 26 | 27 | func getTrainingDataEM( 28 | from dataset: OISTBeeVideo, 29 | numberForeground: Int = 300, 30 | numberBackground: Int = 300 31 | ) -> [LikelihoodModel.Datum] { 32 | let bgBoxes = dataset.makeBackgroundBoundingBoxes(patchSize: (40, 70), batchSize: numberBackground).map { 33 | (frame: $0.frame, type: LikelihoodModel.PatchType.bg, obb: $0.obb) 34 | } 35 | let fgBoxes = dataset.makeForegroundBoundingBoxes(patchSize: (40, 70), batchSize: numberForeground).map { 36 | (frame: $0.frame, type: LikelihoodModel.PatchType.fg, obb: $0.obb) 37 | } 38 | 39 | return fgBoxes + bgBoxes 40 | } 41 | 42 | // Just runs an RP tracker and saves image to file 43 | // Make sure you have a folder `Results/fan10` before running 44 | func run() { 45 | let kHiddenDimension = 100 46 | let dataDir = URL(fileURLWithPath: "./OIST_Data") 47 | 48 | let generator = ARC4RandomNumberGenerator(seed: 42) 49 | var em = MonteCarloEM(sourceOfEntropy: generator) 50 | 51 | let trainingDataset = OISTBeeVideo(directory: dataDir, length: 30)! 52 | 53 | let trainingData = getTrainingDataEM(from: trainingDataset) 54 | 55 | let trackingModel = em.run( 56 | with: trainingData, 57 | iterationCount: 3, 58 | hook: { i, _, _ in 59 | print("EM run iteration \(i)") 60 | }, 61 | given: LikelihoodModel.HyperParameters( 62 | encoder: PretrainedDenseRAE.HyperParameters(hiddenDimension: kHiddenDimension, latentDimension: featureSize, weightFile: "./oist_rae_weight_\(featureSize).npy") 63 | ) 64 | ) 65 | let exprName = "fan10_ae_mg_mg_track\(trackId)_\(featureSize)" 66 | let imagesPath = "Results/fan10/\(exprName)" 67 | if !FileManager.default.fileExists(atPath: imagesPath) { 68 | do { 69 | try FileManager.default.createDirectory(atPath: imagesPath, withIntermediateDirectories: true, attributes: nil) 70 | } catch { 71 | print(error.localizedDescription); 72 | } 73 | } 74 | 75 | let (fig, track, gt) = runProbabilisticTracker( 76 | directory: dataDir, 77 | likelihoodModel: trackingModel, 78 | onTrack: trackId, forFrames: trackLength, withSampling: true, 79 | withFeatureSize: featureSize, 80 | savePatchesIn: "Results/fan10/\(exprName)" 81 | ) 82 | 83 | /// Actual track v.s. ground truth track 84 | fig.savefig("Results/fan10/\(exprName).pdf", bbox_inches: "tight") 85 | fig.savefig("Results/fan10/\(exprName).png", bbox_inches: "tight") 86 | 87 | let json = JSONEncoder() 88 | json.outputFormatting = .prettyPrinted 89 | 90 | let track_data = try! json.encode(track) 91 | try! track_data.write(to: URL(fileURLWithPath: "Results/fan10/\(exprName)_track.json")) 92 | 93 | let gt_data = try! json.encode(gt) 94 | try! gt_data.write(to: URL(fileURLWithPath: "Results/fan10/\(exprName)_gt.json")) 95 | } 96 | } 97 | -------------------------------------------------------------------------------- /Scripts/Fan12.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeDataset 5 | import BeeTracking 6 | import TensorFlow 7 | import PythonKit 8 | import Foundation 9 | 10 | /// Fan12: RAE training 11 | struct Fan12: ParsableCommand { 12 | typealias LikelihoodModel = TrackingLikelihoodModel 13 | 14 | @Option(help: "Size of feature space") 15 | var featureSize: Int = 5 16 | 17 | @Flag(help: "Training mode") 18 | var training: Bool = false 19 | 20 | func getTrainingData( 21 | from dataset: OISTBeeVideo, 22 | numberForeground: Int = 3000 23 | ) -> [LikelihoodModel.Datum] { 24 | let fgBoxes = dataset.makeForegroundBoundingBoxes(patchSize: (40, 70), batchSize: numberForeground).map { 25 | (frame: $0.frame, type: LikelihoodModel.PatchType.fg, obb: $0.obb) 26 | } 27 | 28 | return fgBoxes 29 | } 30 | 31 | // Just runs an RP tracker and saves image to file 32 | // Make sure you have a folder `Results/fan12` before running 33 | func run() { 34 | let kHiddenDimension = 100 35 | let dataDir = URL(fileURLWithPath: "./OIST_Data") 36 | 37 | let trainingDataset = OISTBeeVideo(directory: dataDir, length: 100)! 38 | 39 | let trainingData = Tensor(stacking: getTrainingData(from: trainingDataset).map { $0.frame!.patch(at: $0.obb) }) 40 | 41 | print("Training...") 42 | let rae: PretrainedDenseRAE = PretrainedDenseRAE( 43 | trainFrom: trainingData, 44 | given: PretrainedDenseRAE.HyperParameters(hiddenDimension: kHiddenDimension, latentDimension: featureSize, weightFile: "") 45 | ) 46 | 47 | rae.save(to: "./oist_rae_weight_\(featureSize).npy") 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /Scripts/Fan14.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeDataset 5 | import BeeTracking 6 | import TensorFlow 7 | import PythonKit 8 | import Foundation 9 | 10 | /// Fan14: Aggregate data 11 | struct Fan14: ParsableCommand { 12 | 13 | @Option(help: "Run for number of frames") 14 | var trackLength: Int = 80 15 | 16 | @Option(help: "Size of feature space") 17 | var featureSize: Int = 20 18 | 19 | @Flag(help: "Training mode") 20 | var training: Bool = false 21 | 22 | func readExperimentData(name: String) -> SubsequenceMetrics { 23 | let decoder = JSONDecoder() 24 | let trackPath = "Results/fan13/\(name)_track.json" 25 | let gtPath = "Results/fan13/\(name)_gt.json" 26 | let decodedTrack = try! decoder.decode([Pose2].self, from: Data(contentsOf: URL(fileURLWithPath: trackPath))).map { OrientedBoundingBox(center: $0, rows: 40, cols: 70)} 27 | let decodedGt = try! decoder.decode([Pose2].self, from: Data(contentsOf: URL(fileURLWithPath: gtPath))).map { OrientedBoundingBox(center: $0, rows: 40, cols: 70)} 28 | return SubsequenceMetrics(groundTruth: decodedGt, prediction: decodedTrack) 29 | } 30 | 31 | func run() { 32 | var metrics: [String: [SubsequenceMetrics]] = [:] 33 | 34 | let frameIds = 0..<10 35 | 36 | metrics["RP"] = frameIds.map { trackId in 37 | let exprNameNoEM = "fan13_rp_mg_mg_noem_track\(trackId)_\(featureSize)" 38 | 39 | return readExperimentData(name: exprNameNoEM) 40 | } 41 | 42 | metrics["PCA"] = frameIds.map { trackId in 43 | let exprNameNoEM = "fan13_pca_mg_mg_noem_track\(trackId)_\(featureSize)" 44 | 45 | return readExperimentData(name: exprNameNoEM) 46 | } 47 | 48 | metrics["AE"] = frameIds.map { trackId in 49 | let exprNameNoEM = "fan13_ae_mg_mg_noem_track\(trackId)_\(featureSize)" 50 | 51 | return readExperimentData(name: exprNameNoEM) 52 | } 53 | 54 | metrics["PCA+EM"] = frameIds.map { trackId in 55 | let exprNameNoEM = "fan13_pca_mg_mg_em_track\(trackId)_\(featureSize)" 56 | 57 | return readExperimentData(name: exprNameNoEM) 58 | } 59 | 60 | metrics["AE+EM"] = frameIds.map { trackId in 61 | let exprNameWithEM = "fan13_ae_mg_mg_track\(trackId)_\(featureSize)" 62 | 63 | return readExperimentData(name: exprNameWithEM) 64 | } 65 | 66 | var ssm: [String: ExpectedAverageOverlap] = [:] 67 | 68 | metrics.forEach { ssm[$0] = ExpectedAverageOverlap($1) } 69 | 70 | if !FileManager.default.fileExists(atPath: "Results/fan14") { 71 | do { 72 | try FileManager.default.createDirectory(atPath: "Results/fan14", withIntermediateDirectories: true, attributes: nil) 73 | } catch { 74 | print(error.localizedDescription); 75 | } 76 | } 77 | 78 | // Now create trajectory and metrics plot 79 | let plt = Python.import("matplotlib.pyplot") 80 | let (fig, ax) = plt.subplots(1, 1, figsize: Python.tuple([8,6])).tuple2 81 | 82 | ssm.forEach { 83 | ax.plot($1.curve, label: $0) 84 | } 85 | 86 | // ax.plot(Array(zip(ssmWithEM.curve, ssmNoEM.curve).map { $0 - $1 }), label: "Diff") 87 | ax.set_title("Expected Average Overlap") 88 | ax.set_xlabel("Frame") 89 | ax.set_ylabel("Overlap") 90 | ax.legend() 91 | 92 | fig.savefig("Results/fan14/overlap_comp.pdf", bbox_inches: "tight") 93 | fig.savefig("Results/fan14/overlap_comp.png", bbox_inches: "tight") 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /Scripts/Frank01.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeTracking 5 | import PythonKit 6 | import Foundation 7 | 8 | /// Random Projections Baseline Tracker 9 | struct Frank01: ParsableCommand { 10 | @Option(help: "Run on track number x") 11 | var trackId: Int = 0 12 | 13 | @Option(help: "Run for number of frames") 14 | var trackLength: Int = 80 15 | 16 | // Just runs an RP tracker and saves image to file 17 | func run() { 18 | let (fig, _, _) = runRPTracker(directory: URL(fileURLWithPath: "./OIST_Data"), onTrack: trackId, forFrames: trackLength) 19 | fig.savefig("frank01.pdf", bbox_inches: "tight") 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /Scripts/Frank02.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeDataset 5 | import BeeTracking 6 | import PythonKit 7 | import Foundation 8 | 9 | /// Random Projections Tracker, with sampling-based initialization 10 | struct Frank02: ParsableCommand { 11 | @Option(help: "Run on track number x") 12 | var trackId: Int = 0 13 | 14 | @Option(help: "Run for number of frames") 15 | var trackLength: Int = 80 16 | 17 | @Option(help: "Size of feature space") 18 | var featureSize: Int = 100 19 | 20 | // Just runs an RP tracker and saves image to file 21 | // Make sure you have a folder `Results/frank02` before running 22 | // TODO: use the generic version instead, remove runRPTracker 23 | func run() { 24 | let (fig, _, _) = runRPTracker( 25 | directory: URL(fileURLWithPath: "./OIST_Data"), 26 | onTrack: trackId, forFrames: trackLength, withSampling: true, 27 | withFeatureSize: featureSize, 28 | savePatchesIn: "Results/frank02" 29 | ) 30 | 31 | /// Actual track v.s. ground truth track 32 | fig.savefig("Results/frank02/frank02_track\(trackId)_\(featureSize).pdf", bbox_inches: "tight") 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /Scripts/Frank03.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeTracking 5 | import PythonKit 6 | import Foundation 7 | 8 | /// Random Projections Tracker, with EM-based training 9 | struct Frank03: ParsableCommand { 10 | @Option(help: "Run on track number x") 11 | var trackId: Int = 0 12 | 13 | @Option(help: "Run for number of frames") 14 | var trackLength: Int = 80 15 | 16 | // Just runs an RP tracker and saves image to file 17 | func run() { 18 | let (fig, _, _) = runRPTracker(directory: URL(fileURLWithPath: "./OIST_Data"), onTrack: trackId, forFrames: trackLength, withSampling:true, usingEM: true) 19 | fig.savefig("frank01.pdf", bbox_inches: "tight") 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /Scripts/Frank04.swift: -------------------------------------------------------------------------------- 1 | import ArgumentParser 2 | 3 | import SwiftFusion 4 | import BeeTracking 5 | import BeeDataset 6 | import PythonKit 7 | import Foundation 8 | 9 | /// Saving labels divided by 2 10 | struct Frank04: ParsableCommand { 11 | 12 | func writeOneFile(filename: URL, labels: [OISTBeeLabel]) { 13 | var lines = "" 14 | for label in labels { 15 | let converted_label = OISTBeeLabel( 16 | frameIndex: label.frameIndex, 17 | label: label.label, 18 | rawLocation: (label.rawLocation.0 / 2.0, label.rawLocation.1 / 2.0, label.rawLocation.2), 19 | offset: (label.offset.0 / 2, label.offset.1 / 2) 20 | ) 21 | lines = lines.appending("\(converted_label.toString())\n") 22 | } 23 | 24 | do { 25 | try lines.write(to: filename, atomically: true, encoding: .utf8) 26 | } catch { 27 | print("error creating file") 28 | } 29 | } 30 | 31 | func run() { 32 | let dataset = OISTBeeVideo(deferLoadingFrames: true)! 33 | 34 | print(dataset.labels.count) 35 | for (index, labels) in dataset.labels.enumerated() { 36 | let frameId = dataset.frameIds[index] 37 | let filename = formOISTFilename(dataset.fps, frameId) 38 | print("Write \(filename)") 39 | writeOneFile(filename: URL(fileURLWithPath: "./OIST_Data"), labels: labels) 40 | } 41 | 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /Scripts/README.md: -------------------------------------------------------------------------------- 1 | swift run -c release Scripts frank01 -------------------------------------------------------------------------------- /Scripts/main.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import ArgumentParser 16 | import PenguinParallelWithFoundation 17 | 18 | struct Scripts: ParsableCommand { 19 | static var configuration = CommandConfiguration( 20 | subcommands: [Andrew01.self, Fan01.self, Fan02.self, Fan03.self, Fan04.self, Fan05.self, Fan10.self, Fan12.self, Fan13.self, Fan14.self, 21 | Frank01.self, Frank02.self, Frank03.self, Frank04.self]) 22 | } 23 | 24 | // It is important to set the global threadpool before doing anything else, so that nothing 25 | // accidentally uses the default threadpool. 26 | ComputeThreadPools.global = 27 | NonBlockingThreadPool(name: "mypool", threadCount: 12) 28 | 29 | Scripts.main() 30 | -------------------------------------------------------------------------------- /Sources/BeeDataset/BeeFrames.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import Foundation 17 | import ModelSupport 18 | import SwiftFusion 19 | import TensorFlow 20 | 21 | /// An ordered collection of frames from a video of bees. 22 | /// 23 | /// Note: Use `BeeVideo` instead. This is for backwards compatibility with old code. 24 | public struct BeeFrames: RandomAccessCollection { 25 | public let directory: URL 26 | public let frameCount: Int 27 | 28 | /// Creates a `BeeFrames` from the sequence named `sequenceName`, automatically downloading the 29 | /// dataset from the internet if it is not already present on the local system. 30 | /// 31 | /// The dataset is originally frmo https://www.cc.gatech.edu/~borg/ijcv_psslds/, and contains 32 | /// sequences named "seq1", "seq2", ..., "seq6". 33 | public init?(sequenceName: String) { 34 | let dir = downloadBeeDatasetIfNotPresent() 35 | self.init(directory: dir.appendingPathComponent("frames").appendingPathComponent(sequenceName)) 36 | } 37 | 38 | /// Creates a `BeeFrames` from the data in the given `directory`. 39 | /// 40 | /// The directory must contain: 41 | /// - A file named "index.txt" whose first line is the total number of frames. 42 | /// - Frames named "frame1.png", "frame2.png", etc. 43 | public init?(directory: URL) { 44 | let indexFile = directory.appendingPathComponent("index.txt") 45 | guard let index = try? String(contentsOf: indexFile) else { return nil } 46 | guard let indexLine = index.split(separator: "\n").first else { 47 | fatalError("index.txt empty") 48 | } 49 | guard let frameCount = Int(indexLine) else { 50 | fatalError("index.txt first line is not a number") 51 | } 52 | self.directory = directory 53 | self.frameCount = frameCount 54 | } 55 | 56 | public var startIndex: Int { 0 } 57 | public var endIndex: Int { frameCount } 58 | 59 | public func index(before i: Int) -> Int { i - 1 } 60 | public func index(after i: Int) -> Int { i + 1 } 61 | 62 | public subscript(index: Int) -> Tensor { 63 | return Tensor( 64 | Image(contentsOf: directory.appendingPathComponent("frame\(index + 1).png")).tensor) 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /Sources/BeeDataset/BeeOrientedBoundingBoxes.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import Foundation 17 | import SwiftFusion 18 | 19 | /// Reads an array of oriented bounding boxes from `file`. 20 | /// 21 | /// Each line of the file is an oriented bounding box, in the format: 22 | /// 23 | /// 24 | /// Note: Use `BeeVideo` instead. This is for backwards compatibility with old code. 25 | public func beeOrientedBoundingBoxes(file: URL) -> [OrientedBoundingBox]? { 26 | // The dimensions of the bee bounding box are hardcoded because they're the same for all the 27 | // sequences in the dataset. 28 | let rows = 28 29 | let cols = 62 30 | guard let lines = try? String(contentsOf: file) else { return nil } 31 | return lines.split(separator: "\n").compactMap { line in 32 | let lineCols = line.split(separator: " ") 33 | guard lineCols.count == 3, 34 | let r = Double(lineCols[0]), 35 | let x = Double(lineCols[1]), 36 | let y = Double(lineCols[2]) 37 | else { return nil } 38 | return OrientedBoundingBox(center: Pose2(Rot2(r), Vector2(x, y)), rows: rows, cols: cols) 39 | } 40 | } 41 | 42 | /// Returns an array of oriented bounding boxes that track a bee in the bee sequence named 43 | /// `sequenceName`, automatically downloading the dataset frmo the internet if it is not already 44 | /// present on the local system. 45 | /// 46 | /// The dataset is originally frmo https://www.cc.gatech.edu/~borg/ijcv_psslds/, and contains 47 | /// sequences named "seq1", "seq2", ..., "seq6". 48 | /// 49 | /// WARNING: The indices of the bounding boxes seem like they are not perfectly aligned with the 50 | /// indices of the `BeeFrames` from the same sequence. (For example, frame `i` might correspond to 51 | /// bounding box `i - 1` or `i - 2`). 52 | /// 53 | /// Note: Use `BeeVideo` instead. This is for backwards compatibility with old code. 54 | public func beeOrientedBoundingBoxes(sequenceName: String) -> [OrientedBoundingBox]? { 55 | let dir = downloadBeeDatasetIfNotPresent() 56 | return beeOrientedBoundingBoxes( 57 | file: dir.appendingPathComponent("obbs").appendingPathComponent("\(sequenceName).txt")) 58 | } 59 | -------------------------------------------------------------------------------- /Sources/BeeDataset/Download.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import Foundation 17 | import ModelSupport 18 | 19 | /// Downloads the bee dataset (if it's not already present), and returns its URL on the local 20 | /// system. 21 | /// 22 | /// Note: Use `BeeVideo` instead. This is for backwards compatibility with old code. 23 | internal func downloadBeeDatasetIfNotPresent() -> URL { 24 | let downloadDir = DatasetUtilities.defaultDirectory.appendingPathComponent( 25 | "bees_v2", isDirectory: true) 26 | let directoryExists = FileManager.default.fileExists(atPath: downloadDir.path) 27 | let contentsOfDir = try? FileManager.default.contentsOfDirectory(atPath: downloadDir.path) 28 | let directoryEmpty = (contentsOfDir == nil) || (contentsOfDir!.isEmpty) 29 | 30 | guard !directoryExists || directoryEmpty else { return downloadDir } 31 | 32 | let remoteRoot = URL( 33 | string: "https://storage.googleapis.com/swift-tensorflow-misc-files/beetracking")! 34 | 35 | let _ = DatasetUtilities.downloadResource( 36 | filename: "beedata_v2", fileExtension: "tar.gz", 37 | remoteRoot: remoteRoot, localStorageDirectory: downloadDir 38 | ) 39 | 40 | return downloadDir 41 | } 42 | -------------------------------------------------------------------------------- /Sources/BeeDataset/Visualization.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import SwiftFusion 16 | import TensorFlow 17 | import Plotly 18 | import ModelSupport 19 | import Foundation 20 | 21 | /// Creates a Plotly figure that displays `frame`, with optional `boxes` overlaid on 22 | /// them. 23 | public func plot( 24 | _ frame: Tensor, boxes: [(name: String, OrientedBoundingBox)] = [], 25 | margin: Double = 30, scale: Double = 1 26 | ) -> Plotly.Figure { 27 | let rows = Double(frame.shape[0]) 28 | let cols = Double(frame.shape[1]) 29 | 30 | // Axis settings: 31 | // - no grid 32 | // - range is the image size 33 | // - scale is anchored, to preserve image aspect ratio 34 | // - y axis reversed so that everything is in "(u, v)" coordinates 35 | let xAx = Layout.XAxis(range: [0, InfoArray(cols)], showGrid: false) 36 | let yAx = Layout.YAxis( 37 | autoRange: .reversed, range: [0, InfoArray(rows)], scaleAnchor: .xAxis(xAx), showGrid: false) 38 | 39 | let tmpPath = URL(fileURLWithPath: "tmpForPlotlyDisplay.png") 40 | ModelSupport.Image(Tensor(frame)).save(to: tmpPath) 41 | let imageData = try! "data:image/png;base64," + Data(contentsOf: tmpPath).base64EncodedString() 42 | 43 | return Figure( 44 | data: [ 45 | // Dummy data because Plotly is confused when there is no data. 46 | Scatter( 47 | x: [0, cols], y: [0, rows], 48 | mode: .markers, marker: Shared.GradientMarker(opacity: 0), 49 | xAxis: xAx, yAxis: yAx 50 | ) 51 | ] + boxes.map { box in 52 | Scatter( 53 | name: box.name, 54 | x: box.1.corners.map { $0.x }, 55 | y: box.1.corners.map { $0.y }, 56 | xAxis: xAx, 57 | yAxis: yAx 58 | ) 59 | }, 60 | layout: Layout( 61 | width: cols * scale + 2 * margin, 62 | height: rows * scale + 2 * margin, 63 | margin: Layout.Margin(l: margin, r: margin, t: margin, b: margin), 64 | images: [ 65 | Layout.Image( 66 | visible: true, 67 | source: imageData, 68 | layer: .below, 69 | xSize: cols, ySize: rows, 70 | sizing: .stretch, 71 | x: 0, y: 0, xReference: .xAxis(xAx), yReference: .yAxis(yAx) 72 | ) 73 | ] 74 | ) 75 | ) 76 | } -------------------------------------------------------------------------------- /Sources/BeeTracking/AppearanceRAE+Serialization.swift: -------------------------------------------------------------------------------- 1 | import PythonKit 2 | import TensorFlow 3 | 4 | extension Dense where Scalar: NumpyScalarCompatible { 5 | /// Loads weights and bias from the numpy arrays in `weights`. 6 | /// 7 | /// `weights[0]` is the dense weight matrix and `weights[1]` is the bias. 8 | mutating func load(weights: PythonObject) { 9 | let weight = Tensor(numpy: weights[0])! 10 | let bias = Tensor(numpy: weights[1])! 11 | precondition( 12 | self.weight.shape == weight.shape, 13 | "expected weight matrix \(self.weight.shape) but got \(weight.shape)") 14 | precondition( 15 | self.bias.shape == bias.shape, "expected bias \(self.bias.shape) but got \(bias.shape)") 16 | self.weight = weight 17 | self.bias = bias 18 | } 19 | 20 | /// The weight and bias as numpy arrays. 21 | /// 22 | /// `numpyWeights[0]` is the dense weight matrix and `numpyWeights[1]` is the bias. 23 | var numpyWeights: PythonObject { 24 | Python.list([self.weight.makeNumpyArray(), self.bias.makeNumpyArray()]) 25 | } 26 | } 27 | 28 | extension Conv2D where Scalar: NumpyScalarCompatible { 29 | /// Loads filter and bias from the numpy arrays in `weights`. 30 | /// 31 | /// `weights[0]` is the filter and `weights[1]` is the bias. 32 | mutating func load(weights: PythonObject) { 33 | let filter = Tensor(numpy: weights[0])! 34 | let bias = Tensor(numpy: weights[1])! 35 | precondition( 36 | self.filter.shape == filter.shape, 37 | "expected filter matrix \(self.filter.shape) but got \(filter.shape)") 38 | precondition( 39 | self.bias.shape == bias.shape, "expected bias \(self.bias.shape) but got \(bias.shape)") 40 | self.filter = filter 41 | self.bias = bias 42 | } 43 | 44 | /// The filter and bias as numpy arrays. 45 | /// 46 | /// `numpyWeights[0]` is the filter and `numpyWeights[1]` is the bias. 47 | var numpyWeights: PythonObject { 48 | Python.list([self.filter.makeNumpyArray(), self.bias.makeNumpyArray()]) 49 | } 50 | } 51 | 52 | extension DenseRAE { 53 | /// Loads model weights from the numpy arrays in `weights`. 54 | public mutating func load(weights: PythonObject) { 55 | self.encoder_conv1.load(weights: weights[0..<2]) 56 | self.encoder1.load(weights: weights[2..<4]) 57 | self.encoder2.load(weights: weights[4..<6]) 58 | self.decoder1.load(weights: weights[6..<8]) 59 | self.decoder2.load(weights: weights[8..<10]) 60 | self.decoder_conv1.load(weights: weights[10..<12]) 61 | } 62 | 63 | /// The model weights as numpy arrays. 64 | public var numpyWeights: PythonObject { 65 | [ 66 | self.encoder_conv1.numpyWeights, 67 | self.encoder1.numpyWeights, 68 | self.encoder2.numpyWeights, 69 | self.decoder1.numpyWeights, 70 | self.decoder2.numpyWeights, 71 | self.decoder_conv1.numpyWeights 72 | ].reduce([], +) 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /Sources/BeeTracking/FrameStatistics.swift: -------------------------------------------------------------------------------- 1 | import TensorFlow 2 | 3 | /// The mean and standard deviation of a collection of frames. 4 | public struct FrameStatistics { 5 | public var mean: Tensor 6 | public var standardDeviation: Tensor 7 | 8 | /// Creates an instance containing the statistics for `frames`. 9 | public init(_ frames: Tensor) { 10 | self.mean = frames.mean() 11 | self.standardDeviation = frames.standardDeviation() 12 | } 13 | 14 | /// Returns `v`, normalized to have mean `0` and standard deviation `1`. 15 | public func normalized(_ v: Tensor) -> Tensor { 16 | return (v - mean) / standardDeviation 17 | } 18 | 19 | /// Returns `n` scaled and shifted so that its mean and standard deviation are `self.mean` 20 | /// and `self.standardDeviation`. 21 | /// 22 | /// Requires that `n` has mean `0` and standard deviation `1`. 23 | public func unnormalized(_ n: Tensor) -> Tensor { 24 | return n * standardDeviation + mean 25 | } 26 | 27 | /// Returns `v`, normalized to have mean `0` and standard deviation `1`. 28 | public func normalized(_ v: Tensor) -> Tensor { 29 | return (v - Tensor(mean)) / Tensor(standardDeviation) 30 | } 31 | 32 | /// Returns `n` scaled and shifted so that its mean and standard deviation are `self.mean` 33 | /// and `self.standardDeviation`. 34 | /// 35 | /// Requires that `n` has mean `0` and standard deviation `1`. 36 | public func unnormalized(_ n: Tensor) -> Tensor { 37 | return n * Tensor(standardDeviation) + Tensor(mean) 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /Sources/BeeTracking/NaiveBayesAETracker.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import SwiftFusion 16 | import TensorFlow 17 | import PenguinStructures 18 | 19 | /// Returns a tracking configuration for a tracker using an RAE. 20 | /// 21 | /// Parameter model: The RAE model to use. 22 | /// Parameter statistics: Normalization statistics for the frames. 23 | /// Parameter frames: The frames of the video where we want to run tracking. 24 | /// Parameter targetSize: The size of the target in the frames. 25 | public func makeNaiveBayesAETracker( 26 | model: DenseRAE, 27 | statistics: FrameStatistics, 28 | frames: [Tensor], 29 | targetSize: (Int, Int), 30 | foregroundModel: MultivariateGaussian, 31 | backgroundModel: GaussianNB 32 | ) -> TrackingConfiguration> { 33 | var variableTemplate = VariableAssignments() 34 | var frameVariableIDs = [Tuple1>]() 35 | for _ in 0.. () in 46 | let (poseID) = unpack(variables) 47 | let (pose) = unpack(values) 48 | graph.store(WeightedPriorFactorPose2(poseID, pose, weight: 1e0, rotWeight: 1e2)) 49 | }, 50 | addTrackingFactor: { (variables, frame, graph) -> () in 51 | let (poseID) = unpack(variables) 52 | graph.store( 53 | ProbablisticTrackingFactor(poseID, 54 | measurement: statistics.normalized(frame), 55 | encoder: model, 56 | patchSize: targetSize, 57 | appearanceModelSize: targetSize, 58 | foregroundModel: foregroundModel, 59 | backgroundModel: backgroundModel, 60 | maxPossibleNegativity: 1e1 61 | ) 62 | ) 63 | }, 64 | addBetweenFactor: { (variables1, variables2, graph) -> () in 65 | let (poseID1) = unpack(variables1) 66 | let (poseID2) = unpack(variables2) 67 | graph.store(WeightedBetweenFactorPose2SD(poseID1, poseID2, Pose2(), sdX: 8, sdY: 4.6, sdTheta: 0.3)) 68 | }, 69 | addFixedBetweenFactor: { (values, variables, graph) -> () in 70 | let (prior) = unpack(values) 71 | let (poseID) = unpack(variables) 72 | graph.store(WeightedPriorFactorPose2SD(poseID, prior, sdX: 8, sdY: 4.6, sdTheta: 0.3)) 73 | }) 74 | } 75 | 76 | /// Returns `t` as a Swift tuple. 77 | fileprivate func unpack(_ t: Tuple2) -> (A, B) { 78 | return (t.head, t.tail.head) 79 | } 80 | /// Returns `t` as a Swift tuple. 81 | fileprivate func unpack(_ t: Tuple1) -> (A) { 82 | return (t.head) 83 | } 84 | -------------------------------------------------------------------------------- /Sources/BeeTracking/PPCATracker.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import SwiftFusion 16 | import TensorFlow 17 | import PenguinStructures 18 | 19 | /// Returns a tracking configuration for a raw pixel tracker. 20 | /// 21 | /// Parameter frames: The frames of the video where we want to run tracking. 22 | /// Parameter target: The pixels of the target. 23 | public func makeRawPixelTracker( 24 | frames: [Tensor], 25 | target: Tensor 26 | ) -> TrackingConfiguration> { 27 | var variableTemplate = VariableAssignments() 28 | var frameVariableIDs = [Tuple1>]() 29 | for _ in 0.. () in 39 | let poseID = variables.head 40 | let pose = values.head 41 | graph.store(WeightedPriorFactor(poseID, pose, weight: 1e0)) 42 | }, 43 | addTrackingFactor: { (variables, frame, graph) -> () in 44 | let poseID = variables.head 45 | graph.store( 46 | RawPixelTrackingFactor(poseID, measurement: frame, target: Tensor(target))) 47 | }, 48 | addBetweenFactor: { (variables1, variables2, graph) -> () in 49 | let poseID1 = variables1.head 50 | let poseID2 = variables2.head 51 | graph.store(WeightedBetweenFactor(poseID1, poseID2, Pose2(), weight: 1e0)) 52 | }) 53 | } 54 | -------------------------------------------------------------------------------- /Sources/BeeTracking/RAETracker.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import SwiftFusion 16 | import TensorFlow 17 | import PenguinStructures 18 | 19 | /// Returns a tracking configuration for a tracker using an RAE. 20 | /// 21 | /// Parameter model: The RAE model to use. 22 | /// Parameter statistics: Normalization statistics for the frames. 23 | /// Parameter frames: The frames of the video where we want to run tracking. 24 | /// Parameter targetSize: The size of the target in the frames. 25 | public func makeRAETracker( 26 | model: DenseRAE, 27 | statistics: FrameStatistics, 28 | frames: [Tensor], 29 | targetSize: (Int, Int) 30 | ) -> TrackingConfiguration> { 31 | var variableTemplate = VariableAssignments() 32 | var frameVariableIDs = [Tuple2, TypedID>]() 33 | for _ in 0.. () in 44 | let (poseID, latentID) = unpack(variables) 45 | let (pose, latent) = unpack(values) 46 | graph.store(WeightedPriorFactor(poseID, pose, weight: 1e-2)) 47 | graph.store(WeightedPriorFactor(latentID, latent, weight: 1e2)) 48 | }, 49 | addTrackingFactor: { (variables, frame, graph) -> () in 50 | let (poseID, latentID) = unpack(variables) 51 | graph.store( 52 | AppearanceTrackingFactor( 53 | poseID, latentID, 54 | measurement: statistics.normalized(frame), 55 | appearanceModel: { x in 56 | model.decode(x.expandingShape(at: 0)).squeezingShape(at: 0) 57 | }, 58 | appearanceModelJacobian: { x in 59 | model.decodeJacobian(x.expandingShape(at: 0)) 60 | .reshaped(to: [model.imageHeight, model.imageWidth, model.imageChannels, model.latentDimension]) 61 | }, 62 | targetSize: targetSize)) 63 | }, 64 | addBetweenFactor: { (variables1, variables2, graph) -> () in 65 | let (poseID1, latentID1) = unpack(variables1) 66 | let (poseID2, latentID2) = unpack(variables2) 67 | graph.store(WeightedBetweenFactor(poseID1, poseID2, Pose2(), weight: 1e-2)) 68 | graph.store(WeightedBetweenFactor(latentID1, latentID2, Vector10(), weight: 1e2)) 69 | }) 70 | } 71 | 72 | /// Returns `t` as a Swift tuple. 73 | fileprivate func unpack(_ t: Tuple2) -> (A, B) { 74 | return (t.head, t.tail.head) 75 | } 76 | /// Returns `t` as a Swift tuple. 77 | fileprivate func unpack(_ t: Tuple1) -> (A) { 78 | return (t.head) 79 | } 80 | -------------------------------------------------------------------------------- /Sources/BeeTracking/RandomProjectionTracker.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import SwiftFusion 16 | import TensorFlow 17 | import PenguinStructures 18 | 19 | /// Returns a tracking configuration for a tracker using an random projection. 20 | /// 21 | /// Parameter model: The random projection model to use. 22 | /// Parameter statistics: Normalization statistics for the frames. 23 | /// Parameter frames: The frames of the video where we want to run tracking. 24 | /// Parameter targetSize: The size of the target in the frames. 25 | public func makeRandomProjectionTracker( 26 | model: RandomProjection, 27 | statistics: FrameStatistics, 28 | frames: [Tensor], 29 | targetSize: (Int, Int), 30 | foregroundModel: MultivariateGaussian, 31 | backgroundModel: GaussianNB 32 | ) -> TrackingConfiguration> { 33 | var variableTemplate = VariableAssignments() 34 | var frameVariableIDs = [Tuple1>]() 35 | for _ in 0..>, values: Tuple1, graph: inout FactorGraph) -> () in 43 | let (poseID) = unpack(variables) 44 | let (pose) = unpack(values) 45 | graph.store(WeightedPriorFactorPose2(poseID, pose, weight: 1e-2, rotWeight: 2e2)) 46 | } 47 | 48 | let addTrackingFactor = { (variables: Tuple1>, frame: Tensor, graph: inout FactorGraph) -> () in 49 | let (poseID) = unpack(variables) 50 | graph.store( 51 | ProbablisticTrackingFactor(poseID, 52 | measurement: statistics.normalized(frame), 53 | encoder: model, 54 | patchSize: targetSize, 55 | appearanceModelSize: targetSize, 56 | foregroundModel: foregroundModel, 57 | backgroundModel: backgroundModel, 58 | maxPossibleNegativity: 1e2 59 | ) 60 | ) 61 | } 62 | 63 | return TrackingConfiguration( 64 | frames: frames, 65 | variableTemplate: variableTemplate, 66 | frameVariableIDs: frameVariableIDs, 67 | addPriorFactor: addPrior, 68 | addTrackingFactor: addTrackingFactor, 69 | addBetweenFactor: { (variables1, variables2, graph) -> () in 70 | let (poseID1) = unpack(variables1) 71 | let (poseID2) = unpack(variables2) 72 | graph.store(WeightedBetweenFactorPose2(poseID1, poseID2, Pose2(), weight: 1e-2, rotWeight: 2e2)) 73 | }, 74 | addFixedBetweenFactor: { (values, variables, graph) -> () in 75 | let (prior) = unpack(values) 76 | let (poseID) = unpack(variables) 77 | graph.store(WeightedPriorFactorPose2SD(poseID, prior, sdX: 8, sdY: 4.6, sdTheta: 0.3)) 78 | }) 79 | } 80 | 81 | /// Returns `t` as a Swift tuple. 82 | fileprivate func unpack(_ t: Tuple2) -> (A, B) { 83 | return (t.head, t.tail.head) 84 | } 85 | /// Returns `t` as a Swift tuple. 86 | fileprivate func unpack(_ t: Tuple1) -> (A) { 87 | return (t.head) 88 | } 89 | -------------------------------------------------------------------------------- /Sources/BeeTracking/RawPixelTracker.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import SwiftFusion 16 | import TensorFlow 17 | import PenguinStructures 18 | 19 | /// Returns a tracking configuration for a tracker using an PPCA. 20 | /// 21 | /// Parameter model: The PPCA model to use. 22 | /// Parameter statistics: Normalization statistics for the frames. 23 | /// Parameter frames: The frames of the video where we want to run tracking. 24 | /// Parameter targetSize: The size of the target in the frames. 25 | public func makePPCATracker( 26 | model: PPCA, 27 | statistics: FrameStatistics, 28 | frames: [Tensor], 29 | targetSize: (Int, Int) 30 | ) -> TrackingConfiguration> { 31 | var variableTemplate = VariableAssignments() 32 | var frameVariableIDs = [Tuple2, TypedID>]() 33 | for _ in 0.. () in 44 | let (poseID, latentID) = unpack(variables) 45 | let (pose, latent) = unpack(values) 46 | graph.store(WeightedPriorFactor(poseID, pose, weight: 1e-2)) 47 | graph.store(WeightedPriorFactor(latentID, latent, weight: 1e2)) 48 | }, 49 | addTrackingFactor: { (variables, frame, graph) -> () in 50 | let (poseID, latentID) = unpack(variables) 51 | graph.store( 52 | AppearanceTrackingFactor( 53 | poseID, latentID, 54 | measurement: statistics.normalized(frame), 55 | appearanceModel: { x in 56 | model.decode(x) 57 | }, 58 | appearanceModelJacobian: { x in 59 | model.W // .reshaped(to: [targetSize.0, targetSize.1, frames[0].shape[3], model.latent_size]) 60 | }, 61 | targetSize: targetSize 62 | ) 63 | ) 64 | }, 65 | addBetweenFactor: { (variables1, variables2, graph) -> () in 66 | let (poseID1, latentID1) = unpack(variables1) 67 | let (poseID2, latentID2) = unpack(variables2) 68 | graph.store(WeightedBetweenFactor(poseID1, poseID2, Pose2(), weight: 1e-2)) 69 | graph.store(WeightedBetweenFactor(latentID1, latentID2, Vector10(), weight: 1e2)) 70 | }) 71 | } 72 | 73 | /// Returns `t` as a Swift tuple. 74 | fileprivate func unpack(_ t: Tuple2) -> (A, B) { 75 | return (t.head, t.tail.head) 76 | } 77 | /// Returns `t` as a Swift tuple. 78 | fileprivate func unpack(_ t: Tuple1) -> (A) { 79 | return (t.head) 80 | } 81 | -------------------------------------------------------------------------------- /Sources/STBImage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(STBImage STATIC 2 | stb_image_write.c 3 | stb_image.c) 4 | set_target_properties(STBImage PROPERTIES 5 | POSITION_INDEPENDENT_CODE YES) 6 | target_include_directories(STBImage PUBLIC 7 | include) 8 | -------------------------------------------------------------------------------- /Sources/STBImage/include/module.modulemap: -------------------------------------------------------------------------------- 1 | module STBImage { 2 | header "stb_image.h" 3 | header "stb_image_write.h" 4 | export * 5 | } 6 | -------------------------------------------------------------------------------- /Sources/STBImage/stb_image.c: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The TensorFlow Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #define STB_IMAGE_IMPLEMENTATION 16 | #include "include/stb_image.h" 17 | -------------------------------------------------------------------------------- /Sources/STBImage/stb_image_write.c: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The TensorFlow Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #define STB_IMAGE_WRITE_IMPLEMENTATION 16 | #include "include/stb_image_write.h" 17 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Core/Dictionary+Differentiable.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | /// This file makes `Dictionary` differentiable. 3 | /// 4 | /// Note: This will eventually be moved into the Swift standard library. Once it is in the 5 | /// standard library, we can delete it from this repository. 6 | 7 | /// Implements the `Differentiable` requirements. 8 | extension Dictionary: Differentiable where Value: Differentiable { 9 | public typealias TangentVector = Dictionary 10 | public mutating func move(along direction: TangentVector) { 11 | for (componentKey, componentDirection) in direction { 12 | func fatalMissingComponent() -> Value { 13 | fatalError("missing component \(componentKey) in moved Dictionary") 14 | } 15 | self[componentKey, default: fatalMissingComponent()].move(along: componentDirection) 16 | } 17 | } 18 | 19 | public var zeroTangentVectorInitializer: () -> TangentVector { 20 | { mapValues { v in v.zeroTangentVector } } 21 | } 22 | } 23 | 24 | /// Implements the `AdditiveArithmetic` requirements. 25 | extension Dictionary: AdditiveArithmetic where Value: AdditiveArithmetic { 26 | public static func + (_ lhs: Self, _ rhs: Self) -> Self { 27 | lhs.merging(rhs, uniquingKeysWith: +) 28 | } 29 | public static func - (_ lhs: Self, _ rhs: Self) -> Self { 30 | lhs.merging(rhs.mapValues { .zero - $0 }, uniquingKeysWith: +) 31 | } 32 | public static var zero: Self { [:] } 33 | } 34 | 35 | /// Provides some differentiable methods for manipulating `Dictionary`. 36 | /// 37 | /// Note: Once the differentiable `Dictionary` is moved into the standard library, the standard 38 | /// `Dictionary` methods will be differentiable and you won't have to use special differentiable 39 | /// methods to manipulate `Dictionary`. 40 | extension Dictionary where Value: Differentiable { 41 | /// Returns the value with `key`. 42 | /// 43 | /// Precondition: `self` contains an entry with key `key`. 44 | @differentiable 45 | public func differentiableSubscript(_ key: Key) -> Value { 46 | self[key]! 47 | } 48 | 49 | @derivative(of: differentiableSubscript) 50 | @usableFromInline 51 | func vjpDifferentiableSubscript(_ key: Key) 52 | -> (value: Value, pullback: (Value.TangentVector) -> TangentVector) 53 | { 54 | (differentiableSubscript(key), { [key: $0] }) 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Core/MathUtil.swift: -------------------------------------------------------------------------------- 1 | //===----------------------------------------------------------------------===// 2 | // Pseudo inverse 3 | //===----------------------------------------------------------------------===// 4 | 5 | import _Differentiation 6 | import TensorFlow 7 | 8 | /// Calculate the pseudo-inverse of a matrix 9 | /// Input: [M, N] 10 | /// Output: [N, M] 11 | public func pinv(_ m: Tensor) -> Tensor { 12 | precondition(m.rank == 2, "Wrong input dimension for pinv()") 13 | 14 | let (J_s, J_u, J_v) = m.svd(computeUV: true, fullMatrices: true) 15 | 16 | let m = J_v!.shape[1] 17 | let n = J_u!.shape[0] 18 | if (m > n) { 19 | let J_ss = J_s.reciprocal.diagonal().concatenated(with: Tensor(repeating: 0, shape: [m-n, n]), alongAxis: 0) 20 | return matmul(matmul(J_v!, J_ss), J_u!.transposed()) 21 | } else if (m < n) { 22 | let J_ss = J_s.reciprocal.diagonal().concatenated(with: Tensor(repeating: 0, shape: [m, n-m]), alongAxis: 1) 23 | return matmul(matmul(J_v!, J_ss), J_u!.transposed()) 24 | } else { 25 | let J_ss = J_s.reciprocal.diagonal() 26 | return matmul(matmul(J_v!, J_ss), J_u!.transposed()) 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Core/Timer.swift: -------------------------------------------------------------------------------- 1 | import Foundation 2 | 3 | /// Utilities for timing and couting regions of code. 4 | /// 5 | /// NOT threadsafe, which we should probably fix soon. 6 | 7 | fileprivate var startedTimers: [String: UInt64] = [:] 8 | fileprivate var accumulatedTimers: [String: UInt64] = [:] 9 | 10 | /// Start the `name` timer. 11 | /// 12 | /// Precondition: The `name` timer is not running. 13 | public func startTimer(_ name: String) { 14 | guard startedTimers[name] == nil else { preconditionFailure("timer \(name) is already started") } 15 | startedTimers[name] = DispatchTime.now().uptimeNanoseconds 16 | } 17 | 18 | /// Stop the `name` timer. 19 | /// 20 | /// Precondition: The `name` timer is running. 21 | public func stopTimer(_ name: String) { 22 | guard let start = startedTimers[name] else { preconditionFailure("timer \(name) is not running") } 23 | startedTimers[name] = nil 24 | accumulatedTimers[name, default: 0] += DispatchTime.now().uptimeNanoseconds - start 25 | } 26 | 27 | /// Print the total times accumulated for each timer. 28 | public func printTimers() { 29 | guard startedTimers.count == 0 else { preconditionFailure("timers are still running: \(startedTimers)") } 30 | for (name, duration) in accumulatedTimers { 31 | print("\(name): \(Double(duration) / 1e9) seconds") 32 | } 33 | } 34 | 35 | fileprivate var counters: [String: Int] = [:] 36 | 37 | /// Increment the `name` counter. 38 | public func incrementCounter(_ name: String) { 39 | counters[name, default: 0] += 1 40 | } 41 | 42 | /// Print the total counts for each counter. 43 | public func printCounters() { 44 | for (name, count) in counters { 45 | print("\(name): \(count)") 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Core/VectorN.swift.gyb: -------------------------------------------------------------------------------- 1 | import TensorFlow 2 | 3 | % dims = range(1, 13) 4 | % for dim in dims: 5 | % if dim <= 3: 6 | % coordinates = ['x', 'y', 'z'][0:dim] 7 | % else: 8 | % coordinates = ['s%d' % i for i in range(dim)] 9 | % end 10 | 11 | /// An element of R^${dim}, with Euclidean inner product. 12 | public struct Vector${dim}: Codable, KeyPathIterable { 13 | % for coordinate in coordinates: 14 | @differentiable public var ${coordinate}: Double 15 | % end 16 | 17 | @differentiable 18 | public init(${', '.join(['_ %s: Double' % c for c in coordinates])}) { 19 | % for (index, coordinate) in enumerate(coordinates): 20 | self.${coordinate} = ${coordinate} 21 | % end 22 | } 23 | } 24 | 25 | /// Conformance to Vector 26 | extension Vector${dim}: AdditiveArithmetic, Vector { 27 | @differentiable 28 | public static func += (_ lhs: inout Self, _ rhs: Self) { 29 | % for coordinate in coordinates: 30 | lhs.${coordinate} += rhs.${coordinate} 31 | % end 32 | } 33 | 34 | @differentiable 35 | public static func -= (_ lhs: inout Self, _ rhs: Self) { 36 | % for coordinate in coordinates: 37 | lhs.${coordinate} -= rhs.${coordinate} 38 | % end 39 | } 40 | 41 | @differentiable 42 | public static func *= (_ lhs: inout Self, _ rhs: Double) { 43 | % for coordinate in coordinates: 44 | lhs.${coordinate} *= rhs 45 | % end 46 | } 47 | 48 | @differentiable 49 | public func dot(_ other: Self) -> Double { 50 | var result = Double(0) 51 | % for coordinate in coordinates: 52 | result += self.${coordinate} * other.${coordinate} 53 | % end 54 | return result 55 | } 56 | 57 | public var dimension: Int { return ${dim} } 58 | 59 | public init(_ scalars: Source) where Source.Element == Double { 60 | var index = scalars.startIndex 61 | % for coordinate in coordinates: 62 | self.${coordinate} = scalars[index] 63 | index = scalars.index(after: index) 64 | % end 65 | } 66 | 67 | /// A type that can represent all of this vector's scalar values in a standard basis. 68 | public struct Scalars: RandomAccessCollection, MutableCollection { 69 | // Deduction of Indices fails without an explicit declaration. 70 | /// A type that can represent all the indices of elements in this collection. 71 | public typealias Indices = Range 72 | 73 | /// The vector whose scalars are reflected by `self`. 74 | internal var base: Vector${dim} 75 | 76 | /// The position of the first element, or `endIndex` if `self.isEmpty`. 77 | public var startIndex: Int { 0 } 78 | 79 | /// The position one step beyond the last contained element. 80 | public var endIndex: Int { base.dimension } 81 | 82 | /// Accesses the scalar at `i`. 83 | public subscript(i: Int) -> Double { 84 | get { 85 | precondition(i >= 0 && i < endIndex) 86 | return withUnsafePointer(to: self) { 87 | UnsafeRawPointer($0).assumingMemoryBound(to: Double.self)[i] 88 | } 89 | } 90 | _modify { 91 | precondition(i >= 0 && i < endIndex) 92 | let p = withUnsafeMutablePointer(to: &self) { $0 } 93 | let q = UnsafeMutableRawPointer(p).assumingMemoryBound(to: Double.self) 94 | defer { _fixLifetime(self) } 95 | yield &q[i] 96 | } 97 | } 98 | } 99 | 100 | /// This vector's scalar values in a standard basis. 101 | public var scalars: Scalars { 102 | get { .init(base: self) } 103 | set { self = newValue.base } 104 | } 105 | } 106 | 107 | extension Vector${dim}: FixedSizeVector { 108 | public static var dimension: Int { return ${dim} } 109 | } 110 | 111 | % end 112 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Datasets/DatasetCache.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import Foundation 17 | #if !os(macOS) 18 | import FoundationNetworking 19 | #endif 20 | 21 | /// Dictionary from dataset id to dataset url. 22 | fileprivate let datasets = [ 23 | "input_INTEL_g2o.txt": 24 | "https://github.com/pkchen1129/Mobile-Robotics/raw/8551df10ba8af36801403daeba710c1f9c9e54cd/ps4/code/dataset/input_INTEL_g2o.txt", 25 | "sphere_bignoise_vertex3.g2o": 26 | "https://github.com/HeYijia/GraphSLAM_tutorials_code/raw/master/g2o_test/data/sphere_bignoise_vertex3.g2o", 27 | "pose3example.txt": 28 | "https://github.com/borglab/gtsam/raw/master/examples/Data/pose3example.txt", 29 | "parking-garage.g2o": 30 | "https://github.com/david-m-rosen/SE-Sync/raw/master/data/parking-garage.g2o", 31 | "sphere2500.g2o": 32 | "https://github.com/david-m-rosen/SE-Sync/raw/master/data/sphere2500.g2o" 33 | ] 34 | 35 | /// Returns a dataset cached on the local system. 36 | /// 37 | /// If the dataset with `id` is available on the local system, returns it immediately. 38 | /// Otherwise, downloads the dataset to the cache and then returns it. 39 | /// 40 | /// - Parameter `cacheDirectory`: The directory where the cached datasets are stored. 41 | public func cachedDataset( 42 | _ id: String, 43 | cacheDirectory: URL = URL(fileURLWithPath: NSHomeDirectory()) 44 | .appendingPathComponent(".SwiftFusionDatasetCache", isDirectory: true) 45 | ) throws -> URL { 46 | try createDirectoryIfMissing(at: cacheDirectory.path) 47 | let cacheEntry = cacheDirectory.appendingPathComponent(id) 48 | if FileManager.default.fileExists(atPath: cacheEntry.path) { 49 | return cacheEntry 50 | } 51 | guard let url = datasets[id] else { 52 | throw DatasetCacheError(message: "No such dataset: \(id)") 53 | } 54 | print("Downloading \(url) to \(cacheEntry)") 55 | guard let source = URL(string: url) else { 56 | throw DatasetCacheError(message: "Could not parse URL: \(url)") 57 | } 58 | let data = try Data.init(contentsOf: source) 59 | try data.write(to: cacheEntry) 60 | print("Downloaded \(cacheEntry)!") 61 | return cacheEntry 62 | } 63 | 64 | /// An error from getting a cached dataset. 65 | public struct DatasetCacheError: Swift.Error { 66 | public let message: String 67 | } 68 | 69 | /// Creates a directory at a path, if missing. If the directory exists, this does nothing. 70 | /// 71 | /// - Parameters: 72 | /// - path: The path of the desired directory. 73 | fileprivate func createDirectoryIfMissing(at path: String) throws { 74 | guard !FileManager.default.fileExists(atPath: path) else { return } 75 | try FileManager.default.createDirectory( 76 | atPath: path, 77 | withIntermediateDirectories: true, 78 | attributes: nil) 79 | } 80 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Geometry/Cal3_S2.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | 3 | /// The five-parameter camera calibration. 4 | public struct Cal3_S2: Manifold, Equatable { 5 | public typealias Coordinate = Cal3_S2Coordinate 6 | public typealias TangentVector = Vector5 7 | 8 | public var coordinateStorage: Cal3_S2Coordinate 9 | 10 | public init() { 11 | self.init(coordinateStorage: Cal3_S2Coordinate()) 12 | } 13 | 14 | public init(coordinateStorage: Cal3_S2Coordinate) { 15 | self.coordinateStorage = coordinateStorage 16 | } 17 | 18 | /// Initializes from individual values. 19 | public init(fx: Double, fy: Double, s: Double, u0: Double, v0: Double) { 20 | self.init(coordinateStorage: Cal3_S2Coordinate(fx: fx, fy: fy, s: s, u0: u0, v0: v0)) 21 | } 22 | 23 | /// Moves the parameter values by the specified direction. 24 | public mutating func move(along direction: Vector5) { 25 | coordinateStorage = coordinateStorage.retract(direction) 26 | } 27 | } 28 | 29 | /// CameraCalibration conformance. 30 | extension Cal3_S2: CameraCalibration { 31 | @differentiable 32 | public func uncalibrate(_ np: Vector2) -> Vector2 { 33 | coordinate.uncalibrate(np) 34 | } 35 | 36 | @differentiable 37 | public func calibrate(_ ip: Vector2) -> Vector2 { 38 | coordinate.calibrate(ip) 39 | } 40 | } 41 | 42 | extension Cal3_S2 { 43 | public func zeroTangentVector() -> Cal3_S2.TangentVector { 44 | return Vector5(0.0, 0.0, 0.0, 0.0, 0.0) 45 | } 46 | } 47 | 48 | /// Manifold coordinate for Cal3_S2. 49 | public struct Cal3_S2Coordinate: Equatable { 50 | /// Focal length in X direction. 51 | public var fx: Double 52 | 53 | /// Focal length in Y direction. 54 | public var fy: Double 55 | 56 | /// Skew factor. 57 | public var s: Double 58 | 59 | /// Image center in X direction. 60 | public var u0: Double 61 | 62 | /// Image center in Y direction. 63 | public var v0: Double 64 | 65 | /// Initializes from individual values. 66 | public init(fx: Double, fy: Double, s: Double, u0: Double, v0: Double) { 67 | self.fx = fx 68 | self.fy = fy 69 | self.s = s 70 | self.u0 = u0 71 | self.v0 = v0 72 | } 73 | 74 | /// Initializes from a vector. 75 | public init(_ params: Vector5) { 76 | self.fx = params.s0 77 | self.fy = params.s1 78 | self.s = params.s2 79 | self.u0 = params.s3 80 | self.v0 = params.s4 81 | } 82 | 83 | /// Initializes with default values, corresponding to the identity element. 84 | public init() { 85 | self.init(fx: 1.0, fy: 1.0, s: 0.0, u0: 0.0, v0: 0.0) 86 | } 87 | 88 | /// Returns the parameters as a vector. 89 | public func asVector() -> Vector5 { 90 | Vector5(fx, fy, s, u0, v0) 91 | } 92 | } 93 | 94 | /// ManifoldCoordinate conformance. 95 | extension Cal3_S2Coordinate: ManifoldCoordinate { 96 | public typealias LocalCoordinate = Vector5 97 | 98 | @differentiable(wrt: local) 99 | public func retract(_ local: Vector5) -> Cal3_S2Coordinate { 100 | Cal3_S2Coordinate(asVector() + local) 101 | } 102 | 103 | @differentiable(wrt: global) 104 | public func localCoordinate(_ global: Cal3_S2Coordinate) -> Vector5 { 105 | global.asVector() - asVector() 106 | } 107 | } 108 | 109 | /// Operations on a point. 110 | extension Cal3_S2Coordinate { 111 | @differentiable 112 | public func uncalibrate(_ np: Vector2) -> Vector2 { 113 | Vector2(fx * np.x + s * np.y + u0, fy * np.y + v0) 114 | } 115 | 116 | @differentiable 117 | public func calibrate(_ ip: Vector2) -> Vector2 { 118 | let (du, dv) = (ip.x - u0, ip.y - v0) 119 | let (fxInv, fyInv) = (1.0 / fx, 1.0 / fy) 120 | return Vector2(fxInv * du - s * fxInv * fyInv * dv, fyInv * dv) 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Geometry/CameraCalibration.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | 3 | /// A protocol for camera calibration parameters. 4 | public protocol CameraCalibration: Differentiable { 5 | /// Initializes to default (usually identity). 6 | init() 7 | 8 | /// Converts from image coordinate to normalized coordinate. 9 | @differentiable 10 | func calibrate(_ ip: Vector2) -> Vector2 11 | 12 | /// Converts from normalized coordinate to image coordinate. 13 | @differentiable 14 | func uncalibrate(_ np: Vector2) -> Vector2 15 | 16 | func zeroTangentVector() -> TangentVector 17 | } 18 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Geometry/PinholeCamera.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | 3 | /// Pinhole camera model. 4 | public struct PinholeCamera: Differentiable { 5 | /// Camera pose in the world/reference frame. 6 | public var wTc: Pose3 7 | 8 | /// Camera calibration. 9 | public var calibration: Calibration 10 | 11 | /// Initializes from camera pose and calibration. 12 | @differentiable 13 | public init(_ calibration: Calibration, _ wTc: Pose3) { 14 | self.calibration = calibration 15 | self.wTc = wTc 16 | } 17 | 18 | /// Initializes with identity pose. 19 | @differentiable 20 | public init(_ calibration: Calibration) { 21 | self.init(calibration, Pose3()) 22 | } 23 | 24 | /// Initializes to default. 25 | public init() { 26 | self.init(Calibration(), Pose3()) 27 | } 28 | } 29 | 30 | /// Project and backproject. 31 | extension PinholeCamera { 32 | /// Projects a 3D point in the world frame to 2D point in the image. 33 | @differentiable 34 | public func project(_ wp: Vector3) -> Vector2 { 35 | let np: Vector2 = projectToNormalized(wp) 36 | let ip = calibration.uncalibrate(np) 37 | return ip 38 | } 39 | 40 | /// Backprojects a 2D image point into 3D point in the world frame at given depth. 41 | @differentiable 42 | public func backproject(_ ip: Vector2, _ depth: Double) -> Vector3 { 43 | let np = calibration.calibrate(ip) 44 | let cp = Vector3(np.x * depth, np.y * depth, depth) 45 | let wp = wTc * cp 46 | return wp 47 | } 48 | 49 | /// Projects a 3D point in the world frame to 2D normalized coordinate. 50 | @differentiable 51 | func projectToNormalized(_ wp: Vector3) -> Vector2 { 52 | projectToNormalized(wp).ip 53 | } 54 | 55 | /// Computes the derivative of the projection function wrt to self and the point wp. 56 | @usableFromInline 57 | @derivative(of: projectToNormalized) 58 | func vjpProjectToNormalized(_ wp: Vector3) -> 59 | (value: Vector2, pullback: (Vector2) -> (TangentVector, Vector3)) 60 | { 61 | let (ip, cRw, zInv) = projectToNormalized(wp) 62 | let (u, v) = (ip.x, ip.y) 63 | let R = cRw.coordinate.R 64 | return ( 65 | value: ip, 66 | pullback: { p in 67 | let dpose = Vector6( 68 | p.x * (u * v) + p.y * (1 + v * v), 69 | p.x * -(1 + u * u) + p.y * -(u * v), 70 | p.x * v + p.y * -u, 71 | p.x * -zInv, 72 | p.y * -zInv, 73 | p.x * (zInv * u) + p.y * (zInv * v)) 74 | 75 | let dpoint = zInv * Vector3( 76 | p.x * (R[0, 0] - u * R[2, 0]) + p.y * (R[1, 0] - v * R[2, 0]), 77 | p.x * (R[0, 1] - u * R[2, 1]) + p.y * (R[1, 1] - v * R[2, 1]), 78 | p.x * (R[0, 2] - u * R[2, 2]) + p.y * (R[1, 2] - v * R[2, 2])) 79 | 80 | return ( 81 | TangentVector(wTc: dpose, calibration: calibration.zeroTangentVector()), 82 | dpoint) 83 | } 84 | ) 85 | } 86 | 87 | /// Projects a 3D point in the world frame to 2D normalized coordinate and returns intermediate values. 88 | func projectToNormalized(_ wp: Vector3) -> 89 | (ip: Vector2, cRw: Rot3, zInv: Double) 90 | { 91 | // Transform the point to camera coordinate 92 | let cTw = wTc.inverse() 93 | let cp = cTw * wp 94 | 95 | // TODO: check for cheirality (whether the point is behind the camera) 96 | 97 | // Project to normalized coordinate 98 | let zInv = 1.0 / cp.z 99 | 100 | return ( 101 | ip: Vector2(cp.x * zInv, cp.y * zInv), 102 | cRw: cTw.rot, 103 | zInv: zInv) 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Image/OrientedBoundingBox.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | import PythonKit 17 | 18 | /// A rectangular region of an image, not necessarily axis-aligned. 19 | public struct OrientedBoundingBox: Codable, Differentiable { 20 | /// The pose of the region's center within the image. 21 | /// 22 | /// The translation component is in `(u, v)` coordinates as defined in `docs/ImageOperations.md`. 23 | @differentiable 24 | public var center: Pose2 25 | 26 | /// The number of pixels along the height axis. 27 | /// 28 | /// This is the `rows` image dimension defined in `docs/ImageOperations.md`. 29 | @noDerivative public var rows: Int 30 | 31 | /// The number of pixels along the width axis. 32 | /// 33 | /// This is the `cols` image dimension defines in `docs/ImageOperations.md`. 34 | @noDerivative public var cols: Int 35 | 36 | /// Creates a instance with the given `center`, `rows`, and `cols`. 37 | @differentiable 38 | public init(center: Pose2, rows: Int, cols: Int) { 39 | self.center = center 40 | self.rows = rows 41 | self.cols = cols 42 | } 43 | 44 | /// The four corners of the region, in `(u, v)` coordinates as defined in 45 | /// `docs/ImageOperations.md`. 46 | @differentiable 47 | public var corners: [Vector2] { 48 | /// Returns a corner of `self`. 49 | /// 50 | /// - Parameter `uFlip`: `-1` or `1`, determines which side of `self`'s `u`-axis the corner is on. 51 | /// - Parameter `vFlip`: `-1` or `1`, determines which side of `self`'s `v`-axis the corner is on. 52 | func corner(_ uFlip: Double, _ vFlip: Double) -> Vector2 { 53 | return center.t + center.rot * (0.5 * Vector2(uFlip * Double(cols), vFlip * Double(rows))) 54 | } 55 | return [corner(1, 1), corner(-1, 1), corner(-1, -1), corner(1, -1)] 56 | } 57 | 58 | /// Returns the ratio `|self ∩ other| / |self ∪ other|`. 59 | /// 60 | /// Precondition: The "Shapely" python library is installed on the system. 61 | public func overlap(_ other: Self) -> Double { 62 | let selfPoly = self.shapelyPolygon 63 | let otherPoly = other.shapelyPolygon 64 | return Double(selfPoly.intersection(otherPoly).area / selfPoly.union(otherPoly).area)! 65 | } 66 | 67 | /// `self`, as a Shapely polygon. 68 | /// 69 | /// Precondition: The "Shapely" python library is installed on the system. 70 | private var shapelyPolygon: PythonObject { 71 | let geometry = Python.import("shapely.geometry") 72 | return geometry.Polygon(corners.map { [$0.x, $0.y] }) 73 | } 74 | } 75 | 76 | extension OrientedBoundingBox: Equatable {} 77 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/ArrayBuffer+Differentiable.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | import _Differentiation 15 | import PenguinStructures 16 | 17 | extension ArrayBuffer: Differentiable where Element: Differentiable { 18 | public typealias TangentVector = ArrayBuffer 19 | 20 | /// Returns the zero `TangentVector`s of the contained elements. 21 | // public var tangentVectorZeros: ArrayBuffer { .init() } 22 | 23 | /// Moves each element of `self` along the corresponding element of `directions`. 24 | /// 25 | /// - Requires: `directions.count == self.count`. 26 | public mutating func move(along directions: TangentVector) { 27 | if directions.isEmpty { return } 28 | update(elementwiseWith: directions, { $0.move(along: $1) }) 29 | } 30 | 31 | /// A function returning the zero `TangentVector`s of the contained elements. 32 | @noDerivative 33 | public var zeroTangentVectorInitializer: () -> TangentVector { 34 | { .zero } 35 | } 36 | } 37 | 38 | extension ArrayBuffer where Element: Differentiable { 39 | // DWA TODO: replace this with the use of zeroTangentVectorInitializer 40 | /// Returns the zero `TangentVector`s of the contained elements. 41 | var tangentVectorZeros: ArrayBuffer { 42 | withUnsafeBufferPointer { vs in 43 | .init(vs.lazy.map { $0.zeroTangentVector }) 44 | } 45 | } 46 | } 47 | 48 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/BearingRangeFactor.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import TensorFlow 17 | import PenguinStructures 18 | 19 | /// A `NonlinearFactor` that calculates the bearing and range error of one pose and one landmark 20 | /// 21 | public struct BearingRangeFactor2 : LinearizableFactor2 { 22 | public typealias Base = Pose2 23 | public typealias Target = Vector2 24 | public typealias Bearing = Rot2 25 | 26 | public let edges: Variables.Indices 27 | public let bearingMeas: Bearing 28 | public let rangeMeas: Double 29 | 30 | 31 | public init(_ baseId: TypedID, _ targetId: TypedID, _ bearingMeas: Bearing, _ rangeMeas: Double) { 32 | self.edges = Tuple2(baseId, targetId) 33 | self.bearingMeas = bearingMeas 34 | self.rangeMeas = rangeMeas 35 | } 36 | 37 | public typealias Variables = Tuple2 38 | @differentiable 39 | public func errorVector(_ base: Base, _ target: Target) -> Vector2 { 40 | let dx = (target - base.t) 41 | let actual_bearing = between(Rot2(c: dx.x / dx.norm, s: dx.y / dx.norm), base.rot) 42 | let actual_range = dx.norm 43 | let error_range = (actual_range - rangeMeas) 44 | let error_bearing = between(actual_bearing, bearingMeas) 45 | 46 | return Vector2(error_bearing.theta, error_range) 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/BetweenFactor.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import PenguinStructures 17 | 18 | /// A factor that specifies a difference between two instances of a Group. 19 | public struct BetweenFactor: LinearizableFactor2 { 20 | public let edges: Variables.Indices 21 | public let difference: Group 22 | 23 | public init(_ startId: TypedID, _ endId: TypedID, _ difference: Group) { 24 | self.edges = Tuple2(startId, endId) 25 | self.difference = difference 26 | } 27 | 28 | @differentiable 29 | public func errorVector(_ start: Group, _ end: Group) -> Group.TangentVector { 30 | let actualMotion = between(start, end) 31 | return difference.localCoordinate(actualMotion) 32 | } 33 | } 34 | 35 | /// A factor that specifies a difference between two instances of a Group, version with weight. 36 | public struct WeightedBetweenFactor: LinearizableFactor2 { 37 | public let edges: Variables.Indices 38 | public let difference: Group 39 | public let weight: Double 40 | 41 | public init(_ startId: TypedID, _ endId: TypedID, _ difference: Group, weight: Double) { 42 | self.edges = Tuple2(startId, endId) 43 | self.difference = difference 44 | self.weight = weight 45 | } 46 | 47 | @differentiable 48 | public func errorVector(_ start: Group, _ end: Group) -> Group.TangentVector { 49 | let actualMotion = between(start, end) 50 | return weight * difference.localCoordinate(actualMotion) 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/BetweenFactorAlternative.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import PenguinStructures 17 | 18 | /// A BetweenFactor alternative that uses the Chordal (Frobenious) norm on rotation for Pose3 19 | public struct BetweenFactorAlternative: LinearizableFactor2 { 20 | public let edges: Variables.Indices 21 | public let difference: Pose3 22 | 23 | public init(_ startId: TypedID, _ endId: TypedID, _ difference: Pose3) { 24 | self.edges = Tuple2(startId, endId) 25 | self.difference = difference 26 | } 27 | 28 | @differentiable 29 | public func errorVector(_ start: Pose3, _ end: Pose3) -> Vector12 { 30 | let actualMotion = between(start, end) 31 | let R = actualMotion.coordinate.rot.coordinate.R + (-1) * difference.rot.coordinate.R 32 | let t = actualMotion.t - difference.t 33 | 34 | return Vector12(concatenating: R, t) 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/DiscreteTransitionFactor.swift: -------------------------------------------------------------------------------- 1 | // 2 | // DiscreteTransitionFactor.swift 3 | // 4 | // 5 | // Frank Dellaert and Marc Rasi 6 | // July 2020 7 | 8 | import _Differentiation 9 | import Foundation 10 | import PenguinStructures 11 | 12 | /// A factor on two discrete labels evaluation the transition probability 13 | struct DiscreteTransitionFactor : Factor { 14 | typealias Variables = Tuple2 15 | 16 | /// The IDs of the variables adjacent to this factor. 17 | public let edges: Variables.Indices 18 | 19 | /// The number of states. 20 | let stateCount: Int 21 | 22 | /// Entry `i * stateCount + j` is the probability of transitioning from state `j` to state `i`. 23 | let transitionMatrix: [Double] 24 | 25 | init( 26 | _ inputId1: TypedID, 27 | _ inputId2: TypedID, 28 | _ stateCount: Int, 29 | _ transitionMatrix: [Double] 30 | ) { 31 | precondition(transitionMatrix.count == stateCount * stateCount) 32 | self.edges = Tuple2(inputId1, inputId2) 33 | self.stateCount = stateCount 34 | self.transitionMatrix = transitionMatrix 35 | } 36 | 37 | func error(at q: Variables) -> Double { 38 | let (label1, label2) = (q.head, q.tail.head) 39 | return -log(transitionMatrix[label2 * stateCount + label1]) 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/IdentityLinearizationFactor.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import PenguinStructures 17 | 18 | /// A linear approximation of a `GaussianFactor`. 19 | /// 20 | /// Since `GaussianFactor`s are linear, they are their own linear approximations. 21 | public struct IdentityLinearizationFactor: LinearApproximationFactor { 22 | /// The appoximated factor. 23 | let base: Base 24 | 25 | /// A tuple of the variable types of variables adjacent to this factor. 26 | public typealias Variables = Base.Variables 27 | 28 | /// The type of the error vector. 29 | public typealias ErrorVector = Base.ErrorVector 30 | 31 | /// The IDs of the variables adjacent to this factor. 32 | public var edges: Variables.Indices { 33 | base.edges 34 | } 35 | 36 | /// Creates a factor that linearly approximates `f` at `x`. 37 | /// 38 | /// - Requires: `F == Base`. 39 | public init(linearizing f: F, at x: F.Variables) 40 | where F.Variables.TangentVector == Variables, F.ErrorVector == ErrorVector { 41 | self.base = f as! Base 42 | } 43 | 44 | /// Returns the error at `x`. 45 | /// 46 | /// This is typically interpreted as negative log-likelihood. 47 | public func error(at x: Variables) -> Double { 48 | base.error(at: x) 49 | } 50 | 51 | /// Returns the error vector given the values of the adjacent variables. 52 | @differentiable 53 | public func errorVector(at x: Variables) -> ErrorVector { 54 | base.errorVector(at: x) 55 | } 56 | 57 | /// The linear component of `errorVector`. 58 | public func errorVector_linearComponent(_ x: Variables) -> ErrorVector { 59 | base.errorVector_linearComponent(x) 60 | } 61 | 62 | /// The adjoint (aka "transpose" or "dual") of the linear component of `errorVector`. 63 | public func errorVector_linearComponent_adjoint(_ y: ErrorVector) -> Variables { 64 | base.errorVector_linearComponent_adjoint(y) 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/IdentityProjection.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | import TensorFlow 15 | import PenguinStructures 16 | 17 | /// A class that does a identity feature mapping (directly outputs the pixels flattened) 18 | /// 19 | /// - Input shape for initialization: [N, H, W, C] 20 | /// - W matrix: [feature, H*W*C] 21 | /// - Output: [feature] 22 | public struct IdentityProjection { 23 | public typealias Patch = Tensor 24 | 25 | /// Sample mean 26 | public let mean: Tensor 27 | 28 | /// Initialize the random projector with a normalized projection matrix 29 | public init(fromShape shape: TensorShape, sampleMean: Tensor? = nil) { 30 | let (H, W, C) = (shape[0], shape[1], shape[2]) 31 | 32 | if let mu = sampleMean { 33 | precondition(mu.shape == [H, W, C], "Wrong mean tensor") 34 | mean = mu 35 | } else { 36 | mean = Tensor(zeros: [H, W, C]) 37 | } 38 | } 39 | 40 | /// Initialize given an image batch 41 | public typealias HyperParameters = Int 42 | public init(from imageBatch: Tensor, given d: HyperParameters? = nil) { 43 | self.init(fromShape: imageBatch.shape.suffix(3), sampleMean: imageBatch.mean(squeezingAxes: 0)) 44 | } 45 | 46 | /// Generate an feature from image or image batch 47 | /// Input: [H, W, C] or [N,H,W,C] 48 | /// Output: [d] or [N, d] 49 | @differentiable 50 | public func encode(_ image: Patch) -> Tensor { 51 | precondition(image.rank == 3 || (image.rank == 4), "wrong feature dimension \(image.shape)") 52 | if image.rank == 4 { 53 | let (N, H, W, C) = (image.shape[0], image.shape[1], image.shape[2], image.shape[3]) 54 | let v_T = (image - mean).reshaped(to: [N, H * W * C]) 55 | return v_T 56 | } else { 57 | let (H, W, C) = (image.shape[0], image.shape[1], image.shape[2]) 58 | return (image - mean).reshaped(to: [H * W * C]) 59 | } 60 | } 61 | } 62 | 63 | extension IdentityProjection: AppearanceModelEncoder {} 64 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/LatentAppearanceTrackingFactor.swift: -------------------------------------------------------------------------------- 1 | import PenguinParallel 2 | import PenguinStructures 3 | import TensorFlow 4 | 5 | public protocol AppearanceModelEncoder { 6 | associatedtype HyperParameters 7 | init(from imageBatch: Tensor, given: HyperParameters?) 8 | 9 | @differentiable 10 | func encode(_ imageBatch: Tensor) -> Tensor 11 | } 12 | 13 | public extension AppearanceModelEncoder { 14 | /// Extension allows to have a default nil parameter 15 | init(from imageBatch: Tensor) { 16 | self.init(from: imageBatch, given: nil) 17 | } 18 | 19 | @differentiable 20 | func encode(sample: Tensor) -> Tensor { 21 | encode(sample.expandingShape(at: 0)).squeezingShape(at: 0) 22 | } 23 | } 24 | 25 | extension AppearanceModelEncoder { 26 | @differentiable 27 | fileprivate func encode(_ image: Tensor) -> V { 28 | V(flatTensor: encode(image.expandingShape(at: 0)).squeezingShape(at: 0)) 29 | } 30 | } 31 | 32 | /// A factor over a target's pose and appearance in an image. 33 | public struct LatentAppearanceTrackingFactor: LinearizableFactor2 { 34 | /// The first adjacent variable, the pose of the target in the image. 35 | /// 36 | /// This explicitly specifies `LinearizableFactor2`'s `associatedtype V0`. 37 | public typealias V0 = Pose2 38 | 39 | /// The second adjacent variable, the latent code for the appearance of the target. 40 | /// 41 | /// This explicitly specifies `LinearizableFactor2`'s `associatedtype V1`. 42 | public typealias V1 = LatentCode 43 | 44 | /// The IDs of the variables adjacent to this factor. 45 | public let edges: Variables.Indices 46 | 47 | /// The image containing the target. 48 | public let measurement: Tensor 49 | 50 | public let encoder: Encoder 51 | 52 | public var patchSize: (Int, Int) 53 | 54 | public var appearanceModelSize: (Int, Int) 55 | 56 | /// Creates an instance. 57 | /// 58 | /// - Parameters: 59 | /// - poseId: the id of the adjacent pose variable. 60 | /// - latentId: the id of the adjacent latent code variable. 61 | /// - measurement: the image containing the target. 62 | /// - appearanceModel: the generative model that produces an appearance from a latent code. 63 | public init( 64 | _ poseId: TypedID, 65 | _ latentId: TypedID, 66 | measurement: Tensor, 67 | encoder: Encoder, 68 | patchSize: (Int, Int), 69 | appearanceModelSize: (Int, Int) 70 | ) { 71 | self.edges = Tuple2(poseId, latentId) 72 | self.measurement = measurement 73 | self.encoder = encoder 74 | self.patchSize = patchSize 75 | self.appearanceModelSize = appearanceModelSize 76 | } 77 | 78 | @differentiable 79 | public func errorVector(_ pose: Pose2, _ latent: LatentCode) -> LatentCode { 80 | let region = OrientedBoundingBox(center: pose, rows: patchSize.0, cols: patchSize.1) 81 | let patch = measurement.patch(at: region, outputSize: appearanceModelSize) 82 | return encoder.encode(patch) - latent 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/PCAEncoder.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | import TensorFlow 15 | import PenguinStructures 16 | 17 | /// A class that does PCA decomposition 18 | /// Input shape for training: [N, H, W, C] 19 | /// W matrix: [H, W, C, latent] 20 | /// Output: [H, W, C] 21 | public struct PCAEncoder { 22 | public typealias Patch = Tensor 23 | 24 | /// Basis 25 | public let U: Tensor 26 | 27 | /// Data mean 28 | public let mu: Tensor 29 | 30 | /// Size of latent 31 | public var d: Int { 32 | get { 33 | U.shape[1] 34 | } 35 | } 36 | 37 | /// Input dimension for one sample 38 | public var n: Int { 39 | get { 40 | U.shape[0] 41 | } 42 | } 43 | 44 | /// Train a PCAEncoder model 45 | /// images should be a Tensor of shape [N, H, W, C] 46 | /// default feature size is 10 47 | /// Input: [N, H, W, C] 48 | public typealias HyperParameters = Int 49 | public init(from imageBatch: Tensor, given p: HyperParameters? = nil) { 50 | precondition(imageBatch.rank == 4, "Wrong image shape \(imageBatch.shape)") 51 | let (N_, H_, W_, C_) = (imageBatch.shape[0], imageBatch.shape[1], imageBatch.shape[2], imageBatch.shape[3]) 52 | let n = H_ * W_ * C_ 53 | let d = p ?? 10 54 | 55 | let images_flattened = imageBatch.reshaped(to: [N_, n]) 56 | let mu = images_flattened.mean(squeezingAxes: 0) 57 | 58 | let images_flattened_without_mean = (images_flattened - mu).transposed() 59 | let (_, U, _) = images_flattened_without_mean.svd(computeUV: true, fullMatrices: false) 60 | 61 | self.init(withBasis: U![TensorRange.ellipsis, 0.., andMean mu: Tensor) { 66 | self.U = U 67 | self.mu = mu 68 | } 69 | 70 | /// Generate an image according to a latent 71 | /// Input: [H, W, C] 72 | /// Output: [d] 73 | @differentiable 74 | public func encode(_ image: Patch) -> Tensor { 75 | precondition(image.rank == 3 || (image.rank == 4), "wrong latent dimension \(image.shape)") 76 | let (N_) = (image.shape[0]) 77 | if image.rank == 4 { 78 | if N_ == 1 { 79 | return matmul(U, transposed: true, image.reshaped(to: [n, 1]) - mu.reshaped(to: [n, 1])).reshaped(to: [1, d]) 80 | } else { 81 | let v = image.reshaped(to: [N_, n]) - mu.reshaped(to: [1, n]) 82 | return matmul(v, U) 83 | } 84 | } else { 85 | let v = image.reshaped(to: [n, 1]) - mu.reshaped(to: [n, 1]) 86 | return matmul(U, transposed: true, v).reshaped(to: [d]) 87 | } 88 | } 89 | } 90 | 91 | extension PCAEncoder: AppearanceModelEncoder {} 92 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/PriorFactor.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import PenguinStructures 17 | 18 | /// A factor that specifies a prior on a Group. 19 | public struct PriorFactor: LinearizableFactor1 { 20 | public let edges: Variables.Indices 21 | public let prior: Group 22 | 23 | public init(_ id: TypedID, _ prior: Group) { 24 | self.edges = Tuple1(id) 25 | self.prior = prior 26 | } 27 | 28 | @differentiable 29 | public func errorVector(_ x: Group) -> Group.TangentVector { 30 | return prior.localCoordinate(x) 31 | } 32 | } 33 | 34 | /// A factor that specifies a prior on a Group. 35 | public struct WeightedPriorFactor: LinearizableFactor1 { 36 | public let edges: Variables.Indices 37 | public let prior: Group 38 | public let weight: Double 39 | 40 | public init(_ id: TypedID, _ prior: Group, weight: Double) { 41 | self.edges = Tuple1(id) 42 | self.prior = prior 43 | self.weight = weight 44 | } 45 | 46 | @differentiable 47 | public func errorVector(_ x: Group) -> Group.TangentVector { 48 | return weight * prior.localCoordinate(x) 49 | } 50 | } 51 | 52 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/ProbablisticTrackingFactor.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2019 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import PenguinParallel 16 | import PenguinStructures 17 | import TensorFlow 18 | 19 | /// A factor over a target's pose and appearance in an image. 20 | public struct ProbablisticTrackingFactor< 21 | Encoder: AppearanceModelEncoder, 22 | ForegroundModel: GenerativeDensity, BackgroundModel: GenerativeDensity 23 | >: LinearizableFactor1 { 24 | /// The first adjacent variable, the pose of the target in the image. 25 | /// 26 | /// This explicitly specifies `LinearizableFactor2`'s `associatedtype V0`. 27 | public typealias V0 = Pose2 28 | 29 | /// The IDs of the variables adjacent to this factor. 30 | public let edges: Variables.Indices 31 | 32 | /// The image containing the target. 33 | public let measurement: ArrayImage 34 | 35 | public let encoder: Encoder 36 | 37 | public var patchSize: (Int, Int) 38 | 39 | public var appearanceModelSize: (Int, Int) 40 | 41 | public var foregroundModel: ForegroundModel 42 | 43 | public var backgroundModel: BackgroundModel 44 | 45 | public var maxPossibleNegativity: Double 46 | 47 | /// Creates an instance. 48 | /// 49 | /// - Parameters: 50 | /// - poseId: the id of the adjacent pose variable. 51 | /// - measurement: the image containing the target. 52 | /// - appearanceModel: the generative model that produces an appearance from a latent code. 53 | /// - foregroundModel: A generative density on the foreground 54 | /// - backgroundModel: A generative density on the background 55 | public init( 56 | _ poseId: TypedID, 57 | measurement: Tensor, 58 | encoder: Encoder, 59 | patchSize: (Int, Int), 60 | appearanceModelSize: (Int, Int), 61 | foregroundModel: ForegroundModel, 62 | backgroundModel: BackgroundModel, 63 | maxPossibleNegativity: Double = 1e10 64 | ) { 65 | self.edges = Tuple1(poseId) 66 | self.measurement = ArrayImage(measurement) 67 | self.encoder = encoder 68 | self.patchSize = patchSize 69 | self.appearanceModelSize = appearanceModelSize 70 | self.foregroundModel = foregroundModel 71 | self.backgroundModel = backgroundModel 72 | self.maxPossibleNegativity = maxPossibleNegativity 73 | } 74 | 75 | @differentiable 76 | public func errorVector(_ pose: Pose2) -> Vector1 { 77 | let region = OrientedBoundingBox(center: pose, rows: patchSize.0, cols: patchSize.1) 78 | let patch = Tensor(measurement.patch(at: region, outputSize: appearanceModelSize).tensor) 79 | let features = encoder.encode(patch.expandingShape(at: 0)).squeezingShape(at: 0) 80 | 81 | let result = maxPossibleNegativity + foregroundModel.negativeLogLikelihood(features) - backgroundModel.negativeLogLikelihood(features) 82 | 83 | if result < 0 { 84 | print("Warning: Negative value encountered in errorVector! (\(result))") 85 | } 86 | 87 | /// TODO: What is the idiomatic way of avoiding negative probability here? 88 | return Vector1(result) 89 | } 90 | } 91 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/RandomProjection.swift: -------------------------------------------------------------------------------- 1 | 2 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | import TensorFlow 16 | import PenguinStructures 17 | 18 | /// A class performing common activities in the RandomProjection framework. 19 | /// Input shape for training: [N, H, W, C] 20 | /// W matrix: [feature, H*W*C] 21 | /// Output: [feature] 22 | public struct RandomProjection { 23 | public typealias Patch = Tensor 24 | 25 | /// Random Basis Matrix 26 | /// When input image is of shape [N, H, W, C] 27 | /// B is of shape [d, H * W * C] 28 | public let B: Tensor 29 | 30 | /// Initialize the random projector with a normalized projection matrix 31 | public init(fromShape shape: TensorShape, toFeatureSize d: Int) { 32 | let (H, W, C) = (shape[0], shape[1], shape[2]) 33 | B = Tensor( 34 | stacking: (0..(randomNormal: [H * W * C]) 36 | return t/sqrt(t.squared().sum()) 37 | } 38 | ) 39 | } 40 | 41 | /// Initialize given an image batch 42 | public typealias HyperParameters = Int 43 | public init(from imageBatch: Tensor, given d: HyperParameters? = nil) { 44 | self.init(fromShape: imageBatch.shape.suffix(3), toFeatureSize: d ?? 5) 45 | } 46 | 47 | /// Generate an feature from image or image batch 48 | /// Input: [H, W, C] or [N,H,W,C] 49 | /// Output: [d] or [N, d] 50 | @differentiable 51 | public func encode(_ image: Patch) -> Tensor { 52 | precondition(image.rank == 3 || (image.rank == 4), "wrong feature dimension \(image.shape)") 53 | let HWC = B.shape[1] 54 | let d = B.shape[0] 55 | if image.rank == 4 { 56 | let N = image.shape[0] 57 | let v_T = (image).reshaped(to: [HWC, N]).transposed() 58 | return matmul(v_T, B.transposed()).reshaped(to: [N, d]) 59 | } else { 60 | return matmul(B, (image).reshaped(to: [HWC, 1])).reshaped(to: [d]) 61 | } 62 | } 63 | } 64 | 65 | extension RandomProjection: AppearanceModelEncoder {} 66 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/ScalarJacobianFactor.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import PenguinStructures 17 | 18 | /// A Gaussian factor that scales its input by a scalar. 19 | public struct ScalarJacobianFactor: GaussianFactor { 20 | public typealias Variables = Tuple1 21 | 22 | public let edges: Variables.Indices 23 | public let scalar: Double 24 | 25 | public init(edges: Variables.Indices, scalar: Double) { 26 | self.edges = edges 27 | self.scalar = scalar 28 | } 29 | 30 | @differentiable 31 | public func errorVector(at x: Variables) -> ErrorVector { 32 | return scalar * x.head 33 | } 34 | 35 | public func error(at x: Tuple1) -> Double { 36 | return 0.5 * errorVector(at: x).squaredNorm 37 | } 38 | 39 | public func errorVector_linearComponent(_ x: Variables) -> ErrorVector { 40 | return errorVector(at: x) 41 | } 42 | 43 | public func errorVector_linearComponent_adjoint(_ y: ErrorVector) -> Variables { 44 | return Tuple1(scalar * y) 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Inference/SwitchingBetweenFactor.swift: -------------------------------------------------------------------------------- 1 | // 2 | // SwitchingBetweenFactor.swift 3 | // 4 | // 5 | // Frank Dellaert and Marc Rasi 6 | // July 2020 7 | 8 | import _Differentiation 9 | import Foundation 10 | import PenguinStructures 11 | 12 | /// A factor with a switchable motion model. 13 | /// 14 | /// `JacobianRows` specifies the `Rows` parameter of the Jacobian of this factor. See the 15 | /// documentation on `JacobianFactor.jacobian` for more information. Use the typealiases below to 16 | /// avoid specifying this type parameter every time you create an instance. 17 | public struct SwitchingBetweenFactor: VectorFactor3 { 18 | public typealias ErrorVector = Pose.TangentVector 19 | public typealias LinearizableComponent = BetweenFactor 20 | 21 | public let edges: Variables.Indices 22 | 23 | /// Movement templates for each label. 24 | let motions: [Pose] 25 | 26 | public init(_ from: TypedID, 27 | _ label: TypedID, 28 | _ to: TypedID, 29 | _ motions: [Pose]) { 30 | self.edges = Tuple3(from, label, to) 31 | self.motions = motions 32 | } 33 | 34 | public func errorVector(_ from: Pose, _ motionLabel: Int, _ to: Pose) -> ErrorVector { 35 | let actualMotion = between(from, to) 36 | return motions[motionLabel].localCoordinate(actualMotion) 37 | } 38 | 39 | public func linearizableComponent(_ from: Pose, _ motionLabel: Int, _ to: Pose) 40 | -> (LinearizableComponent, LinearizableComponent.Variables) 41 | { 42 | return ( 43 | BetweenFactor(input0ID, input2ID, motions[motionLabel]), 44 | Tuple2(from, to) 45 | ) 46 | } 47 | } 48 | 49 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/MCMC/RandomWalkMetropolis.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import Foundation 17 | import TensorFlow 18 | 19 | /// Runs one step of the RWM algorithm with symmetric proposal. 20 | /// Inspired by tfp [RandomWalkMetropolis](https://www.tensorflow.org/probability/api_docs/python/tfp/mcmc/RandomWalkMetropolis) 21 | public class RandomWalkMetropolis : TransitionKernel { 22 | public typealias Results = Double // target_log_prob_fn of previously accepted state 23 | 24 | let target_log_prob_fn: (State) -> Double 25 | let new_state_fn : (State, inout AnyRandomNumberGenerator)->State 26 | var sourceOfEntropy: AnyRandomNumberGenerator 27 | 28 | public init( 29 | sourceOfEntropy: RandomNumberGenerator = SystemRandomNumberGenerator(), 30 | target_log_prob_fn: @escaping (State) -> Double, 31 | new_state_fn : @escaping (State, inout AnyRandomNumberGenerator)->State 32 | ) { 33 | self.target_log_prob_fn = target_log_prob_fn 34 | self.new_state_fn = new_state_fn 35 | self.sourceOfEntropy = .init(sourceOfEntropy) 36 | } 37 | 38 | /// Runs one iteration of Random Walk Metropolis. 39 | /// TODO(frank): should this be done with inout params in Value-semantics world? 40 | public func one_step(_ current_state: State, _ previous_kernel_results: Results) -> (State, Results) { 41 | 42 | // calculate next state, and new log probability 43 | let new_state = new_state_fn(current_state, &sourceOfEntropy) 44 | let new_log_prob = target_log_prob_fn(new_state) 45 | 46 | // Calculate log of acceptance ratio p(x')/p(x) = log p(x') - log p(x) 47 | let current_log_prob = previous_kernel_results 48 | let log_accept_ratio = new_log_prob - current_log_prob 49 | 50 | // If p(x')/p(x) >= 1 , i.e., log_accept_ratio >= 0, we always accept 51 | // otherwise we accept randomly with probability p(x')/p(x). 52 | // We do this by randomly sampling u from [0,1], and comparing log(u) with log_accept_ratio. 53 | let u = Double.random(in: 0..<1, using: &sourceOfEntropy) 54 | if (log(u) <= log_accept_ratio) { 55 | return (new_state, new_log_prob) 56 | } else { 57 | return (current_state, current_log_prob) 58 | } 59 | } 60 | 61 | /// Initializes side information 62 | public func bootstrap_results(_ init_state: State) -> Results { 63 | return target_log_prob_fn(init_state) 64 | } 65 | } 66 | 67 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/MCMC/TransitionKernel.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | /// Minimal requirements to efficiently implement a Markov chain Monte Carlo (MCMC) transition kernel. A transition kernel returns a new state given some old state. It also takes (and returns) "side information" which may be used for debugging or optimization purposes (i.e, to "recycle" previously computed results). 17 | /// Inspired by tpf [TransitionKernel](https://www.tensorflow.org/probability/api_docs/python/tfp/mcmc/TransitionKernel) 18 | public protocol TransitionKernel { 19 | associatedtype State 20 | associatedtype Results 21 | 22 | /// Takes one step of the TransitionKernel. 23 | func one_step(_ current_state: State, _ previous_kernel_results: Results) -> (State, Results) 24 | 25 | /// Returns an object with the same type as returned by one_step(...)[1]. 26 | func bootstrap_results(_ init_state: State) -> Results 27 | } 28 | 29 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/MCMC/sample.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | /// Implements Markov chain Monte Carlo via repeated TransitionKernel steps 17 | /// Inspired by tfp [sample_chain](https://www.tensorflow.org/probability/api_docs/python/tfp/mcmc/sample_chain) 18 | public func sampleChain(_ num_results:Int, 19 | _ init_state:Kernel.State, 20 | _ kernel:Kernel, 21 | _ num_burnin_steps:Int) -> Array { 22 | // Initialize kernel side information 23 | var results = kernel.bootstrap_results(init_state) 24 | 25 | // Allocate result 26 | var states = [init_state] 27 | states.reserveCapacity(num_burnin_steps + num_results) 28 | 29 | // Run sampler 30 | for _ in 1.. precision { 47 | incrementCounter("cgls step") 48 | // print("[CGLS ] residual = \(r.squaredNorm), true = \(gfg.errorVectors(at: x).squaredNorm)") 49 | let q = gfg.errorVectors_linearComponent(at: p) // q(k) = A * p(k) 50 | 51 | let alpha: Double = gamma / q.squaredNorm // α(k) = γ(k)/||q(k)||^2 52 | x = x + (alpha * p) // x(k+1) = x(k) + α(k) * p(k) 53 | r = r + (-alpha) * q // r(k+1) = r(k) - α(k) * q(k) 54 | s = gfg.errorVectors_linearComponent_adjoint(r) // s(k+1) = A.T * r(k+1) 55 | 56 | let gamma_next = s.squaredNorm // γ(k+1) = ||s(k+1)||^2 57 | let beta: Double = gamma_next/gamma // β(k) = γ(k+1)/γ(k) 58 | gamma = gamma_next 59 | p = s + beta * p // p(k+1) = s(k+1) + β(k) * p(k) 60 | 61 | if (alpha * p).squaredNorm < precision { 62 | break 63 | } 64 | step += 1 65 | } 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Optimizers/GradientDescent.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | /// Optimizes the variables in a factor graph to minimize the error, using gradient descent. 17 | public struct GradientDescent { 18 | /// The fraction of the gradient to move per step. 19 | public var learningRate: Double 20 | 21 | /// Creates an instance with the given `learningRate`. 22 | public init(learningRate: Double) { 23 | self.learningRate = learningRate 24 | } 25 | 26 | /// Moves `values` along the gradient of `objective`'s error function for a single gradient 27 | /// descent step. 28 | public func update(_ values: inout VariableAssignments, objective: FactorGraph) { 29 | values.move(along: -learningRate * objective.errorGradient(at: values)) 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Probability/MultivariateGaussian.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2019 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import TensorFlow 16 | 17 | /// Calculate the covariance of a data matrix 18 | /// preconditon: X.rank == 2 19 | public func cov(_ X: Tensor) -> Tensor { 20 | precondition(X.rank == 2, "cov() can only handle 2D data") 21 | let X_norm = X - X.mean(squeezingAxes: 0) 22 | let N = X.shape[0] 23 | return matmul(X_norm.transposed(), X_norm) / Double(N - 1) 24 | } 25 | 26 | /// A Multivariate Gaussian Density 27 | /// 28 | /// This is a density where all dimensions 29 | /// share one multivariate Gaussian density. 30 | public struct MultivariateGaussian: GenerativeDensity { 31 | public typealias T = Tensor 32 | public typealias HyperParameters = Double /// Just the regularizer 33 | 34 | public let mean: T /// mean 35 | public let information: T /// Information matrix 36 | public let constant : Double /// normalization constant 37 | 38 | /** 39 | Initialize a Multivariate Gaussian Model 40 | - Parameters: 41 | - mean: n-dimensional vector 42 | - information: information matrix of shape [n,n] 43 | */ 44 | public init(mean: T, information: T) { 45 | precondition(mean.rank == 1, "mean has to be a vector") 46 | let n = mean.shape[0] 47 | precondition(information.shape == [n,n], "information has to be nxn") 48 | self.mean = mean 49 | self.information = information 50 | self.constant = sqrt(_Raw.matrixDeterminant(information/(2.0 * .pi)).scalarized()) 51 | } 52 | 53 | /// Initalize by fitting the model to the data 54 | /// - data: Tensor of shape [N, ] 55 | public init(from data: T, regularizer r: Double) { 56 | assert(data.shape.dropFirst().rank == 1) 57 | let mean = data.mean(squeezingAxes: 0) 58 | let cov_data = cov(data) 59 | let regularized_cov = cov_data.withDiagonal(cov_data.diagonalPart() + r) 60 | self.init(mean:mean, information: pinv(regularized_cov)) 61 | } 62 | 63 | /// Initalize by fitting the model to the data 64 | /// - data: Tensor of shape [N, ] 65 | public init(from data: T, given p:HyperParameters? = nil) { 66 | self.init(from:data, regularizer: p ?? 1e-10) 67 | } 68 | 69 | /// Calculated the negative log likelihood of *one* data point 70 | /// Note this is NOT normalized probability 71 | @differentiable public func negativeLogLikelihood(_ sample: T) -> Double { 72 | precondition(sample.shape == mean.shape) 73 | let normalized = (sample - mean).expandingShape(at: 1) 74 | /// FIXME: this is a workaround for bug in the derivative of `.scalarized()` 75 | let t = matmul(normalized, transposed: true, matmul(information, normalized)).withDerivative { 76 | $0 = $0.reshaped(to: [1, 1]) 77 | } 78 | 79 | return t.scalarized() / 2.0 80 | } 81 | 82 | /// Calculated normalized probability 83 | @differentiable public func probability(_ sample: T) -> Double { 84 | // - ToDo: Precalculate constant 85 | let E = negativeLogLikelihood(sample) 86 | return exp(-E) * self.constant 87 | } 88 | } 89 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Probability/NaiveBayes.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2019 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import TensorFlow 16 | 17 | /// A Gaussian Naive Bayes density 18 | /// 19 | /// This is a density where each dimension has its own 1-d Gaussian density. 20 | public struct GaussianNB: GenerativeDensity { 21 | public typealias T = Tensor 22 | public typealias HyperParameters = Double /// Just the regularizer 23 | 24 | public let mu: T /// Sample Mean 25 | public let sigmas: T /// Sample standard deviation 26 | public let precisions: T /// Cached precisions 27 | 28 | /** Initalize by fitting the model to the data 29 | - Parameters: 30 | - data: Tensor of shape [N, ] 31 | - regularizer: avoids division by zero when the data is zero variance 32 | */ 33 | public init(from data: T, regularizer r: Double) { 34 | self.mu = data.mean(squeezingAxes: 0) 35 | let sigmas = data.standardDeviation(squeezingAxes: 0) + r 36 | self.sigmas = sigmas 37 | self.precisions = 1.0 / sigmas.squared() 38 | } 39 | 40 | /// Initalize by fitting the model to the data 41 | /// - data: Tensor of shape [N, ] 42 | public init(from data: T, given p:HyperParameters? = nil) { 43 | self.init(from:data, regularizer: p ?? 1e-10) 44 | } 45 | 46 | /// Calculated the negative log likelihood of *one* data point 47 | /// Note this is NOT normalized probability 48 | @differentiable public func negativeLogLikelihood(_ sample: T) -> Double { 49 | precondition(sample.shape == mu.shape) 50 | let t = (sample - mu).squared() * precisions 51 | return t.sum().scalarized() / 2.0 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /Sources/SwiftFusion/Probability/ProbabilityModel.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2019 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import TensorFlow 16 | 17 | public protocol GenerativeDensity { 18 | associatedtype HyperParameters 19 | init(from data: Tensor, given: HyperParameters?) 20 | @differentiable func negativeLogLikelihood(_ data: Tensor) -> Double 21 | } 22 | 23 | public extension GenerativeDensity { 24 | /// Extension allows to have a default nil parameter 25 | init(from data: Tensor) { 26 | self.init(from: data, given: nil) 27 | } 28 | } 29 | 30 | -------------------------------------------------------------------------------- /Sources/SwiftFusionBenchmarks/PPCATracking.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import Benchmark 17 | import SwiftFusion 18 | import TensorFlow 19 | 20 | let ppcaTrackingBenchmark = BenchmarkSuite(name: "PPCATracking") { suite in 21 | /// Returns a factor graph with a single `PPCATrackingFactor` with randomly initialized 22 | /// parameters. 23 | func makeFactorGraphWithOnePPCATrackingFactor() -> (FactorGraph, VariableAssignments) { 24 | var x = VariableAssignments() 25 | let poseId = x.store(Pose2(100, 100, 0)) 26 | let latentId = x.store(Vector5.zero) 27 | 28 | var fg = FactorGraph() 29 | fg.store(PPCATrackingFactor.testFixture(poseId, latentId, seed: (1, 1))) 30 | return (fg, x) 31 | } 32 | 33 | /// Measures how long it takes to linearize a `PPCATrackingFactor`. 34 | suite.benchmark( 35 | "linearize PPCATrackingFactor", 36 | settings: Iterations(1), TimeUnit(.ms) 37 | ) { state in 38 | let (fg, x) = makeFactorGraphWithOnePPCATrackingFactor() 39 | try state.measure { 40 | _ = fg.linearized(at: x) 41 | } 42 | } 43 | 44 | /// Measures how long it takes to run CGLS on the lineraization of a `PPCATrackingFactor`. 45 | suite.benchmark( 46 | "cgls LinearizedPPCATrackingFactor", 47 | settings: Iterations(1), TimeUnit(.ms) 48 | ) { state in 49 | let (fg, x) = makeFactorGraphWithOnePPCATrackingFactor() 50 | let gfg = fg.linearized(at: x) 51 | try state.measure { 52 | var optimizer = GenericCGLS() 53 | var dx = x.tangentVectorZeros 54 | optimizer.optimize(gfg: gfg, initial: &dx) 55 | } 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /Sources/SwiftFusionBenchmarks/Patch.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /// Benchmarks Pose2SLAM solutions. 16 | 17 | import _Differentiation 18 | import Benchmark 19 | import SwiftFusion 20 | import TensorFlow 21 | 22 | let patchBenchmark = BenchmarkSuite(name: "Patch") { suite in 23 | suite.benchmark( 24 | "PatchForward", 25 | settings: Iterations(1), TimeUnit(.ms) 26 | ) { 27 | let rows = 100 28 | let cols = 100 29 | let image = Tensor(randomNormal: [500, 500, 1]) 30 | _ = image.patch(at: OrientedBoundingBox(center: Pose2(100, 100, 0.5), rows: rows, cols: cols)) 31 | } 32 | 33 | suite.benchmark( 34 | "PatchJacobian", 35 | settings: Iterations(1), TimeUnit(.ms) 36 | ) { 37 | let rows = 28 38 | let cols = 62 39 | let latentDimension = 5 40 | 41 | let image = Tensor(randomNormal: [500, 500]) 42 | 43 | let W = Tensor(randomNormal: [rows * cols, latentDimension]) 44 | let mu = Tensor(randomNormal: [rows, cols]) 45 | func errorVector(_ center: Pose2, _ latent: Vector5) -> Tensor { 46 | let bbox = OrientedBoundingBox(center: center, rows: rows, cols: cols) 47 | let generated = mu + matmul(W, latent.flatTensor.expandingShape(at: 1)).reshaped(to: [rows, cols]) 48 | return generated - image.patch(at: bbox) 49 | } 50 | 51 | let (value, pb) = valueWithPullback(at: Pose2(100, 100, 0.5), Vector5.zero, in: errorVector) 52 | 53 | for i in 0..(zeros: [rows, cols]) 56 | basisVector[i, j] = Tensor(1) 57 | _ = pb(basisVector) 58 | } 59 | } 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /Sources/SwiftFusionBenchmarks/Pose2SLAM.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /// Benchmarks Pose2SLAM solutions. 16 | 17 | import _Differentiation 18 | import Benchmark 19 | import SwiftFusion 20 | 21 | let pose2SLAM = BenchmarkSuite(name: "Pose2SLAM") { suite in 22 | let intelDataset = 23 | try! G2OReader.G2OFactorGraph(g2oFile2D: try! cachedDataset("input_INTEL_g2o.txt")) 24 | check( 25 | intelDataset.graph.error(at: intelDataset.initialGuess), 26 | near: 0.5 * 73565.64, 27 | accuracy: 1e-2) 28 | 29 | // Uses `FactorGraph` on the Intel dataset. 30 | // The solvers are configured to run for a constant number of steps. 31 | // The nonlinear solver is 10 iterations of Gauss-Newton. 32 | // The linear solver is 500 iterations of CGLS. 33 | suite.benchmark( 34 | "FactorGraph, Intel, 10 Gauss-Newton steps, 500 CGLS steps", 35 | settings: Iterations(1), TimeUnit(.ms) 36 | ) { 37 | var x = intelDataset.initialGuess 38 | var graph = intelDataset.graph 39 | graph.store(PriorFactor(TypedID(0), Pose2(0, 0, 0))) 40 | 41 | for _ in 0..<10 { 42 | let linearized = graph.linearized(at: x) 43 | var dx = x.tangentVectorZeros 44 | var optimizer = GenericCGLS(precision: 0, max_iteration: 500) 45 | optimizer.optimize(gfg: linearized, initial: &dx) 46 | x.move(along: dx) 47 | } 48 | 49 | check(graph.error(at: x), near: 0.5 * 0.987, accuracy: 1e-2) 50 | } 51 | } 52 | 53 | func check(_ actual: Double, near expected: Double, accuracy: Double) { 54 | if abs(actual - expected) > accuracy { 55 | print("ERROR: \(actual) != \(expected) (accuracy \(accuracy))") 56 | fatalError() 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /Sources/SwiftFusionBenchmarks/Pose3SLAM.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /// Benchmarks Pose3SLAM solutions. 16 | 17 | import _Differentiation 18 | import Benchmark 19 | import SwiftFusion 20 | 21 | let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in 22 | let sphere2500URL = try! cachedDataset("sphere2500.g2o") 23 | let sphere2500Dataset = try! G2OReader.G2OFactorGraph(g2oFile3D: sphere2500URL) 24 | 25 | // Uses `FactorGraph` on the sphere2500 dataset. 26 | // The solvers are configured to run for a constant number of *LM steps*, except when the LM solver is 27 | // unable to progress even with maximum lambda. 28 | // The linear solver is 200 iterations of CGLS. 29 | suite.benchmark( 30 | "FactorGraph, sphere2500, 30 LM steps, 200 CGLS steps", 31 | settings: Iterations(1), TimeUnit(.ms) 32 | ) { 33 | var val = sphere2500Dataset.initialGuess 34 | var graph = sphere2500Dataset.graph 35 | 36 | graph.store(PriorFactor(TypedID(0), Pose3())) 37 | 38 | var optimizer = LM() 39 | optimizer.max_iteration = 30 40 | optimizer.max_inner_iteration = 200 41 | 42 | do { 43 | try optimizer.optimize(graph: graph, initial: &val) 44 | } catch let error { 45 | print("The solver gave up, message: \(error.localizedDescription)") 46 | } 47 | } 48 | 49 | suite.benchmark( 50 | "sphere2500, chordal initialization", 51 | settings: Iterations(1), TimeUnit(.ms) 52 | ) { 53 | _ = ChordalInitialization.GetInitializations( 54 | graph: sphere2500Dataset.graph, ids: sphere2500Dataset.variableId) 55 | } 56 | 57 | let sphere2500DatasetChordal = try! G2OReader.G2OFactorGraph( 58 | g2oFile3D: sphere2500URL, chordal: true) 59 | 60 | suite.benchmark( 61 | "sphere2500, chordal graph, 1 LM step, 200 CGLS steps", 62 | settings: Iterations(1), TimeUnit(.ms) 63 | ) { 64 | var val = sphere2500DatasetChordal.initialGuess 65 | var graph = sphere2500DatasetChordal.graph 66 | 67 | graph.store(PriorFactor(TypedID(0), Pose3())) 68 | 69 | var optimizer = LM() 70 | optimizer.max_iteration = 1 71 | optimizer.max_inner_iteration = 200 72 | 73 | do { 74 | try optimizer.optimize(graph: graph, initial: &val) 75 | } catch let error { 76 | print("The solver gave up, message: \(error.localizedDescription)") 77 | } 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /Sources/SwiftFusionBenchmarks/main.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import Benchmark 17 | 18 | Benchmark.main([ 19 | patchBenchmark, 20 | ppcaTrackingBenchmark, 21 | pose2SLAM, 22 | pose3SLAM 23 | ]) 24 | -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/BeeDatasetTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import BeeDataset 17 | import SwiftFusion 18 | import TensorFlow 19 | import XCTest 20 | 21 | final class BeeDatasetTests: XCTestCase { 22 | /// Tests that we can load the frames from a fake dataset. 23 | func testLoadFrames() { 24 | let frames = BeeFrames( 25 | directory: datasetDirectory.appendingPathComponent("frames").appendingPathComponent("seq1"))! 26 | XCTAssertEqual(frames.count, 2) 27 | let t1 = frames[0] 28 | XCTAssertEqual(t1.shape, [200, 100, 3]) 29 | XCTAssertEqual(t1[0, 0], Tensor([255, 0, 0])) 30 | let t2 = frames[1] 31 | XCTAssertEqual(t2.shape, [200, 100, 3]) 32 | XCTAssertEqual(t2[0, 0], Tensor([0, 255, 0])) 33 | } 34 | 35 | /// Tests that we can load the oriented bounding boxes from a fake dataset. 36 | func testLoadOrientedBoundingBoxes() { 37 | let obbs = beeOrientedBoundingBoxes( 38 | file: datasetDirectory.appendingPathComponent("obbs").appendingPathComponent("seq1.txt"))! 39 | XCTAssertEqual(obbs.count, 2) 40 | XCTAssertEqual(obbs[0], OrientedBoundingBox( 41 | center: Pose2(Rot2(1), Vector2(100, 200)), rows: 28, cols: 62)) 42 | XCTAssertEqual(obbs[1], OrientedBoundingBox( 43 | center: Pose2(Rot2(1.5), Vector2(105, 201)), rows: 28, cols: 62)) 44 | } 45 | 46 | /// Tests that we can load a `BeeVideo`. 47 | func testLoadBeeVideo() { 48 | let v = BeeVideo(videoName: "bee_video_1")! 49 | XCTAssertEqual(v.frames.count, 96) 50 | XCTAssertGreaterThanOrEqual(v.tracks.count, 2) 51 | XCTAssertEqual(v.tracks[0].count, 96) 52 | } 53 | 54 | /// Directory of a fake dataset for tests. 55 | let datasetDirectory = URL.sourceFileDirectory().appendingPathComponent("fakeDataset") 56 | } 57 | 58 | extension URL { 59 | /// Creates a URL for the directory containing the caller's source file. 60 | static func sourceFileDirectory(file: String = #filePath) -> URL { 61 | return URL(fileURLWithPath: file).deletingLastPathComponent() 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/BeePPCATests.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | import BeeDataset 3 | import SwiftFusion 4 | import TensorFlow 5 | import XCTest 6 | 7 | import PenguinStructures 8 | 9 | final class BeePPCATests: XCTestCase { 10 | /// Sanity test that nothing underlying the PPCA algorithm has changed 11 | func testPPCA() { 12 | let frames = BeeFrames(sequenceName: "seq4")! 13 | let obbs = beeOrientedBoundingBoxes(sequenceName: "seq4")! 14 | 15 | let num_samples = 20 16 | let images_bw = (0..(frames[1].mean(alongAxes: [2])), 50 | appearanceModel: ppca.decode, appearanceModelJacobian: { _ in ppca.W })) 51 | 52 | // Prior on latent initialized by PPCA decode on the previous frame 53 | fg.store(PriorFactor(latentId, initialLatent)) 54 | 55 | var optimizer = LM() 56 | optimizer.verbosity = .SILENT 57 | 58 | try? optimizer.optimize(graph: fg, initial: &v) 59 | 60 | let expected = Pose2(Rot2(-1.3365823146263909), Vector2(364.59389156740497, 176.17400761774488)) 61 | 62 | XCTAssertEqual(expected.localCoordinate(v[poseId]).norm, 0, accuracy: 1e-2) 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/OISTDatasetTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import BeeDataset 16 | import Foundation 17 | import PenguinParallelWithFoundation 18 | import XCTest 19 | 20 | final class OISTDatasetTests: XCTestCase { 21 | /// Directory of a fake dataset for tests. 22 | let datasetDirectory = URL.sourceFileDirectory().appendingPathComponent("fakeDataset") 23 | 24 | /// Test that eager dataset loading works properly. 25 | func testEagerDatasetLoad() throws { 26 | if let _ = ProcessInfo.processInfo.environment["CI"] { 27 | throw XCTSkip("Test skipped on CI because it downloads a lot of data.") 28 | } 29 | ComputeThreadPools.local = 30 | NonBlockingThreadPool(name: "mypool", threadCount: 5) 31 | 32 | // Truncate the frames so that this test does not take a huge amount of time. 33 | let video = OISTBeeVideo(directory: datasetDirectory, length: 2)! 34 | 35 | XCTAssertEqual(video.frames.count, 2) 36 | XCTAssertNotEqual(video.frames[0], video.frames[1]) 37 | 38 | // There are fewer tracks because we truncated the frames. 39 | XCTAssertEqual(video.tracks.count, 1) 40 | 41 | // The tracks are shorter because we truncated the frames. 42 | XCTAssertEqual(video.tracks[0].boxes.count, 2) 43 | } 44 | 45 | func testToString() { 46 | let label = OISTBeeLabel( 47 | frameIndex: 1515, 48 | label: .Body, 49 | rawLocation: (1.2, 38, 1.13559), 50 | offset: (114514, 1919810) 51 | ) 52 | 53 | let string = label.toString() 54 | let expected = "114514\t1919810\t1\t1.2\t38.0\t1.13559" 55 | XCTAssertEqual(string, expected) 56 | } 57 | 58 | func testTrackLoading() { 59 | /// 628.0 1211.0 -1.1518463267948966 40 70 60 | let dataset = OISTBeeVideo(directory: datasetDirectory, afterIndex: 1, length: 1)! 61 | 62 | XCTAssertEqual(dataset.tracks[0].boxes[0].center.rot.theta, -1.279746, accuracy: 1e-3) 63 | XCTAssertEqual(dataset.tracks.count, 1) 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/fakeDataset/downsampled/frame_30fps_001515.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/SwiftFusion/5e97a567d5201489bc77cbc720e5ce806f470f44/Tests/BeeDatasetTests/fakeDataset/downsampled/frame_30fps_001515.png -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/fakeDataset/downsampled/frame_30fps_001530.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/SwiftFusion/5e97a567d5201489bc77cbc720e5ce806f470f44/Tests/BeeDatasetTests/fakeDataset/downsampled/frame_30fps_001530.png -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/fakeDataset/frames/seq1/frame1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/SwiftFusion/5e97a567d5201489bc77cbc720e5ce806f470f44/Tests/BeeDatasetTests/fakeDataset/frames/seq1/frame1.png -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/fakeDataset/frames/seq1/frame2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/borglab/SwiftFusion/5e97a567d5201489bc77cbc720e5ce806f470f44/Tests/BeeDatasetTests/fakeDataset/frames/seq1/frame2.png -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/fakeDataset/frames/seq1/index.txt: -------------------------------------------------------------------------------- 1 | 2 2 | -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/fakeDataset/obbs/seq1.txt: -------------------------------------------------------------------------------- 1 | 1 100 200 2 | 1.5 105 201 3 | -------------------------------------------------------------------------------- /Tests/BeeDatasetTests/fakeDataset/tracks/track000.txt: -------------------------------------------------------------------------------- 1 | 0 2 | 628.0 1201.0 -1.1016863267948964 40 70 3 | 632.0 1206.0 -1.2797463267948965 40 70 4 | -------------------------------------------------------------------------------- /Tests/BeeTrackingTests/AppearanceRAETests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import TensorFlow 16 | import XCTest 17 | 18 | import BeeTracking 19 | 20 | class BeeTrackingTests: XCTestCase { 21 | /// Test that the hand-coded Jacobian for the decode method gives the same results as the 22 | /// AD-generated Jacobian. 23 | func testDecodeJacobian() { 24 | // Size of the images. 25 | let h = 10 26 | let w = 10 27 | let c = 10 28 | 29 | // Number of batches to test. (`decodeJacobian` currently only supports one batch at a time). 30 | let batchCount = 1 31 | 32 | // Model size parameters. 33 | let latentDimension = 5 34 | let hiddenDimension = 100 35 | 36 | // A random model and a random latent code to decode. 37 | let model = DenseRAE( 38 | imageHeight: h, imageWidth: w, imageChannels: c, 39 | hiddenDimension: hiddenDimension, latentDimension: latentDimension) 40 | let latent = Tensor(randomNormal: [batchCount, latentDimension]) 41 | 42 | // The hand-coded Jacobian. 43 | let actualJacobian = model.decodeJacobian(latent) 44 | 45 | // The AD-generated pullback function. 46 | let pb = pullback(at: latent) { model.decode($0) } 47 | 48 | // Pass all the unit vectors throught the AD-generated pullback function and check that the 49 | // results match the hand-coded Jacobian. 50 | for batch in 0..(zeros: [batchCount, h, w, c]) 55 | unit[batch, i, j, k] = Tensor(1) 56 | XCTAssertLessThan( 57 | (actualJacobian[batch, i, j, k] - pb(unit)).squared().sum().scalar!, 1e-6) 58 | } 59 | } 60 | } 61 | } 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /Tests/BeeTrackingTests/LikelihoodModelEMTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2019 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /// This file tests Monte Carlo EM training of appearance models 16 | 17 | import BeeTracking 18 | import SwiftFusion 19 | import TensorFlow 20 | import XCTest 21 | 22 | // Test with a random projection feature space, and Gaussian/NB for FG/BG 23 | typealias RPGaussianNB = TrackingLikelihoodModel 24 | 25 | final class TrackingLikelihoodModelTests: XCTestCase { 26 | /// Test fitting a simple 2-component mixture 27 | func testTrackingLikelihoodModel() { 28 | let frame = Tensor(zeros:[1000,1000,1]) 29 | let boundingBoxes = [Vector2(100, 200), Vector2(150, 201), Vector2(600, 800)].map { 30 | OrientedBoundingBox(center: Pose2(Rot2(0), $0), rows: 70, cols: 40) 31 | } 32 | let patches = Tensor(boundingBoxes.map {obb in frame.patch(at: obb)}) 33 | let model = RPGaussianNB(from:patches, and:patches) 34 | XCTAssertEqual(model.encoder.B.shape, [5,70*40]) 35 | XCTAssertEqual(model.foregroundModel.mean.shape, [5]) 36 | XCTAssertEqual(model.backgroundModel.mu.shape, [5]) 37 | } 38 | } 39 | 40 | final class TrackingLikelihoodModelEMTests: XCTestCase { 41 | /// Test fitting a simple 2-component mixture 42 | func testTrackingLikelihoodModel() { 43 | let generator = ARC4RandomNumberGenerator(seed: 42) 44 | let frame = Tensor(zeros:[1000,1000,1]) 45 | let data = [(Vector2(100, 200), RPGaussianNB.PatchType.fg), 46 | (Vector2(150, 201), RPGaussianNB.PatchType.fg), 47 | (Vector2(600, 800), RPGaussianNB.PatchType.bg)].map { 48 | (frame, $1, OrientedBoundingBox(center: Pose2(Rot2(0), $0), rows: 70, cols: 40)) 49 | } 50 | var em = MonteCarloEM(sourceOfEntropy: generator) 51 | let model = em.run(with:data, iterationCount: 3) 52 | XCTAssertEqual(model.encoder.B.shape, [5,70*40]) 53 | XCTAssertEqual(model.foregroundModel.mean.shape, [5]) 54 | XCTAssertEqual(model.backgroundModel.mu.shape, [5]) 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /Tests/BeeTrackingTests/OISTBeeVideo+BatchesTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import Foundation 16 | import PenguinParallelWithFoundation 17 | import TensorFlow 18 | import XCTest 19 | 20 | import BeeDataset 21 | import BeeTracking 22 | 23 | final class OISTBeeVideoBatchesTests: XCTestCase { /// Directory of a fake dataset for tests. 24 | let datasetDirectory = URL.sourceFileDirectory().appendingPathComponent("../BeeDatasetTests/fakeDataset") 25 | 26 | /// Tests getting a batch of bee patches and a batch of background patches. 27 | func testBeeBatch() throws { 28 | ComputeThreadPools.local = 29 | NonBlockingThreadPool(name: "mypool", threadCount: 5) 30 | let video = OISTBeeVideo(directory: datasetDirectory, deferLoadingFrames: false)! 31 | let (batch, statistics) = video.makeBatch( 32 | appearanceModelSize: (100, 100), 33 | randomFrameCount: 2, 34 | batchSize: 200 35 | ) 36 | XCTAssertEqual(batch.shape, [200, 100, 100, 1]) 37 | XCTAssertEqual(statistics.mean.shape, []) 38 | XCTAssertEqual(statistics.standardDeviation.shape, []) 39 | 40 | let bgBatch = video.makeBackgroundBatch( 41 | patchSize: (40, 70), appearanceModelSize: (100, 100), 42 | statistics: statistics, 43 | randomFrameCount: 2, 44 | batchSize: 200 45 | ) 46 | XCTAssertEqual(bgBatch.shape, [200, 100, 100, 1]) 47 | } 48 | } 49 | 50 | extension URL { 51 | /// Creates a URL for the directory containing the caller's source file. 52 | fileprivate static func sourceFileDirectory(file: String = #filePath) -> URL { 53 | return URL(fileURLWithPath: file).deletingLastPathComponent() 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /Tests/BeeTrackingTests/TrackingFactorGraphTests.swift: -------------------------------------------------------------------------------- 1 | import XCTest 2 | import BeeTracking 3 | import PythonKit 4 | import PenguinStructures 5 | import BeeDataset 6 | import BeeTracking 7 | import SwiftFusion 8 | import TensorFlow 9 | 10 | class TrackingFactorGraphTests: XCTestCase { 11 | let datasetDirectory = URL.sourceFileDirectory().appendingPathComponent("../BeeDatasetTests/fakeDataset") 12 | 13 | func testGetTrainingBatches() { 14 | let dataset = OISTBeeVideo(directory: datasetDirectory, length: 2)! 15 | 16 | let (fg, bg, _) = getTrainingBatches( 17 | dataset: dataset, boundingBoxSize: (40, 70), 18 | fgBatchSize: 10, bgBatchSize: 11, 19 | fgRandomFrameCount: 1, 20 | bgRandomFrameCount: 1 21 | ) 22 | 23 | XCTAssertEqual(fg.shape, TensorShape([10, 40, 70, 1])) 24 | XCTAssertEqual(bg.shape, TensorShape([11, 40, 70, 1])) 25 | } 26 | 27 | func testTrainRPTracker() { 28 | let trainingData = OISTBeeVideo(directory: datasetDirectory, length: 1)! 29 | let testData = OISTBeeVideo(directory: datasetDirectory, length: 2)! 30 | 31 | // Test regular training 32 | var tracker : TrackingConfiguration> = trainRPTracker( 33 | trainingData: trainingData, 34 | frames: testData.frames, boundingBoxSize: (40, 70), withFeatureSize: 100, 35 | fgRandomFrameCount: 1, 36 | bgRandomFrameCount: 1 37 | ) 38 | XCTAssertEqual(tracker.frameVariableIDs.count, 2) 39 | 40 | // Test training with Monte Carlo EM 41 | tracker = trainRPTracker( 42 | trainingData: trainingData, 43 | frames: testData.frames, boundingBoxSize: (40, 70), withFeatureSize: 100, 44 | fgRandomFrameCount: 1, bgRandomFrameCount: 1, usingEM: true 45 | ) 46 | XCTAssertEqual(tracker.frameVariableIDs.count, 2) 47 | } 48 | 49 | func testCreateSingleRPTrack() { 50 | let trainingData = OISTBeeVideo(directory: datasetDirectory, length: 2)! 51 | let testData = OISTBeeVideo(directory: datasetDirectory, length: 2)! 52 | var tracker = trainRPTracker( 53 | trainingData: trainingData, 54 | frames: testData.frames, boundingBoxSize: (40, 70), withFeatureSize: 100, 55 | fgRandomFrameCount: 2, 56 | bgRandomFrameCount: 2 57 | ) 58 | 59 | var (track, groundTruth) = createSingleTrack( 60 | onTrack: 0, withTracker: &tracker, andTestData: testData 61 | ) 62 | XCTAssertEqual(track.count, 2) 63 | XCTAssertEqual(groundTruth.count, 2) 64 | 65 | // Now try with sampling 66 | (track, groundTruth) = createSingleTrack( 67 | onTrack: 0, withTracker: &tracker, andTestData: testData, withSampling: true 68 | ) 69 | XCTAssertEqual(track.count, 2) 70 | XCTAssertEqual(groundTruth.count, 2) 71 | } 72 | 73 | // func testRunRPTracker() { 74 | // let fig: PythonObject = runRPTracker(onTrack: 15) 75 | // XCTAssertEqual(fig.axes.count, 2) 76 | // } 77 | } 78 | 79 | extension URL { 80 | /// Creates a URL for the directory containing the caller's source file. 81 | fileprivate static func sourceFileDirectory(file: String = #filePath) -> URL { 82 | return URL(fileURLWithPath: file).deletingLastPathComponent() 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /Tests/LinuxMain.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | fatalError("Run the tests with `swift test --enable-test-discovery`.") 3 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Applications/ManipulationTests.swift: -------------------------------------------------------------------------------- 1 | // 2 | // File.swift 3 | // 4 | // 5 | // Created by Fan Jiang on 2020/4/22. 6 | // 7 | 8 | import _Differentiation 9 | import Foundation 10 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Core/Dictionary+DifferentiableTests.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | import Foundation 3 | import XCTest 4 | 5 | import SwiftFusion 6 | 7 | class DictionaryDifferentiableTests: XCTestCase { 8 | /// Test the `AdditiveArithmetic` `zero` requirement. 9 | func testAdditiveArithmeticZero() { 10 | XCTAssertEqual(Dictionary.zero, [:]) 11 | } 12 | 13 | /// Test the `AdditiveArithmetic` `+` requirement. 14 | func testAdditiveArithmeticPlus() { 15 | XCTAssertEqual( 16 | ([:] as [String: Int]) + [:], 17 | [:] 18 | ) 19 | XCTAssertEqual( 20 | ["a": 1] + [:], 21 | ["a": 1] 22 | ) 23 | XCTAssertEqual( 24 | [:] + ["b": 1], 25 | ["b": 1] 26 | ) 27 | XCTAssertEqual( 28 | ["a": 1] + ["b": 1], 29 | ["a": 1, "b": 1] 30 | ) 31 | XCTAssertEqual( 32 | ["a": 1, "b": 1] + ["b": 1], 33 | ["a": 1, "b": 2] 34 | ) 35 | } 36 | 37 | /// Test the `AdditiveArithmetic` `-` requirement. 38 | func testAdditiveArithmeticMinus() { 39 | XCTAssertEqual( 40 | ([:] as [String: Int]) - [:], 41 | [:] 42 | ) 43 | XCTAssertEqual( 44 | ["a": 1] - [:], 45 | ["a": 1] 46 | ) 47 | XCTAssertEqual( 48 | [:] - ["b": 1], 49 | ["b": -1] 50 | ) 51 | XCTAssertEqual( 52 | ["a": 1] - ["b": 1], 53 | ["a": 1, "b": -1] 54 | ) 55 | XCTAssertEqual( 56 | ["a": 1, "b": 1] - ["b": 1], 57 | ["a": 1, "b": 0] 58 | ) 59 | } 60 | 61 | /// Test the `Differentiable` `move` requirement. 62 | func testMove() { 63 | var foo: [String: Double] = ["a": 0, "b": 0] 64 | foo.move(along: ["a": 1]) 65 | XCTAssertEqual(foo, ["a": 1, "b": 0]) 66 | foo.move(along: ["b": 1]) 67 | XCTAssertEqual(foo, ["a": 1, "b": 1]) 68 | foo.move(along: ["a": 1, "b": 2]) 69 | XCTAssertEqual(foo, ["a": 2, "b": 3]) 70 | } 71 | 72 | /// Test the `Differentiable` `zeroTangentVector` requirement. 73 | func testZeroTangentVector() { 74 | XCTAssertEqual( 75 | ([:] as [String: Double]).zeroTangentVector, 76 | [:] 77 | ) 78 | XCTAssertEqual( 79 | (["a": 1] as [String: Double]).zeroTangentVector, 80 | ["a": 0] 81 | ) 82 | } 83 | 84 | /// Test the value and derivative of `differentiableSubscript`. 85 | func testDifferentiableSubscript() { 86 | let point: [String: Double] = ["a": 1, "b": 2] 87 | XCTAssertEqual(point.differentiableSubscript("a"), 1) 88 | XCTAssertEqual(point.differentiableSubscript("b"), 2) 89 | XCTAssertEqual( 90 | gradient(at: point) { $0.differentiableSubscript("a") }, 91 | ["a": 1] 92 | ) 93 | XCTAssertEqual( 94 | gradient(at: point) { $0.differentiableSubscript("a") + $0.differentiableSubscript("b") }, 95 | ["a": 1, "b": 1] 96 | ) 97 | } 98 | 99 | static var allTests = [ 100 | ("testAdditiveArithmeticZero", testAdditiveArithmeticZero), 101 | ("testAdditiveArithmeticPlus", testAdditiveArithmeticPlus), 102 | ("testAdditiveArithmeticMinus", testAdditiveArithmeticMinus), 103 | ("testMove", testMove), 104 | ("testZeroTangentVector", testZeroTangentVector), 105 | ("testDifferentiableSubscript", testDifferentiableSubscript) 106 | ] 107 | } 108 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Core/MatrixNTests.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | import SwiftFusion 3 | import XCTest 4 | 5 | class MatrixNTests: XCTestCase { 6 | /// Test matrix-matrix multiplication. 7 | func testMatMul() { 8 | let matrix1 = Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1) 9 | 10 | XCTAssertEqual( 11 | matmul(matrix1, matrix1) as Matrix3, 12 | matrix1 as Matrix3 13 | ) 14 | 15 | let matrix4 = Matrix3(5, 1 ,3, 16 | 1, 1 , 1, 17 | 1, 2 , 1) 18 | let matrix5 = Vector3(1, 2, 3) 19 | 20 | XCTAssertEqual( 21 | matvec(matrix4, matrix5), 22 | Vector3(16, 6, 8) 23 | ) 24 | } 25 | 26 | /// Test matrix dimensionality 27 | func testMatrix3DimAsLinearSpace() { 28 | XCTAssertEqual(Matrix3.dimension, 9) 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Core/TensorFlowMatrixTests.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | import XCTest 3 | 4 | import TensorFlow 5 | import SwiftFusion 6 | 7 | class TensorFlowMatrixTests: XCTestCase { 8 | //-------------------------------------------------------------------------- 9 | // testConcat 10 | func testConcat() { 11 | let t1 = Tensor(shape: [2, 3], scalars: (1...6).map { Double($0) }) 12 | let t2 = Tensor(shape: [2, 3], scalars: (7...12).map { Double($0) }) 13 | let c1 = t1.concatenated(with: t2) 14 | let c1Expected = Tensor([ 15 | 1, 2, 3, 16 | 4, 5, 6, 17 | 7, 8, 9, 18 | 10, 11, 12, 19 | ]) 20 | 21 | XCTAssert(c1.flattened() == c1Expected) 22 | 23 | let c2 = t1.concatenated(with: t2, alongAxis: 1) 24 | let c2Expected = Tensor([ 25 | 1, 2, 3, 7, 8, 9, 26 | 4, 5, 6, 10, 11, 12 27 | ]) 28 | 29 | XCTAssert(c2.flattened() == c2Expected) 30 | } 31 | 32 | //-------------------------------------------------------------------------- 33 | // test_log 34 | func test_log() { 35 | let range = 0..<6 36 | let matrix = Tensor(shape: [3, 2], scalars: (range).map { Double($0) }) 37 | let values = log(matrix).flattened() 38 | let expected = Tensor(range.map { log(Double($0)) }) 39 | assertEqual(values, expected, accuracy: 1e-8) 40 | } 41 | 42 | //-------------------------------------------------------------------------- 43 | // test_neg 44 | func test_neg() { 45 | let range = 0..<6 46 | let matrix = Tensor(shape: [3, 2], scalars: (range).map { Double($0) }) 47 | let expected = Tensor(range.map { -Double($0) }) 48 | 49 | let values = (-matrix).flattened() 50 | assertEqual(values, expected, accuracy: 1e-8) 51 | } 52 | 53 | //-------------------------------------------------------------------------- 54 | // test_squared 55 | func test_squared() { 56 | let matrix = Tensor(shape: [3, 2], scalars: ([0, -1, 2, -3, 4, 5]).map { Double($0) }) 57 | let values = matrix.squared().flattened() 58 | let expected = Tensor((0...5).map { Double ($0 * $0) }) 59 | assertEqual(values, expected, accuracy: 1e-8) 60 | } 61 | 62 | //-------------------------------------------------------------------------- 63 | // test_multiplication 64 | func test_multiplication() { 65 | let matrix = Tensor(shape: [3, 2], scalars: ([0, -1, 2, -3, 4, 5]).map { Double($0) }) 66 | let values = (matrix * matrix).flattened() 67 | let expected = Tensor((0...5).map { Double($0 * $0) }) 68 | assertEqual(values, expected, accuracy: 1e-8) 69 | } 70 | } 71 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Core/TrappingDoubleTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import XCTest 17 | 18 | import PenguinStructures 19 | import SwiftFusion 20 | 21 | class TrappingDoubleTests: XCTestCase { 22 | /// Tests simple `TrappingDouble` operations. 23 | func testOperations() { 24 | let x: TrappingDouble = 1.0 25 | let y: TrappingDouble = 2.0 26 | XCTAssertEqual(y + x, 3.0) 27 | XCTAssertEqual(y - x, 1.0) 28 | } 29 | 30 | /// Tests that `TrappingDouble` traps on `NaN`s. 31 | /// 32 | /// Until https://github.com/saeta/penguin/issues/64 is addressed, we have no way to assert that a 33 | /// trap happens, so this test is skipped. You can comment the line back in and run this test to 34 | /// manually verify the trapping behavior. We don't use XCTSkipIf(true) because it generates 35 | /// diagnostics. 36 | func testTrap() { 37 | // let _: TrappingDouble = .infinity - .infinity 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Core/VectorNTests.swift.gyb: -------------------------------------------------------------------------------- 1 | import Foundation 2 | import TensorFlow 3 | import XCTest 4 | 5 | import SwiftFusion 6 | 7 | % import math 8 | % vectorSizes = range(1, 10) 9 | 10 | class VectorNTests: XCTestCase { 11 | % for N in vectorSizes: 12 | % if N <= 3: 13 | % coordinates = ['x', 'y', 'z'][0:N] 14 | % else: 15 | % coordinates = ['s%d' % i for i in range(N)] 16 | % end 17 | % values1 = range(1, N + 1) 18 | % values2 = range(N + 1, 2 * N + 1) 19 | 20 | /// Test that initializing a vector from coordinate values works. 21 | func testVector${N}Init() { 22 | let vector1 = Vector${N}(${', '.join([str(v) for v in values1])}) 23 | % for (index, coordinate) in enumerate(coordinates): 24 | XCTAssertEqual(vector1.${coordinate}, ${values1[index]}) 25 | % end 26 | } 27 | 28 | func testVector${N}VectorConformance() { 29 | let s = (0..<${N}).lazy.map { Double($0) } 30 | let v = Vector${N}(${', '.join([str(v) for v in range(N)])}) 31 | v.checkVectorSemantics( 32 | expectingScalars: s, 33 | writingScalars: (${N}..<${2 * N}).lazy.map { Double($0) }, 34 | maxSupportedScalarCount: ${N}) 35 | v.scalars.checkRandomAccessCollectionSemantics( 36 | expecting: s, 37 | maxSupportedCount: ${N}) 38 | } 39 | % end 40 | } 41 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Datasets/Data/malformed.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE2 0 0 0 2 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Datasets/Data/simple2d.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE2 0 0.1 0.2 0.3 2 | VERTEX_SE2 1 0.4 0.5 0.6 3 | EDGE_SE2 0 1 0.7 0.8 0.9 1.1 1.2 1.3 1.4 1.5 1.6 4 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Datasets/Data/simple3d.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE3:QUAT 0 18.7381 2.74428e-07 98.2287 0 0 0 1 2 | VERTEX_SE3:QUAT 1 19.0477 2.34636 98.2319 -0.139007 0.0806488 0.14657 0.976059 3 | EDGE_SE3:QUAT 0 1 0.309576 2.34636 0.00315914 -0.139007 0.0806488 0.14657 0.976059 1 9.62965e-19 9.62965e-19 5.88441e-08 -2.03096e-08 3.40337e-09 1 9.62965e-19 5.88441e-08 -2.03096e-08 3.40337e-09 1 5.88441e-08 -2.03096e-08 3.40337e-09 4108.72 -34.2982 884.091 3951.5 40.2084 4100.08 4 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Geometry/Cal3_S2Tests.swift: -------------------------------------------------------------------------------- 1 | 2 | import _Differentiation 3 | import SwiftFusion 4 | import TensorFlow 5 | 6 | import XCTest 7 | 8 | final class Cal3_S2Tests: XCTestCase { 9 | /// Tests default constructor. 10 | func testConstructorDefault() { 11 | let K1 = Cal3_S2() 12 | let K2 = Cal3_S2(fx: 1.0, fy: 1.0, s: 0.0, u0: 0.0, v0: 0.0) 13 | XCTAssertEqual(K1, K2) 14 | } 15 | 16 | /// Tests uncalibrate. 17 | func testCalibrateUncalibrate() { 18 | let K = Cal3_S2(fx: 200.0, fy: 200.0, s: 1.0, u0: 320.0, v0: 240.0) 19 | let np = Vector2(1.0, 2.0) 20 | 21 | let expected = Vector2(522.0, 640.0) // Manually calculated 22 | 23 | XCTAssertEqual(K.uncalibrate(np), expected) 24 | } 25 | 26 | /// Tests calibrate identity. 27 | func testCalibrateIdentity() { 28 | let K = Cal3_S2(fx: 200.0, fy: 200.0, s: 1.0, u0: 320.0, v0: 240.0) 29 | let np = Vector2(1.0, 2.0) 30 | 31 | XCTAssertEqual(K.calibrate(K.uncalibrate(np)), np) 32 | } 33 | 34 | /// Tests manifold. 35 | func testManifold() { 36 | var K1 = Cal3_S2(fx: 200.0, fy: 200.0, s: 1.0, u0: 320.0, v0: 240.0) 37 | let K2 = Cal3_S2(fx: 201.0, fy: 202.0, s: 4.0, u0: 324.0, v0: 245.0) 38 | let dK = Vector5(1.0, 2.0, 3.0, 4.0, 5.0) 39 | 40 | XCTAssertEqual(K1.retract(dK), K2) 41 | XCTAssertEqual(K1.localCoordinate(K2), dK) 42 | 43 | K1.move(along: dK) 44 | 45 | XCTAssertEqual(K1, K2) 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Image/ArrayImageTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import ModelSupport 17 | import SwiftFusion 18 | import TensorFlow 19 | import XCTest 20 | 21 | final class ArrayImageTests: XCTestCase { 22 | func testCreateBlank() { 23 | let rows = 10 24 | let cols = 10 25 | let channels = 3 26 | let image = ArrayImage(rows: rows, cols: cols, channels: channels) 27 | for i in 0..>, Vector3> 24 | 25 | class IdentityLinearizationFactorTests: XCTestCase { 26 | /// Test that `IdentityLinearizationFactor` forwards to the underlying factor's methods. 27 | func testForwardsMethods() { 28 | let base = TestGaussianFactor( 29 | jacobian: Matrix3([1, 2, 3, 4, 5, 6, 7, 8, 9]), 30 | error: Vector3(10, 20, 30), 31 | edges: Tuple1(TypedID(0))) 32 | let f = IdentityLinearizationFactor( 33 | linearizing: base, at: Tuple1(Vector3(0, 0, 0))) 34 | XCTAssertEqual(f.edges, base.edges) 35 | let x = Tuple1(Vector3(100, 200, 300)) 36 | let y = Vector3(100, 200, 300) 37 | XCTAssertEqual(f.error(at: x), base.error(at: x)) 38 | XCTAssertEqual(f.errorVector(at: x), base.errorVector(at: x)) 39 | XCTAssertEqual(f.errorVector_linearComponent(x), base.errorVector_linearComponent(x)) 40 | XCTAssertEqual( 41 | f.errorVector_linearComponent_adjoint(y), 42 | base.errorVector_linearComponent_adjoint(y)) 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Inference/PCAEncoderTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import Foundation 16 | import TensorFlow 17 | import XCTest 18 | 19 | import PenguinStructures 20 | @testable import SwiftFusion 21 | 22 | class PCAEncoderTests: XCTestCase { 23 | func testRandomMatrix() { 24 | let A = Tensor( 25 | [ 26 | [1, -1, 0], 27 | [0, 1, -1], 28 | [-1, 0, 1] 29 | ] 30 | ).reshaped(to: [3, 3, 1, 1]) 31 | 32 | let v = Tensor( 33 | [ 34 | [1], 35 | [0], 36 | [-1] 37 | ] 38 | ).reshaped(to: [3, 1, 1]) 39 | 40 | let pca = PCAEncoder(from: A, given: 2) 41 | let features = pca.encode(v) 42 | XCTAssertEqual(features.shape, [2]) 43 | 44 | assertEqual(pca.encode(v.expandingShape(at: 0)), features.expandingShape(at: 0), accuracy: 1e-8) 45 | assertEqual(pca.encode(Tensor(stacking: [v, v], alongAxis: 0)), Tensor(stacking: [features, features]), accuracy: 1e-8) 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Inference/RandomProjectionTests.swift: -------------------------------------------------------------------------------- 1 | import XCTest 2 | import PenguinStructures 3 | import SwiftFusion 4 | import TensorFlow 5 | 6 | class RandomProjectionTests: XCTestCase { 7 | func testEncode() { 8 | let image = Tensor(randomNormal: [20, 30, 1]) 9 | let d = 5 10 | let projector = RandomProjection(fromShape: image.shape, toFeatureSize: d) 11 | let features = projector.encode(image) 12 | 13 | XCTAssertEqual(features.rank, 1) 14 | XCTAssertEqual(features.shape[0], 5) 15 | } 16 | 17 | func testEncodeBatch() { 18 | let image = Tensor(randomNormal: [3, 20, 30, 1]) 19 | let d = 5 20 | let projector = RandomProjection(fromShape: [20, 30, 1], toFeatureSize: d) 21 | 22 | 23 | XCTAssertEqual(projector.B.shape, TensorShape([5, 20*30*1])) 24 | 25 | let features = projector.encode(image) 26 | 27 | XCTAssertEqual(features.rank, 2) 28 | XCTAssertEqual(features.shape[0], 3) 29 | XCTAssertEqual(features.shape[1], 5) 30 | } 31 | } -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Inference/VariableAssignmentsTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import Foundation 17 | import TensorFlow 18 | import XCTest 19 | 20 | import PenguinStructures 21 | import SwiftFusion 22 | 23 | class VariableAssignmentsTests: XCTestCase { 24 | func testStoreAndSubscript() { 25 | var x = VariableAssignments() 26 | 27 | let intID = x.store(1) 28 | XCTAssertEqual(x[intID], 1) 29 | 30 | x[intID] = 2 31 | XCTAssertEqual(x[intID], 2) 32 | 33 | let doubleID = x.store(2.0) 34 | XCTAssertEqual(x[doubleID], 2.0) 35 | 36 | let vectorID = x.store(Vector2(3, 4)) 37 | XCTAssertEqual(x[vectorID], Vector2(3, 4)) 38 | } 39 | 40 | func testTangentVectorZeros() { 41 | var x = VariableAssignments() 42 | _ = x.store(1) 43 | _ = x.store(Vector1(2)) 44 | _ = x.store(Vector2(3, 4)) 45 | 46 | let t = x.tangentVectorZeros 47 | // TODO: Assert that there are only 2 elements when there is some API for count. 48 | XCTAssertEqual(t[TypedID(0)], Vector1(0)) 49 | XCTAssertEqual(t[TypedID(0)], Vector2(0, 0)) 50 | } 51 | 52 | func testMove() { 53 | var x = VariableAssignments() 54 | _ = x.store(1) 55 | let v1ID = x.store(Vector1(2)) 56 | let v2ID = x.store(Vector2(3, 4)) 57 | 58 | var t = VariableAssignments() 59 | _ = t.store(Vector1(10)) 60 | _ = t.store(Vector2(20, 20)) 61 | 62 | x.move(along: t) 63 | XCTAssertEqual(x[v1ID], Vector1(12)) 64 | XCTAssertEqual(x[v2ID], Vector2(23, 24)) 65 | } 66 | 67 | func testSquaredNorm() { 68 | var x = VariableAssignments() 69 | _ = x.store(Vector1(2)) 70 | _ = x.store(Vector2(3, 4)) 71 | XCTAssertEqual(x.squaredNorm, 29) 72 | } 73 | 74 | func testScalarMultiply() { 75 | var x = VariableAssignments() 76 | let v1ID = x.store(Vector1(2)) 77 | let v2ID = x.store(Vector2(3, 4)) 78 | 79 | let r = 10 * x 80 | XCTAssertEqual(r[v1ID], Vector1(20)) 81 | XCTAssertEqual(r[v2ID], Vector2(30, 40)) 82 | } 83 | 84 | func testPlus() { 85 | var x = VariableAssignments() 86 | let v1ID = x.store(Vector1(2)) 87 | let v2ID = x.store(Vector2(3, 4)) 88 | 89 | var t = VariableAssignments() 90 | _ = t.store(Vector1(10)) 91 | _ = t.store(Vector2(20, 20)) 92 | 93 | let r = x + t 94 | XCTAssertEqual(r[v1ID], Vector1(12)) 95 | XCTAssertEqual(r[v2ID], Vector2(23, 24)) 96 | } 97 | } 98 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/MCMC/MCMCTests.swift: -------------------------------------------------------------------------------- 1 | // 2 | // MCMCTests.swift 3 | // SwiftFusionTests 4 | // 5 | // Created by Frank Dellaert on 6/23/20. 6 | // 7 | 8 | import _Differentiation 9 | import TensorFlow 10 | import SwiftFusion 11 | 12 | import XCTest 13 | 14 | class MCMCTests: XCTestCase { 15 | 16 | /// Sampling from the Standard Normal Distribution. 17 | /// Inspired by testRWM1DUniform from tfp 18 | func testRWM1DUniform() throws { 19 | let deterministicEntropy = ARC4RandomNumberGenerator(seed: 42) 20 | 21 | // Create kernel for MCMC sampler: 22 | // target_log_prob_fn is proportional to log p(x), where p is a zero-mean Gaussian 23 | // new_state_fn is a symmetric (required for Metropolis) proposal density, 24 | // in this case perturbing x with uniformly sampled perturbations. 25 | let kernel = RandomWalkMetropolis( 26 | sourceOfEntropy: deterministicEntropy, 27 | target_log_prob_fn: {(x: Double) in -0.5*x*x}, // tfd.Normal(loc=dtype(0), scale=dtype(1)) 28 | new_state_fn: {(x: Double, r: inout AnyRandomNumberGenerator) in x + Double.random(in: -1..<1, using: &r)} 29 | ) 30 | 31 | // Run the sampler for 2500 steps, discarding the first 500 asa burn-in 32 | let num_results = 2000 33 | let initial_state: Double = 1.0 34 | let num_burnin_steps = 500 35 | let samples = sampleChain(num_results, initial_state, kernel, num_burnin_steps) 36 | 37 | // Assert samples have the right type and size 38 | _ = samples as [Double] 39 | XCTAssertEqual(samples.count, num_results) 40 | 41 | // Check the mean and standard deviation, which should be 0 and 1, respectively 42 | let tensor = Tensor(samples) 43 | let sample_mean = tensor.mean().scalarized() 44 | let sample_std = tensor.standardDeviation().scalarized() 45 | XCTAssertEqual(0, sample_mean, accuracy: 0.17) 46 | XCTAssertEqual(1, sample_std, accuracy: 0.2) 47 | } 48 | 49 | } 50 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Optimizers/CGLSTests.swift: -------------------------------------------------------------------------------- 1 | // This file tests for the CGLS optimizer 2 | 3 | import _Differentiation 4 | import SwiftFusion 5 | import PenguinStructures 6 | import TensorFlow 7 | import XCTest 8 | 9 | final class CGLSTests: XCTestCase { 10 | /// test convergence for a simple gaussian factor graph 11 | func testCGLSSolver() { 12 | let gfg = SimpleGaussianFactorGraph.create() 13 | 14 | var optimizer = GenericCGLS(precision: 1e-7, max_iteration: 10) 15 | var x: VariableAssignments = SimpleGaussianFactorGraph.zeroDelta() 16 | optimizer.optimize(gfg: gfg, initial: &x) 17 | 18 | let expected = SimpleGaussianFactorGraph.correctDelta() 19 | 20 | for k in [SimpleGaussianFactorGraph.x1ID, SimpleGaussianFactorGraph.x2ID] { 21 | assertEqual(x[k].flatTensor, expected[k].flatTensor, accuracy: 1e-6) 22 | } 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Optimizers/GradientDescentTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2020 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | import _Differentiation 16 | import SwiftFusion 17 | import XCTest 18 | 19 | final class GradientDescentTests: XCTestCase { 20 | /// Test convergence for a simple Pose2SLAM graph. 21 | func testPose2SLAM() { 22 | var x = VariableAssignments() 23 | let pose1ID = x.store(Pose2(Rot2(0.2), Vector2(0.5, 0.0))) 24 | let pose2ID = x.store(Pose2(Rot2(-0.2), Vector2(2.3, 0.1))) 25 | let pose3ID = x.store(Pose2(Rot2(.pi / 2), Vector2(4.1, 0.1))) 26 | let pose4ID = x.store(Pose2(Rot2(.pi), Vector2(4.0, 2.0))) 27 | let pose5ID = x.store(Pose2(Rot2(-.pi / 2), Vector2(2.1, 2.1))) 28 | 29 | var graph = FactorGraph() 30 | graph.store(BetweenFactor(pose2ID, pose1ID, Pose2(2.0, 0.0, .pi / 2))) 31 | graph.store(BetweenFactor(pose3ID, pose2ID, Pose2(2.0, 0.0, .pi / 2))) 32 | graph.store(BetweenFactor(pose4ID, pose3ID, Pose2(2.0, 0.0, .pi / 2))) 33 | graph.store(BetweenFactor(pose5ID, pose4ID, Pose2(2.0, 0.0, .pi / 2))) 34 | graph.store(PriorFactor(pose1ID, Pose2(0, 0, 0))) 35 | 36 | let optimizer = GradientDescent(learningRate: 1e-2) 37 | for _ in 0..<10000 { 38 | optimizer.update(&x, objective: graph) 39 | } 40 | 41 | // Test condition: pose 5 should be identical to pose 1 (close loop). 42 | XCTAssertEqual(between(x[pose1ID], x[pose5ID]).t.norm, 0.0, accuracy: 1e-2) 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Optimizers/LMTests.swift: -------------------------------------------------------------------------------- 1 | // This file tests for the CGLS optimizer 2 | 3 | import _Differentiation 4 | import SwiftFusion 5 | import TensorFlow 6 | import XCTest 7 | 8 | final class LMTests: XCTestCase { 9 | /// test convergence for a simple gaussian factor graph 10 | func testBasicLMConvergence() { 11 | var x = VariableAssignments() 12 | let pose1ID = x.store(Pose2(Rot2(0.2), Vector2(0.5, 0.0))) 13 | let pose2ID = x.store(Pose2(Rot2(-0.2), Vector2(2.3, 0.1))) 14 | let pose3ID = x.store(Pose2(Rot2(.pi / 2), Vector2(4.1, 0.1))) 15 | let pose4ID = x.store(Pose2(Rot2(.pi), Vector2(4.0, 2.0))) 16 | let pose5ID = x.store(Pose2(Rot2(-.pi / 2), Vector2(2.1, 2.1))) 17 | 18 | var graph = FactorGraph() 19 | graph.store(BetweenFactor(pose2ID, pose1ID, Pose2(2.0, 0.0, .pi / 2))) 20 | graph.store(BetweenFactor(pose3ID, pose2ID, Pose2(2.0, 0.0, .pi / 2))) 21 | graph.store(BetweenFactor(pose4ID, pose3ID, Pose2(2.0, 0.0, .pi / 2))) 22 | graph.store(BetweenFactor(pose5ID, pose4ID, Pose2(2.0, 0.0, .pi / 2))) 23 | graph.store(PriorFactor(pose1ID, Pose2(0, 0, 0))) 24 | 25 | var optimizer = LM(precision: 1e-3, max_iteration: 10) 26 | optimizer.verbosity = .SILENT 27 | 28 | try? optimizer.optimize(graph: graph, initial: &x) 29 | 30 | // Test condition: pose 5 should be identical to pose 1 (close loop). 31 | XCTAssertEqual(between(x[pose1ID], x[pose5ID]).t.norm, 0.0, accuracy: 1e-2) 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Probability/MultivariateGaussianTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2019 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /// This file tests proper Multivariate Gaussian behavior 16 | 17 | import SwiftFusion 18 | import TensorFlow 19 | import XCTest 20 | 21 | final class MultivariateGaussianTests: XCTestCase { 22 | typealias T = Tensor 23 | 24 | /// Compare with the NumPy implementation 25 | func testSimpleCovMatrix() { 26 | let data = T([[1.5, 0.9], [0.5, 1.9]]) 27 | 28 | assertEqual(cov(data), T([[0.5, -0.5], [-0.5, 0.5]]), accuracy: 1e-8) 29 | 30 | let data_zerovar = T([[1.0, 2.0, 3.0], [1.0, 2.0, 3.1], [0.9, 2.0, 2.9]]) 31 | assertEqual(cov(data_zerovar), T( 32 | [[0.00333333, 0 , 0.005 ], 33 | [0 , 0 , 0 ], 34 | [0.005 , 0 , 0.01 ]]), accuracy: 1e-8 35 | ) 36 | } 37 | 38 | /// Test negative log likelihood 39 | func testMultivariateNegativeLogLikelihood() { 40 | let data = T([[1.5, 0.9], [0.5, 1.9], [1.0, 1.5]]) 41 | 42 | let model = MultivariateGaussian(from: data) 43 | 44 | XCTAssertEqual( 45 | model.negativeLogLikelihood(T([1.0, 1.5])), 46 | 1.33333333 / 2.0, 47 | accuracy: 1e-5 48 | ) 49 | } 50 | 51 | /// Test negative log likelihood and probability for simple case 52 | func testMultivariateProbability() { 53 | let model = MultivariateGaussian(mean: T(zeros:[2]), information: eye(rowCount: 2)) 54 | let v = T([1, 2]) 55 | let expectedE : Double = (1+4)/2 56 | XCTAssertEqual(model.negativeLogLikelihood(v), expectedE, accuracy: 1e-5) 57 | XCTAssertEqual(model.probability(v), exp(-expectedE)/sqrt(4 * .pi * .pi), accuracy: 1e-5) 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/Probability/NaiveBayesTests.swift: -------------------------------------------------------------------------------- 1 | // Copyright 2019 The SwiftFusion Authors. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /// This file tests proper Naive Bayes behavior 16 | 17 | import SwiftFusion 18 | import TensorFlow 19 | import XCTest 20 | 21 | final class NaiveBayesTests: XCTestCase { 22 | func testGaussianNBFittingParams() { 23 | let data = Tensor(randomNormal: [5000, 10, 10], mean: Tensor(4.888), standardDeviation: Tensor(2.999)) 24 | let gnb = GaussianNB(from: data) 25 | 26 | let sigma = Tensor.init(ones: [10, 10]) * 2.999 27 | let mu = Tensor.init(ones: [10, 10]) * 4.888 28 | 29 | assertEqual(gnb.sigmas, sigma, accuracy: 3e-1) 30 | assertEqual(gnb.mu, mu, accuracy: 3e-1) 31 | } 32 | 33 | func testGaussianNB() { 34 | let data = Tensor([[0.9, 1.1], [1.1, 0.9]]) 35 | let gnb = GaussianNB(from: data) 36 | 37 | let sigma = data.standardDeviation().scalar! 38 | let mu = data.mean().scalar! 39 | let p: Tensor = [0.4, 1.0] 40 | let gaussianOfP: Tensor = exp(-0.5 * pow(p - mu, 2)/(sigma * sigma)) 41 | 42 | XCTAssertEqual( 43 | gnb.negativeLogLikelihood(p), 44 | -log(gaussianOfP).sum().scalarized(), accuracy: 1e-6 45 | ) 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /Tests/SwiftFusionTests/TestUtilities.swift: -------------------------------------------------------------------------------- 1 | import _Differentiation 2 | import PenguinStructures 3 | import TensorFlow 4 | import SwiftFusion 5 | import XCTest 6 | 7 | /// Asserts that `x` and `y` have the same shape and that their values have absolute difference 8 | /// less than `accuracy`. 9 | func assertEqual( 10 | _ x: Tensor, _ y: Tensor, accuracy: T, file: StaticString = #filePath, line: UInt = #line 11 | ) { 12 | guard x.shape == y.shape else { 13 | XCTFail( 14 | "shape mismatch: \(x.shape) is not equal to \(y.shape)", 15 | file: (file), 16 | line: line 17 | ) 18 | return 19 | } 20 | XCTAssert( 21 | abs(x - y).max().scalarized() < accuracy, 22 | "value mismatch:\n\(x)\nis not equal to\n\(y)\nwith accuracy \(accuracy)", 23 | file: (file), 24 | line: line 25 | ) 26 | } 27 | 28 | /// Asserts that `x` and `y` have absolute difference less than `accuracy`. 29 | func assertAllKeyPathEqual( 30 | _ x: T, _ y: T, accuracy: Double, file: StaticString = #filePath, line: UInt = #line 31 | ) { 32 | let result: [Bool] = x.recursivelyAllKeyPaths(to: Double.self).map { 33 | if !(abs(x[keyPath: $0] - y[keyPath: $0]) < accuracy) { 34 | return false 35 | } else { 36 | return true 37 | } 38 | } 39 | if !result.allSatisfy({ x in x == true }) { 40 | XCTAssert( 41 | false, 42 | "value mismatch:\n\(x)\nis not equal to\n\(y)\nwith accuracy \(accuracy)", 43 | file: (file), 44 | line: line 45 | ) 46 | } 47 | } 48 | 49 | /// Factor graph with 2 2D factors on 3 2D variables. 50 | public enum SimpleGaussianFactorGraph { 51 | public static let x1ID = TypedID(2) 52 | public static let x2ID = TypedID(0) 53 | public static let l1ID = TypedID(1) 54 | 55 | public static func create() -> GaussianFactorGraph { 56 | let I_2x2 = Matrix2.identity 57 | var fg = GaussianFactorGraph(zeroValues: zeroDelta()) 58 | fg.store(JacobianFactor2x2_1( 59 | jacobian: 10 * I_2x2, 60 | error: -1 * Vector2(1, 1), 61 | edges: Tuple1(x1ID))) 62 | fg.store(JacobianFactor2x2_2( 63 | jacobians: 10 * I_2x2, -10 * I_2x2, 64 | error: Vector2(2, -1), 65 | edges: Tuple2(x2ID, x1ID))) 66 | fg.store(JacobianFactor2x2_2( 67 | jacobians: 5 * I_2x2, -5 * I_2x2, 68 | error: Vector2(0, 1), 69 | edges: Tuple2(l1ID, x1ID))) 70 | fg.store(JacobianFactor2x2_2( 71 | jacobians: -5 * I_2x2, 5 * I_2x2, 72 | error: Vector2(-1, 1.5), 73 | edges: Tuple2(x2ID, l1ID))) 74 | return fg 75 | } 76 | 77 | public static func correctDelta() -> VariableAssignments { 78 | var x = VariableAssignments() 79 | let actualX2ID = x.store(Vector2(0.1, -0.2)) 80 | assert(actualX2ID == x2ID) 81 | let actualL1ID = x.store(Vector2(-0.1, 0.1)) 82 | assert(actualL1ID == l1ID) 83 | let actualX1ID = x.store(Vector2(-0.1, -0.1)) 84 | assert(actualX1ID == x1ID) 85 | return x 86 | } 87 | 88 | public static func zeroDelta() -> VariableAssignments { 89 | var x = VariableAssignments() 90 | let actualX2ID = x.store(Vector2.zero) 91 | assert(actualX2ID == x2ID) 92 | let actualL1ID = x.store(Vector2.zero) 93 | assert(actualL1ID == l1ID) 94 | let actualX1ID = x.store(Vector2.zero) 95 | assert(actualX1ID == x1ID) 96 | return x 97 | } 98 | } 99 | 100 | extension URL { 101 | /// Creates a URL for the directory containing the caller's source file. 102 | static func sourceFileDirectory(file: String = #filePath) -> URL { 103 | return URL(fileURLWithPath: file).deletingLastPathComponent() 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /Util/BeeAnnotations/README.md: -------------------------------------------------------------------------------- 1 | # How to annotate a video 2 | 3 | ## Step 1: Save frames as jpegs. 4 | 5 | You should save the frames as jpeg, numbered consecutively `frame0.jpeg`, 6 | `frame1.jpeg`, etc. 7 | 8 | `youtube-dl` is useful for downloading videos from YouTube. 9 | 10 | Here's a command that turns a video files into frame jpegs. 11 | ```bash 12 | ffmpeg -ss 163 -t 4 -i bee_video_1.mkv -start_number 0 bee_video_1/frame%d.jpeg 13 | ``` 14 | 15 | `-ss` specifies the start time (seconds) of the interval that frames are taken 16 | from. `-t` specifies the duration (seconds) of the interval that frames are 17 | taken from. 18 | 19 | ## Step 2: Run the annotation tool. 20 | 21 | Update `frameDir` in `app.js` to point at the directory containing the frames. 22 | Update `frameWidth` and `frameHeight` to be the width and height of the frames 23 | in pixels. 24 | 25 | Open `annotator.html` in a browser. Usage: 26 | * Click on the frame to move the bounding box. 27 | * W/A/S/D keys also move the bounding box. 28 | * Q/E keys rotate the bounding box. 29 | * The text fields adjust the x, y, theta, width, and height of the bounding box. 30 | * Left/Right arrow keys change the frame. 31 | 32 | When you are done, save the contents of the text area to a file named 33 | `track0.txt` (or `track1.txt`, `track2.txt`) in the same directory as the 34 | frames. 35 | 36 | Now you can load the frames and the tracks from Swift using: 37 | 38 | ```swift 39 | BeeVideo(directory: URL(fileURLWithPath: "")) 40 | ``` 41 | -------------------------------------------------------------------------------- /Util/BeeAnnotations/annotator.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 |
7 |
8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 |
16 |
Frame {{ frameIndex }}
17 | 18 | 19 | 20 | 21 | 22 | 23 |
24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /Util/BeeAnnotations/app.js: -------------------------------------------------------------------------------- 1 | var app = new Vue({ 2 | el: '#app', 3 | data: { 4 | frameDir: 'bee_video_1', 5 | frameWidth: 1920, 6 | frameHeight: 1080, 7 | 8 | frameScale: 0.5, 9 | 10 | frameIndex: 0, 11 | bbX: 0, 12 | bbY: 0, 13 | bbTheta: 0, 14 | bbWidth: 100, 15 | bbHeight: 100, 16 | 17 | bbs: [] 18 | }, 19 | computed: { 20 | frameUrl: function () { 21 | return this.frameDir + '/frame' + this.frameIndex + '.jpeg'; 22 | }, 23 | svgWidth: function () { 24 | return this.frameScale * this.frameWidth; 25 | }, 26 | svgHeight: function () { 27 | return this.frameScale * this.frameHeight; 28 | }, 29 | svgViewBox: function () { 30 | return "0 0 " + this.frameWidth + " " + this.frameHeight; 31 | }, 32 | bbTransform: function () { 33 | let rot = 180 * this.bbTheta / Math.PI; 34 | let trans = "" + this.bbX + " " + this.bbY; 35 | let centerX = -this.bbWidth / 2; 36 | let centerY = -this.bbHeight / 2; 37 | return "translate(" + trans + ") " + "rotate(" + rot + ") translate(" + centerX + " " + centerY + ")"; 38 | }, 39 | halfBBWidth: function() { 40 | return this.bbWidth / 2; 41 | }, 42 | halfBBHeight: function() { 43 | return this.bbHeight / 2; 44 | }, 45 | track: function() { 46 | var t = ""; 47 | t += this.bbHeight + " " + this.bbWidth + "\n"; 48 | for (var i = 0; i < this.bbs.length; i++) { 49 | let bb = this.bbs[i]; 50 | t += i + " " + bb.x + " " + bb.y + " " + bb.theta + "\n"; 51 | }; 52 | return t; 53 | } 54 | }, 55 | mounted: function () { 56 | document.addEventListener("keyup", this.keyPress); 57 | }, 58 | methods: { 59 | updateBB: function() { 60 | this.bbs.splice(this.frameIndex, 1, { 61 | x: this.bbX, 62 | y: this.bbY, 63 | theta: this.bbTheta 64 | }); 65 | }, 66 | keyPress: function () { 67 | if (event.key == "ArrowLeft") { 68 | this.frameIndex -= 1; 69 | } else if (event.key == "ArrowRight") { 70 | this.frameIndex += 1; 71 | } else if (event.key == "a") { 72 | this.bbX -= 1; 73 | } else if (event.key == "d") { 74 | this.bbX += 1; 75 | } else if (event.key == "w") { 76 | this.bbY -= 1; 77 | } else if (event.key == "s") { 78 | this.bbY += 1; 79 | } else if (event.key == "q") { 80 | this.bbTheta -= 0.01; 81 | } else if (event.key == "e") { 82 | this.bbTheta += 0.01; 83 | } 84 | this.updateBB(); 85 | }, 86 | clickFrame: function() { 87 | this.bbX = event.x / this.frameScale; 88 | this.bbY = event.y / this.frameScale; 89 | this.updateBB(); 90 | } 91 | } 92 | }); 93 | -------------------------------------------------------------------------------- /doc/ImageOperations.md: -------------------------------------------------------------------------------- 1 | # Image Operations 2 | 3 | ## Coordinate systems 4 | 5 | We have three coordinate systems for images: 6 | 7 | - `(i, j)` are *integer* coordinates referring to pixels, and correspond to row 8 | and column in a matrix. The origin is `(i=0, j=0)` in the top-left. 9 | - `(u, v)` are *float* coordinates, `u` is horizontal (associated with `j`) and 10 | `v` is vertical (associated with `i`). 11 | - `(x, y)` are *float* coordinates, like `(u, v)`, but are "calibrated": `(x=0.0, 12 | y=0.0)` is the optical axis. 13 | 14 | Pixel `(i, j)` is the square bounded by the corners with coordinates 15 | `(u=float(j), v=float(i))` and `(u=float(j)+1.0, v=float(i)+1.0)`. 16 | 17 | For example, pixel `(3, 1)` the square bounded by the corners with coordinates 18 | `(u=1.0, v=3.0)` and `(u=2.0, v=4.0)`. 19 | 20 | Image dimensions are measured in two ways: 21 | 22 | - `(rows, cols)` are *integer* dimensions corresponding to the `(i, j)` coordinates. 23 | - `(width, height)` are *float* dimensions cooresponding to the `(u, v)` and `(x, 24 | y)` coordinates, with `(width=float(cols), height=float(rows))`. 25 | 26 | For example, `(rows=3, cols=5)` corresponds to `(width=5, height=3)`. In this 27 | image, the bottom-right pixel is `(i=2, j=4)`, corresponding to the square 28 | bounded by `(u=4.0, v=2.0)` and `(u=5.0, v=3.0)`. 29 | 30 | -------------------------------------------------------------------------------- /doc/TensorBoardUsage.md: -------------------------------------------------------------------------------- 1 | How to use the TensorBoard logging system 2 | ================ 3 | 4 | Currently the TensorBoard metric logging infra is only implemented for Pose3SLAMG2O demo application. 5 | 6 | ## Usage 7 | 8 | To run an experiment, do the following: 9 | ```bash 10 | mkdir -p logs/lm_sphere # Your experiment name 11 | swift build -c release -Xswiftc -cross-module-optimization 12 | swift run -c release -Xswiftc -cross-module-optimization Pose3SLAMG2O ./.txt -l ./logs/ 13 | ``` 14 | At the same time, in a separate terminal: 15 | ``` 16 | # in a separate terminal 17 | tensorboard dev upload --logdir logs \ 18 | --name "My first SwiftFusion in tensorboard.dev" \ 19 | --description "One small step for myself, a giant step for human kind." 20 | ``` 21 | There should be a logging prompt, log in to your google account. 22 | 23 | Now there should be a prompt at the second terminal that says: 24 | ``` 25 | View your TensorBoard live at: https://tensorboard.dev/experiment// 26 | ``` 27 | 28 | Navigate to the URL on your web browser, and you should see the metric. 29 | -------------------------------------------------------------------------------- /generate.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Generates code. 4 | # 5 | # Instructions: 6 | # 1. Clone https://github.com/apple/swift to any directory. 7 | # 2. Let `SWIFT_BASE` be the directory where you cloned it. 8 | # 3. Run `./generate $SWIFT_BASE`. 9 | set -e 10 | 11 | if [[ $(uname) = 'Darwin' ]]; then 12 | SED="gsed" 13 | else 14 | SED="sed" 15 | fi 16 | 17 | SWIFT_BASE="$1" 18 | if [ -z "$SWIFT_BASE" ] 19 | then 20 | echo "SWIFT_BASE not specified. See instructions at the top of generate.sh." 21 | exit 1 22 | fi 23 | 24 | GYB="$SWIFT_BASE"/utils/gyb 25 | 26 | GENERATED_FILES=( 27 | "Sources/SwiftFusion/Core/VectorN.swift" 28 | "Sources/SwiftFusion/Inference/FactorBoilerplate.swift" 29 | "Tests/SwiftFusionTests/Core/VectorNTests.swift" 30 | ) 31 | 32 | for GENERATED_FILE in "${GENERATED_FILES[@]}" 33 | do 34 | cat > "$GENERATED_FILE" << EOF 35 | // WARNING: This is a generated file. Do not edit it. Instead, edit the corresponding ".gyb" file. 36 | // See "generate.sh" in the root of this repository for instructions how to regenerate files. 37 | 38 | EOF 39 | 40 | "$GYB" --line-directive '' "$GENERATED_FILE.gyb" >> "$GENERATED_FILE" 41 | $SED -i'' "s|###sourceLocation(file: \"$(pwd)/|###sourceLocation(file: \"|" "$GENERATED_FILE" 42 | done 43 | --------------------------------------------------------------------------------