Merge pull request #16 from pmokeev/migrate_to_library

Migrate to library
MobileRoboticsSkoltech · Sep 1, 2021 · 7eb5eed · 7eb5eed
2 parents 6040a26 + 8ec8aeb
commit 7eb5eed
Show file tree

Hide file tree

Showing 19 changed files with 609 additions and 297 deletions.
diff --git a/.github/workflows/main-ubuntuAndMacOS.yml b/.github/workflows/main-ubuntuAndMacOS.yml
@@ -38,7 +38,7 @@ jobs:
         if: matrix.os == 'macos-latest'
         uses: reactivecircus/android-emulator-runner@v2
         with:
-          api-level: 21
+          api-level: 23
           target: default
           arch: x86
           ndk: 22.1.7171670

diff --git a/.gitignore b/.gitignore
@@ -92,6 +92,5 @@ lint/tmp/
 
 *.ipynb
 
-sync-server/__pycache__/
-
-*.csv
+# Scapix unnecessary files
+app/src/main/cpp/generated/
diff --git a/app/build.gradle b/app/build.gradle
@@ -1,16 +1,25 @@
 apply plugin: 'com.android.application'
 
 android {
-    compileSdkVersion 29
-    buildToolsVersion "29.0.2"
+    compileSdkVersion 30
+    buildToolsVersion '30.0.3'
     ndkVersion "22.1.7171670"
     defaultConfig {
         applicationId "skoltech.mobilerobotics.capturesync"
         minSdkVersion 23
-        targetSdkVersion 29
+        targetSdkVersion 30
         versionCode 1
         versionName "1.0"
         testInstrumentationRunner "androidx.test.runner.AndroidJUnitRunner"
+        externalNativeBuild {
+            cmake {
+                cppFlags '-O2'
+                arguments "-DANDROID_ARM_NEON=TRUE"
+            }
+            ndkBuild {
+                abiFilters 'x86', 'x86_64', 'armeabi-v7a', 'arm64-v8a'
+            }
+        }
     }
     buildTypes {
         release {
@@ -22,9 +31,28 @@ android {
         sourceCompatibility = 1.8
         targetCompatibility = 1.8
     }
+    externalNativeBuild {
+        cmake {
+            path file('src/main/cpp/CMakeLists.txt')
+            version '3.18.1'
+        }
+    }
+    buildFeatures {
+        viewBinding true
+    }
     lintOptions {
         disable 'LongLogTag'
     }
+    sourceSets {
+        main {
+            java {
+                srcDirs = [
+                        "src/main/cpp/generated/bridge/java",
+                        "src/main/java"
+                ]
+            }
+        }
+    }
 }
 
 tasks.withType(Test) {
@@ -37,8 +65,8 @@ tasks.withType(Test) {
 
 dependencies {
     implementation fileTree(dir: 'libs', include: ['*.jar'])
-    implementation 'androidx.appcompat:appcompat:1.0.2'
-    testImplementation 'junit:junit:4.12'
-    androidTestImplementation 'androidx.test.ext:junit:1.1.0'
-    androidTestImplementation 'androidx.test.espresso:espresso-core:3.1.1'
+    implementation 'androidx.appcompat:appcompat:1.3.1'
+    testImplementation 'junit:junit:4.13.2'
+    androidTestImplementation 'androidx.test.ext:junit:1.1.3'
+    androidTestImplementation 'androidx.test.espresso:espresso-core:3.4.0'
 }
diff --git a/app/src/main/cpp/CMakeLists.txt b/app/src/main/cpp/CMakeLists.txt
@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.14)
+
+project(twist-n-sync)
+
+include(FetchContent)
+
+FetchContent_Declare(
+        cmodule
+        URL "https://github.com/scapix-com/cmodule/archive/v1.0.29.tar.gz"
+        URL_HASH SHA256=b49019b355423aebacd927e99541b146c900ef416ae1da6a8343a2a274dd4876
+)
+
+FetchContent_MakeAvailable(cmodule)
+
+set(SCAPIX_BRIDGE "java" CACHE STRING "cpp, java, objc, python, js, cs")
+set(SCAPIX_JAVA_API "android-28" CACHE STRING "one of the folders inside 'scapix/java_api': jdk-11.0.2, android-28, etc.")
+
+set(SOURCE_LIST twist-n-sync-cpp/TimeSync.cpp twist-n-sync-cpp/util/CubicSpline.cpp twist-n-sync-cpp/util/TSUtil.cpp)
+add_library(bridge SHARED ${SOURCE_LIST})
+
+find_package(Scapix REQUIRED)
+scapix_bridge_headers(bridge "com.googleresearch.capturesync.softwaresync" twist-n-sync-cpp/TimeSync.h)
+
+FetchContent_Declare(
+        eigen
+        GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
+        GIT_TAG origin/master
+)
+
+FetchContent_Declare(
+        spline
+        GIT_REPOSITORY https://github.com/ttk592/spline.git
+        GIT_TAG origin/master
+)
+
+FetchContent_GetProperties(eigen)
+if(NOT eigen_POPULATED)
+    FetchContent_Populate(eigen)
+    set(eigen_EIGEN_BUILD_DOC OFF)
+    set(eigen_BUILD_TESTS OFF)
+    include_directories(bridge PUBLIC ${eigen_SOURCE_DIR})
+endif()
+
+FetchContent_GetProperties(spline)
+if(NOT spline_POPULATED)
+    FetchContent_Populate(spline)
+    include_directories(bridge PUBLIC ${spline_SOURCE_DIR}/src)
+endif()
+
+set(GENERATED_DIR generated/bridge/java)
+
+include_directories(
+        ${GENERATED_DIR}
+        twist-n-sync-cpp
+)
diff --git a/app/src/main/cpp/twist-n-sync-cpp/LICENSE_TWISTNSYNCMODULE b/app/src/main/cpp/twist-n-sync-cpp/LICENSE_TWISTNSYNCMODULE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2021 https://github.com/achains 
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/app/src/main/cpp/twist-n-sync-cpp/TimeSync.cpp b/app/src/main/cpp/twist-n-sync-cpp/TimeSync.cpp
@@ -0,0 +1,134 @@
+#include "TimeSync.h"
+#include "util/TSUtil.h"
+#include "util/CubicSpline.h"
+
+#include "Eigen/Dense"
+
+#include <numeric>
+#include <memory>
+
+
+TimeSync::TimeSync(std::vector<std::vector<double>> const & gyro_first,
+                   std::vector<std::vector<double>> const & gyro_second,
+                   std::vector<double> const & ts_first,
+                   std::vector<double> const & ts_second,
+                   bool const & do_resample):
+                        gyro_first_(tsutil::vectorToEigMatrixX3d(gyro_first)),
+                        gyro_second_(tsutil::vectorToEigMatrixX3d(gyro_second)),
+                        ts_first_(tsutil::vectorToEigVectorXd(ts_first)),
+                        ts_second_(tsutil::vectorToEigVectorXd(ts_second)),
+                        do_resample_(do_resample) {}
+
+
+tsutil::CorrData TimeSync::getInitialIndex() const {
+
+    Eigen::VectorXd norm_first = tsutil::getNormOfRows(gyro_first_);
+    Eigen::VectorXd norm_second = tsutil::getNormOfRows(gyro_second_);
+
+    Eigen::VectorXd cross_cor = tsutil::eigenCrossCor(norm_second, norm_first);
+
+    return {cross_cor, std::distance(cross_cor.begin(), std::max_element(cross_cor.begin(), cross_cor.end()))};
+}
+
+Eigen::MatrixX3d & TimeSync::interpolateGyro(Eigen::VectorXd const & ts_old, Eigen::MatrixX3d const & gyro_old,
+                                           Eigen::VectorXd const & ts_new, Eigen::MatrixX3d & gyro_new) {
+    assert (gyro_old.rows() == gyro_new.rows());
+
+    gyro_new << tsutil::interpolate(ts_old, gyro_old(Eigen::all, 0), ts_new),
+                tsutil::interpolate(ts_old, gyro_old(Eigen::all, 1), ts_new),
+                tsutil::interpolate(ts_old, gyro_old(Eigen::all, 2), ts_new);
+    return gyro_new;
+}
+
+void TimeSync::resample(double const & accuracy){
+    double time_first_mean = tsutil::adjDiffEigen(ts_first_).mean();
+    double time_second_mean = tsutil::adjDiffEigen(ts_second_).mean();
+
+    dt_ = std::min({accuracy, time_first_mean, time_second_mean});
+
+    if (do_resample_){
+        Eigen::VectorXd ts_first_new = tsutil::arangeEigen(ts_first_[0], *ts_first_.end() + dt_, dt_);
+        Eigen::VectorXd ts_second_new = tsutil::arangeEigen(ts_second_[0], *ts_second_.end() + dt_, dt_);
+
+        TimeSync::interpolateGyro(ts_first_, gyro_first_, ts_first_new, gyro_first_);
+        TimeSync::interpolateGyro(ts_second_, gyro_second_, ts_second_new, gyro_second_);
+    }
+}
+
+Eigen::Vector2d TimeSync::obtainRoots(Eigen::VectorXd const & coeffs, Eigen::Index const & order){
+    Eigen::VectorXd equation(order);
+    for (auto i = 0; i < order; ++i){
+        equation[i] = static_cast<double>(order - i) * coeffs[i];
+    }
+    return tsutil::quadraticRoots(equation.reverse());
+}
+
+void TimeSync::obtainDelay(){
+
+    // Correction of index numbering
+    Eigen::Index shift = -gyro_first_.rows() + 1;
+
+    // Cross-cor estimation
+    tsutil::CorrData corr_data = TimeSync::getInitialIndex();
+    corr_data.initial_index += shift;
+
+    Eigen::MatrixX3d tmp_xx1;
+    Eigen::MatrixX3d tmp_xx2;
+
+    if (corr_data.initial_index > 0){
+        tmp_xx1 = gyro_first_(Eigen::seq(0, Eigen::last - corr_data.initial_index), Eigen::all).eval();
+        tmp_xx2 = gyro_second_(Eigen::seq(corr_data.initial_index, Eigen::last), Eigen::all).eval();
+    }
+    else if (corr_data.initial_index < 0){
+        tmp_xx1 = gyro_first_(Eigen::seq(-corr_data.initial_index, Eigen::last), Eigen::all).eval();
+        tmp_xx2 = gyro_second_(Eigen::seq(0, corr_data.initial_index), Eigen::all).eval();
+    }
+    else{
+        tmp_xx1 = gyro_first_;
+        tmp_xx2 = gyro_second_;
+    }
+
+    Eigen::Index size = std::min(tmp_xx1.rows(), tmp_xx2.rows());
+    tmp_xx1 = tmp_xx1(Eigen::seq(0, size - 1), Eigen::all).eval();
+    tmp_xx2 = tmp_xx2(Eigen::seq(0, size - 1), Eigen::all).eval();
+
+    // Calibration
+    Eigen::Matrix3d M = (tmp_xx2.transpose() * tmp_xx1) * (tmp_xx1.transpose() * tmp_xx1).inverse();
+
+    gyro_first_ = (M * gyro_first_.transpose()).transpose().eval();
+
+    // Cross-correlation re-estimation
+    corr_data = TimeSync::getInitialIndex();
+
+    // Cross-cor, based cubic spline coefficients
+    CubicSpline cubic_spline(tsutil::arangeEigen(0., static_cast<double>(corr_data.cross_cor.size())),
+                             corr_data.cross_cor);
+
+    Eigen::Matrix4Xd spline_coefficients = cubic_spline.getCoefficients();
+
+    Eigen::VectorXd coeffs = spline_coefficients.col(corr_data.initial_index);
+
+    // Check cubic spline derivative sign and redefine initial_index if needed
+    if (coeffs(Eigen::last - 1) < 0) {
+        corr_data.initial_index -= 1;
+        coeffs = spline_coefficients(Eigen::all, corr_data.initial_index);
+    }
+
+    // Solve quadratic equation to obtain roots
+    Eigen::Index order = coeffs.size() - 1;
+    Eigen::Vector2d roots = TimeSync::obtainRoots(coeffs, order);
+
+    auto result = *std::max_element(roots.begin(), roots.end());
+    std::vector<double> check_solution(order);
+    for (int i = 0; i < order; ++i)
+        check_solution[i] = static_cast<double>(order - i) * coeffs[i] *
+                std::pow((roots[0] + roots[1]) / 2, (order - i - 1));
+    if (std::accumulate(check_solution.begin(), check_solution.end(), 0.0) < 0.0)
+        result = *std::min_element(roots.begin(), roots.end());
+
+    time_delay_ = (static_cast<double>(corr_data.initial_index + shift) + result) * dt_;
+}
+
+double TimeSync::getTimeDelay() const {
+    return time_delay_;
+}
diff --git a/app/src/main/cpp/twist-n-sync-cpp/TimeSync.h b/app/src/main/cpp/twist-n-sync-cpp/TimeSync.h
@@ -0,0 +1,48 @@
+#ifndef TWIST_N_SYNC_CPP_MODULE_TIMESYNC_H
+#define TWIST_N_SYNC_CPP_MODULE_TIMESYNC_H
+
+#include <scapix/bridge/object.h>
+#include "Eigen/Core"
+#include "util/TSUtil.h"
+#include <vector>
+
+class TimeSync : public scapix::bridge::object<TimeSync> {
+ public:
+    explicit TimeSync(std::vector<std::vector<double>> const & gyro_first,
+                      std::vector<std::vector<double>> const & gyro_second,
+                      std::vector<double> const & ts_first,
+                      std::vector<double> const & ts_second,
+                      bool const & do_resample = true);
+
+    void obtainDelay();
+
+    void resample(double const & accuracy);
+
+    double getTimeDelay() const;
+
+ private:
+
+    static Eigen::MatrixX3d & interpolateGyro(Eigen::VectorXd const & ts_old, Eigen::MatrixX3d const & gyro_old,
+                                            Eigen::VectorXd const & ts_new, Eigen::MatrixX3d & gyro_new);
+
+    static Eigen::Vector2d obtainRoots(Eigen::VectorXd const & coeffs, Eigen::Index const & order);
+
+    tsutil::CorrData getInitialIndex() const;
+
+    // 3D angular velocities from devices' gyros
+    Eigen::MatrixX3d gyro_first_;
+    Eigen::MatrixX3d gyro_second_;
+
+    // Gyros' timestamps
+    Eigen::VectorXd ts_first_;
+    Eigen::VectorXd ts_second_;
+
+    double dt_ = 0.0;
+
+    // Flag to do resampling of angular velocities
+    bool do_resample_;
+    double time_delay_ = 0.0;
+};
+
+
+#endif //TWIST_N_SYNC_CPP_MODULE_TIMESYNC_H