From 9d41178070a883f1fdc2f9b55449d845e3e3a274 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 19 Oct 2020 07:52:12 +0200 Subject: [PATCH 001/169] Use METIS system library if so selected --- CMakeLists.txt | 5 +++- cmake/HandleMetis.cmake | 44 ++++++++++++++++++++++++++++ cmake/HandlePrintConfiguration.cmake | 1 + gtsam/3rdparty/CMakeLists.txt | 5 +--- gtsam/CMakeLists.txt | 15 ++-------- 5 files changed, 53 insertions(+), 17 deletions(-) create mode 100644 cmake/HandleMetis.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 35c487fd3..fa9e7d507 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,11 +38,14 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() +include(cmake/HandleGeneralOptions.cmake) # CMake build options + +# Libraries: include(cmake/HandleBoost.cmake) # Boost include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 -include(cmake/HandleGeneralOptions.cmake) # CMake build options +include(cmake/HandleMetis.cmake) # metis include(cmake/HandleMKL.cmake) # MKL include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools diff --git a/cmake/HandleMetis.cmake b/cmake/HandleMetis.cmake new file mode 100644 index 000000000..9c29e5776 --- /dev/null +++ b/cmake/HandleMetis.cmake @@ -0,0 +1,44 @@ +############################################################################### +# Metis library + +# For both system or bundle version, a cmake target "metis-gtsam-if" is defined (interface library) + +# Dont try to use metis if GTSAM_SUPPORT_NESTED_DISSECTION is disabled: +if (NOT GTSAM_SUPPORT_NESTED_DISSECTION) + return() +endif() + +option(GTSAM_USE_SYSTEM_METIS "Find and use system-installed libmetis. If 'off', use the one bundled with GTSAM" OFF) + +if(GTSAM_USE_SYSTEM_METIS) + # Debian package: libmetis-dev + + find_path(METIS_INCLUDE_DIR metis.h REQUIRED) + find_library(METIS_LIBRARY metis REQUIRED) + + if(METIS_INCLUDE_DIR AND METIS_LIBRARY) + mark_as_advanced(METIS_INCLUDE_DIR) + mark_as_advanced(METIS_LIBRARY) + + add_library(metis-gtsam-if INTERFACE) + target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR}) + target_link_libraries(metis-gtsam-if INTERFACE ${METIS_LIBRARY}) + endif() +else() + # Bundled version: + option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) + add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis) + + target_include_directories(metis-gtsam BEFORE PUBLIC + $ + $ + $ + $ + ) + + add_library(metis-gtsam-if INTERFACE) + target_link_libraries(metis-gtsam-if INTERFACE metis-gtsam) +endif() + +list(APPEND GTSAM_EXPORTED_TARGETS metis-gtsam-if) +install(TARGETS metis-gtsam-if EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index 4ffd00e54..ad6ac5c5c 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -32,6 +32,7 @@ endif() print_build_options_for_target(gtsam) print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") +print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}") if(GTSAM_USE_TBB) print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})") diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 8b356393b..a1e917bbe 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -49,10 +49,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() -option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -if(GTSAM_SUPPORT_NESTED_DISSECTION) - add_subdirectory(metis) -endif() +# metis: already handled in ROOT/cmake/HandleMetis.cmake add_subdirectory(ceres) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 37c4a1f88..91253b932 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -88,7 +88,8 @@ list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/d install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) if(GTSAM_SUPPORT_NESTED_DISSECTION) - list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam) + # target metis-gtsam-if is defined in both cases: embedded metis or system version: + list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam-if) endif() # Versions @@ -142,7 +143,7 @@ target_include_directories(gtsam BEFORE PUBLIC $ ) # 3rdparty libraries: use the "system" flag so they are included via "-isystem" -# and warnings (and warnings-considered-errors) in those headers are not +# and warnings (and warnings-considered-errors) in those headers are not # reported as warnings/errors in our targets: target_include_directories(gtsam SYSTEM BEFORE PUBLIC # SuiteSparse_config @@ -154,16 +155,6 @@ target_include_directories(gtsam SYSTEM BEFORE PUBLIC $ $ ) -if(GTSAM_SUPPORT_NESTED_DISSECTION) - target_include_directories(gtsam BEFORE PUBLIC - $ - $ - $ - $ - ) -endif() - - if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library if (NOT BUILD_SHARED_LIBS) From f6a432961a845dd89fbff337a8da8ad0c441162e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Aug 2021 05:25:56 -0700 Subject: [PATCH 002/169] first pass at IMUKittiExampleGPS.py --- python/gtsam/examples/IMUKittiExampleGPS.py | 227 ++++++++++++++++++++ 1 file changed, 227 insertions(+) create mode 100644 python/gtsam/examples/IMUKittiExampleGPS.py diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py new file mode 100644 index 000000000..33e032614 --- /dev/null +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -0,0 +1,227 @@ +""" +Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE +""" +import argparse +from typing import List + +import gtsam +import numpy as np +from gtsam import Pose3, Rot3, noiseModel +from gtsam.symbol_shorthand import B, V, X + + +class KittiCalibration: + def __init__(self, bodyTimu: gtsam.Pose3): + self.bodyTimu = bodyTimu + + +class ImuMeasurement: + def __init__(self, time, dt, accelerometer, gyroscope): + pass + + +class GpsMeasurement: + def __init__(self, time, position: gtsam.Point3): + self.time = time + self.position = position + + +def lodKittiData(): + pass + + +def parse_args(): + parser = argparse.ArgumentParser() + return parser.parse_args() + + +def getImuParams(kitti_calibration): + GRAVITY = 9.8 + w_coriolis = np.zeros(3) + + # Set IMU preintegration parameters + measured_acc_cov = np.eye(3) * np.power( + kitti_calibration.accelerometer_sigma, 2) + measured_omega_cov = np.eye(3) * np.powwe( + kitti_calibration.gyroscope_sigma, 2) + # error committed in integrating position from velocities + integration_error_cov = np.eye(3) * np.power( + kitti_calibration.integration_sigma, 2) + + imu_params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) + imu_params.accelerometerCovariance = measured_acc_cov # acc white noise in continuous + imu_params.integrationCovariance = integration_error_cov # integration uncertainty continuous + imu_params.gyroscopeCovariance = measured_omega_cov # gyro white noise in continuous + imu_params.omegaCoriolis = w_coriolis + + return imu_params + + +def main(): + args = parse_args() + kitti_calibration, imu_measurements, gps_measurements = lodKittiData() + + if kitti_calibration.bodyTimu != gtsam.Pose3: + raise ValueError( + "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" + ) + + # Configure different variables + first_gps_pose = 1 + gps_skip = 10 + + # Configure noise models + noise_model_gps = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0, 1.0 / 0.07, 1.0 / 0.07, 1.0 / 0.07])) + + # Set initial conditions for the estimated trajectory + # initial pose is the reference frame (navigation frame) + current_pose_global = Pose3(Rot3(), + gps_measurements[first_gps_pose].position) + # the vehicle is stationary at the beginning at position 0,0,0 + current_velocity_global = np.zeros(3) + current_bias = gtsam.imuBias.ConstantBias() # init with zero bias + + sigma_init_x = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0, 1, 1, 1])) + sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) + sigma_init_b = noiseModel.Diagonal.Sigmas( + np.asarray([0.1, 0.1, 0.1, 5.00e-05, 5.00e-05, 5.00e-05])) + + imu_params = getImuParams() + + # Set ISAM2 parameters and create ISAM2 solver object + isam_params = gtsam.ISAM2Params() + isam_params.setFactorization("CHOLESKY") + isam_params.setRelinearizeSkip(10) + + isam = gtsam.ISAM2(isam_params) + + # Create the factor graph and values object that will store new factors and + # values to add to the incremental graph + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values( + ) # values storing the initial estimates of new nodes in the factor graph + + # Main loop: + # (1) we read the measurements + # (2) we create the corresponding factors in the graph + # (3) we solve the graph to obtain and optimal estimate of robot trajectory + print( + "-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n" + ) + j = 0 + for i in range(first_gps_pose, len(gps_measurements) - 1): + # At each non=IMU measurement we initialize a new node in the graph + current_pose_key = X(i) + current_vel_key = V(i) + current_bias_key = B(i) + t = gps_measurements[i].time + + if i == first_gps_pose: + # Create initial estimate and prior on initial pose, velocity, and biases + new_values.insert(current_pose_key, current_pose_global) + new_values.insert(current_vel_key, current_velocity_global) + new_values.insert(current_bias_key, current_bias) + + new_factors.addPriorPose3(current_pose_key, current_pose_global, + sigma_init_x) + new_factors.addPriorVector(current_vel_key, + current_velocity_global, sigma_init_v) + new_factors.addPriorConstantBias(current_bias_key, current_bias, + sigma_init_b) + else: + t_previous = gps_measurements[i - 1].time + + # Summarize IMU data between the previous GPS measurement and now + current_summarized_measurement = gtsam.PreintegratedImuMeasurements( + imu_params, current_bias) + + included_imu_measurement_count = 0 + while (j < imu_measurements.size() + and imu_measurements[j].time <= t): + if imu_measurements[j].time >= t_previous: + current_summarized_measurement.integrateMeasurement( + imu_measurements[j].accelerometer, + imu_measurements[j].gyroscope, imu_measurements[j].dt) + included_imu_measurement_count += 1 + j += 1 + + # Create IMU factor + previous_pose_key = X(i - 1) + previous_vel_key = V(i - 1) + previous_bias_key = B(i - 1) + + new_factors.push_back( + gtsam.ImuFactor > (previous_pose_key, previous_vel_key, + current_pose_key, current_vel_key, + previous_bias_key, + current_summarized_measurement)) + + # Bias evolution as given in the IMU metadata + sigma_between_b = noiseModel.Diagonal.Sigmas( + np.asarray([ + np.sqrt(included_imu_measurement_count) * + kitti_calibration.accelerometer_bias_sigma + ] * 3 + [ + np.sqrt(included_imu_measurement_count) * + kitti_calibration.gyroscope_bias_sigma + ] * 3)) + + new_factors.push_back( + gtsam.BetweenFactorConstantBias(previous_bias_key, + current_bias_key, + gtsam.imuBias.ConstantBias(), + sigma_between_b)) + + # Create GPS factor + gps_pose = Pose3(current_pose_global.rotation(), + gps_measurements[i].position) + if (i % gps_skip) == 0: + new_factors.addPriorPose3(current_pose_key, gps_pose, + noise_model_gps) + new_values.insert(current_pose_key, gps_pose) + + print( + "################ POSE INCLUDED AT TIME %lf ################\n", + t) + print(gps_pose.translation(), "\n") + else: + new_values.insert(current_pose_key, current_pose_global) + + # Add initial values for velocity and bias based on the previous + # estimates + new_values.insert(current_vel_key, current_velocity_global) + new_values.insert(current_bias_key, current_bias) + + # Update solver + # ======================================================================= + # We accumulate 2*GPSskip GPS measurements before updating the solver at + # first so that the heading becomes observable. + if i > (first_gps_pose + 2 * gps_skip): + print( + "################ NEW FACTORS AT TIME %lf ################\n", + t) + new_factors.print() + + isam.update(new_factors, new_values) + + # Reset the newFactors and newValues list + new_factors.resize(0) + new_values.clear() + + # Extract the result/current estimates + result = isam.calculateEstimate() + + current_pose_global = result.atPose3(current_pose_key) + current_velocity_global = result.atVector(current_vel_key) + current_bias = result.atConstantBias(current_bias_key) + + print("\n################ POSE AT TIME %lf ################\n", + t) + current_pose_global.print() + print("\n\n") + + +if __name__ == "__main__": + main() From 5dff04648867eeae2a8067f475e714d1f644d0e4 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 14 Aug 2021 01:14:23 -0400 Subject: [PATCH 003/169] Start wrapping the verbosity options for GNC --- gtsam/nonlinear/nonlinear.i | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 73eaef125..c3f17c02e 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -527,8 +527,13 @@ template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); + void setVerbosityGNC(const gtsam::GncParams::Verbosity value); void print(const string& str) const; }; + +typedef gtsam::GncParams::Verbosity::SILENT GncVerbositySilent; +typedef gtsam::GncParams::Verbosity::SUMMARY GncVerbositySummary; +typedef gtsam::GncParams::Verbosity::VALUES GncVerbosityValues; typedef gtsam::GncParams GncGaussNewtonParams; typedef gtsam::GncParams GncLMParams; From a4a58cf8030a7a0a72c0ce1d83fdddc3e0392e47 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 13:33:57 -0400 Subject: [PATCH 004/169] only format c++ file (no code changes) --- examples/IMUKittiExampleGPS.cpp | 571 +++++++++++++++++--------------- 1 file changed, 297 insertions(+), 274 deletions(-) diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index e2ca49647..a2c82575f 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -11,21 +11,23 @@ /** * @file IMUKittiExampleGPS - * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE - * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics + * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI + * VISION BENCHMARK SUITE + * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ + * Electronics */ // GTSAM related includes. +#include #include #include #include -#include -#include -#include #include #include #include -#include +#include +#include +#include #include #include @@ -34,35 +36,35 @@ using namespace std; using namespace gtsam; -using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) -using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) struct KittiCalibration { - double body_ptx; - double body_pty; - double body_ptz; - double body_prx; - double body_pry; - double body_prz; - double accelerometer_sigma; - double gyroscope_sigma; - double integration_sigma; - double accelerometer_bias_sigma; - double gyroscope_bias_sigma; - double average_delta_t; + double body_ptx; + double body_pty; + double body_ptz; + double body_prx; + double body_pry; + double body_prz; + double accelerometer_sigma; + double gyroscope_sigma; + double integration_sigma; + double accelerometer_bias_sigma; + double gyroscope_bias_sigma; + double average_delta_t; }; struct ImuMeasurement { - double time; - double dt; - Vector3 accelerometer; - Vector3 gyroscope; // omega + double time; + double dt; + Vector3 accelerometer; + Vector3 gyroscope; // omega }; struct GpsMeasurement { - double time; - Vector3 position; // x,y,z + double time; + Vector3 position; // x,y,z }; const string output_filename = "IMUKittiExampleGPSResults.csv"; @@ -70,290 +72,311 @@ const string output_filename = "IMUKittiExampleGPSResults.csv"; void loadKittiData(KittiCalibration& kitti_calibration, vector& imu_measurements, vector& gps_measurements) { - string line; + string line; - // Read IMU metadata and compute relative sensor pose transforms - // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma - // AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT - string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); - ifstream imu_metadata(imu_metadata_file.c_str()); + // Read IMU metadata and compute relative sensor pose transforms + // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma + // GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma + // AverageDeltaT + string imu_metadata_file = + findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); + ifstream imu_metadata(imu_metadata_file.c_str()); - printf("-- Reading sensor metadata\n"); + printf("-- Reading sensor metadata\n"); - getline(imu_metadata, line, '\n'); // ignore the first line + getline(imu_metadata, line, '\n'); // ignore the first line - // Load Kitti calibration - getline(imu_metadata, line, '\n'); - sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", - &kitti_calibration.body_ptx, - &kitti_calibration.body_pty, - &kitti_calibration.body_ptz, - &kitti_calibration.body_prx, - &kitti_calibration.body_pry, - &kitti_calibration.body_prz, - &kitti_calibration.accelerometer_sigma, - &kitti_calibration.gyroscope_sigma, - &kitti_calibration.integration_sigma, - &kitti_calibration.accelerometer_bias_sigma, - &kitti_calibration.gyroscope_bias_sigma, - &kitti_calibration.average_delta_t); - printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", - kitti_calibration.body_ptx, - kitti_calibration.body_pty, - kitti_calibration.body_ptz, - kitti_calibration.body_prx, - kitti_calibration.body_pry, - kitti_calibration.body_prz, - kitti_calibration.accelerometer_sigma, - kitti_calibration.gyroscope_sigma, - kitti_calibration.integration_sigma, - kitti_calibration.accelerometer_bias_sigma, - kitti_calibration.gyroscope_bias_sigma, - kitti_calibration.average_delta_t); + // Load Kitti calibration + getline(imu_metadata, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + &kitti_calibration.body_ptx, &kitti_calibration.body_pty, + &kitti_calibration.body_ptz, &kitti_calibration.body_prx, + &kitti_calibration.body_pry, &kitti_calibration.body_prz, + &kitti_calibration.accelerometer_sigma, + &kitti_calibration.gyroscope_sigma, + &kitti_calibration.integration_sigma, + &kitti_calibration.accelerometer_bias_sigma, + &kitti_calibration.gyroscope_bias_sigma, + &kitti_calibration.average_delta_t); + printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", + kitti_calibration.body_ptx, kitti_calibration.body_pty, + kitti_calibration.body_ptz, kitti_calibration.body_prx, + kitti_calibration.body_pry, kitti_calibration.body_prz, + kitti_calibration.accelerometer_sigma, + kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma, + kitti_calibration.accelerometer_bias_sigma, + kitti_calibration.gyroscope_bias_sigma, + kitti_calibration.average_delta_t); - // Read IMU data - // Time dt accelX accelY accelZ omegaX omegaY omegaZ - string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); - printf("-- Reading IMU measurements from file\n"); - { - ifstream imu_data(imu_data_file.c_str()); - getline(imu_data, line, '\n'); // ignore the first line + // Read IMU data + // Time dt accelX accelY accelZ omegaX omegaY omegaZ + string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); + printf("-- Reading IMU measurements from file\n"); + { + ifstream imu_data(imu_data_file.c_str()); + getline(imu_data, line, '\n'); // ignore the first line - double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0; - while (!imu_data.eof()) { - getline(imu_data, line, '\n'); - sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", - &time, &dt, - &acc_x, &acc_y, &acc_z, - &gyro_x, &gyro_y, &gyro_z); + double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, + gyro_y = 0, gyro_z = 0; + while (!imu_data.eof()) { + getline(imu_data, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt, + &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z); - ImuMeasurement measurement; - measurement.time = time; - measurement.dt = dt; - measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); - measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); - imu_measurements.push_back(measurement); - } + ImuMeasurement measurement; + measurement.time = time; + measurement.dt = dt; + measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); + measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); + imu_measurements.push_back(measurement); } + } - // Read GPS data - // Time,X,Y,Z - string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); - printf("-- Reading GPS measurements from file\n"); - { - ifstream gps_data(gps_data_file.c_str()); - getline(gps_data, line, '\n'); // ignore the first line + // Read GPS data + // Time,X,Y,Z + string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); + printf("-- Reading GPS measurements from file\n"); + { + ifstream gps_data(gps_data_file.c_str()); + getline(gps_data, line, '\n'); // ignore the first line - double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; - while (!gps_data.eof()) { - getline(gps_data, line, '\n'); - sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); + double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; + while (!gps_data.eof()) { + getline(gps_data, line, '\n'); + sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); - GpsMeasurement measurement; - measurement.time = time; - measurement.position = Vector3(gps_x, gps_y, gps_z); - gps_measurements.push_back(measurement); - } + GpsMeasurement measurement; + measurement.time = time; + measurement.position = Vector3(gps_x, gps_y, gps_z); + gps_measurements.push_back(measurement); } + } } int main(int argc, char* argv[]) { - KittiCalibration kitti_calibration; - vector imu_measurements; - vector gps_measurements; - loadKittiData(kitti_calibration, imu_measurements, gps_measurements); + KittiCalibration kitti_calibration; + vector imu_measurements; + vector gps_measurements; + loadKittiData(kitti_calibration, imu_measurements, gps_measurements); - Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz, - kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz) - .finished(); - auto body_T_imu = Pose3::Expmap(BodyP); - if (!body_T_imu.equals(Pose3(), 1e-5)) { - printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same"); - exit(-1); - } + Vector6 BodyP = + (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, + kitti_calibration.body_ptz, kitti_calibration.body_prx, + kitti_calibration.body_pry, kitti_calibration.body_prz) + .finished(); + auto body_T_imu = Pose3::Expmap(BodyP); + if (!body_T_imu.equals(Pose3(), 1e-5)) { + printf( + "Currently only support IMUinBody is identity, i.e. IMU and body frame " + "are the same"); + exit(-1); + } - // Configure different variables - // double t_offset = gps_measurements[0].time; - size_t first_gps_pose = 1; - size_t gps_skip = 10; // Skip this many GPS measurements each time - double g = 9.8; - auto w_coriolis = Vector3::Zero(); // zero vector + // Configure different variables + // double t_offset = gps_measurements[0].time; + size_t first_gps_pose = 1; + size_t gps_skip = 10; // Skip this many GPS measurements each time + double g = 9.8; + auto w_coriolis = Vector3::Zero(); // zero vector - // Configure noise models - auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), - Vector3::Constant(1.0/0.07)) - .finished()); + // Configure noise models + auto noise_model_gps = noiseModel::Diagonal::Precisions( + (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07)) + .finished()); - // Set initial conditions for the estimated trajectory - // initial pose is the reference frame (navigation frame) - auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position); - // the vehicle is stationary at the beginning at position 0,0,0 - Vector3 current_velocity_global = Vector3::Zero(); - auto current_bias = imuBias::ConstantBias(); // init with zero bias + // Set initial conditions for the estimated trajectory + // initial pose is the reference frame (navigation frame) + auto current_pose_global = + Pose3(Rot3(), gps_measurements[first_gps_pose].position); + // the vehicle is stationary at the beginning at position 0,0,0 + Vector3 current_velocity_global = Vector3::Zero(); + auto current_bias = imuBias::ConstantBias(); // init with zero bias - auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), - Vector3::Constant(1.0)) - .finished()); - auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); - auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100), - Vector3::Constant(5.00e-05)) - .finished()); + auto sigma_init_x = noiseModel::Diagonal::Precisions( + (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished()); + auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); + auto sigma_init_b = noiseModel::Diagonal::Sigmas( + (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05)) + .finished()); - // Set IMU preintegration parameters - Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); - Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); - // error committed in integrating position from velocities - Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2); + // Set IMU preintegration parameters + Matrix33 measured_acc_cov = + I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); + Matrix33 measured_omega_cov = + I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); + // error committed in integrating position from velocities + Matrix33 integration_error_cov = + I_3x3 * pow(kitti_calibration.integration_sigma, 2); - auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); - imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous - imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous - imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous - imu_params->omegaCoriolis = w_coriolis; + auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); + imu_params->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + imu_params->integrationCovariance = + integration_error_cov; // integration uncertainty continuous + imu_params->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous + imu_params->omegaCoriolis = w_coriolis; - std::shared_ptr current_summarized_measurement = nullptr; + std::shared_ptr current_summarized_measurement = + nullptr; - // Set ISAM2 parameters and create ISAM2 solver object - ISAM2Params isam_params; - isam_params.factorization = ISAM2Params::CHOLESKY; - isam_params.relinearizeSkip = 10; + // Set ISAM2 parameters and create ISAM2 solver object + ISAM2Params isam_params; + isam_params.factorization = ISAM2Params::CHOLESKY; + isam_params.relinearizeSkip = 10; - ISAM2 isam(isam_params); + ISAM2 isam(isam_params); - // Create the factor graph and values object that will store new factors and values to add to the incremental graph - NonlinearFactorGraph new_factors; - Values new_values; // values storing the initial estimates of new nodes in the factor graph + // Create the factor graph and values object that will store new factors and + // values to add to the incremental graph + NonlinearFactorGraph new_factors; + Values new_values; // values storing the initial estimates of new nodes in + // the factor graph - /// Main loop: - /// (1) we read the measurements - /// (2) we create the corresponding factors in the graph - /// (3) we solve the graph to obtain and optimal estimate of robot trajectory - printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n"); - size_t j = 0; - for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { - // At each non=IMU measurement we initialize a new node in the graph - auto current_pose_key = X(i); - auto current_vel_key = V(i); - auto current_bias_key = B(i); - double t = gps_measurements[i].time; + /// Main loop: + /// (1) we read the measurements + /// (2) we create the corresponding factors in the graph + /// (3) we solve the graph to obtain and optimal estimate of robot trajectory + printf( + "-- Starting main loop: inference is performed at each time step, but we " + "plot trajectory every 10 steps\n"); + size_t j = 0; + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + // At each non=IMU measurement we initialize a new node in the graph + auto current_pose_key = X(i); + auto current_vel_key = V(i); + auto current_bias_key = B(i); + double t = gps_measurements[i].time; - if (i == first_gps_pose) { - // Create initial estimate and prior on initial pose, velocity, and biases - new_values.insert(current_pose_key, current_pose_global); - new_values.insert(current_vel_key, current_velocity_global); - new_values.insert(current_bias_key, current_bias); - new_factors.emplace_shared>(current_pose_key, current_pose_global, sigma_init_x); - new_factors.emplace_shared>(current_vel_key, current_velocity_global, sigma_init_v); - new_factors.emplace_shared>(current_bias_key, current_bias, sigma_init_b); - } else { - double t_previous = gps_measurements[i-1].time; + if (i == first_gps_pose) { + // Create initial estimate and prior on initial pose, velocity, and biases + new_values.insert(current_pose_key, current_pose_global); + new_values.insert(current_vel_key, current_velocity_global); + new_values.insert(current_bias_key, current_bias); + new_factors.emplace_shared>( + current_pose_key, current_pose_global, sigma_init_x); + new_factors.emplace_shared>( + current_vel_key, current_velocity_global, sigma_init_v); + new_factors.emplace_shared>( + current_bias_key, current_bias, sigma_init_b); + } else { + double t_previous = gps_measurements[i - 1].time; - // Summarize IMU data between the previous GPS measurement and now - current_summarized_measurement = std::make_shared(imu_params, current_bias); - static size_t included_imu_measurement_count = 0; - while (j < imu_measurements.size() && imu_measurements[j].time <= t) { - if (imu_measurements[j].time >= t_previous) { - current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer, - imu_measurements[j].gyroscope, - imu_measurements[j].dt); - included_imu_measurement_count++; - } - j++; - } - - // Create IMU factor - auto previous_pose_key = X(i-1); - auto previous_vel_key = V(i-1); - auto previous_bias_key = B(i-1); - - new_factors.emplace_shared(previous_pose_key, previous_vel_key, - current_pose_key, current_vel_key, - previous_bias_key, *current_summarized_measurement); - - // Bias evolution as given in the IMU metadata - auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() << - Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma), - Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma)) - .finished()); - new_factors.emplace_shared>(previous_bias_key, - current_bias_key, - imuBias::ConstantBias(), - sigma_between_b); - - // Create GPS factor - auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position); - if ((i % gps_skip) == 0) { - new_factors.emplace_shared>(current_pose_key, gps_pose, noise_model_gps); - new_values.insert(current_pose_key, gps_pose); - - printf("################ POSE INCLUDED AT TIME %lf ################\n", t); - cout << gps_pose.translation(); - printf("\n\n"); - } else { - new_values.insert(current_pose_key, current_pose_global); - } - - // Add initial values for velocity and bias based on the previous estimates - new_values.insert(current_vel_key, current_velocity_global); - new_values.insert(current_bias_key, current_bias); - - // Update solver - // ======================================================================= - // We accumulate 2*GPSskip GPS measurements before updating the solver at - // first so that the heading becomes observable. - if (i > (first_gps_pose + 2*gps_skip)) { - printf("################ NEW FACTORS AT TIME %lf ################\n", t); - new_factors.print(); - - isam.update(new_factors, new_values); - - // Reset the newFactors and newValues list - new_factors.resize(0); - new_values.clear(); - - // Extract the result/current estimates - Values result = isam.calculateEstimate(); - - current_pose_global = result.at(current_pose_key); - current_velocity_global = result.at(current_vel_key); - current_bias = result.at(current_bias_key); - - printf("\n################ POSE AT TIME %lf ################\n", t); - current_pose_global.print(); - printf("\n\n"); - } + // Summarize IMU data between the previous GPS measurement and now + current_summarized_measurement = + std::make_shared(imu_params, + current_bias); + static size_t included_imu_measurement_count = 0; + while (j < imu_measurements.size() && imu_measurements[j].time <= t) { + if (imu_measurements[j].time >= t_previous) { + current_summarized_measurement->integrateMeasurement( + imu_measurements[j].accelerometer, imu_measurements[j].gyroscope, + imu_measurements[j].dt); + included_imu_measurement_count++; } + j++; + } + + // Create IMU factor + auto previous_pose_key = X(i - 1); + auto previous_vel_key = V(i - 1); + auto previous_bias_key = B(i - 1); + + new_factors.emplace_shared( + previous_pose_key, previous_vel_key, current_pose_key, + current_vel_key, previous_bias_key, *current_summarized_measurement); + + // Bias evolution as given in the IMU metadata + auto sigma_between_b = noiseModel::Diagonal::Sigmas( + (Vector6() << Vector3::Constant( + sqrt(included_imu_measurement_count) * + kitti_calibration.accelerometer_bias_sigma), + Vector3::Constant(sqrt(included_imu_measurement_count) * + kitti_calibration.gyroscope_bias_sigma)) + .finished()); + new_factors.emplace_shared>( + previous_bias_key, current_bias_key, imuBias::ConstantBias(), + sigma_between_b); + + // Create GPS factor + auto gps_pose = + Pose3(current_pose_global.rotation(), gps_measurements[i].position); + if ((i % gps_skip) == 0) { + new_factors.emplace_shared>( + current_pose_key, gps_pose, noise_model_gps); + new_values.insert(current_pose_key, gps_pose); + + printf("################ POSE INCLUDED AT TIME %lf ################\n", + t); + cout << gps_pose.translation(); + printf("\n\n"); + } else { + new_values.insert(current_pose_key, current_pose_global); + } + + // Add initial values for velocity and bias based on the previous + // estimates + new_values.insert(current_vel_key, current_velocity_global); + new_values.insert(current_bias_key, current_bias); + + // Update solver + // ======================================================================= + // We accumulate 2*GPSskip GPS measurements before updating the solver at + // first so that the heading becomes observable. + if (i > (first_gps_pose + 2 * gps_skip)) { + printf("################ NEW FACTORS AT TIME %lf ################\n", + t); + new_factors.print(); + + isam.update(new_factors, new_values); + + // Reset the newFactors and newValues list + new_factors.resize(0); + new_values.clear(); + + // Extract the result/current estimates + Values result = isam.calculateEstimate(); + + current_pose_global = result.at(current_pose_key); + current_velocity_global = result.at(current_vel_key); + current_bias = result.at(current_bias_key); + + printf("\n################ POSE AT TIME %lf ################\n", t); + current_pose_global.print(); + printf("\n\n"); + } } + } - // Save results to file - printf("\nWriting results to file...\n"); - FILE* fp_out = fopen(output_filename.c_str(), "w+"); - fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n"); + // Save results to file + printf("\nWriting results to file...\n"); + FILE* fp_out = fopen(output_filename.c_str(), "w+"); + fprintf(fp_out, + "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n"); - Values result = isam.calculateEstimate(); - for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { - auto pose_key = X(i); - auto vel_key = V(i); - auto bias_key = B(i); + Values result = isam.calculateEstimate(); + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + auto pose_key = X(i); + auto vel_key = V(i); + auto bias_key = B(i); - auto pose = result.at(pose_key); - auto velocity = result.at(vel_key); - auto bias = result.at(bias_key); + auto pose = result.at(pose_key); + auto velocity = result.at(vel_key); + auto bias = result.at(bias_key); - auto pose_quat = pose.rotation().toQuaternion(); - auto gps = gps_measurements[i].position; + auto pose_quat = pose.rotation().toQuaternion(); + auto gps = gps_measurements[i].position; - cout << "State at #" << i << endl; - cout << "Pose:" << endl << pose << endl; - cout << "Velocity:" << endl << velocity << endl; - cout << "Bias:" << endl << bias << endl; + cout << "State at #" << i << endl; + cout << "Pose:" << endl << pose << endl; + cout << "Velocity:" << endl << velocity << endl; + cout << "Bias:" << endl << bias << endl; - fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", - gps_measurements[i].time, - pose.x(), pose.y(), pose.z(), - pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), - gps(0), gps(1), gps(2)); - } + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + gps_measurements[i].time, pose.x(), pose.y(), pose.z(), + pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0), + gps(1), gps(2)); + } - fclose(fp_out); + fclose(fp_out); } From 23858f31e9298d4dd9bd6a6f9304c9434e691821 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 13:34:26 -0400 Subject: [PATCH 005/169] working implementation --- python/gtsam/examples/IMUKittiExampleGPS.py | 195 +++++++++++++++----- 1 file changed, 152 insertions(+), 43 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 33e032614..a23f98186 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -1,67 +1,173 @@ """ Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE + +Author: Varun Agrawal """ import argparse -from typing import List + +import numpy as np import gtsam -import numpy as np -from gtsam import Pose3, Rot3, noiseModel +from gtsam import Pose3, noiseModel from gtsam.symbol_shorthand import B, V, X class KittiCalibration: - def __init__(self, bodyTimu: gtsam.Pose3): - self.bodyTimu = bodyTimu + """Class to hold KITTI calibration info.""" + def __init__(self, body_ptx: float, body_pty: float, body_ptz: float, + body_prx: float, body_pry: float, body_prz: float, + accelerometer_sigma: float, gyroscope_sigma: float, + integration_sigma: float, accelerometer_bias_sigma: float, + gyroscope_bias_sigma: float, average_delta_t: float): + self.bodyTimu = Pose3(gtsam.Rot3.RzRyRx(body_prx, body_pry, body_prz), + gtsam.Point3(body_ptx, body_pty, body_ptz)) + self.accelerometer_sigma = accelerometer_sigma + self.gyroscope_sigma = gyroscope_sigma + self.integration_sigma = integration_sigma + self.accelerometer_bias_sigma = accelerometer_bias_sigma + self.gyroscope_bias_sigma = gyroscope_bias_sigma + self.average_delta_t = average_delta_t class ImuMeasurement: + """An instance of an IMU measurement.""" def __init__(self, time, dt, accelerometer, gyroscope): - pass + self.time = time + self.dt = dt + self.accelerometer = accelerometer + self.gyroscope = gyroscope class GpsMeasurement: + """An instance of a GPS measurement.""" def __init__(self, time, position: gtsam.Point3): self.time = time self.position = position -def lodKittiData(): - pass +def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", + gps_data_file="KittiGps_converted.txt", + imu_metadata_file="KittiEquivBiasedImu_metadata.txt"): + """ + Load the KITTI Dataset. + """ + # Read IMU metadata and compute relative sensor pose transforms + # BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma + # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma + # AverageDeltaT + imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) + with open(imu_metadata_file) as imu_metadata: + print("-- Reading sensor metadata") + line = imu_metadata.readline() # Ignore the first line + line = imu_metadata.readline().strip() + data = list(map(float, line.split(' '))) + kitti_calibration = KittiCalibration(*data) + print("IMU metadata:", data) + # Read IMU data + # Time dt accelX accelY accelZ omegaX omegaY omegaZ + imu_data_file = gtsam.findExampleDataFile(imu_data_file) + imu_measurements = [] -def parse_args(): - parser = argparse.ArgumentParser() - return parser.parse_args() + print("-- Reading IMU measurements from file") + with open(imu_data_file) as imu_data: + data = imu_data.readlines() + for i in range(1, len(data)): # ignore the first line + time, dt, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = map( + float, data[i].split(' ')) + imu_measurement = ImuMeasurement( + time, dt, np.asarray([acc_x, acc_y, acc_z]), + np.asarray([gyro_x, gyro_y, gyro_z])) + imu_measurements.append(imu_measurement) + + # Read GPS data + # Time,X,Y,Z + gps_data_file = gtsam.findExampleDataFile(gps_data_file) + gps_measurements = [] + + print("-- Reading GPS measurements from file") + with open(gps_data_file) as gps_data: + data = gps_data.readlines() + for i in range(1, len(data)): + time, x, y, z = map(float, data[i].split(',')) + gps_measurement = GpsMeasurement(time, np.asarray([x, y, z])) + gps_measurements.append(gps_measurement) + + return kitti_calibration, imu_measurements, gps_measurements def getImuParams(kitti_calibration): + """Get the IMU parameters from the KITTI calibration data.""" GRAVITY = 9.8 w_coriolis = np.zeros(3) # Set IMU preintegration parameters measured_acc_cov = np.eye(3) * np.power( kitti_calibration.accelerometer_sigma, 2) - measured_omega_cov = np.eye(3) * np.powwe( + measured_omega_cov = np.eye(3) * np.power( kitti_calibration.gyroscope_sigma, 2) # error committed in integrating position from velocities integration_error_cov = np.eye(3) * np.power( kitti_calibration.integration_sigma, 2) imu_params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) - imu_params.accelerometerCovariance = measured_acc_cov # acc white noise in continuous - imu_params.integrationCovariance = integration_error_cov # integration uncertainty continuous - imu_params.gyroscopeCovariance = measured_omega_cov # gyro white noise in continuous - imu_params.omegaCoriolis = w_coriolis + # acc white noise in continuous + imu_params.setAccelerometerCovariance(measured_acc_cov) + # integration uncertainty continuous + imu_params.setIntegrationCovariance(integration_error_cov) + # gyro white noise in continuous + imu_params.setGyroscopeCovariance(measured_omega_cov) + imu_params.setOmegaCoriolis(w_coriolis) return imu_params -def main(): - args = parse_args() - kitti_calibration, imu_measurements, gps_measurements = lodKittiData() +def save_results(isam, output_filename, first_gps_pose, gps_measurements): + """Write the results from `isam` to `output_filename`.""" + # Save results to file + print("Writing results to file...") + with open(output_filename, 'w') as fp_out: + fp_out.write( + "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n") - if kitti_calibration.bodyTimu != gtsam.Pose3: + result = isam.calculateEstimate() + for i in range(first_gps_pose, len(gps_measurements)): + pose_key = X(i) + vel_key = V(i) + bias_key = B(i) + + pose = result.atPose3(pose_key) + velocity = result.atVector(vel_key) + bias = result.atConstantBias(bias_key) + + pose_quat = pose.rotation().toQuaternion() + gps = gps_measurements[i].position + + print("State at #{}".format(i)) + print("Pose:\n", pose) + print("Velocity:\n", velocity) + print("Bias:\n", bias) + + fp_out.write("{},{},{},{},{},{},{},{},{},{},{}\n".format( + gps_measurements[i].time, pose.x(), pose.y(), pose.z(), + pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), + gps[0], gps[1], gps[2])) + + +def parse_args(): + """Parse command line arguments.""" + parser = argparse.ArgumentParser() + parser.add_argument("--output_filename", + default="IMUKittiExampleGPSResults.csv") + return parser.parse_args() + + +def main(): + """Main runner.""" + args = parse_args() + kitti_calibration, imu_measurements, gps_measurements = loadKittiData() + + if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8): raise ValueError( "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" ) @@ -72,12 +178,13 @@ def main(): # Configure noise models noise_model_gps = noiseModel.Diagonal.Precisions( - np.asarray([0, 0, 0, 1.0 / 0.07, 1.0 / 0.07, 1.0 / 0.07])) + np.asarray([0, 0, 0] + [1.0 / 0.07] * 3)) # Set initial conditions for the estimated trajectory # initial pose is the reference frame (navigation frame) - current_pose_global = Pose3(Rot3(), + current_pose_global = Pose3(gtsam.Rot3(), gps_measurements[first_gps_pose].position) + # the vehicle is stationary at the beginning at position 0,0,0 current_velocity_global = np.zeros(3) current_bias = gtsam.imuBias.ConstantBias() # init with zero bias @@ -86,9 +193,9 @@ def main(): np.asarray([0, 0, 0, 1, 1, 1])) sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) sigma_init_b = noiseModel.Diagonal.Sigmas( - np.asarray([0.1, 0.1, 0.1, 5.00e-05, 5.00e-05, 5.00e-05])) + np.asarray([0.1] * 3 + [5.00e-05] * 3)) - imu_params = getImuParams() + imu_params = getImuParams(kitti_calibration) # Set ISAM2 parameters and create ISAM2 solver object isam_params = gtsam.ISAM2Params() @@ -100,18 +207,17 @@ def main(): # Create the factor graph and values object that will store new factors and # values to add to the incremental graph new_factors = gtsam.NonlinearFactorGraph() - new_values = gtsam.Values( - ) # values storing the initial estimates of new nodes in the factor graph + # values storing the initial estimates of new nodes in the factor graph + new_values = gtsam.Values() # Main loop: # (1) we read the measurements # (2) we create the corresponding factors in the graph # (3) we solve the graph to obtain and optimal estimate of robot trajectory - print( - "-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n" - ) + print("-- Starting main loop: inference is performed at each time step, " + "but we plot trajectory every 10 steps") j = 0 - for i in range(first_gps_pose, len(gps_measurements) - 1): + for i in range(first_gps_pose, len(gps_measurements)): # At each non=IMU measurement we initialize a new node in the graph current_pose_key = X(i) current_vel_key = V(i) @@ -138,7 +244,7 @@ def main(): imu_params, current_bias) included_imu_measurement_count = 0 - while (j < imu_measurements.size() + while (j < len(imu_measurements) and imu_measurements[j].time <= t): if imu_measurements[j].time >= t_previous: current_summarized_measurement.integrateMeasurement( @@ -153,13 +259,13 @@ def main(): previous_bias_key = B(i - 1) new_factors.push_back( - gtsam.ImuFactor > (previous_pose_key, previous_vel_key, - current_pose_key, current_vel_key, - previous_bias_key, - current_summarized_measurement)) + gtsam.ImuFactor(previous_pose_key, previous_vel_key, + current_pose_key, current_vel_key, + previous_bias_key, + current_summarized_measurement)) # Bias evolution as given in the IMU metadata - sigma_between_b = noiseModel.Diagonal.Sigmas( + sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas( np.asarray([ np.sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma @@ -183,8 +289,8 @@ def main(): new_values.insert(current_pose_key, gps_pose) print( - "################ POSE INCLUDED AT TIME %lf ################\n", - t) + "################ POSE INCLUDED AT TIME {} ################" + .format(t)) print(gps_pose.translation(), "\n") else: new_values.insert(current_pose_key, current_pose_global) @@ -200,8 +306,8 @@ def main(): # first so that the heading becomes observable. if i > (first_gps_pose + 2 * gps_skip): print( - "################ NEW FACTORS AT TIME %lf ################\n", - t) + "################ NEW FACTORS AT TIME {:.6f} ################" + .format(t)) new_factors.print() isam.update(new_factors, new_values) @@ -217,10 +323,13 @@ def main(): current_velocity_global = result.atVector(current_vel_key) current_bias = result.atConstantBias(current_bias_key) - print("\n################ POSE AT TIME %lf ################\n", - t) + print( + "################ POSE AT TIME {} ################".format( + t)) current_pose_global.print() - print("\n\n") + print("\n") + + save_results(isam, args.output_filename, first_gps_pose, gps_measurements) if __name__ == "__main__": From bc1f64e94331d703c70809edfe8e274644f4804e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 13:05:26 -0400 Subject: [PATCH 006/169] Add section about Boost version requirement --- INSTALL.md | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index 3a0f2896a..965246304 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -13,7 +13,8 @@ $ make install ## Important Installation Notes 1. GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.65 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes). + - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher @@ -66,11 +67,15 @@ execute commands as follows for an out-of-source build: This will build the library and unit tests, run all of the unit tests, and then install the library itself. +## Boost Notes + +Versions of Boost prior to 1.65 have a known bug that prevents proper "deep" serialization of objects, which means that objects encapsulated inside other objects don't get serialized. +This is particularly seen when using `clang` as the C++ compiler. + +For this reason we require Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager. + ## Known Issues -- When using `GTSAM_BUILD_WITH_MARCH_NATIVE=ON`, you may encounter issues in running tests which we are still investigating: - - Use of a version of GCC < 7.5 results in an "Indeterminant Linear System" error for `testSmartProjectionFactor`. - - Use of Boost version < 1.65 with clang will give a segfault for mulitple test cases. - MSVC 2013 is not yet supported because it cannot build the serialization module of Boost 1.55 (or earlier). # Windows Installation From 4f33cb8835ec800d3721592a593b37fac4803172 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 02:22:28 -0400 Subject: [PATCH 007/169] add guards for system Metis --- gtsam/config.h.in | 3 +++ gtsam_unstable/partition/FindSeparator-inl.h | 4 ++++ gtsam_unstable/partition/tests/testFindSeparator.cpp | 4 ++++ 3 files changed, 11 insertions(+) diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 9d1bd4ebd..9f7106187 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -77,3 +77,6 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_TANGENT_PREINTEGRATION + +// Whether to use the system installed Metis instead of the provided one +#cmakedefine GTSAM_USE_SYSTEM_METIS diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index ce657e7a0..2e48b0d45 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -20,6 +20,8 @@ #include "FindSeparator.h" +#ifndef GTSAM_USE_SYSTEM_METIS + extern "C" { #include #include "metislib.h" @@ -564,3 +566,5 @@ namespace gtsam { namespace partition { } }} //namespace + +#endif diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index fe49de928..63acc8f18 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -20,6 +20,8 @@ using namespace std; using namespace gtsam; using namespace gtsam::partition; +#ifndef GTSAM_USE_SYSTEM_METIS + /* ************************************************************************* */ // x0 - x1 - x2 // l3 l4 @@ -227,6 +229,8 @@ TEST ( Partition, findSeparator3_with_reduced_camera ) LONGS_EQUAL(2, partitionTable[28]); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 94dc709f9ba91c98643390806a79931c05641a64 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 09:04:43 -0400 Subject: [PATCH 008/169] CI special case for using system version of 3rd party libraries --- .github/scripts/unix.sh | 2 ++ .github/workflows/build-special.yml | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 6abbb5596..7fb925593 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -68,6 +68,8 @@ function configure() -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ + -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \ + -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DBOOST_ROOT=$BOOST_ROOT \ -DBoost_NO_SYSTEM_PATHS=ON \ diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 3bb3fa298..b7eb24c67 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -55,6 +55,12 @@ jobs: version: "9" flag: cayley + - name: ubuntu-system-libs + os: ubuntu-18.04 + compiler: gcc + version: "9" + flag: system-libs + steps: - name: Checkout uses: actions/checkout@v2 @@ -126,6 +132,12 @@ jobs: echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV echo "GTSAM Uses Cayley map for Rot3" + -name: Use system versions of 3rd party libraries + if: matrix.flag == 'system' + run: | + echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV + echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV + - name: Build & Test run: | bash .github/scripts/unix.sh -t From d83e9d0fd83bd0d175a31a3dd207f0f358781e8c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 09:05:55 -0400 Subject: [PATCH 009/169] formatting --- .github/workflows/build-special.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index b7eb24c67..647b9c0f1 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -132,11 +132,11 @@ jobs: echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV echo "GTSAM Uses Cayley map for Rot3" - -name: Use system versions of 3rd party libraries - if: matrix.flag == 'system' - run: | - echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV - echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV + - name: Use system versions of 3rd party libraries + if: matrix.flag == 'system' + run: | + echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV + echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV - name: Build & Test run: | From 4e295b91f22c1235dc7c9f85585e07a4e02ac043 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 11:18:58 -0400 Subject: [PATCH 010/169] add appropriate guards for metis --- gtsam/inference/Ordering.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 266c54ca6..440d2b828 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -25,8 +25,12 @@ #include #ifdef GTSAM_SUPPORT_NESTED_DISSECTION +#ifdef GTSAM_USE_SYSTEM_METIS +#include +#else #include #endif +#endif using namespace std; From ff7ddf48bd8bb2f993437f01cd32a4727d6f023b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Aug 2021 08:10:31 -0400 Subject: [PATCH 011/169] Basis functions (#403) --- doc/CMakeLists.txt | 25 +- gtsam/CMakeLists.txt | 1 + gtsam/basis/Basis.h | 507 ++++++++++++++++++ gtsam/basis/BasisFactors.h | 424 +++++++++++++++ gtsam/basis/CMakeLists.txt | 6 + gtsam/basis/Chebyshev.cpp | 98 ++++ gtsam/basis/Chebyshev.h | 109 ++++ gtsam/basis/Chebyshev2.cpp | 205 +++++++ gtsam/basis/Chebyshev2.h | 148 +++++ gtsam/basis/FitBasis.h | 99 ++++ gtsam/basis/Fourier.h | 108 ++++ gtsam/basis/ParameterMatrix.h | 215 ++++++++ gtsam/basis/basis.i | 146 +++++ gtsam/basis/tests/CMakeLists.txt | 1 + gtsam/basis/tests/testChebyshev.cpp | 236 ++++++++ gtsam/basis/tests/testChebyshev2.cpp | 435 +++++++++++++++ gtsam/basis/tests/testFourier.cpp | 254 +++++++++ gtsam/basis/tests/testParameterMatrix.cpp | 145 +++++ gtsam/nonlinear/FunctorizedFactor.h | 2 +- .../nonlinear/tests/testFunctorizedFactor.cpp | 140 ++++- python/CMakeLists.txt | 1 + python/gtsam/preamble/basis.h | 12 + python/gtsam/specializations/basis.h | 12 + 23 files changed, 3313 insertions(+), 16 deletions(-) create mode 100644 gtsam/basis/Basis.h create mode 100644 gtsam/basis/BasisFactors.h create mode 100644 gtsam/basis/CMakeLists.txt create mode 100644 gtsam/basis/Chebyshev.cpp create mode 100644 gtsam/basis/Chebyshev.h create mode 100644 gtsam/basis/Chebyshev2.cpp create mode 100644 gtsam/basis/Chebyshev2.h create mode 100644 gtsam/basis/FitBasis.h create mode 100644 gtsam/basis/Fourier.h create mode 100644 gtsam/basis/ParameterMatrix.h create mode 100644 gtsam/basis/basis.i create mode 100644 gtsam/basis/tests/CMakeLists.txt create mode 100644 gtsam/basis/tests/testChebyshev.cpp create mode 100644 gtsam/basis/tests/testChebyshev2.cpp create mode 100644 gtsam/basis/tests/testFourier.cpp create mode 100644 gtsam/basis/tests/testParameterMatrix.cpp create mode 100644 python/gtsam/preamble/basis.h create mode 100644 python/gtsam/specializations/basis.h diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 7c43a8989..2218addcf 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -22,18 +22,19 @@ if (GTSAM_BUILD_DOCS) # GTSAM core subfolders set(gtsam_doc_subdirs - gtsam/base - gtsam/discrete - gtsam/geometry - gtsam/inference - gtsam/linear - gtsam/navigation - gtsam/nonlinear - gtsam/sam - gtsam/sfm - gtsam/slam - gtsam/smart - gtsam/symbolic + gtsam/base + gtsam/basis + gtsam/discrete + gtsam/geometry + gtsam/inference + gtsam/linear + gtsam/navigation + gtsam/nonlinear + gtsam/sam + gtsam/sfm + gtsam/slam + gtsam/smart + gtsam/symbolic gtsam ) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 71daf0653..e2f2ad828 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,6 +5,7 @@ project(gtsam LANGUAGES CXX) # The following variable is the master list of subdirs to add set (gtsam_subdirs base + basis geometry inference symbolic diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h new file mode 100644 index 000000000..d8bd28c1a --- /dev/null +++ b/gtsam/basis/Basis.h @@ -0,0 +1,507 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Basis.h + * @brief Compute an interpolating basis + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +/** + * This file supports creating continuous functions `f(x;p)` as a linear + * combination of `basis functions` such as the Fourier basis on SO(2) or a set + * of Chebyshev polynomials on [-1,1]. + * + * In the expression `f(x;p)` the variable `x` is + * the continuous argument at which the function is evaluated, and `p` are + * the parameters which are coefficients of the different basis functions, + * e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial. + * However, different parameterizations are also possible. + + * The `Basis` class below defines a number of functors that can be used to + * evaluate `f(x;p)` at a given `x`, and these functors also calculate + * the Jacobian of `f(x;p)` with respect to the parameters `p`. + * This is actually the most important calculation, as it will allow GTSAM + * to optimize over the parameters `p`. + + * This functionality is implemented using the `CRTP` or "Curiously recurring + * template pattern" C++ idiom, which is a meta-programming technique in which + * the derived class is passed as a template argument to `Basis`. + * The DERIVED class is assumed to satisfy a C++ concept, + * i.e., we expect it to define the following types and methods: + + - type `Parameters`: the parameters `p` in f(x;p) + - `CalculateWeights(size_t N, double x, double a=default, double b=default)` + - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` + + where `Weights` is an N*1 row vector which defines the basis values for the + polynomial at the specified point `x`. + + E.g. A Fourier series would give the following: + - `CalculateWeights` -> For N=5, the values for the bases: + [1, cos(x), sin(x), cos(2x), sin(2x)] + - `DerivativeWeights` -> For N=5, these are: + [0, -sin(x), cos(x), -2sin(2x), 2cos(x)] + + Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are + instead the values for the Barycentric interpolation formula, since the values + at the polynomial points (e.g. Chebyshev points) define the bases. + */ + +namespace gtsam { + +using Weights = Eigen::Matrix; /* 1xN vector */ + +/** + * @brief Function for computing the kronecker product of the 1*N Weight vector + * `w` with the MxM identity matrix `I` efficiently. + * The main reason for this is so we don't need to use Eigen's Unsupported + * library. + * + * @tparam M Size of the identity matrix. + * @param w The weights of the polynomial. + * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I] + */ +template +Matrix kroneckerProductIdentity(const Weights& w) { + Matrix result(M, w.cols() * M); + result.setZero(); + + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); + } + return result; +} + +/// CRTP Base class for function bases +template +class GTSAM_EXPORT Basis { + public: + /** + * Calculate weights for all x in vector X. + * Returns M*N matrix where M is the size of the vector X, + * and N is the number of basis functions. + */ + static Matrix WeightMatrix(size_t N, const Vector& X) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i)); + return W; + } + + /** + * @brief Calculate weights for all x in vector X, with interval [a,b]. + * + * @param N The number of basis functions. + * @param X The vector for which to compute the weights. + * @param a The lower bound for the interval range. + * @param b The upper bound for the interval range. + * @return Returns M*N matrix where M is the size of the vector X. + */ + static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); + return W; + } + + /** + * An instance of an EvaluationFunctor calculates f(x;p) at a given `x`, + * applied to Parameters `p`. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific N*1 vector of Parameters, returns the scalar + * value of the function at x, possibly with Jacobians wrpt the parameters. + */ + class EvaluationFunctor { + protected: + Weights weights_; + + public: + /// For serialization + EvaluationFunctor() {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x) + : weights_(DERIVED::CalculateWeights(N, x)) {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x, double a, double b) + : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} + + /// Regular 1D evaluation + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + if (H) *H = weights_; + return (weights_ * p)(0); + } + + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + return apply(p, H); // might call apply in derived + } + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + */ + template + class VectorEvaluationFunctor : protected EvaluationFunctor { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorEvaluationFunctor() {} + + /// Default Constructor + VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + calculateJacobian(); + } + + /// Constructor, with interval [a,b] + VectorEvaluationFunctor(size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b) { + calculateJacobian(); + } + + /// M-dimensional evaluation + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + + /// c++ sugar + VectorM operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * VectorComponentFunctor computes the N-vector value for a specific row + * component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class VectorComponentFunctor : public EvaluationFunctor { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorEvaluationFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) + H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + } + + public: + /// For serialization + VectorComponentFunctor() {} + + /// Construct with row index + VectorComponentFunctor(size_t N, size_t i, double x) + : EvaluationFunctor(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + + /// Calculate component of component rowIndex_ of P + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); + } + + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + * + * The difference with the VectorEvaluationFunctor is that after computing the + * M*1 vector xi=F(x;P), with x a scalar and P the M*N parameter vector, we + * also retract xi back to the T manifold. + * For example, if T==Rot3, then we first compute a 3-vector xi using x and P, + * and then map that 3-vector xi back to the Rot3 manifold, yielding a valid + * 3D rotation. + */ + template + class ManifoldEvaluationFunctor + : public VectorEvaluationFunctor::dimension> { + enum { M = traits::dimension }; + using Base = VectorEvaluationFunctor; + + public: + /// For serialization + ManifoldEvaluationFunctor() {} + + /// Default Constructor + ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + + /// Constructor, with interval [a,b] + ManifoldEvaluationFunctor(size_t N, double x, double a, double b) + : Base(N, x, a, b) {} + + /// Manifold evaluation + T apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + // Interpolate the M-dimensional vector to yield a vector in tangent space + Eigen::Matrix xi = Base::operator()(P, H); + + // Now call retract with this M-vector, possibly with derivatives + Eigen::Matrix D_result_xi; + T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0); + + // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N) + // derivative of interpolation and D_result_xi is MxM derivative of + // retract. + if (H) *H = D_result_xi * (*H); + + // and return a T + return result; + } + + /// c++ sugar + T operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); // might call apply in derived + } + }; + + /// Base class for functors below that calculate derivative weights + class DerivativeFunctorBase { + protected: + Weights weights_; + + public: + /// For serialization + DerivativeFunctorBase() {} + + DerivativeFunctorBase(size_t N, double x) + : weights_(DERIVED::DerivativeWeights(N, x)) {} + + DerivativeFunctorBase(size_t N, double x, double a, double b) + : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * An instance of a DerivativeFunctor calculates f'(x;p) at a given `x`, + * applied to Parameters `p`. + * When given a scalar value x and a specific N*1 vector of Parameters, + * this functor returns the scalar derivative value of the function at x, + * possibly with Jacobians wrpt the parameters. + */ + class DerivativeFunctor : protected DerivativeFunctorBase { + public: + /// For serialization + DerivativeFunctor() {} + + DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {} + + DerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) {} + + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + if (H) *H = this->weights_; + return (this->weights_ * p)(0); + } + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + return apply(p, H); // might call apply in derived + } + }; + + /** + * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * + * This functor is used to evaluate the derivatives of a parameterized + * function at a given scalar value x. When given a specific M*N parameters, + * returns an M-vector the M corresponding function derivatives at x, possibly + * with Jacobians wrpt the parameters. + */ + template + class VectorDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorDerivativeFunctor() {} + + /// Default Constructor + VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + calculateJacobian(); + } + + /// Constructor, with optional interval [a,b] + VectorDerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) { + calculateJacobian(); + } + + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + /// c++ sugar + VectorM operator()( + const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * ComponentDerivativeFunctor computes the N-vector derivative for a specific + * row component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class ComponentDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorDerivativeFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < this->weights_.size(); j++) + H_(0, rowIndex_ + j * M) = this->weights_(j); + } + + public: + /// For serialization + ComponentDerivativeFunctor() {} + + /// Construct with row index + ComponentDerivativeFunctor(size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + /// Calculate derivative of component rowIndex_ of F + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * this->weights_.transpose(); + } + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + // Vector version for MATLAB :-( + static double Derivative(double x, const Vector& p, // + OptionalJacobian H = boost::none) { + return DerivativeFunctor(x)(p.transpose(), H); + } +}; + +} // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h new file mode 100644 index 000000000..0b3d4c1a0 --- /dev/null +++ b/gtsam/basis/BasisFactors.h @@ -0,0 +1,424 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BasisFactors.h + * @brief Factor definitions for various Basis functors. + * @author Varun Agrawal + * @date July 4, 2020 + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Factor for enforcing the scalar value of the polynomial BASIS + * representation at `x` is the same as the measurement `z` when using a + * pseudo-spectral parameterization. + * + * @tparam BASIS The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + EvaluationFactor() {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} + + virtual ~EvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (M, N) is equal to a vector-valued measurement at the same point, + when + * using a pseudo-spectral parameterization. + * + * This factors tries to enforce the basis function evaluation `f(x;p)` to take + * on the value `z` at location `x`, providing a gradient on the parameters p. + * In a probabilistic estimation context, `z` is known as a measurement, and the + * parameterized basis function can be seen as a + * measurement prediction function. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector. + */ +template +class GTSAM_EXPORT VectorEvaluationFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorEvaluationFactor() {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} + + virtual ~VectorEvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (P, N) is equal to specified measurement at the same point, when + * using a pseudo-spectral parameterization. + * + * This factor is similar to `VectorEvaluationFactor` with the key difference + * being that it only enforces the constraint for a single scalar in the vector, + * indexed by `i`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the fixed-size vector. + * + * Example: + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); + * where N is the degree and i is the component index. + */ +template +class GTSAM_EXPORT VectorComponentFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorComponentFactor() {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x) + : Base(key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x)) {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate 0the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { + } + + virtual ~VectorComponentFactor() {} +}; + +/** + * For a measurement value of type T i.e. `T z = g(x)`, this unary + * factor enforces that the polynomial basis, when evaluated at `x`, gives us + * the same `z`, i.e. `T z = g(x) = f(x;p)`. + * + * This is done via computations on the tangent space of the + * manifold of T. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param T: Object type which is synthesized by the provided functor. + * + * Example: + * ManifoldEvaluationFactor rotationFactor(key, measurement, + * model, N, x, a, b); + * + * where `x` is the value (e.g. timestep) at which the rotation was evaluated. + */ +template +class GTSAM_EXPORT ManifoldEvaluationFactor + : public FunctorizedFactor::dimension>> { + private: + using Base = FunctorizedFactor::dimension>>; + + public: + ManifoldEvaluationFactor() {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x, a, b)) { + } + + virtual ~ManifoldEvaluationFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point`x` is equal to the scalar measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT DerivativeFactor + : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + DerivativeFactor() {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {} + + virtual ~DerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point `x` is equal to the vector value `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector derivative. + */ +template +class GTSAM_EXPORT VectorDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template VectorDerivativeFunctor; + + public: + VectorDerivativeFactor() {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, Func(N, x)) {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, Func(N, x, a, b)) {} + + virtual ~VectorDerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial is equal to the scalar value at a specific index `i` of a + * vector-valued measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the control component derivative. + */ +template +class GTSAM_EXPORT ComponentDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template ComponentDerivativeFunctor

; + + public: + ComponentDerivativeFactor() {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x) + : Base(key, z, model, Func(N, i, x)) {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x, double a, double b) + : Base(key, z, model, Func(N, i, x, a, b)) {} + + virtual ~ComponentDerivativeFactor() {} +}; + +} // namespace gtsam diff --git a/gtsam/basis/CMakeLists.txt b/gtsam/basis/CMakeLists.txt new file mode 100644 index 000000000..203fd96a2 --- /dev/null +++ b/gtsam/basis/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB basis_headers "*.h") +install(FILES ${basis_headers} DESTINATION include/gtsam/basis) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp new file mode 100644 index 000000000..3b5825fc3 --- /dev/null +++ b/gtsam/basis/Chebyshev.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev.cpp + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +/** + * @brief Scale x from [a, b] to [t1, t2] + * + * ((b'-a') * (x - a) / (b - a)) + a' + * + * @param x Value to scale to new range. + * @param a Original lower limit. + * @param b Original upper limit. + * @param t1 New lower limit. + * @param t2 New upper limit. + * @return double + */ +static double scale(double x, double a, double b, double t1, double t2) { + return ((t2 - t1) * (x - a) / (b - a)) + t1; +} + +Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Tx(1, N); + + x = scale(x, a, b, -1, 1); + + Tx(0) = 1; + Tx(1) = x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); + } + return Tx; +} + +Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + Weights weights = Weights::Zero(N); + for (size_t n = 1; n < N; n++) { + weights(n) = n * Ux(n - 1); + } + return weights; +} + +Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Ux(N); + + x = scale(x, a, b, -1, 1); + + Ux(0) = 1; + Ux(1) = 2 * x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2); + } + return Ux; +} + +Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b); + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + + Weights weights(N); + + x = scale(x, a, b, -1, 1); + if (x == -1 || x == 1) { + throw std::runtime_error( + "Derivative of Chebyshev2 Basis does not exist at range limits."); + } + + for (size_t n = 0; n < N; n++) { + weights(n) = ((n + 1) * Tx(n + 1) - x * Ux(n)) / (x * x - 1); + } + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h new file mode 100644 index 000000000..d16ccfaac --- /dev/null +++ b/gtsam/basis/Chebyshev.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev.h + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * Basis of Chebyshev polynomials of the first kind + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind + * These are typically denoted with the symbol T_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + */ +struct Chebyshev1Basis : Basis { + using Parameters = Eigen::Matrix; + + Parameters parameters_; + + /** + * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * The derivative weights are pre-multiplied to the polynomial Parameters. + * + * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) + * I.e. the derivative fo a first kind cheb is just a second kind cheb + * So, we define a second kind basis here of order N-1 + * Note that it has one less weight. + * + * The Parameters pertain to 1st kind chebs up to order N-1 + * But of course the first one (order 0) is constant, so omit that weight. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev1Basis + +/** + * Basis of Chebyshev polynomials of the second kind. + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind + * These are typically denoted with the symbol U_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + * In contrast to the templates in Chebyshev2, the classes below specify + * basis functions, weighted combinations of which are used to approximate + * functions. In this sense, they are like the sines and cosines of the Fourier + * basis. + */ +struct Chebyshev2Basis : Basis { + using Parameters = Eigen::Matrix; + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev2Basis + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp new file mode 100644 index 000000000..44876b6e9 --- /dev/null +++ b/gtsam/basis/Chebyshev2.cpp @@ -0,0 +1,205 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev2.cpp + * @brief Chebyshev parameterizations on Chebyshev points of second kind + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weights(N); + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weights.setZero(); + weights(j) = 1; + return weights; + } + distances(j) = dj; + } + + // Beginning of interval, j = 0, x(0) = a + weights(0) = 0.5 / distances(0); + + // All intermediate points j=1:N-2 + double d = weights(0), s = -1; // changes sign s at every iteration + for (size_t j = 1; j < N - 1; j++, s = -s) { + weights(j) = s / distances(j); + d += weights(j); + } + + // End of interval, j = N-1, x(N-1) = b + weights(N - 1) = 0.5 * s / distances(N - 1); + d += weights(N - 1); + + // normalize + return weights / d; +} + +Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weightDerivatives(N); + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weightDerivatives.setZero(); + // compute the jth row of the differentiation matrix for this point + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + for (size_t k = 0; k < N; k++) { + if (j == 0 && k == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (j == N - 1 && k == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (k == j) { + double xj = Point(N, j); + double xj2 = xj * xj; + weightDerivatives(k) = -0.5 * xj / (1 - xj2); + } else { + double xj = Point(N, j); + double xk = Point(N, k); + double ck = (k == 0 || k == N - 1) ? 2. : 1.; + t = ((j + k) % 2) == 0 ? 1 : -1; + weightDerivatives(k) = (cj / ck) * t / (xj - xk); + } + } + return 2 * weightDerivatives / (b - a); + } + distances(j) = dj; + } + + // This section of code computes the derivative of + // the Barycentric Interpolation weights formula by applying + // the chain rule on the original formula. + + // g and k are multiplier terms which represent the derivatives of + // the numerator and denominator + double g = 0, k = 0; + double w = 1; + + for (size_t j = 0; j < N; j++) { + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + w = 1.0; + } + + t = (j % 2 == 0) ? 1 : -1; + + double c = t / distances(j); + g += w * c; + k += (w * c / distances(j)); + } + + double s = 1; // changes sign s at every iteration + double g2 = g * g; + + for (size_t j = 0; j < N; j++, s = -s) { + // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1, + // x0 = 1.0 + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + // All intermediate points j=1:N-2 + w = 1.0; + } + weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - + (w * -s * k / (g2 * distances(j))); + } + + return weightDerivatives; +} + +Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, + double b) { + DiffMatrix D(N, N); + if (N == 1) { + D(0, 0) = 1; + return D; + } + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + for (size_t i = 0; i < N; i++) { + double xi = Point(N, i); + double ci = (i == 0 || i == N - 1) ? 2. : 1.; + for (size_t j = 0; j < N; j++) { + if (i == 0 && j == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == N - 1 && j == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == j) { + double xi2 = xi * xi; + D(i, j) = -xi / (2 * (1 - xi2)); + } else { + double xj = Point(N, j); + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + t = ((i + j) % 2) == 0 ? 1 : -1; + D(i, j) = (ci / cj) * t / (xi - xj); + } + } + } + // scale the matrix to the range + return D / ((b - a) / 2.0); +} + +Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { + // Allocate space for weights + Weights weights(N); + size_t K = N - 1, // number of intervals between N points + K2 = K * K; + weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); + weights(N - 1) = weights(0); + + size_t last_k = K / 2 + K % 2 - 1; + + for (size_t i = 1; i <= N - 2; ++i) { + double theta = i * M_PI / K; + weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; + + for (size_t k = 1; k <= last_k; ++k) + weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1); + weights(i) *= (b - a) / K; + } + + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h new file mode 100644 index 000000000..28590961d --- /dev/null +++ b/gtsam/basis/Chebyshev2.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev2.h + * @brief Pseudo-spectral parameterization for Chebyshev polynomials of the + * second kind. + * + * In a pseudo-spectral case, rather than the parameters acting as + * weights for the bases polynomials (as in Chebyshev2Basis), here the + * parameters are the *values* at a specific set of points in the interval, the + * "Chebyshev points". These values uniquely determine the polynomial that + * interpolates them at the Chebyshev points. + * + * This is different from Chebyshev.h since it leverage ideas from + * pseudo-spectral optimization, i.e. we don't decompose into basis functions, + * rather estimate function parameters that enforce function nodes at Chebyshev + * points. + * + * Please refer to Agrawal21icra for more details. + * + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Chebyshev Interpolation on Chebyshev points of the second kind + * Note that N here, the number of points, is one less than N from + * 'Approximation Theory and Approximation Practice by L. N. Trefethen (pg.42)'. + */ +class GTSAM_EXPORT Chebyshev2 : public Basis { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using Base = Basis; + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /// Specific Chebyshev point + static double Point(size_t N, int j) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(- M_PI_2 + dtheta*j); also works + return cos(-M_PI + dtheta * j); + } + + /// Specific Chebyshev point, within [a,b] interval + static double Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N - 1); + // We add -PI so that we get values ordered from -1 to +1 + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; + } + + /// All Chebyshev points + static Vector Points(size_t N) { + Vector points(N); + for (size_t j = 0; j < N; j++) points(j) = Point(N, j); + return points; + } + + /// All Chebyshev points, within [a,b] interval + static Vector Points(size_t N, double a, double b) { + Vector points = Points(N); + const double T1 = (a + b) / 2, T2 = (b - a) / 2; + points = T1 + (T2 * points).array(); + return points; + } + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * These weights implement barycentric interpolation at a specific x. + * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the + * values of the function f at the Chebyshev points. As such, for a given x we + * obtain a linear map from parameter vectors f to interpolated values f(x). + * Optional [a,b] interval can be specified as well. + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * Evaluate derivative of barycentric weights. + * This is easy and efficient via the DifferentiationMatrix. + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); + + /// compute D = differentiation matrix, Trefethen00book p.53 + /// when given a parameter vector f of function values at the Chebyshev + /// points, D*f are the values of f'. + /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4 + static DiffMatrix DifferentiationMatrix(size_t N, double a = -1, + double b = 1); + + /** + * Evaluate Clenshaw-Curtis integration weights. + * Trefethen00book, pg 128, clencurt.m + * Note that N in clencurt.m is 1 less than our N + * K = N-1; + theta = pi*(0:K)'/K; + w = zeros(1,N); ii = 2:K; v = ones(K-1, 1); + if mod(K,2) == 0 + w(1) = 1/(K^2-1); w(N) = w(1); + for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + v = v - cos(K*theta(ii))/(K^2-1); + else + w(1) = 1/K^2; w(N) = w(1); + for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + end + w(ii) = 2*v/K; + + */ + static Weights IntegrationWeights(size_t N, double a = -1, double b = 1); + + /** + * Create matrix of values at Chebyshev points given vector-valued function. + */ + template + static Matrix matrix(boost::function(double)> f, + size_t N, double a = -1, double b = 1) { + Matrix Xmat(M, N); + for (size_t j = 0; j < N; j++) { + Xmat.col(j) = f(Point(N, j, a, b)); + } + return Xmat; + } +}; // \ Chebyshev2 + +} // namespace gtsam diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h new file mode 100644 index 000000000..f5cb99bd7 --- /dev/null +++ b/gtsam/basis/FitBasis.h @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FitBasis.h + * @date July 4, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Fit a Basis using least-squares + */ + +/* + * Concept needed for LS. Parameters = Coefficients | Values + * - Parameters, Jacobian + * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H) + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Our sequence representation is a map of {x: y} values where y = f(x) +using Sequence = std::map; +/// A sample is a key-value pair from a sequence. +using Sample = std::pair; + +/** + * Class that does regression via least squares + * Example usage: + * size_t N = 3; + * auto fit = FitBasis(data_points, noise_model, N); + * Vector coefficients = fit.parameters(); + * + * where `data_points` are a map from `x` to `y` values indicating a function + * mapping at specific points, `noise_model` is the gaussian noise model, and + * `N` is the degree of the polynomial basis used to fit the function. + */ +template +class FitBasis { + public: + using Parameters = typename Basis::Parameters; + + private: + Parameters parameters_; + + public: + /// Create nonlinear FG from Sequence + static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, + const SharedNoiseModel& model, + size_t N) { + NonlinearFactorGraph graph; + for (const Sample sample : sequence) { + graph.emplace_shared>(0, sample.second, model, N, + sample.first); + } + return graph; + } + + /// Create linear FG from Sequence + static GaussianFactorGraph::shared_ptr LinearGraph( + const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); + Values values; + values.insert(0, Parameters::Zero(N)); + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + return gfg; + } + + /** + * @brief Construct a new FitBasis object. + * + * @param sequence map of x->y values for a function, a.k.a. y = f(x). + * @param model The noise model to use. + * @param N The degree of the polynomial to fit. + */ + FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N); + VectorValues solution = gfg->optimize(); + parameters_ = solution.at(0); + } + + /// Return Fourier coefficients + Parameters parameters() const { return parameters_; } +}; + +} // namespace gtsam diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h new file mode 100644 index 000000000..6943150d7 --- /dev/null +++ b/gtsam/basis/Fourier.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Fourier.h + * @brief Fourier decomposition, see e.g. + * http://mathworld.wolfram.com/FourierSeries.html + * @author Varun Agrawal, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include + +namespace gtsam { + +/// Fourier basis +class GTSAM_EXPORT FourierBasis : public Basis { + public: + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x) { + Weights b(N); + b[0] = 1; + for (size_t i = 1, n = 1; i < N; i += 2, n++) { + b[i] = cos(n * x); + b[i + 1] = sin(n * x); + } + return b; + } + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x, double a, double b) { + // TODO(Varun) How do we enforce an interval for Fourier series? + return CalculateWeights(N, x); + } + + /** + * Compute D = differentiation matrix. + * Given coefficients c of a Fourier series c, D*c are the values of c'. + */ + static DiffMatrix DifferentiationMatrix(size_t N) { + DiffMatrix D = DiffMatrix::Zero(N, N); + double k = 1; + for (size_t i = 1; i < N; i += 2) { + D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x) + D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x) + k += 1; + } + + return D; + } + + /** + * @brief Get weights at a given x that calculate the derivative. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x) { + return CalculateWeights(N, x) * DifferentiationMatrix(N); + } + + /** + * @brief Get derivative weights at a given x that calculate the derivative, + in the interval [a, b]. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a, double b) { + return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N); + } + +}; // FourierBasis + +} // namespace gtsam diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h new file mode 100644 index 000000000..df2d9f62e --- /dev/null +++ b/gtsam/basis/ParameterMatrix.h @@ -0,0 +1,215 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ParamaterMatrix.h + * @brief Define ParameterMatrix class which is used to store values at + * interpolation points. + * @author Varun Agrawal, Frank Dellaert + * @date September 21, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * A matrix abstraction of MxN values at the Basis points. + * This class serves as a wrapper over an Eigen matrix. + * @tparam M: The dimension of the type you wish to evaluate. + * @param N: the number of Basis points (e.g. Chebyshev points of the second + * kind). + */ +template +class ParameterMatrix { + using MatrixType = Eigen::Matrix; + + private: + MatrixType matrix_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum { dimension = Eigen::Dynamic }; + + /** + * Create ParameterMatrix using the number of basis points. + * @param N: The number of basis points (the columns). + */ + ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); } + + /** + * Create ParameterMatrix from an MxN Eigen Matrix. + * @param matrix: An Eigen matrix used to initialze the ParameterMatrix. + */ + ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {} + + /// Get the number of rows. + size_t rows() const { return matrix_.rows(); } + + /// Get the number of columns. + size_t cols() const { return matrix_.cols(); } + + /// Get the underlying matrix. + MatrixType matrix() const { return matrix_; } + + /// Return the tranpose of the underlying matrix. + Eigen::Matrix transpose() const { return matrix_.transpose(); } + + /** + * Get the matrix row specified by `index`. + * @param index: The row index to retrieve. + */ + Eigen::Matrix row(size_t index) const { + return matrix_.row(index); + } + + /** + * Set the matrix row specified by `index`. + * @param index: The row index to set. + */ + auto row(size_t index) -> Eigen::Block { + return matrix_.row(index); + } + + /** + * Get the matrix column specified by `index`. + * @param index: The column index to retrieve. + */ + Eigen::Matrix col(size_t index) const { + return matrix_.col(index); + } + + /** + * Set the matrix column specified by `index`. + * @param index: The column index to set. + */ + auto col(size_t index) -> Eigen::Block { + return matrix_.col(index); + } + + /** + * Set all matrix coefficients to zero. + */ + void setZero() { matrix_.setZero(); } + + /** + * Add a ParameterMatrix to another. + * @param other: ParameterMatrix to add. + */ + ParameterMatrix operator+(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ + other.matrix()); + } + + /** + * Add a MxN-sized vector to the ParameterMatrix. + * @param other: Vector which is reshaped and added. + */ + ParameterMatrix operator+( + const Eigen::Matrix& other) const { + // This form avoids a deep copy and instead typecasts `other`. + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ + other_); + } + + /** + * Subtract a ParameterMatrix from another. + * @param other: ParameterMatrix to subtract. + */ + ParameterMatrix operator-(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ - other.matrix()); + } + + /** + * Subtract a MxN-sized vector from the ParameterMatrix. + * @param other: Vector which is reshaped and subracted. + */ + ParameterMatrix operator-( + const Eigen::Matrix& other) const { + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ - other_); + } + + /** + * Multiply ParameterMatrix with an Eigen matrix. + * @param other: Eigen matrix which should be multiplication compatible with + * the ParameterMatrix. + */ + MatrixType operator*(const Eigen::Matrix& other) const { + return matrix_ * other; + } + + /// @name Vector Space requirements, following LieMatrix + /// @{ + + /** + * Print the ParameterMatrix. + * @param s: The prepend string to add more contextual info. + */ + void print(const std::string& s = "") const { + std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; + } + + /** + * Check for equality up to absolute tolerance. + * @param other: The ParameterMatrix to check equality with. + * @param tol: The absolute tolerance threshold. + */ + bool equals(const ParameterMatrix& other, double tol = 1e-8) const { + return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol); + } + + /// Returns dimensionality of the tangent space + inline size_t dim() const { return matrix_.size(); } + + /// Convert to vector form, is done row-wise + inline Vector vector() const { + using RowMajor = Eigen::Matrix; + Vector result(matrix_.size()); + Eigen::Map(&result(0), rows(), cols()) = matrix_; + return result; + } + + /** Identity function to satisfy VectorSpace traits. + * + * NOTE: The size at compile time is unknown so this identity is zero + * length and thus not valid. + */ + inline static ParameterMatrix identity() { + // throw std::runtime_error( + // "ParameterMatrix::identity(): Don't use this function"); + return ParameterMatrix(0); + } + + /// @} +}; + +// traits for ParameterMatrix +template +struct traits> + : public internal::VectorSpace> {}; + +/* ************************************************************************* */ +// Stream operator that takes a ParameterMatrix. Used for printing. +template +inline std::ostream& operator<<(std::ostream& os, + const ParameterMatrix& parameterMatrix) { + os << parameterMatrix.matrix(); + return os; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i new file mode 100644 index 000000000..8f06fd2e1 --- /dev/null +++ b/gtsam/basis/basis.i @@ -0,0 +1,146 @@ +//************************************************************************* +// basis +//************************************************************************* + +namespace gtsam { + +// TODO(gerry): add all the Functors to the Basis interfaces, e.g. +// `EvaluationFunctor` + +#include + +class FourierBasis { + static Vector CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); + + static Matrix DifferentiationMatrix(size_t N); + static Vector DerivativeWeights(size_t N, double x); +}; + +#include + +class Chebyshev1Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector X); +}; + +class Chebyshev2Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); +}; + +#include +class Chebyshev2 { + static double Point(size_t N, int j); + static double Point(size_t N, int j, double a, double b); + + static Vector Points(size_t N); + static Vector Points(size_t N, double a, double b); + + static Matrix WeightMatrix(size_t N, Vector X); + static Matrix WeightMatrix(size_t N, Vector X, double a, double b); + + static Matrix CalculateWeights(size_t N, double x, double a, double b); + static Matrix DerivativeWeights(size_t N, double x, double a, double b); + static Matrix IntegrationWeights(size_t N, double a, double b); + static Matrix DifferentiationMatrix(size_t N, double a, double b); + + // TODO Needs OptionalJacobian + // static double Derivative(double x, Vector f); +}; + +#include + +template +class ParameterMatrix { + ParameterMatrix(const size_t N); + ParameterMatrix(const Matrix& matrix); + + Matrix matrix() const; + + void print(const string& s = "") const; +}; + +#include + +template +virtual class EvaluationFactor : gtsam::NoiseModelFactor { + EvaluationFactor(); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { + VectorEvaluationFactor(); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(Varun) Better way to support arbitrary dimensions? +// Especially if users mainly do `pip install gtsam` for the Python wrapper. +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D3; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D4; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D12; + +template +virtual class VectorComponentFactor : gtsam::NoiseModelFactor { + VectorComponentFactor(); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x, double a, double b); +}; + +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D3; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D4; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D12; + +template +virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { + ManifoldEvaluationFactor(); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and +// `ComponentDerivativeFactor` + +#include +template +class FitBasis { + FitBasis(const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + + static gtsam::NonlinearFactorGraph NonlinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + Parameters parameters() const; +}; + +} // namespace gtsam diff --git a/gtsam/basis/tests/CMakeLists.txt b/gtsam/basis/tests/CMakeLists.txt new file mode 100644 index 000000000..63cad0be6 --- /dev/null +++ b/gtsam/basis/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam") diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp new file mode 100644 index 000000000..64c925886 --- /dev/null +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +const size_t N = 3; + +//****************************************************************************** +TEST(Chebyshev, Chebyshev1) { + using Synth = Chebyshev1Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Chebyshev2) { + using Synth = Chebyshev2Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Evaluation) { + Chebyshev1Basis::EvaluationFunctor fx(N, 0.5); + Vector c(N); + c << 3, 5, -12; + EXPECT_DOUBLES_EQUAL(11.5, fx(c), 1e-9); +} + +//****************************************************************************** +#include +#include +TEST(Chebyshev, Expression) { + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + + // Let's pretend we have 6 GPS measurements (we just do x coordinate) + // at times + const size_t m = 6; + Vector t(m); + t << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + Vector x(m); + x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + + for (size_t i = 0; i < m; i++) { + graph.emplace_shared>(key, x(i), model, N, + t(i)); + } + + // Solve + Values initial; + initial.insert(key, Vector::Zero(N)); // initial does not matter + + // ... and optimize + GaussNewtonParams parameters; + GaussNewtonOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check + Vector expected(N); + expected << 0, 1, 0; + Vector actual_c = result.at(key); + EXPECT(assert_equal(expected, actual_c)); + + // Calculate and print covariances + Marginals marginals(graph, result); + Matrix3 cov = marginals.marginalCovariance(key); + EXPECT_DOUBLES_EQUAL(0.626, cov(1, 1), 1e-3); + + // Predict x at time 1.0 + Chebyshev1Basis::EvaluationFunctor f(N, 1.0); + Matrix H; + double actual = f(actual_c, H); + EXPECT_DOUBLES_EQUAL(1.0, actual, 1e-9); + + // Calculate predictive variance on prediction + double actual_variance_on_prediction = (H * cov * H.transpose())(0); + EXPECT_DOUBLES_EQUAL(1.1494, actual_variance_on_prediction, 1e-4); +} + +//****************************************************************************** +TEST(Chebyshev, Decomposition) { + const size_t M = 16; + + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < M; i++) { + double x = ((double)i / M); // - 0.99; + double y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, N); + + // Check + Vector expected = Vector::Zero(N); + expected(1) = 1; + EXPECT(assert_equal(expected, (Vector)actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev1, Derivative) { + Vector c(N); + c << 12, 3, 2; + + Weights D; + + double x = -1.0; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-5, (D * c)(0), 1e-9); + + x = -0.5; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-1, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(5.4, (D * c)(0), 1e-9); +} + +//****************************************************************************** +Vector3 f(-6, 1, 0.5); + +double proxy1(double x, size_t N) { + return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev1, Derivative2) { + const double x = 0.5; + auto D = Chebyshev1Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy1, x, N); + // regression + EXPECT_DOUBLES_EQUAL(2, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(2, (D * f)(0), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev2, Derivative) { + Vector c(N); + c << 12, 6, 2; + + Weights D; + + double x = -1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + x = 1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + + x = -0.5; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(4, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(16.8, (D * c)(0), 1e-9); + + x = 0.75; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(24, (D * c)(0), 1e-9); + + x = 10; + D = Chebyshev2Basis::DerivativeWeights(N, x, 0, 20); + // regression + EXPECT_DOUBLES_EQUAL(12, (D * c)(0), 1e-9); +} + +//****************************************************************************** +double proxy2(double x, size_t N) { + return Chebyshev2Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev2, Derivative2) { + const double x = 0.5; + auto D = Chebyshev2Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy2, x, N); + // regression + EXPECT_DOUBLES_EQUAL(4, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(4, (D * f)(0), 1e-9); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp new file mode 100644 index 000000000..4cee70daf --- /dev/null +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -0,0 +1,435 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral + * methods + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::placeholders; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + +const size_t N = 32; + +//****************************************************************************** +TEST(Chebyshev2, Point) { + static const int N = 5; + auto points = Chebyshev2::Points(N); + Vector expected(N); + expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // Check symmetry + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 0), -Chebyshev2::Point(N, 4), tol); + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 1), -Chebyshev2::Point(N, 3), tol); +} + +//****************************************************************************** +TEST(Chebyshev2, PointInInterval) { + static const int N = 5; + auto points = Chebyshev2::Points(N, 0, 20); + Vector expected(N); + expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.; + expected *= 10.0; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // all at once + Vector actual = Chebyshev2::Points(N, 0, 20); + CHECK(assert_equal(expected, actual)); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5] +TEST(Chebyshev2, Interpolate2) { + size_t N = 3; + Chebyshev2::EvaluationFunctor fx(N, 0.5); + Vector f(N); + f << 4, 2, 6; + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5] +TEST(Chebyshev2, Interpolate2_Interval) { + Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2); + Vector3 f(4, 2, 6); + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1, +// 3}}, 0.5] +TEST(Chebyshev2, Interpolate5) { + Chebyshev2::EvaluationFunctor fx(5, 0.5); + Vector f(5); + f << 4, 2, 6, 3, 3; + EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5); +} + +//****************************************************************************** +// Interpolating vectors +TEST(Chebyshev2, InterpolateVector) { + double t = 30, a = 0, b = 100; + const size_t N = 3; + // Create 2x3 matrix with Vectors at Chebyshev points + ParameterMatrix<2> X(N); + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + + // Check value + Vector expected(2); + expected << t, 0; + Eigen::Matrix actualH(2, 2 * N); + + Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); + + // Check derivative + boost::function)> f = boost::bind( + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); + Matrix numericalH = + numericalDerivative11, 2 * N>(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +//****************************************************************************** +TEST(Chebyshev2, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = (double)i / 16. - 0.99, y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, 3); + + // Check + Vector expected(3); + expected << -1, 0, 1; + EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DifferentiationMatrix3) { + // Trefethen00book, p.55 + const size_t N = 3; + Matrix expected(N, N); + // Differentiation matrix computed from Chebfun + expected << 1.5000, -2.0000, 0.5000, // + 0.5000, -0.0000, -0.5000, // + -0.5000, 2.0000, -1.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DerivativeMatrix6) { + // Trefethen00book, p.55 + const size_t N = 6; + Matrix expected(N, N); + expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000, // + 2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764, // + -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820, // + 0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // + -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // + 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); +} + +// test function for CalculateWeights and DerivativeWeights +double f(double x) { + // return 3*(x**3) - 2*(x**2) + 5*x - 11 + return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11; +} + +// its derivative +double fprime(double x) { + // return 9*(x**2) - 4*(x) + 5 + return 9.0 * pow(x, 2) - 4.0 * x + 5.0; +} + +//****************************************************************************** +TEST(Chebyshev2, CalculateWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376; + Weights weights1 = Chebyshev2::CalculateWeights(N, x1); + Weights weights2 = Chebyshev2::CalculateWeights(N, x2); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8); +} + +TEST(Chebyshev2, CalculateWeights2) { + double a = 0, b = 10, x1 = 7, x2 = 4.12; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + + Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); + double expected2 = f(x2); // 185.454784 + double actual2 = weights2 * fvals; + EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); +} + +TEST(Chebyshev2, DerivativeWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376, x3 = 0.0; + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9); + + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9); + + // test if derivative calculation and cheb point is correct + double x4 = Chebyshev2::Point(N, 3); + Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); + EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9); +} + +TEST(Chebyshev2, DerivativeWeights2) { + double x1 = 5, x2 = 4.12, a = 0, b = 10; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); + + // test if derivative calculation and cheb point is correct + double x3 = Chebyshev2::Point(N, 3, a, b); + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) { + const size_t N6 = 6; + double x1 = 0.311; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; + Weights actual = Chebyshev2::DerivativeWeights(N6, x1); + EXPECT(assert_equal(expected, actual, 1e-12)); + + double a = -3, b = 8, x2 = 5.05; + Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b); + Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; + Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); + EXPECT(assert_equal(expected1, actual1, 1e-12)); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights6) { + const size_t N6 = 6; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights7) { + const size_t N7 = 7; + Matrix D7 = Chebyshev2::DifferentiationMatrix(N7); + Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x))); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished(); +double proxy3(double x) { + return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6) { + // Check Derivative evaluation at point x=0.2 + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy3, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Assert that derivative also works in non-standard interval [0,3] +double proxy4(double x) { + return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6_03) { + // Check Derivative evaluation at point x=0.2, in interval [0,3] + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy4, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor +TEST(Chebyshev2, VectorDerivativeFunctor) { + const size_t N = 3, M = 2; + const double x = 0.2; + using VecD = Chebyshev2::VectorDerivativeFunctor; + VecD fx(N, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(M, M * N); + EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); + + // Test Jacobian + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor with polynomial function +TEST(Chebyshev2, VectorDerivativeFunctor2) { + const size_t N = 64, M = 1, T = 15; + using VecD = Chebyshev2::VectorDerivativeFunctor; + + const Vector points = Chebyshev2::Points(N, 0, T); + + // Assign the parameter matrix + Vector values(N); + for (size_t i = 0; i < N; ++i) { + values(i) = f(points(i)); + } + ParameterMatrix X(values); + + // Evaluate the derivative at the chebyshev points using + // VectorDerivativeFunctor. + for (size_t i = 0; i < N; ++i) { + VecD d(N, points(i), 0, T); + Vector1 Dx = d(X); + EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); + } + + // Test Jacobian at the first chebyshev point. + Matrix actualH(M, M * N); + VecD vecd(N, points(0), 0, T); + vecd(X, actualH); + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), vecd, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-6)); +} + +//****************************************************************************** +// Test ComponentDerivativeFunctor +TEST(Chebyshev2, ComponentDerivativeFunctor) { + const size_t N = 6, M = 2; + const double x = 0.2; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; + size_t row = 1; + CompFunc fx(N, row, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(1, M * N); + EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); + + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +TEST(Chebyshev2, IntegralWeights) { + const size_t N7 = 7; + Vector actual = Chebyshev2::IntegrationWeights(N7); + Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, + 0.457142857142857, 0.520634920634921, 0.457142857142857, + 0.253968253968254, 0.0285714285714286) + .finished(); + EXPECT(assert_equal(expected, actual)); + + const size_t N8 = 8; + Vector actual2 = Chebyshev2::IntegrationWeights(N8); + Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, + 0.352242423718159, 0.437208405798326, 0.437208405798326, + 0.352242423718159, 0.190141007218208, 0.0204081632653061) + .finished(); + EXPECT(assert_equal(expected2, actual2)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp new file mode 100644 index 000000000..7a53cfcc9 --- /dev/null +++ b/gtsam/basis/tests/testFourier.cpp @@ -0,0 +1,254 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testFourier.cpp + * @date July 4, 2020 + * @author Frank Dellaert, Varun Agrawal + * @brief Unit tests for Fourier Basis Decompositions with Expressions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +// Coefficients for testing, respectively 3 and 7 parameter Fourier basis. +// They correspond to best approximation of TestFunction(x) +const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished(); +const Vector7 k7Coefficients = + (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943) + .finished(); + +// The test-function used below +static double TestFunction(double x) { return exp(sin(x) + cos(x)); } + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctor) { + // fx(0) takes coefficients c to calculate the value f(c;x==0) + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients), 1e-9); +} + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctorDerivative) { + // If we give the H argument, we get the derivative of fx(0) wrpt coefficients + // Needs to be Matrix so it can be used by OptionalJacobian. + Matrix H(1, 3); + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients, H), 1e-9); + + Matrix13 expectedH(1, 1, 0); + EXPECT(assert_equal(expectedH, H)); +} + +//****************************************************************************** +TEST(Basis, Manual) { + const size_t N = 3; + + // We will create a linear factor graph + GaussianFactorGraph graph; + + // We create an unknown Vector expression for the coefficients + Key key(1); + + // We will need values below to test the Jacobians + Values values; + values.insert(key, Vector::Zero(N)); // value does not really matter + + // At 16 different samples points x, check Predict_ expression + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, N); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A, b); + graph.add(linearFactor); + + // Create factor to predict value at x + EvaluationFactor predictFactor(key, desiredValue, model, N, + x); + + // Check expression Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); + + auto linearizedFactor = predictFactor.linearize(values); + auto linearizedJacobianFactor = + boost::dynamic_pointer_cast(linearizedFactor); + CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor + EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9)); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, EvaluationFactor) { + // Check fitting a function with a 7-parameter Fourier basis + + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, desiredValue = TestFunction(x); + graph.emplace_shared>(key, desiredValue, + model, 7, x); + } + + // Solve FourierFactorGraph + Values values; + values.insert(key, Vector::Zero(7)); + GaussianFactorGraph::shared_ptr lfg = graph.linearize(values); + VectorValues actual = lfg->optimize(); + + EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, WeightMatrix) { + // The WeightMatrix creates an m*n matrix, where m is the number of sample + // points, and n is the number of parameters. + Matrix expected(2, 3); + expected.row(0) << 1, cos(1), sin(1); + expected.row(1) << 1, cos(2), sin(2); + Vector2 X(1, 2); + Matrix actual = FourierBasis::WeightMatrix(3, X); + EXPECT(assert_equal(expected, actual, 1e-9)); +} + +//****************************************************************************** +TEST(Basis, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = TestFunction(x); + sequence[x] = y; + } + + // Do Fourier Decomposition + FitBasis actual(sequence, model, 3); + + // Check + EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +double proxy(double x) { + return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients); +} + +TEST(Basis, Derivative7) { + // Check Derivative evaluation at point x=0.2 + + // Calculate expected values by numerical derivative of proxy. + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy, x); + + // Calculate derivatives at Chebyshev points using D7, interpolate + Matrix D7 = FourierBasis::DifferentiationMatrix(7); + Vector derivativeCoefficients = D7 * k7Coefficients; + FourierBasis::EvaluationFunctor fx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8); + + // Do directly + FourierBasis::DerivativeFunctor dfdx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8); +} + +//****************************************************************************** +TEST(Basis, VecDerivativeFunctor) { + using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + const size_t N = 3; + + // MATLAB example, Dec 27 2019, commit 014eded5 + double h = 2 * M_PI / 16; + Vector2 dotShape(0.5556, -0.8315); // at h/2 + DotShape dotShapeFunction(N, h / 2); + Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); + ParameterMatrix<2> theta(theta_mat); + EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); +} + +//****************************************************************************** +// Suppose we want to parameterize a periodic function with function values at +// specific times, rather than coefficients. Can we do it? This would be a +// generalization of the Fourier transform, and constitute a "pseudo-spectral" +// parameterization. One way to do this is to establish hard constraints that +// express the relationship between the new parameters and the coefficients. +// For example, below I'd like the parameters to be the function values at +// X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients. +// Because the values f(X) = at these points can be written as f(X) = W(X)*c, +// we can simply express the coefficents c as c=inv(W(X))*f, which is a +// generalized Fourier transform. That also means we can create factors with the +// unknown f-values, as done manually below. +TEST(Basis, PseudoSpectral) { + // We will create a linear factor graph + GaussianFactorGraph graph; + + const size_t N = 3; + const Key key(1); + + // The correct values at X = {0.1,0.2,0.3} are simply W*c + const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); + const Matrix W = FourierBasis::WeightMatrix(N, X); + const Vector expected = W * k3Coefficients; + + // Check those values are indeed correct values of Fourier approximation + using Eval = FourierBasis::EvaluationFunctor; + EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); + + // Calculate "inverse Fourier transform" matrix + const Matrix invW = W.inverse(); + + // At 16 different samples points x, add a factor on fExpr + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, 3); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A * invW, b); + graph.add(linearFactor); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp new file mode 100644 index 000000000..ec62e8eea --- /dev/null +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testParameterMatrix.cpp + * @date Sep 22, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Unit tests for ParameterMatrix.h + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using Parameters = Chebyshev2::Parameters; + +const size_t M = 2, N = 5; + +//****************************************************************************** +TEST(ParameterMatrix, Constructor) { + ParameterMatrix<2> actual1(3); + ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); + EXPECT(assert_equal(expected1, actual1)); + + ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); + ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Dimensions) { + ParameterMatrix params(N); + EXPECT_LONGS_EQUAL(params.rows(), M); + EXPECT_LONGS_EQUAL(params.cols(), N); + EXPECT_LONGS_EQUAL(params.dim(), M * N); +} + +//****************************************************************************** +TEST(ParameterMatrix, Getters) { + ParameterMatrix params(N); + + Matrix expectedMatrix = Matrix::Zero(2, 5); + EXPECT(assert_equal(expectedMatrix, params.matrix())); + + Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); + EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); + + ParameterMatrix p2(Matrix::Ones(M, N)); + Vector expectedRowVector = Vector::Ones(N); + for (size_t r = 0; r < M; ++r) { + EXPECT(assert_equal(p2.row(r), expectedRowVector)); + } + + ParameterMatrix p3(2 * Matrix::Ones(M, N)); + Vector expectedColVector = 2 * Vector::Ones(M); + for (size_t c = 0; c < M; ++c) { + EXPECT(assert_equal(p3.col(c), expectedColVector)); + } +} + +//****************************************************************************** +TEST(ParameterMatrix, Setters) { + ParameterMatrix params(Matrix::Zero(M, N)); + Matrix expected = Matrix::Zero(M, N); + + // row + params.row(0) = Vector::Ones(N); + expected.row(0) = Vector::Ones(N); + EXPECT(assert_equal(expected, params.matrix())); + + // col + params.col(2) = Vector::Ones(M); + expected.col(2) = Vector::Ones(M); + + EXPECT(assert_equal(expected, params.matrix())); + + // setZero + params.setZero(); + expected.setZero(); + EXPECT(assert_equal(expected, params.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Addition) { + ParameterMatrix params(Matrix::Ones(M, N)); + ParameterMatrix expected(2 * Matrix::Ones(M, N)); + + // Add vector + EXPECT(assert_equal(expected, params + Vector::Ones(M * N))); + // Add another ParameterMatrix + ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Subtraction) { + ParameterMatrix params(2 * Matrix::Ones(M, N)); + ParameterMatrix expected(Matrix::Ones(M, N)); + + // Subtract vector + EXPECT(assert_equal(expected, params - Vector::Ones(M * N))); + // Subtract another ParameterMatrix + ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Multiplication) { + ParameterMatrix params(Matrix::Ones(M, N)); + Matrix multiplier = 2 * Matrix::Ones(N, 2); + Matrix expected = 2 * N * Matrix::Ones(M, 2); + EXPECT(assert_equal(expected, params * multiplier)); +} + +//****************************************************************************** +TEST(ParameterMatrix, VectorSpace) { + ParameterMatrix params(Matrix::Ones(M, N)); + // vector + EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); + // identity + EXPECT(assert_equal(ParameterMatrix::identity(), + ParameterMatrix(Matrix::Zero(M, 0)))); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 691ab8ac8..e1f8ece8d 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -110,7 +110,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { bool equals(const NonlinearFactor &other, double tol = 1e-9) const override { const FunctorizedFactor *e = dynamic_cast *>(&other); - return e && Base::equals(other, tol) && + return e != nullptr && Base::equals(other, tol) && traits::Equals(this->measured_, e->measured_, tol); } /// @} diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index b0ec5e722..14a14fc19 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -20,8 +20,12 @@ #include #include #include +#include +#include +#include #include #include +#include #include using namespace std; @@ -60,7 +64,7 @@ class ProjectionFunctor { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; } @@ -255,18 +259,148 @@ TEST(FunctorizedFactor, Lambda2) { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; }; // FunctorizedFactor factor(key, measurement, model, lambda); - auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, model2, lambda); + auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, + model2, lambda); Vector error = factor.evaluateError(A, x); EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); } +const size_t N = 2; + +//****************************************************************************** +TEST(FunctorizedFactor, Print2) { + const size_t M = 1; + + Vector measured = Vector::Ones(M) * 42; + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, priorFactor)); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorEvaluationFactor) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(priorFactor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorComponentFactor) { + const int P = 4; + const size_t i = 2; + const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + VectorComponentFactor controlPrior(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(controlPrior); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VecDerivativePrior) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, ComponentDerivativeFactor) { + const size_t M = 4; + + double measured = 0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + ComponentDerivativeFactor controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bdda15acb..a5fdc80a6 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i + ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h new file mode 100644 index 000000000..d07a75f6f --- /dev/null +++ b/python/gtsam/preamble/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h new file mode 100644 index 000000000..da8842eaf --- /dev/null +++ b/python/gtsam/specializations/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ From 15120ce9ab48db2860ea2704a504d799a95420b2 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 4 Aug 2021 15:30:35 -0400 Subject: [PATCH 012/169] python unit test for FitBasis --- python/gtsam/tests/test_basis.py | 38 ++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 python/gtsam/tests/test_basis.py diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py new file mode 100644 index 000000000..944bdd9f3 --- /dev/null +++ b/python/gtsam/tests/test_basis.py @@ -0,0 +1,38 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Basis unit tests. +Author: Frank Dellaert & Gerry Chen (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import B + +class TestBasis(GtsamTestCase): + def test_fit_basis(self): + f = lambda x: x # line y = x + N = 2 + datax = [0., 0.5, 0.75] + interpx = np.linspace(0., 1., 10) + noise = gtsam.noiseModel.Unit.Create(1) + def testBasis(fitter, basis, f=f): + data = {x: f(x) for x in datax} + fit = fitter(N, data, noise) + coeff = fit.parameters() + interpy = basis.WeightMatrix(N, interpx) @ coeff + np.testing.assert_almost_equal(interpy, np.array([f(x) for x in interpx]), decimal=7) + testBasis(gtsam.FitBasisFourierBasis, gtsam.FourierBasis, f=lambda x: 0.7*np.cos(x)) + testBasis(gtsam.FitBasisChebyshev1Basis, gtsam.Chebyshev1Basis) + testBasis(gtsam.FitBasisChebyshev2Basis, gtsam.Chebyshev2Basis) + testBasis(gtsam.FitBasisChebyshev2, gtsam.Chebyshev2) + +if __name__ == "__main__": + unittest.main() From b99bf4e92912f4ad0020f79d6b222a3d6593514f Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 27 Aug 2021 11:23:38 -0400 Subject: [PATCH 013/169] add and fix constructor argument order --- python/gtsam/preamble/basis.h | 2 ++ python/gtsam/tests/test_basis.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h index d07a75f6f..56a07cfdd 100644 --- a/python/gtsam/preamble/basis.h +++ b/python/gtsam/preamble/basis.h @@ -10,3 +10,5 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +#include diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 944bdd9f3..0df9a80b0 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -25,7 +25,7 @@ class TestBasis(GtsamTestCase): noise = gtsam.noiseModel.Unit.Create(1) def testBasis(fitter, basis, f=f): data = {x: f(x) for x in datax} - fit = fitter(N, data, noise) + fit = fitter(data, noise, N) coeff = fit.parameters() interpy = basis.WeightMatrix(N, interpx) @ coeff np.testing.assert_almost_equal(interpy, np.array([f(x) for x in interpx]), decimal=7) From 2f6b8d6314ede652df5cb2125cd9d41490da98a9 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 27 Aug 2021 12:01:06 -0400 Subject: [PATCH 014/169] docstrings and formatting --- python/gtsam/tests/test_basis.py | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 0df9a80b0..8d4039249 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -16,23 +16,41 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase from gtsam.symbol_shorthand import B + class TestBasis(GtsamTestCase): + """Tests Basis module python bindings + """ def test_fit_basis(self): + """Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and + Chebyshev2. + It tests FitBasis by fitting to a ground-truth function that can be represented exactly in + the basis, then checking that the regression (fit result) matches the function. For the + Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is + used. + """ f = lambda x: x # line y = x N = 2 datax = [0., 0.5, 0.75] interpx = np.linspace(0., 1., 10) noise = gtsam.noiseModel.Unit.Create(1) + + def evaluate(basis, fitparams, x): + # until wrapper for Basis functors are ready, this is how to evaluate a basis function. + return basis.WeightMatrix(N, x) @ fitparams + def testBasis(fitter, basis, f=f): + # test a basis by checking that the fit result matches the function at x-values interpx. data = {x: f(x) for x in datax} fit = fitter(data, noise, N) coeff = fit.parameters() - interpy = basis.WeightMatrix(N, interpx) @ coeff + interpy = evaluate(basis, coeff, interpx) np.testing.assert_almost_equal(interpy, np.array([f(x) for x in interpx]), decimal=7) - testBasis(gtsam.FitBasisFourierBasis, gtsam.FourierBasis, f=lambda x: 0.7*np.cos(x)) + + testBasis(gtsam.FitBasisFourierBasis, gtsam.FourierBasis, f=lambda x: 0.7 * np.cos(x)) testBasis(gtsam.FitBasisChebyshev1Basis, gtsam.Chebyshev1Basis) testBasis(gtsam.FitBasisChebyshev2Basis, gtsam.Chebyshev2Basis) testBasis(gtsam.FitBasisChebyshev2, gtsam.Chebyshev2) + if __name__ == "__main__": unittest.main() From 286b2fa4b0acd8611be292e4e7a9e8b29a144a5f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Aug 2021 15:37:06 -0400 Subject: [PATCH 015/169] fix python tests and make verbose so they are easy to debug --- python/CMakeLists.txt | 5 +++-- python/gtsam/tests/test_basis.py | 6 ++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index a5fdc80a6..4254a21c6 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -166,5 +166,6 @@ add_custom_target( COMMAND ${CMAKE_COMMAND} -E env # add package to python path so no need to install "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} -m unittest discover -s "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests" - DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}) + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 8d4039249..3af0a87f1 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -18,10 +18,12 @@ from gtsam.symbol_shorthand import B class TestBasis(GtsamTestCase): - """Tests Basis module python bindings + """ + Tests Basis module python bindings. """ def test_fit_basis(self): - """Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and + """ + Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and Chebyshev2. It tests FitBasis by fitting to a ground-truth function that can be represented exactly in the basis, then checking that the regression (fit result) matches the function. For the From f712d6215086b8508062df29658337af62ee3d11 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 16:30:25 -0400 Subject: [PATCH 016/169] Added override --- .../slam/ProjectionFactorRollingShutter.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 5827a538c..ed56ad8f3 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -129,7 +129,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 (gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -140,7 +140,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3measured_.size() != + this->cameras(values).size()) // 1 observation per interpolated camera + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); if (!this->result_) { // failed: return "empty/zero" Hessian if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, 0.0); + for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) v = Vector::Zero(DimPose); + return boost::make_shared>(this->keys_, + Gs, gs, 0.0); } else { - throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); } } // compute Jacobian given triangulated 3D Point From 2b3709ec73496f74e98b11b26024ccfc2a35fede Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:19:53 -0400 Subject: [PATCH 018/169] Got rid of SchurComplementAndRearrangeBlocks_3_12_6 --- gtsam/geometry/CameraSet.h | 13 ------------- gtsam/geometry/tests/testCameraSet.cpp | 5 ++--- .../slam/SmartProjectionPoseFactorRollingShutter.h | 2 +- 3 files changed, 3 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index b8fbb4200..58122e33e 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -286,19 +286,6 @@ public: return augmentedHessianUniqueKeys; } - /** - * non-templated version of function above - */ - static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( - const std::vector, - Eigen::aligned_allocator > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, - jacobianKeys, - hessianKeys); - } - /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index e583c0e80..144f28314 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -185,9 +185,8 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { // Actual SymmetricBlockMatrix augmentedHessianBM = - Set::SchurComplementAndRearrangeBlocks_3_12_6(Fs, E, P, b, - nonuniqueKeys, - uniqueKeys); + Set::SchurComplementAndRearrangeBlocks<3, 12, 6>( + Fs, E, P, b, nonuniqueKeys, uniqueKeys); Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); // Expected diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 3624e3f8e..236ae6c95 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -363,7 +363,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor( Fs, E, P, b, nonuniqueKeys, this->keys_); return boost::make_shared < RegularHessianFactor From d0505d4bc3767b79aebfc28c7fc59258f309f90c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:20:08 -0400 Subject: [PATCH 019/169] Check equals not assert_equal --- ...stSmartProjectionPoseFactorRollingShutter.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index bf8a8c4ab..a7441e170 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -138,8 +138,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(assert_equal(*factor1, *factor2)); - EXPECT(assert_equal(*factor1, *factor3)); + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); } { // create slightly different factors (different keys) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -147,8 +147,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } { // create slightly different factors (different extrinsics) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -156,8 +156,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } { // create slightly different factors (different interp factors) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -165,8 +165,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } } From bafcde9ee195216a0b4bae1381bfa74e59f45fad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:36:14 -0400 Subject: [PATCH 020/169] Google-style formatting in new files. --- gtsam/geometry/CameraSet.h | 41 +- .../slam/ProjectionFactorRollingShutter.cpp | 39 +- .../slam/ProjectionFactorRollingShutter.h | 173 +++--- .../SmartProjectionPoseFactorRollingShutter.h | 230 ++++---- .../testProjectionFactorRollingShutter.cpp | 258 +++++---- ...martProjectionPoseFactorRollingShutter.cpp | 494 +++++++++++------- 6 files changed, 695 insertions(+), 540 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 58122e33e..143d4bc3c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -218,48 +218,52 @@ public: size_t nrNonuniqueKeys = jacobianKeys.size(); size_t nrUniqueKeys = hessianKeys.size(); - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs,E,P,b); + // Marginalize point: note - we reuse the standard SchurComplement function. + SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs, E, P, b); - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term + // Pack into an Hessian factor, allow space for b term. + std::vector dims(nrUniqueKeys + 1); std::fill(dims.begin(), dims.end() - 1, NDD); dims.back() = 1; SymmetricBlockMatrix augmentedHessianUniqueKeys; - // here we have to deal with the fact that some blocks may share the same keys - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + // Deal with the fact that some blocks may share the same keys. + if (nrUniqueKeys == nrNonuniqueKeys) { + // Case when there is 1 calibration key per camera: augmentedHessianUniqueKeys = SymmetricBlockMatrix( dims, Matrix(augmentedHessian.selfadjointView())); - } else { // if multiple cameras share a calibration we have to rearrange - // the results of the Schur complement matrix - std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + } else { + // When multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix. + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // includes b std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); nonuniqueDims.back() = 1; augmentedHessian = SymmetricBlockMatrix( nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + // Get map from key to location in the new augmented Hessian matrix (the + // one including only unique keys). std::map keyToSlotMap; for (size_t k = 0; k < nrUniqueKeys; k++) { keyToSlotMap[hessianKeys[k]] = k; } - // initialize matrix to zero + // Initialize matrix to zero. augmentedHessianUniqueKeys = SymmetricBlockMatrix( dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); - // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // and populates an Hessian that only includes the unique keys (that is what we want to return) + // Add contributions for each key: note this loops over the hessian with + // nonUnique keys (augmentedHessian) and populates an Hessian that only + // includes the unique keys (that is what we want to return). for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows Key key_i = jacobianKeys.at(i); - // update information vector + // Update information vector. augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i], nrUniqueKeys, augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - // update blocks + // Update blocks. for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols Key key_j = jacobianKeys.at(j); if (i == j) { @@ -273,13 +277,14 @@ public: } else { augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); } } } } - // update bottom right element of the matrix + + // Update bottom right element of the matrix. augmentedHessianUniqueKeys.updateDiagonalBlock( nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); } diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp index 5fc1c05eb..c92a13daf 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -23,7 +23,6 @@ Vector ProjectionFactorRollingShutter::evaluateError( const Pose3& pose_a, const Pose3& pose_b, const Point3& point, boost::optional H1, boost::optional H2, boost::optional H3) const { - try { Pose3 pose = interpolate(pose_a, pose_b, alpha_, H1, H2); gtsam::Matrix Hprj; @@ -32,12 +31,10 @@ Vector ProjectionFactorRollingShutter::evaluateError( gtsam::Matrix HbodySensor; PinholeCamera camera( pose.compose(*body_P_sensor_, HbodySensor), *K_); - Point2 reprojectionError( - camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) - *H1 = Hprj * HbodySensor * (*H1); - if (H2) - *H2 = Hprj * HbodySensor * (*H2); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + measured_); + if (H1) *H1 = Hprj * HbodySensor * (*H1); + if (H2) *H2 = Hprj * HbodySensor * (*H2); return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); @@ -45,29 +42,23 @@ Vector ProjectionFactorRollingShutter::evaluateError( } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError( - camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) - *H1 = Hprj * (*H1); - if (H2) - *H2 = Hprj * (*H2); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + measured_); + if (H1) *H1 = Hprj * (*H1); + if (H2) *H2 = Hprj * (*H2); return reprojectionError; } } catch (CheiralityException& e) { - if (H1) - *H1 = Matrix::Zero(2, 6); - if (H2) - *H2 = Matrix::Zero(2, 6); - if (H3) - *H3 = Matrix::Zero(2, 3); + if (H1) *H1 = Matrix::Zero(2, 6); + if (H2) *H2 = Matrix::Zero(2, 6); + if (H3) *H3 = Matrix::Zero(2, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " - << DefaultKeyFormatter(this->key2()) << " moved behind camera " - << DefaultKeyFormatter(this->key1()) << std::endl; - if (throwCheirality_) - throw CheiralityException(this->key2()); + << DefaultKeyFormatter(this->key2()) << " moved behind camera " + << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) throw CheiralityException(this->key2()); } return Vector2::Constant(2.0 * K_->fx()); } -} //namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index ed56ad8f3..c92653c13 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -17,41 +17,47 @@ #pragma once -#include -#include -#include #include +#include +#include +#include + #include namespace gtsam { /** - * Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. The calibration is known here. - * This version takes rolling shutter information into account as follows: consider two consecutive poses A and B, - * and a Point2 measurement taken starting at time A using a rolling shutter camera. - * Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) - * corresponding to the time of exposure of the row of the image the pixel belongs to. - * Let us define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by - * the alpha to project the corresponding landmark to Point2. + * Non-linear factor for 2D projection measurement obtained using a rolling + * shutter camera. The calibration is known here. This version takes rolling + * shutter information into account as follows: consider two consecutive poses A + * and B, and a Point2 measurement taken starting at time A using a rolling + * shutter camera. Pose A has timestamp t_A, and Pose B has timestamp t_B. The + * Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) corresponding + * to the time of exposure of the row of the image the pixel belongs to. Let us + * define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose + * interpolated between A and B by the alpha to project the corresponding + * landmark to Point2. * @addtogroup SLAM */ -class ProjectionFactorRollingShutter : public NoiseModelFactor3 { +class ProjectionFactorRollingShutter + : public NoiseModelFactor3 { protected: - // Keep a copy of measurement and calibration for I/O - Point2 measured_; ///< 2D measurement - double alpha_; ///< interpolation parameter in [0,1] corresponding to the point2 measurement + Point2 measured_; ///< 2D measurement + double alpha_; ///< interpolation parameter in [0,1] corresponding to the + ///< point2 measurement boost::shared_ptr K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + boost::optional + body_P_sensor_; ///< The pose of the sensor in the body frame // verbosity handling for Cheirality Exceptions - bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: + ///< false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions + ///< (default: false) public: - /// shorthand for base class type typedef NoiseModelFactor3 Base; @@ -66,72 +72,72 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, - boost::optional body_P_sensor = - boost::none) + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), - verboseCheirality_(false) { - } + verboseCheirality_(false) {} /** * Constructor with exception-handling flags - * @param measured is the 2-dimensional pixel location of point in the image (the measurement) + * @param measured is the 2-dimensional pixel location of point in the image + * (the measurement) * @param alpha in [0,1] is the rolling shutter parameter for the measurement * @param model is the noise model * @param poseKey_a is the key of the first camera * @param poseKey_b is the key of the second camera * @param pointKey is the key of the landmark * @param K shared pointer to the constant calibration - * @param throwCheirality determines whether Cheirality exceptions are rethrown - * @param verboseCheirality determines whether exceptions are printed for Cheirality - * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param throwCheirality determines whether Cheirality exceptions are + * rethrown + * @param verboseCheirality determines whether exceptions are printed for + * Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) */ - ProjectionFactorRollingShutter(const Point2& measured, double alpha, - const SharedNoiseModel& model, Key poseKey_a, - Key poseKey_b, Key pointKey, - const boost::shared_ptr& K, - bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = - boost::none) + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, bool throwCheirality, + bool verboseCheirality, + boost::optional body_P_sensor = boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(throwCheirality), - verboseCheirality_(verboseCheirality) { - } + verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~ProjectionFactorRollingShutter() { - } + virtual ~ProjectionFactorRollingShutter() {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -139,8 +145,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3>& world_P_body_key_pairs, const std::vector& alphas, const std::vector>& Ks, - const std::vector body_P_sensors) { + const std::vector& body_P_sensors) { assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == alphas.size()); assert(world_P_body_key_pairs.size() == Ks.size()); @@ -151,20 +169,24 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor>& world_P_body_key_pairs, const std::vector& alphas, - const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { + const boost::shared_ptr& K, + const Pose3& body_P_sensor = Pose3::identity()) { assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == alphas.size()); for (size_t i = 0; i < measurements.size(); i++) { @@ -174,39 +196,37 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor> calibration() const { + const std::vector>& calibration() const { return K_all_; } - /// return (for each observation) the keys of the pair of poses from which we interpolate - const std::vector> world_P_body_key_pairs() const { + /// return (for each observation) the keys of the pair of poses from which we + /// interpolate + const std::vector>& world_P_body_key_pairs() const { return world_P_body_key_pairs_; } /// return the interpolation factors alphas - const std::vector alphas() const { - return alphas_; - } + const std::vector& alphas() const { return alphas_; } /// return the extrinsic camera calibration body_P_sensors - const std::vector body_P_sensors() const { - return body_P_sensors_; - } + const std::vector& body_P_sensors() const { return body_P_sensors_; } /** * print * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override { + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << " pose1 key: " - << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; + << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " - << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; + << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); @@ -217,17 +237,20 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor* e = - dynamic_cast*>(&p); + dynamic_cast*>( + &p); double keyPairsEqual = true; - if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ - for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){ + if (this->world_P_body_key_pairs_.size() == + e->world_P_body_key_pairs().size()) { + for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) { const Key key1own = world_P_body_key_pairs_[k].first; const Key key1e = e->world_P_body_key_pairs()[k].first; const Key key2own = world_P_body_key_pairs_[k].second; const Key key2e = e->world_P_body_key_pairs()[k].second; - if ( !(key1own == key1e) || !(key2own == key2e) ){ - keyPairsEqual = false; break; + if (!(key1own == key1e) || !(key2own == key2e)) { + keyPairsEqual = false; + break; } } } else { @@ -235,18 +258,19 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactorbody_P_sensors_.size() == e->body_P_sensors().size()){ - for(size_t i=0; i< this->body_P_sensors_.size(); i++){ - if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){ - extrinsicCalibrationEqual = false; break; + if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { + for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { + if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { + extrinsicCalibrationEqual = false; + break; } } } else { extrinsicCalibrationEqual = false; } - return e && Base::equals(p, tol) && K_all_ == e->calibration() - && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) && K_all_ == e->calibration() && + alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -264,12 +288,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactormeasured_.size(); - E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) + E = Matrix::Zero(2 * numViews, + 3); // a Point2 for each view (point jacobian) b = Vector::Zero(2 * numViews); // a Point2 for each view // intermediate Jacobians Eigen::Matrix dProject_dPoseCam; Eigen::Matrix dInterpPose_dPoseBody1, - dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; Eigen::Matrix Ei; for (size_t i = 0; i < numViews; i++) { // for each camera/measurement @@ -285,14 +310,16 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor camera(w_P_cam, *K_all_[i]); // get jacobians and error vector for current measurement - Point2 reprojectionError_i = Point2( - camera.project(*this->result_, dProject_dPoseCam, Ei) - - this->measured_.at(i)); + Point2 reprojectionError_i = + Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) - + this->measured_.at(i)); Eigen::Matrix J; // 2 x 12 - J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose - * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) - J.block(0, 6, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose - * dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) + J.block(0, 0, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) + J.block(0, 6, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) // fit into the output structures Fs.push_back(J); @@ -353,21 +380,23 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor( Fs, E, P, b, nonuniqueKeys, this->keys_); - return boost::make_shared < RegularHessianFactor - > (this->keys_, augmentedHessianUniqueKeys); + return boost::make_shared>( + this->keys_, augmentedHessianUniqueKeys); } /** @@ -376,7 +405,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactoractive(values)) { return this->totalReprojectionError(this->cameras(values)); - } else { // else of active flag + } else { // else of active flag return 0.0; } } @@ -396,10 +425,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor(world_P_body_key_pairs_[i].first); - const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + const Pose3& w_P_body1 = + values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = + values.at(world_P_body_key_pairs_[i].second); double interpolationFactor = alphas_[i]; - const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); + const Pose3& w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor); const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& w_P_cam = w_P_body.compose(body_P_cam); cameras.emplace_back(w_P_cam, K_all_[i]); @@ -408,44 +440,46 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor linearizeDamped( const Values& values, const double lambda = 0.0) const { - // depending on flag set on construction we may linearize to different linear factors + // depending on flag set on construction we may linearize to different + // linear factors switch (this->params_.linearizationMode) { case HESSIAN: return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); + "SmartProjectionPoseFactorRollingShutter: unknown linearization " + "mode"); } } /// linearize - boost::shared_ptr linearize(const Values& values) const - override { + boost::shared_ptr linearize( + const Values& values) const override { return this->linearizeDamped(values); } private: /// Serialization function friend class boost::serialization::access; - template + template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(K_all_); } - }; // end of class declaration /// traits -template -struct traits > : -public Testable > { -}; +template +struct traits> + : public Testable> {}; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 943e350d4..161c9aa55 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -16,34 +16,33 @@ * @date July 2021 */ -#include +#include #include -#include -#include +#include #include #include -#include -#include #include - -#include +#include +#include +#include +#include using namespace std::placeholders; using namespace std; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w=640,h=480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static double fov = 60; // degrees +static size_t w = 640, h = 480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); // Create a noise model for the pixel error static SharedNoiseModel model(noiseModel::Unit::Create(2)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; using symbol_shorthand::T; +using symbol_shorthand::X; // Convenience to define common variables across many tests static Key poseKey1(X(1)); @@ -51,74 +50,84 @@ static Key poseKey2(X(2)); static Key pointKey(L(1)); static double interp_params = 0.5; static Point2 measurement(323.0, 240.0); -static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Constructor) { - ProjectionFactorRollingShutter factor(measurement, interp_params, model, poseKey1, poseKey2, pointKey, K); -} - -/* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ConstructorWithTransform) { +TEST(ProjectionFactorRollingShutter, Constructor) { ProjectionFactorRollingShutter factor(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K); } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Equals ) { - { // factors are equal - ProjectionFactorRollingShutter factor1(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); +TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Equals) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal (keys are different) - ProjectionFactorRollingShutter factor1(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, interp_params, - model, poseKey1, poseKey1, pointKey, K); - CHECK(!assert_equal(factor1, factor2)); // not equal + { // factors are NOT equal (keys are different) + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey1, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal } - { // factors are NOT equal (different interpolation) - ProjectionFactorRollingShutter factor1(measurement, 0.1, - model, poseKey1, poseKey1, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, 0.5, - model, poseKey1, poseKey2, pointKey, K); - CHECK(!assert_equal(factor1, factor2)); // not equal + { // factors are NOT equal (different interpolation) + ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1, + poseKey1, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1, + poseKey2, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { - { // factors are equal +TEST(ProjectionFactorRollingShutter, EqualsWithTransform) { + { // factors are equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal + { // factors are NOT equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); - Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + Pose3 body_P_sensor2( + Rot3::RzRyRx(0.0, 0.0, 0.0), + Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor2); + poseKey1, poseKey2, pointKey, K, + body_P_sensor2); CHECK(!assert_equal(factor1, factor2)); } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Error ) { +TEST(ProjectionFactorRollingShutter, Error) { { // Create the factor with a measurement that is 3 pixels off in x // Camera pose corresponds to the first camera double t = 0.0; - ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); // Set the linearization point - Pose3 pose1(Rot3(), Point3(0,0,-6)); - Pose3 pose2(Rot3(), Point3(0,0,-4)); + Pose3 pose1(Rot3(), Point3(0, 0, -6)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); Point3 point(0.0, 0.0, 0.0); // Use the factor to calculate the error @@ -134,11 +143,12 @@ TEST( ProjectionFactorRollingShutter, Error ) { // Create the factor with a measurement that is 3 pixels off in x // Camera pose is actually interpolated now double t = 0.5; - ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); // Set the linearization point - Pose3 pose1(Rot3(), Point3(0,0,-8)); - Pose3 pose2(Rot3(), Point3(0,0,-4)); + Pose3 pose1(Rot3(), Point3(0, 0, -8)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); Point3 point(0.0, 0.0, 0.0); // Use the factor to calculate the error @@ -153,15 +163,16 @@ TEST( ProjectionFactorRollingShutter, Error ) { { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -175,19 +186,20 @@ TEST( ProjectionFactorRollingShutter, Error ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { +TEST(ProjectionFactorRollingShutter, ErrorWithTransform) { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); - Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); - PinholeCamera camera(poseInterp*body_P_sensor3, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -200,18 +212,19 @@ TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Jacobian ) { +TEST(ProjectionFactorRollingShutter, Jacobian) { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -222,22 +235,25 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -245,19 +261,20 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { +TEST(ProjectionFactorRollingShutter, JacobianWithTransform) { // Create measurement by projecting 3D landmark double t = 0.6; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); - Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); - PinholeCamera camera(poseInterp*body_P_sensor3, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -268,22 +285,25 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -291,41 +311,48 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, cheirality ) { +TEST(ProjectionFactorRollingShutter, cheirality) { // Create measurement by projecting 3D landmark behind camera double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera + Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - Point2 measured = Point2(0.0,0.0); // project would throw an exception - { // check that exception is thrown if we set throwCheirality = true + Point2 measured = Point2(0.0, 0.0); // project would throw an exception + { // check that exception is thrown if we set throwCheirality = true bool throwCheirality = true; bool verboseCheirality = true; - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point), CheiralityException); } - { // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct - bool throwCheirality = false; // default - bool verboseCheirality = false; // default - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + { // check that exception is NOT thrown if we set throwCheirality = false, + // and outputs are correct + bool throwCheirality = false; // default + bool verboseCheirality = false; // default + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); // Use the factor to calculate the error Matrix H1Actual, H2Actual, H3Actual; - Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual)); + Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, + H2Actual, H3Actual)); // The expected error is zero - Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera + Vector expectedError = Vector2::Constant( + 2.0 * K->fx()); // this is what we return when point is behind camera // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); - CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5)); - CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5)); - CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5)); } #else { @@ -333,7 +360,8 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -344,22 +372,25 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -368,8 +399,9 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { #endif } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index a7441e170..0b94d2c3f 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -16,17 +16,19 @@ * @date July 2021 */ -#include "gtsam/slam/tests/smartFactorScenarios.h" -#include -#include -#include -#include -#include +#include #include #include -#include +#include +#include +#include +#include +#include + #include #include + +#include "gtsam/slam/tests/smartFactorScenarios.h" #define DISABLE_TIMING using namespace gtsam; @@ -39,8 +41,8 @@ static const double sigma = 0.1; static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -48,8 +50,8 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Symbol x4('X', 4); static Symbol l0('L', 0); -static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), - Point3(0.1, 0.0, 0.0)); +static Pose3 body_P_sensor = + Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0)); static Point2 measurement1(323.0, 240.0); static Point2 measurement2(200.0, 220.0); @@ -64,38 +66,39 @@ static double interp_factor3 = 0.5; namespace vanillaPoseRS { typedef PinholePose Camera; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); -Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); -Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); -Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above, level_pose, interp_factor3); Camera cam1(interp_pose1, sharedK); Camera cam2(interp_pose2, sharedK); Camera cam3(interp_pose3, sharedK); -} +} // namespace vanillaPoseRS LevenbergMarquardtParams lmParams; -typedef SmartProjectionPoseFactorRollingShutter< PinholePose > SmartFactorRS; +typedef SmartProjectionPoseFactorRollingShutter> + SmartFactorRS; /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { SmartProjectionParams params; params.setRankTolerance(rankTol); SmartFactorRS factor1(model, params); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, add) { +TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPose; SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { +TEST(SmartProjectionPoseFactorRollingShutter, Equals) { using namespace vanillaPose; // create fake measurements @@ -104,10 +107,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { measurements.push_back(measurement2); measurements.push_back(measurement3); - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x4)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x4)); std::vector> intrinsicCalibrations; intrinsicCalibrations.push_back(sharedK); @@ -126,13 +129,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); - factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, + extrinsicCalibrations); // create by adding a batch of measurements with a single calibrations SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); - { // create equal factors and show equal returns true + { // create equal factors and show equal returns true SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); @@ -141,28 +145,34 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor3)); } - { // create slightly different factors (different keys) and show equal returns false + { // create slightly different factors (different keys) and show equal + // returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different extrinsics) and show equal returns false + { // create slightly different factors (different extrinsics) and show equal + // returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, + body_P_sensor * body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different interp factors) and show equal returns false + { // create slightly different factors (different interp factors) and show + // equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); @@ -170,13 +180,16 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } } -static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated -static const int ZDim = 2; ///< Measurement dimension (Point2) -typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) -typedef std::vector > FBlocks; // vector of F blocks +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from + ///< which the observation pose is interpolated +static const int ZDim = 2; ///< Measurement dimension (Point2) +typedef Eigen::Matrix + MatrixZD; // F blocks (derivatives wrt camera) +typedef std::vector> + FBlocks; // vector of F blocks /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { +TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { using namespace vanillaPoseRS; // Project two landmarks into two cameras @@ -188,7 +201,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, level_pose); values.insert(x2, pose_right); values.insert(x3, pose_above); @@ -200,41 +213,56 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { // Check triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal( + landmark1, + *point)); // check triangulation result matches expected 3D landmark // Check Jacobians // -- actual Jacobians FBlocks actualFs; Matrix actualE; Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorId); Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); - EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + Vector expectedb1 = factor1.evaluateError( + level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, body_P_sensorId); Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); - EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + Vector expectedb2 = factor2.evaluateError( + pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { // also includes non-identical extrinsic calibration using namespace vanillaPoseRS; @@ -246,9 +274,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { SmartFactorRS factor(model); factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); - factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, + body_P_sensorNonId); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, level_pose); values.insert(x2, pose_right); values.insert(x3, pose_above); @@ -256,7 +285,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { // Perform triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid Point3 landmarkNoisy = *point; // Check Jacobians @@ -264,32 +293,48 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { FBlocks actualFs; Matrix actualE; Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorNonId); Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); - EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + Vector expectedb1 = + factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, + expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, + body_P_sensorNonId); Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); - EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + Vector expectedb2 = + factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, + expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); // Check errors - double actualError = factor.error(values); // from smart factor + double actualError = factor.error(values); // from smart factor NonlinearFactorGraph nfg; nfg.add(factor1); nfg.add(factor2); @@ -299,8 +344,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { - +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -310,10 +354,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); @@ -344,20 +388,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -366,11 +412,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { - // here we replicate a test in SmartProjectionPoseFactor by setting interpolation - // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements) - // Note: this is a quite extreme test since in typical camera you would not have more than - // 1 measurement per landmark at each interpolated pose +TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { + // here we replicate a test in SmartProjectionPoseFactor by setting + // interpolation factors to 0 and 1 (such that the rollingShutter measurements + // falls back to standard pixel measurements) Note: this is a quite extreme + // test since in typical camera you would not have more than 1 measurement per + // landmark at each interpolated pose using namespace vanillaPose; // Default cameras for simple derivatives @@ -423,7 +470,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { perturbedDelta.insert(x2, delta); double expectedError = 2500; - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + // After eliminating the point, A1 and A2 contain 2-rank information on + // cameras: Matrix16 A1, A2; A1 << -10, 0, 0, 0, 1, 0; A2 << 10, 0, 1, 0, -1, 0; @@ -449,8 +497,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); @@ -458,7 +506,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -478,7 +526,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - double excludeLandmarksFutherThanDist = 1e10; //very large + double excludeLandmarksFutherThanDist = 1e10; // very large SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); @@ -486,13 +534,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -509,7 +557,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); // Optimization should correct 3rd pose @@ -520,7 +569,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_landmarkDistance) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -548,13 +598,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -571,10 +621,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - // All factors are disabled (due to the distance threshold) and pose should remain where it is + // All factors are disabled (due to the distance threshold) and pose should + // remain where it is Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); @@ -582,7 +634,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_dynamicOutlierRejection) { using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -594,7 +647,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); - measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10); // add outlier + measurements_lmk4.at(0) = + measurements_lmk4.at(0) + Point2(10, 10); // add outlier // create inputs std::vector> key_pairs; @@ -608,7 +662,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie interp_factors.push_back(interp_factor3); double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error SmartProjectionParams params; params.setRankTolerance(1.0); @@ -640,12 +695,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x2, pose_right, noisePrior); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in + Pose3 noise_pose = Pose3( + Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, + 0.01)); // smaller noise, otherwise outlier rejection will kick in Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); // Optimization should correct 3rd pose @@ -656,8 +714,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { - +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -683,10 +741,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -695,8 +758,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -714,46 +777,52 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, model, x1, x2, l0, sharedK); Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual; ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual; ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual; // whiten - F = (1/sigma) * F; - E = (1/sigma) * E; - b = (1/sigma) * b; + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -771,9 +840,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { - // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark - // at a single pose, a setup that occurs in multi-camera systems +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -783,7 +854,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // create redundant measurements: Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement // create inputs std::vector> key_pairs; @@ -799,17 +871,23 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli interp_factors.push_back(interp_factor1); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, + sharedK); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -818,8 +896,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -828,62 +906,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli TriangulationResult point = smartFactor1->point(); EXPECT(point.valid()); // check triangulated point is valid - // Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians + // Use standard ProjectionFactorRollingShutter factor to calculate the + // Jacobians Matrix F = Matrix::Zero(2 * 4, 6 * 3); Matrix E = Matrix::Zero(2 * 4, 3); Vector b = Vector::Zero(8); // create projection factors rolling shutter - ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1, - model, x1, x2, l0, sharedK); + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], + interp_factor1, model, x1, x2, l0, + sharedK); Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual; - ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2, - model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], + interp_factor2, model, x2, x3, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual; - ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3, - model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], + interp_factor3, model, x3, x1, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual; - ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1, - model, x1, x2, l0, sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], + interp_factor1, model, x1, x2, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(6, 0) = H1Actual; F.block<2, 6>(6, 6) = H2Actual; E.block<2, 3>(6, 0) = H3Actual; // whiten - F = (1/sigma) * F; - E = (1/sigma) * E; - b = (1/sigma) * b; + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -902,8 +992,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) { - +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_measurementsFromSamePose) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -913,27 +1003,32 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - // For first factor, we create redundant measurement (taken by the same keys as factor 1, to - // make sure the redundancy in the keys does not create problems) + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement - std::vector> key_pairs_redundant = key_pairs; - key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back( + key_pairs.at(0)); // we readd the first pair of keys std::vector interp_factors_redundant = interp_factors; - interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor + interp_factors_redundant.push_back( + interp_factors.at(0)); // we readd the first interp factor SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, + interp_factors_redundant, sharedK); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); @@ -956,20 +1051,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -980,11 +1077,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF #ifndef DISABLE_TIMING #include // -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) -//| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: +// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 +// children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, timing ) { - +TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; // Default cameras for simple derivatives @@ -1007,14 +1104,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { size_t nrTests = 1000; - for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); Values values; values.insert(x1, pose1); @@ -1024,7 +1121,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { gttoc_(SF_RS_LINEARIZE); } - for(size_t i = 0; iadd(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1046,4 +1143,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From 9798bfa8151788b31e225be9acce0ca4c4d30b10 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:46:12 -0400 Subject: [PATCH 021/169] Cleaned up interpolate --- gtsam/base/Lie.h | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 34422edd7..ac7c2a9a5 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -320,28 +320,28 @@ T expm(const Vector& x, int K=7) { } /** - * Linear interpolation between X and Y by coefficient t (typically t \in [0,1], - * but can also be used to extrapolate before pose X or after pose Y), with optional jacobians. + * Linear interpolation between X and Y by coefficient t. Typically t \in [0,1], + * but can also be used to extrapolate before pose X or after pose Y. */ template T interpolate(const T& X, const T& Y, double t, typename MakeOptionalJacobian::type Hx = boost::none, typename MakeOptionalJacobian::type Hy = boost::none) { - if (Hx || Hy) { - typename traits::TangentVector log_Xinv_Y; typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; + const T between = + traits::Between(X, Y, between_H_x); // between_H_y = identity + typename traits::TangentVector delta = traits::Logmap(between, log_H); + const T Delta = traits::Expmap(t * delta, exp_H); + const T result = traits::Compose( + X, Delta, compose_H_x); // compose_H_xinv_y = identity - T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity - log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); - Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); - Xinv_Y = traits::Compose(X, Xinv_Y, compose_H_x); // compose_H_xinv_y = identity - - if(Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; - if(Hy) *Hy = t * exp_H * log_H; - return Xinv_Y; + if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if (Hy) *Hy = t * exp_H * log_H; + return result; } - return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); + return traits::Compose( + X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } /** From 066bd0f05bb91a0f7aedfbc7a382bfef2acf0869 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Aug 2021 20:03:03 -0400 Subject: [PATCH 022/169] verbose python testing --- .github/scripts/python.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 22098ec08..3f5701281 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -85,4 +85,4 @@ make -j2 install cd $GITHUB_WORKSPACE/build/python $PYTHON setup.py install --user --prefix= cd $GITHUB_WORKSPACE/python/gtsam/tests -$PYTHON -m unittest discover +$PYTHON -m unittest discover -v From e9641ba26b1214529af00dc82515ab4be6eb1636 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 28 Aug 2021 20:09:24 -0400 Subject: [PATCH 023/169] Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors # Conflicts: # gtsam/geometry/CameraSet.h --- doc/CMakeLists.txt | 25 +- gtsam/CMakeLists.txt | 1 + gtsam/base/Lie.h | 26 +- gtsam/basis/Basis.h | 507 +++++++++++++++++ gtsam/basis/BasisFactors.h | 424 +++++++++++++++ gtsam/basis/CMakeLists.txt | 6 + gtsam/basis/Chebyshev.cpp | 98 ++++ gtsam/basis/Chebyshev.h | 109 ++++ gtsam/basis/Chebyshev2.cpp | 205 +++++++ gtsam/basis/Chebyshev2.h | 148 +++++ gtsam/basis/FitBasis.h | 99 ++++ gtsam/basis/Fourier.h | 108 ++++ gtsam/basis/ParameterMatrix.h | 215 ++++++++ gtsam/basis/basis.i | 146 +++++ gtsam/basis/tests/CMakeLists.txt | 1 + gtsam/basis/tests/testChebyshev.cpp | 236 ++++++++ gtsam/basis/tests/testChebyshev2.cpp | 435 +++++++++++++++ gtsam/basis/tests/testFourier.cpp | 254 +++++++++ gtsam/basis/tests/testParameterMatrix.cpp | 145 +++++ gtsam/geometry/CameraSet.h | 165 +++--- gtsam/geometry/tests/testCameraSet.cpp | 5 +- gtsam/nonlinear/FunctorizedFactor.h | 2 +- .../nonlinear/tests/testFunctorizedFactor.cpp | 140 ++++- gtsam/slam/SmartProjectionFactorP.h | 2 +- .../slam/ProjectionFactorRollingShutter.cpp | 39 +- .../slam/ProjectionFactorRollingShutter.h | 187 ++++--- .../SmartProjectionPoseFactorRollingShutter.h | 301 ++++++----- .../testProjectionFactorRollingShutter.cpp | 258 +++++---- ...martProjectionPoseFactorRollingShutter.cpp | 510 +++++++++++------- python/CMakeLists.txt | 1 + python/gtsam/preamble/basis.h | 12 + python/gtsam/specializations/basis.h | 12 + 32 files changed, 4132 insertions(+), 690 deletions(-) create mode 100644 gtsam/basis/Basis.h create mode 100644 gtsam/basis/BasisFactors.h create mode 100644 gtsam/basis/CMakeLists.txt create mode 100644 gtsam/basis/Chebyshev.cpp create mode 100644 gtsam/basis/Chebyshev.h create mode 100644 gtsam/basis/Chebyshev2.cpp create mode 100644 gtsam/basis/Chebyshev2.h create mode 100644 gtsam/basis/FitBasis.h create mode 100644 gtsam/basis/Fourier.h create mode 100644 gtsam/basis/ParameterMatrix.h create mode 100644 gtsam/basis/basis.i create mode 100644 gtsam/basis/tests/CMakeLists.txt create mode 100644 gtsam/basis/tests/testChebyshev.cpp create mode 100644 gtsam/basis/tests/testChebyshev2.cpp create mode 100644 gtsam/basis/tests/testFourier.cpp create mode 100644 gtsam/basis/tests/testParameterMatrix.cpp create mode 100644 python/gtsam/preamble/basis.h create mode 100644 python/gtsam/specializations/basis.h diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 7c43a8989..2218addcf 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -22,18 +22,19 @@ if (GTSAM_BUILD_DOCS) # GTSAM core subfolders set(gtsam_doc_subdirs - gtsam/base - gtsam/discrete - gtsam/geometry - gtsam/inference - gtsam/linear - gtsam/navigation - gtsam/nonlinear - gtsam/sam - gtsam/sfm - gtsam/slam - gtsam/smart - gtsam/symbolic + gtsam/base + gtsam/basis + gtsam/discrete + gtsam/geometry + gtsam/inference + gtsam/linear + gtsam/navigation + gtsam/nonlinear + gtsam/sam + gtsam/sfm + gtsam/slam + gtsam/smart + gtsam/symbolic gtsam ) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 71daf0653..e2f2ad828 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,6 +5,7 @@ project(gtsam LANGUAGES CXX) # The following variable is the master list of subdirs to add set (gtsam_subdirs base + basis geometry inference symbolic diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 34422edd7..ac7c2a9a5 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -320,28 +320,28 @@ T expm(const Vector& x, int K=7) { } /** - * Linear interpolation between X and Y by coefficient t (typically t \in [0,1], - * but can also be used to extrapolate before pose X or after pose Y), with optional jacobians. + * Linear interpolation between X and Y by coefficient t. Typically t \in [0,1], + * but can also be used to extrapolate before pose X or after pose Y. */ template T interpolate(const T& X, const T& Y, double t, typename MakeOptionalJacobian::type Hx = boost::none, typename MakeOptionalJacobian::type Hy = boost::none) { - if (Hx || Hy) { - typename traits::TangentVector log_Xinv_Y; typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; + const T between = + traits::Between(X, Y, between_H_x); // between_H_y = identity + typename traits::TangentVector delta = traits::Logmap(between, log_H); + const T Delta = traits::Expmap(t * delta, exp_H); + const T result = traits::Compose( + X, Delta, compose_H_x); // compose_H_xinv_y = identity - T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity - log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); - Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); - Xinv_Y = traits::Compose(X, Xinv_Y, compose_H_x); // compose_H_xinv_y = identity - - if(Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; - if(Hy) *Hy = t * exp_H * log_H; - return Xinv_Y; + if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if (Hy) *Hy = t * exp_H * log_H; + return result; } - return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); + return traits::Compose( + X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } /** diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h new file mode 100644 index 000000000..d8bd28c1a --- /dev/null +++ b/gtsam/basis/Basis.h @@ -0,0 +1,507 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Basis.h + * @brief Compute an interpolating basis + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +/** + * This file supports creating continuous functions `f(x;p)` as a linear + * combination of `basis functions` such as the Fourier basis on SO(2) or a set + * of Chebyshev polynomials on [-1,1]. + * + * In the expression `f(x;p)` the variable `x` is + * the continuous argument at which the function is evaluated, and `p` are + * the parameters which are coefficients of the different basis functions, + * e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial. + * However, different parameterizations are also possible. + + * The `Basis` class below defines a number of functors that can be used to + * evaluate `f(x;p)` at a given `x`, and these functors also calculate + * the Jacobian of `f(x;p)` with respect to the parameters `p`. + * This is actually the most important calculation, as it will allow GTSAM + * to optimize over the parameters `p`. + + * This functionality is implemented using the `CRTP` or "Curiously recurring + * template pattern" C++ idiom, which is a meta-programming technique in which + * the derived class is passed as a template argument to `Basis`. + * The DERIVED class is assumed to satisfy a C++ concept, + * i.e., we expect it to define the following types and methods: + + - type `Parameters`: the parameters `p` in f(x;p) + - `CalculateWeights(size_t N, double x, double a=default, double b=default)` + - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` + + where `Weights` is an N*1 row vector which defines the basis values for the + polynomial at the specified point `x`. + + E.g. A Fourier series would give the following: + - `CalculateWeights` -> For N=5, the values for the bases: + [1, cos(x), sin(x), cos(2x), sin(2x)] + - `DerivativeWeights` -> For N=5, these are: + [0, -sin(x), cos(x), -2sin(2x), 2cos(x)] + + Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are + instead the values for the Barycentric interpolation formula, since the values + at the polynomial points (e.g. Chebyshev points) define the bases. + */ + +namespace gtsam { + +using Weights = Eigen::Matrix; /* 1xN vector */ + +/** + * @brief Function for computing the kronecker product of the 1*N Weight vector + * `w` with the MxM identity matrix `I` efficiently. + * The main reason for this is so we don't need to use Eigen's Unsupported + * library. + * + * @tparam M Size of the identity matrix. + * @param w The weights of the polynomial. + * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I] + */ +template +Matrix kroneckerProductIdentity(const Weights& w) { + Matrix result(M, w.cols() * M); + result.setZero(); + + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); + } + return result; +} + +/// CRTP Base class for function bases +template +class GTSAM_EXPORT Basis { + public: + /** + * Calculate weights for all x in vector X. + * Returns M*N matrix where M is the size of the vector X, + * and N is the number of basis functions. + */ + static Matrix WeightMatrix(size_t N, const Vector& X) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i)); + return W; + } + + /** + * @brief Calculate weights for all x in vector X, with interval [a,b]. + * + * @param N The number of basis functions. + * @param X The vector for which to compute the weights. + * @param a The lower bound for the interval range. + * @param b The upper bound for the interval range. + * @return Returns M*N matrix where M is the size of the vector X. + */ + static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); + return W; + } + + /** + * An instance of an EvaluationFunctor calculates f(x;p) at a given `x`, + * applied to Parameters `p`. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific N*1 vector of Parameters, returns the scalar + * value of the function at x, possibly with Jacobians wrpt the parameters. + */ + class EvaluationFunctor { + protected: + Weights weights_; + + public: + /// For serialization + EvaluationFunctor() {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x) + : weights_(DERIVED::CalculateWeights(N, x)) {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x, double a, double b) + : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} + + /// Regular 1D evaluation + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + if (H) *H = weights_; + return (weights_ * p)(0); + } + + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + return apply(p, H); // might call apply in derived + } + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + */ + template + class VectorEvaluationFunctor : protected EvaluationFunctor { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorEvaluationFunctor() {} + + /// Default Constructor + VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + calculateJacobian(); + } + + /// Constructor, with interval [a,b] + VectorEvaluationFunctor(size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b) { + calculateJacobian(); + } + + /// M-dimensional evaluation + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + + /// c++ sugar + VectorM operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * VectorComponentFunctor computes the N-vector value for a specific row + * component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class VectorComponentFunctor : public EvaluationFunctor { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorEvaluationFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) + H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + } + + public: + /// For serialization + VectorComponentFunctor() {} + + /// Construct with row index + VectorComponentFunctor(size_t N, size_t i, double x) + : EvaluationFunctor(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + + /// Calculate component of component rowIndex_ of P + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); + } + + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + * + * The difference with the VectorEvaluationFunctor is that after computing the + * M*1 vector xi=F(x;P), with x a scalar and P the M*N parameter vector, we + * also retract xi back to the T manifold. + * For example, if T==Rot3, then we first compute a 3-vector xi using x and P, + * and then map that 3-vector xi back to the Rot3 manifold, yielding a valid + * 3D rotation. + */ + template + class ManifoldEvaluationFunctor + : public VectorEvaluationFunctor::dimension> { + enum { M = traits::dimension }; + using Base = VectorEvaluationFunctor; + + public: + /// For serialization + ManifoldEvaluationFunctor() {} + + /// Default Constructor + ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + + /// Constructor, with interval [a,b] + ManifoldEvaluationFunctor(size_t N, double x, double a, double b) + : Base(N, x, a, b) {} + + /// Manifold evaluation + T apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + // Interpolate the M-dimensional vector to yield a vector in tangent space + Eigen::Matrix xi = Base::operator()(P, H); + + // Now call retract with this M-vector, possibly with derivatives + Eigen::Matrix D_result_xi; + T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0); + + // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N) + // derivative of interpolation and D_result_xi is MxM derivative of + // retract. + if (H) *H = D_result_xi * (*H); + + // and return a T + return result; + } + + /// c++ sugar + T operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); // might call apply in derived + } + }; + + /// Base class for functors below that calculate derivative weights + class DerivativeFunctorBase { + protected: + Weights weights_; + + public: + /// For serialization + DerivativeFunctorBase() {} + + DerivativeFunctorBase(size_t N, double x) + : weights_(DERIVED::DerivativeWeights(N, x)) {} + + DerivativeFunctorBase(size_t N, double x, double a, double b) + : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * An instance of a DerivativeFunctor calculates f'(x;p) at a given `x`, + * applied to Parameters `p`. + * When given a scalar value x and a specific N*1 vector of Parameters, + * this functor returns the scalar derivative value of the function at x, + * possibly with Jacobians wrpt the parameters. + */ + class DerivativeFunctor : protected DerivativeFunctorBase { + public: + /// For serialization + DerivativeFunctor() {} + + DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {} + + DerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) {} + + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + if (H) *H = this->weights_; + return (this->weights_ * p)(0); + } + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + return apply(p, H); // might call apply in derived + } + }; + + /** + * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * + * This functor is used to evaluate the derivatives of a parameterized + * function at a given scalar value x. When given a specific M*N parameters, + * returns an M-vector the M corresponding function derivatives at x, possibly + * with Jacobians wrpt the parameters. + */ + template + class VectorDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorDerivativeFunctor() {} + + /// Default Constructor + VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + calculateJacobian(); + } + + /// Constructor, with optional interval [a,b] + VectorDerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) { + calculateJacobian(); + } + + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + /// c++ sugar + VectorM operator()( + const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * ComponentDerivativeFunctor computes the N-vector derivative for a specific + * row component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class ComponentDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorDerivativeFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < this->weights_.size(); j++) + H_(0, rowIndex_ + j * M) = this->weights_(j); + } + + public: + /// For serialization + ComponentDerivativeFunctor() {} + + /// Construct with row index + ComponentDerivativeFunctor(size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + /// Calculate derivative of component rowIndex_ of F + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * this->weights_.transpose(); + } + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + // Vector version for MATLAB :-( + static double Derivative(double x, const Vector& p, // + OptionalJacobian H = boost::none) { + return DerivativeFunctor(x)(p.transpose(), H); + } +}; + +} // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h new file mode 100644 index 000000000..0b3d4c1a0 --- /dev/null +++ b/gtsam/basis/BasisFactors.h @@ -0,0 +1,424 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BasisFactors.h + * @brief Factor definitions for various Basis functors. + * @author Varun Agrawal + * @date July 4, 2020 + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Factor for enforcing the scalar value of the polynomial BASIS + * representation at `x` is the same as the measurement `z` when using a + * pseudo-spectral parameterization. + * + * @tparam BASIS The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + EvaluationFactor() {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} + + virtual ~EvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (M, N) is equal to a vector-valued measurement at the same point, + when + * using a pseudo-spectral parameterization. + * + * This factors tries to enforce the basis function evaluation `f(x;p)` to take + * on the value `z` at location `x`, providing a gradient on the parameters p. + * In a probabilistic estimation context, `z` is known as a measurement, and the + * parameterized basis function can be seen as a + * measurement prediction function. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector. + */ +template +class GTSAM_EXPORT VectorEvaluationFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorEvaluationFactor() {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} + + virtual ~VectorEvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (P, N) is equal to specified measurement at the same point, when + * using a pseudo-spectral parameterization. + * + * This factor is similar to `VectorEvaluationFactor` with the key difference + * being that it only enforces the constraint for a single scalar in the vector, + * indexed by `i`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the fixed-size vector. + * + * Example: + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); + * where N is the degree and i is the component index. + */ +template +class GTSAM_EXPORT VectorComponentFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorComponentFactor() {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x) + : Base(key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x)) {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate 0the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { + } + + virtual ~VectorComponentFactor() {} +}; + +/** + * For a measurement value of type T i.e. `T z = g(x)`, this unary + * factor enforces that the polynomial basis, when evaluated at `x`, gives us + * the same `z`, i.e. `T z = g(x) = f(x;p)`. + * + * This is done via computations on the tangent space of the + * manifold of T. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param T: Object type which is synthesized by the provided functor. + * + * Example: + * ManifoldEvaluationFactor rotationFactor(key, measurement, + * model, N, x, a, b); + * + * where `x` is the value (e.g. timestep) at which the rotation was evaluated. + */ +template +class GTSAM_EXPORT ManifoldEvaluationFactor + : public FunctorizedFactor::dimension>> { + private: + using Base = FunctorizedFactor::dimension>>; + + public: + ManifoldEvaluationFactor() {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x, a, b)) { + } + + virtual ~ManifoldEvaluationFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point`x` is equal to the scalar measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT DerivativeFactor + : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + DerivativeFactor() {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {} + + virtual ~DerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point `x` is equal to the vector value `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector derivative. + */ +template +class GTSAM_EXPORT VectorDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template VectorDerivativeFunctor; + + public: + VectorDerivativeFactor() {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, Func(N, x)) {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, Func(N, x, a, b)) {} + + virtual ~VectorDerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial is equal to the scalar value at a specific index `i` of a + * vector-valued measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the control component derivative. + */ +template +class GTSAM_EXPORT ComponentDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template ComponentDerivativeFunctor

; + + public: + ComponentDerivativeFactor() {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x) + : Base(key, z, model, Func(N, i, x)) {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x, double a, double b) + : Base(key, z, model, Func(N, i, x, a, b)) {} + + virtual ~ComponentDerivativeFactor() {} +}; + +} // namespace gtsam diff --git a/gtsam/basis/CMakeLists.txt b/gtsam/basis/CMakeLists.txt new file mode 100644 index 000000000..203fd96a2 --- /dev/null +++ b/gtsam/basis/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB basis_headers "*.h") +install(FILES ${basis_headers} DESTINATION include/gtsam/basis) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp new file mode 100644 index 000000000..3b5825fc3 --- /dev/null +++ b/gtsam/basis/Chebyshev.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev.cpp + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +/** + * @brief Scale x from [a, b] to [t1, t2] + * + * ((b'-a') * (x - a) / (b - a)) + a' + * + * @param x Value to scale to new range. + * @param a Original lower limit. + * @param b Original upper limit. + * @param t1 New lower limit. + * @param t2 New upper limit. + * @return double + */ +static double scale(double x, double a, double b, double t1, double t2) { + return ((t2 - t1) * (x - a) / (b - a)) + t1; +} + +Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Tx(1, N); + + x = scale(x, a, b, -1, 1); + + Tx(0) = 1; + Tx(1) = x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); + } + return Tx; +} + +Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + Weights weights = Weights::Zero(N); + for (size_t n = 1; n < N; n++) { + weights(n) = n * Ux(n - 1); + } + return weights; +} + +Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Ux(N); + + x = scale(x, a, b, -1, 1); + + Ux(0) = 1; + Ux(1) = 2 * x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2); + } + return Ux; +} + +Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b); + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + + Weights weights(N); + + x = scale(x, a, b, -1, 1); + if (x == -1 || x == 1) { + throw std::runtime_error( + "Derivative of Chebyshev2 Basis does not exist at range limits."); + } + + for (size_t n = 0; n < N; n++) { + weights(n) = ((n + 1) * Tx(n + 1) - x * Ux(n)) / (x * x - 1); + } + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h new file mode 100644 index 000000000..d16ccfaac --- /dev/null +++ b/gtsam/basis/Chebyshev.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev.h + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * Basis of Chebyshev polynomials of the first kind + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind + * These are typically denoted with the symbol T_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + */ +struct Chebyshev1Basis : Basis { + using Parameters = Eigen::Matrix; + + Parameters parameters_; + + /** + * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * The derivative weights are pre-multiplied to the polynomial Parameters. + * + * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) + * I.e. the derivative fo a first kind cheb is just a second kind cheb + * So, we define a second kind basis here of order N-1 + * Note that it has one less weight. + * + * The Parameters pertain to 1st kind chebs up to order N-1 + * But of course the first one (order 0) is constant, so omit that weight. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev1Basis + +/** + * Basis of Chebyshev polynomials of the second kind. + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind + * These are typically denoted with the symbol U_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + * In contrast to the templates in Chebyshev2, the classes below specify + * basis functions, weighted combinations of which are used to approximate + * functions. In this sense, they are like the sines and cosines of the Fourier + * basis. + */ +struct Chebyshev2Basis : Basis { + using Parameters = Eigen::Matrix; + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev2Basis + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp new file mode 100644 index 000000000..44876b6e9 --- /dev/null +++ b/gtsam/basis/Chebyshev2.cpp @@ -0,0 +1,205 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev2.cpp + * @brief Chebyshev parameterizations on Chebyshev points of second kind + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weights(N); + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weights.setZero(); + weights(j) = 1; + return weights; + } + distances(j) = dj; + } + + // Beginning of interval, j = 0, x(0) = a + weights(0) = 0.5 / distances(0); + + // All intermediate points j=1:N-2 + double d = weights(0), s = -1; // changes sign s at every iteration + for (size_t j = 1; j < N - 1; j++, s = -s) { + weights(j) = s / distances(j); + d += weights(j); + } + + // End of interval, j = N-1, x(N-1) = b + weights(N - 1) = 0.5 * s / distances(N - 1); + d += weights(N - 1); + + // normalize + return weights / d; +} + +Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weightDerivatives(N); + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weightDerivatives.setZero(); + // compute the jth row of the differentiation matrix for this point + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + for (size_t k = 0; k < N; k++) { + if (j == 0 && k == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (j == N - 1 && k == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (k == j) { + double xj = Point(N, j); + double xj2 = xj * xj; + weightDerivatives(k) = -0.5 * xj / (1 - xj2); + } else { + double xj = Point(N, j); + double xk = Point(N, k); + double ck = (k == 0 || k == N - 1) ? 2. : 1.; + t = ((j + k) % 2) == 0 ? 1 : -1; + weightDerivatives(k) = (cj / ck) * t / (xj - xk); + } + } + return 2 * weightDerivatives / (b - a); + } + distances(j) = dj; + } + + // This section of code computes the derivative of + // the Barycentric Interpolation weights formula by applying + // the chain rule on the original formula. + + // g and k are multiplier terms which represent the derivatives of + // the numerator and denominator + double g = 0, k = 0; + double w = 1; + + for (size_t j = 0; j < N; j++) { + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + w = 1.0; + } + + t = (j % 2 == 0) ? 1 : -1; + + double c = t / distances(j); + g += w * c; + k += (w * c / distances(j)); + } + + double s = 1; // changes sign s at every iteration + double g2 = g * g; + + for (size_t j = 0; j < N; j++, s = -s) { + // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1, + // x0 = 1.0 + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + // All intermediate points j=1:N-2 + w = 1.0; + } + weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - + (w * -s * k / (g2 * distances(j))); + } + + return weightDerivatives; +} + +Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, + double b) { + DiffMatrix D(N, N); + if (N == 1) { + D(0, 0) = 1; + return D; + } + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + for (size_t i = 0; i < N; i++) { + double xi = Point(N, i); + double ci = (i == 0 || i == N - 1) ? 2. : 1.; + for (size_t j = 0; j < N; j++) { + if (i == 0 && j == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == N - 1 && j == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == j) { + double xi2 = xi * xi; + D(i, j) = -xi / (2 * (1 - xi2)); + } else { + double xj = Point(N, j); + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + t = ((i + j) % 2) == 0 ? 1 : -1; + D(i, j) = (ci / cj) * t / (xi - xj); + } + } + } + // scale the matrix to the range + return D / ((b - a) / 2.0); +} + +Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { + // Allocate space for weights + Weights weights(N); + size_t K = N - 1, // number of intervals between N points + K2 = K * K; + weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); + weights(N - 1) = weights(0); + + size_t last_k = K / 2 + K % 2 - 1; + + for (size_t i = 1; i <= N - 2; ++i) { + double theta = i * M_PI / K; + weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; + + for (size_t k = 1; k <= last_k; ++k) + weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1); + weights(i) *= (b - a) / K; + } + + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h new file mode 100644 index 000000000..28590961d --- /dev/null +++ b/gtsam/basis/Chebyshev2.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev2.h + * @brief Pseudo-spectral parameterization for Chebyshev polynomials of the + * second kind. + * + * In a pseudo-spectral case, rather than the parameters acting as + * weights for the bases polynomials (as in Chebyshev2Basis), here the + * parameters are the *values* at a specific set of points in the interval, the + * "Chebyshev points". These values uniquely determine the polynomial that + * interpolates them at the Chebyshev points. + * + * This is different from Chebyshev.h since it leverage ideas from + * pseudo-spectral optimization, i.e. we don't decompose into basis functions, + * rather estimate function parameters that enforce function nodes at Chebyshev + * points. + * + * Please refer to Agrawal21icra for more details. + * + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Chebyshev Interpolation on Chebyshev points of the second kind + * Note that N here, the number of points, is one less than N from + * 'Approximation Theory and Approximation Practice by L. N. Trefethen (pg.42)'. + */ +class GTSAM_EXPORT Chebyshev2 : public Basis { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using Base = Basis; + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /// Specific Chebyshev point + static double Point(size_t N, int j) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(- M_PI_2 + dtheta*j); also works + return cos(-M_PI + dtheta * j); + } + + /// Specific Chebyshev point, within [a,b] interval + static double Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N - 1); + // We add -PI so that we get values ordered from -1 to +1 + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; + } + + /// All Chebyshev points + static Vector Points(size_t N) { + Vector points(N); + for (size_t j = 0; j < N; j++) points(j) = Point(N, j); + return points; + } + + /// All Chebyshev points, within [a,b] interval + static Vector Points(size_t N, double a, double b) { + Vector points = Points(N); + const double T1 = (a + b) / 2, T2 = (b - a) / 2; + points = T1 + (T2 * points).array(); + return points; + } + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * These weights implement barycentric interpolation at a specific x. + * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the + * values of the function f at the Chebyshev points. As such, for a given x we + * obtain a linear map from parameter vectors f to interpolated values f(x). + * Optional [a,b] interval can be specified as well. + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * Evaluate derivative of barycentric weights. + * This is easy and efficient via the DifferentiationMatrix. + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); + + /// compute D = differentiation matrix, Trefethen00book p.53 + /// when given a parameter vector f of function values at the Chebyshev + /// points, D*f are the values of f'. + /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4 + static DiffMatrix DifferentiationMatrix(size_t N, double a = -1, + double b = 1); + + /** + * Evaluate Clenshaw-Curtis integration weights. + * Trefethen00book, pg 128, clencurt.m + * Note that N in clencurt.m is 1 less than our N + * K = N-1; + theta = pi*(0:K)'/K; + w = zeros(1,N); ii = 2:K; v = ones(K-1, 1); + if mod(K,2) == 0 + w(1) = 1/(K^2-1); w(N) = w(1); + for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + v = v - cos(K*theta(ii))/(K^2-1); + else + w(1) = 1/K^2; w(N) = w(1); + for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + end + w(ii) = 2*v/K; + + */ + static Weights IntegrationWeights(size_t N, double a = -1, double b = 1); + + /** + * Create matrix of values at Chebyshev points given vector-valued function. + */ + template + static Matrix matrix(boost::function(double)> f, + size_t N, double a = -1, double b = 1) { + Matrix Xmat(M, N); + for (size_t j = 0; j < N; j++) { + Xmat.col(j) = f(Point(N, j, a, b)); + } + return Xmat; + } +}; // \ Chebyshev2 + +} // namespace gtsam diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h new file mode 100644 index 000000000..f5cb99bd7 --- /dev/null +++ b/gtsam/basis/FitBasis.h @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FitBasis.h + * @date July 4, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Fit a Basis using least-squares + */ + +/* + * Concept needed for LS. Parameters = Coefficients | Values + * - Parameters, Jacobian + * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H) + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Our sequence representation is a map of {x: y} values where y = f(x) +using Sequence = std::map; +/// A sample is a key-value pair from a sequence. +using Sample = std::pair; + +/** + * Class that does regression via least squares + * Example usage: + * size_t N = 3; + * auto fit = FitBasis(data_points, noise_model, N); + * Vector coefficients = fit.parameters(); + * + * where `data_points` are a map from `x` to `y` values indicating a function + * mapping at specific points, `noise_model` is the gaussian noise model, and + * `N` is the degree of the polynomial basis used to fit the function. + */ +template +class FitBasis { + public: + using Parameters = typename Basis::Parameters; + + private: + Parameters parameters_; + + public: + /// Create nonlinear FG from Sequence + static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, + const SharedNoiseModel& model, + size_t N) { + NonlinearFactorGraph graph; + for (const Sample sample : sequence) { + graph.emplace_shared>(0, sample.second, model, N, + sample.first); + } + return graph; + } + + /// Create linear FG from Sequence + static GaussianFactorGraph::shared_ptr LinearGraph( + const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); + Values values; + values.insert(0, Parameters::Zero(N)); + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + return gfg; + } + + /** + * @brief Construct a new FitBasis object. + * + * @param sequence map of x->y values for a function, a.k.a. y = f(x). + * @param model The noise model to use. + * @param N The degree of the polynomial to fit. + */ + FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N); + VectorValues solution = gfg->optimize(); + parameters_ = solution.at(0); + } + + /// Return Fourier coefficients + Parameters parameters() const { return parameters_; } +}; + +} // namespace gtsam diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h new file mode 100644 index 000000000..6943150d7 --- /dev/null +++ b/gtsam/basis/Fourier.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Fourier.h + * @brief Fourier decomposition, see e.g. + * http://mathworld.wolfram.com/FourierSeries.html + * @author Varun Agrawal, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include + +namespace gtsam { + +/// Fourier basis +class GTSAM_EXPORT FourierBasis : public Basis { + public: + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x) { + Weights b(N); + b[0] = 1; + for (size_t i = 1, n = 1; i < N; i += 2, n++) { + b[i] = cos(n * x); + b[i + 1] = sin(n * x); + } + return b; + } + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x, double a, double b) { + // TODO(Varun) How do we enforce an interval for Fourier series? + return CalculateWeights(N, x); + } + + /** + * Compute D = differentiation matrix. + * Given coefficients c of a Fourier series c, D*c are the values of c'. + */ + static DiffMatrix DifferentiationMatrix(size_t N) { + DiffMatrix D = DiffMatrix::Zero(N, N); + double k = 1; + for (size_t i = 1; i < N; i += 2) { + D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x) + D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x) + k += 1; + } + + return D; + } + + /** + * @brief Get weights at a given x that calculate the derivative. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x) { + return CalculateWeights(N, x) * DifferentiationMatrix(N); + } + + /** + * @brief Get derivative weights at a given x that calculate the derivative, + in the interval [a, b]. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a, double b) { + return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N); + } + +}; // FourierBasis + +} // namespace gtsam diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h new file mode 100644 index 000000000..df2d9f62e --- /dev/null +++ b/gtsam/basis/ParameterMatrix.h @@ -0,0 +1,215 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ParamaterMatrix.h + * @brief Define ParameterMatrix class which is used to store values at + * interpolation points. + * @author Varun Agrawal, Frank Dellaert + * @date September 21, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * A matrix abstraction of MxN values at the Basis points. + * This class serves as a wrapper over an Eigen matrix. + * @tparam M: The dimension of the type you wish to evaluate. + * @param N: the number of Basis points (e.g. Chebyshev points of the second + * kind). + */ +template +class ParameterMatrix { + using MatrixType = Eigen::Matrix; + + private: + MatrixType matrix_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum { dimension = Eigen::Dynamic }; + + /** + * Create ParameterMatrix using the number of basis points. + * @param N: The number of basis points (the columns). + */ + ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); } + + /** + * Create ParameterMatrix from an MxN Eigen Matrix. + * @param matrix: An Eigen matrix used to initialze the ParameterMatrix. + */ + ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {} + + /// Get the number of rows. + size_t rows() const { return matrix_.rows(); } + + /// Get the number of columns. + size_t cols() const { return matrix_.cols(); } + + /// Get the underlying matrix. + MatrixType matrix() const { return matrix_; } + + /// Return the tranpose of the underlying matrix. + Eigen::Matrix transpose() const { return matrix_.transpose(); } + + /** + * Get the matrix row specified by `index`. + * @param index: The row index to retrieve. + */ + Eigen::Matrix row(size_t index) const { + return matrix_.row(index); + } + + /** + * Set the matrix row specified by `index`. + * @param index: The row index to set. + */ + auto row(size_t index) -> Eigen::Block { + return matrix_.row(index); + } + + /** + * Get the matrix column specified by `index`. + * @param index: The column index to retrieve. + */ + Eigen::Matrix col(size_t index) const { + return matrix_.col(index); + } + + /** + * Set the matrix column specified by `index`. + * @param index: The column index to set. + */ + auto col(size_t index) -> Eigen::Block { + return matrix_.col(index); + } + + /** + * Set all matrix coefficients to zero. + */ + void setZero() { matrix_.setZero(); } + + /** + * Add a ParameterMatrix to another. + * @param other: ParameterMatrix to add. + */ + ParameterMatrix operator+(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ + other.matrix()); + } + + /** + * Add a MxN-sized vector to the ParameterMatrix. + * @param other: Vector which is reshaped and added. + */ + ParameterMatrix operator+( + const Eigen::Matrix& other) const { + // This form avoids a deep copy and instead typecasts `other`. + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ + other_); + } + + /** + * Subtract a ParameterMatrix from another. + * @param other: ParameterMatrix to subtract. + */ + ParameterMatrix operator-(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ - other.matrix()); + } + + /** + * Subtract a MxN-sized vector from the ParameterMatrix. + * @param other: Vector which is reshaped and subracted. + */ + ParameterMatrix operator-( + const Eigen::Matrix& other) const { + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ - other_); + } + + /** + * Multiply ParameterMatrix with an Eigen matrix. + * @param other: Eigen matrix which should be multiplication compatible with + * the ParameterMatrix. + */ + MatrixType operator*(const Eigen::Matrix& other) const { + return matrix_ * other; + } + + /// @name Vector Space requirements, following LieMatrix + /// @{ + + /** + * Print the ParameterMatrix. + * @param s: The prepend string to add more contextual info. + */ + void print(const std::string& s = "") const { + std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; + } + + /** + * Check for equality up to absolute tolerance. + * @param other: The ParameterMatrix to check equality with. + * @param tol: The absolute tolerance threshold. + */ + bool equals(const ParameterMatrix& other, double tol = 1e-8) const { + return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol); + } + + /// Returns dimensionality of the tangent space + inline size_t dim() const { return matrix_.size(); } + + /// Convert to vector form, is done row-wise + inline Vector vector() const { + using RowMajor = Eigen::Matrix; + Vector result(matrix_.size()); + Eigen::Map(&result(0), rows(), cols()) = matrix_; + return result; + } + + /** Identity function to satisfy VectorSpace traits. + * + * NOTE: The size at compile time is unknown so this identity is zero + * length and thus not valid. + */ + inline static ParameterMatrix identity() { + // throw std::runtime_error( + // "ParameterMatrix::identity(): Don't use this function"); + return ParameterMatrix(0); + } + + /// @} +}; + +// traits for ParameterMatrix +template +struct traits> + : public internal::VectorSpace> {}; + +/* ************************************************************************* */ +// Stream operator that takes a ParameterMatrix. Used for printing. +template +inline std::ostream& operator<<(std::ostream& os, + const ParameterMatrix& parameterMatrix) { + os << parameterMatrix.matrix(); + return os; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i new file mode 100644 index 000000000..8f06fd2e1 --- /dev/null +++ b/gtsam/basis/basis.i @@ -0,0 +1,146 @@ +//************************************************************************* +// basis +//************************************************************************* + +namespace gtsam { + +// TODO(gerry): add all the Functors to the Basis interfaces, e.g. +// `EvaluationFunctor` + +#include + +class FourierBasis { + static Vector CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); + + static Matrix DifferentiationMatrix(size_t N); + static Vector DerivativeWeights(size_t N, double x); +}; + +#include + +class Chebyshev1Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector X); +}; + +class Chebyshev2Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); +}; + +#include +class Chebyshev2 { + static double Point(size_t N, int j); + static double Point(size_t N, int j, double a, double b); + + static Vector Points(size_t N); + static Vector Points(size_t N, double a, double b); + + static Matrix WeightMatrix(size_t N, Vector X); + static Matrix WeightMatrix(size_t N, Vector X, double a, double b); + + static Matrix CalculateWeights(size_t N, double x, double a, double b); + static Matrix DerivativeWeights(size_t N, double x, double a, double b); + static Matrix IntegrationWeights(size_t N, double a, double b); + static Matrix DifferentiationMatrix(size_t N, double a, double b); + + // TODO Needs OptionalJacobian + // static double Derivative(double x, Vector f); +}; + +#include + +template +class ParameterMatrix { + ParameterMatrix(const size_t N); + ParameterMatrix(const Matrix& matrix); + + Matrix matrix() const; + + void print(const string& s = "") const; +}; + +#include + +template +virtual class EvaluationFactor : gtsam::NoiseModelFactor { + EvaluationFactor(); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { + VectorEvaluationFactor(); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(Varun) Better way to support arbitrary dimensions? +// Especially if users mainly do `pip install gtsam` for the Python wrapper. +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D3; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D4; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D12; + +template +virtual class VectorComponentFactor : gtsam::NoiseModelFactor { + VectorComponentFactor(); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x, double a, double b); +}; + +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D3; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D4; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D12; + +template +virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { + ManifoldEvaluationFactor(); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and +// `ComponentDerivativeFactor` + +#include +template +class FitBasis { + FitBasis(const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + + static gtsam::NonlinearFactorGraph NonlinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + Parameters parameters() const; +}; + +} // namespace gtsam diff --git a/gtsam/basis/tests/CMakeLists.txt b/gtsam/basis/tests/CMakeLists.txt new file mode 100644 index 000000000..63cad0be6 --- /dev/null +++ b/gtsam/basis/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam") diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp new file mode 100644 index 000000000..64c925886 --- /dev/null +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +const size_t N = 3; + +//****************************************************************************** +TEST(Chebyshev, Chebyshev1) { + using Synth = Chebyshev1Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Chebyshev2) { + using Synth = Chebyshev2Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Evaluation) { + Chebyshev1Basis::EvaluationFunctor fx(N, 0.5); + Vector c(N); + c << 3, 5, -12; + EXPECT_DOUBLES_EQUAL(11.5, fx(c), 1e-9); +} + +//****************************************************************************** +#include +#include +TEST(Chebyshev, Expression) { + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + + // Let's pretend we have 6 GPS measurements (we just do x coordinate) + // at times + const size_t m = 6; + Vector t(m); + t << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + Vector x(m); + x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + + for (size_t i = 0; i < m; i++) { + graph.emplace_shared>(key, x(i), model, N, + t(i)); + } + + // Solve + Values initial; + initial.insert(key, Vector::Zero(N)); // initial does not matter + + // ... and optimize + GaussNewtonParams parameters; + GaussNewtonOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check + Vector expected(N); + expected << 0, 1, 0; + Vector actual_c = result.at(key); + EXPECT(assert_equal(expected, actual_c)); + + // Calculate and print covariances + Marginals marginals(graph, result); + Matrix3 cov = marginals.marginalCovariance(key); + EXPECT_DOUBLES_EQUAL(0.626, cov(1, 1), 1e-3); + + // Predict x at time 1.0 + Chebyshev1Basis::EvaluationFunctor f(N, 1.0); + Matrix H; + double actual = f(actual_c, H); + EXPECT_DOUBLES_EQUAL(1.0, actual, 1e-9); + + // Calculate predictive variance on prediction + double actual_variance_on_prediction = (H * cov * H.transpose())(0); + EXPECT_DOUBLES_EQUAL(1.1494, actual_variance_on_prediction, 1e-4); +} + +//****************************************************************************** +TEST(Chebyshev, Decomposition) { + const size_t M = 16; + + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < M; i++) { + double x = ((double)i / M); // - 0.99; + double y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, N); + + // Check + Vector expected = Vector::Zero(N); + expected(1) = 1; + EXPECT(assert_equal(expected, (Vector)actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev1, Derivative) { + Vector c(N); + c << 12, 3, 2; + + Weights D; + + double x = -1.0; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-5, (D * c)(0), 1e-9); + + x = -0.5; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-1, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(5.4, (D * c)(0), 1e-9); +} + +//****************************************************************************** +Vector3 f(-6, 1, 0.5); + +double proxy1(double x, size_t N) { + return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev1, Derivative2) { + const double x = 0.5; + auto D = Chebyshev1Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy1, x, N); + // regression + EXPECT_DOUBLES_EQUAL(2, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(2, (D * f)(0), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev2, Derivative) { + Vector c(N); + c << 12, 6, 2; + + Weights D; + + double x = -1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + x = 1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + + x = -0.5; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(4, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(16.8, (D * c)(0), 1e-9); + + x = 0.75; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(24, (D * c)(0), 1e-9); + + x = 10; + D = Chebyshev2Basis::DerivativeWeights(N, x, 0, 20); + // regression + EXPECT_DOUBLES_EQUAL(12, (D * c)(0), 1e-9); +} + +//****************************************************************************** +double proxy2(double x, size_t N) { + return Chebyshev2Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev2, Derivative2) { + const double x = 0.5; + auto D = Chebyshev2Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy2, x, N); + // regression + EXPECT_DOUBLES_EQUAL(4, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(4, (D * f)(0), 1e-9); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp new file mode 100644 index 000000000..4cee70daf --- /dev/null +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -0,0 +1,435 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral + * methods + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::placeholders; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + +const size_t N = 32; + +//****************************************************************************** +TEST(Chebyshev2, Point) { + static const int N = 5; + auto points = Chebyshev2::Points(N); + Vector expected(N); + expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // Check symmetry + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 0), -Chebyshev2::Point(N, 4), tol); + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 1), -Chebyshev2::Point(N, 3), tol); +} + +//****************************************************************************** +TEST(Chebyshev2, PointInInterval) { + static const int N = 5; + auto points = Chebyshev2::Points(N, 0, 20); + Vector expected(N); + expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.; + expected *= 10.0; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // all at once + Vector actual = Chebyshev2::Points(N, 0, 20); + CHECK(assert_equal(expected, actual)); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5] +TEST(Chebyshev2, Interpolate2) { + size_t N = 3; + Chebyshev2::EvaluationFunctor fx(N, 0.5); + Vector f(N); + f << 4, 2, 6; + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5] +TEST(Chebyshev2, Interpolate2_Interval) { + Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2); + Vector3 f(4, 2, 6); + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1, +// 3}}, 0.5] +TEST(Chebyshev2, Interpolate5) { + Chebyshev2::EvaluationFunctor fx(5, 0.5); + Vector f(5); + f << 4, 2, 6, 3, 3; + EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5); +} + +//****************************************************************************** +// Interpolating vectors +TEST(Chebyshev2, InterpolateVector) { + double t = 30, a = 0, b = 100; + const size_t N = 3; + // Create 2x3 matrix with Vectors at Chebyshev points + ParameterMatrix<2> X(N); + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + + // Check value + Vector expected(2); + expected << t, 0; + Eigen::Matrix actualH(2, 2 * N); + + Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); + + // Check derivative + boost::function)> f = boost::bind( + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); + Matrix numericalH = + numericalDerivative11, 2 * N>(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +//****************************************************************************** +TEST(Chebyshev2, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = (double)i / 16. - 0.99, y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, 3); + + // Check + Vector expected(3); + expected << -1, 0, 1; + EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DifferentiationMatrix3) { + // Trefethen00book, p.55 + const size_t N = 3; + Matrix expected(N, N); + // Differentiation matrix computed from Chebfun + expected << 1.5000, -2.0000, 0.5000, // + 0.5000, -0.0000, -0.5000, // + -0.5000, 2.0000, -1.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DerivativeMatrix6) { + // Trefethen00book, p.55 + const size_t N = 6; + Matrix expected(N, N); + expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000, // + 2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764, // + -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820, // + 0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // + -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // + 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); +} + +// test function for CalculateWeights and DerivativeWeights +double f(double x) { + // return 3*(x**3) - 2*(x**2) + 5*x - 11 + return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11; +} + +// its derivative +double fprime(double x) { + // return 9*(x**2) - 4*(x) + 5 + return 9.0 * pow(x, 2) - 4.0 * x + 5.0; +} + +//****************************************************************************** +TEST(Chebyshev2, CalculateWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376; + Weights weights1 = Chebyshev2::CalculateWeights(N, x1); + Weights weights2 = Chebyshev2::CalculateWeights(N, x2); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8); +} + +TEST(Chebyshev2, CalculateWeights2) { + double a = 0, b = 10, x1 = 7, x2 = 4.12; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + + Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); + double expected2 = f(x2); // 185.454784 + double actual2 = weights2 * fvals; + EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); +} + +TEST(Chebyshev2, DerivativeWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376, x3 = 0.0; + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9); + + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9); + + // test if derivative calculation and cheb point is correct + double x4 = Chebyshev2::Point(N, 3); + Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); + EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9); +} + +TEST(Chebyshev2, DerivativeWeights2) { + double x1 = 5, x2 = 4.12, a = 0, b = 10; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); + + // test if derivative calculation and cheb point is correct + double x3 = Chebyshev2::Point(N, 3, a, b); + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) { + const size_t N6 = 6; + double x1 = 0.311; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; + Weights actual = Chebyshev2::DerivativeWeights(N6, x1); + EXPECT(assert_equal(expected, actual, 1e-12)); + + double a = -3, b = 8, x2 = 5.05; + Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b); + Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; + Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); + EXPECT(assert_equal(expected1, actual1, 1e-12)); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights6) { + const size_t N6 = 6; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights7) { + const size_t N7 = 7; + Matrix D7 = Chebyshev2::DifferentiationMatrix(N7); + Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x))); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished(); +double proxy3(double x) { + return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6) { + // Check Derivative evaluation at point x=0.2 + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy3, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Assert that derivative also works in non-standard interval [0,3] +double proxy4(double x) { + return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6_03) { + // Check Derivative evaluation at point x=0.2, in interval [0,3] + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy4, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor +TEST(Chebyshev2, VectorDerivativeFunctor) { + const size_t N = 3, M = 2; + const double x = 0.2; + using VecD = Chebyshev2::VectorDerivativeFunctor; + VecD fx(N, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(M, M * N); + EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); + + // Test Jacobian + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor with polynomial function +TEST(Chebyshev2, VectorDerivativeFunctor2) { + const size_t N = 64, M = 1, T = 15; + using VecD = Chebyshev2::VectorDerivativeFunctor; + + const Vector points = Chebyshev2::Points(N, 0, T); + + // Assign the parameter matrix + Vector values(N); + for (size_t i = 0; i < N; ++i) { + values(i) = f(points(i)); + } + ParameterMatrix X(values); + + // Evaluate the derivative at the chebyshev points using + // VectorDerivativeFunctor. + for (size_t i = 0; i < N; ++i) { + VecD d(N, points(i), 0, T); + Vector1 Dx = d(X); + EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); + } + + // Test Jacobian at the first chebyshev point. + Matrix actualH(M, M * N); + VecD vecd(N, points(0), 0, T); + vecd(X, actualH); + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), vecd, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-6)); +} + +//****************************************************************************** +// Test ComponentDerivativeFunctor +TEST(Chebyshev2, ComponentDerivativeFunctor) { + const size_t N = 6, M = 2; + const double x = 0.2; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; + size_t row = 1; + CompFunc fx(N, row, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(1, M * N); + EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); + + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +TEST(Chebyshev2, IntegralWeights) { + const size_t N7 = 7; + Vector actual = Chebyshev2::IntegrationWeights(N7); + Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, + 0.457142857142857, 0.520634920634921, 0.457142857142857, + 0.253968253968254, 0.0285714285714286) + .finished(); + EXPECT(assert_equal(expected, actual)); + + const size_t N8 = 8; + Vector actual2 = Chebyshev2::IntegrationWeights(N8); + Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, + 0.352242423718159, 0.437208405798326, 0.437208405798326, + 0.352242423718159, 0.190141007218208, 0.0204081632653061) + .finished(); + EXPECT(assert_equal(expected2, actual2)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp new file mode 100644 index 000000000..7a53cfcc9 --- /dev/null +++ b/gtsam/basis/tests/testFourier.cpp @@ -0,0 +1,254 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testFourier.cpp + * @date July 4, 2020 + * @author Frank Dellaert, Varun Agrawal + * @brief Unit tests for Fourier Basis Decompositions with Expressions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +// Coefficients for testing, respectively 3 and 7 parameter Fourier basis. +// They correspond to best approximation of TestFunction(x) +const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished(); +const Vector7 k7Coefficients = + (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943) + .finished(); + +// The test-function used below +static double TestFunction(double x) { return exp(sin(x) + cos(x)); } + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctor) { + // fx(0) takes coefficients c to calculate the value f(c;x==0) + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients), 1e-9); +} + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctorDerivative) { + // If we give the H argument, we get the derivative of fx(0) wrpt coefficients + // Needs to be Matrix so it can be used by OptionalJacobian. + Matrix H(1, 3); + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients, H), 1e-9); + + Matrix13 expectedH(1, 1, 0); + EXPECT(assert_equal(expectedH, H)); +} + +//****************************************************************************** +TEST(Basis, Manual) { + const size_t N = 3; + + // We will create a linear factor graph + GaussianFactorGraph graph; + + // We create an unknown Vector expression for the coefficients + Key key(1); + + // We will need values below to test the Jacobians + Values values; + values.insert(key, Vector::Zero(N)); // value does not really matter + + // At 16 different samples points x, check Predict_ expression + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, N); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A, b); + graph.add(linearFactor); + + // Create factor to predict value at x + EvaluationFactor predictFactor(key, desiredValue, model, N, + x); + + // Check expression Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); + + auto linearizedFactor = predictFactor.linearize(values); + auto linearizedJacobianFactor = + boost::dynamic_pointer_cast(linearizedFactor); + CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor + EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9)); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, EvaluationFactor) { + // Check fitting a function with a 7-parameter Fourier basis + + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, desiredValue = TestFunction(x); + graph.emplace_shared>(key, desiredValue, + model, 7, x); + } + + // Solve FourierFactorGraph + Values values; + values.insert(key, Vector::Zero(7)); + GaussianFactorGraph::shared_ptr lfg = graph.linearize(values); + VectorValues actual = lfg->optimize(); + + EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, WeightMatrix) { + // The WeightMatrix creates an m*n matrix, where m is the number of sample + // points, and n is the number of parameters. + Matrix expected(2, 3); + expected.row(0) << 1, cos(1), sin(1); + expected.row(1) << 1, cos(2), sin(2); + Vector2 X(1, 2); + Matrix actual = FourierBasis::WeightMatrix(3, X); + EXPECT(assert_equal(expected, actual, 1e-9)); +} + +//****************************************************************************** +TEST(Basis, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = TestFunction(x); + sequence[x] = y; + } + + // Do Fourier Decomposition + FitBasis actual(sequence, model, 3); + + // Check + EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +double proxy(double x) { + return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients); +} + +TEST(Basis, Derivative7) { + // Check Derivative evaluation at point x=0.2 + + // Calculate expected values by numerical derivative of proxy. + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy, x); + + // Calculate derivatives at Chebyshev points using D7, interpolate + Matrix D7 = FourierBasis::DifferentiationMatrix(7); + Vector derivativeCoefficients = D7 * k7Coefficients; + FourierBasis::EvaluationFunctor fx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8); + + // Do directly + FourierBasis::DerivativeFunctor dfdx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8); +} + +//****************************************************************************** +TEST(Basis, VecDerivativeFunctor) { + using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + const size_t N = 3; + + // MATLAB example, Dec 27 2019, commit 014eded5 + double h = 2 * M_PI / 16; + Vector2 dotShape(0.5556, -0.8315); // at h/2 + DotShape dotShapeFunction(N, h / 2); + Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); + ParameterMatrix<2> theta(theta_mat); + EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); +} + +//****************************************************************************** +// Suppose we want to parameterize a periodic function with function values at +// specific times, rather than coefficients. Can we do it? This would be a +// generalization of the Fourier transform, and constitute a "pseudo-spectral" +// parameterization. One way to do this is to establish hard constraints that +// express the relationship between the new parameters and the coefficients. +// For example, below I'd like the parameters to be the function values at +// X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients. +// Because the values f(X) = at these points can be written as f(X) = W(X)*c, +// we can simply express the coefficents c as c=inv(W(X))*f, which is a +// generalized Fourier transform. That also means we can create factors with the +// unknown f-values, as done manually below. +TEST(Basis, PseudoSpectral) { + // We will create a linear factor graph + GaussianFactorGraph graph; + + const size_t N = 3; + const Key key(1); + + // The correct values at X = {0.1,0.2,0.3} are simply W*c + const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); + const Matrix W = FourierBasis::WeightMatrix(N, X); + const Vector expected = W * k3Coefficients; + + // Check those values are indeed correct values of Fourier approximation + using Eval = FourierBasis::EvaluationFunctor; + EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); + + // Calculate "inverse Fourier transform" matrix + const Matrix invW = W.inverse(); + + // At 16 different samples points x, add a factor on fExpr + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, 3); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A * invW, b); + graph.add(linearFactor); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp new file mode 100644 index 000000000..ec62e8eea --- /dev/null +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testParameterMatrix.cpp + * @date Sep 22, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Unit tests for ParameterMatrix.h + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using Parameters = Chebyshev2::Parameters; + +const size_t M = 2, N = 5; + +//****************************************************************************** +TEST(ParameterMatrix, Constructor) { + ParameterMatrix<2> actual1(3); + ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); + EXPECT(assert_equal(expected1, actual1)); + + ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); + ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Dimensions) { + ParameterMatrix params(N); + EXPECT_LONGS_EQUAL(params.rows(), M); + EXPECT_LONGS_EQUAL(params.cols(), N); + EXPECT_LONGS_EQUAL(params.dim(), M * N); +} + +//****************************************************************************** +TEST(ParameterMatrix, Getters) { + ParameterMatrix params(N); + + Matrix expectedMatrix = Matrix::Zero(2, 5); + EXPECT(assert_equal(expectedMatrix, params.matrix())); + + Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); + EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); + + ParameterMatrix p2(Matrix::Ones(M, N)); + Vector expectedRowVector = Vector::Ones(N); + for (size_t r = 0; r < M; ++r) { + EXPECT(assert_equal(p2.row(r), expectedRowVector)); + } + + ParameterMatrix p3(2 * Matrix::Ones(M, N)); + Vector expectedColVector = 2 * Vector::Ones(M); + for (size_t c = 0; c < M; ++c) { + EXPECT(assert_equal(p3.col(c), expectedColVector)); + } +} + +//****************************************************************************** +TEST(ParameterMatrix, Setters) { + ParameterMatrix params(Matrix::Zero(M, N)); + Matrix expected = Matrix::Zero(M, N); + + // row + params.row(0) = Vector::Ones(N); + expected.row(0) = Vector::Ones(N); + EXPECT(assert_equal(expected, params.matrix())); + + // col + params.col(2) = Vector::Ones(M); + expected.col(2) = Vector::Ones(M); + + EXPECT(assert_equal(expected, params.matrix())); + + // setZero + params.setZero(); + expected.setZero(); + EXPECT(assert_equal(expected, params.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Addition) { + ParameterMatrix params(Matrix::Ones(M, N)); + ParameterMatrix expected(2 * Matrix::Ones(M, N)); + + // Add vector + EXPECT(assert_equal(expected, params + Vector::Ones(M * N))); + // Add another ParameterMatrix + ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Subtraction) { + ParameterMatrix params(2 * Matrix::Ones(M, N)); + ParameterMatrix expected(Matrix::Ones(M, N)); + + // Subtract vector + EXPECT(assert_equal(expected, params - Vector::Ones(M * N))); + // Subtract another ParameterMatrix + ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Multiplication) { + ParameterMatrix params(Matrix::Ones(M, N)); + Matrix multiplier = 2 * Matrix::Ones(N, 2); + Matrix expected = 2 * N * Matrix::Ones(M, 2); + EXPECT(assert_equal(expected, params * multiplier)); +} + +//****************************************************************************** +TEST(ParameterMatrix, VectorSpace) { + ParameterMatrix params(Matrix::Ones(M, N)); + // vector + EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); + // identity + EXPECT(assert_equal(ParameterMatrix::identity(), + ParameterMatrix(Matrix::Zero(M, 0)))); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index c09eba9bb..143d4bc3c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -147,113 +147,123 @@ public: * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * Fixed size version - */ - template // N = 2 or 3 (point dimension), ND is the camera dimension - static SymmetricBlockMatrix SchurComplement( - const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + */ + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix SchurComplement( + const std::vector< + Eigen::Matrix, + Eigen::aligned_allocator>>& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + // a single point is observed in m cameras + size_t m = Fs.size(); - // a single point is observed in m cameras - size_t m = Fs.size(); + // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) + size_t M1 = ND * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, ND); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); - // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) - size_t M1 = ND * m + 1; - std::vector dims(m + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, ND); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera - // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera + const Eigen::Matrix& Fi = Fs[i]; + const auto FiT = Fi.transpose(); + const Eigen::Matrix Ei_P = // + E.block(ZDim * i, 0, ZDim, N) * P; - const Eigen::Matrix& Fi = Fs[i]; - const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = // - E.block(ZDim * i, 0, ZDim, N) * P; + // D = (Dx2) * ZDim + augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) - // D = (Dx2) * ZDim - augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b - - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian.setDiagonalBlock(i, FiT + * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian.setDiagonalBlock(i, FiT - * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const Eigen::Matrix& Fj = Fs[j]; - // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera - const Eigen::Matrix& Fj = Fs[j]; + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian.setOffDiagonalBlock(i, j, -FiT + * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); + } + } // end of for over cameras - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian.setOffDiagonalBlock(i, j, -FiT - * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); - } - } // end of for over cameras - - augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); - return augmentedHessian; - } + augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) - * In this version, we allow for the case where the keys in the Jacobian are organized - * differently from the keys in the output SymmetricBlockMatrix - * In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration) - * such that F keeps the block structure that makes the Schur complement trick fast. + * In this version, we allow for the case where the keys in the Jacobian are + * organized differently from the keys in the output SymmetricBlockMatrix In + * particular: each diagonal block of the Jacobian F captures 2 poses (useful + * for rolling shutter and extrinsic calibration) such that F keeps the block + * structure that makes the Schur complement trick fast. + * + * N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is + * the Hessian block dimension */ - template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension + template static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( - const std::vector, - Eigen::aligned_allocator > >& Fs, + const std::vector< + Eigen::Matrix, + Eigen::aligned_allocator>>& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - + const KeyVector& jacobianKeys, const KeyVector& hessianKeys) { size_t nrNonuniqueKeys = jacobianKeys.size(); size_t nrUniqueKeys = hessianKeys.size(); - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs,E,P,b); + // Marginalize point: note - we reuse the standard SchurComplement function. + SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs, E, P, b); - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term + // Pack into an Hessian factor, allow space for b term. + std::vector dims(nrUniqueKeys + 1); std::fill(dims.begin(), dims.end() - 1, NDD); dims.back() = 1; SymmetricBlockMatrix augmentedHessianUniqueKeys; - // here we have to deal with the fact that some blocks may share the same keys - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + // Deal with the fact that some blocks may share the same keys. + if (nrUniqueKeys == nrNonuniqueKeys) { + // Case when there is 1 calibration key per camera: augmentedHessianUniqueKeys = SymmetricBlockMatrix( dims, Matrix(augmentedHessian.selfadjointView())); - } else { // if multiple cameras share a calibration we have to rearrange - // the results of the Schur complement matrix - std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + } else { + // When multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix. + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // includes b std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); nonuniqueDims.back() = 1; augmentedHessian = SymmetricBlockMatrix( nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + // Get map from key to location in the new augmented Hessian matrix (the + // one including only unique keys). std::map keyToSlotMap; for (size_t k = 0; k < nrUniqueKeys; k++) { keyToSlotMap[hessianKeys[k]] = k; } - // initialize matrix to zero + // Initialize matrix to zero. augmentedHessianUniqueKeys = SymmetricBlockMatrix( dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); - // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // and populates an Hessian that only includes the unique keys (that is what we want to return) + // Add contributions for each key: note this loops over the hessian with + // nonUnique keys (augmentedHessian) and populates an Hessian that only + // includes the unique keys (that is what we want to return). for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows Key key_i = jacobianKeys.at(i); - // update information vector + // Update information vector. augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i], nrUniqueKeys, augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - // update blocks + // Update blocks. for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols Key key_j = jacobianKeys.at(j); if (i == j) { @@ -267,45 +277,20 @@ public: } else { augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); } } } } - // update bottom right element of the matrix + + // Update bottom right element of the matrix. augmentedHessianUniqueKeys.updateDiagonalBlock( nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); } return augmentedHessianUniqueKeys; } - /** - * non-templated version of function above - */ - static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( - const std::vector, - Eigen::aligned_allocator > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, - jacobianKeys, - hessianKeys); - } - - /** - * non-templated version of function above - */ - static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_6_6( - const std::vector, - Eigen::aligned_allocator > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - return SchurComplementAndRearrangeBlocks<3,6,6>(Fs, E, P, b, - jacobianKeys, - hessianKeys); - } - /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index e583c0e80..144f28314 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -185,9 +185,8 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { // Actual SymmetricBlockMatrix augmentedHessianBM = - Set::SchurComplementAndRearrangeBlocks_3_12_6(Fs, E, P, b, - nonuniqueKeys, - uniqueKeys); + Set::SchurComplementAndRearrangeBlocks<3, 12, 6>( + Fs, E, P, b, nonuniqueKeys, uniqueKeys); Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); // Expected diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 691ab8ac8..e1f8ece8d 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -110,7 +110,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { bool equals(const NonlinearFactor &other, double tol = 1e-9) const override { const FunctorizedFactor *e = dynamic_cast *>(&other); - return e && Base::equals(other, tol) && + return e != nullptr && Base::equals(other, tol) && traits::Equals(this->measured_, e->measured_, tol); } /// @} diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index b0ec5e722..14a14fc19 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -20,8 +20,12 @@ #include #include #include +#include +#include +#include #include #include +#include #include using namespace std; @@ -60,7 +64,7 @@ class ProjectionFunctor { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; } @@ -255,18 +259,148 @@ TEST(FunctorizedFactor, Lambda2) { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; }; // FunctorizedFactor factor(key, measurement, model, lambda); - auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, model2, lambda); + auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, + model2, lambda); Vector error = factor.evaluateError(A, x); EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); } +const size_t N = 2; + +//****************************************************************************** +TEST(FunctorizedFactor, Print2) { + const size_t M = 1; + + Vector measured = Vector::Ones(M) * 42; + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, priorFactor)); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorEvaluationFactor) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(priorFactor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorComponentFactor) { + const int P = 4; + const size_t i = 2; + const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + VectorComponentFactor controlPrior(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(controlPrior); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VecDerivativePrior) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, ComponentDerivativeFactor) { + const size_t M = 4; + + double measured = 0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + ComponentDerivativeFactor controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 929ec41f7..b2076cc14 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -305,7 +305,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { // Build augmented Hessian (with last row/column being the information vector) // Note: we need to get the augumented hessian wrt the unique keys in key_ SymmetricBlockMatrix augmentedHessianUniqueKeys = - Base::Cameras::SchurComplementAndRearrangeBlocks_3_6_6( + Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>( Fs, E, P, b, nonUniqueKeys_, this->keys_); return boost::make_shared < RegularHessianFactor diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp index 5fc1c05eb..c92a13daf 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -23,7 +23,6 @@ Vector ProjectionFactorRollingShutter::evaluateError( const Pose3& pose_a, const Pose3& pose_b, const Point3& point, boost::optional H1, boost::optional H2, boost::optional H3) const { - try { Pose3 pose = interpolate(pose_a, pose_b, alpha_, H1, H2); gtsam::Matrix Hprj; @@ -32,12 +31,10 @@ Vector ProjectionFactorRollingShutter::evaluateError( gtsam::Matrix HbodySensor; PinholeCamera camera( pose.compose(*body_P_sensor_, HbodySensor), *K_); - Point2 reprojectionError( - camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) - *H1 = Hprj * HbodySensor * (*H1); - if (H2) - *H2 = Hprj * HbodySensor * (*H2); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + measured_); + if (H1) *H1 = Hprj * HbodySensor * (*H1); + if (H2) *H2 = Hprj * HbodySensor * (*H2); return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); @@ -45,29 +42,23 @@ Vector ProjectionFactorRollingShutter::evaluateError( } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError( - camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) - *H1 = Hprj * (*H1); - if (H2) - *H2 = Hprj * (*H2); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + measured_); + if (H1) *H1 = Hprj * (*H1); + if (H2) *H2 = Hprj * (*H2); return reprojectionError; } } catch (CheiralityException& e) { - if (H1) - *H1 = Matrix::Zero(2, 6); - if (H2) - *H2 = Matrix::Zero(2, 6); - if (H3) - *H3 = Matrix::Zero(2, 3); + if (H1) *H1 = Matrix::Zero(2, 6); + if (H2) *H2 = Matrix::Zero(2, 6); + if (H3) *H3 = Matrix::Zero(2, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " - << DefaultKeyFormatter(this->key2()) << " moved behind camera " - << DefaultKeyFormatter(this->key1()) << std::endl; - if (throwCheirality_) - throw CheiralityException(this->key2()); + << DefaultKeyFormatter(this->key2()) << " moved behind camera " + << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) throw CheiralityException(this->key2()); } return Vector2::Constant(2.0 * K_->fx()); } -} //namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 5827a538c..c92653c13 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -17,41 +17,47 @@ #pragma once -#include -#include -#include #include +#include +#include +#include + #include namespace gtsam { /** - * Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. The calibration is known here. - * This version takes rolling shutter information into account as follows: consider two consecutive poses A and B, - * and a Point2 measurement taken starting at time A using a rolling shutter camera. - * Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) - * corresponding to the time of exposure of the row of the image the pixel belongs to. - * Let us define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by - * the alpha to project the corresponding landmark to Point2. + * Non-linear factor for 2D projection measurement obtained using a rolling + * shutter camera. The calibration is known here. This version takes rolling + * shutter information into account as follows: consider two consecutive poses A + * and B, and a Point2 measurement taken starting at time A using a rolling + * shutter camera. Pose A has timestamp t_A, and Pose B has timestamp t_B. The + * Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) corresponding + * to the time of exposure of the row of the image the pixel belongs to. Let us + * define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose + * interpolated between A and B by the alpha to project the corresponding + * landmark to Point2. * @addtogroup SLAM */ -class ProjectionFactorRollingShutter : public NoiseModelFactor3 { +class ProjectionFactorRollingShutter + : public NoiseModelFactor3 { protected: - // Keep a copy of measurement and calibration for I/O - Point2 measured_; ///< 2D measurement - double alpha_; ///< interpolation parameter in [0,1] corresponding to the point2 measurement + Point2 measured_; ///< 2D measurement + double alpha_; ///< interpolation parameter in [0,1] corresponding to the + ///< point2 measurement boost::shared_ptr K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + boost::optional + body_P_sensor_; ///< The pose of the sensor in the body frame // verbosity handling for Cheirality Exceptions - bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: + ///< false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions + ///< (default: false) public: - /// shorthand for base class type typedef NoiseModelFactor3 Base; @@ -66,72 +72,72 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, - boost::optional body_P_sensor = - boost::none) + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), - verboseCheirality_(false) { - } + verboseCheirality_(false) {} /** * Constructor with exception-handling flags - * @param measured is the 2-dimensional pixel location of point in the image (the measurement) + * @param measured is the 2-dimensional pixel location of point in the image + * (the measurement) * @param alpha in [0,1] is the rolling shutter parameter for the measurement * @param model is the noise model * @param poseKey_a is the key of the first camera * @param poseKey_b is the key of the second camera * @param pointKey is the key of the landmark * @param K shared pointer to the constant calibration - * @param throwCheirality determines whether Cheirality exceptions are rethrown - * @param verboseCheirality determines whether exceptions are printed for Cheirality - * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param throwCheirality determines whether Cheirality exceptions are + * rethrown + * @param verboseCheirality determines whether exceptions are printed for + * Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) */ - ProjectionFactorRollingShutter(const Point2& measured, double alpha, - const SharedNoiseModel& model, Key poseKey_a, - Key poseKey_b, Key pointKey, - const boost::shared_ptr& K, - bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = - boost::none) + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, bool throwCheirality, + bool verboseCheirality, + boost::optional body_P_sensor = boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(throwCheirality), - verboseCheirality_(verboseCheirality) { - } + verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~ProjectionFactorRollingShutter() { - } + virtual ~ProjectionFactorRollingShutter() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -139,8 +145,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3>& world_P_body_key_pairs, const std::vector& alphas, const std::vector>& Ks, - const std::vector body_P_sensors) { + const std::vector& body_P_sensors) { assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == alphas.size()); assert(world_P_body_key_pairs.size() == Ks.size()); @@ -150,20 +169,24 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor>& world_P_body_key_pairs, const std::vector& alphas, - const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { + const boost::shared_ptr& K, + const Pose3& body_P_sensor = Pose3::identity()) { assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == alphas.size()); for (size_t i = 0; i < measurements.size(); i++) { @@ -173,39 +196,37 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor> calibration() const { + const std::vector>& calibration() const { return K_all_; } - /// return (for each observation) the keys of the pair of poses from which we interpolate - const std::vector> world_P_body_key_pairs() const { + /// return (for each observation) the keys of the pair of poses from which we + /// interpolate + const std::vector>& world_P_body_key_pairs() const { return world_P_body_key_pairs_; } /// return the interpolation factors alphas - const std::vector alphas() const { - return alphas_; - } + const std::vector& alphas() const { return alphas_; } /// return the extrinsic camera calibration body_P_sensors - const std::vector body_P_sensors() const { - return body_P_sensors_; - } + const std::vector& body_P_sensors() const { return body_P_sensors_; } /** * print * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override { + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << " pose1 key: " - << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; + << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " - << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; + << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); @@ -216,41 +237,50 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor* e = - dynamic_cast*>(&p); + dynamic_cast*>( + &p); double keyPairsEqual = true; - if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ - for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){ + if (this->world_P_body_key_pairs_.size() == + e->world_P_body_key_pairs().size()) { + for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) { const Key key1own = world_P_body_key_pairs_[k].first; const Key key1e = e->world_P_body_key_pairs()[k].first; const Key key2own = world_P_body_key_pairs_[k].second; const Key key2e = e->world_P_body_key_pairs()[k].second; - if ( !(key1own == key1e) || !(key2own == key2e) ){ - keyPairsEqual = false; break; + if (!(key1own == key1e) || !(key2own == key2e)) { + keyPairsEqual = false; + break; } } - }else{ keyPairsEqual = false; } + } else { + keyPairsEqual = false; + } double extrinsicCalibrationEqual = true; - if(this->body_P_sensors_.size() == e->body_P_sensors().size()){ - for(size_t i=0; i< this->body_P_sensors_.size(); i++){ - if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){ - extrinsicCalibrationEqual = false; break; + if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { + for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { + if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { + extrinsicCalibrationEqual = false; + break; } } - }else{ extrinsicCalibrationEqual = false; } + } else { + extrinsicCalibrationEqual = false; + } - return e && Base::equals(p, tol) && K_all_ == e->calibration() - && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) && K_all_ == e->calibration() && + alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; } /** * Compute jacobian F, E and error vector at a given linearization point * @param values Values structure which must contain camera poses * corresponding to keys involved in this factor - * @return Return arguments are the camera jacobians Fs (including the jacobian with - * respect to both body poses we interpolate from), the point Jacobian E, - * and the error vector b. Note that the jacobians are computed for a given point. + * @return Return arguments are the camera jacobians Fs (including the + * jacobian with respect to both body poses we interpolate from), the point + * Jacobian E, and the error vector b. Note that the jacobians are computed + * for a given point. */ void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { @@ -258,33 +288,38 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactormeasured_.size(); - E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) + E = Matrix::Zero(2 * numViews, + 3); // a Point2 for each view (point jacobian) b = Vector::Zero(2 * numViews); // a Point2 for each view // intermediate Jacobians Eigen::Matrix dProject_dPoseCam; Eigen::Matrix dInterpPose_dPoseBody1, - dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; Eigen::Matrix Ei; for (size_t i = 0; i < numViews; i++) { // for each camera/measurement - const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); - const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + auto w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + auto w_P_body2 = values.at(world_P_body_key_pairs_[i].second); double interpolationFactor = alphas_[i]; // get interpolated pose: - const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - const Pose3& body_P_cam = body_P_sensors_[i]; - const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + auto w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor, + dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); + auto body_P_cam = body_P_sensors_[i]; + auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); PinholeCamera camera(w_P_cam, *K_all_[i]); // get jacobians and error vector for current measurement - Point2 reprojectionError_i = Point2( - camera.project(*this->result_, dProject_dPoseCam, Ei) - - this->measured_.at(i)); + Point2 reprojectionError_i = + Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) - + this->measured_.at(i)); Eigen::Matrix J; // 2 x 12 - J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose - * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) - J.block(0, 6, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose - * dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) + J.block(0, 0, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) + J.block(0, 6, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) // fit into the output structures Fs.push_back(J); @@ -296,37 +331,40 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor > createHessianFactor( - const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const { - - // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), - // hence the number of unique keys may be smaller than 2 * nrMeasurements - size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys + boost::shared_ptr> createHessianFactor( + const Values& values, const double lambda = 0.0, + bool diagonalDamping = false) const { + // we may have multiple observation sharing the same keys (due to the + // rolling shutter interpolation), hence the number of unique keys may be + // smaller than 2 * nrMeasurements + size_t nrUniqueKeys = + this->keys_ + .size(); // note: by construction, keys_ only contains unique keys // Create structures for Hessian Factors KeyVector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector < Vector > gs(nrUniqueKeys); + std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector gs(nrUniqueKeys); - if (this->measured_.size() != this->cameras(values).size()) // 1 observation per interpolated camera - throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "measured_.size() inconsistent with input"); + if (this->measured_.size() != + this->cameras(values).size()) // 1 observation per interpolated camera + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); if (!this->result_) { // failed: return "empty/zero" Hessian if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, 0.0); + for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) v = Vector::Zero(DimPose); + return boost::make_shared>(this->keys_, + Gs, gs, 0.0); } else { - throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); } } // compute Jacobian given triangulated 3D Point @@ -342,21 +380,23 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor( Fs, E, P, b, nonuniqueKeys, this->keys_); - return boost::make_shared < RegularHessianFactor - > (this->keys_, augmentedHessianUniqueKeys); + return boost::make_shared>( + this->keys_, augmentedHessianUniqueKeys); } /** @@ -365,7 +405,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactoractive(values)) { return this->totalReprojectionError(this->cameras(values)); - } else { // else of active flag + } else { // else of active flag return 0.0; } } @@ -385,10 +425,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor(world_P_body_key_pairs_[i].first); - const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + const Pose3& w_P_body1 = + values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = + values.at(world_P_body_key_pairs_[i].second); double interpolationFactor = alphas_[i]; - const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); + const Pose3& w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor); const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& w_P_cam = w_P_body.compose(body_P_cam); cameras.emplace_back(w_P_cam, K_all_[i]); @@ -397,44 +440,46 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor linearizeDamped( const Values& values, const double lambda = 0.0) const { - // depending on flag set on construction we may linearize to different linear factors + // depending on flag set on construction we may linearize to different + // linear factors switch (this->params_.linearizationMode) { case HESSIAN: return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); + "SmartProjectionPoseFactorRollingShutter: unknown linearization " + "mode"); } } /// linearize - boost::shared_ptr linearize(const Values& values) const - override { + boost::shared_ptr linearize( + const Values& values) const override { return this->linearizeDamped(values); } private: /// Serialization function friend class boost::serialization::access; - template + template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(K_all_); } - }; // end of class declaration /// traits -template -struct traits > : -public Testable > { -}; +template +struct traits> + : public Testable> {}; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 943e350d4..161c9aa55 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -16,34 +16,33 @@ * @date July 2021 */ -#include +#include #include -#include -#include +#include #include #include -#include -#include #include - -#include +#include +#include +#include +#include using namespace std::placeholders; using namespace std; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w=640,h=480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static double fov = 60; // degrees +static size_t w = 640, h = 480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); // Create a noise model for the pixel error static SharedNoiseModel model(noiseModel::Unit::Create(2)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; using symbol_shorthand::T; +using symbol_shorthand::X; // Convenience to define common variables across many tests static Key poseKey1(X(1)); @@ -51,74 +50,84 @@ static Key poseKey2(X(2)); static Key pointKey(L(1)); static double interp_params = 0.5; static Point2 measurement(323.0, 240.0); -static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Constructor) { - ProjectionFactorRollingShutter factor(measurement, interp_params, model, poseKey1, poseKey2, pointKey, K); -} - -/* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ConstructorWithTransform) { +TEST(ProjectionFactorRollingShutter, Constructor) { ProjectionFactorRollingShutter factor(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K); } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Equals ) { - { // factors are equal - ProjectionFactorRollingShutter factor1(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); +TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Equals) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal (keys are different) - ProjectionFactorRollingShutter factor1(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, interp_params, - model, poseKey1, poseKey1, pointKey, K); - CHECK(!assert_equal(factor1, factor2)); // not equal + { // factors are NOT equal (keys are different) + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey1, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal } - { // factors are NOT equal (different interpolation) - ProjectionFactorRollingShutter factor1(measurement, 0.1, - model, poseKey1, poseKey1, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, 0.5, - model, poseKey1, poseKey2, pointKey, K); - CHECK(!assert_equal(factor1, factor2)); // not equal + { // factors are NOT equal (different interpolation) + ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1, + poseKey1, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1, + poseKey2, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { - { // factors are equal +TEST(ProjectionFactorRollingShutter, EqualsWithTransform) { + { // factors are equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal + { // factors are NOT equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); - Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + Pose3 body_P_sensor2( + Rot3::RzRyRx(0.0, 0.0, 0.0), + Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor2); + poseKey1, poseKey2, pointKey, K, + body_P_sensor2); CHECK(!assert_equal(factor1, factor2)); } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Error ) { +TEST(ProjectionFactorRollingShutter, Error) { { // Create the factor with a measurement that is 3 pixels off in x // Camera pose corresponds to the first camera double t = 0.0; - ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); // Set the linearization point - Pose3 pose1(Rot3(), Point3(0,0,-6)); - Pose3 pose2(Rot3(), Point3(0,0,-4)); + Pose3 pose1(Rot3(), Point3(0, 0, -6)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); Point3 point(0.0, 0.0, 0.0); // Use the factor to calculate the error @@ -134,11 +143,12 @@ TEST( ProjectionFactorRollingShutter, Error ) { // Create the factor with a measurement that is 3 pixels off in x // Camera pose is actually interpolated now double t = 0.5; - ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); // Set the linearization point - Pose3 pose1(Rot3(), Point3(0,0,-8)); - Pose3 pose2(Rot3(), Point3(0,0,-4)); + Pose3 pose1(Rot3(), Point3(0, 0, -8)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); Point3 point(0.0, 0.0, 0.0); // Use the factor to calculate the error @@ -153,15 +163,16 @@ TEST( ProjectionFactorRollingShutter, Error ) { { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -175,19 +186,20 @@ TEST( ProjectionFactorRollingShutter, Error ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { +TEST(ProjectionFactorRollingShutter, ErrorWithTransform) { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); - Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); - PinholeCamera camera(poseInterp*body_P_sensor3, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -200,18 +212,19 @@ TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Jacobian ) { +TEST(ProjectionFactorRollingShutter, Jacobian) { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -222,22 +235,25 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -245,19 +261,20 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { +TEST(ProjectionFactorRollingShutter, JacobianWithTransform) { // Create measurement by projecting 3D landmark double t = 0.6; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); - Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); - PinholeCamera camera(poseInterp*body_P_sensor3, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -268,22 +285,25 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -291,41 +311,48 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, cheirality ) { +TEST(ProjectionFactorRollingShutter, cheirality) { // Create measurement by projecting 3D landmark behind camera double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera + Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - Point2 measured = Point2(0.0,0.0); // project would throw an exception - { // check that exception is thrown if we set throwCheirality = true + Point2 measured = Point2(0.0, 0.0); // project would throw an exception + { // check that exception is thrown if we set throwCheirality = true bool throwCheirality = true; bool verboseCheirality = true; - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point), CheiralityException); } - { // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct - bool throwCheirality = false; // default - bool verboseCheirality = false; // default - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + { // check that exception is NOT thrown if we set throwCheirality = false, + // and outputs are correct + bool throwCheirality = false; // default + bool verboseCheirality = false; // default + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); // Use the factor to calculate the error Matrix H1Actual, H2Actual, H3Actual; - Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual)); + Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, + H2Actual, H3Actual)); // The expected error is zero - Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera + Vector expectedError = Vector2::Constant( + 2.0 * K->fx()); // this is what we return when point is behind camera // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); - CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5)); - CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5)); - CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5)); } #else { @@ -333,7 +360,8 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -344,22 +372,25 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -368,8 +399,9 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { #endif } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index bf8a8c4ab..0b94d2c3f 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -16,17 +16,19 @@ * @date July 2021 */ -#include "gtsam/slam/tests/smartFactorScenarios.h" -#include -#include -#include -#include -#include +#include #include #include -#include +#include +#include +#include +#include +#include + #include #include + +#include "gtsam/slam/tests/smartFactorScenarios.h" #define DISABLE_TIMING using namespace gtsam; @@ -39,8 +41,8 @@ static const double sigma = 0.1; static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -48,8 +50,8 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Symbol x4('X', 4); static Symbol l0('L', 0); -static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), - Point3(0.1, 0.0, 0.0)); +static Pose3 body_P_sensor = + Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0)); static Point2 measurement1(323.0, 240.0); static Point2 measurement2(200.0, 220.0); @@ -64,38 +66,39 @@ static double interp_factor3 = 0.5; namespace vanillaPoseRS { typedef PinholePose Camera; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); -Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); -Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); -Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above, level_pose, interp_factor3); Camera cam1(interp_pose1, sharedK); Camera cam2(interp_pose2, sharedK); Camera cam3(interp_pose3, sharedK); -} +} // namespace vanillaPoseRS LevenbergMarquardtParams lmParams; -typedef SmartProjectionPoseFactorRollingShutter< PinholePose > SmartFactorRS; +typedef SmartProjectionPoseFactorRollingShutter> + SmartFactorRS; /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { SmartProjectionParams params; params.setRankTolerance(rankTol); SmartFactorRS factor1(model, params); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, add) { +TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPose; SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { +TEST(SmartProjectionPoseFactorRollingShutter, Equals) { using namespace vanillaPose; // create fake measurements @@ -104,10 +107,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { measurements.push_back(measurement2); measurements.push_back(measurement3); - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x4)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x4)); std::vector> intrinsicCalibrations; intrinsicCalibrations.push_back(sharedK); @@ -126,57 +129,67 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); - factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, + extrinsicCalibrations); // create by adding a batch of measurements with a single calibrations SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); - { // create equal factors and show equal returns true + { // create equal factors and show equal returns true SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(assert_equal(*factor1, *factor2)); - EXPECT(assert_equal(*factor1, *factor3)); + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); } - { // create slightly different factors (different keys) and show equal returns false + { // create slightly different factors (different keys) and show equal + // returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different extrinsics) and show equal returns false + { // create slightly different factors (different extrinsics) and show equal + // returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, + body_P_sensor * body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different interp factors) and show equal returns false + { // create slightly different factors (different interp factors) and show + // equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } } -static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated -static const int ZDim = 2; ///< Measurement dimension (Point2) -typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) -typedef std::vector > FBlocks; // vector of F blocks +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from + ///< which the observation pose is interpolated +static const int ZDim = 2; ///< Measurement dimension (Point2) +typedef Eigen::Matrix + MatrixZD; // F blocks (derivatives wrt camera) +typedef std::vector> + FBlocks; // vector of F blocks /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { +TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { using namespace vanillaPoseRS; // Project two landmarks into two cameras @@ -188,7 +201,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, level_pose); values.insert(x2, pose_right); values.insert(x3, pose_above); @@ -200,41 +213,56 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { // Check triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal( + landmark1, + *point)); // check triangulation result matches expected 3D landmark // Check Jacobians // -- actual Jacobians FBlocks actualFs; Matrix actualE; Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorId); Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); - EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + Vector expectedb1 = factor1.evaluateError( + level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, body_P_sensorId); Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); - EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + Vector expectedb2 = factor2.evaluateError( + pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { // also includes non-identical extrinsic calibration using namespace vanillaPoseRS; @@ -246,9 +274,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { SmartFactorRS factor(model); factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); - factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, + body_P_sensorNonId); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, level_pose); values.insert(x2, pose_right); values.insert(x3, pose_above); @@ -256,7 +285,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { // Perform triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid Point3 landmarkNoisy = *point; // Check Jacobians @@ -264,32 +293,48 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { FBlocks actualFs; Matrix actualE; Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorNonId); Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); - EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + Vector expectedb1 = + factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, + expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, + body_P_sensorNonId); Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); - EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + Vector expectedb2 = + factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, + expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); // Check errors - double actualError = factor.error(values); // from smart factor + double actualError = factor.error(values); // from smart factor NonlinearFactorGraph nfg; nfg.add(factor1); nfg.add(factor2); @@ -299,8 +344,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { - +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -310,10 +354,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); @@ -344,20 +388,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -366,11 +412,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { - // here we replicate a test in SmartProjectionPoseFactor by setting interpolation - // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements) - // Note: this is a quite extreme test since in typical camera you would not have more than - // 1 measurement per landmark at each interpolated pose +TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { + // here we replicate a test in SmartProjectionPoseFactor by setting + // interpolation factors to 0 and 1 (such that the rollingShutter measurements + // falls back to standard pixel measurements) Note: this is a quite extreme + // test since in typical camera you would not have more than 1 measurement per + // landmark at each interpolated pose using namespace vanillaPose; // Default cameras for simple derivatives @@ -423,7 +470,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { perturbedDelta.insert(x2, delta); double expectedError = 2500; - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + // After eliminating the point, A1 and A2 contain 2-rank information on + // cameras: Matrix16 A1, A2; A1 << -10, 0, 0, 0, 1, 0; A2 << 10, 0, 1, 0, -1, 0; @@ -449,8 +497,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); @@ -458,7 +506,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -478,7 +526,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - double excludeLandmarksFutherThanDist = 1e10; //very large + double excludeLandmarksFutherThanDist = 1e10; // very large SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); @@ -486,13 +534,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -509,7 +557,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); // Optimization should correct 3rd pose @@ -520,7 +569,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_landmarkDistance) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -548,13 +598,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -571,10 +621,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - // All factors are disabled (due to the distance threshold) and pose should remain where it is + // All factors are disabled (due to the distance threshold) and pose should + // remain where it is Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); @@ -582,7 +634,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_dynamicOutlierRejection) { using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -594,7 +647,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); - measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10); // add outlier + measurements_lmk4.at(0) = + measurements_lmk4.at(0) + Point2(10, 10); // add outlier // create inputs std::vector> key_pairs; @@ -608,7 +662,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie interp_factors.push_back(interp_factor3); double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error SmartProjectionParams params; params.setRankTolerance(1.0); @@ -640,12 +695,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x2, pose_right, noisePrior); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in + Pose3 noise_pose = Pose3( + Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, + 0.01)); // smaller noise, otherwise outlier rejection will kick in Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); // Optimization should correct 3rd pose @@ -656,8 +714,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { - +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -683,10 +741,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -695,8 +758,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -714,46 +777,52 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, model, x1, x2, l0, sharedK); Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual; ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual; ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual; // whiten - F = (1/sigma) * F; - E = (1/sigma) * E; - b = (1/sigma) * b; + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -771,9 +840,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { - // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark - // at a single pose, a setup that occurs in multi-camera systems +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -783,7 +854,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // create redundant measurements: Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement // create inputs std::vector> key_pairs; @@ -799,17 +871,23 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli interp_factors.push_back(interp_factor1); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, + sharedK); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -818,8 +896,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -828,62 +906,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli TriangulationResult point = smartFactor1->point(); EXPECT(point.valid()); // check triangulated point is valid - // Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians + // Use standard ProjectionFactorRollingShutter factor to calculate the + // Jacobians Matrix F = Matrix::Zero(2 * 4, 6 * 3); Matrix E = Matrix::Zero(2 * 4, 3); Vector b = Vector::Zero(8); // create projection factors rolling shutter - ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1, - model, x1, x2, l0, sharedK); + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], + interp_factor1, model, x1, x2, l0, + sharedK); Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual; - ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2, - model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], + interp_factor2, model, x2, x3, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual; - ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3, - model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], + interp_factor3, model, x3, x1, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual; - ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1, - model, x1, x2, l0, sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], + interp_factor1, model, x1, x2, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(6, 0) = H1Actual; F.block<2, 6>(6, 6) = H2Actual; E.block<2, 3>(6, 0) = H3Actual; // whiten - F = (1/sigma) * F; - E = (1/sigma) * E; - b = (1/sigma) * b; + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -902,8 +992,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) { - +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_measurementsFromSamePose) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -913,27 +1003,32 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - // For first factor, we create redundant measurement (taken by the same keys as factor 1, to - // make sure the redundancy in the keys does not create problems) + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement - std::vector> key_pairs_redundant = key_pairs; - key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back( + key_pairs.at(0)); // we readd the first pair of keys std::vector interp_factors_redundant = interp_factors; - interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor + interp_factors_redundant.push_back( + interp_factors.at(0)); // we readd the first interp factor SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, + interp_factors_redundant, sharedK); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); @@ -956,20 +1051,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -980,11 +1077,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF #ifndef DISABLE_TIMING #include // -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) -//| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: +// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 +// children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, timing ) { - +TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; // Default cameras for simple derivatives @@ -1007,14 +1104,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { size_t nrTests = 1000; - for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); Values values; values.insert(x1, pose1); @@ -1024,7 +1121,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { gttoc_(SF_RS_LINEARIZE); } - for(size_t i = 0; iadd(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1046,4 +1143,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bdda15acb..a5fdc80a6 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i + ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h new file mode 100644 index 000000000..d07a75f6f --- /dev/null +++ b/python/gtsam/preamble/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h new file mode 100644 index 000000000..da8842eaf --- /dev/null +++ b/python/gtsam/specializations/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ From 65837c103010beb48ddc152bc684983c1fd8671b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Aug 2021 04:21:57 -0400 Subject: [PATCH 024/169] Fix bug in FourierBasis --- gtsam/basis/Fourier.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index 6943150d7..d264e182d 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -40,9 +40,13 @@ class GTSAM_EXPORT FourierBasis : public Basis { static Weights CalculateWeights(size_t N, double x) { Weights b(N); b[0] = 1; - for (size_t i = 1, n = 1; i < N; i += 2, n++) { - b[i] = cos(n * x); - b[i + 1] = sin(n * x); + for (size_t i = 1, n = 1; i < N; i++) { + if (i % 2 == 1) { + b[i] = cos(n * x); + } else { + b[i] = sin(n * x); + n++; + } } return b; } From 289cb8f35b1b68f280b220f3eb755a5d44148eae Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Aug 2021 04:36:57 -0400 Subject: [PATCH 025/169] break down tests to make reporting clearer --- python/gtsam/tests/test_basis.py | 92 ++++++++++++++++++++++---------- 1 file changed, 65 insertions(+), 27 deletions(-) diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 3af0a87f1..5d3c5ace3 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -19,39 +19,77 @@ from gtsam.symbol_shorthand import B class TestBasis(GtsamTestCase): """ - Tests Basis module python bindings. + Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and Chebyshev2. + + It tests FitBasis by fitting to a ground-truth function that can be represented exactly in + the basis, then checking that the regression (fit result) matches the function. For the + Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is + used. """ - def test_fit_basis(self): + def setUp(self): + self.N = 2 + self.x = [0., 0.5, 0.75] + self.interpx = np.linspace(0., 1., 10) + self.noise = gtsam.noiseModel.Unit.Create(1) + + def evaluate(self, basis, fitparams, x): """ - Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and - Chebyshev2. - It tests FitBasis by fitting to a ground-truth function that can be represented exactly in - the basis, then checking that the regression (fit result) matches the function. For the - Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is - used. + Until wrapper for Basis functors are ready, + this is how to evaluate a basis function. """ - f = lambda x: x # line y = x - N = 2 - datax = [0., 0.5, 0.75] - interpx = np.linspace(0., 1., 10) - noise = gtsam.noiseModel.Unit.Create(1) + return basis.WeightMatrix(self.N, x) @ fitparams - def evaluate(basis, fitparams, x): - # until wrapper for Basis functors are ready, this is how to evaluate a basis function. - return basis.WeightMatrix(N, x) @ fitparams + def fit_basis_helper(self, fitter, basis, f=lambda x: x): + """Helper method to fit data to a specified fitter using a specified basis.""" + data = {x: f(x) for x in self.x} + fit = fitter(data, self.noise, self.N) + coeff = fit.parameters() + interpy = self.evaluate(basis, coeff, self.interpx) + return interpy - def testBasis(fitter, basis, f=f): - # test a basis by checking that the fit result matches the function at x-values interpx. - data = {x: f(x) for x in datax} - fit = fitter(data, noise, N) - coeff = fit.parameters() - interpy = evaluate(basis, coeff, interpx) - np.testing.assert_almost_equal(interpy, np.array([f(x) for x in interpx]), decimal=7) + def test_fit_basis_fourier(self): + """Fit a Fourier basis.""" - testBasis(gtsam.FitBasisFourierBasis, gtsam.FourierBasis, f=lambda x: 0.7 * np.cos(x)) - testBasis(gtsam.FitBasisChebyshev1Basis, gtsam.Chebyshev1Basis) - testBasis(gtsam.FitBasisChebyshev2Basis, gtsam.Chebyshev2Basis) - testBasis(gtsam.FitBasisChebyshev2, gtsam.Chebyshev2) + f = lambda x: 0.7 * np.cos(x) + interpy = self.fit_basis_helper(gtsam.FitBasisFourierBasis, + gtsam.FourierBasis, f) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev1basis(self): + """Fit a Chebyshev1 basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev1Basis, + gtsam.Chebyshev1Basis, f) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev2basis(self): + """Fit a Chebyshev2 basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev2Basis, + gtsam.Chebyshev2Basis) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev2(self): + """Fit a Chebyshev2 pseudospectral basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev2, + gtsam.Chebyshev2) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) if __name__ == "__main__": From 0b9c368acaebf5bfa4a1e5a64f6301289fffec27 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 16:33:13 -0400 Subject: [PATCH 026/169] Removed types defined in Base class --- gtsam/slam/SmartProjectionFactorP.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index b2076cc14..b01420446 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -71,8 +71,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor { public: typedef CAMERA Camera; typedef CameraSet Cameras; - typedef Eigen::Matrix MatrixZD; // F blocks - typedef std::vector > FBlocks; // vector of F blocks /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -234,7 +232,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { * respect to both body poses we interpolate from), the point Jacobian E, * and the error vector b. Note that the jacobians are computed for a given point. */ - void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, + void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs, + Matrix& E, Vector& b, const Cameras& cameras) const { if (!this->result_) { throw("computeJacobiansWithTriangulatedPoint"); @@ -289,7 +288,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { } // compute Jacobian given triangulated 3D Point - FBlocks Fs; + typename Base::FBlocks Fs; Matrix E; Vector b; this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); @@ -300,7 +299,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { Fs[i] = this->noiseModel_->Whiten(Fs[i]); } - Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); // Build augmented Hessian (with last row/column being the information vector) // Note: we need to get the augumented hessian wrt the unique keys in key_ From e0946dfc8653b007658eb212f4ff381ea8644f47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 16:33:50 -0400 Subject: [PATCH 027/169] Documented linear factors better. --- gtsam/slam/JacobianFactorSVD.h | 55 +++++++++++++------------ gtsam/slam/RegularImplicitSchurFactor.h | 41 ++++++++++++++---- 2 files changed, 61 insertions(+), 35 deletions(-) diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index bc906d24e..f6bc1dd8c 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -9,20 +9,21 @@ namespace gtsam { /** - * JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis + * JacobianFactor for Schur complement that uses the "Nullspace Trick" by + * Mourikis et al. * * This trick is equivalent to the Schur complement, but can be faster. - * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, - * is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., - * The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) - * where Enull is an m x (m-3) matrix - * Then Enull'*E*dp = 0, and + * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are + * poses, is multiplied by Enull, a matrix that spans the left nullspace of E, + * i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * + * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and * |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| * Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix. * - * The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. - * Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) - * Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult + * The code below assumes that F is block diagonal and is given as a vector of + * ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for + * D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as + * a 1x2 * 2x6 multiplication. */ template class JacobianFactorSVD: public RegularJacobianFactor { @@ -37,10 +38,10 @@ public: JacobianFactorSVD() { } - /// Empty constructor with keys - JacobianFactorSVD(const KeyVector& keys, // - const SharedDiagonal& model = SharedDiagonal()) : - Base() { + /// Empty constructor with keys. + JacobianFactorSVD(const KeyVector& keys, + const SharedDiagonal& model = SharedDiagonal()) + : Base() { Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -51,18 +52,21 @@ public: } /** - * @brief Constructor - * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) - * and a reduced point derivative, Enull - * and creates a reduced-rank Jacobian factor on the CameraSet + * @brief Construct a new JacobianFactorSVD object, createing a reduced-rank + * Jacobian factor on the CameraSet. * - * @Fblocks: + * @param keys keys associated with F blocks. + * @param Fblocks CameraSet derivatives, ZDim*D blocks of block-diagonal F + * @param Enull a reduced point derivative + * @param b right-hand side + * @param model noise model */ - JacobianFactorSVD(const KeyVector& keys, - const std::vector >& Fblocks, const Matrix& Enull, - const Vector& b, // - const SharedDiagonal& model = SharedDiagonal()) : - Base() { + JacobianFactorSVD( + const KeyVector& keys, + const std::vector >& Fblocks, + const Matrix& Enull, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) + : Base() { size_t numKeys = Enull.rows() / ZDim; size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? // PLAIN nullptr SPACE TRICK @@ -74,9 +78,8 @@ public: QF.reserve(numKeys); for (size_t k = 0; k < Fblocks.size(); ++k) { Key key = keys[k]; - QF.push_back( - KeyMatrix(key, - (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k])); + QF.emplace_back( + key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]); } JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 2ed6aa491..b4a341719 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -1,6 +1,6 @@ /** * @file RegularImplicitSchurFactor.h - * @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor + * @brief A subclass of GaussianFactor specialized to structureless SFM. * @author Frank Dellaert * @author Luca Carlone */ @@ -20,6 +20,20 @@ namespace gtsam { /** * RegularImplicitSchurFactor + * + * A specialization of a GaussianFactor to structure-less SFM, which is very + * fast in a conjugate gradient (CG) solver. Specifically, as measured in + * timeSchurFactors.cpp, it stays very fast for an increasing number of cameras. + * The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at + * the core of CG, and implements + * y += F'*alpha*(I - E*P*E')*F*x + * where + * - F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses + * - E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point + * - P is the covariance on the point + * The equation above implicitly executes the Schur complement by removing the + * information E*P*E' from the Hessian. It is also very fast as we do not use + * the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks. */ template class RegularImplicitSchurFactor: public GaussianFactor { @@ -38,9 +52,10 @@ protected: static const int ZDim = traits::dimension; ///< Measurement dimension typedef Eigen::Matrix MatrixZD; ///< type of an F block - typedef Eigen::Matrix MatrixDD; ///< camera hessian + typedef Eigen::Matrix MatrixDD; ///< camera Hessian + typedef std::vector > FBlocks; - const std::vector > FBlocks_; ///< All ZDim*D F blocks (one for each camera) + FBlocks FBlocks_; ///< All ZDim*D F blocks (one for each camera) const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Vector b_; ///< 2m-dimensional RHS vector @@ -52,17 +67,25 @@ public: } /// Construct from blocks of F, E, inv(E'*E), and RHS vector b - RegularImplicitSchurFactor(const KeyVector& keys, - const std::vector >& FBlocks, const Matrix& E, const Matrix& P, - const Vector& b) : - GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { - } + + /** + * @brief Construct a new RegularImplicitSchurFactor object. + * + * @param keys keys corresponding to cameras + * @param Fs All ZDim*D F blocks (one for each camera) + * @param E Jacobian of measurements wrpt point. + * @param P point covariance matrix + * @param b RHS vector + */ + RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs, + const Matrix& E, const Matrix& P, const Vector& b) + : GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {} /// Destructor ~RegularImplicitSchurFactor() override { } - std::vector >& FBlocks() const { + const FBlocks& Fs() const { return FBlocks_; } From 4ef234bbbb485cdee26576ae0ecc0995812d91fe Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 16:46:53 -0400 Subject: [PATCH 028/169] Formatting and better documentation --- gtsam/slam/ProjectionFactor.h | 17 +- gtsam/slam/SmartFactorBase.h | 155 ++++++++++-------- gtsam/slam/SmartProjectionFactor.h | 106 +++++++----- .../slam/SmartStereoProjectionFactor.h | 22 +-- .../slam/SmartStereoProjectionFactorPP.h | 8 +- 5 files changed, 173 insertions(+), 135 deletions(-) diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index ada304f27..169fe8a0a 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -11,7 +11,7 @@ /** * @file ProjectionFactor.h - * @brief Basic bearing factor from 2D measurement + * @brief Reprojection of a LANDMARK to a 2D point. * @author Chris Beall * @author Richard Roberts * @author Frank Dellaert @@ -22,17 +22,20 @@ #include #include +#include +#include #include #include namespace gtsam { /** - * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. - * i.e. the main building block for visual SLAM. + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration is known here. + * The main building block for visual SLAM. * @addtogroup SLAM */ - template + template class GenericProjectionFactor: public NoiseModelFactor2 { protected: @@ -57,9 +60,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Default constructor - GenericProjectionFactor() : - measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { - } + GenericProjectionFactor() : + measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { + } /** * Constructor diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e7584a4da..f815200ce 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -37,12 +37,14 @@ namespace gtsam { /** - * @brief Base class for smart factors + * @brief Base class for smart factors. * This base class has no internal point, but it has a measurement, noise model * and an optional sensor pose. - * This class mainly computes the derivatives and returns them as a variety of factors. - * The methods take a Cameras argument, which should behave like PinholeCamera, and - * the value of a point, which is kept in the base class. + * This class mainly computes the derivatives and returns them as a variety of + * factors. The methods take a CameraSet argument and the value of a + * point, which is kept in the derived class. + * + * @tparam CAMERA should behave like a set of PinholeCamera. */ template class SmartFactorBase: public NonlinearFactor { @@ -64,19 +66,20 @@ protected: /** * As of Feb 22, 2015, the noise model is the same for all measurements and * is isotropic. This allows for moving most calculations of Schur complement - * etc to be moved to CameraSet very easily, and also agrees pragmatically + * etc. to be easily moved to CameraSet, and also agrees pragmatically * with what is normally done. */ SharedIsotropic noiseModel_; /** - * 2D measurement and noise model for each of the m views - * We keep a copy of measurements for I/O and computing the error. + * Measurements for each of the m views. + * We keep a copy of the measurements for I/O and computing the error. * The order is kept the same as the keys that we use to create the factor. */ ZVector measured_; - boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + boost::optional + body_P_sensor_; ///< Pose of the camera in the body frame // Cache for Fblocks, to avoid a malloc ever time we re-linearize mutable FBlocks Fs; @@ -84,16 +87,16 @@ protected: public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW - /// shorthand for a smart pointer to a factor + /// shorthand for a smart pointer to a factor. typedef boost::shared_ptr shared_ptr; - /// We use the new CameraSte data structure to refer to a set of cameras + /// The CameraSet data structure is used to refer to a set of cameras. typedef CameraSet Cameras; - /// Default Constructor, for serialization + /// Default Constructor, for serialization. SmartFactorBase() {} - /// Constructor + /// Construct with given noise model and optional arguments. SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, boost::optional body_P_sensor = boost::none, size_t expectedNumberCameras = 10) @@ -111,12 +114,12 @@ protected: noiseModel_ = sharedIsotropic; } - /// Virtual destructor, subclasses from NonlinearFactor + /// Virtual destructor, subclasses from NonlinearFactor. ~SmartFactorBase() override { } /** - * Add a new measurement and pose/camera key + * Add a new measurement and pose/camera key. * @param measured is the 2m dimensional projection of a single landmark * @param key is the index corresponding to the camera observing the landmark */ @@ -129,9 +132,7 @@ protected: this->keys_.push_back(key); } - /** - * Add a bunch of measurements, together with the camera keys - */ + /// Add a bunch of measurements, together with the camera keys. void add(const ZVector& measurements, const KeyVector& cameraKeys) { assert(measurements.size() == cameraKeys.size()); for (size_t i = 0; i < measurements.size(); i++) { @@ -140,8 +141,8 @@ protected: } /** - * Adds an entire SfM_track (collection of cameras observing a single point). - * The noise is assumed to be the same for all measurements + * Add an entire SfM_track (collection of cameras observing a single point). + * The noise is assumed to be the same for all measurements. */ template void add(const SFM_TRACK& trackToAdd) { @@ -151,17 +152,13 @@ protected: } } - /// get the dimension (number of rows!) of the factor - size_t dim() const override { - return ZDim * this->measured_.size(); - } + /// Return the dimension (number of rows!) of the factor. + size_t dim() const override { return ZDim * this->measured_.size(); } - /** return the measurements */ - const ZVector& measured() const { - return measured_; - } + /// Return the 2D measurements (ZDim, in general). + const ZVector& measured() const { return measured_; } - /// Collect all cameras: important that in key order + /// Collect all cameras: important that in key order. virtual Cameras cameras(const Values& values) const { Cameras cameras; for(const Key& k: this->keys_) @@ -188,25 +185,30 @@ protected: /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { - const This *e = dynamic_cast(&p); - - bool areMeasurementsEqual = true; - for (size_t i = 0; i < measured_.size(); i++) { - if (traits::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false) - areMeasurementsEqual = false; - break; + if (const This* e = dynamic_cast(&p)) { + // Check that all measurements are the same. + for (size_t i = 0; i < measured_.size(); i++) { + if (!traits::Equals(this->measured_.at(i), e->measured_.at(i), tol)) + return false; + } + // If so, check base class. + return Base::equals(p, tol); + } else { + return false; } - return e && Base::equals(p, tol) && areMeasurementsEqual; } /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and - /// derivatives + /// derivatives. This is the error before the noise model is applied. template Vector unwhitenedError( const Cameras& cameras, const POINT& point, boost::optional Fs = boost::none, // boost::optional E = boost::none) const { - Vector ue = cameras.reprojectionError(point, measured_, Fs, E); + // Reproject, with optional derivatives. + Vector error = cameras.reprojectionError(point, measured_, Fs, E); + + // Apply chain rule if body_P_sensor_ is given. if (body_P_sensor_ && Fs) { const Pose3 sensor_P_body = body_P_sensor_->inverse(); constexpr int camera_dim = traits::dimension; @@ -224,52 +226,60 @@ protected: Fs->at(i) = Fs->at(i) * J; } } - correctForMissingMeasurements(cameras, ue, Fs, E); - return ue; + + // Correct the Jacobians in case some measurements are missing. + correctForMissingMeasurements(cameras, error, Fs, E); + + return error; } /** - * This corrects the Jacobians for the case in which some pixel measurement is missing (nan) - * In practice, this does not do anything in the monocular case, but it is implemented in the stereo version + * This corrects the Jacobians for the case in which some 2D measurement is + * missing (nan). In practice, this does not do anything in the monocular + * case, but it is implemented in the stereo version. */ - virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional Fs = boost::none, - boost::optional E = boost::none) const {} + virtual void correctForMissingMeasurements( + const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const {} /** - * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] - * Noise model applied + * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - + * z], with the noise model applied. */ template Vector whitenedError(const Cameras& cameras, const POINT& point) const { - Vector e = cameras.reprojectionError(point, measured_); + Vector error = cameras.reprojectionError(point, measured_); if (noiseModel_) - noiseModel_->whitenInPlace(e); - return e; + noiseModel_->whitenInPlace(error); + return error; } - /** Calculate the error of the factor. - * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. - * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model - * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. - * Will be used in "error(Values)" function required by NonlinearFactor base class + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of + * Gaussian. In this class, we take the raw prediction error \f$ h(x)-z \f$, + * ask the noise model to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and + * then multiply by 0.5. Will be used in "error(Values)" function required by + * NonlinearFactor base class */ template double totalReprojectionError(const Cameras& cameras, const POINT& point) const { - Vector e = whitenedError(cameras, point); - return 0.5 * e.dot(e); + Vector error = whitenedError(cameras, point); + return 0.5 * error.dot(error); } - /// Computes Point Covariance P from E - static Matrix PointCov(Matrix& E) { + /// Computes Point Covariance P from the "point Jacobian" E. + static Matrix PointCov(const Matrix& E) { return (E.transpose() * E).inverse(); } /** - * Compute F, E, and b (called below in both vanilla and SVD versions), where - * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives - * with respect to the point. The value of cameras/point are passed as parameters. - * TODO: Kill this obsolete method + * Compute F, E, and b (called below in both vanilla and SVD versions), where + * F is a vector of derivatives wrpt the cameras, and E the stacked + * derivatives with respect to the point. The value of cameras/point are + * passed as parameters. */ template void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b, @@ -281,7 +291,11 @@ protected: b = -unwhitenedError(cameras, point, Fs, E); } - /// SVD version + /** + * SVD version that produces smaller Jacobian matrices by doing an SVD + * decomposition on E, and returning the left nulkl-space of E. + * See JacobianFactorSVD for more documentation. + * */ template void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull, Vector& b, const Cameras& cameras, const POINT& point) const { @@ -291,14 +305,14 @@ protected: static const int N = FixedDimension::value; // 2 (Unit3) or 3 (Point3) - // Do SVD on A + // Do SVD on A. Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); - Vector s = svd.singularValues(); size_t m = this->keys_.size(); Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns } - /// Linearize to a Hessianfactor + /// Linearize to a Hessianfactor. + // TODO(dellaert): Not used/tested anywhere and not properly whitened. boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -351,9 +365,7 @@ protected: P, b); } - /** - * Return Jacobians as JacobianFactorQ - */ + /// Return Jacobians as JacobianFactorQ. boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -368,8 +380,8 @@ protected: } /** - * Return Jacobians as JacobianFactorSVD - * TODO lambda is currently ignored + * Return Jacobians as JacobianFactorSVD. + * TODO(dellaert): lambda is currently ignored */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { @@ -393,6 +405,7 @@ protected: F.block(ZDim * i, Dim * i) = Fs.at(i); } + // Return sensor pose. Pose3 body_P_sensor() const{ if(body_P_sensor_) return *body_P_sensor_; diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index f67ca0740..9fb9c6905 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -61,10 +61,11 @@ protected: /// @name Caching triangulation /// @{ mutable TriangulationResult result_; ///< result from triangulateSafe - mutable std::vector > cameraPosesTriangulation_; ///< current triangulation poses + mutable std::vector > + cameraPosesTriangulation_; ///< current triangulation poses /// @} -public: + public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -116,21 +117,31 @@ public: && Base::equals(p, tol); } - /// Check if the new linearization point is the same as the one used for previous triangulation + /** + * @brief Check if the new linearization point is the same as the one used for + * previous triangulation. + * + * @param cameras + * @return true if we need to re-triangulate. + */ bool decideIfTriangulate(const Cameras& cameras) const { - // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate - // Note that this is not yet "selecting linearization", that will come later, and we only check if the - // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point + // Several calls to linearize will be done from the same linearization + // point, hence it is not needed to re-triangulate. Note that this is not + // yet "selecting linearization", that will come later, and we only check if + // the current linearization is the "same" (up to tolerance) w.r.t. the last + // time we triangulated the point. size_t m = cameras.size(); bool retriangulate = false; - // if we do not have a previous linearization point or the new linearization point includes more poses + // Definitely true if we do not have a previous linearization point or the + // new linearization point includes more poses. if (cameraPosesTriangulation_.empty() || cameras.size() != cameraPosesTriangulation_.size()) retriangulate = true; + // Otherwise, check poses against cache. if (!retriangulate) { for (size_t i = 0; i < cameras.size(); i++) { if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], @@ -141,7 +152,8 @@ public: } } - if (retriangulate) { // we store the current poses used for triangulation + // Store the current poses used for triangulation if we will re-triangulate. + if (retriangulate) { cameraPosesTriangulation_.clear(); cameraPosesTriangulation_.reserve(m); for (size_t i = 0; i < m; i++) @@ -149,10 +161,15 @@ public: cameraPosesTriangulation_.push_back(cameras[i].pose()); } - return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation + return retriangulate; } - /// triangulateSafe + /** + * @brief Call gtsam::triangulateSafe iff we need to re-triangulate. + * + * @param cameras + * @return TriangulationResult + */ TriangulationResult triangulateSafe(const Cameras& cameras) const { size_t m = cameras.size(); @@ -166,17 +183,21 @@ public: return result_; } - /// triangulate + /** + * @brief Possibly re-triangulate before calculating Jacobians. + * + * @param cameras + * @return true if we could safely triangulate + */ bool triangulateForLinearize(const Cameras& cameras) const { triangulateSafe(cameras); // imperative, might reset result_ return bool(result_); } - /// linearize returns a Hessianfactor that is an approximation of error(p) + /// Create a Hessianfactor that is an approximation of error(p). boost::shared_ptr > createHessianFactor( - const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = - false) const { - + const Cameras& cameras, const double lambda = 0.0, + bool diagonalDamping = false) const { size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors KeyVector js; @@ -184,39 +205,38 @@ public: std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) - throw std::runtime_error("SmartProjectionHessianFactor: this->measured_" - ".size() inconsistent with input"); + throw std::runtime_error( + "SmartProjectionHessianFactor: this->measured_" + ".size() inconsistent with input"); triangulateSafe(cameras); if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian - for(Matrix& m: Gs) - m = Matrix::Zero(Base::Dim, Base::Dim); - for(Vector& v: gs) - v = Vector::Zero(Base::Dim); + for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim); + for (Vector& v : gs) v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, - Gs, gs, 0.0); + Gs, gs, 0.0); } // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). - std::vector > Fblocks; + typename Base::FBlocks Fs; Matrix E; Vector b; - computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); // Whiten using noise model - Base::whitenJacobians(Fblocks, E, b); + Base::whitenJacobians(Fs, E, b); // build augmented hessian - SymmetricBlockMatrix augmentedHessian = // - Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); + SymmetricBlockMatrix augmentedHessian = // + Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping); - return boost::make_shared >(this->keys_, - augmentedHessian); + return boost::make_shared >( + this->keys_, augmentedHessian); } - // create factor + // Create RegularImplicitSchurFactor factor. boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) @@ -226,7 +246,7 @@ public: return boost::shared_ptr >(); } - /// create factor + /// Create JacobianFactorQ factor. boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) @@ -236,13 +256,13 @@ public: return boost::make_shared >(this->keys_); } - /// Create a factor, takes values + /// Create JacobianFactorQ factor, takes values. boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { return createJacobianQFactor(this->cameras(values), lambda); } - /// different (faster) way to compute Jacobian factor + /// Different (faster) way to compute a JacobianFactorSVD factor. boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) @@ -252,19 +272,19 @@ public: return boost::make_shared >(this->keys_); } - /// linearize to a Hessianfactor + /// Linearize to a Hessianfactor. virtual boost::shared_ptr > linearizeToHessian( const Values& values, double lambda = 0.0) const { return createHessianFactor(this->cameras(values), lambda); } - /// linearize to an Implicit Schur factor + /// Linearize to an Implicit Schur factor. virtual boost::shared_ptr > linearizeToImplicit( const Values& values, double lambda = 0.0) const { return createRegularImplicitSchurFactor(this->cameras(values), lambda); } - /// linearize to a JacobianfactorQ + /// Linearize to a JacobianfactorQ. virtual boost::shared_ptr > linearizeToJacobian( const Values& values, double lambda = 0.0) const { return createJacobianQFactor(this->cameras(values), lambda); @@ -334,7 +354,7 @@ public: /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate void computeJacobiansWithTriangulatedPoint( - std::vector >& Fblocks, Matrix& E, Vector& b, + typename Base::FBlocks& Fs, Matrix& E, Vector& b, const Cameras& cameras) const { if (!result_) { @@ -342,32 +362,32 @@ public: // TODO check flag whether we should do this Unit3 backProjected = cameras[0].backprojectPointAtInfinity( this->measured_.at(0)); - Base::computeJacobians(Fblocks, E, b, cameras, backProjected); + Base::computeJacobians(Fs, E, b, cameras, backProjected); } else { // valid result: just return Base version - Base::computeJacobians(Fblocks, E, b, cameras, *result_); + Base::computeJacobians(Fs, E, b, cameras, *result_); } } /// Version that takes values, and creates the point bool triangulateAndComputeJacobians( - std::vector >& Fblocks, Matrix& E, Vector& b, + typename Base::FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); return nonDegenerate; } /// takes values bool triangulateAndComputeJacobiansSVD( - std::vector >& Fblocks, Matrix& Enull, Vector& b, + typename Base::FBlocks& Fs, Matrix& Enull, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_); return nonDegenerate; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index e361dddac..88e112998 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -447,23 +447,23 @@ public: } /** - * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) + * This corrects the Jacobians and error vector for the case in which the + * right 2D measurement in the monocular camera is missing (nan). */ - void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, - boost::optional Fs = boost::none, - boost::optional E = boost::none) const override - { + void correctForMissingMeasurements( + const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const override { // when using stereo cameras, some of the measurements might be missing: - for(size_t i=0; i < cameras.size(); i++){ + for (size_t i = 0; i < cameras.size(); i++) { const StereoPoint2& z = measured_.at(i); - if(std::isnan(z.uR())) // if the right pixel is invalid + if (std::isnan(z.uR())) // if the right 2D measurement is invalid { - if(Fs){ // delete influence of right point on jacobian Fs + if (Fs) { // delete influence of right point on jacobian Fs MatrixZD& Fi = Fs->at(i); - for(size_t ii=0; iirow(ZDim * i + 1) = Matrix::Zero(1, E->cols()); // set the corresponding entry of vector ue to zero diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 25be48b0f..ce6df15cb 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -33,9 +33,11 @@ namespace gtsam { */ /** - * This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body). - * Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. - * This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). + * This factor optimizes the pose of the body as well as the extrinsic camera + * calibration (pose of camera wrt body). Each camera may have its own extrinsic + * calibration or the same calibration can be shared by multiple cameras. This + * factor requires that values contain the involved poses and extrinsics (both + * are Pose3 variables). * @addtogroup SLAM */ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { From 372ae27a5e66b9c0b53c9ec5f09189562c07f151 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 16:47:32 -0400 Subject: [PATCH 029/169] Added two ReadMe files to document the plethora of smart factors. --- gtsam/slam/ReadMe.md | 64 +++++++++++++++++++++++++++++++++++ gtsam_unstable/slam/ReadMe.md | 39 +++++++++++++++++++++ 2 files changed, 103 insertions(+) create mode 100644 gtsam/slam/ReadMe.md create mode 100644 gtsam_unstable/slam/ReadMe.md diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md new file mode 100644 index 000000000..22a5778f2 --- /dev/null +++ b/gtsam/slam/ReadMe.md @@ -0,0 +1,64 @@ +# SLAM Factors + +## GenericProjectionFactor (defined in ProjectionFactor.h) + +Non-linear factor for a constraint derived from a 2D measurement. +The calibration is assumed known and passed in the constructor. +The main building block for visual SLAM. + +Templated on +- `POSE`, default `Pose3` +- `LANDMARK`, default `Point3` +- `CALIBRATION`, default `Cal3_S2` + +## SmartFactors + +These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras. + +### SmartFactorBase + +This is the base class for smart factors, templated on a `CAMERA` type. +It has no internal point, but it saves the measurements, keeps a noise model, and an optional sensor pose. + +### SmartProjectionFactor + +Also templated on `CAMERA`. Triangulates a 3D point and keeps an estimate of it around. +This factor operates with monocular cameras, and is used to optimize the camera pose +*and* calibration for each camera, and requires variables of type `CAMERA` in values. + +If the calibration is fixed use `SmartProjectionPoseFactor` instead! + + +### SmartProjectionPoseFactor + +Derives from `SmartProjectionFactor` but is templated on a `CALIBRATION` type, setting `CAMERA = PinholePose`. +This factor assumes that the camera calibration is fixed and the same for all cameras involved in this factor. +The factor only constrains poses. + +If the calibration should be optimized, as well, use `SmartProjectionFactor` instead! + +### SmartProjectionFactorP + +Same as `SmartProjectionPoseFactor`, except: +- it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole; +- it admits a different calibration for each measurement, i.e., it can model a multi-camera system; +- it allows multiple observations from the same pose/key, again, to model a multi-camera system. + +TODO: DimPose and ZDim are hardcoded. Copy/paste from `SmartProjectionPoseFactor`. Unclear what the use case is. + +### RegularImplicitSchurFactor + +A specialization of a GaussianFactor to structure-less SFM, which is very fast in a conjugate gradient (CG) solver. +It is produced by calling `createRegularImplicitSchurFactor` in `SmartFactorBase` or `SmartProjectionFactor`. + +### JacobianFactorQ + +A RegularJacobianFactor that uses some badly documented reduction on the Jacobians. + +### JacobianFactorQR + +A RegularJacobianFactor that eliminates a point using sequential elimination. + +### JacobianFactorQR + +A RegularJacobianFactor that uses the "Nullspace Trick" by Mourikis et al. See the documentation in the file, which *is* well documented. \ No newline at end of file diff --git a/gtsam_unstable/slam/ReadMe.md b/gtsam_unstable/slam/ReadMe.md new file mode 100644 index 000000000..78d53090a --- /dev/null +++ b/gtsam_unstable/slam/ReadMe.md @@ -0,0 +1,39 @@ +# SLAM Factors + +## SmartFactors + +These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras. + +### SmartRangeFactor + +An experiment in creating a structure-less 2D range-SLAM factor with range-only measurements. +It uses a sophisticated `triangulate` logic based on circle intersections. + +### SmartStereoProjectionFactor + +Version of `SmartProjectionFactor` for stereo observations, specializes SmartFactorBase for `CAMERA == StereoCamera`. + +TODO: a lot of commented out code and could move a lot to .cpp file. + +### SmartStereoProjectionPoseFactor + +Derives from `SmartStereoProjectionFactor` but adds an array of `Cal3_S2Stereo` calibration objects . + +TODO: Again, as no template arguments, we could move a lot to .cpp file. + +### SmartStereoProjectionFactorPP + +Similar `SmartStereoProjectionPoseFactor` but *additionally* adds an array of body_P_cam poses. The dimensions seem to be hardcoded and the types defined in the SmartFactorBase have been re-defined. + +TODO: See above, same issues as `SmartStereoProjectionPoseFactor`. + +### SmartProjectionPoseFactorRollingShutter + +Is templated on a `CAMERA` type and derives from `SmartProjectionFactor`. + +This factor optimizes two consecutive poses of a body assuming a rolling +shutter model of the camera with given readout time. The factor requires that +values contain (for each 2D observation) two consecutive camera poses from +which the 2D observation pose can be interpolated. + +TODO: the dimensions seem to be hardcoded and the types defined in the SmartFactorBase have been re-defined. Also, possibly a lot of copy/paste computation of things that (should) happen in base class. \ No newline at end of file From c0262b074c04e8e3ca75e66c01b08a0dc79bb641 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 19:38:20 -0400 Subject: [PATCH 030/169] Address review comments, docs only. --- gtsam/slam/ProjectionFactor.h | 3 ++- gtsam/slam/ReadMe.md | 8 +++++++- gtsam/slam/SmartFactorBase.h | 2 +- gtsam_unstable/slam/ReadMe.md | 1 + 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 169fe8a0a..42dba8bd0 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -35,7 +35,8 @@ namespace gtsam { * The main building block for visual SLAM. * @addtogroup SLAM */ - template + template class GenericProjectionFactor: public NoiseModelFactor2 { protected: diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md index 22a5778f2..c43f0769a 100644 --- a/gtsam/slam/ReadMe.md +++ b/gtsam/slam/ReadMe.md @@ -2,7 +2,7 @@ ## GenericProjectionFactor (defined in ProjectionFactor.h) -Non-linear factor for a constraint derived from a 2D measurement. +Non-linear factor that minimizes the re-projection error with respect to a 2D measurement. The calibration is assumed known and passed in the constructor. The main building block for visual SLAM. @@ -14,6 +14,7 @@ Templated on ## SmartFactors These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras. +While one typically adds multiple GenericProjectionFactors (one for each observation of a landmark), a SmartFactor collects all measurements for a landmark, i.e., the factor graph contains 1 smart factor per landmark. ### SmartFactorBase @@ -46,6 +47,11 @@ Same as `SmartProjectionPoseFactor`, except: TODO: DimPose and ZDim are hardcoded. Copy/paste from `SmartProjectionPoseFactor`. Unclear what the use case is. +## Linearized Smart Factors + +The factors below are less likely to be relevant to the user, but result from using the non-linear smart factors above. + + ### RegularImplicitSchurFactor A specialization of a GaussianFactor to structure-less SFM, which is very fast in a conjugate gradient (CG) solver. diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index f815200ce..ddf56b289 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -44,7 +44,7 @@ namespace gtsam { * factors. The methods take a CameraSet argument and the value of a * point, which is kept in the derived class. * - * @tparam CAMERA should behave like a set of PinholeCamera. + * @tparam CAMERA should behave like a PinholeCamera. */ template class SmartFactorBase: public NonlinearFactor { diff --git a/gtsam_unstable/slam/ReadMe.md b/gtsam_unstable/slam/ReadMe.md index 78d53090a..9aa0fed78 100644 --- a/gtsam_unstable/slam/ReadMe.md +++ b/gtsam_unstable/slam/ReadMe.md @@ -24,6 +24,7 @@ TODO: Again, as no template arguments, we could move a lot to .cpp file. ### SmartStereoProjectionFactorPP Similar `SmartStereoProjectionPoseFactor` but *additionally* adds an array of body_P_cam poses. The dimensions seem to be hardcoded and the types defined in the SmartFactorBase have been re-defined. +The body_P_cam poses are optimized here! TODO: See above, same issues as `SmartStereoProjectionPoseFactor`. From 13c164f25d4683bd1720b564ee3dcbf67b4ffff5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:38:47 -0400 Subject: [PATCH 031/169] add Pose2.align() to wrapper --- gtsam/geometry/geometry.i | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0e303cbcd..c495c22d9 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -31,6 +31,14 @@ class Point2 { // enable pickling in python void pickle() const; }; + +class Point2Pairs { + Point2Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point2Pair at(size_t n) const; + void push_back(const gtsam::Point2Pair& point_pair); +}; // std::vector class Point2Vector { @@ -428,6 +436,8 @@ class Pose2 { // enable pickling in python void pickle() const; + + gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); }; #include From ce495cb7bc0ab3ed5fac8482e083f629aafeb684 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:41:27 -0400 Subject: [PATCH 032/169] add Point2Pairs typedef to Point2.h --- gtsam/geometry/Point2.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 17f87f656..cdb9f4480 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -25,6 +25,12 @@ namespace gtsam { /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point2 to Vector2 typedef Vector2 Point2; + +// Convenience typedef +using Point2Pair = std::pair; +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); + +using Point2Pairs = std::vector; /// Distance of the point from the origin, with Jacobian GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); @@ -34,10 +40,6 @@ GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q, OptionalJacobian<1, 2> H1 = boost::none, OptionalJacobian<1, 2> H2 = boost::none); -// Convenience typedef -typedef std::pair Point2Pair; -GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); - // For MATLAB wrapper typedef std::vector > Point2Vector; From 8bd2e6a976071ae4f5fa6063ec6d40c5a38de0c9 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:44:11 -0400 Subject: [PATCH 033/169] add gtsam::Point2Pairs to CMakeLists.txt --- python/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 4254a21c6..c3524adad 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -43,6 +43,7 @@ set(ignore gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector + gtsam::Point2Pairs gtsam::Point3Pairs gtsam::Pose3Pairs gtsam::Pose3Vector From 55785f81809777fdc0115a0761a16f10f5ad8ab6 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:44:58 -0400 Subject: [PATCH 034/169] add Point2Pairs to specializations --- python/gtsam/specializations/geometry.h | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index e11d3cc4f..a492ce8eb 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -14,6 +14,7 @@ py::bind_vector< std::vector>>( m_, "Point2Vector"); +py::bind_vector>(m_, "Point2Pairs"); py::bind_vector>(m_, "Point3Pairs"); py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); From 50f3b93324a6041aad6465159b1cddc86724dca4 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 18:01:17 -0600 Subject: [PATCH 035/169] move align as function, not as class method --- gtsam/geometry/geometry.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index c495c22d9..bf906d67f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -436,9 +436,9 @@ class Pose2 { // enable pickling in python void pickle() const; - - gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); }; + +gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); #include class Pose3 { From cff3c5b4f43662c81a095fd9123afeae94f9970c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 06:26:52 -0600 Subject: [PATCH 036/169] start python unit test for align() --- python/gtsam/tests/test_Pose2.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 9652b594a..b44914a94 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -6,7 +6,7 @@ All Rights Reserved See LICENSE for the license information Pose2 unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Author: Frank Dellaert & Duy Nguyen Ta & John Lambert """ import unittest @@ -20,13 +20,20 @@ from gtsam.utils.test_case import GtsamTestCase class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" - def test_adjoint(self): + def test_adjoint(self) -> None: """Test adjoint method.""" xi = np.array([1, 2, 3]) expected = np.dot(Pose2.adjointMap_(xi), xi) actual = Pose2.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) + def test_align(self) -> None: + """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. + + """ + pass + + if __name__ == "__main__": unittest.main() From 2d2ca21d1aea4d5bd4b92523075553598afe60c0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 09:14:59 -0400 Subject: [PATCH 037/169] add python unit test on Pose2.align() --- python/gtsam/tests/test_Pose2.py | 36 ++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index b44914a94..fc9c7acf9 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -30,9 +30,41 @@ class TestPose2(GtsamTestCase): def test_align(self) -> None: """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. - """ - pass + Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: + | X---X + | | + | X---X + ------------------ + | + | + O | O + | | | + O---O + """ + # fmt: off + pts_a = [ + Point2(3, 1), + Point2(1, 1), + Point2(1, 3), + Point2(3, 3), + ] + pts_b = [ + Point2(1, -3), + Point2(1, -5), + Point2(-1, -5), + Point2(-1, -3) + ] + + # fmt: on + ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) + bTa = Pose2.align(ab_pairs) + aTb = bTa.inverse() + assert aTb is not None + + for pt_a, pt_b in zip(pts_a, pts_b): + pt_a_ = aTb.transformFrom(pt_b) + assert np.allclose(pt_a, pt_a_) if __name__ == "__main__": From 5c737c3cc4592a588a6bfd781b277de1feeb67a1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 09:42:49 -0400 Subject: [PATCH 038/169] fix missing imports --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index fc9c7acf9..f18eab079 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Pose2 +from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -47,7 +47,7 @@ class TestPose2(GtsamTestCase): Point2(3, 1), Point2(1, 1), Point2(1, 3), - Point2(3, 3), + Point2(3, 3) ] pts_b = [ Point2(1, -3), From 3fc7692b4a72f62ef944532d554652f369543c6d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 08:35:43 -0600 Subject: [PATCH 039/169] import align from gtsam directly --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index f18eab079..213de655e 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Point2, Point2Pairs, Pose2 +from gtsam import align, Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -58,7 +58,7 @@ class TestPose2(GtsamTestCase): # fmt: on ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) - bTa = Pose2.align(ab_pairs) + bTa = align(ab_pairs) aTb = bTa.inverse() assert aTb is not None From bc641f893dc8b8e16f51ff579f8fd802573a6e78 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 14:14:37 -0400 Subject: [PATCH 040/169] directly import only classes from gtsam --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 213de655e..569046596 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import align, Point2, Point2Pairs, Pose2 +from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -58,7 +58,7 @@ class TestPose2(GtsamTestCase): # fmt: on ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) - bTa = align(ab_pairs) + bTa = gtsam.align(ab_pairs) aTb = bTa.inverse() assert aTb is not None From 6d57016a51b936c014167a693e7315fe40521a66 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 1 Sep 2021 10:15:00 -0600 Subject: [PATCH 041/169] use boost::optional in .i file directly --- gtsam/geometry/geometry.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index bf906d67f..9baa49e8e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -438,7 +438,7 @@ class Pose2 { void pickle() const; }; -gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); +boost::optional align(const gtsam::Point2Pairs& pairs); #include class Pose3 { From cd682fecc3301a8601e8f8bd4ffe951a6bf4bf15 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 14:31:29 -0400 Subject: [PATCH 042/169] add a cmake flag for easy toggling BetweenFactor jacobian computations --- cmake/HandleGeneralOptions.cmake | 29 +++++++++++++++-------------- gtsam/config.h.in | 3 +++ gtsam/slam/BetweenFactor.h | 2 +- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index ee86066a2..64c239f39 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -14,20 +14,21 @@ if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) endif() -option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) -option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) -option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) -option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) -option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) -option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) -option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) -option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) -option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) -option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) -option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) +option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) +option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) +option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) +option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) +option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) +option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) if(NOT MSVC AND NOT XCODE_VERSION) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) endif() diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 9d1bd4ebd..cebc1d0b6 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -77,3 +77,6 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_TANGENT_PREINTEGRATION + +// Toggle switch for BetweenFactor jacobian computation +#cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index aef41d5fd..8a1ffdd72 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -103,7 +103,7 @@ namespace gtsam { boost::none, boost::optional H2 = boost::none) const override { T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) -#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR +#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR typename traits::ChartJacobian::Jacobian Hlocal; Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); if (H1) *H1 = Hlocal * (*H1); From fbdef91c54aff13b6cdb1e80095627b4cd12b217 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:29:29 -0400 Subject: [PATCH 043/169] add support for boost::optional return type in geometry.i --- python/gtsam/preamble/geometry.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 498c7dc58..9ddb6e208 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -10,9 +10,18 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ +#include + +// Support for binding boost::optional types. +// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html +namespace pybind11 { namespace detail { + template + struct type_caster> : optional_caster> {}; +}} PYBIND11_MAKE_OPAQUE( std::vector>); +PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(std::vector); From 9f661c01cf0e7ce144a94a39f1a83343e03f29ed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:29:39 -0400 Subject: [PATCH 044/169] formatting --- python/gtsam/tests/test_Pose2.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 569046596..e5ffbad7d 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -19,7 +19,6 @@ from gtsam.utils.test_case import GtsamTestCase class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" - def test_adjoint(self) -> None: """Test adjoint method.""" xi = np.array([1, 2, 3]) @@ -29,9 +28,9 @@ class TestPose2(GtsamTestCase): def test_align(self) -> None: """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. - + Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: - + | X---X | | | X---X @@ -42,18 +41,17 @@ class TestPose2(GtsamTestCase): | | | O---O """ - # fmt: off pts_a = [ Point2(3, 1), Point2(1, 1), Point2(1, 3), - Point2(3, 3) + Point2(3, 3), ] pts_b = [ Point2(1, -3), Point2(1, -5), Point2(-1, -5), - Point2(-1, -3) + Point2(-1, -3), ] # fmt: on @@ -65,7 +63,7 @@ class TestPose2(GtsamTestCase): for pt_a, pt_b in zip(pts_a, pts_b): pt_a_ = aTb.transformFrom(pt_b) assert np.allclose(pt_a, pt_a_) - + if __name__ == "__main__": unittest.main() From 1205df2c07757d20b01d43827bc88f8b0f696884 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:32:43 -0400 Subject: [PATCH 045/169] update documentation for boost::optional binding --- python/gtsam/preamble/geometry.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 9ddb6e208..35fe2a577 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -12,7 +12,7 @@ */ #include -// Support for binding boost::optional types. +// Support for binding boost::optional types in C++11. // https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html namespace pybind11 { namespace detail { template From 62b1e2ce9cf72d664fd540b2fb0eb78ea674d0d8 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 2 Sep 2021 15:27:49 -0400 Subject: [PATCH 046/169] use updated wrap syntax for Verbosity enum in .i file --- gtsam/nonlinear/nonlinear.i | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index c3f17c02e..39b4f5933 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -527,13 +527,15 @@ template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); - void setVerbosityGNC(const gtsam::GncParams::Verbosity value); + void setVerbosityGNC(const This::Verbosity value); void print(const string& str) const; -}; -typedef gtsam::GncParams::Verbosity::SILENT GncVerbositySilent; -typedef gtsam::GncParams::Verbosity::SUMMARY GncVerbositySummary; -typedef gtsam::GncParams::Verbosity::VALUES GncVerbosityValues; + enum Verbosity { + SILENT, + SUMMARY, + VALUES + }; +}; typedef gtsam::GncParams GncGaussNewtonParams; typedef gtsam::GncParams GncLMParams; From 67a26c1f9359d68e39d3efe7414d8958e4c10e03 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Sep 2021 08:04:59 -0400 Subject: [PATCH 047/169] refactor to remove all pylint errors --- python/gtsam/examples/ImuFactorExample.py | 114 +++++++++++++--------- 1 file changed, 66 insertions(+), 48 deletions(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index bb707a8f5..86613234d 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -10,28 +10,30 @@ A script validating and demonstrating the ImuFactor inference. Author: Frank Dellaert, Varun Agrawal """ +# pylint: disable=no-name-in-module,unused-import,arguments-differ + from __future__ import print_function import argparse import math -import gtsam import matplotlib.pyplot as plt import numpy as np +from mpl_toolkits.mplot3d import Axes3D + +import gtsam from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 -from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample BIAS_KEY = B(0) - np.set_printoptions(precision=3, suppress=True) class ImuFactorExample(PreintegrationExample): - + """Class to run example of the Imu Factor.""" def __init__(self, twist_scenario="sick_twist"): self.velocity = np.array([2, 0, 0]) self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) @@ -42,9 +44,8 @@ class ImuFactorExample(PreintegrationExample): zero_twist=(np.zeros(3), np.zeros(3)), forward_twist=(np.zeros(3), self.velocity), loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity), - sick_twist=(np.array([math.radians(30), -math.radians(30), 0]), - self.velocity) - ) + sick_twist=(np.array([math.radians(30), -math.radians(30), + 0]), self.velocity)) accBias = np.array([-0.3, 0.1, 0.2]) gyroBias = np.array([0.1, 0.3, -0.1]) @@ -55,19 +56,44 @@ class ImuFactorExample(PreintegrationExample): bias, dt) def addPrior(self, i, graph): + """Add priors at time step `i`.""" state = self.scenario.navState(i) - graph.push_back(gtsam.PriorFactorPose3( - X(i), state.pose(), self.priorNoise)) - graph.push_back(gtsam.PriorFactorVector( - V(i), state.velocity(), self.velNoise)) + graph.push_back( + gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) + graph.push_back( + gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise)) + + def optimize(self, graph, initial): + """Optimize using Levenberg-Marquardt optimization.""" + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("SUMMARY") + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + return result + + def plot(self, result): + """Plot resulting poses.""" + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + plot_pose3(POSES_FIG + 1, pose_i, 1) + i += 1 + plt.title("Estimated Trajectory") + + gtsam.utils.plot.set_axes_equal(POSES_FIG + 1) + + print("Bias Values", result.atConstantBias(BIAS_KEY)) + + plt.ioff() + plt.show() def run(self, T=12, compute_covariances=False, verbose=True): + """Main runner.""" graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) - T = 12 num_poses = T # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) @@ -91,14 +117,13 @@ class ImuFactorExample(PreintegrationExample): if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) - if (k+1) % int(1 / self.dt) == 0: + if (k + 1) % int(1 / self.dt) == 0: # Plot every second self.plotGroundTruthPose(t, scale=1) plt.title("Ground Truth Trajectory") # create IMU factor every second - factor = gtsam.ImuFactor(X(i), V(i), - X(i + 1), V(i + 1), + factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) graph.push_back(factor) @@ -108,34 +133,34 @@ class ImuFactorExample(PreintegrationExample): pim.resetIntegration() - rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1) - translationNoise = gtsam.Point3(*np.random.randn(3)*1) + rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1) + translationNoise = gtsam.Point3(*np.random.randn(3) * 1) poseNoise = gtsam.Pose3(rotationNoise, translationNoise) actual_state_i = self.scenario.navState(t + self.dt) print("Actual state at {0}:\n{1}".format( - t+self.dt, actual_state_i)) + t + self.dt, actual_state_i)) noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), - actual_state_i.velocity() + np.random.randn(3)*0.1) + actual_state_i.velocity() + np.random.randn(3) * 0.1) - initial.insert(X(i+1), noisy_state_i.pose()) - initial.insert(V(i+1), noisy_state_i.velocity()) + initial.insert(X(i + 1), noisy_state_i.pose()) + initial.insert(V(i + 1), noisy_state_i.velocity()) i += 1 # add priors on end self.addPrior(num_poses - 1, graph) - initial.print_("Initial values:") + initial.print("Initial values:") - # optimize using Levenberg-Marquardt optimization - params = gtsam.LevenbergMarquardtParams() - params.setVerbosityLM("SUMMARY") - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) - result = optimizer.optimize() + result = self.optimize(graph, initial) - result.print_("Optimized values:") + result.print("Optimized values:") + print("------------------") + print(graph.error(initial)) + print(graph.error(result)) + print("------------------") if compute_covariances: # Calculate and print marginal covariances @@ -148,33 +173,26 @@ class ImuFactorExample(PreintegrationExample): print("Covariance on vel {}:\n{}\n".format( i, marginals.marginalCovariance(V(i)))) - # Plot resulting poses - i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - plot_pose3(POSES_FIG+1, pose_i, 1) - i += 1 - plt.title("Estimated Trajectory") - - gtsam.utils.plot.set_axes_equal(POSES_FIG+1) - - print("Bias Values", result.atConstantBias(BIAS_KEY)) - - plt.ioff() - plt.show() + self.plot(result) if __name__ == '__main__': parser = argparse.ArgumentParser("ImuFactorExample.py") parser.add_argument("--twist_scenario", default="sick_twist", - choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist")) - parser.add_argument("--time", "-T", default=12, - type=int, help="Total time in seconds") + choices=("zero_twist", "forward_twist", "loop_twist", + "sick_twist")) + parser.add_argument("--time", + "-T", + default=12, + type=int, + help="Total time in seconds") parser.add_argument("--compute_covariances", - default=False, action='store_true') + default=False, + action='store_true') parser.add_argument("--verbose", default=False, action='store_true') args = parser.parse_args() - ImuFactorExample(args.twist_scenario).run( - args.time, args.compute_covariances, args.verbose) + ImuFactorExample(args.twist_scenario).run(args.time, + args.compute_covariances, + args.verbose) From 5131f6b0a60a11980236edda2bfa0ba29c8bcb5a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Sep 2021 08:05:11 -0400 Subject: [PATCH 048/169] fix matplotlib deprecation --- python/gtsam/utils/plot.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index f324da63a..7ea393077 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -23,7 +23,10 @@ def set_axes_equal(fignum: int) -> None: fignum: An integer representing the figure number for Matplotlib. """ fig = plt.figure(fignum) - ax = fig.gca(projection='3d') + if not fig.axes: + ax = fig.add_subplot(projection='3d') + else: + ax = fig.axes[0] limits = np.array([ ax.get_xlim3d(), @@ -339,7 +342,11 @@ def plot_pose3( """ # get figure object fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] + plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) axes.set_xlabel(axis_labels[0]) From 9bc3c0b6a06f47902f15cdf5bcc090c01399e471 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 5 Sep 2021 11:48:54 -0400 Subject: [PATCH 049/169] removed duplicate --- gtsam/navigation/tests/testImuFactor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 801d87895..585da38b1 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -819,7 +819,6 @@ struct ImuFactorMergeTest { loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) { // arbitrary noise values p_->gyroscopeCovariance = I_3x3 * 0.01; - p_->accelerometerCovariance = I_3x3 * 0.02; p_->accelerometerCovariance = I_3x3 * 0.03; } From 617014f2714d95fb692c079ba591346d8ba9aeb4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 5 Sep 2021 11:49:15 -0400 Subject: [PATCH 050/169] wrap smart flags for various noise models --- gtsam/linear/linear.i | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 63047bf4f..f687acdec 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -16,9 +16,9 @@ virtual class Base { }; virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* Information(Matrix R); - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + static gtsam::noiseModel::Gaussian* Information(Matrix R, bool smart = true); + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R, bool smart = true); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R, bool smart = true); bool equals(gtsam::noiseModel::Base& expected, double tol); @@ -37,9 +37,9 @@ virtual class Gaussian : gtsam::noiseModel::Base { }; virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas, bool smart = true); + static gtsam::noiseModel::Diagonal* Variances(Vector variances, bool smart = true); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions, bool smart = true); Matrix R() const; // access to noise model @@ -69,9 +69,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { }; virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma, bool smart = true); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace, bool smart = true); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision, bool smart = true); // access to noise model double sigma() const; From 9cc594e9e55396ee20e170d967730c6d6a1bb234 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Sep 2021 11:26:10 -0400 Subject: [PATCH 051/169] Added virtual constructor and re-formatted constructors --- gtsam/inference/BayesTreeCliqueBase.h | 26 +++++++++++++++++++------- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 7b79ccf68..c9bb6db94 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -70,16 +70,23 @@ namespace gtsam { /// @name Standard Constructors /// @{ - /** Default constructor */ + /// Default constructor BayesTreeCliqueBase() : problemSize_(1) {} - /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {} + /// Construct from a conditional, leaving parent and child pointers + /// uninitialized. + BayesTreeCliqueBase(const sharedConditional& conditional) + : conditional_(conditional), problemSize_(1) {} - /** Shallow copy constructor */ - BayesTreeCliqueBase(const BayesTreeCliqueBase& c) : conditional_(c.conditional_), parent_(c.parent_), children(c.children), problemSize_(c.problemSize_), is_root(c.is_root) {} + /// Shallow copy constructor. + BayesTreeCliqueBase(const BayesTreeCliqueBase& c) + : conditional_(c.conditional_), + parent_(c.parent_), + children(c.children), + problemSize_(c.problemSize_), + is_root(c.is_root) {} - /** Shallow copy assignment constructor */ + /// Shallow copy assignment constructor BayesTreeCliqueBase& operator=(const BayesTreeCliqueBase& c) { conditional_ = c.conditional_; parent_ = c.parent_; @@ -89,6 +96,9 @@ namespace gtsam { return *this; } + // Virtual destructor. + virtual ~BayesTreeCliqueBase() {} + /// @} /// This stores the Cached separator marginal P(S) @@ -119,7 +129,9 @@ namespace gtsam { bool equals(const DERIVED& other, double tol = 1e-9) const; /** print this node */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Standard Interface From 711968900d3061688bac2c233c952daf27fc1bc5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Sep 2021 16:14:23 -0400 Subject: [PATCH 052/169] Switched to borglab hub --- docker/ubuntu-boost-tbb/build.sh | 4 ++-- docker/ubuntu-gtsam-python-vnc/Dockerfile | 2 +- docker/ubuntu-gtsam-python-vnc/build.sh | 4 ++-- docker/ubuntu-gtsam-python-vnc/vnc.sh | 2 +- docker/ubuntu-gtsam-python/Dockerfile | 6 ++++-- docker/ubuntu-gtsam-python/build.sh | 4 ++-- docker/ubuntu-gtsam/Dockerfile | 2 +- docker/ubuntu-gtsam/build.sh | 4 ++-- 8 files changed, 15 insertions(+), 13 deletions(-) diff --git a/docker/ubuntu-boost-tbb/build.sh b/docker/ubuntu-boost-tbb/build.sh index 2dac4c3db..ff465c970 100755 --- a/docker/ubuntu-boost-tbb/build.sh +++ b/docker/ubuntu-boost-tbb/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-boost-tbb:bionic . +# TODO(borglab): use docker compose and/or cmake +docker build --no-cache -t borglab/ubuntu-boost-tbb:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/Dockerfile b/docker/ubuntu-gtsam-python-vnc/Dockerfile index 61ecd9b9a..8039698c3 100644 --- a/docker/ubuntu-gtsam-python-vnc/Dockerfile +++ b/docker/ubuntu-gtsam-python-vnc/Dockerfile @@ -1,7 +1,7 @@ # This GTSAM image connects to the host X-server via VNC to provide a Graphical User Interface for interaction. # Get the base Ubuntu/GTSAM image from Docker Hub -FROM dellaert/ubuntu-gtsam-python:bionic +FROM borglab/ubuntu-gtsam-python:bionic # Things needed to get a python GUI ENV DEBIAN_FRONTEND noninteractive diff --git a/docker/ubuntu-gtsam-python-vnc/build.sh b/docker/ubuntu-gtsam-python-vnc/build.sh index 8d280252f..95d7dd575 100755 --- a/docker/ubuntu-gtsam-python-vnc/build.sh +++ b/docker/ubuntu-gtsam-python-vnc/build.sh @@ -1,4 +1,4 @@ # Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake +# TODO(borglab): use docker compose and/or cmake # Needs to be run in docker/ubuntu-gtsam-python-vnc directory -docker build -t dellaert/ubuntu-gtsam-python-vnc:bionic . +docker build -t borglab/ubuntu-gtsam-python-vnc:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/vnc.sh b/docker/ubuntu-gtsam-python-vnc/vnc.sh index c0ab692c6..b749093af 100755 --- a/docker/ubuntu-gtsam-python-vnc/vnc.sh +++ b/docker/ubuntu-gtsam-python-vnc/vnc.sh @@ -2,4 +2,4 @@ docker run -it \ --workdir="/usr/src/gtsam" \ -p 5900:5900 \ - dellaert/ubuntu-gtsam-python-vnc:bionic \ No newline at end of file + borglab/ubuntu-gtsam-python-vnc:bionic \ No newline at end of file diff --git a/docker/ubuntu-gtsam-python/Dockerfile b/docker/ubuntu-gtsam-python/Dockerfile index ce5d8fdca..85eed4d4e 100644 --- a/docker/ubuntu-gtsam-python/Dockerfile +++ b/docker/ubuntu-gtsam-python/Dockerfile @@ -1,7 +1,7 @@ # GTSAM Ubuntu image with Python wrapper support. # Get the base Ubuntu/GTSAM image from Docker Hub -FROM dellaert/ubuntu-gtsam:bionic +FROM borglab/ubuntu-gtsam:bionic # Install pip RUN apt-get install -y python3-pip python3-dev @@ -22,7 +22,9 @@ RUN cmake \ .. # Build again, as ubuntu-gtsam image cleaned -RUN make -j4 install && make clean +RUN make -j4 install +RUN make python-install +RUN make clean # Needed to run python wrapper: RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc diff --git a/docker/ubuntu-gtsam-python/build.sh b/docker/ubuntu-gtsam-python/build.sh index 1696f6c61..367847ba5 100755 --- a/docker/ubuntu-gtsam-python/build.sh +++ b/docker/ubuntu-gtsam-python/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-gtsam-python:bionic . +# TODO(borglab): use docker compose and/or cmake +docker build --no-cache -t borglab/ubuntu-gtsam-python:bionic . diff --git a/docker/ubuntu-gtsam/Dockerfile b/docker/ubuntu-gtsam/Dockerfile index f2b476f15..ce6927fe8 100644 --- a/docker/ubuntu-gtsam/Dockerfile +++ b/docker/ubuntu-gtsam/Dockerfile @@ -1,7 +1,7 @@ # Ubuntu image with GTSAM installed. Configured with Boost and TBB support. # Get the base Ubuntu image from Docker Hub -FROM dellaert/ubuntu-boost-tbb:bionic +FROM borglab/ubuntu-boost-tbb:bionic # Install git RUN apt-get update && \ diff --git a/docker/ubuntu-gtsam/build.sh b/docker/ubuntu-gtsam/build.sh index bf545e9c2..24a7d4f7f 100755 --- a/docker/ubuntu-gtsam/build.sh +++ b/docker/ubuntu-gtsam/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-gtsam:bionic . +# TODO(borglab): use docker compose and/or cmake +docker build --no-cache -t borglab/ubuntu-gtsam:bionic . From cfc06b244701500c07707db1c864c11fc62edc4b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Sep 2021 16:14:36 -0400 Subject: [PATCH 053/169] Updated README --- docker/README.md | 64 +++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 53 insertions(+), 11 deletions(-) diff --git a/docker/README.md b/docker/README.md index 0c136f94c..41cae6128 100644 --- a/docker/README.md +++ b/docker/README.md @@ -1,6 +1,57 @@ # Instructions -Build all docker images, in order: +# Images on Docker Hub + +There are 4 images available on https://hub.docker.com/orgs/borglab/repositories: + +- `borglab/ubuntu-boost-tbb`: 18.06 Linux (nicknamed `bionic`) base image, with Boost and TBB installed. +- `borglab/ubuntu-gtsam`: GTSAM Release version installed in `/usr/local`. +- `borglab/ubuntu-gtsam-python`: installed GTSAM with python wrapper. +- `borglab/ubuntu-gtsam-python-vnc`: image with GTSAM+python wrapper that will run a VNC server to connect to. + +# Using the images + +## Just GTSAM + +To start the Docker image, execute +```bash +docker run -it borglab/ubuntu-gtsam:bionic +``` +after you will find yourself in a bash shell, in the directory `/usr/src/gtsam/build`. +## GTSAM with Python wrapper + +To use GTSAM via the python wrapper, similarly execute +```bash +docker run -it borglab/ubuntu-gtsam-python:bionic +``` +and then launch `python3`: +```bash +python3 +>>> import gtsam +>>> gtsam.Pose2(1,2,3) +(1, 2, 3) +``` + +## GTSAM with Python wrapper and VNC + +First, start the docker image, which will run a VNC server on port 5900: +```bash +docker run -p 5900:5900 borglab/ubuntu-gtsam-python-vnc:bionic +``` + +Then open a remote VNC X client, for example: + +### Linux +```bash +sudo apt-get install tigervnc-viewer +xtigervncviewer :5900 +``` +### Mac +In the finder, in the Go menu, select "Connect to Server...", enter `vnc://127.0.0.1`, and then press "Connect". When prompted for a password, leave blank and press "Sign In". + +# Re-building the images locally + +To build all docker images, in order: ```bash (cd ubuntu-boost-tbb && ./build.sh) @@ -9,13 +60,4 @@ Build all docker images, in order: (cd ubuntu-gtsam-python-vnc && ./build.sh) ``` -Then launch with: - - docker run -p 5900:5900 dellaert/ubuntu-gtsam-python-vnc:bionic - -Then open a remote VNC X client, for example: - - sudo apt-get install tigervnc-viewer - xtigervncviewer :5900 - - +Note: building GTSAM can take a lot of memory because of the heavy templating. It is advisable to give Docker enough resources, e.g., 8GB, to avoid OOM errors while compiling. \ No newline at end of file From f49e35ecf085a59830b55248593143271a188db4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Sep 2021 16:19:54 -0400 Subject: [PATCH 054/169] Fixed TODOs --- docker/ubuntu-boost-tbb/build.sh | 2 +- docker/ubuntu-gtsam-python-vnc/build.sh | 2 +- docker/ubuntu-gtsam-python/build.sh | 2 +- docker/ubuntu-gtsam/build.sh | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docker/ubuntu-boost-tbb/build.sh b/docker/ubuntu-boost-tbb/build.sh index ff465c970..35b349c6a 100755 --- a/docker/ubuntu-boost-tbb/build.sh +++ b/docker/ubuntu-boost-tbb/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(borglab): use docker compose and/or cmake +# TODO(dellaert): use docker compose and/or cmake docker build --no-cache -t borglab/ubuntu-boost-tbb:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/build.sh b/docker/ubuntu-gtsam-python-vnc/build.sh index 95d7dd575..a0bbb6a96 100755 --- a/docker/ubuntu-gtsam-python-vnc/build.sh +++ b/docker/ubuntu-gtsam-python-vnc/build.sh @@ -1,4 +1,4 @@ # Build command for Docker image -# TODO(borglab): use docker compose and/or cmake +# TODO(dellaert): use docker compose and/or cmake # Needs to be run in docker/ubuntu-gtsam-python-vnc directory docker build -t borglab/ubuntu-gtsam-python-vnc:bionic . diff --git a/docker/ubuntu-gtsam-python/build.sh b/docker/ubuntu-gtsam-python/build.sh index 367847ba5..68827074d 100755 --- a/docker/ubuntu-gtsam-python/build.sh +++ b/docker/ubuntu-gtsam-python/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(borglab): use docker compose and/or cmake +# TODO(dellaert): use docker compose and/or cmake docker build --no-cache -t borglab/ubuntu-gtsam-python:bionic . diff --git a/docker/ubuntu-gtsam/build.sh b/docker/ubuntu-gtsam/build.sh index 24a7d4f7f..790ee1575 100755 --- a/docker/ubuntu-gtsam/build.sh +++ b/docker/ubuntu-gtsam/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(borglab): use docker compose and/or cmake +# TODO(dellaert): use docker compose and/or cmake docker build --no-cache -t borglab/ubuntu-gtsam:bionic . From 63f651b1d4e2aa1668ac4dd4662a5c556841ba3d Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 6 Sep 2021 22:35:58 -0400 Subject: [PATCH 055/169] Fixed VNC docs for Mac --- docker/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/README.md b/docker/README.md index 41cae6128..37c47a27f 100644 --- a/docker/README.md +++ b/docker/README.md @@ -47,7 +47,7 @@ sudo apt-get install tigervnc-viewer xtigervncviewer :5900 ``` ### Mac -In the finder, in the Go menu, select "Connect to Server...", enter `vnc://127.0.0.1`, and then press "Connect". When prompted for a password, leave blank and press "Sign In". +The Finder's "Connect to Server..." with `vnc://127.0.0.1` does not work, for some reason. Using the free [VNC Viewer](https://www.realvnc.com/en/connect/download/viewer/), enter `0.0.0.0:5900` as the server. # Re-building the images locally From 19850425b8e4e3b30c8d4a7ea21974ed83484d8c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:02:14 -0400 Subject: [PATCH 056/169] clean up and refactoring to use custom preintegration params --- gtsam/navigation/navigation.i | 2 + python/gtsam/examples/ImuFactorExample.py | 48 ++++++++++++------- .../gtsam/examples/PreintegrationExample.py | 38 ++++++++------- 3 files changed, 56 insertions(+), 32 deletions(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 48a5a35de..7a879c3ef 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -88,6 +88,8 @@ virtual class PreintegratedRotationParams { virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); + gtsam::Vector n_gravity; + static gtsam::PreintegrationParams* MakeSharedD(double g); static gtsam::PreintegrationParams* MakeSharedU(double g); static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 86613234d..d6e6793f2 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -10,20 +10,19 @@ A script validating and demonstrating the ImuFactor inference. Author: Frank Dellaert, Varun Agrawal """ -# pylint: disable=no-name-in-module,unused-import,arguments-differ +# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order from __future__ import print_function import argparse import math +import gtsam import matplotlib.pyplot as plt import numpy as np -from mpl_toolkits.mplot3d import Axes3D - -import gtsam from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 +from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample @@ -51,12 +50,23 @@ class ImuFactorExample(PreintegrationExample): gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) + g = 9.81 + params = gtsam.PreintegrationParams.MakeSharedU(g) + + # Some arbitrary noise sigmas + gyro_sigma = 1e-3 + accel_sigma = 1e-3 + I_3x3 = np.eye(3) + params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3) + params.setAccelerometerCovariance(accel_sigma**2 * I_3x3) + params.setIntegrationCovariance(1e-7**2 * I_3x3) + dt = 1e-2 super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario], - bias, dt) + bias, params, dt) def addPrior(self, i, graph): - """Add priors at time step `i`.""" + """Add a prior on the navigation state at time `i`.""" state = self.scenario.navState(i) graph.push_back( gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) @@ -71,21 +81,27 @@ class ImuFactorExample(PreintegrationExample): result = optimizer.optimize() return result - def plot(self, result): - """Plot resulting poses.""" + def plot(self, + values, + title="Estimated Trajectory", + fignum=POSES_FIG + 1, + show=False): + """Plot poses in values.""" i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - plot_pose3(POSES_FIG + 1, pose_i, 1) + while values.exists(X(i)): + pose_i = values.atPose3(X(i)) + plot_pose3(fignum, pose_i, 1) i += 1 - plt.title("Estimated Trajectory") + plt.title(title) - gtsam.utils.plot.set_axes_equal(POSES_FIG + 1) + gtsam.utils.plot.set_axes_equal(fignum) - print("Bias Values", result.atConstantBias(BIAS_KEY)) + print("Bias Values", values.atConstantBias(BIAS_KEY)) plt.ioff() - plt.show() + + if show: + plt.show() def run(self, T=12, compute_covariances=False, verbose=True): """Main runner.""" @@ -173,7 +189,7 @@ class ImuFactorExample(PreintegrationExample): print("Covariance on vel {}:\n{}\n".format( i, marginals.marginalCovariance(V(i)))) - self.plot(result) + self.plot(result, show=True) if __name__ == '__main__': diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index 458248c30..f38ff7bc9 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -5,9 +5,13 @@ All Rights Reserved See LICENSE for the license information -A script validating the Preintegration of IMU measurements +A script validating the Preintegration of IMU measurements. + +Authors: Frank Dellaert, Varun Agrawal. """ +# pylint: disable=invalid-name,unused-import,wrong-import-order + import math import gtsam @@ -21,22 +25,20 @@ POSES_FIG = 2 class PreintegrationExample(object): - + """Base class for all preintegration examples.""" @staticmethod def defaultParams(g): """Create default parameters with Z *up* and realistic noise parameters""" params = gtsam.PreintegrationParams.MakeSharedU(g) kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW kAccelSigma = 0.1 / 60 # 10 cm VRW - params.setGyroscopeCovariance( - kGyroSigma ** 2 * np.identity(3, float)) - params.setAccelerometerCovariance( - kAccelSigma ** 2 * np.identity(3, float)) - params.setIntegrationCovariance( - 0.0000001 ** 2 * np.identity(3, float)) + params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float)) + params.setAccelerometerCovariance(kAccelSigma**2 * + np.identity(3, float)) + params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float)) return params - def __init__(self, twist=None, bias=None, dt=1e-2): + def __init__(self, twist=None, bias=None, params=None, dt=1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -58,9 +60,11 @@ class PreintegrationExample(object): self.labels = list('xyz') self.colors = list('rgb') - # Create runner - self.g = 10 # simple gravity constant - self.params = self.defaultParams(self.g) + if params: + self.params = params + else: + # Default params with simple gravity constant + self.params = self.defaultParams(g=10) if bias is not None: self.actualBias = bias @@ -69,13 +73,15 @@ class PreintegrationExample(object): gyroBias = np.array([0, 0, 0]) self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias) - self.runner = gtsam.ScenarioRunner( - self.scenario, self.params, self.dt, self.actualBias) + # Create runner + self.runner = gtsam.ScenarioRunner(self.scenario, self.params, self.dt, + self.actualBias) fig, self.axes = plt.subplots(4, 3) fig.set_tight_layout(True) def plotImu(self, t, measuredOmega, measuredAcc): + """Plot IMU measurements.""" plt.figure(IMU_FIG) # plot angular velocity @@ -109,7 +115,7 @@ class PreintegrationExample(object): ax.set_xlabel('specific force ' + label) def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): - # plot ground truth pose, as well as prediction from integrated IMU measurements + """Plot ground truth pose, as well as prediction from integrated IMU measurements.""" actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) t = actualPose.translation() @@ -122,7 +128,7 @@ class PreintegrationExample(object): plt.pause(time_interval) def run(self, T=12): - # simulate the loop + """Simulate the loop.""" for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) From b3da7e0697f9c3b539a4c5dd0b02e90b1e3a1055 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:43:51 -0400 Subject: [PATCH 057/169] formatting --- gtsam/inference/Key.h | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index 2cc19d07a..31428a50e 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -32,7 +32,7 @@ namespace gtsam { /// Typedef for a function to format a key, i.e. to convert it to a string -typedef std::function KeyFormatter; +using KeyFormatter = std::function; // Helper function for DefaultKeyFormatter GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); @@ -83,28 +83,32 @@ class key_formatter { }; /// Define collection type once and for all - also used in wrappers -typedef FastVector KeyVector; +using KeyVector = FastVector; // TODO(frank): Nothing fast about these :-( -typedef FastList KeyList; -typedef FastSet KeySet; -typedef FastMap KeyGroupMap; +using KeyList = FastList; +using KeySet = FastSet; +using KeyGroupMap = FastMap; /// Utility function to print one key with optional prefix -GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKey( + Key key, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeyList( + const KeyList &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s = - "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeyVector( + const KeyVector &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeySet( + const KeySet &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); // Define Key to be Testable by specializing gtsam::traits template struct traits; From 0a8080f2fcf3fc1127073042df1272f96a071b0f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:53:06 -0400 Subject: [PATCH 058/169] wrap key printing funcs and remove redundancy --- gtsam/nonlinear/nonlinear.i | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 61f164b43..fdd795c93 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -75,12 +75,15 @@ size_t Z(size_t j); } // namespace symbol_shorthand // Default keyformatter -void PrintKeyList(const gtsam::KeyList& keys); -void PrintKeyList(const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet(const gtsam::KeySet& keys); -void PrintKeySet(const gtsam::KeySet& keys, string s); +void PrintKeyList( + const gtsam::KeyList& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +void PrintKeyVector( + const gtsam::KeyVector& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +void PrintKeySet( + const gtsam::KeySet& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); #include class LabeledSymbol { From 0257f2118d9a36691503e618069d1a7b8333ae62 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:53:21 -0400 Subject: [PATCH 059/169] wrap more JacobianFactor constructors --- gtsam/linear/linear.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index f687acdec..8635c55f8 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -289,6 +289,13 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, Vector b, const gtsam::noiseModel::Diagonal* model); JacobianFactor(const gtsam::GaussianFactorGraph& graph); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::VariableSlots& p_variableSlots); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::Ordering& ordering); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::Ordering& ordering, + const gtsam::VariableSlots& p_variableSlots); //Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = From 7076244b60c89119c6b4163d3f0faf32c9104b6b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:53:41 -0400 Subject: [PATCH 060/169] update template to record correct name of cpp file --- python/gtsam/gtsam.tpl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index 93e7ffbaf..b760e4eb5 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -1,8 +1,8 @@ /** - * @file gtsam.cpp + * @file {module_name}.cpp * @brief The auto-generated wrapper C++ source code. - * @author Duy-Nguyen Ta, Fan Jiang, Matthew Sklar - * @date Aug. 18, 2020 + * @author Duy-Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal + * @date Aug. 18, 2020 * * ** THIS FILE IS AUTO-GENERATED, DO NOT MODIFY! ** */ From d99a9432bcd68eb0c872648965344922eb586afb Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Fri, 17 Sep 2021 17:59:46 -0400 Subject: [PATCH 061/169] Update Find TBB to handle TBB installed with homebrew on OS X Better Error message for TBB handling with versions greater than 2021.1 on OS X --- cmake/FindTBB.cmake | 16 ++++++++++++++-- cmake/HandleTBB.cmake | 4 ++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake index e2b1df6e3..0ecd4ca0e 100644 --- a/cmake/FindTBB.cmake +++ b/cmake/FindTBB.cmake @@ -144,7 +144,8 @@ if(NOT TBB_FOUND) elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin") # OS X - set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb") + set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb" + "/usr/local/opt/tbb") # TODO: Check to see which C++ library is being used by the compiler. if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0) @@ -181,7 +182,18 @@ if(NOT TBB_FOUND) ################################## if(TBB_INCLUDE_DIRS) - file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file) + set(_tbb_version_file_prior_to_tbb_2021_1 "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h") + set(_tbb_version_file_after_tbb_2021_1 "${TBB_INCLUDE_DIRS}/oneapi/tbb/version.h") + + if (EXISTS "${_tbb_version_file_prior_to_tbb_2021_1}") + file(READ "${_tbb_version_file_prior_to_tbb_2021_1}" _tbb_version_file ) + elseif (EXISTS "${_tbb_version_file_after_tbb_2021_1}") + file(READ "${_tbb_version_file_after_tbb_2021_1}" _tbb_version_file ) + else() + message(FATAL_ERROR "Found TBB installation: ${TBB_INCLUDE_DIRS} " + "missing version header.") + endif() + string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1" TBB_VERSION_MAJOR "${_tbb_version_file}") string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1" diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index cedee55ea..e2ba02d94 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -5,6 +5,10 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) set(GTSAM_USE_TBB 1) # This will go into config.h + if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) + message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") + endif() + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) set(TBB_GREATER_EQUAL_2020 1) else() From 536a1035d60f0d243d106c036f59d330caeca126 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 18 Sep 2021 02:17:47 -0400 Subject: [PATCH 062/169] Squashed 'wrap/' changes from 571c23952..add6075e8 add6075e8 Merge pull request #121 from borglab/feature/constructor-templates 42d4145bb update instantiate_ctors to handle constructor level templates 455ce6169 update test fixtures 3c37fc2a0 update wrapper test fixtures ffdad925d update interface_parser to pass the tests bf7416213 add interface_parser test for templated constructor 9fe05d0c9 Merge pull request #120 from borglab/feature/templated-namespace 7622e6432 typo fix 88779c5e6 update instantiator to handle templates in the namespace 0ee86f9a3 add test for templated type in namespace git-subtree-dir: wrap git-subtree-split: add6075e8ec0e28d5f47d0a2ecab00deaa9a3da7 --- gtwrap/interface_parser/classes.py | 23 ++--- gtwrap/interface_parser/function.py | 2 +- gtwrap/template_instantiator.py | 47 ++++++++- tests/expected/matlab/MyFactorPosePoint2.m | 8 +- tests/expected/matlab/class_wrapper.cpp | 111 +++++++++++++++++++-- tests/expected/python/class_pybind.cpp | 6 ++ tests/expected/python/enum_pybind.cpp | 10 ++ tests/fixtures/class.i | 10 ++ tests/fixtures/enum.i | 13 +++ tests/test_interface_parser.py | 19 ++++ 10 files changed, 217 insertions(+), 32 deletions(-) diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index 11317962d..841c963c2 100644 --- a/gtwrap/interface_parser/classes.py +++ b/gtwrap/interface_parser/classes.py @@ -119,24 +119,27 @@ class Constructor: Can have 0 or more arguments. """ rule = ( - IDENT("name") # + Optional(Template.rule("template")) # + + IDENT("name") # + LPAREN # + ArgumentList.rule("args_list") # + RPAREN # + SEMI_COLON # BR - ).setParseAction(lambda t: Constructor(t.name, t.args_list)) + ).setParseAction(lambda t: Constructor(t.name, t.args_list, t.template)) def __init__(self, name: str, args: ArgumentList, + template: Union[Template, Any], parent: Union["Class", Any] = ''): self.name = name self.args = args + self.template = template self.parent = parent def __repr__(self) -> str: - return "Constructor: {}".format(self.name) + return "Constructor: {}{}".format(self.name, self.args) class Operator: @@ -260,17 +263,9 @@ class Class: + RBRACE # + SEMI_COLON # BR ).setParseAction(lambda t: Class( - t.template, - t.is_virtual, - t.name, - t.parent_class, - t.members.ctors, - t.members.methods, - t.members.static_methods, - t.members.properties, - t.members.operators, - t.members.enums - )) + t.template, t.is_virtual, t.name, t.parent_class, t.members.ctors, t. + members.methods, t.members.static_methods, t.members.properties, t. + members.operators, t.members.enums)) def __init__( self, diff --git a/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py index 9fe1f56f0..9e68c6ece 100644 --- a/gtwrap/interface_parser/function.py +++ b/gtwrap/interface_parser/function.py @@ -81,7 +81,7 @@ class ArgumentList: return ArgumentList([]) def __repr__(self) -> str: - return self.args_list.__repr__() + return repr(tuple(self.args_list)) def __len__(self) -> int: return len(self.args_list) diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index 0cda93d5d..f5beb0c69 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -41,6 +41,8 @@ def instantiate_type(ctype: parser.Type, str_arg_typename = str(ctype.typename) + # Instantiate templates which have enumerated instantiations in the template. + # E.g. `template`. if str_arg_typename in template_typenames: idx = template_typenames.index(str_arg_typename) return parser.Type( @@ -51,14 +53,15 @@ def instantiate_type(ctype: parser.Type, is_ref=ctype.is_ref, is_basic=ctype.is_basic, ) + + # If a method has the keyword `This`, we replace it with the (instantiated) class. elif str_arg_typename == 'This': + # Check if the class is template instantiated + # so we can replace it with the instantiated version. if instantiated_class: name = instantiated_class.original.name namespaces_name = instantiated_class.namespaces() namespaces_name.append(name) - # print("INST: {}, {}, CPP: {}, CLS: {}".format( - # ctype, instantiations, cpp_typename, instantiated_class.instantiations - # ), file=sys.stderr) cpp_typename = parser.Typename( namespaces_name, instantiations=instantiated_class.instantiations) @@ -71,6 +74,14 @@ def instantiate_type(ctype: parser.Type, is_ref=ctype.is_ref, is_basic=ctype.is_basic, ) + + # Case when 'This' is present in the type namespace, e.g `This::Subclass`. + elif 'This' in str_arg_typename: + # Simply get the index of `This` in the namespace and replace it with the instantiated name. + namespace_idx = ctype.typename.namespaces.index('This') + ctype.typename.namespaces[namespace_idx] = cpp_typename.name + return ctype + else: return ctype @@ -368,19 +379,45 @@ class InstantiatedClass(parser.Class): """ instantiated_ctors = [] - for ctor in self.original.ctors: + def instantiate(instantiated_ctors, ctor, typenames, instantiations): instantiated_args = instantiate_args_list( ctor.args.list(), typenames, - self.instantiations, + instantiations, self.cpp_typename(), ) instantiated_ctors.append( parser.Constructor( name=self.name, args=parser.ArgumentList(instantiated_args), + template=self.original.template, parent=self, )) + return instantiated_ctors + + for ctor in self.original.ctors: + # Add constructor templates to the typenames and instantiations + if isinstance(ctor.template, parser.template.Template): + typenames.extend(ctor.template.typenames) + + # Get all combinations of template args + for instantiations in itertools.product( + *ctor.template.instantiations): + instantiations = self.instantiations + list(instantiations) + + instantiated_ctors = instantiate( + instantiated_ctors, + ctor, + typenames=typenames, + instantiations=instantiations) + + else: + # If no constructor level templates, just use the class templates + instantiated_ctors = instantiate( + instantiated_ctors, + ctor, + typenames=typenames, + instantiations=self.instantiations) return instantiated_ctors def instantiate_static_methods(self, typenames): diff --git a/tests/expected/matlab/MyFactorPosePoint2.m b/tests/expected/matlab/MyFactorPosePoint2.m index 2dd4b5428..e4cd92196 100644 --- a/tests/expected/matlab/MyFactorPosePoint2.m +++ b/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(56, my_ptr); + class_wrapper(62, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = class_wrapper(57, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(63, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - class_wrapper(58, obj.ptr_MyFactorPosePoint2); + class_wrapper(64, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,7 +36,7 @@ classdef MyFactorPosePoint2 < handle % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(59, this, varargin{:}); + class_wrapper(65, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp index fab9c1450..48d6b2dce 100644 --- a/tests/expected/matlab/class_wrapper.cpp +++ b/tests/expected/matlab/class_wrapper.cpp @@ -33,6 +33,8 @@ typedef std::set*> Collector_Multip static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; typedef std::set*> Collector_ForwardKinematics; static Collector_ForwardKinematics collector_ForwardKinematics; +typedef std::set*> Collector_TemplatedConstructor; +static Collector_TemplatedConstructor collector_TemplatedConstructor; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -97,6 +99,12 @@ void _deleteAllObjects() collector_ForwardKinematics.erase(iter++); anyDeleted = true; } } + { for(Collector_TemplatedConstructor::iterator iter = collector_TemplatedConstructor.begin(); + iter != collector_TemplatedConstructor.end(); ) { + delete *iter; + collector_TemplatedConstructor.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -682,7 +690,76 @@ void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, } } -void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_TemplatedConstructor.insert(self); +} + +void TemplatedConstructor_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new TemplatedConstructor()); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + string& arg = *unwrap_shared_ptr< string >(in[0], "ptr_string"); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + int arg = unwrap< int >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double arg = unwrap< double >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_deconstructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_TemplatedConstructor",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_TemplatedConstructor::iterator item; + item = collector_TemplatedConstructor.find(self); + if(item != collector_TemplatedConstructor.end()) { + delete self; + collector_TemplatedConstructor.erase(item); + } +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -691,7 +768,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -706,7 +783,7 @@ void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -719,7 +796,7 @@ void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin } } -void MyFactorPosePoint2_print_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -909,16 +986,34 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1); break; case 56: - MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + TemplatedConstructor_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); break; case 57: - MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_57(nargout, out, nargin-1, in+1); break; case 58: - MyFactorPosePoint2_deconstructor_58(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_58(nargout, out, nargin-1, in+1); break; case 59: - MyFactorPosePoint2_print_59(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_59(nargout, out, nargin-1, in+1); + break; + case 60: + TemplatedConstructor_constructor_60(nargout, out, nargin-1, in+1); + break; + case 61: + TemplatedConstructor_deconstructor_61(nargout, out, nargin-1, in+1); + break; + case 62: + MyFactorPosePoint2_collectorInsertAndMakeBase_62(nargout, out, nargin-1, in+1); + break; + case 63: + MyFactorPosePoint2_constructor_63(nargout, out, nargin-1, in+1); + break; + case 64: + MyFactorPosePoint2_deconstructor_64(nargout, out, nargin-1, in+1); + break; + case 65: + MyFactorPosePoint2_print_65(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/tests/expected/python/class_pybind.cpp b/tests/expected/python/class_pybind.cpp index 4c2371a42..f36c0a84c 100644 --- a/tests/expected/python/class_pybind.cpp +++ b/tests/expected/python/class_pybind.cpp @@ -86,6 +86,12 @@ PYBIND11_MODULE(class_py, m_) { py::class_>(m_, "ForwardKinematics") .def(py::init(), py::arg("robot"), py::arg("start_link_name"), py::arg("end_link_name"), py::arg("joint_angles"), py::arg("l2Tp") = gtsam::Pose3()); + py::class_>(m_, "TemplatedConstructor") + .def(py::init<>()) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")) .def("print",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) diff --git a/tests/expected/python/enum_pybind.cpp b/tests/expected/python/enum_pybind.cpp index ffc68ece0..73b74bd86 100644 --- a/tests/expected/python/enum_pybind.cpp +++ b/tests/expected/python/enum_pybind.cpp @@ -69,6 +69,16 @@ PYBIND11_MODULE(enum_py, m_) { .value("Groot", gtsam::MCU::GotG::Groot); + py::class_, std::shared_ptr>> optimizergaussnewtonparams(m_gtsam, "OptimizerGaussNewtonParams"); + optimizergaussnewtonparams + .def("setVerbosity",[](gtsam::Optimizer* self, const Optimizer::Verbosity value){ self->setVerbosity(value);}, py::arg("value")); + + py::enum_::Verbosity>(optimizergaussnewtonparams, "Verbosity", py::arithmetic()) + .value("SILENT", gtsam::Optimizer::Verbosity::SILENT) + .value("SUMMARY", gtsam::Optimizer::Verbosity::SUMMARY) + .value("VERBOSE", gtsam::Optimizer::Verbosity::VERBOSE); + + #include "python/specializations.h" diff --git a/tests/fixtures/class.i b/tests/fixtures/class.i index 9e30b17b5..9923ffce7 100644 --- a/tests/fixtures/class.i +++ b/tests/fixtures/class.i @@ -7,6 +7,7 @@ class FunRange { template class Fun { + static This staticMethodWithThis(); template @@ -118,5 +119,14 @@ class ForwardKinematics { const gtsam::Pose3& l2Tp = gtsam::Pose3()); }; +// Test for templated constructor +class TemplatedConstructor { + TemplatedConstructor(); + + template + TemplatedConstructor(const T& arg); +}; + + class SuperCoolFactor; typedef SuperCoolFactor SuperCoolFactorPose3; diff --git a/tests/fixtures/enum.i b/tests/fixtures/enum.i index 9386a33df..71918c25a 100644 --- a/tests/fixtures/enum.i +++ b/tests/fixtures/enum.i @@ -42,4 +42,17 @@ class MCU { }; +template +class Optimizer { + enum Verbosity { + SILENT, + SUMMARY, + VERBOSE + }; + + void setVerbosity(const This::Verbosity value); +}; + +typedef gtsam::Optimizer OptimizerGaussNewtonParams; + } // namespace gtsam diff --git a/tests/test_interface_parser.py b/tests/test_interface_parser.py index 95987f42f..25e4178a6 100644 --- a/tests/test_interface_parser.py +++ b/tests/test_interface_parser.py @@ -314,6 +314,25 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual(5, len(ret.args)) self.assertEqual("gtsam::Pose3()", ret.args.list()[4].default) + def test_constructor_templated(self): + """Test for templated class constructor.""" + f = """ + template + Class(); + """ + ret = Constructor.rule.parseString(f)[0] + self.assertEqual("Class", ret.name) + self.assertEqual(0, len(ret.args)) + + f = """ + template + Class(const T& name); + """ + ret = Constructor.rule.parseString(f)[0] + self.assertEqual("Class", ret.name) + self.assertEqual(1, len(ret.args)) + self.assertEqual("const T & name", ret.args.args_list[0].to_cpp()) + def test_operator_overload(self): """Test for operator overloading.""" # Unary operator From d8b6f1524719ba7538fd9d5887194af4d81941f9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 18 Sep 2021 02:48:31 -0400 Subject: [PATCH 063/169] make TBB finding depend on the GTSAM_WITH_TBB flag --- cmake/HandleTBB.cmake | 41 ++++++++++++++++++++++------------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index cedee55ea..56c443932 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -1,24 +1,27 @@ ############################################################################### -# Find TBB -find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) +if (GTSAM_WITH_TBB) + # Find TBB + find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) -# Set up variables if we're using TBB -if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) - set(TBB_GREATER_EQUAL_2020 1) + # Set up variables if we're using TBB + if(TBB_FOUND) + set(GTSAM_USE_TBB 1) # This will go into config.h + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) else() - set(TBB_GREATER_EQUAL_2020 0) + set(GTSAM_USE_TBB 0) # This will go into config.h endif() - # all definitions and link requisites will go via imported targets: - # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) -else() - set(GTSAM_USE_TBB 0) # This will go into config.h -endif() -############################################################################### -# Prohibit Timing build mode in combination with TBB -if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) - message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") -endif() + ############################################################################### + # Prohibit Timing build mode in combination with TBB + if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") + endif() + +endif() \ No newline at end of file From 4307b842c18a28fddd09eed95716cc4965e23b0d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 28 Sep 2021 10:09:10 -0400 Subject: [PATCH 064/169] Squashed 'wrap/' changes from add6075e8..d6350c214 d6350c214 Merge pull request #123 from borglab/feature/constructor-arg-templates c667aa0ed add matlab test b9513b0db update docs 2b44678c9 instantiate scoped templates f99a75110 add tests for templates git-subtree-dir: wrap git-subtree-split: d6350c214bb4e012f1a0003b1ffc052bb7f35f33 --- DOCS.md | 3 +- gtwrap/template_instantiator.py | 35 +++- tests/expected/matlab/template_wrapper.cpp | 225 +++++++++++++++++++++ tests/expected/python/templates_pybind.cpp | 38 ++++ tests/fixtures/templates.i | 15 ++ tests/test_matlab_wrapper.py | 17 ++ tests/test_pybind_wrapper.py | 8 + 7 files changed, 339 insertions(+), 2 deletions(-) create mode 100644 tests/expected/matlab/template_wrapper.cpp create mode 100644 tests/expected/python/templates_pybind.cpp create mode 100644 tests/fixtures/templates.i diff --git a/DOCS.md b/DOCS.md index c8285baef..f08f741ff 100644 --- a/DOCS.md +++ b/DOCS.md @@ -133,9 +133,10 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the template class Class2 { ... }; typedef Class2 MyInstantiatedClass; ``` - - Templates can also be defined for methods, properties and static methods. + - Templates can also be defined for constructors, methods, properties and static methods. - In the class definition, appearances of the template argument(s) will be replaced with their instantiated types, e.g. `void setValue(const T& value);`. + - Values scoped within templates are supported. E.g. one can use the form `T::Value` where T is a template, as an argument to a method. - To refer to the instantiation of the template class itself, use `This`, i.e. `static This Create();`. - To create new instantiations in other modules, you must copy-and-paste the whole class definition into the new module, but use only your new instantiation types. diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index f5beb0c69..b4d79655d 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -9,6 +9,17 @@ from typing import Any, Iterable, List, Sequence import gtwrap.interface_parser as parser +def is_scoped_template(template_typenames, str_arg_typename): + """ + Check if the template given by `str_arg_typename` is a scoped template, + and if so, return what template and index matches the scoped template correctly. + """ + for idx, template in enumerate(template_typenames): + if template in str_arg_typename.split("::"): + return template, idx + return False, -1 + + def instantiate_type(ctype: parser.Type, template_typenames: List[str], instantiations: List[parser.Typename], @@ -41,9 +52,30 @@ def instantiate_type(ctype: parser.Type, str_arg_typename = str(ctype.typename) + # Check if template is a scoped template e.g. T::Value where T is the template + scoped_template, scoped_idx = is_scoped_template(template_typenames, + str_arg_typename) + # Instantiate templates which have enumerated instantiations in the template. # E.g. `template`. - if str_arg_typename in template_typenames: + + # Instantiate scoped templates, e.g. T::Value. + if scoped_template: + # Create a copy of the instantiation so we can modify it. + instantiation = deepcopy(instantiations[scoped_idx]) + # Replace the part of the template with the instantiation + instantiation.name = str_arg_typename.replace(scoped_template, + instantiation.name) + return parser.Type( + typename=instantiation, + is_const=ctype.is_const, + is_shared_ptr=ctype.is_shared_ptr, + is_ptr=ctype.is_ptr, + is_ref=ctype.is_ref, + is_basic=ctype.is_basic, + ) + # Check for exact template match. + elif str_arg_typename in template_typenames: idx = template_typenames.index(str_arg_typename) return parser.Type( typename=instantiations[idx], @@ -418,6 +450,7 @@ class InstantiatedClass(parser.Class): ctor, typenames=typenames, instantiations=self.instantiations) + return instantiated_ctors def instantiate_static_methods(self, typenames): diff --git a/tests/expected/matlab/template_wrapper.cpp b/tests/expected/matlab/template_wrapper.cpp new file mode 100644 index 000000000..532bdd57a --- /dev/null +++ b/tests/expected/matlab/template_wrapper.cpp @@ -0,0 +1,225 @@ +#include +#include + +#include +#include +#include + + + +typedef ScopedTemplate ScopedTemplateResult; + +typedef std::set*> Collector_TemplatedConstructor; +static Collector_TemplatedConstructor collector_TemplatedConstructor; +typedef std::set*> Collector_ScopedTemplateResult; +static Collector_ScopedTemplateResult collector_ScopedTemplateResult; + + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_TemplatedConstructor::iterator iter = collector_TemplatedConstructor.begin(); + iter != collector_TemplatedConstructor.end(); ) { + delete *iter; + collector_TemplatedConstructor.erase(iter++); + anyDeleted = true; + } } + { for(Collector_ScopedTemplateResult::iterator iter = collector_ScopedTemplateResult.begin(); + iter != collector_ScopedTemplateResult.end(); ) { + delete *iter; + collector_ScopedTemplateResult.erase(iter++); + anyDeleted = true; + } } + + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" + "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); +} + +void _template_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_template_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + + + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(const StringPair& rtti_matlab: types) { + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + } + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(newAlreadyCreated); + } +} + +void TemplatedConstructor_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_TemplatedConstructor.insert(self); +} + +void TemplatedConstructor_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new TemplatedConstructor()); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + string& arg = *unwrap_shared_ptr< string >(in[0], "ptr_string"); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + int arg = unwrap< int >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double arg = unwrap< double >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_deconstructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_TemplatedConstructor",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_TemplatedConstructor::iterator item; + item = collector_TemplatedConstructor.find(self); + if(item != collector_TemplatedConstructor.end()) { + delete self; + collector_TemplatedConstructor.erase(item); + } +} + +void ScopedTemplateResult_collectorInsertAndMakeBase_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_ScopedTemplateResult.insert(self); +} + +void ScopedTemplateResult_constructor_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Result::Value& arg = *unwrap_shared_ptr< Result::Value >(in[0], "ptr_Result::Value"); + Shared *self = new Shared(new ScopedTemplate(arg)); + collector_ScopedTemplateResult.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void ScopedTemplateResult_deconstructor_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr> Shared; + checkArguments("delete_ScopedTemplateResult",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_ScopedTemplateResult::iterator item; + item = collector_ScopedTemplateResult.find(self); + if(item != collector_ScopedTemplateResult.end()) { + delete self; + collector_ScopedTemplateResult.erase(item); + } +} + + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _template_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + TemplatedConstructor_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + TemplatedConstructor_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + TemplatedConstructor_constructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + TemplatedConstructor_constructor_3(nargout, out, nargin-1, in+1); + break; + case 4: + TemplatedConstructor_constructor_4(nargout, out, nargin-1, in+1); + break; + case 5: + TemplatedConstructor_deconstructor_5(nargout, out, nargin-1, in+1); + break; + case 6: + ScopedTemplateResult_collectorInsertAndMakeBase_6(nargout, out, nargin-1, in+1); + break; + case 7: + ScopedTemplateResult_constructor_7(nargout, out, nargin-1, in+1); + break; + case 8: + ScopedTemplateResult_deconstructor_8(nargout, out, nargin-1, in+1); + break; + } + } catch(const std::exception& e) { + mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); + } + + std::cout.rdbuf(outbuf); +} diff --git a/tests/expected/python/templates_pybind.cpp b/tests/expected/python/templates_pybind.cpp new file mode 100644 index 000000000..4d13d3731 --- /dev/null +++ b/tests/expected/python/templates_pybind.cpp @@ -0,0 +1,38 @@ + + +#include +#include +#include +#include +#include "gtsam/nonlinear/utilities.h" // for RedirectCout. + + +#include "wrap/serialization.h" +#include + + + + + +using namespace std; + +namespace py = pybind11; + +PYBIND11_MODULE(templates_py, m_) { + m_.doc() = "pybind11 wrapper of templates_py"; + + + py::class_>(m_, "TemplatedConstructor") + .def(py::init<>()) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")); + + py::class_, std::shared_ptr>>(m_, "ScopedTemplateResult") + .def(py::init(), py::arg("arg")); + + +#include "python/specializations.h" + +} + diff --git a/tests/fixtures/templates.i b/tests/fixtures/templates.i new file mode 100644 index 000000000..c485041c6 --- /dev/null +++ b/tests/fixtures/templates.i @@ -0,0 +1,15 @@ +// Test for templated constructor +class TemplatedConstructor { + TemplatedConstructor(); + + template + TemplatedConstructor(const T& arg); +}; + +// Test for a scoped value inside a template +template +class ScopedTemplate { + // T should be properly substituted here. + ScopedTemplate(const T::Value& arg); +}; + diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index 112750721..89e53ab21 100644 --- a/tests/test_matlab_wrapper.py +++ b/tests/test_matlab_wrapper.py @@ -123,6 +123,23 @@ class TestWrap(unittest.TestCase): for file in files: self.compare_and_diff(file) + def test_templates(self): + """Test interface file with template info.""" + file = osp.join(self.INTERFACE_DIR, 'templates.i') + + wrapper = MatlabWrapper( + module_name='template', + top_module_namespace=['gtsam'], + ignore_classes=[''], + ) + + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) + + files = ['template_wrapper.cpp'] + + for file in files: + self.compare_and_diff(file) + def test_inheritance(self): """Test interface file with class inheritance definitions.""" file = osp.join(self.INTERFACE_DIR, 'inheritance.i') diff --git a/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py index 67c637d14..b47b4aca1 100644 --- a/tests/test_pybind_wrapper.py +++ b/tests/test_pybind_wrapper.py @@ -95,6 +95,14 @@ class TestWrap(unittest.TestCase): self.compare_and_diff('class_pybind.cpp', output) + def test_templates(self): + """Test interface file with templated class.""" + source = osp.join(self.INTERFACE_DIR, 'templates.i') + output = self.wrap_content([source], 'templates_py', + self.PYTHON_ACTUAL_DIR) + + self.compare_and_diff('templates_pybind.cpp', output) + def test_inheritance(self): """Test interface file with class inheritance definitions.""" source = osp.join(self.INTERFACE_DIR, 'inheritance.i') From b49bd123f428009699a3ed17518deb1cc07e6b13 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 19:08:54 -0400 Subject: [PATCH 065/169] renamed smartProjectionFactorP -> smartProjectionRigFactor --- gtsam/slam/ReadMe.md | 2 +- ...onFactorP.h => SmartProjectionRigFactor.h} | 22 ++++----- gtsam/slam/tests/smartFactorScenarios.h | 8 ++-- ...P.cpp => testSmartProjectionRigFactor.cpp} | 48 +++++++++---------- 4 files changed, 40 insertions(+), 40 deletions(-) rename gtsam/slam/{SmartProjectionFactorP.h => SmartProjectionRigFactor.h} (95%) rename gtsam/slam/tests/{testSmartProjectionFactorP.cpp => testSmartProjectionRigFactor.cpp} (96%) diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md index c43f0769a..d216b9181 100644 --- a/gtsam/slam/ReadMe.md +++ b/gtsam/slam/ReadMe.md @@ -38,7 +38,7 @@ The factor only constrains poses. If the calibration should be optimized, as well, use `SmartProjectionFactor` instead! -### SmartProjectionFactorP +### SmartProjectionRigFactor Same as `SmartProjectionPoseFactor`, except: - it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole; diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionRigFactor.h similarity index 95% rename from gtsam/slam/SmartProjectionFactorP.h rename to gtsam/slam/SmartProjectionRigFactor.h index b01420446..26cbffe7c 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionFactorP.h + * @file SmartProjectionRigFactor.h * @brief Smart factor on poses, assuming camera calibration is fixed. * Same as SmartProjectionPoseFactor, except: * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) @@ -47,11 +47,11 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionFactorP : public SmartProjectionFactor { +class SmartProjectionRigFactor : public SmartProjectionFactor { private: typedef SmartProjectionFactor Base; - typedef SmartProjectionFactorP This; + typedef SmartProjectionRigFactor This; typedef typename CAMERA::CalibrationType CALIBRATION; static const int DimPose = 6; ///< Pose3 dimension @@ -76,7 +76,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { typedef boost::shared_ptr shared_ptr; /// Default constructor, only for serialization - SmartProjectionFactorP() { + SmartProjectionRigFactor() { } /** @@ -84,7 +84,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { * @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param params parameters for the smart projection factors */ - SmartProjectionFactorP(const SharedNoiseModel& sharedNoiseModel, + SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { @@ -94,7 +94,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { } /** Virtual destructor */ - ~SmartProjectionFactorP() override { + ~SmartProjectionRigFactor() override { } /** @@ -167,7 +167,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "SmartProjectionFactorP: \n "; + std::cout << s << "SmartProjectionRigFactor: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; @@ -266,7 +266,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { std::vector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera - throw std::runtime_error("SmartProjectionFactorP: " + throw std::runtime_error("SmartProjectionRigFactor: " "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point @@ -282,7 +282,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { > (this->keys_, Gs, gs, 0.0); } else { throw std::runtime_error( - "SmartProjectionFactorP: " + "SmartProjectionRigFactor: " "only supported degeneracy mode is ZERO_ON_DEGENERACY"); } } @@ -351,8 +351,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { /// traits template -struct traits > : public Testable< - SmartProjectionFactorP > { +struct traits > : public Testable< + SmartProjectionRigFactor > { }; } // \ namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 7421f73af..f35bdc950 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -18,11 +18,11 @@ #pragma once #include -#include #include #include #include #include +#include "../SmartProjectionRigFactor.h" using namespace std; using namespace gtsam; @@ -70,7 +70,7 @@ SmartProjectionParams params; namespace vanillaPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Camera level_camera(level_pose, sharedK); Camera level_camera_right(pose_right, sharedK); @@ -84,7 +84,7 @@ Camera cam3(pose_above, sharedK); namespace vanillaPose2 { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); Camera level_camera(level_pose, sharedK2); Camera level_camera_right(pose_right, sharedK2); @@ -114,7 +114,7 @@ typedef GeneralSFMFactor SFMFactor; namespace bundlerPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp similarity index 96% rename from gtsam/slam/tests/testSmartProjectionFactorP.cpp rename to gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 4591dc08e..ef330cdd0 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionFactorP.cpp - * @brief Unit tests for SmartProjectionFactorP Class + * @file testSmartProjectionRigFactor.cpp + * @brief Unit tests for SmartProjectionRigFactor Class * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira @@ -52,13 +52,13 @@ LevenbergMarquardtParams lmParams; // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor) { +TEST( SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor2) { +TEST( SmartProjectionRigFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -66,14 +66,14 @@ TEST( SmartProjectionFactorP, Constructor2) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor3) { +TEST( SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); factor1->add(measurement1, x1, sharedK); } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor4) { +TEST( SmartProjectionRigFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -82,7 +82,7 @@ TEST( SmartProjectionFactorP, Constructor4) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Equals ) { +TEST( SmartProjectionRigFactor, Equals ) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); factor1->add(measurement1, x1, sharedK); @@ -94,7 +94,7 @@ TEST( SmartProjectionFactorP, Equals ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, noiseless ) { +TEST( SmartProjectionRigFactor, noiseless ) { using namespace vanillaPose; @@ -153,7 +153,7 @@ TEST( SmartProjectionFactorP, noiseless ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, noisy ) { +TEST( SmartProjectionRigFactor, noisy ) { using namespace vanillaPose; @@ -191,7 +191,7 @@ TEST( SmartProjectionFactorP, noisy ) { } /* *************************************************************************/ -TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { +TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) @@ -279,7 +279,7 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { +TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -346,7 +346,7 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Factors ) { +TEST( SmartProjectionRigFactor, Factors ) { using namespace vanillaPose; @@ -437,7 +437,7 @@ TEST( SmartProjectionFactorP, Factors ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { +TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -497,7 +497,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, landmarkDistance ) { +TEST( SmartProjectionRigFactor, landmarkDistance ) { using namespace vanillaPose; @@ -558,7 +558,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { +TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -626,7 +626,7 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, CheckHessian) { +TEST( SmartProjectionRigFactor, CheckHessian) { KeyVector views { x1, x2, x3 }; @@ -711,7 +711,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Hessian ) { +TEST( SmartProjectionRigFactor, Hessian ) { using namespace vanillaPose2; @@ -746,7 +746,7 @@ TEST( SmartProjectionFactorP, Hessian ) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { +TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -755,7 +755,7 @@ TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Cal3Bundler ) { +TEST( SmartProjectionRigFactor, Cal3Bundler ) { using namespace bundlerPose; @@ -820,7 +820,7 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { typedef GenericProjectionFactor TestProjectionFactor; static Symbol l0('L', 0); /* *************************************************************************/ -TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSamePose) { +TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark // at a single pose, a setup that occurs in multi-camera systems @@ -951,7 +951,7 @@ TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSameP } /* *************************************************************************/ -TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { +TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { using namespace vanillaPose; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -1033,7 +1033,7 @@ TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { //| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) //| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionFactorP, timing ) { +TEST( SmartProjectionRigFactor, timing ) { using namespace vanillaPose; @@ -1095,7 +1095,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropi BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); -TEST(SmartProjectionFactorP, serialize) { +TEST(SmartProjectionRigFactor, serialize) { using namespace vanillaPose; using namespace gtsam::serializationTestHelpers; SmartProjectionParams params; @@ -1108,7 +1108,7 @@ TEST(SmartProjectionFactorP, serialize) { } // SERIALIZATION TEST FAILS: to be fixed -//TEST(SmartProjectionFactorP, serialize2) { +//TEST(SmartProjectionRigFactor, serialize2) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; From a5db090fb4d01d16287b0a38f43322387c260d04 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 21:00:25 -0400 Subject: [PATCH 066/169] modernized factor --- gtsam/slam/SmartProjectionFactor.h | 1 + gtsam/slam/SmartProjectionRigFactor.h | 121 +- gtsam/slam/tests/smartFactorScenarios.h | 1 + .../tests/testSmartProjectionRigFactor.cpp | 2130 +++++++++-------- 4 files changed, 1119 insertions(+), 1134 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 9fb9c6905..f9c101cb8 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -71,6 +71,7 @@ protected: typedef boost::shared_ptr shared_ptr; /// shorthand for a set of cameras + typedef CAMERA Camera; typedef CameraSet Cameras; /** diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 26cbffe7c..600e48e8d 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -14,11 +14,10 @@ * @brief Smart factor on poses, assuming camera calibration is fixed. * Same as SmartProjectionPoseFactor, except: * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) - * - it admits a different calibration for each measurement (i.e., it can model a multi-camera system) + * - it admits a different calibration for each measurement (i.e., it can model a multi-camera rig system) * - it allows multiple observations from the same pose/key (again, to model a multi-camera system) * @author Luca Carlone - * @author Chris Beall - * @author Zsolt Kira + * @author Frank Dellaert */ #pragma once @@ -34,7 +33,6 @@ namespace gtsam { * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally * independent sets in factor graphs: a unifying perspective based on smart factors, * Int. Conf. on Robotics and Automation (ICRA), 2014. - * */ /** @@ -60,13 +58,13 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { protected: /// vector of keys (one for each observation) with potentially repeated keys - std::vector nonUniqueKeys_; + FastVector nonUniqueKeys_; - /// shared pointer to calibration object (one for each observation) - std::vector > K_all_; + /// cameras in the rig (fixed poses wrt body + fixed intrinsics) + typename Base::Cameras cameraRig_; - /// Pose of the camera in the body frame (one for each observation) - std::vector body_P_sensors_; + /// vector of camera Ids (one for each observation), identifying which camera took the measurement + FastVector cameraIds_; public: typedef CAMERA Camera; @@ -82,12 +80,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /** * Constructor * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in the camera rig * @param params parameters for the smart projection factors */ SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, + const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) { + : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.linearizationMode = gtsam::HESSIAN; @@ -98,17 +98,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** - * add a new measurement, corresponding to an observation from pose "poseKey" whose camera - * has intrinsic calibration K and extrinsic calibration body_P_sensor. + * add a new measurement, corresponding to an observation from pose "poseKey" + * and taken from the camera in the rig identified by "cameraId" * @param measured 2-dimensional location of the projection of a * single landmark in a single view (the measurement) * @param poseKey key corresponding to the body pose of the camera taking the measurement - * @param K (fixed) camera intrinsic calibration - * @param body_P_sensor (fixed) camera extrinsic calibration + * @param cameraId ID of the camera in the rig taking the measurement */ - void add(const Point2& measured, const Key& poseKey, - const boost::shared_ptr& K, const Pose3 body_P_sensor = - Pose3::identity()) { + void add(const Point2& measured, const Key& poseKey, const size_t cameraId) { // store measurement and key this->measured_.push_back(measured); this->nonUniqueKeys_.push_back(poseKey); @@ -117,10 +114,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) == this->keys_.end()) this->keys_.push_back(poseKey); // add only unique keys - // store fixed intrinsic calibration - K_all_.push_back(K); - // store fixed extrinsics of the camera - body_P_sensors_.push_back(body_P_sensor); + // store id of the camera taking the measurement + cameraIds_.push_back(cameraId); } /** @@ -128,38 +123,32 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param measurements vector of the 2m dimensional location of the projection * of a single landmark in the m views (the measurements) * @param poseKeys keys corresponding to the body poses of the cameras taking the measurements - * @param Ks vector of (fixed) intrinsic calibration objects - * @param body_P_sensors vector of (fixed) extrinsic calibration objects + * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as measurements) */ - void add(const Point2Vector& measurements, const std::vector& poseKeys, - const std::vector>& Ks, - const std::vector body_P_sensors = std::vector()) { + void add(const Point2Vector& measurements, const FastVector& poseKeys, + const FastVector& cameraIds) { assert(poseKeys.size() == measurements.size()); - assert(poseKeys.size() == Ks.size()); + assert(poseKeys.size() == cameraIds.size()); for (size_t i = 0; i < measurements.size(); i++) { - if (poseKeys.size() == body_P_sensors.size()) { - add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]); - } else { - add(measurements[i], poseKeys[i], Ks[i]); // use default extrinsics - } + add(measurements[i], poseKeys[i], cameraIds[i]); } } - /// return the calibration object - inline std::vector> calibration() const { - return K_all_; - } - - /// return the extrinsic camera calibration body_P_sensors - const std::vector body_P_sensors() const { - return body_P_sensors_; - } - /// return (for each observation) the (possibly non unique) keys involved in the measurements - const std::vector nonUniqueKeys() const { + const FastVector nonUniqueKeys() const { return nonUniqueKeys_; } + /// return the calibration object + inline Cameras cameraRig() const { + return cameraRig_; + } + + /// return the calibration object + inline FastVector cameraIds() const { + return cameraIds_; + } + /** * print * @param s optional string naming the factor @@ -168,11 +157,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionRigFactor: \n "; - for (size_t i = 0; i < K_all_.size(); i++) { + for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; - body_P_sensors_[i].print("extrinsic calibration:\n"); - K_all_[i]->print("intrinsic calibration = "); + std::cout << "cameraId: " << cameraIds_[i] << std::endl; + cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -180,35 +169,26 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); - double extrinsicCalibrationEqual = true; - if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { - for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { - if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { - extrinsicCalibrationEqual = false; - break; - } - } - } else { - extrinsicCalibrationEqual = false; - } - - return e && Base::equals(p, tol) && K_all_ == e->calibration() - && nonUniqueKeys_ == e->nonUniqueKeys() && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) + && nonUniqueKeys_ == e->nonUniqueKeys() + && cameraRig_.equals(e->cameraRig()); +// && cameraIds_ == e->cameraIds(); } /** * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses corresponding + * @param values Values structure which must contain body poses corresponding * to keys involved in this factor * @return vector of cameras */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Pose3& body_P_cam_i = body_P_sensors_[i]; + const Pose3& body_P_cam_i = cameraRig_[i].pose(); const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) * body_P_cam_i; - cameras.emplace_back(world_P_sensor_i, K_all_[i]); + cameras.emplace_back(world_P_sensor_i, + make_shared(cameraRig_[i].calibration())); } return cameras; } @@ -240,10 +220,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Pose3 sensor_P_body = body_P_sensors_[i].inverse(); + const Pose3 body_P_sensor = cameraRig_[i].pose(); + const Pose3 sensor_P_body = body_P_sensor.inverse(); const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; Eigen::Matrix H; - world_P_body.compose(body_P_sensors_[i], H); + world_P_body.compose(body_P_sensor, H); Fs.at(i) = Fs.at(i) * H; } } @@ -262,8 +243,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { // Create structures for Hessian Factors KeyVector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector < Vector > gs(nrUniqueKeys); + FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + FastVector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera throw std::runtime_error("SmartProjectionRigFactor: " @@ -324,7 +305,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectioFactorP: unknown linearization mode"); + "SmartProjectionRigFactor: unknown linearization mode"); } } @@ -341,9 +322,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); ar & BOOST_SERIALIZATION_NVP(nonUniqueKeys_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensors_); + ar & BOOST_SERIALIZATION_NVP(cameraRig_); + ar & BOOST_SERIALIZATION_NVP(cameraIds_); } }; diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index f35bdc950..bd4fb0642 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -69,6 +69,7 @@ SmartProjectionParams params; // default Cal3_S2 poses namespace vanillaPose { typedef PinholePose Camera; +typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartFactorP; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index ef330cdd0..19a1a9f19 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -54,1082 +54,1084 @@ LevenbergMarquardtParams lmParams; /* ************************************************************************* */ TEST( SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; - SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); + Cameras cameraRig; + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); } -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor2) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactorP factor1(model, params); -} - -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor3) { - using namespace vanillaPose; - SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); - factor1->add(measurement1, x1, sharedK); -} - -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor4) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactorP factor1(model, params); - factor1.add(measurement1, x1, sharedK); -} - -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Equals ) { - using namespace vanillaPose; - SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); - factor1->add(measurement1, x1, sharedK); - - SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); - factor2->add(measurement1, x1, sharedK); - - CHECK(assert_equal(*factor1, *factor2)); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, noiseless ) { - - using namespace vanillaPose; - - // Project two landmarks into two cameras - Point2 level_uv = level_camera.project(landmark1); - Point2 level_uv_right = level_camera_right.project(landmark1); - - SmartFactorP factor(model); - factor.add(level_uv, x1, sharedK); - factor.add(level_uv_right, x2, sharedK); - - Values values; // it's a pose factor, hence these are poses - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - double actualError = factor.error(values); - double expectedError = 0.0; - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - - SmartFactorP::Cameras cameras = factor.cameras(values); - double actualError2 = factor.totalReprojectionError(cameras); - EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); - - // Calculate expected derivative for point (easiest to check) - std::function f = // - std::bind(&SmartFactorP::whitenedError, factor, cameras, - std::placeholders::_1); - - // Calculate using computeEP - Matrix actualE; - factor.triangulateAndComputeE(actualE, values); - - // get point - boost::optional point = factor.point(); - CHECK(point); - - // calculate numerical derivative with triangulated point - Matrix expectedE = sigma * numericalDerivative11(f, *point); - EXPECT(assert_equal(expectedE, actualE, 1e-7)); - - // Calculate using reprojectionError - SmartFactorP::Cameras::FBlocks F; - Matrix E; - Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); - EXPECT(assert_equal(expectedE, E, 1e-7)); - - EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); - - // Calculate using computeJacobians - Vector b; - SmartFactorP::FBlocks Fs; - factor.computeJacobians(Fs, E, b, cameras, *point); - double actualError3 = b.squaredNorm(); - EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, noisy ) { - - using namespace vanillaPose; - - // Project two landmarks into two cameras - Point2 pixelError(0.2, 0.2); - Point2 level_uv = level_camera.project(landmark1) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark1); - - Values values; - values.insert(x1, cam1.pose()); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - values.insert(x2, pose_right.compose(noise_pose)); - - SmartFactorP::shared_ptr factor(new SmartFactorP(model)); - factor->add(level_uv, x1, sharedK); - factor->add(level_uv_right, x2, sharedK); - - double actualError1 = factor->error(values); - - SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); - Point2Vector measurements; - measurements.push_back(level_uv); - measurements.push_back(level_uv_right); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - KeyVector views { x1, x2 }; - - factor2->add(measurements, views, sharedKs); - double actualError2 = factor2->error(values); - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); -} - -/* *************************************************************************/ -TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { - using namespace vanillaPose; - - // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - - // These are the poses we want to estimate, from camera measurements - const Pose3 sensor_T_body = body_T_sensor.inverse(); - Pose3 wTb1 = cam1.pose() * sensor_T_body; - Pose3 wTb2 = cam2.pose() * sensor_T_body; - Pose3 wTb3 = cam3.pose() * sensor_T_body; - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - // Create smart factors - KeyVector views { x1, x2, x3 }; - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); - params.setEnableEPI(false); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - std::vector body_T_sensors; - body_T_sensors.push_back(body_T_sensor); - body_T_sensors.push_back(body_T_sensor); - body_T_sensors.push_back(body_T_sensor); - - SmartFactorP smartFactor1(model, params); - smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); - - SmartFactorP smartFactor2(model, params); - smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); - - SmartFactorP smartFactor3(model, params); - smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); - ; - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - // Put all factors in factor graph, adding priors - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, wTb1, noisePrior); - graph.addPrior(x2, wTb2, noisePrior); - - // Check errors at ground truth poses - Values gtValues; - gtValues.insert(x1, wTb1); - gtValues.insert(x2, wTb2); - gtValues.insert(x3, wTb3); - double actualError = graph.error(gtValues); - double expectedError = 0.0; - DOUBLES_EQUAL(expectedError, actualError, 1e-7) - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); - Values values; - values.insert(x1, wTb1); - values.insert(x2, wTb2); - // initialize third pose with some noise, we expect it to move back to - // original pose3 - values.insert(x3, wTb3 * noise_pose); - - LevenbergMarquardtParams lmParams; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); -// graph.print("graph\n"); - EXPECT(assert_equal(wTb3, result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { - - using namespace vanillaPose2; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; - sharedK2s.push_back(sharedK2); - sharedK2s.push_back(sharedK2); - sharedK2s.push_back(sharedK2); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_cam1, views, sharedK2s); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); - smartFactor2->add(measurements_cam2, views, sharedK2s); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); - smartFactor3->add(measurements_cam3, views, sharedK2s); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - Values groundTruth; - groundTruth.insert(x1, cam1.pose()); - groundTruth.insert(x2, cam2.pose()); - groundTruth.insert(x3, cam3.pose()); - DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, Factors ) { - - using namespace vanillaPose; - - // Default cameras for simple derivatives - Rot3 R; - static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); - Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( - Pose3(R, Point3(1, 0, 0)), sharedK); - - // one landmarks 1m in front of camera - Point3 landmark1(0, 0, 10); - - Point2Vector measurements_cam1; - - // Project 2 landmarks into 2 cameras - measurements_cam1.push_back(cam1.project(landmark1)); - measurements_cam1.push_back(cam2.project(landmark1)); - - // Create smart factors - KeyVector views { x1, x2 }; - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP - > (model); - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::Cameras cameras; - cameras.push_back(cam1); - cameras.push_back(cam2); - - // Make sure triangulation works - CHECK(smartFactor1->triangulateSafe(cameras)); - CHECK(!smartFactor1->isDegenerate()); - CHECK(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); - CHECK(p); - EXPECT(assert_equal(landmark1, *p)); - - VectorValues zeroDelta; - Vector6 delta; - delta.setZero(); - zeroDelta.insert(x1, delta); - zeroDelta.insert(x2, delta); - - VectorValues perturbedDelta; - delta.setOnes(); - perturbedDelta.insert(x1, delta); - perturbedDelta.insert(x2, delta); - double expectedError = 2500; - - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: - Matrix16 A1, A2; - A1 << -10, 0, 0, 0, 1, 0; - A2 << 10, 0, 1, 0, -1, 0; - A1 *= 10. / sigma; - A2 *= 10. / sigma; - Matrix expectedInformation; // filled below - { - // createHessianFactor - Matrix66 G11 = 0.5 * A1.transpose() * A1; - Matrix66 G12 = 0.5 * A1.transpose() * A2; - Matrix66 G22 = 0.5 * A2.transpose() * A2; - - Vector6 g1; - g1.setZero(); - Vector6 g2; - g2.setZero(); - - double f = 0; - - RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); - expectedInformation = expected.information(); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values, 0.0); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual, 1e-6)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { - - using namespace vanillaPose; - - KeyVector views { x1, x2, x3 }; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); - smartFactor2->add(measurements_cam2, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); - smartFactor3->add(measurements_cam3, views, sharedKs); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, - -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, landmarkDistance ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 2; - - KeyVector views { x1, x2, x3 }; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); - smartFactor2->add(measurements_cam2, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); - smartFactor3->add(measurements_cam3, views, sharedKs); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, pose_above * noise_pose); - - // All factors are disabled and pose should remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3), result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - - KeyVector views { x1, x2, x3 }; - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - // add fourth landmark - Point3 landmark4(5, -0.5, 1); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4; - - // Project 4 landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - - SmartProjectionParams params; - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); - smartFactor2->add(measurements_cam2, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); - smartFactor3->add(measurements_cam3, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params)); - smartFactor4->add(measurements_cam4, views, sharedKs); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(smartFactor4); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - // All factors are disabled and pose should remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(cam3.pose(), result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, CheckHessian) { - - KeyVector views { x1, x2, x3 }; - - using namespace vanillaPose; - - // Two slightly different cameras - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Camera cam2(pose2, sharedK); - Camera cam3(pose3, sharedK); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartProjectionParams params; - params.setRankTolerance(10); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default - smartFactor2->add(measurements_cam2, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default - smartFactor3->add(measurements_cam3, views, sharedKs); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); - - boost::shared_ptr factor1 = smartFactor1->linearize(values); - boost::shared_ptr factor2 = smartFactor2->linearize(values); - boost::shared_ptr factor3 = smartFactor3->linearize(values); - - Matrix CumulativeInformation = factor1->information() + factor2->information() - + factor3->information(); - - boost::shared_ptr GaussianGraph = graph.linearize( - values); - Matrix GraphInformation = GaussianGraph->hessian().first; - - // Check Hessian - EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); - - Matrix AugInformationMatrix = factor1->augmentedInformation() - + factor2->augmentedInformation() + factor3->augmentedInformation(); - - // Check Information vector - Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector - - // Check Hessian - EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, Hessian ) { - - using namespace vanillaPose2; - - KeyVector views { x1, x2 }; - - // Project three landmarks into 2 cameras - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2Vector measurements_cam1; - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; - sharedK2s.push_back(sharedK2); - sharedK2s.push_back(sharedK2); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_cam1, views, sharedK2s); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - boost::shared_ptr factor = smartFactor1->linearize(values); - - // compute triangulation from linearization point - // compute reprojection errors (sum squared) - // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) - // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] -} - -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { - using namespace bundlerPose; - SmartProjectionParams params; - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactorP factor(model, params); - factor.add(measurement1, x1, sharedBundlerK); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, Cal3Bundler ) { - - using namespace bundlerPose; - - // three landmarks ~5 meters in front of camera - Point3 landmark3(3, 0, 3.0); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - KeyVector views { x1, x2, x3 }; - - std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; - sharedBundlerKs.push_back(sharedBundlerK); - sharedBundlerKs.push_back(sharedBundlerK); - sharedBundlerKs.push_back(sharedBundlerK); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_cam1, views, sharedBundlerKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); - smartFactor2->add(measurements_cam2, views, sharedBundlerKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); - smartFactor3->add(measurements_cam3, views, sharedBundlerKs); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); -} - -#include -typedef GenericProjectionFactor TestProjectionFactor; -static Symbol l0('L', 0); -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { - // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark - // at a single pose, a setup that occurs in multi-camera systems - - using namespace vanillaPose; - Point2Vector measurements_lmk1; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - - // create redundant measurements: - Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement - - // create inputs - std::vector keys; - keys.push_back(x1); - keys.push_back(x2); - keys.push_back(x3); - keys.push_back(x1); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point - values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - - // linearization point for the poses - Pose3 pose1 = level_pose; - Pose3 pose2 = pose_right; - Pose3 pose3 = pose_above * noise_pose; - - // ==== check Hessian of smartFactor1 ===== - // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); - Matrix actualHessian = linearfactor1->information(); - - // -- compute expected Hessian from manual Schur complement from Jacobians - // linearization point for the 3D point - smartFactor1->triangulateSafe(smartFactor1->cameras(values)); - TriangulationResult point = smartFactor1->point(); - EXPECT(point.valid()); // check triangulated point is valid - - // Use standard ProjectionFactor factor to calculate the Jacobians - Matrix F = Matrix::Zero(2 * 4, 6 * 3); - Matrix E = Matrix::Zero(2 * 4, 3); - Vector b = Vector::Zero(2 * 4); - - // create projection factors rolling shutter - TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, - sharedK); - Matrix HPoseActual, HEActual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); - F.block<2, 6>(0, 0) = HPoseActual; - E.block<2, 3>(0, 0) = HEActual; - - TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, - sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, - HEActual); - F.block<2, 6>(2, 6) = HPoseActual; - E.block<2, 3>(2, 0) = HEActual; - - TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, - sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, - HEActual); - F.block<2, 6>(4, 12) = HPoseActual; - E.block<2, 3>(4, 0) = HEActual; - - TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, - sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); - F.block<2, 6>(6, 0) = HPoseActual; - E.block<2, 3>(6, 0) = HEActual; - - // whiten - F = (1 / sigma) * F; - E = (1 / sigma) * E; - b = (1 / sigma) * b; - //* G = F' * F - F' * E * P * E' * F - Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); - EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); - - // ==== check Information vector of smartFactor1 ===== - GaussianFactorGraph gfg; - gfg.add(linearfactor1); - Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian - - // -- compute actual information vector - Vector actualInfoVector = gfg.hessian().second; - - // -- compute expected information vector from manual Schur complement from Jacobians - //* g = F' * (b - E * P * E' * b) - Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); - EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); - - // ==== check error of smartFactor1 (again) ===== - NonlinearFactorGraph nfg_projFactors; - nfg_projFactors.add(factor11); - nfg_projFactors.add(factor12); - nfg_projFactors.add(factor13); - nfg_projFactors.add(factor14); - values.insert(l0, *point); - - double actualError = smartFactor1->error(values); - double expectedError = nfg_projFactors.error(values); - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { - - using namespace vanillaPose; - Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); - - // create inputs - std::vector keys; - keys.push_back(x1); - keys.push_back(x2); - keys.push_back(x3); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - // For first factor, we create redundant measurement (taken by the same keys as factor 1, to - // make sure the redundancy in the keys does not create problems) - Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement - std::vector keys_redundant = keys; - keys_redundant.push_back(keys.at(0)); // we readd the first key - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; - sharedKs_redundant.push_back(sharedK);// we readd the first calibration - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); - smartFactor2->add(measurements_lmk2, keys, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); - smartFactor3->add(measurements_lmk3, keys, sharedKs); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - Values groundTruth; - groundTruth.insert(x1, level_pose); - groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); - DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); -} - -#ifndef DISABLE_TIMING -#include -// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor -//-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) -//| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) -//| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, timing ) { - - using namespace vanillaPose; - - // Default cameras for simple derivatives - static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); - - Rot3 R = Rot3::identity(); - Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); - Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); - Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); - Pose3 body_P_sensorId = Pose3::identity(); - - // one landmarks 1m in front of camera - Point3 landmark1(0, 0, 10); - - Point2Vector measurements_lmk1; - - // Project 2 landmarks into 2 cameras - measurements_lmk1.push_back(cam1.project(landmark1)); - measurements_lmk1.push_back(cam2.project(landmark1)); - - size_t nrTests = 1000; - - for(size_t i = 0; iadd(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId); - smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - gttic_(SmartFactorP_LINEARIZE); - smartFactorP->linearize(values); - gttoc_(SmartFactorP_LINEARIZE); - } - - for(size_t i = 0; iadd(measurements_lmk1[0], x1); - smartFactor->add(measurements_lmk1[1], x2); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - gttic_(SmartPoseFactor_LINEARIZE); - smartFactor->linearize(values); - gttoc_(SmartPoseFactor_LINEARIZE); - } - tictoc_print_(); -} -#endif - -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); - -TEST(SmartProjectionRigFactor, serialize) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactorP factor(model, params); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - -// SERIALIZATION TEST FAILS: to be fixed -//TEST(SmartProjectionRigFactor, serialize2) { +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, Constructor2) { +// using namespace vanillaPose; +// SmartProjectionParams params; +// params.setRankTolerance(rankTol); +// SmartFactorP factor1(model, params); +//} +// +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, Constructor3) { +// using namespace vanillaPose; +// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); +// factor1->add(measurement1, x1, sharedK); +//} +// +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, Constructor4) { +// using namespace vanillaPose; +// SmartProjectionParams params; +// params.setRankTolerance(rankTol); +// SmartFactorP factor1(model, params); +// factor1.add(measurement1, x1, sharedK); +//} +// +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, Equals ) { +// using namespace vanillaPose; +// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); +// factor1->add(measurement1, x1, sharedK); +// +// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); +// factor2->add(measurement1, x1, sharedK); +// +// CHECK(assert_equal(*factor1, *factor2)); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, noiseless ) { +// +// using namespace vanillaPose; +// +// // Project two landmarks into two cameras +// Point2 level_uv = level_camera.project(landmark1); +// Point2 level_uv_right = level_camera_right.project(landmark1); +// +// SmartFactorP factor(model); +// factor.add(level_uv, x1, sharedK); +// factor.add(level_uv_right, x2, sharedK); +// +// Values values; // it's a pose factor, hence these are poses +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// +// double actualError = factor.error(values); +// double expectedError = 0.0; +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +// +// SmartFactorP::Cameras cameras = factor.cameras(values); +// double actualError2 = factor.totalReprojectionError(cameras); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); +// +// // Calculate expected derivative for point (easiest to check) +// std::function f = // +// std::bind(&SmartFactorP::whitenedError, factor, cameras, +// std::placeholders::_1); +// +// // Calculate using computeEP +// Matrix actualE; +// factor.triangulateAndComputeE(actualE, values); +// +// // get point +// boost::optional point = factor.point(); +// CHECK(point); +// +// // calculate numerical derivative with triangulated point +// Matrix expectedE = sigma * numericalDerivative11(f, *point); +// EXPECT(assert_equal(expectedE, actualE, 1e-7)); +// +// // Calculate using reprojectionError +// SmartFactorP::Cameras::FBlocks F; +// Matrix E; +// Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); +// EXPECT(assert_equal(expectedE, E, 1e-7)); +// +// EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); +// +// // Calculate using computeJacobians +// Vector b; +// SmartFactorP::FBlocks Fs; +// factor.computeJacobians(Fs, E, b, cameras, *point); +// double actualError3 = b.squaredNorm(); +// EXPECT(assert_equal(expectedE, E, 1e-7)); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, noisy ) { +// +// using namespace vanillaPose; +// +// // Project two landmarks into two cameras +// Point2 pixelError(0.2, 0.2); +// Point2 level_uv = level_camera.project(landmark1) + pixelError; +// Point2 level_uv_right = level_camera_right.project(landmark1); +// +// Values values; +// values.insert(x1, cam1.pose()); +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), +// Point3(0.5, 0.1, 0.3)); +// values.insert(x2, pose_right.compose(noise_pose)); +// +// SmartFactorP::shared_ptr factor(new SmartFactorP(model)); +// factor->add(level_uv, x1, sharedK); +// factor->add(level_uv_right, x2, sharedK); +// +// double actualError1 = factor->error(values); +// +// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); +// Point2Vector measurements; +// measurements.push_back(level_uv); +// measurements.push_back(level_uv_right); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// KeyVector views { x1, x2 }; +// +// factor2->add(measurements, views, sharedKs); +// double actualError2 = factor2->error(values); +// DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { +// using namespace vanillaPose; +// +// // create arbitrary body_T_sensor (transforms from sensor to body) +// Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), +// Point3(1, 1, 1)); +// +// // These are the poses we want to estimate, from camera measurements +// const Pose3 sensor_T_body = body_T_sensor.inverse(); +// Pose3 wTb1 = cam1.pose() * sensor_T_body; +// Pose3 wTb2 = cam2.pose() * sensor_T_body; +// Pose3 wTb3 = cam3.pose() * sensor_T_body; +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); +// +// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// // Create smart factors +// KeyVector views { x1, x2, x3 }; +// +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setDegeneracyMode(IGNORE_DEGENERACY); +// params.setEnableEPI(false); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// std::vector body_T_sensors; +// body_T_sensors.push_back(body_T_sensor); +// body_T_sensors.push_back(body_T_sensor); +// body_T_sensors.push_back(body_T_sensor); +// +// SmartFactorP smartFactor1(model, params); +// smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); +// +// SmartFactorP smartFactor2(model, params); +// smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); +// +// SmartFactorP smartFactor3(model, params); +// smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); +// ; +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// // Put all factors in factor graph, adding priors +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, wTb1, noisePrior); +// graph.addPrior(x2, wTb2, noisePrior); +// +// // Check errors at ground truth poses +// Values gtValues; +// gtValues.insert(x1, wTb1); +// gtValues.insert(x2, wTb2); +// gtValues.insert(x3, wTb3); +// double actualError = graph.error(gtValues); +// double expectedError = 0.0; +// DOUBLES_EQUAL(expectedError, actualError, 1e-7) +// +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); +// Values values; +// values.insert(x1, wTb1); +// values.insert(x2, wTb2); +// // initialize third pose with some noise, we expect it to move back to +// // original pose3 +// values.insert(x3, wTb3 * noise_pose); +// +// LevenbergMarquardtParams lmParams; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +//// graph.print("graph\n"); +// EXPECT(assert_equal(wTb3, result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { +// +// using namespace vanillaPose2; +// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// KeyVector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; +// sharedK2s.push_back(sharedK2); +// sharedK2s.push_back(sharedK2); +// sharedK2s.push_back(sharedK2); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_cam1, views, sharedK2s); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_cam2, views, sharedK2s); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_cam3, views, sharedK2s); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, cam1.pose(), noisePrior); +// graph.addPrior(x2, cam2.pose(), noisePrior); +// +// Values groundTruth; +// groundTruth.insert(x1, cam1.pose()); +// groundTruth.insert(x2, cam2.pose()); +// groundTruth.insert(x3, cam3.pose()); +// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); +// +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// // initialize third pose with some noise, we expect it to move back to original pose_above +// values.insert(x3, pose_above * noise_pose); +// EXPECT( +// assert_equal( +// Pose3( +// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, +// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), +// Point3(0.1, -0.1, 1.9)), +// values.at(x3))); +// +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, Factors ) { +// +// using namespace vanillaPose; +// +// // Default cameras for simple derivatives +// Rot3 R; +// static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); +// Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( +// Pose3(R, Point3(1, 0, 0)), sharedK); +// +// // one landmarks 1m in front of camera +// Point3 landmark1(0, 0, 10); +// +// Point2Vector measurements_cam1; +// +// // Project 2 landmarks into 2 cameras +// measurements_cam1.push_back(cam1.project(landmark1)); +// measurements_cam1.push_back(cam2.project(landmark1)); +// +// // Create smart factors +// KeyVector views { x1, x2 }; +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP +// > (model); +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::Cameras cameras; +// cameras.push_back(cam1); +// cameras.push_back(cam2); +// +// // Make sure triangulation works +// CHECK(smartFactor1->triangulateSafe(cameras)); +// CHECK(!smartFactor1->isDegenerate()); +// CHECK(!smartFactor1->isPointBehindCamera()); +// boost::optional p = smartFactor1->point(); +// CHECK(p); +// EXPECT(assert_equal(landmark1, *p)); +// +// VectorValues zeroDelta; +// Vector6 delta; +// delta.setZero(); +// zeroDelta.insert(x1, delta); +// zeroDelta.insert(x2, delta); +// +// VectorValues perturbedDelta; +// delta.setOnes(); +// perturbedDelta.insert(x1, delta); +// perturbedDelta.insert(x2, delta); +// double expectedError = 2500; +// +// // After eliminating the point, A1 and A2 contain 2-rank information on cameras: +// Matrix16 A1, A2; +// A1 << -10, 0, 0, 0, 1, 0; +// A2 << 10, 0, 1, 0, -1, 0; +// A1 *= 10. / sigma; +// A2 *= 10. / sigma; +// Matrix expectedInformation; // filled below +// { +// // createHessianFactor +// Matrix66 G11 = 0.5 * A1.transpose() * A1; +// Matrix66 G12 = 0.5 * A1.transpose() * A2; +// Matrix66 G22 = 0.5 * A2.transpose() * A2; +// +// Vector6 g1; +// g1.setZero(); +// Vector6 g2; +// g2.setZero(); +// +// double f = 0; +// +// RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); +// expectedInformation = expected.information(); +// +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// +// boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 +// ->createHessianFactor(values, 0.0); +// EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); +// EXPECT(assert_equal(expected, *actual, 1e-6)); +// EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); +// EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +// } +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { +// +// using namespace vanillaPose; +// +// KeyVector views { x1, x2, x3 }; +// +// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_cam2, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_cam3, views, sharedKs); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, cam1.pose(), noisePrior); +// graph.addPrior(x2, cam2.pose(), noisePrior); +// +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// // initialize third pose with some noise, we expect it to move back to original pose_above +// values.insert(x3, pose_above * noise_pose); +// EXPECT( +// assert_equal( +// Pose3( +// Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, +// -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, +// -0.0313952598), +// Point3(0.1, -0.1, 1.9)), +// values.at(x3))); +// +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, landmarkDistance ) { +// +// using namespace vanillaPose; +// +// double excludeLandmarksFutherThanDist = 2; +// +// KeyVector views { x1, x2, x3 }; +// +// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::JACOBIAN_SVD); +// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setEnableEPI(false); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); +// smartFactor2->add(measurements_cam2, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); +// smartFactor3->add(measurements_cam3, views, sharedKs); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, cam1.pose(), noisePrior); +// graph.addPrior(x2, cam2.pose(), noisePrior); +// +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// values.insert(x3, pose_above * noise_pose); +// +// // All factors are disabled and pose should remain where it is +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(values.at(x3), result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { +// +// using namespace vanillaPose; +// +// double excludeLandmarksFutherThanDist = 1e10; +// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error +// +// KeyVector views { x1, x2, x3 }; +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// // add fourth landmark +// Point3 landmark4(5, -0.5, 1); +// +// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, +// measurements_cam4; +// +// // Project 4 landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); +// measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier +// +// SmartProjectionParams params; +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); +// smartFactor2->add(measurements_cam2, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); +// smartFactor3->add(measurements_cam3, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params)); +// smartFactor4->add(measurements_cam4, views, sharedKs); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(smartFactor4); +// graph.addPrior(x1, cam1.pose(), noisePrior); +// graph.addPrior(x2, cam2.pose(), noisePrior); +// +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// values.insert(x3, cam3.pose()); +// +// // All factors are disabled and pose should remain where it is +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(cam3.pose(), result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, CheckHessian) { +// +// KeyVector views { x1, x2, x3 }; +// +// using namespace vanillaPose; +// +// // Two slightly different cameras +// Pose3 pose2 = level_pose +// * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); +// Camera cam2(pose2, sharedK); +// Camera cam3(pose3, sharedK); +// +// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartProjectionParams params; +// params.setRankTolerance(10); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default +// smartFactor2->add(measurements_cam2, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default +// smartFactor3->add(measurements_cam3, views, sharedKs); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// // initialize third pose with some noise, we expect it to move back to original pose_above +// values.insert(x3, pose3 * noise_pose); +// EXPECT( +// assert_equal( +// Pose3( +// Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, +// -0.130426831, -0.0115837907, 0.130819108, -0.98278564, +// -0.130455917), +// Point3(0.0897734171, -0.110201006, 0.901022872)), +// values.at(x3))); +// +// boost::shared_ptr factor1 = smartFactor1->linearize(values); +// boost::shared_ptr factor2 = smartFactor2->linearize(values); +// boost::shared_ptr factor3 = smartFactor3->linearize(values); +// +// Matrix CumulativeInformation = factor1->information() + factor2->information() +// + factor3->information(); +// +// boost::shared_ptr GaussianGraph = graph.linearize( +// values); +// Matrix GraphInformation = GaussianGraph->hessian().first; +// +// // Check Hessian +// EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); +// +// Matrix AugInformationMatrix = factor1->augmentedInformation() +// + factor2->augmentedInformation() + factor3->augmentedInformation(); +// +// // Check Information vector +// Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector +// +// // Check Hessian +// EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, Hessian ) { +// +// using namespace vanillaPose2; +// +// KeyVector views { x1, x2 }; +// +// // Project three landmarks into 2 cameras +// Point2 cam1_uv1 = cam1.project(landmark1); +// Point2 cam2_uv1 = cam2.project(landmark1); +// Point2Vector measurements_cam1; +// measurements_cam1.push_back(cam1_uv1); +// measurements_cam1.push_back(cam2_uv1); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; +// sharedK2s.push_back(sharedK2); +// sharedK2s.push_back(sharedK2); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_cam1, views, sharedK2s); +// +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), +// Point3(0.5, 0.1, 0.3)); +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// +// boost::shared_ptr factor = smartFactor1->linearize(values); +// +// // compute triangulation from linearization point +// // compute reprojection errors (sum squared) +// // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) +// // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] +//} +// +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { +// using namespace bundlerPose; +// SmartProjectionParams params; +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// SmartFactorP factor(model, params); +// factor.add(measurement1, x1, sharedBundlerK); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, Cal3Bundler ) { +// +// using namespace bundlerPose; +// +// // three landmarks ~5 meters in front of camera +// Point3 landmark3(3, 0, 3.0); +// +// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// KeyVector views { x1, x2, x3 }; +// +// std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; +// sharedBundlerKs.push_back(sharedBundlerK); +// sharedBundlerKs.push_back(sharedBundlerK); +// sharedBundlerKs.push_back(sharedBundlerK); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_cam1, views, sharedBundlerKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_cam2, views, sharedBundlerKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_cam3, views, sharedBundlerKs); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, cam1.pose(), noisePrior); +// graph.addPrior(x2, cam2.pose(), noisePrior); +// +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// // initialize third pose with some noise, we expect it to move back to original pose_above +// values.insert(x3, pose_above * noise_pose); +// EXPECT( +// assert_equal( +// Pose3( +// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, +// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), +// Point3(0.1, -0.1, 1.9)), +// values.at(x3))); +// +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); +//} +// +//#include +//typedef GenericProjectionFactor TestProjectionFactor; +//static Symbol l0('L', 0); +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { +// // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark +// // at a single pose, a setup that occurs in multi-camera systems +// +// using namespace vanillaPose; +// Point2Vector measurements_lmk1; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// +// // create redundant measurements: +// Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; +// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement +// +// // create inputs +// std::vector keys; +// keys.push_back(x1); +// keys.push_back(x2); +// keys.push_back(x3); +// keys.push_back(x1); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); +// +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise to get a nontrivial linearization point +// values.insert(x3, pose_above * noise_pose); +// EXPECT( // check that the pose is actually noisy +// assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); +// +// // linearization point for the poses +// Pose3 pose1 = level_pose; +// Pose3 pose2 = pose_right; +// Pose3 pose3 = pose_above * noise_pose; +// +// // ==== check Hessian of smartFactor1 ===== +// // -- compute actual Hessian +// boost::shared_ptr linearfactor1 = smartFactor1->linearize( +// values); +// Matrix actualHessian = linearfactor1->information(); +// +// // -- compute expected Hessian from manual Schur complement from Jacobians +// // linearization point for the 3D point +// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); +// TriangulationResult point = smartFactor1->point(); +// EXPECT(point.valid()); // check triangulated point is valid +// +// // Use standard ProjectionFactor factor to calculate the Jacobians +// Matrix F = Matrix::Zero(2 * 4, 6 * 3); +// Matrix E = Matrix::Zero(2 * 4, 3); +// Vector b = Vector::Zero(2 * 4); +// +// // create projection factors rolling shutter +// TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, +// sharedK); +// Matrix HPoseActual, HEActual; +// // note: b is minus the reprojection error, cf the smart factor jacobian computation +// b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, +// HEActual); +// F.block<2, 6>(0, 0) = HPoseActual; +// E.block<2, 3>(0, 0) = HEActual; +// +// TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, +// sharedK); +// b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, +// HEActual); +// F.block<2, 6>(2, 6) = HPoseActual; +// E.block<2, 3>(2, 0) = HEActual; +// +// TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, +// sharedK); +// b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, +// HEActual); +// F.block<2, 6>(4, 12) = HPoseActual; +// E.block<2, 3>(4, 0) = HEActual; +// +// TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, +// sharedK); +// b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, +// HEActual); +// F.block<2, 6>(6, 0) = HPoseActual; +// E.block<2, 3>(6, 0) = HEActual; +// +// // whiten +// F = (1 / sigma) * F; +// E = (1 / sigma) * E; +// b = (1 / sigma) * b; +// //* G = F' * F - F' * E * P * E' * F +// Matrix P = (E.transpose() * E).inverse(); +// Matrix expectedHessian = F.transpose() * F +// - (F.transpose() * E * P * E.transpose() * F); +// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); +// +// // ==== check Information vector of smartFactor1 ===== +// GaussianFactorGraph gfg; +// gfg.add(linearfactor1); +// Matrix actualHessian_v2 = gfg.hessian().first; +// EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian +// +// // -- compute actual information vector +// Vector actualInfoVector = gfg.hessian().second; +// +// // -- compute expected information vector from manual Schur complement from Jacobians +// //* g = F' * (b - E * P * E' * b) +// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); +// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); +// +// // ==== check error of smartFactor1 (again) ===== +// NonlinearFactorGraph nfg_projFactors; +// nfg_projFactors.add(factor11); +// nfg_projFactors.add(factor12); +// nfg_projFactors.add(factor13); +// nfg_projFactors.add(factor14); +// values.insert(l0, *point); +// +// double actualError = smartFactor1->error(values); +// double expectedError = nfg_projFactors.error(values); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { +// +// using namespace vanillaPose; +// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); +// +// // create inputs +// std::vector keys; +// keys.push_back(x1); +// keys.push_back(x2); +// keys.push_back(x3); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// // For first factor, we create redundant measurement (taken by the same keys as factor 1, to +// // make sure the redundancy in the keys does not create problems) +// Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; +// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement +// std::vector keys_redundant = keys; +// keys_redundant.push_back(keys.at(0)); // we readd the first key +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; +// sharedKs_redundant.push_back(sharedK);// we readd the first calibration +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_lmk2, keys, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_lmk3, keys, sharedKs); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, level_pose, noisePrior); +// graph.addPrior(x2, pose_right, noisePrior); +// +// Values groundTruth; +// groundTruth.insert(x1, level_pose); +// groundTruth.insert(x2, pose_right); +// groundTruth.insert(x3, pose_above); +// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); +// +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise, we expect it to move back to original pose_above +// values.insert(x3, pose_above * noise_pose); +// EXPECT( // check that the pose is actually noisy +// assert_equal( +// Pose3( +// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, +// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), +// Point3(0.1, -0.1, 1.9)), values.at(x3))); +// +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +//} +// +//#ifndef DISABLE_TIMING +//#include +//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor +////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) +////| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) +////| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, timing ) { +// +// using namespace vanillaPose; +// +// // Default cameras for simple derivatives +// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); +// +// Rot3 R = Rot3::identity(); +// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); +// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); +// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); +// Pose3 body_P_sensorId = Pose3::identity(); +// +// // one landmarks 1m in front of camera +// Point3 landmark1(0, 0, 10); +// +// Point2Vector measurements_lmk1; +// +// // Project 2 landmarks into 2 cameras +// measurements_lmk1.push_back(cam1.project(landmark1)); +// measurements_lmk1.push_back(cam2.project(landmark1)); +// +// size_t nrTests = 1000; +// +// for(size_t i = 0; iadd(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId); +// smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(SmartFactorP_LINEARIZE); +// smartFactorP->linearize(values); +// gttoc_(SmartFactorP_LINEARIZE); +// } +// +// for(size_t i = 0; iadd(measurements_lmk1[0], x1); +// smartFactor->add(measurements_lmk1[1], x2); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(SmartPoseFactor_LINEARIZE); +// smartFactor->linearize(values); +// gttoc_(SmartPoseFactor_LINEARIZE); +// } +// tictoc_print_(); +//} +//#endif +// +///* ************************************************************************* */ +//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +//BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +// +//TEST(SmartProjectionRigFactor, serialize) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); // SmartFactorP factor(model, params); // -// // insert some measurements -// KeyVector key_view; -// Point2Vector meas_view; -// std::vector> sharedKs; -// -// -// key_view.push_back(Symbol('x', 1)); -// meas_view.push_back(Point2(10, 10)); -// sharedKs.push_back(sharedK); -// factor.add(meas_view, key_view, sharedKs); -// // EXPECT(equalsObj(factor)); // EXPECT(equalsXML(factor)); // EXPECT(equalsBinary(factor)); //} +// +//// SERIALIZATION TEST FAILS: to be fixed +////TEST(SmartProjectionRigFactor, serialize2) { +//// using namespace vanillaPose; +//// using namespace gtsam::serializationTestHelpers; +//// SmartProjectionParams params; +//// params.setRankTolerance(rankTol); +//// SmartFactorP factor(model, params); +//// +//// // insert some measurements +//// KeyVector key_view; +//// Point2Vector meas_view; +//// std::vector> sharedKs; +//// +//// +//// key_view.push_back(Symbol('x', 1)); +//// meas_view.push_back(Point2(10, 10)); +//// sharedKs.push_back(sharedK); +//// factor.add(meas_view, key_view, sharedKs); +//// +//// EXPECT(equalsObj(factor)); +//// EXPECT(equalsXML(factor)); +//// EXPECT(equalsBinary(factor)); +////} /* ************************************************************************* */ int main() { From 4c10be9808983c4c4533b780fecf503cefc4ddf9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 21:12:46 -0400 Subject: [PATCH 067/169] fixed equal --- gtsam/slam/SmartProjectionRigFactor.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 600e48e8d..07efd775c 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -58,13 +58,13 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { protected: /// vector of keys (one for each observation) with potentially repeated keys - FastVector nonUniqueKeys_; + KeyVector nonUniqueKeys_; /// cameras in the rig (fixed poses wrt body + fixed intrinsics) typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation), identifying which camera took the measurement - FastVector cameraIds_; + KeyVector cameraIds_; public: typedef CAMERA Camera; @@ -125,7 +125,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param poseKeys keys corresponding to the body poses of the cameras taking the measurements * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as measurements) */ - void add(const Point2Vector& measurements, const FastVector& poseKeys, + void add(const Point2Vector& measurements, const KeyVector& poseKeys, const FastVector& cameraIds) { assert(poseKeys.size() == measurements.size()); assert(poseKeys.size() == cameraIds.size()); @@ -135,7 +135,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// return (for each observation) the (possibly non unique) keys involved in the measurements - const FastVector nonUniqueKeys() const { + const KeyVector nonUniqueKeys() const { return nonUniqueKeys_; } @@ -145,7 +145,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// return the calibration object - inline FastVector cameraIds() const { + inline KeyVector cameraIds() const { return cameraIds_; } @@ -171,8 +171,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() - && cameraRig_.equals(e->cameraRig()); -// && cameraIds_ == e->cameraIds(); + && cameraRig_.equals(e->cameraRig()) + && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } /** From 117f0d1f45506085e39aab5aad4a9cc2226d3bd4 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 21:47:44 -0400 Subject: [PATCH 068/169] fixing tests --- gtsam/slam/SmartProjectionRigFactor.h | 14 +- gtsam/slam/tests/smartFactorScenarios.h | 1 + .../tests/testSmartProjectionRigFactor.cpp | 580 +++++++++--------- 3 files changed, 301 insertions(+), 294 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 07efd775c..3d7f7f626 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -64,7 +64,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation), identifying which camera took the measurement - KeyVector cameraIds_; + FastVector cameraIds_; public: typedef CAMERA Camera; @@ -145,7 +145,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// return the calibration object - inline KeyVector cameraIds() const { + inline FastVector cameraIds() const { return cameraIds_; } @@ -184,11 +184,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Pose3& body_P_cam_i = cameraRig_[i].pose(); + const Key cameraId = cameraIds_[i]; + const Pose3& body_P_cam_i = cameraRig_[cameraId].pose(); const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) * body_P_cam_i; cameras.emplace_back(world_P_sensor_i, - make_shared(cameraRig_[i].calibration())); + make_shared(cameraRig_[cameraId].calibration())); } return cameras; } @@ -220,7 +221,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Pose3 body_P_sensor = cameraRig_[i].pose(); + const Key cameraId = cameraIds_[i]; + const Pose3 body_P_sensor = cameraRig_[cameraId].pose(); const Pose3 sensor_P_body = body_P_sensor.inverse(); const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; Eigen::Matrix H; @@ -242,7 +244,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Cameras cameras = this->cameras(values); // Create structures for Hessian Factors - KeyVector js; + FastVector js; FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); FastVector < Vector > gs(nrUniqueKeys); diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index bd4fb0642..a9d0c43f7 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -84,6 +84,7 @@ Camera cam3(pose_above, sharedK); // default Cal3_S2 poses namespace vanillaPose2 { typedef PinholePose Camera; +typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartFactorP; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 19a1a9f19..d5206818e 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -45,6 +45,10 @@ static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); +Key cameraId1 = 0; // first camera +Key cameraId2 = 1; +Key cameraId3 = 2; + static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; @@ -59,294 +63,294 @@ TEST( SmartProjectionRigFactor, Constructor) { SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); } -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, Constructor2) { -// using namespace vanillaPose; -// SmartProjectionParams params; -// params.setRankTolerance(rankTol); -// SmartFactorP factor1(model, params); -//} -// -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, Constructor3) { -// using namespace vanillaPose; -// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); -// factor1->add(measurement1, x1, sharedK); -//} -// -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, Constructor4) { -// using namespace vanillaPose; -// SmartProjectionParams params; -// params.setRankTolerance(rankTol); -// SmartFactorP factor1(model, params); -// factor1.add(measurement1, x1, sharedK); -//} -// -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, Equals ) { -// using namespace vanillaPose; -// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); -// factor1->add(measurement1, x1, sharedK); -// -// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); -// factor2->add(measurement1, x1, sharedK); -// -// CHECK(assert_equal(*factor1, *factor2)); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, noiseless ) { -// -// using namespace vanillaPose; -// -// // Project two landmarks into two cameras -// Point2 level_uv = level_camera.project(landmark1); -// Point2 level_uv_right = level_camera_right.project(landmark1); -// -// SmartFactorP factor(model); -// factor.add(level_uv, x1, sharedK); -// factor.add(level_uv_right, x2, sharedK); -// -// Values values; // it's a pose factor, hence these are poses -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// -// double actualError = factor.error(values); -// double expectedError = 0.0; -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -// -// SmartFactorP::Cameras cameras = factor.cameras(values); -// double actualError2 = factor.totalReprojectionError(cameras); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); -// -// // Calculate expected derivative for point (easiest to check) -// std::function f = // -// std::bind(&SmartFactorP::whitenedError, factor, cameras, -// std::placeholders::_1); -// -// // Calculate using computeEP -// Matrix actualE; -// factor.triangulateAndComputeE(actualE, values); -// -// // get point -// boost::optional point = factor.point(); -// CHECK(point); -// -// // calculate numerical derivative with triangulated point -// Matrix expectedE = sigma * numericalDerivative11(f, *point); -// EXPECT(assert_equal(expectedE, actualE, 1e-7)); -// -// // Calculate using reprojectionError -// SmartFactorP::Cameras::FBlocks F; -// Matrix E; -// Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); -// EXPECT(assert_equal(expectedE, E, 1e-7)); -// -// EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); -// -// // Calculate using computeJacobians -// Vector b; -// SmartFactorP::FBlocks Fs; -// factor.computeJacobians(Fs, E, b, cameras, *point); -// double actualError3 = b.squaredNorm(); -// EXPECT(assert_equal(expectedE, E, 1e-7)); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, noisy ) { -// -// using namespace vanillaPose; -// -// // Project two landmarks into two cameras -// Point2 pixelError(0.2, 0.2); -// Point2 level_uv = level_camera.project(landmark1) + pixelError; -// Point2 level_uv_right = level_camera_right.project(landmark1); -// -// Values values; -// values.insert(x1, cam1.pose()); -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), -// Point3(0.5, 0.1, 0.3)); -// values.insert(x2, pose_right.compose(noise_pose)); -// -// SmartFactorP::shared_ptr factor(new SmartFactorP(model)); -// factor->add(level_uv, x1, sharedK); -// factor->add(level_uv_right, x2, sharedK); -// -// double actualError1 = factor->error(values); -// -// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); -// Point2Vector measurements; -// measurements.push_back(level_uv); -// measurements.push_back(level_uv_right); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// KeyVector views { x1, x2 }; -// -// factor2->add(measurements, views, sharedKs); -// double actualError2 = factor2->error(values); -// DOUBLES_EQUAL(actualError1, actualError2, 1e-7); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { -// using namespace vanillaPose; -// -// // create arbitrary body_T_sensor (transforms from sensor to body) -// Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), -// Point3(1, 1, 1)); -// -// // These are the poses we want to estimate, from camera measurements -// const Pose3 sensor_T_body = body_T_sensor.inverse(); -// Pose3 wTb1 = cam1.pose() * sensor_T_body; -// Pose3 wTb2 = cam2.pose() * sensor_T_body; -// Pose3 wTb3 = cam3.pose() * sensor_T_body; -// -// // three landmarks ~5 meters infront of camera -// Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); -// -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// -// // Create smart factors -// KeyVector views { x1, x2, x3 }; -// -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setDegeneracyMode(IGNORE_DEGENERACY); -// params.setEnableEPI(false); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// std::vector body_T_sensors; -// body_T_sensors.push_back(body_T_sensor); -// body_T_sensors.push_back(body_T_sensor); -// body_T_sensors.push_back(body_T_sensor); -// -// SmartFactorP smartFactor1(model, params); -// smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); -// -// SmartFactorP smartFactor2(model, params); -// smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); -// -// SmartFactorP smartFactor3(model, params); -// smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); -// ; -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// // Put all factors in factor graph, adding priors -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, wTb1, noisePrior); -// graph.addPrior(x2, wTb2, noisePrior); -// -// // Check errors at ground truth poses -// Values gtValues; -// gtValues.insert(x1, wTb1); -// gtValues.insert(x2, wTb2); -// gtValues.insert(x3, wTb3); -// double actualError = graph.error(gtValues); -// double expectedError = 0.0; -// DOUBLES_EQUAL(expectedError, actualError, 1e-7) -// -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); -// Values values; -// values.insert(x1, wTb1); -// values.insert(x2, wTb2); -// // initialize third pose with some noise, we expect it to move back to -// // original pose3 -// values.insert(x3, wTb3 * noise_pose); -// -// LevenbergMarquardtParams lmParams; -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -//// graph.print("graph\n"); -// EXPECT(assert_equal(wTb3, result.at(x3))); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { -// -// using namespace vanillaPose2; -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// -// KeyVector views; -// views.push_back(x1); -// views.push_back(x2); -// views.push_back(x3); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; -// sharedK2s.push_back(sharedK2); -// sharedK2s.push_back(sharedK2); -// sharedK2s.push_back(sharedK2); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedK2s); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_cam2, views, sharedK2s); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_cam3, views, sharedK2s); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, cam1.pose(), noisePrior); -// graph.addPrior(x2, cam2.pose(), noisePrior); -// -// Values groundTruth; -// groundTruth.insert(x1, cam1.pose()); -// groundTruth.insert(x2, cam2.pose()); -// groundTruth.insert(x3, cam3.pose()); -// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// // initialize third pose with some noise, we expect it to move back to original pose_above -// values.insert(x3, pose_above * noise_pose); -// EXPECT( -// assert_equal( -// Pose3( -// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), -// Point3(0.1, -0.1, 1.9)), -// values.at(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -//} -// +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Constructor2) { + using namespace vanillaPose; + Cameras cameraRig; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorP factor1(model, cameraRig, params); +} + +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Constructor3) { + using namespace vanillaPose; + Cameras cameraRig; + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + factor1->add(measurement1, x1, cameraId1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Constructor4) { + using namespace vanillaPose; + Cameras cameraRig; + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorP factor1(model, cameraRig, params); + factor1.add(measurement1, x1, cameraId1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Equals ) { + using namespace vanillaPose; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + + SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + factor1->add(measurement1, x1, cameraId1); + + SmartFactorP::shared_ptr factor2(new SmartFactorP(model, cameraRig)); + factor2->add(measurement1, x1, cameraId1); + + CHECK(assert_equal(*factor1, *factor2)); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, noiseless ) { + + using namespace vanillaPose; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); + + SmartFactorP factor(model, cameraRig); + factor.add(level_uv, x1, cameraId1); // both taken from the same camera + factor.add(level_uv_right, x2, cameraId1); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + SmartFactorP::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // Calculate expected derivative for point (easiest to check) + std::function f = // + std::bind(&SmartFactorP::whitenedError, factor, cameras, + std::placeholders::_1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using reprojectionError + SmartFactorP::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + SmartFactorP::FBlocks Fs; + factor.computeJacobians(Fs, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, noisy ) { + + using namespace vanillaPose; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + + // Project two landmarks into two cameras + Point2 pixelError(0.2, 0.2); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); + + Values values; + values.insert(x1, cam1.pose()); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + values.insert(x2, pose_right.compose(noise_pose)); + + SmartFactorP::shared_ptr factor(new SmartFactorP(model,cameraRig)); + factor->add(level_uv, x1, cameraId1); + factor->add(level_uv_right, x2, cameraId1); + + double actualError1 = factor->error(values); + + // create other factor by passing multiple measurements + SmartFactorP::shared_ptr factor2(new SmartFactorP(model,cameraRig)); + + Point2Vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + KeyVector views { x1, x2 }; + FastVector cameraIds { 0, 0 }; + + factor2->add(measurements, views, cameraIds); + double actualError2 = factor2->error(values); + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { + using namespace vanillaPose; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(body_T_sensor, sharedK) ); + + // These are the poses we want to estimate, from camera measurements + const Pose3 sensor_T_body = body_T_sensor.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body; + Pose3 wTb2 = cam2.pose() * sensor_T_body; + Pose3 wTb3 = cam3.pose() * sensor_T_body; + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + KeyVector views { x1, x2, x3 }; + FastVector cameraIds { 0, 0, 0 }; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + SmartFactorP smartFactor1(model, cameraRig, params); + smartFactor1.add(measurements_cam1, views, cameraIds); + + SmartFactorP smartFactor2(model, cameraRig, params); + smartFactor2.add(measurements_cam2, views, cameraIds); + + SmartFactorP smartFactor3(model, cameraRig, params); + smartFactor3.add(measurements_cam3, views, cameraIds); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, wTb1, noisePrior); + graph.addPrior(x2, wTb2, noisePrior); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, wTb1); + gtValues.insert(x2, wTb2); + gtValues.insert(x3, wTb3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); + Values values; + values.insert(x1, wTb1); + values.insert(x2, wTb2); + // initialize third pose with some noise, we expect it to move back to + // original pose3 + values.insert(x3, wTb3 * noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(wTb3, result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { + + using namespace vanillaPose2; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + KeyVector views {x1,x2,x3}; + FastVector cameraIds{0,0,0};// 3 measurements from the same camera in the rig + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model,cameraRig)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model,cameraRig)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values groundTruth; + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + groundTruth.insert(x3, cam3.pose()); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + ///* *************************************************************************/ //TEST( SmartProjectionRigFactor, Factors ) { // From eb82878044396bfd2d025274965e1190c6131bac Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 21:51:44 -0400 Subject: [PATCH 069/169] halfway there --- .../tests/testSmartProjectionRigFactor.cpp | 306 +++++++++--------- 1 file changed, 155 insertions(+), 151 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index d5206818e..fcdf3e2a2 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -351,157 +351,161 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, Factors ) { -// -// using namespace vanillaPose; -// -// // Default cameras for simple derivatives -// Rot3 R; -// static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); -// Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( -// Pose3(R, Point3(1, 0, 0)), sharedK); -// -// // one landmarks 1m in front of camera -// Point3 landmark1(0, 0, 10); -// -// Point2Vector measurements_cam1; -// -// // Project 2 landmarks into 2 cameras -// measurements_cam1.push_back(cam1.project(landmark1)); -// measurements_cam1.push_back(cam2.project(landmark1)); -// -// // Create smart factors -// KeyVector views { x1, x2 }; -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP -// > (model); -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::Cameras cameras; -// cameras.push_back(cam1); -// cameras.push_back(cam2); -// -// // Make sure triangulation works -// CHECK(smartFactor1->triangulateSafe(cameras)); -// CHECK(!smartFactor1->isDegenerate()); -// CHECK(!smartFactor1->isPointBehindCamera()); -// boost::optional p = smartFactor1->point(); -// CHECK(p); -// EXPECT(assert_equal(landmark1, *p)); -// -// VectorValues zeroDelta; -// Vector6 delta; -// delta.setZero(); -// zeroDelta.insert(x1, delta); -// zeroDelta.insert(x2, delta); -// -// VectorValues perturbedDelta; -// delta.setOnes(); -// perturbedDelta.insert(x1, delta); -// perturbedDelta.insert(x2, delta); -// double expectedError = 2500; -// -// // After eliminating the point, A1 and A2 contain 2-rank information on cameras: -// Matrix16 A1, A2; -// A1 << -10, 0, 0, 0, 1, 0; -// A2 << 10, 0, 1, 0, -1, 0; -// A1 *= 10. / sigma; -// A2 *= 10. / sigma; -// Matrix expectedInformation; // filled below -// { -// // createHessianFactor -// Matrix66 G11 = 0.5 * A1.transpose() * A1; -// Matrix66 G12 = 0.5 * A1.transpose() * A2; -// Matrix66 G22 = 0.5 * A2.transpose() * A2; -// -// Vector6 g1; -// g1.setZero(); -// Vector6 g2; -// g2.setZero(); -// -// double f = 0; -// -// RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); -// expectedInformation = expected.information(); -// -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// -// boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 -// ->createHessianFactor(values, 0.0); -// EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); -// EXPECT(assert_equal(expected, *actual, 1e-6)); -// EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); -// EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); -// } -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { -// -// using namespace vanillaPose; -// -// KeyVector views { x1, x2, x3 }; -// -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_cam2, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_cam3, views, sharedKs); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, cam1.pose(), noisePrior); -// graph.addPrior(x2, cam2.pose(), noisePrior); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// // initialize third pose with some noise, we expect it to move back to original pose_above -// values.insert(x3, pose_above * noise_pose); -// EXPECT( -// assert_equal( -// Pose3( -// Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, -// -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -// -0.0313952598), -// Point3(0.1, -0.1, 1.9)), -// values.at(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); -//} -// +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, Factors ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Pose3(R, Point3(1, 0, 0)), sharedK); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + + KeyVector views { x1, x2 }; + FastVector cameraIds { 0, 0 }; + + SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP + > (model,cameraRig); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + CHECK(smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + { + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 + ->createHessianFactor(values, 0.0); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { + + using namespace vanillaPose; + + KeyVector views { x1, x2, x3 }; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + // create smart factor + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0}; + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); +} + ///* *************************************************************************/ //TEST( SmartProjectionRigFactor, landmarkDistance ) { // From fa4de18742e1b11c6742ae50cfff6c96b3df4a21 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 22:00:09 -0400 Subject: [PATCH 070/169] working on tests --- gtsam/slam/SmartProjectionRigFactor.h | 6 +- .../tests/testSmartProjectionRigFactor.cpp | 495 +++++++++--------- 2 files changed, 250 insertions(+), 251 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 3d7f7f626..2ec2ed86f 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -127,8 +127,10 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, const FastVector& cameraIds) { - assert(poseKeys.size() == measurements.size()); - assert(poseKeys.size() == cameraIds.size()); + if(poseKeys.size() != measurements.size() || poseKeys.size() != cameraIds.size()){ + throw std::runtime_error("SmartProjectionRigFactor: " + "trying to add inconsistent inputs"); + } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], poseKeys[i], cameraIds[i]); } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index fcdf3e2a2..4c6b33655 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -506,255 +506,252 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, landmarkDistance ) { -// -// using namespace vanillaPose; -// -// double excludeLandmarksFutherThanDist = 2; -// -// KeyVector views { x1, x2, x3 }; -// -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::JACOBIAN_SVD); -// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setEnableEPI(false); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); -// smartFactor2->add(measurements_cam2, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); -// smartFactor3->add(measurements_cam3, views, sharedKs); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, cam1.pose(), noisePrior); -// graph.addPrior(x2, cam2.pose(), noisePrior); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// values.insert(x3, pose_above * noise_pose); -// -// // All factors are disabled and pose should remain where it is -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(values.at(x3), result.at(x3))); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { -// -// using namespace vanillaPose; -// -// double excludeLandmarksFutherThanDist = 1e10; -// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error -// -// KeyVector views { x1, x2, x3 }; -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// // add fourth landmark -// Point3 landmark4(5, -0.5, 1); -// -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, -// measurements_cam4; -// -// // Project 4 landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); -// measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier -// -// SmartProjectionParams params; -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); -// smartFactor2->add(measurements_cam2, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); -// smartFactor3->add(measurements_cam3, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params)); -// smartFactor4->add(measurements_cam4, views, sharedKs); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.push_back(smartFactor4); -// graph.addPrior(x1, cam1.pose(), noisePrior); -// graph.addPrior(x2, cam2.pose(), noisePrior); -// -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// values.insert(x3, cam3.pose()); -// -// // All factors are disabled and pose should remain where it is -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(cam3.pose(), result.at(x3))); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, CheckHessian) { -// -// KeyVector views { x1, x2, x3 }; -// -// using namespace vanillaPose; -// -// // Two slightly different cameras -// Pose3 pose2 = level_pose -// * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); -// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); -// Camera cam2(pose2, sharedK); -// Camera cam3(pose3, sharedK); -// -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartProjectionParams params; -// params.setRankTolerance(10); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default -// smartFactor2->add(measurements_cam2, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default -// smartFactor3->add(measurements_cam3, views, sharedKs); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// // initialize third pose with some noise, we expect it to move back to original pose_above -// values.insert(x3, pose3 * noise_pose); -// EXPECT( -// assert_equal( -// Pose3( -// Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, -// -0.130426831, -0.0115837907, 0.130819108, -0.98278564, -// -0.130455917), -// Point3(0.0897734171, -0.110201006, 0.901022872)), -// values.at(x3))); -// -// boost::shared_ptr factor1 = smartFactor1->linearize(values); -// boost::shared_ptr factor2 = smartFactor2->linearize(values); -// boost::shared_ptr factor3 = smartFactor3->linearize(values); -// -// Matrix CumulativeInformation = factor1->information() + factor2->information() -// + factor3->information(); -// -// boost::shared_ptr GaussianGraph = graph.linearize( -// values); -// Matrix GraphInformation = GaussianGraph->hessian().first; -// -// // Check Hessian -// EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); -// -// Matrix AugInformationMatrix = factor1->augmentedInformation() -// + factor2->augmentedInformation() + factor3->augmentedInformation(); -// -// // Check Information vector -// Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector -// -// // Check Hessian -// EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, Hessian ) { -// -// using namespace vanillaPose2; -// -// KeyVector views { x1, x2 }; -// -// // Project three landmarks into 2 cameras -// Point2 cam1_uv1 = cam1.project(landmark1); -// Point2 cam2_uv1 = cam2.project(landmark1); -// Point2Vector measurements_cam1; -// measurements_cam1.push_back(cam1_uv1); -// measurements_cam1.push_back(cam2_uv1); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; -// sharedK2s.push_back(sharedK2); -// sharedK2s.push_back(sharedK2); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedK2s); -// -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), -// Point3(0.5, 0.1, 0.3)); -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// -// boost::shared_ptr factor = smartFactor1->linearize(values); -// -// // compute triangulation from linearization point -// // compute reprojection errors (sum squared) -// // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) -// // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] -//} -// +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, landmarkDistance ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 2; + + KeyVector views { x1, x2, x3 }; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0}; + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + KeyVector views { x1, x2, x3 }; + + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; + + // Project 4 landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + + SmartProjectionParams params; + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0}; + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, cameraRig, params)); + smartFactor4->add(measurements_cam4, views, cameraIds); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, CheckHessian) { + + KeyVector views { x1, x2, x3 }; + + using namespace vanillaPose; + + // Two slightly different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0}; + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views, cameraIds); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); + + boost::shared_ptr factor1 = smartFactor1->linearize(values); + boost::shared_ptr factor2 = smartFactor2->linearize(values); + boost::shared_ptr factor3 = smartFactor3->linearize(values); + + Matrix CumulativeInformation = factor1->information() + factor2->information() + + factor3->information(); + + boost::shared_ptr GaussianGraph = graph.linearize( + values); + Matrix GraphInformation = GaussianGraph->hessian().first; + + // Check Hessian + EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); + + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + factor3->augmentedInformation(); + + // Check Information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector + + // Check Hessian + EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, Hessian ) { + + using namespace vanillaPose2; + + KeyVector views { x1, x2 }; + + // Project three landmarks into 2 cameras + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2Vector measurements_cam1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); + FastVector cameraIds { 0, 0 }; + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + boost::shared_ptr factor = smartFactor1->linearize(values); + + // compute triangulation from linearization point + // compute reprojection errors (sum squared) + // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) + // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] +} + ///* ************************************************************************* */ //TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { // using namespace bundlerPose; From 3758fdaa5d7e92d5b45f3b9defe075afce6d46d9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 23:10:05 -0400 Subject: [PATCH 071/169] all tests work except serialization --- gtsam/slam/SmartProjectionRigFactor.h | 25 +- gtsam/slam/tests/smartFactorScenarios.h | 8 +- .../tests/testSmartProjectionRigFactor.cpp | 754 +++++++++--------- .../SmartProjectionPoseFactorRollingShutter.h | 6 +- 4 files changed, 395 insertions(+), 398 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 2ec2ed86f..581859b1f 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -38,7 +38,7 @@ namespace gtsam { /** * This factor assumes that camera calibration is fixed (but each camera * measurement can have a different extrinsic and intrinsic calibration). - * The factor only constrains poses (variable dimension is 6). + * The factor only constrains poses (variable dimension is 6 for each pose). * This factor requires that values contains the involved poses (Pose3). * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! * If the calibration should be optimized, as well, use SmartProjectionFactor instead! @@ -60,7 +60,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// vector of keys (one for each observation) with potentially repeated keys KeyVector nonUniqueKeys_; - /// cameras in the rig (fixed poses wrt body + fixed intrinsics) + /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation), identifying which camera took the measurement @@ -185,13 +185,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; + cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Key cameraId = cameraIds_[i]; - const Pose3& body_P_cam_i = cameraRig_[cameraId].pose(); const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) - * body_P_cam_i; + * cameraRig_[ cameraIds_[i] ].pose(); // cameraRig_[ cameraIds_[i] ].pose() is body_P_cam_i cameras.emplace_back(world_P_sensor_i, - make_shared(cameraRig_[cameraId].calibration())); + make_shared(cameraRig_[ cameraIds_[i] ].calibration())); } return cameras; } @@ -223,10 +222,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Key cameraId = cameraIds_[i]; - const Pose3 body_P_sensor = cameraRig_[cameraId].pose(); - const Pose3 sensor_P_body = body_P_sensor.inverse(); - const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; + const Pose3& body_P_sensor = cameraRig_[ cameraIds_[i] ].pose(); + const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); Eigen::Matrix H; world_P_body.compose(body_P_sensor, H); Fs.at(i) = Fs.at(i) * H; @@ -246,11 +243,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Cameras cameras = this->cameras(values); // Create structures for Hessian Factors - FastVector js; - FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - FastVector < Vector > gs(nrUniqueKeys); + std::vector js; + std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector < Vector > gs(nrUniqueKeys); - if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera + if (this->measured_.size() != cameras.size()) // 1 observation per camera throw std::runtime_error("SmartProjectionRigFactor: " "measured_.size() inconsistent with input"); diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index a9d0c43f7..b17ffdac6 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -71,7 +71,7 @@ namespace vanillaPose { typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Camera level_camera(level_pose, sharedK); Camera level_camera_right(pose_right, sharedK); @@ -86,7 +86,7 @@ namespace vanillaPose2 { typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); Camera level_camera(level_pose, sharedK2); Camera level_camera_right(pose_right, sharedK2); @@ -99,6 +99,7 @@ Camera cam3(pose_above, sharedK2); // Cal3Bundler cameras namespace bundler { typedef PinholeCamera Camera; +typedef CameraSet Cameras; typedef SmartProjectionFactor SmartFactor; static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); Camera level_camera(level_pose, K); @@ -115,8 +116,9 @@ typedef GeneralSFMFactor SFMFactor; // Cal3Bundler poses namespace bundlerPose { typedef PinholePose Camera; +typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 4c6b33655..db077525b 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -60,7 +60,7 @@ TEST( SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; Cameras cameraRig; cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); } /* ************************************************************************* */ @@ -69,7 +69,7 @@ TEST( SmartProjectionRigFactor, Constructor2) { Cameras cameraRig; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorP factor1(model, cameraRig, params); + SmartRigFactor factor1(model, cameraRig, params); } /* ************************************************************************* */ @@ -77,7 +77,7 @@ TEST( SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; Cameras cameraRig; cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); } @@ -88,7 +88,7 @@ TEST( SmartProjectionRigFactor, Constructor4) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorP factor1(model, cameraRig, params); + SmartRigFactor factor1(model, cameraRig, params); factor1.add(measurement1, x1, cameraId1); } @@ -98,10 +98,10 @@ TEST( SmartProjectionRigFactor, Equals ) { Cameras cameraRig; // single camera in the rig cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); - SmartFactorP::shared_ptr factor2(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); @@ -119,7 +119,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartFactorP factor(model, cameraRig); + SmartRigFactor factor(model, cameraRig); factor.add(level_uv, x1, cameraId1); // both taken from the same camera factor.add(level_uv_right, x2, cameraId1); @@ -131,13 +131,13 @@ TEST( SmartProjectionRigFactor, noiseless ) { double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactorP::Cameras cameras = factor.cameras(values); + SmartRigFactor::Cameras cameras = factor.cameras(values); double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // Calculate expected derivative for point (easiest to check) std::function f = // - std::bind(&SmartFactorP::whitenedError, factor, cameras, + std::bind(&SmartRigFactor::whitenedError, factor, cameras, std::placeholders::_1); // Calculate using computeEP @@ -153,7 +153,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { EXPECT(assert_equal(expectedE, actualE, 1e-7)); // Calculate using reprojectionError - SmartFactorP::Cameras::FBlocks F; + SmartRigFactor::Cameras::FBlocks F; Matrix E; Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); EXPECT(assert_equal(expectedE, E, 1e-7)); @@ -162,7 +162,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { // Calculate using computeJacobians Vector b; - SmartFactorP::FBlocks Fs; + SmartRigFactor::FBlocks Fs; factor.computeJacobians(Fs, E, b, cameras, *point); double actualError3 = b.squaredNorm(); EXPECT(assert_equal(expectedE, E, 1e-7)); @@ -188,14 +188,14 @@ TEST( SmartProjectionRigFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartFactorP::shared_ptr factor(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr factor(new SmartRigFactor(model,cameraRig)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartFactorP::shared_ptr factor2(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model,cameraRig)); Point2Vector measurements; measurements.push_back(level_uv); @@ -244,13 +244,13 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setDegeneracyMode(IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartFactorP smartFactor1(model, cameraRig, params); + SmartRigFactor smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_cam1, views, cameraIds); - SmartFactorP smartFactor2(model, cameraRig, params); + SmartRigFactor smartFactor2(model, cameraRig, params); smartFactor2.add(measurements_cam2, views, cameraIds); - SmartFactorP smartFactor3(model, cameraRig, params); + SmartRigFactor smartFactor3(model, cameraRig, params); smartFactor3.add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -305,13 +305,13 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { KeyVector views {x1,x2,x3}; FastVector cameraIds{0,0,0};// 3 measurements from the same camera in the rig - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model,cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model,cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -378,11 +378,11 @@ TEST( SmartProjectionRigFactor, Factors ) { KeyVector views { x1, x2 }; FastVector cameraIds { 0, 0 }; - SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP + SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor > (model,cameraRig); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::Cameras cameras; + SmartRigFactor::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); @@ -465,13 +465,13 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { Cameras cameraRig; // single camera in the rig cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -533,13 +533,13 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -599,16 +599,16 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); - SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(model, cameraRig, params)); smartFactor4->add(measurements_cam4, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -662,13 +662,13 @@ TEST( SmartProjectionRigFactor, CheckHessian) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, cameraIds); NonlinearFactorGraph graph; @@ -735,7 +735,7 @@ TEST( SmartProjectionRigFactor, Hessian ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); FastVector cameraIds { 0, 0 }; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), @@ -752,347 +752,340 @@ TEST( SmartProjectionRigFactor, Hessian ) { // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { -// using namespace bundlerPose; -// SmartProjectionParams params; -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// SmartFactorP factor(model, params); -// factor.add(measurement1, x1, sharedBundlerK); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, Cal3Bundler ) { -// -// using namespace bundlerPose; -// -// // three landmarks ~5 meters in front of camera -// Point3 landmark3(3, 0, 3.0); -// -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// -// KeyVector views { x1, x2, x3 }; -// -// std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; -// sharedBundlerKs.push_back(sharedBundlerK); -// sharedBundlerKs.push_back(sharedBundlerK); -// sharedBundlerKs.push_back(sharedBundlerK); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedBundlerKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_cam2, views, sharedBundlerKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_cam3, views, sharedBundlerKs); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, cam1.pose(), noisePrior); -// graph.addPrior(x2, cam2.pose(), noisePrior); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// // initialize third pose with some noise, we expect it to move back to original pose_above -// values.insert(x3, pose_above * noise_pose); -// EXPECT( -// assert_equal( -// Pose3( -// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), -// Point3(0.1, -0.1, 1.9)), -// values.at(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); -//} -// -//#include -//typedef GenericProjectionFactor TestProjectionFactor; -//static Symbol l0('L', 0); -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { -// // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark -// // at a single pose, a setup that occurs in multi-camera systems -// -// using namespace vanillaPose; -// Point2Vector measurements_lmk1; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// -// // create redundant measurements: -// Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; -// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement -// -// // create inputs -// std::vector keys; -// keys.push_back(x1); -// keys.push_back(x2); -// keys.push_back(x3); -// keys.push_back(x1); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); -// -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise to get a nontrivial linearization point -// values.insert(x3, pose_above * noise_pose); -// EXPECT( // check that the pose is actually noisy -// assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); -// -// // linearization point for the poses -// Pose3 pose1 = level_pose; -// Pose3 pose2 = pose_right; -// Pose3 pose3 = pose_above * noise_pose; -// -// // ==== check Hessian of smartFactor1 ===== -// // -- compute actual Hessian -// boost::shared_ptr linearfactor1 = smartFactor1->linearize( -// values); -// Matrix actualHessian = linearfactor1->information(); -// -// // -- compute expected Hessian from manual Schur complement from Jacobians -// // linearization point for the 3D point -// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); -// TriangulationResult point = smartFactor1->point(); -// EXPECT(point.valid()); // check triangulated point is valid -// -// // Use standard ProjectionFactor factor to calculate the Jacobians -// Matrix F = Matrix::Zero(2 * 4, 6 * 3); -// Matrix E = Matrix::Zero(2 * 4, 3); -// Vector b = Vector::Zero(2 * 4); -// -// // create projection factors rolling shutter -// TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, -// sharedK); -// Matrix HPoseActual, HEActual; -// // note: b is minus the reprojection error, cf the smart factor jacobian computation -// b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(0, 0) = HPoseActual; -// E.block<2, 3>(0, 0) = HEActual; -// -// TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, -// sharedK); -// b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(2, 6) = HPoseActual; -// E.block<2, 3>(2, 0) = HEActual; -// -// TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, -// sharedK); -// b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(4, 12) = HPoseActual; -// E.block<2, 3>(4, 0) = HEActual; -// -// TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, -// sharedK); -// b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(6, 0) = HPoseActual; -// E.block<2, 3>(6, 0) = HEActual; -// -// // whiten -// F = (1 / sigma) * F; -// E = (1 / sigma) * E; -// b = (1 / sigma) * b; -// //* G = F' * F - F' * E * P * E' * F -// Matrix P = (E.transpose() * E).inverse(); -// Matrix expectedHessian = F.transpose() * F -// - (F.transpose() * E * P * E.transpose() * F); -// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); -// -// // ==== check Information vector of smartFactor1 ===== -// GaussianFactorGraph gfg; -// gfg.add(linearfactor1); -// Matrix actualHessian_v2 = gfg.hessian().first; -// EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian -// -// // -- compute actual information vector -// Vector actualInfoVector = gfg.hessian().second; -// -// // -- compute expected information vector from manual Schur complement from Jacobians -// //* g = F' * (b - E * P * E' * b) -// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); -// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); -// -// // ==== check error of smartFactor1 (again) ===== -// NonlinearFactorGraph nfg_projFactors; -// nfg_projFactors.add(factor11); -// nfg_projFactors.add(factor12); -// nfg_projFactors.add(factor13); -// nfg_projFactors.add(factor14); -// values.insert(l0, *point); -// -// double actualError = smartFactor1->error(values); -// double expectedError = nfg_projFactors.error(values); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { -// -// using namespace vanillaPose; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// std::vector keys; -// keys.push_back(x1); -// keys.push_back(x2); -// keys.push_back(x3); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// // For first factor, we create redundant measurement (taken by the same keys as factor 1, to -// // make sure the redundancy in the keys does not create problems) -// Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; -// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement -// std::vector keys_redundant = keys; -// keys_redundant.push_back(keys.at(0)); // we readd the first key -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; -// sharedKs_redundant.push_back(sharedK);// we readd the first calibration -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_lmk2, keys, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_lmk3, keys, sharedKs); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Values groundTruth; -// groundTruth.insert(x1, level_pose); -// groundTruth.insert(x2, pose_right); -// groundTruth.insert(x3, pose_above); -// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to original pose_above -// values.insert(x3, pose_above * noise_pose); -// EXPECT( // check that the pose is actually noisy -// assert_equal( -// Pose3( -// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), -// Point3(0.1, -0.1, 1.9)), values.at(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); -//} -// -//#ifndef DISABLE_TIMING -//#include -//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor -////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) -////| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) -////| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, timing ) { -// -// using namespace vanillaPose; -// -// // Default cameras for simple derivatives -// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); -// -// Rot3 R = Rot3::identity(); -// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); -// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); -// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// // one landmarks 1m in front of camera -// Point3 landmark1(0, 0, 10); -// -// Point2Vector measurements_lmk1; -// -// // Project 2 landmarks into 2 cameras -// measurements_lmk1.push_back(cam1.project(landmark1)); -// measurements_lmk1.push_back(cam2.project(landmark1)); -// -// size_t nrTests = 1000; -// -// for(size_t i = 0; iadd(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId); -// smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(SmartFactorP_LINEARIZE); -// smartFactorP->linearize(values); -// gttoc_(SmartFactorP_LINEARIZE); -// } -// -// for(size_t i = 0; iadd(measurements_lmk1[0], x1); -// smartFactor->add(measurements_lmk1[1], x2); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(SmartPoseFactor_LINEARIZE); -// smartFactor->linearize(values); -// gttoc_(SmartPoseFactor_LINEARIZE); -// } -// tictoc_print_(); -//} -//#endif -// +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { + using namespace bundlerPose; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartRigFactor factor(model, cameraRig, params); + factor.add(measurement1, x1, cameraId1); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, Cal3Bundler ) { + + using namespace bundlerPose; + + // three landmarks ~5 meters in front of camera + Point3 landmark3(3, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + KeyVector views { x1, x2, x3 }; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + FastVector cameraIds { 0, 0, 0 }; + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); +} + +#include +typedef GenericProjectionFactor TestProjectionFactor; +static Symbol l0('L', 0); +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark + // at a single pose, a setup that occurs in multi-camera systems + + using namespace vanillaPose; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create redundant measurements: + Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + + // create inputs + std::vector keys { x1, x2, x3, x1}; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0, 0 }; + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = smartFactor1->linearize( + values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use standard ProjectionFactor factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(2 * 4); + + // create projection factors rolling shutter + TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, + sharedK); + Matrix HPoseActual, HEActual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(0, 0) = HPoseActual; + E.block<2, 3>(0, 0) = HEActual; + + TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, + HEActual); + F.block<2, 6>(2, 6) = HPoseActual; + E.block<2, 3>(2, 0) = HEActual; + + TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, + HEActual); + F.block<2, 6>(4, 12) = HPoseActual; + E.block<2, 3>(4, 0) = HEActual; + + TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(6, 0) = HPoseActual; + E.block<2, 3>(6, 0) = HEActual; + + // whiten + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = F.transpose() * F + - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactors; + nfg_projFactors.add(factor11); + nfg_projFactors.add(factor12); + nfg_projFactors.add(factor13); + nfg_projFactors.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactors.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { + + using namespace vanillaPose; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector keys { x1, x2, x3 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0 }; + FastVector cameraIdsRedundant { 0, 0, 0, 0 }; + + // For first factor, we create redundant measurement (taken by the same keys as factor 1, to + // make sure the redundancy in the keys does not create problems) + Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + std::vector keys_redundant = keys; + keys_redundant.push_back(keys.at(0)); // we readd the first key + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); + + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + smartFactor2->add(measurements_lmk2, keys, cameraIds); + + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + smartFactor3->add(measurements_lmk3, keys, cameraIds); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +} + +#ifndef DISABLE_TIMING +#include +// this factor is slightly slower (but comparable) to original SmartProjectionPoseFactor +//-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 children, min: 0 max: 0) +//| -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 times, 0.065103 wall, 0.06 children, min: 0 max: 0) +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, timing ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(body_P_sensorId, sharedKSimple) ); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + size_t nrTests = 10000; + + for(size_t i = 0; iadd(measurements_lmk1[0], x1, cameraId1); + smartFactorP->add(measurements_lmk1[1], x1, cameraId1); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SmartRigFactor_LINEARIZE); + smartFactorP->linearize(values); + gttoc_(SmartRigFactor_LINEARIZE); + } + + for(size_t i = 0; iadd(measurements_lmk1[0], x1); + smartFactor->add(measurements_lmk1[1], x2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SmartPoseFactor_LINEARIZE); + smartFactor->linearize(values); + gttoc_(SmartPoseFactor_LINEARIZE); + } + tictoc_print_(); +} +#endif + ///* ************************************************************************* */ //BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); //BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); @@ -1107,7 +1100,11 @@ TEST( SmartProjectionRigFactor, Hessian ) { // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); -// SmartFactorP factor(model, params); +// +// Cameras cameraRig; // single camera in the rig +// cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); +// +// SmartRigFactor factor(model, cameraRig, params); // // EXPECT(equalsObj(factor)); // EXPECT(equalsXML(factor)); @@ -1120,14 +1117,13 @@ TEST( SmartProjectionRigFactor, Hessian ) { //// using namespace gtsam::serializationTestHelpers; //// SmartProjectionParams params; //// params.setRankTolerance(rankTol); -//// SmartFactorP factor(model, params); +//// SmartRigFactor factor(model, params); //// //// // insert some measurements //// KeyVector key_view; //// Point2Vector meas_view; //// std::vector> sharedKs; //// -//// //// key_view.push_back(Symbol('x', 1)); //// meas_view.push_back(Point2(10, 10)); //// sharedKs.push_back(sharedK); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 7660ff236..d58aea148 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -341,19 +341,21 @@ class SmartProjectionPoseFactorRollingShutter this->keys_ .size(); // note: by construction, keys_ only contains unique keys + typename Base::Cameras cameras = this->cameras(values); + // Create structures for Hessian Factors KeyVector js; std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector gs(nrUniqueKeys); if (this->measured_.size() != - this->cameras(values).size()) // 1 observation per interpolated camera + cameras.size()) // 1 observation per interpolated camera throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point - this->triangulateSafe(this->cameras(values)); + this->triangulateSafe(cameras); if (!this->result_) { // failed: return "empty/zero" Hessian if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { From 4a88574f6749063ed917ae3c86c38080b98f5303 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 23:16:28 -0400 Subject: [PATCH 072/169] added extra test with multi cam --- .../tests/testSmartProjectionRigFactor.cpp | 87 +++++++++++++++++++ 1 file changed, 87 insertions(+) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index db077525b..fb3f3c461 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -288,6 +288,93 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { EXPECT(assert_equal(wTb3, result.at(x3))); } +/* *************************************************************************/ +TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { + using namespace vanillaPose; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); + Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), + Point3(0, 0, 1)); + Pose3 body_T_sensor3 = Pose3::identity(); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(body_T_sensor1, sharedK) ); + cameraRig.push_back( Camera(body_T_sensor2, sharedK) ); + cameraRig.push_back( Camera(body_T_sensor3, sharedK) ); + + // These are the poses we want to estimate, from camera measurements + const Pose3 sensor_T_body1 = body_T_sensor1.inverse(); + const Pose3 sensor_T_body2 = body_T_sensor2.inverse(); + const Pose3 sensor_T_body3 = body_T_sensor3.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body1; + Pose3 wTb2 = cam2.pose() * sensor_T_body2; + Pose3 wTb3 = cam3.pose() * sensor_T_body3; + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + KeyVector views { x1, x2, x3 }; + FastVector cameraIds { 0, 1, 2 }; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + SmartRigFactor smartFactor1(model, cameraRig, params); + smartFactor1.add(measurements_cam1, views, cameraIds); + + SmartRigFactor smartFactor2(model, cameraRig, params); + smartFactor2.add(measurements_cam2, views, cameraIds); + + SmartRigFactor smartFactor3(model, cameraRig, params); + smartFactor3.add(measurements_cam3, views, cameraIds); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, wTb1, noisePrior); + graph.addPrior(x2, wTb2, noisePrior); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, wTb1); + gtValues.insert(x2, wTb2); + gtValues.insert(x3, wTb3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); + Values values; + values.insert(x1, wTb1); + values.insert(x2, wTb2); + // initialize third pose with some noise, we expect it to move back to + // original pose3 + values.insert(x3, wTb3 * noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(wTb3, result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { From 658effa99958c63843493e3e516d0ce3e898f271 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 02:16:47 -0400 Subject: [PATCH 073/169] OptionalJacobian fixed constructor with dynamic pointer --- gtsam/base/OptionalJacobian.h | 7 +++ gtsam/base/tests/testOptionalJacobian.cpp | 64 ++++++++++++++--------- 2 files changed, 45 insertions(+), 26 deletions(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 4b580f82e..07801df7a 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -89,6 +89,13 @@ public: usurp(dynamic.data()); } + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd* dynamic) : + map_(nullptr) { + dynamic->resize(Rows, Cols); // no malloc if correct size + usurp(dynamic->data()); + } + #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 128576107..ae91642f4 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -24,40 +24,33 @@ using namespace std; using namespace gtsam; //****************************************************************************** +#define TEST_CONSTRUCTOR(DIM1, DIM2, X, TRUTHY) \ + { \ + OptionalJacobian H(X); \ + EXPECT(H == TRUTHY); \ + } TEST( OptionalJacobian, Constructors ) { Matrix23 fixed; - - OptionalJacobian<2, 3> H1; - EXPECT(!H1); - - OptionalJacobian<2, 3> H2(fixed); - EXPECT(H2); - - OptionalJacobian<2, 3> H3(&fixed); - EXPECT(H3); - Matrix dynamic; - OptionalJacobian<2, 3> H4(dynamic); - EXPECT(H4); - - OptionalJacobian<2, 3> H5(boost::none); - EXPECT(!H5); - boost::optional optional(dynamic); - OptionalJacobian<2, 3> H6(optional); - EXPECT(H6); + OptionalJacobian<2, 3> H; + EXPECT(!H); + + TEST_CONSTRUCTOR(2, 3, fixed, true); + TEST_CONSTRUCTOR(2, 3, &fixed, true); + TEST_CONSTRUCTOR(2, 3, dynamic, true); + TEST_CONSTRUCTOR(2, 3, &dynamic, true); + TEST_CONSTRUCTOR(2, 3, boost::none, false); + TEST_CONSTRUCTOR(2, 3, optional, true); + + // Test dynamic OptionalJacobian<-1, -1> H7; EXPECT(!H7); - OptionalJacobian<-1, -1> H8(dynamic); - EXPECT(H8); - - OptionalJacobian<-1, -1> H9(boost::none); - EXPECT(!H9); - - OptionalJacobian<-1, -1> H10(optional); - EXPECT(H10); + TEST_CONSTRUCTOR(-1, -1, dynamic, true); + TEST_CONSTRUCTOR(-1, -1, boost::none, false); + TEST_CONSTRUCTOR(-1, -1, optional, true); } //****************************************************************************** @@ -101,6 +94,25 @@ TEST( OptionalJacobian, Fixed) { dynamic2.setOnes(); test(dynamic2); EXPECT(assert_equal(kTestMatrix, dynamic2)); + + { // Dynamic pointer + // Passing in an empty matrix means we want it resized + Matrix dynamic0; + test(&dynamic0); + EXPECT(assert_equal(kTestMatrix, dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test(&dynamic1); + EXPECT(assert_equal(kTestMatrix, dynamic1)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test(&dynamic2); + EXPECT(assert_equal(kTestMatrix, dynamic2)); + } } //****************************************************************************** From fe59f3db19a0b56a18d560e482c48890bfbd8258 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 03:56:25 -0400 Subject: [PATCH 074/169] AdjointMap jacobians for Pose3 --- gtsam/geometry/Pose3.cpp | 36 ++++++++++++++++++++++++++++++ gtsam/geometry/Pose3.h | 10 +++++---- gtsam/geometry/tests/testPose3.cpp | 30 +++++++++++++++++++++++++ 3 files changed, 72 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index c183e32ed..93d6248e6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -63,6 +63,42 @@ Matrix6 Pose3::AdjointMap() const { return adj; } +/* ************************************************************************* */ +// Calculate AdjointMap applied to xi_b, with Jacobians +Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, + OptionalJacobian<6, 6> H_xib) const { + // Ad * xi = [ R 0 * [w + // [t]R R ] v] + + // Declarations and aliases + Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw; + Vector6 result; + auto Rw = result.head<3>(); + const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); + + // Calculations + Rw = R_.rotate(w, Rw_H_R, Rw_H_w); + const Vector3 Rv = R_.rotate(v, Rv_H_R, Rv_H_v); + const Vector3 pRw = cross(t_, Rw, pRw_H_t, pRw_H_Rw); + result.tail<3>() = pRw + Rv; + pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason + + // Jacobians + if (H_this) { + *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // + /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) + .finished(); + } + if (H_xib) { + *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // + /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) + .finished(); + } + + // Return + return result; +} + /* ************************************************************************* */ Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d76e1b41a..9a8a7e939 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -148,12 +148,14 @@ public: Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect /** - * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a + * body-fixed velocity, transforming it to the spatial frame * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + * Note that H_xib = AdjointMap() */ - Vector6 Adjoint(const Vector6& xi_b) const { - return AdjointMap() * xi_b; - } /// FIXME Not tested - marked as incorrect + Vector6 Adjoint(const Vector6& xi_b, + OptionalJacobian<6, 6> H_this = boost::none, + OptionalJacobian<6, 6> H_xib = boost::none) const; /** * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 7c1fa81e6..9fde4c2c1 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -145,6 +145,36 @@ TEST(Pose3, Adjoint_full) EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } +/* ************************************************************************* */ +// Check Adjoint numerical derivatives +TEST(Pose3, Adjoint_jacobians) +{ + Matrix6 actualH1, actualH2, expectedH1, expectedH2; + std::function Adjoint_proxy = + [](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); }; + + gtsam::Vector6 xi = + (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + T.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T2.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T2, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T2, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T3.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T3, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T3, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) TEST(Pose3, Adjoint_hat) From bcafdb75573bcaff421be29fd104a4cd911ab6ae Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 04:04:28 -0400 Subject: [PATCH 075/169] only compute jacobians when needed --- gtsam/geometry/Pose3.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 93d6248e6..2f4fa4da3 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -67,9 +67,8 @@ Matrix6 Pose3::AdjointMap() const { // Calculate AdjointMap applied to xi_b, with Jacobians Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, OptionalJacobian<6, 6> H_xib) const { - // Ad * xi = [ R 0 * [w - // [t]R R ] v] - + // Ad * xi = [ R 0 . [w + // [t]R R ] v] // Declarations and aliases Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw; Vector6 result; @@ -77,14 +76,16 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); // Calculations - Rw = R_.rotate(w, Rw_H_R, Rw_H_w); - const Vector3 Rv = R_.rotate(v, Rv_H_R, Rv_H_v); - const Vector3 pRw = cross(t_, Rw, pRw_H_t, pRw_H_Rw); + Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); + const Vector3 Rv = + R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); + const Vector3 pRw = cross(t_, Rw, H_this ? &pRw_H_t : nullptr, + H_this || H_xib ? &pRw_H_Rw : nullptr); result.tail<3>() = pRw + Rv; - pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason // Jacobians if (H_this) { + pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) .finished(); From ecf53a68543248ac0ca210654759349941f6973d Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 04:58:16 -0400 Subject: [PATCH 076/169] AdjointTranspose --- gtsam/geometry/Pose3.cpp | 36 ++++++++++++++++++++++++++ gtsam/geometry/Pose3.h | 5 ++++ gtsam/geometry/tests/testPose3.cpp | 41 +++++++++++++++++++++++++++++- 3 files changed, 81 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2f4fa4da3..daa889c0d 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -100,6 +100,42 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, return result; } +/* ************************************************************************* */ +/// The dual version of Adjoint +Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, + OptionalJacobian<6, 6> H_x) const { + // Ad^T * xi = [ R^T R^T.[-t] . [w + // 0 R^T ] v] + // Declarations and aliases + Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, tv_H_t, tv_H_v, Rtv_H_R, Rtv_H_tv; + Vector6 result; + const Vector3 &w = x.head<3>(), &v = x.tail<3>(); + auto Rv = result.tail<3>(); + + // Calculations + const Vector3 Rw = + R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr); + Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr); + const Vector3 tv = cross(t_, v, tv_H_t, tv_H_v); + const Vector3 Rtv = R_.unrotate(tv, Rtv_H_R, Rtv_H_tv); + result.head<3>() = Rw - Rtv; + + // Jacobians + if (H_this) { + *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rv_H_R, // -Rtv_H_tv * tv_H_t + /* */ Rv_H_R, /* */ Z_3x3) + .finished(); + } + if (H_x) { + *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // + /* */ Z_3x3, Rv_H_v) + .finished(); + } + + // Return + return result; +} + /* ************************************************************************* */ Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 9a8a7e939..cff5fd231 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -156,6 +156,11 @@ public: Vector6 Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this = boost::none, OptionalJacobian<6, 6> H_xib = boost::none) const; + + /// The dual version of Adjoint + Vector6 AdjointTranspose(const Vector6& x, + OptionalJacobian<6, 6> H_this = boost::none, + OptionalJacobian<6, 6> H_x = boost::none) const; /** * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9fde4c2c1..722087934 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -151,7 +151,11 @@ TEST(Pose3, Adjoint_jacobians) { Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function Adjoint_proxy = - [](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); }; + [&](const Pose3& T, const Vector6& xi) { + Vector6 res1 = T.AdjointMap() * xi, res2 = T.Adjoint(xi); + EXPECT(assert_equal(res1, res2)); + return res1; + }; gtsam::Vector6 xi = (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); @@ -175,6 +179,41 @@ TEST(Pose3, Adjoint_jacobians) EXPECT(assert_equal(expectedH2, actualH2)); } +/* ************************************************************************* */ +// Check AdjointTranspose and jacobians +TEST(Pose3, AdjointTranspose) +{ + Matrix6 actualH1, actualH2, expectedH1, expectedH2; + std::function AdjointTranspose_proxy = + [&](const Pose3& T, const Vector6& xi) { + Vector6 res1 = T.AdjointMap().transpose() * xi, + res2 = T.AdjointTranspose(xi); + EXPECT(assert_equal(res1, res2)); + return res1; + }; + + gtsam::Vector6 xi = + (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + T.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T2.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T3.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) TEST(Pose3, Adjoint_hat) From ab1b926dcd92c49004b293f79bf84762b093fb18 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 05:03:00 -0400 Subject: [PATCH 077/169] only compute intermediate jacobians when needed --- gtsam/geometry/Pose3.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index daa889c0d..27a5cf557 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -80,7 +80,7 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); const Vector3 pRw = cross(t_, Rw, H_this ? &pRw_H_t : nullptr, - H_this || H_xib ? &pRw_H_Rw : nullptr); + (H_this || H_xib) ? &pRw_H_Rw : nullptr); result.tail<3>() = pRw + Rv; // Jacobians @@ -116,8 +116,10 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, const Vector3 Rw = R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr); Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr); - const Vector3 tv = cross(t_, v, tv_H_t, tv_H_v); - const Vector3 Rtv = R_.unrotate(tv, Rtv_H_R, Rtv_H_tv); + const Vector3 tv = + cross(t_, v, H_this ? &tv_H_t : nullptr, H_x ? &tv_H_v : nullptr); + const Vector3 Rtv = R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr, + (H_this || H_x) ? &Rtv_H_tv : nullptr); result.head<3>() = Rw - Rtv; // Jacobians From f7fed8b5f50218a289a97e65b104465e1f1aca7b Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 11:19:19 -0400 Subject: [PATCH 078/169] review comment: move check outside lambda --- gtsam/geometry/tests/testPose3.cpp | 36 +++++++++++++++++------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 722087934..fe527ab2e 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -149,16 +149,17 @@ TEST(Pose3, Adjoint_full) // Check Adjoint numerical derivatives TEST(Pose3, Adjoint_jacobians) { + Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + // Check evaluation sanity check + EQUALITY(static_cast(T.AdjointMap() * xi), T.Adjoint(xi)); + EQUALITY(static_cast(T2.AdjointMap() * xi), T2.Adjoint(xi)); + EQUALITY(static_cast(T3.AdjointMap() * xi), T3.Adjoint(xi)); + + // Check jacobians Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function Adjoint_proxy = - [&](const Pose3& T, const Vector6& xi) { - Vector6 res1 = T.AdjointMap() * xi, res2 = T.Adjoint(xi); - EXPECT(assert_equal(res1, res2)); - return res1; - }; - - gtsam::Vector6 xi = - (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + [&](const Pose3& T, const Vector6& xi) { return T.AdjointMap() * xi; }; T.Adjoint(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi); @@ -183,18 +184,23 @@ TEST(Pose3, Adjoint_jacobians) // Check AdjointTranspose and jacobians TEST(Pose3, AdjointTranspose) { + Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + // Check evaluation + EQUALITY(static_cast(T.AdjointMap().transpose() * xi), + T.AdjointTranspose(xi)); + EQUALITY(static_cast(T2.AdjointMap().transpose() * xi), + T2.AdjointTranspose(xi)); + EQUALITY(static_cast(T3.AdjointMap().transpose() * xi), + T3.AdjointTranspose(xi)); + + // Check jacobians Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function AdjointTranspose_proxy = [&](const Pose3& T, const Vector6& xi) { - Vector6 res1 = T.AdjointMap().transpose() * xi, - res2 = T.AdjointTranspose(xi); - EXPECT(assert_equal(res1, res2)); - return res1; + return T.AdjointMap().transpose() * xi; }; - gtsam::Vector6 xi = - (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); - T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); From 222380ce485a06c179e1cba2d25598ecb079a8e3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 4 Oct 2021 20:29:40 -0400 Subject: [PATCH 079/169] added more comments here and there --- gtsam/slam/SmartProjectionRigFactor.h | 35 +++++++++++++++------------ 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 581859b1f..0246d6327 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -36,10 +36,10 @@ namespace gtsam { */ /** - * This factor assumes that camera calibration is fixed (but each camera - * measurement can have a different extrinsic and intrinsic calibration). - * The factor only constrains poses (variable dimension is 6 for each pose). - * This factor requires that values contains the involved poses (Pose3). + * This factor assumes that camera calibration is fixed (but each measurement + * can be taken by a different camera in the rig, hence can have a different + * extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension + * is 6 for each pose). This factor requires that values contains the involved poses (Pose3). * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! * If the calibration should be optimized, as well, use SmartProjectionFactor instead! * @addtogroup SLAM @@ -53,7 +53,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typedef typename CAMERA::CalibrationType CALIBRATION; static const int DimPose = 6; ///< Pose3 dimension - static const int ZDim = 2; ///< Measurement dimension (Point2) + static const int ZDim = 2; ///< Measurement dimension protected: @@ -63,7 +63,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation), identifying which camera took the measurement + /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement FastVector cameraIds_; public: @@ -84,10 +84,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param params parameters for the smart projection factors */ SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, - const Cameras& cameraRig, - const SmartProjectionParams& params = - SmartProjectionParams()) - : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { + const Cameras& cameraRig, + const SmartProjectionParams& params = + SmartProjectionParams()) + : Base(sharedNoiseModel, params), + cameraRig_(cameraRig) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.linearizationMode = gtsam::HESSIAN; @@ -123,7 +124,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param measurements vector of the 2m dimensional location of the projection * of a single landmark in the m views (the measurements) * @param poseKeys keys corresponding to the body poses of the cameras taking the measurements - * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as measurements) + * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements) */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, const FastVector& cameraIds) { @@ -185,12 +186,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; - cameras.reserve(nonUniqueKeys_.size()); // preallocate + cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) - * cameraRig_[ cameraIds_[i] ].pose(); // cameraRig_[ cameraIds_[i] ].pose() is body_P_cam_i - cameras.emplace_back(world_P_sensor_i, - make_shared(cameraRig_[ cameraIds_[i] ].calibration())); + const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) // = world_P_body + * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i + cameras.emplace_back( + world_P_sensor_i, + make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } From 7988a7050f26e448e94e5ca532251af01d4188dc Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 4 Oct 2021 21:33:26 -0400 Subject: [PATCH 080/169] changing API for rolling shutter --- .../SmartProjectionPoseFactorRollingShutter.h | 160 +- ...martProjectionPoseFactorRollingShutter.cpp | 2026 +++++++++-------- 2 files changed, 1092 insertions(+), 1094 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index d58aea148..cf1526673 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -11,8 +11,7 @@ /** * @file SmartProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses modeling rolling shutter effect with - * given readout time + * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time * @author Luca Carlone */ @@ -43,7 +42,10 @@ namespace gtsam { template class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { - public: + + private: + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactorRollingShutter This; typedef typename CAMERA::CalibrationType CALIBRATION; protected: @@ -58,24 +60,23 @@ class SmartProjectionPoseFactorRollingShutter /// pair of consecutive poses std::vector alphas_; - /// Pose of the camera in the body frame - std::vector body_P_sensors_; + /// one or more cameras in the rig (fixed poses wrt body + fixed intrinsics) + typename Base::Cameras cameraRig_; + + /// vector of camera Ids (one for each observation, in the same order), identifying which camera in the rig took the measurement + FastVector cameraIds_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /// shorthand for base class type - typedef SmartProjectionFactor> Base; - - /// shorthand for this class - typedef SmartProjectionPoseFactorRollingShutter This; + typedef CAMERA Camera; + typedef CameraSet Cameras; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int DimBlock = - 12; ///< size of the variable stacking 2 poses from which the observation - ///< pose is interpolated + static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation + ///< pose is interpolated static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 2; ///< Measurement dimension (Point2) typedef Eigen::Matrix @@ -89,9 +90,30 @@ class SmartProjectionPoseFactorRollingShutter * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( - const SharedNoiseModel& sharedNoiseModel, + const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) {} + : Base(sharedNoiseModel, params), + cameraRig_(cameraRig) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + } + + /** + * Constructor + * @param Isotropic measurement noise + * @param params internal parameters of the smart factors + */ + SmartProjectionPoseFactorRollingShutter( + const SharedNoiseModel& sharedNoiseModel, const Camera& camera, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + cameraRig_.push_back(camera); + } + /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; @@ -112,8 +134,7 @@ class SmartProjectionPoseFactorRollingShutter */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, - const boost::shared_ptr& K, - const Pose3& body_P_sensor = Pose3::identity()) { + const size_t cameraId = 0) { // store measurements in base class this->measured_.push_back(measured); @@ -133,11 +154,8 @@ class SmartProjectionPoseFactorRollingShutter // store interpolation factor alphas_.push_back(alpha); - // store fixed intrinsic calibration - K_all_.push_back(K); - - // store fixed extrinsics of the camera - body_P_sensors_.push_back(body_P_sensor); + // store id of the camera taking the measurement + cameraIds_.push_back(cameraId); } /** @@ -156,61 +174,39 @@ class SmartProjectionPoseFactorRollingShutter void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& alphas, - const std::vector>& Ks, - const std::vector& body_P_sensors) { - assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == alphas.size()); - assert(world_P_body_key_pairs.size() == Ks.size()); + const FastVector& cameraIds = FastVector()) { + + if (world_P_body_key_pairs.size() != measurements.size() + || world_P_body_key_pairs.size() != alphas.size() + || world_P_body_key_pairs.size() != cameraIds.size()) { + throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " + "trying to add inconsistent inputs"); + } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, alphas[i], Ks[i], - body_P_sensors[i]); + world_P_body_key_pairs[i].second, alphas[i], + cameraIds.size() == 0 ? 0 : cameraIds[i]); // use 0 as default if cameraIds was not specified } } - /** - * Variant of the previous "add" function in which we include multiple - * measurements with the same (intrinsic and extrinsic) calibration - * @param measurements vector of the 2m dimensional location of the projection - * of a single landmark in the m views (the measurements) - * @param world_P_body_key_pairs vector where the i-th element contains a pair - * of keys corresponding to the pair of poses from which the observation pose - * for the i0-th measurement can be interpolated - * @param alphas vector of interpolation params (in [0,1]), one for each - * measurement (in the same order) - * @param K (fixed) camera intrinsic calibration (same for all measurements) - * @param body_P_sensor (fixed) camera extrinsic calibration (same for all - * measurements) - */ - void add(const Point2Vector& measurements, - const std::vector>& world_P_body_key_pairs, - const std::vector& alphas, - const boost::shared_ptr& K, - const Pose3& body_P_sensor = Pose3::identity()) { - assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == alphas.size()); - for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor); - } - } - - /// return the calibration object - const std::vector>& calibration() const { - return K_all_; - } - /// return (for each observation) the keys of the pair of poses from which we /// interpolate - const std::vector>& world_P_body_key_pairs() const { + const std::vector> world_P_body_key_pairs() const { return world_P_body_key_pairs_; } /// return the interpolation factors alphas - const std::vector& alphas() const { return alphas_; } + const std::vector alphas() const { return alphas_; } - /// return the extrinsic camera calibration body_P_sensors - const std::vector& body_P_sensors() const { return body_P_sensors_; } + /// return the calibration object + inline Cameras cameraRig() const { + return cameraRig_; + } + + /// return the calibration object + inline FastVector cameraIds() const { + return cameraIds_; + } /** * print @@ -228,8 +224,8 @@ class SmartProjectionPoseFactorRollingShutter std::cout << " pose2 key: " << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; - body_P_sensors_[i].print("extrinsic calibration:\n"); - K_all_[i]->print("intrinsic calibration = "); + std::cout << "cameraId: " << cameraIds_[i] << std::endl; + cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -237,8 +233,7 @@ class SmartProjectionPoseFactorRollingShutter /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const SmartProjectionPoseFactorRollingShutter* e = - dynamic_cast*>( - &p); + dynamic_cast*>(&p); double keyPairsEqual = true; if (this->world_P_body_key_pairs_.size() == @@ -257,20 +252,9 @@ class SmartProjectionPoseFactorRollingShutter keyPairsEqual = false; } - double extrinsicCalibrationEqual = true; - if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { - for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { - if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { - extrinsicCalibrationEqual = false; - break; - } - } - } else { - extrinsicCalibrationEqual = false; - } - - return e && Base::equals(p, tol) && K_all_ == e->calibration() && - alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) && alphas_ == e->alphas() && keyPairsEqual + && cameraRig_.equals(e->cameraRig()) + && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } /** @@ -305,9 +289,9 @@ class SmartProjectionPoseFactorRollingShutter auto w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - auto body_P_cam = body_P_sensors_[i]; + auto body_P_cam = cameraRig_[cameraIds_[i]].pose(); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); - PinholeCamera camera(w_P_cam, *K_all_[i]); + PinholeCamera camera(w_P_cam, cameraRig_[cameraIds_[i]].calibration()); // get jacobians and error vector for current measurement Point2 reprojectionError_i = @@ -434,9 +418,10 @@ class SmartProjectionPoseFactorRollingShutter double interpolationFactor = alphas_[i]; const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); - const Pose3& body_P_cam = body_P_sensors_[i]; + const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, K_all_[i]); + cameras.emplace_back(w_P_cam, make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } @@ -474,7 +459,6 @@ class SmartProjectionPoseFactorRollingShutter template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_NVP(K_all_); } }; // end of class declaration diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 0b94d2c3f..6c28cbda5 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -61,10 +61,13 @@ static double interp_factor1 = 0.3; static double interp_factor2 = 0.4; static double interp_factor3 = 0.5; +static size_t cameraId1 = 0; + /* ************************************************************************* */ // default Cal3_S2 poses with rolling shutter effect namespace vanillaPoseRS { typedef PinholePose Camera; +typedef CameraSet Cameras; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); @@ -80,21 +83,23 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + using namespace vanillaPoseRS; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { + using namespace vanillaPoseRS; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorRS factor1(model, params); + SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { - using namespace vanillaPose; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); + using namespace vanillaPoseRS; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + factor1->add(measurement1, x1, x2, interp_factor); } /* ************************************************************************* */ @@ -112,1030 +117,1039 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { key_pairs.push_back(std::make_pair(x2, x3)); key_pairs.push_back(std::make_pair(x3, x4)); - std::vector> intrinsicCalibrations; - intrinsicCalibrations.push_back(sharedK); - intrinsicCalibrations.push_back(sharedK); - intrinsicCalibrations.push_back(sharedK); - - std::vector extrinsicCalibrations; - extrinsicCalibrations.push_back(body_P_sensor); - extrinsicCalibrations.push_back(body_P_sensor); - extrinsicCalibrations.push_back(body_P_sensor); - std::vector interp_factors; interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); + std::vector cameraIds{0, 0, 0}; + + Cameras cameraRig; + cameraRig.push_back( Camera(body_P_sensor, sharedK) ); + // create by adding a batch of measurements with a bunch of calibrations - SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); - factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, - extrinsicCalibrations); + SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); + factor2->add(measurements, key_pairs, interp_factors, cameraIds); // create by adding a batch of measurements with a single calibrations - SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); - factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); + SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); + factor3->add(measurements, key_pairs, interp_factors, cameraIds); - { // create equal factors and show equal returns true - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); - factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - - EXPECT(factor1->equals(*factor2)); - EXPECT(factor1->equals(*factor3)); - } - { // create slightly different factors (different keys) and show equal - // returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x2, interp_factor2, sharedK, - body_P_sensor); // different! - factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - - EXPECT(!factor1->equals(*factor2)); - EXPECT(!factor1->equals(*factor3)); - } +// { // create equal factors and show equal returns true +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); +// factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); +// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); +// +// EXPECT(factor1->equals(*factor2)); +// EXPECT(factor1->equals(*factor3)); +// } +// { // create equal factors and show equal returns true (use default cameraId) +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1); +// factor1->add(measurement2, x2, x3, interp_factor2); +// factor1->add(measurement3, x3, x4, interp_factor3); +// +// EXPECT(factor1->equals(*factor2)); +// EXPECT(factor1->equals(*factor3)); +// } +// { // create equal factors and show equal returns true (use default cameraId) +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurements, key_pairs, interp_factors); +// +// EXPECT(factor1->equals(*factor2)); +// EXPECT(factor1->equals(*factor3)); +// } +// { // create slightly different factors (different keys) and show equal +// // returns false +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); +// factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! +// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); +// +// EXPECT(!factor1->equals(*factor2)); +// EXPECT(!factor1->equals(*factor3)); +// } { // create slightly different factors (different extrinsics) and show equal // returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor2, sharedK, - body_P_sensor * body_P_sensor); // different! - factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + Cameras cameraRig2; + cameraRig2.push_back( Camera(body_P_sensor * body_P_sensor, sharedK) ); + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } { // create slightly different factors (different interp factors) and show // equal returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor1, sharedK, - body_P_sensor); // different! - factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } } -static const int DimBlock = 12; ///< size of the variable stacking 2 poses from - ///< which the observation pose is interpolated -static const int ZDim = 2; ///< Measurement dimension (Point2) -typedef Eigen::Matrix - MatrixZD; // F blocks (derivatives wrt camera) -typedef std::vector> - FBlocks; // vector of F blocks - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { - using namespace vanillaPoseRS; - - // Project two landmarks into two cameras - Point2 level_uv = cam1.project(landmark1); - Point2 level_uv_right = cam2.project(landmark1); - Pose3 body_P_sensorId = Pose3::identity(); - - SmartFactorRS factor(model); - factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); - factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); - - Values values; // it's a pose factor, hence these are poses - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above); - - double actualError = factor.error(values); - double expectedError = 0.0; - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - - // Check triangulation - factor.triangulateSafe(factor.cameras(values)); - TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - EXPECT(assert_equal( - landmark1, - *point)); // check triangulation result matches expected 3D landmark - - // Check Jacobians - // -- actual Jacobians - FBlocks actualFs; - Matrix actualE; - Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, - values); - EXPECT(actualE.rows() == 4); - EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); - EXPECT(actualb.cols() == 1); - EXPECT(actualFs.size() == 2); - - // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, - x2, l0, sharedK, body_P_sensorId); - Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError( - level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); - EXPECT( - assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); - EXPECT( - assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); - EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus - // reprojectionError - EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, - x2, x3, l0, sharedK, body_P_sensorId); - Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError( - pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); - EXPECT( - assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); - EXPECT( - assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); - EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus - // reprojectionError - EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { - // also includes non-identical extrinsic calibration - using namespace vanillaPoseRS; - - // Project two landmarks into two cameras - Point2 pixelError(0.5, 1.0); - Point2 level_uv = cam1.project(landmark1) + pixelError; - Point2 level_uv_right = cam2.project(landmark1); - Pose3 body_P_sensorNonId = body_P_sensor; - - SmartFactorRS factor(model); - factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); - factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, - body_P_sensorNonId); - - Values values; // it's a pose factor, hence these are poses - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above); - - // Perform triangulation - factor.triangulateSafe(factor.cameras(values)); - TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - Point3 landmarkNoisy = *point; - - // Check Jacobians - // -- actual Jacobians - FBlocks actualFs; - Matrix actualE; - Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, - values); - EXPECT(actualE.rows() == 4); - EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); - EXPECT(actualb.cols() == 1); - EXPECT(actualFs.size() == 2); - - // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, - x2, l0, sharedK, body_P_sensorNonId); - Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = - factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, - expectedF12, expectedE1); - EXPECT( - assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); - EXPECT( - assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); - EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus - // reprojectionError - EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, - x2, x3, l0, sharedK, - body_P_sensorNonId); - Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = - factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, - expectedF22, expectedE2); - EXPECT( - assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); - EXPECT( - assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); - EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus - // reprojectionError - EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); - - // Check errors - double actualError = factor.error(values); // from smart factor - NonlinearFactorGraph nfg; - nfg.add(factor1); - nfg.add(factor2); - values.insert(l0, landmarkNoisy); - double expectedError = nfg.error(values); - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { - using namespace vanillaPoseRS; - Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); - - // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1, x2)); - key_pairs.push_back(std::make_pair(x2, x3)); - key_pairs.push_back(std::make_pair(x3, x1)); - - std::vector interp_factors; - interp_factors.push_back(interp_factor1); - interp_factors.push_back(interp_factor2); - interp_factors.push_back(interp_factor3); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - Values groundTruth; - groundTruth.insert(x1, level_pose); - groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); - DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), - // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to - // original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, - -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { - // here we replicate a test in SmartProjectionPoseFactor by setting - // interpolation factors to 0 and 1 (such that the rollingShutter measurements - // falls back to standard pixel measurements) Note: this is a quite extreme - // test since in typical camera you would not have more than 1 measurement per - // landmark at each interpolated pose - using namespace vanillaPose; - - // Default cameras for simple derivatives - static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); - - Rot3 R = Rot3::identity(); - Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); - Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); - Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); - Pose3 body_P_sensorId = Pose3::identity(); - - // one landmarks 1m in front of camera - Point3 landmark1(0, 0, 10); - - Point2Vector measurements_lmk1; - - // Project 2 landmarks into 2 cameras - measurements_lmk1.push_back(cam1.project(landmark1)); - measurements_lmk1.push_back(cam2.project(landmark1)); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - double interp_factor = 0; // equivalent to measurement taken at pose 1 - smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); - interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); - - SmartFactor::Cameras cameras; - cameras.push_back(cam1); - cameras.push_back(cam2); - - // Make sure triangulation works - EXPECT(smartFactor1->triangulateSafe(cameras)); - EXPECT(!smartFactor1->isDegenerate()); - EXPECT(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); - EXPECT(p); - EXPECT(assert_equal(landmark1, *p)); - - VectorValues zeroDelta; - Vector6 delta; - delta.setZero(); - zeroDelta.insert(x1, delta); - zeroDelta.insert(x2, delta); - - VectorValues perturbedDelta; - delta.setOnes(); - perturbedDelta.insert(x1, delta); - perturbedDelta.insert(x2, delta); - double expectedError = 2500; - - // After eliminating the point, A1 and A2 contain 2-rank information on - // cameras: - Matrix16 A1, A2; - A1 << -10, 0, 0, 0, 1, 0; - A2 << 10, 0, 1, 0, -1, 0; - A1 *= 10. / sigma; - A2 *= 10. / sigma; - Matrix expectedInformation; // filled below - - // createHessianFactor - Matrix66 G11 = 0.5 * A1.transpose() * A1; - Matrix66 G12 = 0.5 * A1.transpose() * A2; - Matrix66 G22 = 0.5 * A2.transpose() * A2; - - Vector6 g1; - g1.setZero(); - Vector6 g2; - g2.setZero(); - - double f = 0; - - RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); - expectedInformation = expected.information(); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - boost::shared_ptr> actual = - smartFactor1->createHessianFactor(values); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual, 1e-6)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { - using namespace vanillaPoseRS; - Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); - - // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1, x2)); - key_pairs.push_back(std::make_pair(x2, x3)); - key_pairs.push_back(std::make_pair(x3, x1)); - - std::vector interp_factors; - interp_factors.push_back(interp_factor1); - interp_factors.push_back(interp_factor2); - interp_factors.push_back(interp_factor3); - - double excludeLandmarksFutherThanDist = 1e10; // very large - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(true); - - SmartFactorRS smartFactor1(model, params); - smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor2(model, params); - smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor3(model, params); - smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to - // original pose_above - values.insert(x3, pose_above * noise_pose); - - // Optimization should correct 3rd pose - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - optimization_3poses_landmarkDistance) { - using namespace vanillaPoseRS; - Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); - - // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1, x2)); - key_pairs.push_back(std::make_pair(x2, x3)); - key_pairs.push_back(std::make_pair(x3, x1)); - - std::vector interp_factors; - interp_factors.push_back(interp_factor1); - interp_factors.push_back(interp_factor2); - interp_factors.push_back(interp_factor3); - - double excludeLandmarksFutherThanDist = 2; - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); - - SmartFactorRS smartFactor1(model, params); - smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor2(model, params); - smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor3(model, params); - smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to - // original pose_above - values.insert(x3, pose_above * noise_pose); - - // All factors are disabled (due to the distance threshold) and pose should - // remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3), result.at(x3))); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - optimization_3poses_dynamicOutlierRejection) { - using namespace vanillaPoseRS; - // add fourth landmark - Point3 landmark4(5, -0.5, 1); - - Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, - measurements_lmk4; - // Project 4 landmarks into cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); - projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); - measurements_lmk4.at(0) = - measurements_lmk4.at(0) + Point2(10, 10); // add outlier - - // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1, x2)); - key_pairs.push_back(std::make_pair(x2, x3)); - key_pairs.push_back(std::make_pair(x3, x1)); - - std::vector interp_factors; - interp_factors.push_back(interp_factor1); - interp_factors.push_back(interp_factor2); - interp_factors.push_back(interp_factor3); - - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = - 3; // max 3 pixel of average reprojection error - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - params.setEnableEPI(false); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); - smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(smartFactor4); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - Pose3 noise_pose = Pose3( - Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.01, 0.01, - 0.01)); // smaller noise, otherwise outlier rejection will kick in - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to - // original pose_above - values.insert(x3, pose_above * noise_pose); - - // Optimization should correct 3rd pose - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - hessianComparedToProjFactorsRollingShutter) { - using namespace vanillaPoseRS; - Point2Vector measurements_lmk1; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - - // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1, x2)); - key_pairs.push_back(std::make_pair(x2, x3)); - key_pairs.push_back(std::make_pair(x3, x1)); - - std::vector interp_factors; - interp_factors.push_back(interp_factor1); - interp_factors.push_back(interp_factor2); - interp_factors.push_back(interp_factor3); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization - // point - values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, - -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); - - // linearization point for the poses - Pose3 pose1 = level_pose; - Pose3 pose2 = pose_right; - Pose3 pose3 = pose_above * noise_pose; - - // ==== check Hessian of smartFactor1 ===== - // -- compute actual Hessian - boost::shared_ptr linearfactor1 = - smartFactor1->linearize(values); - Matrix actualHessian = linearfactor1->information(); - - // -- compute expected Hessian from manual Schur complement from Jacobians - // linearization point for the 3D point - smartFactor1->triangulateSafe(smartFactor1->cameras(values)); - TriangulationResult point = smartFactor1->point(); - EXPECT(point.valid()); // check triangulated point is valid - - // Use the factor to calculate the Jacobians - Matrix F = Matrix::Zero(2 * 3, 6 * 3); - Matrix E = Matrix::Zero(2 * 3, 3); - Vector b = Vector::Zero(6); - - // create projection factors rolling shutter - ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, - model, x1, x2, l0, sharedK); - Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian - // computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(0, 0) = H1Actual; - F.block<2, 6>(0, 6) = H2Actual; - E.block<2, 3>(0, 0) = H3Actual; - - ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, - model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(2, 6) = H1Actual; - F.block<2, 6>(2, 12) = H2Actual; - E.block<2, 3>(2, 0) = H3Actual; - - ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, - model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(4, 12) = H1Actual; - F.block<2, 6>(4, 0) = H2Actual; - E.block<2, 3>(4, 0) = H3Actual; - - // whiten - F = (1 / sigma) * F; - E = (1 / sigma) * E; - b = (1 / sigma) * b; - //* G = F' * F - F' * E * P * E' * F - Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = - F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); - EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); - - // ==== check Information vector of smartFactor1 ===== - GaussianFactorGraph gfg; - gfg.add(linearfactor1); - Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, - 1e-6)); // sanity check on hessian - - // -- compute actual information vector - Vector actualInfoVector = gfg.hessian().second; - - // -- compute expected information vector from manual Schur complement from - // Jacobians - //* g = F' * (b - E * P * E' * b) - Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); - EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); - - // ==== check error of smartFactor1 (again) ===== - NonlinearFactorGraph nfg_projFactorsRS; - nfg_projFactorsRS.add(factor11); - nfg_projFactorsRS.add(factor12); - nfg_projFactorsRS.add(factor13); - values.insert(l0, *point); - - double actualError = smartFactor1->error(values); - double expectedError = nfg_projFactorsRS.error(values); - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { - // in this test we make sure the fact works even if we have multiple pixel - // measurements of the same landmark at a single pose, a setup that occurs in - // multi-camera systems - - using namespace vanillaPoseRS; - Point2Vector measurements_lmk1; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - - // create redundant measurements: - Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back( - measurements_lmk1.at(0)); // we readd the first measurement - - // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1, x2)); - key_pairs.push_back(std::make_pair(x2, x3)); - key_pairs.push_back(std::make_pair(x3, x1)); - key_pairs.push_back(std::make_pair(x1, x2)); - - std::vector interp_factors; - interp_factors.push_back(interp_factor1); - interp_factors.push_back(interp_factor2); - interp_factors.push_back(interp_factor3); - interp_factors.push_back(interp_factor1); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, - sharedK); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization - // point - values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, - -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); - - // linearization point for the poses - Pose3 pose1 = level_pose; - Pose3 pose2 = pose_right; - Pose3 pose3 = pose_above * noise_pose; - - // ==== check Hessian of smartFactor1 ===== - // -- compute actual Hessian - boost::shared_ptr linearfactor1 = - smartFactor1->linearize(values); - Matrix actualHessian = linearfactor1->information(); - - // -- compute expected Hessian from manual Schur complement from Jacobians - // linearization point for the 3D point - smartFactor1->triangulateSafe(smartFactor1->cameras(values)); - TriangulationResult point = smartFactor1->point(); - EXPECT(point.valid()); // check triangulated point is valid - - // Use standard ProjectionFactorRollingShutter factor to calculate the - // Jacobians - Matrix F = Matrix::Zero(2 * 4, 6 * 3); - Matrix E = Matrix::Zero(2 * 4, 3); - Vector b = Vector::Zero(8); - - // create projection factors rolling shutter - ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], - interp_factor1, model, x1, x2, l0, - sharedK); - Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian - // computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(0, 0) = H1Actual; - F.block<2, 6>(0, 6) = H2Actual; - E.block<2, 3>(0, 0) = H3Actual; - - ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], - interp_factor2, model, x2, x3, l0, - sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(2, 6) = H1Actual; - F.block<2, 6>(2, 12) = H2Actual; - E.block<2, 3>(2, 0) = H3Actual; - - ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], - interp_factor3, model, x3, x1, l0, - sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(4, 12) = H1Actual; - F.block<2, 6>(4, 0) = H2Actual; - E.block<2, 3>(4, 0) = H3Actual; - - ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], - interp_factor1, model, x1, x2, l0, - sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(6, 0) = H1Actual; - F.block<2, 6>(6, 6) = H2Actual; - E.block<2, 3>(6, 0) = H3Actual; - - // whiten - F = (1 / sigma) * F; - E = (1 / sigma) * E; - b = (1 / sigma) * b; - //* G = F' * F - F' * E * P * E' * F - Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = - F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); - EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); - - // ==== check Information vector of smartFactor1 ===== - GaussianFactorGraph gfg; - gfg.add(linearfactor1); - Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, - 1e-6)); // sanity check on hessian - - // -- compute actual information vector - Vector actualInfoVector = gfg.hessian().second; - - // -- compute expected information vector from manual Schur complement from - // Jacobians - //* g = F' * (b - E * P * E' * b) - Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); - EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); - - // ==== check error of smartFactor1 (again) ===== - NonlinearFactorGraph nfg_projFactorsRS; - nfg_projFactorsRS.add(factor11); - nfg_projFactorsRS.add(factor12); - nfg_projFactorsRS.add(factor13); - nfg_projFactorsRS.add(factor14); - values.insert(l0, *point); - - double actualError = smartFactor1->error(values); - double expectedError = nfg_projFactorsRS.error(values); - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - optimization_3poses_measurementsFromSamePose) { - using namespace vanillaPoseRS; - Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); - - // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1, x2)); - key_pairs.push_back(std::make_pair(x2, x3)); - key_pairs.push_back(std::make_pair(x3, x1)); - - std::vector interp_factors; - interp_factors.push_back(interp_factor1); - interp_factors.push_back(interp_factor2); - interp_factors.push_back(interp_factor3); - - // For first factor, we create redundant measurement (taken by the same keys - // as factor 1, to make sure the redundancy in the keys does not create - // problems) - Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back( - measurements_lmk1.at(0)); // we readd the first measurement - std::vector> key_pairs_redundant = key_pairs; - key_pairs_redundant.push_back( - key_pairs.at(0)); // we readd the first pair of keys - std::vector interp_factors_redundant = interp_factors; - interp_factors_redundant.push_back( - interp_factors.at(0)); // we readd the first interp factor - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, - interp_factors_redundant, sharedK); - - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - Values groundTruth; - groundTruth.insert(x1, level_pose); - groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); - DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), - // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to - // original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, - -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); -} - -#ifndef DISABLE_TIMING -#include -// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: -// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 -// children, min: 0 max: 0) -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, timing) { - using namespace vanillaPose; - - // Default cameras for simple derivatives - static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); - - Rot3 R = Rot3::identity(); - Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); - Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); - Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); - Pose3 body_P_sensorId = Pose3::identity(); - - // one landmarks 1m in front of camera - Point3 landmark1(0, 0, 10); - - Point2Vector measurements_lmk1; - - // Project 2 landmarks into 2 cameras - measurements_lmk1.push_back(cam1.project(landmark1)); - measurements_lmk1.push_back(cam2.project(landmark1)); - - size_t nrTests = 1000; - - for (size_t i = 0; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); - double interp_factor = 0; // equivalent to measurement taken at pose 1 - smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, - sharedKSimple, body_P_sensorId); - interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, - sharedKSimple, body_P_sensorId); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - gttic_(SF_RS_LINEARIZE); - smartFactorRS->linearize(values); - gttoc_(SF_RS_LINEARIZE); - } - - for (size_t i = 0; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); - smartFactor->add(measurements_lmk1[0], x1); - smartFactor->add(measurements_lmk1[1], x2); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - gttic_(RS_LINEARIZE); - smartFactor->linearize(values); - gttoc_(RS_LINEARIZE); - } - tictoc_print_(); -} -#endif +//static const int DimBlock = 12; ///< size of the variable stacking 2 poses from +// ///< which the observation pose is interpolated +//static const int ZDim = 2; ///< Measurement dimension (Point2) +//typedef Eigen::Matrix +// MatrixZD; // F blocks (derivatives wrt camera) +//typedef std::vector> +// FBlocks; // vector of F blocks +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { +// using namespace vanillaPoseRS; +// +// // Project two landmarks into two cameras +// Point2 level_uv = cam1.project(landmark1); +// Point2 level_uv_right = cam2.project(landmark1); +// Pose3 body_P_sensorId = Pose3::identity(); +// +// SmartFactorRS factor(model); +// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); +// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); +// +// Values values; // it's a pose factor, hence these are poses +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// values.insert(x3, pose_above); +// +// double actualError = factor.error(values); +// double expectedError = 0.0; +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +// +// // Check triangulation +// factor.triangulateSafe(factor.cameras(values)); +// TriangulationResult point = factor.point(); +// EXPECT(point.valid()); // check triangulated point is valid +// EXPECT(assert_equal( +// landmark1, +// *point)); // check triangulation result matches expected 3D landmark +// +// // Check Jacobians +// // -- actual Jacobians +// FBlocks actualFs; +// Matrix actualE; +// Vector actualb; +// factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, +// values); +// EXPECT(actualE.rows() == 4); +// EXPECT(actualE.cols() == 3); +// EXPECT(actualb.rows() == 4); +// EXPECT(actualb.cols() == 1); +// EXPECT(actualFs.size() == 2); +// +// // -- expected Jacobians from ProjectionFactorsRollingShutter +// ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, +// x2, l0, sharedK, body_P_sensorId); +// Matrix expectedF11, expectedF12, expectedE1; +// Vector expectedb1 = factor1.evaluateError( +// level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); +// EXPECT( +// assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); +// EXPECT( +// assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); +// EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); +// // by definition computeJacobiansWithTriangulatedPoint returns minus +// // reprojectionError +// EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); +// +// ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, +// x2, x3, l0, sharedK, body_P_sensorId); +// Matrix expectedF21, expectedF22, expectedE2; +// Vector expectedb2 = factor2.evaluateError( +// pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); +// EXPECT( +// assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); +// EXPECT( +// assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); +// EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); +// // by definition computeJacobiansWithTriangulatedPoint returns minus +// // reprojectionError +// EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { +// // also includes non-identical extrinsic calibration +// using namespace vanillaPoseRS; +// +// // Project two landmarks into two cameras +// Point2 pixelError(0.5, 1.0); +// Point2 level_uv = cam1.project(landmark1) + pixelError; +// Point2 level_uv_right = cam2.project(landmark1); +// Pose3 body_P_sensorNonId = body_P_sensor; +// +// SmartFactorRS factor(model); +// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); +// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, +// body_P_sensorNonId); +// +// Values values; // it's a pose factor, hence these are poses +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// values.insert(x3, pose_above); +// +// // Perform triangulation +// factor.triangulateSafe(factor.cameras(values)); +// TriangulationResult point = factor.point(); +// EXPECT(point.valid()); // check triangulated point is valid +// Point3 landmarkNoisy = *point; +// +// // Check Jacobians +// // -- actual Jacobians +// FBlocks actualFs; +// Matrix actualE; +// Vector actualb; +// factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, +// values); +// EXPECT(actualE.rows() == 4); +// EXPECT(actualE.cols() == 3); +// EXPECT(actualb.rows() == 4); +// EXPECT(actualb.cols() == 1); +// EXPECT(actualFs.size() == 2); +// +// // -- expected Jacobians from ProjectionFactorsRollingShutter +// ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, +// x2, l0, sharedK, body_P_sensorNonId); +// Matrix expectedF11, expectedF12, expectedE1; +// Vector expectedb1 = +// factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, +// expectedF12, expectedE1); +// EXPECT( +// assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); +// EXPECT( +// assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); +// EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); +// // by definition computeJacobiansWithTriangulatedPoint returns minus +// // reprojectionError +// EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); +// +// ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, +// x2, x3, l0, sharedK, +// body_P_sensorNonId); +// Matrix expectedF21, expectedF22, expectedE2; +// Vector expectedb2 = +// factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, +// expectedF22, expectedE2); +// EXPECT( +// assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); +// EXPECT( +// assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); +// EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); +// // by definition computeJacobiansWithTriangulatedPoint returns minus +// // reprojectionError +// EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); +// +// // Check errors +// double actualError = factor.error(values); // from smart factor +// NonlinearFactorGraph nfg; +// nfg.add(factor1); +// nfg.add(factor2); +// values.insert(l0, landmarkNoisy); +// double expectedError = nfg.error(values); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { +// using namespace vanillaPoseRS; +// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); +// +// // create inputs +// std::vector> key_pairs; +// key_pairs.push_back(std::make_pair(x1, x2)); +// key_pairs.push_back(std::make_pair(x2, x3)); +// key_pairs.push_back(std::make_pair(x3, x1)); +// +// std::vector interp_factors; +// interp_factors.push_back(interp_factor1); +// interp_factors.push_back(interp_factor2); +// interp_factors.push_back(interp_factor3); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); +// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); +// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, level_pose, noisePrior); +// graph.addPrior(x2, pose_right, noisePrior); +// +// Values groundTruth; +// groundTruth.insert(x1, level_pose); +// groundTruth.insert(x2, pose_right); +// groundTruth.insert(x3, pose_above); +// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); +// +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), +// // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise, we expect it to move back to +// // original pose_above +// values.insert(x3, pose_above * noise_pose); +// EXPECT( // check that the pose is actually noisy +// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, +// -0.0313952598, -0.000986635786, 0.0314107591, +// -0.999013364, -0.0313952598), +// Point3(0.1, -0.1, 1.9)), +// values.at(x3))); +// +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { +// // here we replicate a test in SmartProjectionPoseFactor by setting +// // interpolation factors to 0 and 1 (such that the rollingShutter measurements +// // falls back to standard pixel measurements) Note: this is a quite extreme +// // test since in typical camera you would not have more than 1 measurement per +// // landmark at each interpolated pose +// using namespace vanillaPose; +// +// // Default cameras for simple derivatives +// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); +// +// Rot3 R = Rot3::identity(); +// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); +// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); +// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); +// Pose3 body_P_sensorId = Pose3::identity(); +// +// // one landmarks 1m in front of camera +// Point3 landmark1(0, 0, 10); +// +// Point2Vector measurements_lmk1; +// +// // Project 2 landmarks into 2 cameras +// measurements_lmk1.push_back(cam1.project(landmark1)); +// measurements_lmk1.push_back(cam2.project(landmark1)); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// double interp_factor = 0; // equivalent to measurement taken at pose 1 +// smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, +// body_P_sensorId); +// interp_factor = 1; // equivalent to measurement taken at pose 2 +// smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, +// body_P_sensorId); +// +// SmartFactor::Cameras cameras; +// cameras.push_back(cam1); +// cameras.push_back(cam2); +// +// // Make sure triangulation works +// EXPECT(smartFactor1->triangulateSafe(cameras)); +// EXPECT(!smartFactor1->isDegenerate()); +// EXPECT(!smartFactor1->isPointBehindCamera()); +// boost::optional p = smartFactor1->point(); +// EXPECT(p); +// EXPECT(assert_equal(landmark1, *p)); +// +// VectorValues zeroDelta; +// Vector6 delta; +// delta.setZero(); +// zeroDelta.insert(x1, delta); +// zeroDelta.insert(x2, delta); +// +// VectorValues perturbedDelta; +// delta.setOnes(); +// perturbedDelta.insert(x1, delta); +// perturbedDelta.insert(x2, delta); +// double expectedError = 2500; +// +// // After eliminating the point, A1 and A2 contain 2-rank information on +// // cameras: +// Matrix16 A1, A2; +// A1 << -10, 0, 0, 0, 1, 0; +// A2 << 10, 0, 1, 0, -1, 0; +// A1 *= 10. / sigma; +// A2 *= 10. / sigma; +// Matrix expectedInformation; // filled below +// +// // createHessianFactor +// Matrix66 G11 = 0.5 * A1.transpose() * A1; +// Matrix66 G12 = 0.5 * A1.transpose() * A2; +// Matrix66 G22 = 0.5 * A2.transpose() * A2; +// +// Vector6 g1; +// g1.setZero(); +// Vector6 g2; +// g2.setZero(); +// +// double f = 0; +// +// RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); +// expectedInformation = expected.information(); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// boost::shared_ptr> actual = +// smartFactor1->createHessianFactor(values); +// EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); +// EXPECT(assert_equal(expected, *actual, 1e-6)); +// EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); +// EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { +// using namespace vanillaPoseRS; +// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); +// +// // create inputs +// std::vector> key_pairs; +// key_pairs.push_back(std::make_pair(x1, x2)); +// key_pairs.push_back(std::make_pair(x2, x3)); +// key_pairs.push_back(std::make_pair(x3, x1)); +// +// std::vector interp_factors; +// interp_factors.push_back(interp_factor1); +// interp_factors.push_back(interp_factor2); +// interp_factors.push_back(interp_factor3); +// +// double excludeLandmarksFutherThanDist = 1e10; // very large +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setEnableEPI(true); +// +// SmartFactorRS smartFactor1(model, params); +// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor2(model, params); +// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor3(model, params); +// smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, level_pose, noisePrior); +// graph.addPrior(x2, pose_right, noisePrior); +// +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise, we expect it to move back to +// // original pose_above +// values.insert(x3, pose_above * noise_pose); +// +// // Optimization should correct 3rd pose +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// optimization_3poses_landmarkDistance) { +// using namespace vanillaPoseRS; +// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); +// +// // create inputs +// std::vector> key_pairs; +// key_pairs.push_back(std::make_pair(x1, x2)); +// key_pairs.push_back(std::make_pair(x2, x3)); +// key_pairs.push_back(std::make_pair(x3, x1)); +// +// std::vector interp_factors; +// interp_factors.push_back(interp_factor1); +// interp_factors.push_back(interp_factor2); +// interp_factors.push_back(interp_factor3); +// +// double excludeLandmarksFutherThanDist = 2; +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setEnableEPI(false); +// +// SmartFactorRS smartFactor1(model, params); +// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor2(model, params); +// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor3(model, params); +// smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, level_pose, noisePrior); +// graph.addPrior(x2, pose_right, noisePrior); +// +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise, we expect it to move back to +// // original pose_above +// values.insert(x3, pose_above * noise_pose); +// +// // All factors are disabled (due to the distance threshold) and pose should +// // remain where it is +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(values.at(x3), result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// optimization_3poses_dynamicOutlierRejection) { +// using namespace vanillaPoseRS; +// // add fourth landmark +// Point3 landmark4(5, -0.5, 1); +// +// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, +// measurements_lmk4; +// // Project 4 landmarks into cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); +// projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); +// measurements_lmk4.at(0) = +// measurements_lmk4.at(0) + Point2(10, 10); // add outlier +// +// // create inputs +// std::vector> key_pairs; +// key_pairs.push_back(std::make_pair(x1, x2)); +// key_pairs.push_back(std::make_pair(x2, x3)); +// key_pairs.push_back(std::make_pair(x3, x1)); +// +// std::vector interp_factors; +// interp_factors.push_back(interp_factor1); +// interp_factors.push_back(interp_factor2); +// interp_factors.push_back(interp_factor3); +// +// double excludeLandmarksFutherThanDist = 1e10; +// double dynamicOutlierRejectionThreshold = +// 3; // max 3 pixel of average reprojection error +// +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); +// params.setEnableEPI(false); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); +// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); +// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); +// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); +// smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(smartFactor4); +// graph.addPrior(x1, level_pose, noisePrior); +// graph.addPrior(x2, pose_right, noisePrior); +// +// Pose3 noise_pose = Pose3( +// Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.01, 0.01, +// 0.01)); // smaller noise, otherwise outlier rejection will kick in +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise, we expect it to move back to +// // original pose_above +// values.insert(x3, pose_above * noise_pose); +// +// // Optimization should correct 3rd pose +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// hessianComparedToProjFactorsRollingShutter) { +// using namespace vanillaPoseRS; +// Point2Vector measurements_lmk1; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// +// // create inputs +// std::vector> key_pairs; +// key_pairs.push_back(std::make_pair(x1, x2)); +// key_pairs.push_back(std::make_pair(x2, x3)); +// key_pairs.push_back(std::make_pair(x3, x1)); +// +// std::vector interp_factors; +// interp_factors.push_back(interp_factor1); +// interp_factors.push_back(interp_factor2); +// interp_factors.push_back(interp_factor3); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise to get a nontrivial linearization +// // point +// values.insert(x3, pose_above * noise_pose); +// EXPECT( // check that the pose is actually noisy +// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, +// -0.0313952598, -0.000986635786, 0.0314107591, +// -0.999013364, -0.0313952598), +// Point3(0.1, -0.1, 1.9)), +// values.at(x3))); +// +// // linearization point for the poses +// Pose3 pose1 = level_pose; +// Pose3 pose2 = pose_right; +// Pose3 pose3 = pose_above * noise_pose; +// +// // ==== check Hessian of smartFactor1 ===== +// // -- compute actual Hessian +// boost::shared_ptr linearfactor1 = +// smartFactor1->linearize(values); +// Matrix actualHessian = linearfactor1->information(); +// +// // -- compute expected Hessian from manual Schur complement from Jacobians +// // linearization point for the 3D point +// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); +// TriangulationResult point = smartFactor1->point(); +// EXPECT(point.valid()); // check triangulated point is valid +// +// // Use the factor to calculate the Jacobians +// Matrix F = Matrix::Zero(2 * 3, 6 * 3); +// Matrix E = Matrix::Zero(2 * 3, 3); +// Vector b = Vector::Zero(6); +// +// // create projection factors rolling shutter +// ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, +// model, x1, x2, l0, sharedK); +// Matrix H1Actual, H2Actual, H3Actual; +// // note: b is minus the reprojection error, cf the smart factor jacobian +// // computation +// b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(0, 0) = H1Actual; +// F.block<2, 6>(0, 6) = H2Actual; +// E.block<2, 3>(0, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, +// model, x2, x3, l0, sharedK); +// b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(2, 6) = H1Actual; +// F.block<2, 6>(2, 12) = H2Actual; +// E.block<2, 3>(2, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, +// model, x3, x1, l0, sharedK); +// b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(4, 12) = H1Actual; +// F.block<2, 6>(4, 0) = H2Actual; +// E.block<2, 3>(4, 0) = H3Actual; +// +// // whiten +// F = (1 / sigma) * F; +// E = (1 / sigma) * E; +// b = (1 / sigma) * b; +// //* G = F' * F - F' * E * P * E' * F +// Matrix P = (E.transpose() * E).inverse(); +// Matrix expectedHessian = +// F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); +// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); +// +// // ==== check Information vector of smartFactor1 ===== +// GaussianFactorGraph gfg; +// gfg.add(linearfactor1); +// Matrix actualHessian_v2 = gfg.hessian().first; +// EXPECT(assert_equal(actualHessian_v2, actualHessian, +// 1e-6)); // sanity check on hessian +// +// // -- compute actual information vector +// Vector actualInfoVector = gfg.hessian().second; +// +// // -- compute expected information vector from manual Schur complement from +// // Jacobians +// //* g = F' * (b - E * P * E' * b) +// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); +// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); +// +// // ==== check error of smartFactor1 (again) ===== +// NonlinearFactorGraph nfg_projFactorsRS; +// nfg_projFactorsRS.add(factor11); +// nfg_projFactorsRS.add(factor12); +// nfg_projFactorsRS.add(factor13); +// values.insert(l0, *point); +// +// double actualError = smartFactor1->error(values); +// double expectedError = nfg_projFactorsRS.error(values); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { +// // in this test we make sure the fact works even if we have multiple pixel +// // measurements of the same landmark at a single pose, a setup that occurs in +// // multi-camera systems +// +// using namespace vanillaPoseRS; +// Point2Vector measurements_lmk1; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// +// // create redundant measurements: +// Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; +// measurements_lmk1_redundant.push_back( +// measurements_lmk1.at(0)); // we readd the first measurement +// +// // create inputs +// std::vector> key_pairs; +// key_pairs.push_back(std::make_pair(x1, x2)); +// key_pairs.push_back(std::make_pair(x2, x3)); +// key_pairs.push_back(std::make_pair(x3, x1)); +// key_pairs.push_back(std::make_pair(x1, x2)); +// +// std::vector interp_factors; +// interp_factors.push_back(interp_factor1); +// interp_factors.push_back(interp_factor2); +// interp_factors.push_back(interp_factor3); +// interp_factors.push_back(interp_factor1); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, +// sharedK); +// +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise to get a nontrivial linearization +// // point +// values.insert(x3, pose_above * noise_pose); +// EXPECT( // check that the pose is actually noisy +// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, +// -0.0313952598, -0.000986635786, 0.0314107591, +// -0.999013364, -0.0313952598), +// Point3(0.1, -0.1, 1.9)), +// values.at(x3))); +// +// // linearization point for the poses +// Pose3 pose1 = level_pose; +// Pose3 pose2 = pose_right; +// Pose3 pose3 = pose_above * noise_pose; +// +// // ==== check Hessian of smartFactor1 ===== +// // -- compute actual Hessian +// boost::shared_ptr linearfactor1 = +// smartFactor1->linearize(values); +// Matrix actualHessian = linearfactor1->information(); +// +// // -- compute expected Hessian from manual Schur complement from Jacobians +// // linearization point for the 3D point +// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); +// TriangulationResult point = smartFactor1->point(); +// EXPECT(point.valid()); // check triangulated point is valid +// +// // Use standard ProjectionFactorRollingShutter factor to calculate the +// // Jacobians +// Matrix F = Matrix::Zero(2 * 4, 6 * 3); +// Matrix E = Matrix::Zero(2 * 4, 3); +// Vector b = Vector::Zero(8); +// +// // create projection factors rolling shutter +// ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], +// interp_factor1, model, x1, x2, l0, +// sharedK); +// Matrix H1Actual, H2Actual, H3Actual; +// // note: b is minus the reprojection error, cf the smart factor jacobian +// // computation +// b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(0, 0) = H1Actual; +// F.block<2, 6>(0, 6) = H2Actual; +// E.block<2, 3>(0, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], +// interp_factor2, model, x2, x3, l0, +// sharedK); +// b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(2, 6) = H1Actual; +// F.block<2, 6>(2, 12) = H2Actual; +// E.block<2, 3>(2, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], +// interp_factor3, model, x3, x1, l0, +// sharedK); +// b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(4, 12) = H1Actual; +// F.block<2, 6>(4, 0) = H2Actual; +// E.block<2, 3>(4, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], +// interp_factor1, model, x1, x2, l0, +// sharedK); +// b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(6, 0) = H1Actual; +// F.block<2, 6>(6, 6) = H2Actual; +// E.block<2, 3>(6, 0) = H3Actual; +// +// // whiten +// F = (1 / sigma) * F; +// E = (1 / sigma) * E; +// b = (1 / sigma) * b; +// //* G = F' * F - F' * E * P * E' * F +// Matrix P = (E.transpose() * E).inverse(); +// Matrix expectedHessian = +// F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); +// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); +// +// // ==== check Information vector of smartFactor1 ===== +// GaussianFactorGraph gfg; +// gfg.add(linearfactor1); +// Matrix actualHessian_v2 = gfg.hessian().first; +// EXPECT(assert_equal(actualHessian_v2, actualHessian, +// 1e-6)); // sanity check on hessian +// +// // -- compute actual information vector +// Vector actualInfoVector = gfg.hessian().second; +// +// // -- compute expected information vector from manual Schur complement from +// // Jacobians +// //* g = F' * (b - E * P * E' * b) +// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); +// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); +// +// // ==== check error of smartFactor1 (again) ===== +// NonlinearFactorGraph nfg_projFactorsRS; +// nfg_projFactorsRS.add(factor11); +// nfg_projFactorsRS.add(factor12); +// nfg_projFactorsRS.add(factor13); +// nfg_projFactorsRS.add(factor14); +// values.insert(l0, *point); +// +// double actualError = smartFactor1->error(values); +// double expectedError = nfg_projFactorsRS.error(values); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// optimization_3poses_measurementsFromSamePose) { +// using namespace vanillaPoseRS; +// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); +// +// // create inputs +// std::vector> key_pairs; +// key_pairs.push_back(std::make_pair(x1, x2)); +// key_pairs.push_back(std::make_pair(x2, x3)); +// key_pairs.push_back(std::make_pair(x3, x1)); +// +// std::vector interp_factors; +// interp_factors.push_back(interp_factor1); +// interp_factors.push_back(interp_factor2); +// interp_factors.push_back(interp_factor3); +// +// // For first factor, we create redundant measurement (taken by the same keys +// // as factor 1, to make sure the redundancy in the keys does not create +// // problems) +// Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; +// measurements_lmk1_redundant.push_back( +// measurements_lmk1.at(0)); // we readd the first measurement +// std::vector> key_pairs_redundant = key_pairs; +// key_pairs_redundant.push_back( +// key_pairs.at(0)); // we readd the first pair of keys +// std::vector interp_factors_redundant = interp_factors; +// interp_factors_redundant.push_back( +// interp_factors.at(0)); // we readd the first interp factor +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, +// interp_factors_redundant, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); +// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); +// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.addPrior(x1, level_pose, noisePrior); +// graph.addPrior(x2, pose_right, noisePrior); +// +// Values groundTruth; +// groundTruth.insert(x1, level_pose); +// groundTruth.insert(x2, pose_right); +// groundTruth.insert(x3, pose_above); +// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); +// +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), +// // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// // initialize third pose with some noise, we expect it to move back to +// // original pose_above +// values.insert(x3, pose_above * noise_pose); +// EXPECT( // check that the pose is actually noisy +// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, +// -0.0313952598, -0.000986635786, 0.0314107591, +// -0.999013364, -0.0313952598), +// Point3(0.1, -0.1, 1.9)), +// values.at(x3))); +// +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +//} +// +//#ifndef DISABLE_TIMING +//#include +//// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) +////| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: +//// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 +//// children, min: 0 max: 0) +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, timing) { +// using namespace vanillaPose; +// +// // Default cameras for simple derivatives +// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); +// +// Rot3 R = Rot3::identity(); +// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); +// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); +// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); +// Pose3 body_P_sensorId = Pose3::identity(); +// +// // one landmarks 1m in front of camera +// Point3 landmark1(0, 0, 10); +// +// Point2Vector measurements_lmk1; +// +// // Project 2 landmarks into 2 cameras +// measurements_lmk1.push_back(cam1.project(landmark1)); +// measurements_lmk1.push_back(cam2.project(landmark1)); +// +// size_t nrTests = 1000; +// +// for (size_t i = 0; i < nrTests; i++) { +// SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); +// double interp_factor = 0; // equivalent to measurement taken at pose 1 +// smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, +// sharedKSimple, body_P_sensorId); +// interp_factor = 1; // equivalent to measurement taken at pose 2 +// smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, +// sharedKSimple, body_P_sensorId); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(SF_RS_LINEARIZE); +// smartFactorRS->linearize(values); +// gttoc_(SF_RS_LINEARIZE); +// } +// +// for (size_t i = 0; i < nrTests; i++) { +// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); +// smartFactor->add(measurements_lmk1[0], x1); +// smartFactor->add(measurements_lmk1[1], x2); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(RS_LINEARIZE); +// smartFactor->linearize(values); +// gttoc_(RS_LINEARIZE); +// } +// tictoc_print_(); +//} +//#endif /* ************************************************************************* */ int main() { From 4fd6c2cb5d12cec2ff7e9b9184d2e2fe1fd78b15 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 22:23:16 -0400 Subject: [PATCH 081/169] bug fix - finalizing last few tests --- .../SmartProjectionPoseFactorRollingShutter.h | 3 +- ...martProjectionPoseFactorRollingShutter.cpp | 1133 ++++++++--------- 2 files changed, 567 insertions(+), 569 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index cf1526673..09d13f0e5 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -178,7 +178,8 @@ class SmartProjectionPoseFactorRollingShutter if (world_P_body_key_pairs.size() != measurements.size() || world_P_body_key_pairs.size() != alphas.size() - || world_P_body_key_pairs.size() != cameraIds.size()) { + || (world_P_body_key_pairs.size() != cameraIds.size() + && cameraIds.size() != 0)) { // cameraIds.size()=0 is default throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "trying to add inconsistent inputs"); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 6c28cbda5..e93c00ba8 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -135,41 +135,41 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); factor3->add(measurements, key_pairs, interp_factors, cameraIds); -// { // create equal factors and show equal returns true -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); -// factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); -// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); -// -// EXPECT(factor1->equals(*factor2)); -// EXPECT(factor1->equals(*factor3)); -// } -// { // create equal factors and show equal returns true (use default cameraId) -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurement1, x1, x2, interp_factor1); -// factor1->add(measurement2, x2, x3, interp_factor2); -// factor1->add(measurement3, x3, x4, interp_factor3); -// -// EXPECT(factor1->equals(*factor2)); -// EXPECT(factor1->equals(*factor3)); -// } -// { // create equal factors and show equal returns true (use default cameraId) -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurements, key_pairs, interp_factors); -// -// EXPECT(factor1->equals(*factor2)); -// EXPECT(factor1->equals(*factor3)); -// } -// { // create slightly different factors (different keys) and show equal -// // returns false -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); -// factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! -// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); -// -// EXPECT(!factor1->equals(*factor2)); -// EXPECT(!factor1->equals(*factor3)); -// } + { // create equal factors and show equal returns true + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create equal factors and show equal returns true (use default cameraId) + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1); + factor1->add(measurement2, x2, x3, interp_factor2); + factor1->add(measurement3, x3, x4, interp_factor3); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create equal factors and show equal returns true (use default cameraId) + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurements, key_pairs, interp_factors); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create slightly different factors (different keys) and show equal + // returns false (use default cameraIds) + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); + + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); + } { // create slightly different factors (different extrinsics) and show equal // returns false Cameras cameraRig2; @@ -194,539 +194,536 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } } -//static const int DimBlock = 12; ///< size of the variable stacking 2 poses from -// ///< which the observation pose is interpolated -//static const int ZDim = 2; ///< Measurement dimension (Point2) -//typedef Eigen::Matrix -// MatrixZD; // F blocks (derivatives wrt camera) -//typedef std::vector> -// FBlocks; // vector of F blocks -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { -// using namespace vanillaPoseRS; -// -// // Project two landmarks into two cameras -// Point2 level_uv = cam1.project(landmark1); -// Point2 level_uv_right = cam2.project(landmark1); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// SmartFactorRS factor(model); -// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); -// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); -// -// Values values; // it's a pose factor, hence these are poses -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// values.insert(x3, pose_above); -// -// double actualError = factor.error(values); -// double expectedError = 0.0; -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -// -// // Check triangulation -// factor.triangulateSafe(factor.cameras(values)); -// TriangulationResult point = factor.point(); -// EXPECT(point.valid()); // check triangulated point is valid -// EXPECT(assert_equal( -// landmark1, -// *point)); // check triangulation result matches expected 3D landmark -// -// // Check Jacobians -// // -- actual Jacobians -// FBlocks actualFs; -// Matrix actualE; -// Vector actualb; -// factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, -// values); -// EXPECT(actualE.rows() == 4); -// EXPECT(actualE.cols() == 3); -// EXPECT(actualb.rows() == 4); -// EXPECT(actualb.cols() == 1); -// EXPECT(actualFs.size() == 2); -// -// // -- expected Jacobians from ProjectionFactorsRollingShutter -// ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, -// x2, l0, sharedK, body_P_sensorId); -// Matrix expectedF11, expectedF12, expectedE1; -// Vector expectedb1 = factor1.evaluateError( -// level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); -// EXPECT( -// assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); -// EXPECT( -// assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); -// EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); -// // by definition computeJacobiansWithTriangulatedPoint returns minus -// // reprojectionError -// EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); -// -// ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, -// x2, x3, l0, sharedK, body_P_sensorId); -// Matrix expectedF21, expectedF22, expectedE2; -// Vector expectedb2 = factor2.evaluateError( -// pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); -// EXPECT( -// assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); -// EXPECT( -// assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); -// EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); -// // by definition computeJacobiansWithTriangulatedPoint returns minus -// // reprojectionError -// EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { -// // also includes non-identical extrinsic calibration -// using namespace vanillaPoseRS; -// -// // Project two landmarks into two cameras -// Point2 pixelError(0.5, 1.0); -// Point2 level_uv = cam1.project(landmark1) + pixelError; -// Point2 level_uv_right = cam2.project(landmark1); -// Pose3 body_P_sensorNonId = body_P_sensor; -// -// SmartFactorRS factor(model); -// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); -// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, -// body_P_sensorNonId); -// -// Values values; // it's a pose factor, hence these are poses -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// values.insert(x3, pose_above); -// -// // Perform triangulation -// factor.triangulateSafe(factor.cameras(values)); -// TriangulationResult point = factor.point(); -// EXPECT(point.valid()); // check triangulated point is valid -// Point3 landmarkNoisy = *point; -// -// // Check Jacobians -// // -- actual Jacobians -// FBlocks actualFs; -// Matrix actualE; -// Vector actualb; -// factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, -// values); -// EXPECT(actualE.rows() == 4); -// EXPECT(actualE.cols() == 3); -// EXPECT(actualb.rows() == 4); -// EXPECT(actualb.cols() == 1); -// EXPECT(actualFs.size() == 2); -// -// // -- expected Jacobians from ProjectionFactorsRollingShutter -// ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, -// x2, l0, sharedK, body_P_sensorNonId); -// Matrix expectedF11, expectedF12, expectedE1; -// Vector expectedb1 = -// factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, -// expectedF12, expectedE1); -// EXPECT( -// assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); -// EXPECT( -// assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); -// EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); -// // by definition computeJacobiansWithTriangulatedPoint returns minus -// // reprojectionError -// EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); -// -// ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, -// x2, x3, l0, sharedK, -// body_P_sensorNonId); -// Matrix expectedF21, expectedF22, expectedE2; -// Vector expectedb2 = -// factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, -// expectedF22, expectedE2); -// EXPECT( -// assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); -// EXPECT( -// assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); -// EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); -// // by definition computeJacobiansWithTriangulatedPoint returns minus -// // reprojectionError -// EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); -// -// // Check errors -// double actualError = factor.error(values); // from smart factor -// NonlinearFactorGraph nfg; -// nfg.add(factor1); -// nfg.add(factor2); -// values.insert(l0, landmarkNoisy); -// double expectedError = nfg.error(values); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// std::vector> key_pairs; -// key_pairs.push_back(std::make_pair(x1, x2)); -// key_pairs.push_back(std::make_pair(x2, x3)); -// key_pairs.push_back(std::make_pair(x3, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); -// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); -// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Values groundTruth; -// groundTruth.insert(x1, level_pose); -// groundTruth.insert(x2, pose_right); -// groundTruth.insert(x3, pose_above); -// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), -// // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to -// // original pose_above -// values.insert(x3, pose_above * noise_pose); -// EXPECT( // check that the pose is actually noisy -// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -// -0.0313952598, -0.000986635786, 0.0314107591, -// -0.999013364, -0.0313952598), -// Point3(0.1, -0.1, 1.9)), -// values.at(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { -// // here we replicate a test in SmartProjectionPoseFactor by setting -// // interpolation factors to 0 and 1 (such that the rollingShutter measurements -// // falls back to standard pixel measurements) Note: this is a quite extreme -// // test since in typical camera you would not have more than 1 measurement per -// // landmark at each interpolated pose -// using namespace vanillaPose; -// -// // Default cameras for simple derivatives -// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); -// -// Rot3 R = Rot3::identity(); -// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); -// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); -// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// // one landmarks 1m in front of camera -// Point3 landmark1(0, 0, 10); -// -// Point2Vector measurements_lmk1; -// -// // Project 2 landmarks into 2 cameras -// measurements_lmk1.push_back(cam1.project(landmark1)); -// measurements_lmk1.push_back(cam2.project(landmark1)); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// double interp_factor = 0; // equivalent to measurement taken at pose 1 -// smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, -// body_P_sensorId); -// interp_factor = 1; // equivalent to measurement taken at pose 2 -// smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, -// body_P_sensorId); -// -// SmartFactor::Cameras cameras; -// cameras.push_back(cam1); -// cameras.push_back(cam2); -// -// // Make sure triangulation works -// EXPECT(smartFactor1->triangulateSafe(cameras)); -// EXPECT(!smartFactor1->isDegenerate()); -// EXPECT(!smartFactor1->isPointBehindCamera()); -// boost::optional p = smartFactor1->point(); -// EXPECT(p); -// EXPECT(assert_equal(landmark1, *p)); -// -// VectorValues zeroDelta; -// Vector6 delta; -// delta.setZero(); -// zeroDelta.insert(x1, delta); -// zeroDelta.insert(x2, delta); -// -// VectorValues perturbedDelta; -// delta.setOnes(); -// perturbedDelta.insert(x1, delta); -// perturbedDelta.insert(x2, delta); -// double expectedError = 2500; -// -// // After eliminating the point, A1 and A2 contain 2-rank information on -// // cameras: -// Matrix16 A1, A2; -// A1 << -10, 0, 0, 0, 1, 0; -// A2 << 10, 0, 1, 0, -1, 0; -// A1 *= 10. / sigma; -// A2 *= 10. / sigma; -// Matrix expectedInformation; // filled below -// -// // createHessianFactor -// Matrix66 G11 = 0.5 * A1.transpose() * A1; -// Matrix66 G12 = 0.5 * A1.transpose() * A2; -// Matrix66 G22 = 0.5 * A2.transpose() * A2; -// -// Vector6 g1; -// g1.setZero(); -// Vector6 g2; -// g2.setZero(); -// -// double f = 0; -// -// RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); -// expectedInformation = expected.information(); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// boost::shared_ptr> actual = -// smartFactor1->createHessianFactor(values); -// EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); -// EXPECT(assert_equal(expected, *actual, 1e-6)); -// EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); -// EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// std::vector> key_pairs; -// key_pairs.push_back(std::make_pair(x1, x2)); -// key_pairs.push_back(std::make_pair(x2, x3)); -// key_pairs.push_back(std::make_pair(x3, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// double excludeLandmarksFutherThanDist = 1e10; // very large -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setEnableEPI(true); -// -// SmartFactorRS smartFactor1(model, params); -// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor2(model, params); -// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor3(model, params); -// smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to -// // original pose_above -// values.insert(x3, pose_above * noise_pose); -// -// // Optimization should correct 3rd pose -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, -// optimization_3poses_landmarkDistance) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// std::vector> key_pairs; -// key_pairs.push_back(std::make_pair(x1, x2)); -// key_pairs.push_back(std::make_pair(x2, x3)); -// key_pairs.push_back(std::make_pair(x3, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// double excludeLandmarksFutherThanDist = 2; -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setEnableEPI(false); -// -// SmartFactorRS smartFactor1(model, params); -// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor2(model, params); -// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor3(model, params); -// smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to -// // original pose_above -// values.insert(x3, pose_above * noise_pose); -// -// // All factors are disabled (due to the distance threshold) and pose should -// // remain where it is -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(values.at(x3), result.at(x3))); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, -// optimization_3poses_dynamicOutlierRejection) { -// using namespace vanillaPoseRS; -// // add fourth landmark -// Point3 landmark4(5, -0.5, 1); -// -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, -// measurements_lmk4; -// // Project 4 landmarks into cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); -// measurements_lmk4.at(0) = -// measurements_lmk4.at(0) + Point2(10, 10); // add outlier -// -// // create inputs -// std::vector> key_pairs; -// key_pairs.push_back(std::make_pair(x1, x2)); -// key_pairs.push_back(std::make_pair(x2, x3)); -// key_pairs.push_back(std::make_pair(x3, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// double excludeLandmarksFutherThanDist = 1e10; -// double dynamicOutlierRejectionThreshold = -// 3; // max 3 pixel of average reprojection error -// -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); -// params.setEnableEPI(false); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); -// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); -// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); -// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); -// smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.push_back(smartFactor4); -// graph.addPrior(x1, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Pose3 noise_pose = Pose3( -// Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.01, 0.01, -// 0.01)); // smaller noise, otherwise outlier rejection will kick in -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to -// // original pose_above -// values.insert(x3, pose_above * noise_pose); -// -// // Optimization should correct 3rd pose -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -//} -// +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from + ///< which the observation pose is interpolated +static const int ZDim = 2; ///< Measurement dimension (Point2) +typedef Eigen::Matrix + MatrixZD; // F blocks (derivatives wrt camera) +typedef std::vector> + FBlocks; // vector of F blocks + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { + using namespace vanillaPoseRS; + + // Project two landmarks into two cameras + Point2 level_uv = cam1.project(landmark1); + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorId = Pose3::identity(); + + SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK)); + factor.add(level_uv, x1, x2, interp_factor1); + factor.add(level_uv_right, x2, x3, interp_factor2); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // Check triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal( + landmark1, + *point)); // check triangulation result matches expected 3D landmark + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError( + level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, body_P_sensorId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError( + pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { + // also includes non-identical extrinsic calibration + using namespace vanillaPoseRS; + + // Project two landmarks into two cameras + Point2 pixelError(0.5, 1.0); + Point2 level_uv = cam1.project(landmark1) + pixelError; + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorNonId = body_P_sensor; + + SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK)); + factor.add(level_uv, x1, x2, interp_factor1); + factor.add(level_uv_right, x2, x3, interp_factor2); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + // Perform triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + Point3 landmarkNoisy = *point; + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorNonId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = + factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, + expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, + body_P_sensorNonId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = + factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, + expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + + // Check errors + double actualError = factor.error(values); // from smart factor + NonlinearFactorGraph nfg; + nfg.add(factor1); + nfg.add(factor2); + values.insert(l0, landmarkNoisy); + double expectedError = nfg.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { + // here we replicate a test in SmartProjectionPoseFactor by setting + // interpolation factors to 0 and 1 (such that the rollingShutter measurements + // falls back to standard pixel measurements) Note: this is a quite extreme + // test since in typical camera you would not have more than 1 measurement per + // landmark at each interpolated pose + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + EXPECT(smartFactor1->triangulateSafe(cameras)); + EXPECT(!smartFactor1->isDegenerate()); + EXPECT(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + EXPECT(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on + // cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; // very large + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(true); + + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_landmarkDistance) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 2; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled (due to the distance threshold) and pose should + // remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_dynamicOutlierRejection) { + using namespace vanillaPoseRS; + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, + measurements_lmk4; + // Project 4 landmarks into cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); + measurements_lmk4.at(0) = + measurements_lmk4.at(0) + Point2(10, 10); // add outlier + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setEnableEPI(false); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + smartFactor4->add(measurements_lmk4, key_pairs, interp_factors); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3( + Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, + 0.01)); // smaller noise, otherwise outlier rejection will kick in + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + ///* *************************************************************************/ //TEST(SmartProjectionPoseFactorRollingShutter, // hessianComparedToProjFactorsRollingShutter) { From 0797eae9a8d55db35f3cbe32702bea2b7178618f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 22:32:02 -0400 Subject: [PATCH 082/169] all tests are passing! --- ...martProjectionPoseFactorRollingShutter.cpp | 842 +++++++++--------- 1 file changed, 419 insertions(+), 423 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index e93c00ba8..34e2678c3 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -724,429 +724,425 @@ TEST(SmartProjectionPoseFactorRollingShutter, EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, -// hessianComparedToProjFactorsRollingShutter) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// -// // create inputs -// std::vector> key_pairs; -// key_pairs.push_back(std::make_pair(x1, x2)); -// key_pairs.push_back(std::make_pair(x2, x3)); -// key_pairs.push_back(std::make_pair(x3, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise to get a nontrivial linearization -// // point -// values.insert(x3, pose_above * noise_pose); -// EXPECT( // check that the pose is actually noisy -// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -// -0.0313952598, -0.000986635786, 0.0314107591, -// -0.999013364, -0.0313952598), -// Point3(0.1, -0.1, 1.9)), -// values.at(x3))); -// -// // linearization point for the poses -// Pose3 pose1 = level_pose; -// Pose3 pose2 = pose_right; -// Pose3 pose3 = pose_above * noise_pose; -// -// // ==== check Hessian of smartFactor1 ===== -// // -- compute actual Hessian -// boost::shared_ptr linearfactor1 = -// smartFactor1->linearize(values); -// Matrix actualHessian = linearfactor1->information(); -// -// // -- compute expected Hessian from manual Schur complement from Jacobians -// // linearization point for the 3D point -// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); -// TriangulationResult point = smartFactor1->point(); -// EXPECT(point.valid()); // check triangulated point is valid -// -// // Use the factor to calculate the Jacobians -// Matrix F = Matrix::Zero(2 * 3, 6 * 3); -// Matrix E = Matrix::Zero(2 * 3, 3); -// Vector b = Vector::Zero(6); -// -// // create projection factors rolling shutter -// ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, -// model, x1, x2, l0, sharedK); -// Matrix H1Actual, H2Actual, H3Actual; -// // note: b is minus the reprojection error, cf the smart factor jacobian -// // computation -// b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, -// H2Actual, H3Actual); -// F.block<2, 6>(0, 0) = H1Actual; -// F.block<2, 6>(0, 6) = H2Actual; -// E.block<2, 3>(0, 0) = H3Actual; -// -// ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, -// model, x2, x3, l0, sharedK); -// b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, -// H2Actual, H3Actual); -// F.block<2, 6>(2, 6) = H1Actual; -// F.block<2, 6>(2, 12) = H2Actual; -// E.block<2, 3>(2, 0) = H3Actual; -// -// ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, -// model, x3, x1, l0, sharedK); -// b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, -// H2Actual, H3Actual); -// F.block<2, 6>(4, 12) = H1Actual; -// F.block<2, 6>(4, 0) = H2Actual; -// E.block<2, 3>(4, 0) = H3Actual; -// -// // whiten -// F = (1 / sigma) * F; -// E = (1 / sigma) * E; -// b = (1 / sigma) * b; -// //* G = F' * F - F' * E * P * E' * F -// Matrix P = (E.transpose() * E).inverse(); -// Matrix expectedHessian = -// F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); -// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); -// -// // ==== check Information vector of smartFactor1 ===== -// GaussianFactorGraph gfg; -// gfg.add(linearfactor1); -// Matrix actualHessian_v2 = gfg.hessian().first; -// EXPECT(assert_equal(actualHessian_v2, actualHessian, -// 1e-6)); // sanity check on hessian -// -// // -- compute actual information vector -// Vector actualInfoVector = gfg.hessian().second; -// -// // -- compute expected information vector from manual Schur complement from -// // Jacobians -// //* g = F' * (b - E * P * E' * b) -// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); -// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); -// -// // ==== check error of smartFactor1 (again) ===== -// NonlinearFactorGraph nfg_projFactorsRS; -// nfg_projFactorsRS.add(factor11); -// nfg_projFactorsRS.add(factor12); -// nfg_projFactorsRS.add(factor13); -// values.insert(l0, *point); -// -// double actualError = smartFactor1->error(values); -// double expectedError = nfg_projFactorsRS.error(values); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, -// hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { -// // in this test we make sure the fact works even if we have multiple pixel -// // measurements of the same landmark at a single pose, a setup that occurs in -// // multi-camera systems -// -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// -// // create redundant measurements: -// Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; -// measurements_lmk1_redundant.push_back( -// measurements_lmk1.at(0)); // we readd the first measurement -// -// // create inputs -// std::vector> key_pairs; -// key_pairs.push_back(std::make_pair(x1, x2)); -// key_pairs.push_back(std::make_pair(x2, x3)); -// key_pairs.push_back(std::make_pair(x3, x1)); -// key_pairs.push_back(std::make_pair(x1, x2)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// interp_factors.push_back(interp_factor1); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, -// sharedK); -// -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise to get a nontrivial linearization -// // point -// values.insert(x3, pose_above * noise_pose); -// EXPECT( // check that the pose is actually noisy -// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -// -0.0313952598, -0.000986635786, 0.0314107591, -// -0.999013364, -0.0313952598), -// Point3(0.1, -0.1, 1.9)), -// values.at(x3))); -// -// // linearization point for the poses -// Pose3 pose1 = level_pose; -// Pose3 pose2 = pose_right; -// Pose3 pose3 = pose_above * noise_pose; -// -// // ==== check Hessian of smartFactor1 ===== -// // -- compute actual Hessian -// boost::shared_ptr linearfactor1 = -// smartFactor1->linearize(values); -// Matrix actualHessian = linearfactor1->information(); -// -// // -- compute expected Hessian from manual Schur complement from Jacobians -// // linearization point for the 3D point -// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); -// TriangulationResult point = smartFactor1->point(); -// EXPECT(point.valid()); // check triangulated point is valid -// -// // Use standard ProjectionFactorRollingShutter factor to calculate the -// // Jacobians -// Matrix F = Matrix::Zero(2 * 4, 6 * 3); -// Matrix E = Matrix::Zero(2 * 4, 3); -// Vector b = Vector::Zero(8); -// -// // create projection factors rolling shutter -// ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], -// interp_factor1, model, x1, x2, l0, -// sharedK); -// Matrix H1Actual, H2Actual, H3Actual; -// // note: b is minus the reprojection error, cf the smart factor jacobian -// // computation -// b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, -// H2Actual, H3Actual); -// F.block<2, 6>(0, 0) = H1Actual; -// F.block<2, 6>(0, 6) = H2Actual; -// E.block<2, 3>(0, 0) = H3Actual; -// -// ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], -// interp_factor2, model, x2, x3, l0, -// sharedK); -// b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, -// H2Actual, H3Actual); -// F.block<2, 6>(2, 6) = H1Actual; -// F.block<2, 6>(2, 12) = H2Actual; -// E.block<2, 3>(2, 0) = H3Actual; -// -// ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], -// interp_factor3, model, x3, x1, l0, -// sharedK); -// b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, -// H2Actual, H3Actual); -// F.block<2, 6>(4, 12) = H1Actual; -// F.block<2, 6>(4, 0) = H2Actual; -// E.block<2, 3>(4, 0) = H3Actual; -// -// ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], -// interp_factor1, model, x1, x2, l0, -// sharedK); -// b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, -// H2Actual, H3Actual); -// F.block<2, 6>(6, 0) = H1Actual; -// F.block<2, 6>(6, 6) = H2Actual; -// E.block<2, 3>(6, 0) = H3Actual; -// -// // whiten -// F = (1 / sigma) * F; -// E = (1 / sigma) * E; -// b = (1 / sigma) * b; -// //* G = F' * F - F' * E * P * E' * F -// Matrix P = (E.transpose() * E).inverse(); -// Matrix expectedHessian = -// F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); -// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); -// -// // ==== check Information vector of smartFactor1 ===== -// GaussianFactorGraph gfg; -// gfg.add(linearfactor1); -// Matrix actualHessian_v2 = gfg.hessian().first; -// EXPECT(assert_equal(actualHessian_v2, actualHessian, -// 1e-6)); // sanity check on hessian -// -// // -- compute actual information vector -// Vector actualInfoVector = gfg.hessian().second; -// -// // -- compute expected information vector from manual Schur complement from -// // Jacobians -// //* g = F' * (b - E * P * E' * b) -// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); -// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); -// -// // ==== check error of smartFactor1 (again) ===== -// NonlinearFactorGraph nfg_projFactorsRS; -// nfg_projFactorsRS.add(factor11); -// nfg_projFactorsRS.add(factor12); -// nfg_projFactorsRS.add(factor13); -// nfg_projFactorsRS.add(factor14); -// values.insert(l0, *point); -// -// double actualError = smartFactor1->error(values); -// double expectedError = nfg_projFactorsRS.error(values); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, -// optimization_3poses_measurementsFromSamePose) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// std::vector> key_pairs; -// key_pairs.push_back(std::make_pair(x1, x2)); -// key_pairs.push_back(std::make_pair(x2, x3)); -// key_pairs.push_back(std::make_pair(x3, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// // For first factor, we create redundant measurement (taken by the same keys -// // as factor 1, to make sure the redundancy in the keys does not create -// // problems) -// Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; -// measurements_lmk1_redundant.push_back( -// measurements_lmk1.at(0)); // we readd the first measurement -// std::vector> key_pairs_redundant = key_pairs; -// key_pairs_redundant.push_back( -// key_pairs.at(0)); // we readd the first pair of keys -// std::vector interp_factors_redundant = interp_factors; -// interp_factors_redundant.push_back( -// interp_factors.at(0)); // we readd the first interp factor -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, -// interp_factors_redundant, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); -// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); -// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Values groundTruth; -// groundTruth.insert(x1, level_pose); -// groundTruth.insert(x2, pose_right); -// groundTruth.insert(x3, pose_above); -// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), -// // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to -// // original pose_above -// values.insert(x3, pose_above * noise_pose); -// EXPECT( // check that the pose is actually noisy -// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -// -0.0313952598, -0.000986635786, 0.0314107591, -// -0.999013364, -0.0313952598), -// Point3(0.1, -0.1, 1.9)), -// values.at(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); -//} -// -//#ifndef DISABLE_TIMING -//#include -//// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) -////| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: -//// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 -//// children, min: 0 max: 0) -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, timing) { -// using namespace vanillaPose; -// -// // Default cameras for simple derivatives -// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); -// -// Rot3 R = Rot3::identity(); -// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); -// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); -// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// // one landmarks 1m in front of camera -// Point3 landmark1(0, 0, 10); -// -// Point2Vector measurements_lmk1; -// -// // Project 2 landmarks into 2 cameras -// measurements_lmk1.push_back(cam1.project(landmark1)); -// measurements_lmk1.push_back(cam2.project(landmark1)); -// -// size_t nrTests = 1000; -// -// for (size_t i = 0; i < nrTests; i++) { -// SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); -// double interp_factor = 0; // equivalent to measurement taken at pose 1 -// smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, -// sharedKSimple, body_P_sensorId); -// interp_factor = 1; // equivalent to measurement taken at pose 2 -// smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, -// sharedKSimple, body_P_sensorId); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(SF_RS_LINEARIZE); -// smartFactorRS->linearize(values); -// gttoc_(SF_RS_LINEARIZE); -// } -// -// for (size_t i = 0; i < nrTests; i++) { -// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); -// smartFactor->add(measurements_lmk1[0], x1); -// smartFactor->add(measurements_lmk1[1], x2); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(RS_LINEARIZE); -// smartFactor->linearize(values); -// gttoc_(RS_LINEARIZE); -// } -// tictoc_print_(); -//} -//#endif +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization + // point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use the factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 3, 6 * 3); + Matrix E = Matrix::Zero(2 * 3, 3); + Vector b = Vector::Zero(6); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, + model, x1, x2, l0, sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; + + ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, + model, x2, x3, l0, sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; + + ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, + model, x3, x1, l0, sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; + + // whiten + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from + // Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create redundant measurements: + Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + key_pairs.push_back(std::make_pair(x1, x2)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + interp_factors.push_back(interp_factor1); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization + // point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use standard ProjectionFactorRollingShutter factor to calculate the + // Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(8); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], + interp_factor1, model, x1, x2, l0, + sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; + + ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], + interp_factor2, model, x2, x3, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; + + ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], + interp_factor3, model, x3, x1, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; + + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], + interp_factor1, model, x1, x2, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(6, 0) = H1Actual; + F.block<2, 6>(6, 6) = H2Actual; + E.block<2, 3>(6, 0) = H3Actual; + + // whiten + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from + // Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + nfg_projFactorsRS.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_measurementsFromSamePose) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) + Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back( + key_pairs.at(0)); // we readd the first pair of keys + std::vector interp_factors_redundant = interp_factors; + interp_factors_redundant.push_back( + interp_factors.at(0)); // we readd the first interp factor + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, + interp_factors_redundant); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +} + +#ifndef DISABLE_TIMING +#include +//-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, min: 0 max: 0) +//| -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 children, min: 0 max: 0) +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, timing) { + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + size_t nrTests = 10000; + + for (size_t i = 0; i < nrTests; i++) { + SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SF_RS_LINEARIZE); + smartFactorRS->linearize(values); + gttoc_(SF_RS_LINEARIZE); + } + + for (size_t i = 0; i < nrTests; i++) { + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + smartFactor->add(measurements_lmk1[0], x1); + smartFactor->add(measurements_lmk1[1], x2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(RS_LINEARIZE); + smartFactor->linearize(values); + gttoc_(RS_LINEARIZE); + } + tictoc_print_(); +} +#endif /* ************************************************************************* */ int main() { From 823a7e7be09bcf160c0cf4f469493a6f91b27654 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 22:53:36 -0400 Subject: [PATCH 083/169] done with tests --- ...martProjectionPoseFactorRollingShutter.cpp | 72 +++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 34e2678c3..c4c670de3 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -424,6 +424,78 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + Cameras cameraRig; + cameraRig.push_back(Camera(body_P_sensor, sharedK)); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1,1,1}); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1,1,1}); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1,1,1}); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); // pose above is the pose of the camera + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // here we replicate a test in SmartProjectionPoseFactor by setting From f0745a9c286c4104647302779ac4442ded799703 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 23:10:45 -0400 Subject: [PATCH 084/169] now I only need to fix comments in rolling shutter factor --- gtsam/slam/SmartProjectionRigFactor.h | 35 ++++++++++++++---- .../tests/testSmartProjectionRigFactor.cpp | 36 +++++++++++-------- .../SmartProjectionPoseFactorRollingShutter.h | 5 +++ 3 files changed, 56 insertions(+), 20 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 0246d6327..3cae1ea64 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -94,6 +94,23 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Base::params_.linearizationMode = gtsam::HESSIAN; } + /** + * Constructor + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param camera single camera (fixed poses wrt body and intrinsics) + * @param params parameters for the smart projection factors + */ + SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, + const Camera& camera, + const SmartProjectionParams& params = + SmartProjectionParams()) + : Base(sharedNoiseModel, params) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + cameraRig_.push_back(camera); + } + /** Virtual destructor */ ~SmartProjectionRigFactor() override { } @@ -104,9 +121,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param measured 2-dimensional location of the projection of a * single landmark in a single view (the measurement) * @param poseKey key corresponding to the body pose of the camera taking the measurement - * @param cameraId ID of the camera in the rig taking the measurement + * @param cameraId ID of the camera in the rig taking the measurement (default 0) */ - void add(const Point2& measured, const Key& poseKey, const size_t cameraId) { + void add(const Point2& measured, const Key& poseKey, const size_t cameraId = 0) { // store measurement and key this->measured_.push_back(measured); this->nonUniqueKeys_.push_back(poseKey); @@ -127,13 +144,19 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements) */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, - const FastVector& cameraIds) { - if(poseKeys.size() != measurements.size() || poseKeys.size() != cameraIds.size()){ + const FastVector& cameraIds = FastVector()) { + if (poseKeys.size() != measurements.size() + || (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { throw std::runtime_error("SmartProjectionRigFactor: " - "trying to add inconsistent inputs"); + "trying to add inconsistent inputs"); + } + if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + throw std::runtime_error( + "SmartProjectionRigFactor: " + "camera rig includes multiple camera but add did not input cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], poseKeys[i], cameraIds[i]); + add(measurements[i], poseKeys[i], cameraIds.size() == 0 ? 0 : cameraIds[i]); } } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index fb3f3c461..a2a30e89a 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -92,6 +92,15 @@ TEST( SmartProjectionRigFactor, Constructor4) { factor1.add(measurement1, x1, cameraId1); } +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Constructor5) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params); + factor1.add(measurement1, x1, cameraId1); +} + /* ************************************************************************* */ TEST( SmartProjectionRigFactor, Equals ) { using namespace vanillaPose; @@ -105,6 +114,11 @@ TEST( SmartProjectionRigFactor, Equals ) { factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); + + SmartRigFactor::shared_ptr factor3(new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + factor3->add(measurement1, x1); // now use default + + CHECK(assert_equal(*factor1, *factor3)); } /* *************************************************************************/ @@ -112,16 +126,13 @@ TEST( SmartProjectionRigFactor, noiseless ) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, cameraRig); - factor.add(level_uv, x1, cameraId1); // both taken from the same camera - factor.add(level_uv_right, x2, cameraId1); + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK) ); + factor.add(level_uv, x1); // both taken from the same camera + factor.add(level_uv_right, x2); Values values; // it's a pose factor, hence these are poses values.insert(x1, cam1.pose()); @@ -245,13 +256,13 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); - smartFactor1.add(measurements_cam1, views, cameraIds); + smartFactor1.add(measurements_cam1, views); // use default CameraIds since we have a single camera SmartRigFactor smartFactor2(model, cameraRig, params); - smartFactor2.add(measurements_cam2, views, cameraIds); + smartFactor2.add(measurements_cam2, views); SmartRigFactor smartFactor3(model, cameraRig, params); - smartFactor3.add(measurements_cam3, views, cameraIds); + smartFactor3.add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -459,15 +470,12 @@ TEST( SmartProjectionRigFactor, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - KeyVector views { x1, x2 }; FastVector cameraIds { 0, 0 }; SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor - > (model,cameraRig); - smartFactor1->add(measurements_cam1, views, cameraIds); + > (model, Camera(Pose3::identity(), sharedK)); + smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds SmartRigFactor::Cameras cameras; cameras.push_back(cam1); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 09d13f0e5..f3f8900f5 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -183,6 +183,11 @@ class SmartProjectionPoseFactorRollingShutter throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "trying to add inconsistent inputs"); } + if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "camera rig includes multiple camera but add did not input cameraIds"); + } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, world_P_body_key_pairs[i].second, alphas[i], From 737dcf65e4c1488b1e605faf081fc94310476d5c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 23:29:20 -0400 Subject: [PATCH 085/169] all set here! --- .../SmartProjectionPoseFactorRollingShutter.h | 29 ++----- ...martProjectionPoseFactorRollingShutter.cpp | 85 +++++++++++++++++++ 2 files changed, 94 insertions(+), 20 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index f3f8900f5..84e3437a7 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -49,9 +49,6 @@ class SmartProjectionPoseFactorRollingShutter typedef typename CAMERA::CalibrationType CALIBRATION; protected: - /// shared pointer to calibration object (one for each observation) - std::vector> K_all_; - /// The keys of the pose of the body (with respect to an external world /// frame): two consecutive poses for each observation std::vector> world_P_body_key_pairs_; @@ -60,10 +57,10 @@ class SmartProjectionPoseFactorRollingShutter /// pair of consecutive poses std::vector alphas_; - /// one or more cameras in the rig (fixed poses wrt body + fixed intrinsics) + /// one or more cameras taking observations (fixed poses wrt body + fixed intrinsics) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation, in the same order), identifying which camera in the rig took the measurement + /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement FastVector cameraIds_; public: @@ -87,6 +84,7 @@ class SmartProjectionPoseFactorRollingShutter /** * Constructor * @param Isotropic measurement noise + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) taking the measurements * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( @@ -102,6 +100,7 @@ class SmartProjectionPoseFactorRollingShutter /** * Constructor * @param Isotropic measurement noise + * @param camera single camera (fixed poses wrt body and intrinsics) * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( @@ -114,13 +113,11 @@ class SmartProjectionPoseFactorRollingShutter cameraRig_.push_back(camera); } - /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; /** - * add a new measurement, with 2 pose keys, interpolation factor, camera - * (intrinsic and extrinsic) calibration, and observed pixel. + * add a new measurement, with 2 pose keys, interpolation factor, and cameraId * @param measured 2-dimensional location of the projection of a single * landmark in a single view (the measurement), interpolated from the 2 poses * @param world_P_body_key1 key corresponding to the first body poses (time <= @@ -129,8 +126,7 @@ class SmartProjectionPoseFactorRollingShutter * >= time pixel is acquired) * @param alpha interpolation factor in [0,1], such that if alpha = 0 the * interpolated pose is the same as world_P_body_key1 - * @param K (fixed) camera intrinsic calibration - * @param body_P_sensor (fixed) camera extrinsic calibration + * @param cameraId ID of the camera taking the measurement (default 0) */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, @@ -168,8 +164,7 @@ class SmartProjectionPoseFactorRollingShutter * for the i0-th measurement can be interpolated * @param alphas vector of interpolation params (in [0,1]), one for each * measurement (in the same order) - * @param Ks vector of (fixed) intrinsic calibration objects - * @param body_P_sensors vector of (fixed) extrinsic calibration objects + * @param cameraIds IDs of the cameras taking each measurement (same order as the measurements) */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, @@ -223,7 +218,7 @@ class SmartProjectionPoseFactorRollingShutter const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; - for (size_t i = 0; i < K_all_.size(); i++) { + for (size_t i = 0; i < cameraIds_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << " pose1 key: " << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; @@ -409,14 +404,8 @@ class SmartProjectionPoseFactorRollingShutter * @return Cameras */ typename Base::Cameras cameras(const Values& values) const override { - size_t numViews = this->measured_.size(); - assert(numViews == K_all_.size()); - assert(numViews == alphas_.size()); - assert(numViews == body_P_sensors_.size()); - assert(numViews == world_P_body_key_pairs_.size()); - typename Base::Cameras cameras; - for (size_t i = 0; i < numViews; i++) { // for each measurement + for (size_t i = 0; i < this->measured_.size(); i++) { // for each measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index c4c670de3..6a3e31dd7 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -496,6 +496,91 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { + using namespace vanillaPoseRS; + + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); + Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), + Point3(0, 0, 1)); + Pose3 body_T_sensor3 = Pose3::identity(); + + Camera camera1(interp_pose1*body_T_sensor1, sharedK); + Camera camera2(interp_pose2*body_T_sensor2, sharedK); + Camera camera3(interp_pose3*body_T_sensor3, sharedK); + + // Project three landmarks into three cameras + projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1); + projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2); + projectToMultipleCameras(camera1, camera2, camera3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + Cameras cameraRig; + cameraRig.push_back(Camera(body_T_sensor1, sharedK)); + cameraRig.push_back(Camera(body_T_sensor2, sharedK)); + cameraRig.push_back(Camera(body_T_sensor3, sharedK)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0,1,2}); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0,1,2}); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0,1,2}); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); // pose above is the pose of the camera + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-4)); +} + /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // here we replicate a test in SmartProjectionPoseFactor by setting From 7083de35a4aea261ab5a2e16585a94307b890c23 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 15:54:56 -0400 Subject: [PATCH 086/169] add failing unit test on axisAngle for Rot3 in c++ --- gtsam/geometry/tests/testRot3.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 34f90c8cc..5f0187ed9 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle) CHECK(assert_equal(expected,actual3,1e-5)); } +/* ************************************************************************* */ +TEST( Rot3, AxisAngle2) +{ + // constructor from a rotation matrix, as doubles in *row-major* order. + Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781); + + Unit3 actualAxis; + double actualAngle; + // convert Rot3 to quaternion using GTSAM + std::tie(actualAxis, actualAngle) = R1.axisAngle(); + + double expectedAngle = 3.1396582; + CHECK(assert_equal(expectedAngle, actualAngle, 1e-7)); +} + /* ************************************************************************* */ TEST( Rot3, Rodrigues) { From bb87dcf759964f7d4aa5dffa751e8c729129bf8e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 17:20:47 -0400 Subject: [PATCH 087/169] add python unit test for Rot3 --- python/gtsam/tests/test_Rot3.py | 41 +++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 python/gtsam/tests/test_Rot3.py diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py new file mode 100644 index 000000000..273e0e210 --- /dev/null +++ b/python/gtsam/tests/test_Rot3.py @@ -0,0 +1,41 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +See LICENSE for the license information +Rot3 unit tests. +Author: John Lambert +""" +# pylint: disable=no-name-in-module + +import unittest + +import numpy as np + +import gtsam +from gtsam import Rot3 +from gtsam.utils.test_case import GtsamTestCase + + +class TestRot3(GtsamTestCase): + """Test selected Rot3 methods.""" + + def test_axisangle(self) -> None: + """Test .axisAngle() method.""" + # fmt: off + R = np.array( + [ + [ -0.999957, 0.00922903, 0.00203116], + [ 0.00926964, 0.999739, 0.0208927], + [ -0.0018374, 0.0209105, -0.999781] + ]) + # fmt: on + + # get back angle in radians + _, actual_angle = Rot3(R).axisAngle() + expected_angle = 3.1396582; + self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) + + +if __name__ == "__main__": + unittest.main() From 0b0897d465f5121f2cfad0888fc3bf045d9bf3ac Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 17:21:22 -0400 Subject: [PATCH 088/169] fix typo --- python/gtsam/tests/test_Rot3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index 273e0e210..d22182433 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -33,7 +33,7 @@ class TestRot3(GtsamTestCase): # get back angle in radians _, actual_angle = Rot3(R).axisAngle() - expected_angle = 3.1396582; + expected_angle = 3.1396582 self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) From 225ac77f2f524ef4828cb7728628deb03d1aeddf Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 21:04:58 -0400 Subject: [PATCH 089/169] fix assert --- python/gtsam/tests/test_Rot3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index d22182433..90c80ab31 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -34,7 +34,7 @@ class TestRot3(GtsamTestCase): # get back angle in radians _, actual_angle = Rot3(R).axisAngle() expected_angle = 3.1396582 - self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) + np.testing.assert_almost_equal(actual_angle, expected_angle, 1e-7) if __name__ == "__main__": From 9c9ea6551a7eb63457c0035f8ec4a800761d1725 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 8 Oct 2021 11:45:56 -0400 Subject: [PATCH 090/169] run tests for C++ CI --- .github/scripts/unix.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 7fb925593..9689d346c 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -113,9 +113,9 @@ function test () # Actual testing if [ "$(uname)" == "Linux" ]; then - make -j$(nproc) + make -j$(nproc) check elif [ "$(uname)" == "Darwin" ]; then - make -j$(sysctl -n hw.physicalcpu) + make -j$(sysctl -n hw.physicalcpu) check fi finish From 21279e6d5107f85021ba0788681b0733502a79bb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 10 Oct 2021 16:20:30 -0400 Subject: [PATCH 091/169] Revert "Revert "replace deprecated tbb functionality"" This reverts commit f819b1a03f74f289f50b96125610026618601a2f. --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 +++++++------------ 2 files changed, 29 insertions(+), 46 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7a88f72eb..30cec3b9a 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - tbb::task::spawn_root_and_wait( - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold)); + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 87d5b0d4c..dc1b45906 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task, tbb::task_list +#include // tbb::task_group #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask : public tbb::task + class PreOrderTask { public: const boost::shared_ptr& treeNode; @@ -42,28 +42,30 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; bool makeNewTasks; - bool isPostOrderPhase; + // Keep track of order phase across multiple calls to the same functor + mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) + tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), + tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() override + void operator()() const { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return nullptr; } else { @@ -71,14 +73,10 @@ namespace gtsam { { if(!treeNode->children.empty()) { - // Allocate post-order task as a continuation - isPostOrderPhase = true; - recycle_as_continuation(); - bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - tbb::task* firstChild = 0; - tbb::task_list childTasks; + // If we have child tasks, start subtasks and wait for them to complete + tbb::task_group ctg; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -86,37 +84,30 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - tbb::task* childTask = - new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if (firstChild) - childTasks.push_back(*childTask); - else - firstChild = childTask; + ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, ctg, overThreshold)); } + ctg.wait(); - // If we have child tasks, start subtasks and wait for them to complete - set_ref_count((int)treeNode->children.size()); - spawn(childTasks); - return firstChild; + // Allocate post-order task as a continuation + isPostOrderPhase = true; + tg.run(*this); } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const { for(const boost::shared_ptr& child: node->children) { @@ -131,7 +122,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask : public tbb::task + class RootTask { public: const ROOTS& roots; @@ -139,38 +130,31 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold) : + int problemSizeThreshold, tbb::task_group& tg) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold) {} + problemSizeThreshold(problemSizeThreshold), tg(tg) {} - tbb::task* execute() override + void operator()() const { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tasks.push_back(*new(allocate_child()) - PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } - // Set TBB ref count - set_ref_count(1 + (int) roots.size()); - // Spawn tasks - spawn_and_wait_for_all(tasks); - // Return nullptr - return nullptr; } }; template - RootTask& - CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); - } + tbb::task_group tg; + tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + } } From 1a97b1413825045d7ff8f6e4a15fab64b8bcb7e4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 19:02:40 -0400 Subject: [PATCH 092/169] Fixed numerical problems near +-pi --- gtsam/geometry/SO3.cpp | 30 ++++++++++++++++++++--------- gtsam/geometry/tests/testRot3.cpp | 32 +++++++++++++++---------------- 2 files changed, 37 insertions(+), 25 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 80c0171ad..a2309e1f4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,25 +261,37 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (tr + 1.0 < 1e-10) { - if (std::abs(R33 + 1.0) > 1e-5) - omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); - else if (std::abs(R22 + 1.0) > 1e-5) - omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); - else + if (tr + 1.0 < 1e-4) { + std::vector diags = {R11, R22, R33}; + size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end())); + if (max_elem == 2) { + const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R33); + const double correction = 1.0 - M_1_PI / r * (R21 - R12); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); + } else if (max_elem == 1) { + const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R22); + const double correction = 1.0 - M_1_PI / r * (R13 - R31); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); + } else { // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit - omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); + const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R11); + const double correction = 1.0 - M_1_PI / r * (R32 - R23); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); + } } else { double magnitude; const double tr_3 = tr - 3.0; // always negative - if (tr_3 < -1e-7) { + if (tr_3 < -1e-6) { double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); } else { // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) // see https://github.com/borglab/gtsam/issues/746 for details - magnitude = 0.5 - tr_3 / 12.0; + magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0; } omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 5f0187ed9..bb6f1a77a 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -134,7 +134,7 @@ TEST( Rot3, AxisAngle2) std::tie(actualAxis, actualAngle) = R1.axisAngle(); double expectedAngle = 3.1396582; - CHECK(assert_equal(expectedAngle, actualAngle, 1e-7)); + CHECK(assert_equal(expectedAngle, actualAngle, 1e-5)); } /* ************************************************************************* */ @@ -196,13 +196,13 @@ TEST( Rot3, retract) } /* ************************************************************************* */ -TEST(Rot3, log) { +TEST( Rot3, log) { static const double PI = boost::math::constants::pi(); Vector w; Rot3 R; #define CHECK_OMEGA(X, Y, Z) \ - w = (Vector(3) << X, Y, Z).finished(); \ + w = (Vector(3) << (X), (Y), (Z)).finished(); \ R = Rot3::Rodrigues(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12)); @@ -234,17 +234,17 @@ TEST(Rot3, log) { CHECK_OMEGA(0, 0, PI) // Windows and Linux have flipped sign in quaternion mode -#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) +//#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) w = (Vector(3) << x * PI, y * PI, z * PI).finished(); R = Rot3::Rodrigues(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12)); -#else - CHECK_OMEGA(x * PI, y * PI, z * PI) -#endif +//#else +// CHECK_OMEGA(x * PI, y * PI, z * PI) +//#endif // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X, Y, Z) \ - w = (Vector(3) << X, Y, Z).finished(); \ + w = (Vector(3) << (X), (Y), (Z)).finished(); \ R = Rot3::Rodrigues(w); \ EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R))); @@ -262,15 +262,15 @@ TEST(Rot3, log) { // Rot3's Logmap returns different, but equivalent compacted // axis-angle vectors depending on whether Rot3 is implemented // by Quaternions or SO3. - #if defined(GTSAM_USE_QUATERNIONS) - // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees - EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), +#if defined(GTSAM_USE_QUATERNIONS) + // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees + EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), + (Vector)Rot3::Logmap(Rlund), 1e-8)); +#else + // SO3 will be approximate because of the non-orthogonality + EXPECT(assert_equal(Vector3(0.264485272, -0.742291088, -3.04136444), (Vector)Rot3::Logmap(Rlund), 1e-8)); - #else - // SO3 does not bound angle resulting in ~180.1 degrees - EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), - (Vector)Rot3::Logmap(Rlund), 1e-8)); - #endif +#endif } /* ************************************************************************* */ From dae409373c66abd6c41cb851a9c4a54b8e625dec Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 19:03:56 -0400 Subject: [PATCH 093/169] Revert "Revert "Revert "replace deprecated tbb functionality""" This reverts commit 21279e6d5107f85021ba0788681b0733502a79bb. --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 ++++++++++++------- 2 files changed, 46 insertions(+), 29 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 30cec3b9a..7a88f72eb 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold); + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index dc1b45906..87d5b0d4c 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task_group +#include // tbb::task, tbb::task_list #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask + class PreOrderTask : public tbb::task { public: const boost::shared_ptr& treeNode; @@ -42,30 +42,28 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; bool makeNewTasks; - // Keep track of order phase across multiple calls to the same functor - mutable bool isPostOrderPhase; + bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - tbb::task_group& tg, bool makeNewTasks = true) + bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), - tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - void operator()() const + tbb::task* execute() override { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); + return nullptr; } else { @@ -73,10 +71,14 @@ namespace gtsam { { if(!treeNode->children.empty()) { + // Allocate post-order task as a continuation + isPostOrderPhase = true; + recycle_as_continuation(); + bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - // If we have child tasks, start subtasks and wait for them to complete - tbb::task_group ctg; + tbb::task* firstChild = 0; + tbb::task_list childTasks; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -84,30 +86,37 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, ctg, overThreshold)); + tbb::task* childTask = + new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, overThreshold); + if (firstChild) + childTasks.push_back(*childTask); + else + firstChild = childTask; } - ctg.wait(); - // Allocate post-order task as a continuation - isPostOrderPhase = true; - tg.run(*this); + // If we have child tasks, start subtasks and wait for them to complete + set_ref_count((int)treeNode->children.size()); + spawn(childTasks); + return firstChild; } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); + return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); + return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) { for(const boost::shared_ptr& child: node->children) { @@ -122,7 +131,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask + class RootTask : public tbb::task { public: const ROOTS& roots; @@ -130,31 +139,38 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold, tbb::task_group& tg) : + int problemSizeThreshold) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold), tg(tg) {} + problemSizeThreshold(problemSizeThreshold) {} - void operator()() const + tbb::task* execute() override { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children + tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + tasks.push_back(*new(allocate_child()) + PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); } + // Set TBB ref count + set_ref_count(1 + (int) roots.size()); + // Spawn tasks + spawn_and_wait_for_all(tasks); + // Return nullptr + return nullptr; } }; template - void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + RootTask& + CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - tbb::task_group tg; - tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); - } + return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); + } } From aa6b4329110368c6cd6985f3d77f6af9e3572bd9 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 20:30:06 -0400 Subject: [PATCH 094/169] Cleanup equation --- gtsam/geometry/SO3.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index a2309e1f4..0cb6c93d1 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -267,19 +267,19 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { if (max_elem == 2) { const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R33); - const double correction = 1.0 - M_1_PI / r * (R21 - R12); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12); + omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); } else if (max_elem == 1) { const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R22); - const double correction = 1.0 - M_1_PI / r * (R13 - R31); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); + omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); - const double correction = 1.0 - M_1_PI / r * (R32 - R23); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); + omega = sgn_w * scale * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); } } else { double magnitude; From f97e55b85beaf9bd9a6cac6bb6ac28ba6c514316 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Oct 2021 21:05:18 -0400 Subject: [PATCH 095/169] update proxy functions to use the Adjoint and AdjointTranpose methods --- gtsam/geometry/tests/testPose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index fe527ab2e..94e2e558b 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -159,7 +159,7 @@ TEST(Pose3, Adjoint_jacobians) // Check jacobians Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function Adjoint_proxy = - [&](const Pose3& T, const Vector6& xi) { return T.AdjointMap() * xi; }; + [&](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); }; T.Adjoint(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi); @@ -198,7 +198,7 @@ TEST(Pose3, AdjointTranspose) Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function AdjointTranspose_proxy = [&](const Pose3& T, const Vector6& xi) { - return T.AdjointMap().transpose() * xi; + return T.AdjointTranspose(xi); }; T.AdjointTranspose(xi, actualH1, actualH2); From 2de623befccc6cce0b064737053e3ca4d9a72389 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Oct 2021 21:50:07 -0400 Subject: [PATCH 096/169] add docs explaining why pRw_H_t is the same as Rw_H_R --- gtsam/geometry/Pose3.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 27a5cf557..484fb9ca9 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -79,13 +79,19 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); - const Vector3 pRw = cross(t_, Rw, H_this ? &pRw_H_t : nullptr, - (H_this || H_xib) ? &pRw_H_Rw : nullptr); + // Since we use the Point3 version of cross, the jacobian of pRw wrpt t + // (pRw_H_t) needs special treatment as detailed below. + const Vector3 pRw = + cross(t_, Rw, boost::none, (H_this || H_xib) ? &pRw_H_Rw : nullptr); result.tail<3>() = pRw + Rv; // Jacobians if (H_this) { - pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason + // By applying the chain rule to the matrix-matrix product of [t]R, we can + // compute a simplified derivative which is the same as Rw_H_R. Details: + // https://github.com/borglab/gtsam/pull/885#discussion_r726591370 + pRw_H_t = Rw_H_R; + *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) .finished(); From 561037f073f075a99fbf60c8e666e3278e20f423 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Oct 2021 23:22:37 -0400 Subject: [PATCH 097/169] make threshold more lenient --- gtsam/geometry/tests/testPose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 94e2e558b..f0f2c0ccd 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -204,7 +204,7 @@ TEST(Pose3, AdjointTranspose) T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); T2.AdjointTranspose(xi, actualH1, actualH2); @@ -216,7 +216,7 @@ TEST(Pose3, AdjointTranspose) T3.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); } From 859c5f8d07abcd5f933bbb540b6281f3f30d04bb Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 13 Oct 2021 00:13:05 -0400 Subject: [PATCH 098/169] added new Python examples using iSAM2 --- python/gtsam/examples/Pose2ISAM2Example.py | 153 +++++++++++++++++++ python/gtsam/examples/Pose3ISAM2Example.py | 165 +++++++++++++++++++++ 2 files changed, 318 insertions(+) create mode 100644 python/gtsam/examples/Pose2ISAM2Example.py create mode 100644 python/gtsam/examples/Pose3ISAM2Example.py diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py new file mode 100644 index 000000000..ba2fbb283 --- /dev/null +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -0,0 +1,153 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Pose SLAM example using iSAM2 in the 2D plane. +Author: Jerred Chen, Yusuf Ali +Modelled after: + - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) + - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) +""" + +from __future__ import print_function +import math +import numpy as np +from numpy.random import multivariate_normal as mult_gauss +import gtsam +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + +def Pose2SLAM_ISAM2_plot(graph, current_estimate): + """ + Plots incremental state of the robot for 2D Pose SLAM using iSAM2 + Author: Jerred Chen + Based on version by: + - Ellon Paiva (Python), + - Duy Nguyen Ta and Frank Dellaert (MATLAB) + """ + marginals = gtsam.Marginals(graph, current_estimate) + + fig = plt.figure(0) + axes = fig.gca() + plt.cla() + + i = 1 + while current_estimate.exists(i): + gtsam_plot.plot_pose2(0, current_estimate.atPose2(i), 0.5, marginals.marginalCovariance(i)) + i += 1 + + plt.axis('equal') + axes.set_xlim(-1, 5) + axes.set_ylim(-1, 3) + plt.pause(1) + return marginals + + +def Pose2SLAM_ISAM2_example(): + plt.ion() + + def vector3(x, y, z): + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=float) + + # Although this example only uses linear measurements and Gaussian noise models, it is important + # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example + # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) + + # Create a Nonlinear factor graph as well as the data structure to hold state estimates. + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + + # iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # update calls are required to perform the relinearization. + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.1) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + # The ground truth odometry measurements (without noise) of the robot during the trajectory. + odom_arr = [(2, 0, 0), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2)] + + # The initial estimates for robot poses 2-5. Pose 1 is initialized by the prior. + # To demonstrate iSAM2 incremental optimization, poor initializations were intentionally inserted. + pose_est = [gtsam.Pose2(2.3, 0.1, -0.2), + gtsam.Pose2(4.1, 0.1, math.pi/2), + gtsam.Pose2(4.0, 2.0, math.pi), + gtsam.Pose2(2.1, 2.1, -math.pi/2), + gtsam.Pose2(1.9, -0.2, 0.2)] + + graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) + initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + + def determine_loop_closure(odom, current_estimate, xy_tol=0.6, theta_tol=0.3): + """ + Simple brute force approach which iterates through previous states + and checks for loop closure. + ### Parameters: + odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) + measurement in the body frame. + current_estimate: (gtsam.Values) The current estimates computed by iSAM2. + xy_tol: (double) Optional argument for the x-y measurement tolerance. + theta_tol: (double) Optional argument for the theta measurement tolerance. + ### Returns: + k: (int) The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + prev_est = current_estimate.atPose2(i+1) + rotated_odom = prev_est.rotation().matrix() @ odom[:2] + curr_xy = np.array([prev_est.x() + rotated_odom[0], + prev_est.y() + rotated_odom[1]]) + curr_theta = prev_est.theta() + odom[2] + for k in range(1, i+1): + pose_xy = np.array([current_estimate.atPose2(k).x(), + current_estimate.atPose2(k).y()]) + pose_theta = current_estimate.atPose2(k).theta() + if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ + (abs(pose_theta - curr_theta) <= theta_tol): + return k + + current_estimate = None + for i in range(len(odom_arr)): + + odom_x, odom_y, odom_theta = odom_arr[i] + # Odometry measurement that is received by the robot and corrupted by gaussian noise + odom = mult_gauss(odom_arr[i], ODOMETRY_NOISE.covariance()) + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state + loop = determine_loop_closure(odom, current_estimate) + if loop: + graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE)) + else: + graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE)) + initial_estimate.insert(i + 2, pose_est[i]) + # Incremental update to iSAM2's internal Baye's tree, which only optimizes upon the added variables + # as well as any affected variables + isam.update(graph, initial_estimate) + # Another iSAM2 update can be performed for additional optimization + isam.update() + current_estimate = isam.calculateEstimate() + print("*"*50) + print(f"Inference after State {i+1}:") + print(current_estimate) + marginals = Pose2SLAM_ISAM2_plot(graph, current_estimate) + initial_estimate.clear() + + for i in range(1, len(odom_arr)+1): + print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") + + plt.ioff() + plt.show() + + +if __name__ == "__main__": + Pose2SLAM_ISAM2_example() diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py new file mode 100644 index 000000000..26d82ae33 --- /dev/null +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -0,0 +1,165 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Pose SLAM example using iSAM2 in 3D space. +Author: Jerred Chen +Modelled after version by: + - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) +""" +from __future__ import print_function +import gtsam +from gtsam import Pose3, Rot3 +from gtsam.symbol_shorthand import X +import gtsam.utils.plot as gtsam_plot +import numpy as np +from numpy import cos, sin, pi +from numpy.random import multivariate_normal as mult_gauss +import matplotlib.pyplot as plt + + +def Pose3SLAM_ISAM2_plot(graph, current_estimate): + """ + Plots incremental state of the robot for 3D Pose SLAM using iSAM2 + Author: Jerred Chen + Based on version by: + - Ellon Paiva (Python), + - Duy Nguyen Ta and Frank Dellaert (MATLAB) + """ + marginals = gtsam.Marginals(graph, current_estimate) + + fig = plt.figure(0) + axes = fig.gca(projection='3d') + plt.cla() + + i = 0 + while current_estimate.exists(X(i)): + gtsam_plot.plot_pose3(0, current_estimate.atPose3(X(i)), 10, + marginals.marginalCovariance(X(i))) + i += 1 + + axes.set_xlim3d(-30, 45) + axes.set_ylim3d(-30, 45) + axes.set_zlim3d(-30, 45) + plt.pause(1) + + return marginals + + +def createPoses(): + """ + Creates ground truth poses of the robot. + """ + P0 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + P1 = np.array([[0, -1, 0, 15], + [1, 0, 0, 15], + [0, 0, 1, 20], + [0, 0, 0, 1]]) + P2 = np.array([[cos(pi/4), 0, sin(pi/4), 30], + [0, 1, 0, 30], + [-sin(pi/4), 0, cos(pi/4), 30], + [0, 0, 0, 1]]) + P3 = np.array([[0, 1, 0, 30], + [0, 0, -1, 0], + [-1, 0, 0, -15], + [0, 0, 0, 1]]) + P4 = np.array([[-1, 0, 0, 0], + [0, -1, 0, -10], + [0, 0, 1, -10], + [0, 0, 0, 1]]) + P5 = P0[:] + + return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)] + +def Pose3_ISAM2_example(): + """ + """ + plt.ion() + + def vector6(x, y, z, a, b, c): + """Create 6d double numpy array.""" + return np.array([x, y, z, a, b, c], dtype=float) + + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3)) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2)) + + poses = createPoses() + + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.1) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + # Add prior factor to the first pose + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], PRIOR_NOISE)) + initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3( + gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + + def determine_loop_closure(odom, current_estimate, xyz_tol=0.6, rot_tol=0.3): + """ + Simple brute force approach which iterates through previous states + and checks for loop closure. + ### Parameters: + odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation + measurement in the body frame, [roll, pitch, yaw, x, y, z] + current_estimate: (gtsam.Values) The current estimates computed by iSAM2. + xyz_tol: (double) Optional argument for the translational tolerance. + rot_tol: (double) Optional argument for the rotational tolerance. + ### Returns: + k: (int) The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + rot = Rot3.RzRyRx(odom[:3]) + odom_tf = Pose3(rot, odom[3:6].reshape(-1,1)) + prev_est = current_estimate.atPose3(X(i-1)) + curr_est = prev_est.transformPoseFrom(odom_tf) + for k in range(i): + pose = current_estimate.atPose3(X(k)) + if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \ + (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): + return k + + current_estimate = None + for i in range(1, len(poses)): + # The odometry "ground truth" measurement between poses + odom_tf = poses[i-1].transformPoseTo(poses[i]) + odom_xyz = odom_tf.x(), odom_tf.y(), odom_tf.z() + odom_rpy = odom_tf.rotation().rpy() + # Odometry measurement that is received by the robot and corrupted by gaussian noise + measurement = mult_gauss(np.hstack((odom_rpy,odom_xyz)), ODOMETRY_NOISE.covariance()) + loop = determine_loop_closure(measurement, current_estimate) + if loop is not None: + graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(loop), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) + else: + graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(i), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) + # Intentionally insert poor initializations + initial_estimate.insert(X(i), poses[i].compose(gtsam.Pose3( + gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + isam.update(graph, initial_estimate) + isam.update() + current_estimate = isam.calculateEstimate() + print("*"*50) + print(f"Inference after State {i}:") + print(current_estimate) + marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) + initial_estimate.clear() + i = 0 + while current_estimate.exists(X(i)): + print(f"X{i} covariance:\n{marginals.marginalCovariance(X(i))}\n") + i += 1 + + plt.ioff() + plt.show() + +if __name__ == '__main__': + Pose3_ISAM2_example() From 47c45c633f607790940859589292c03d35f4ceb4 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 13 Oct 2021 11:03:40 -0400 Subject: [PATCH 099/169] Added minor comments for documentation --- python/gtsam/examples/Pose2ISAM2Example.py | 7 +++++-- python/gtsam/examples/Pose3ISAM2Example.py | 22 +++++++++++++++++++--- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index ba2fbb283..6710dc706 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -48,6 +48,8 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate): def Pose2SLAM_ISAM2_example(): + """ + """ plt.ion() def vector3(x, y, z): @@ -93,6 +95,7 @@ def Pose2SLAM_ISAM2_example(): """ Simple brute force approach which iterates through previous states and checks for loop closure. + Author: Jerred ### Parameters: odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) measurement in the body frame. @@ -119,7 +122,7 @@ def Pose2SLAM_ISAM2_example(): current_estimate = None for i in range(len(odom_arr)): - + # The "ground truth" change between poses odom_x, odom_y, odom_theta = odom_arr[i] # Odometry measurement that is received by the robot and corrupted by gaussian noise odom = mult_gauss(odom_arr[i], ODOMETRY_NOISE.covariance()) @@ -141,7 +144,7 @@ def Pose2SLAM_ISAM2_example(): print(current_estimate) marginals = Pose2SLAM_ISAM2_plot(graph, current_estimate) initial_estimate.clear() - + # Print the final covariance matrix for each pose after completing inference for i in range(1, len(odom_arr)+1): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 26d82ae33..a15424d04 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -10,6 +10,7 @@ Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen Modelled after version by: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) + - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ from __future__ import print_function import gtsam @@ -80,6 +81,8 @@ def createPoses(): def Pose3_ISAM2_example(): """ + Perform 3D SLAM given ground truth poses as well as simple + loop closure detection. """ plt.ion() @@ -87,19 +90,27 @@ def Pose3_ISAM2_example(): """Create 6d double numpy array.""" return np.array([x, y, z, a, b, c], dtype=float) + # Although this example only uses linear measurements and Gaussian noise models, it is important + # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example + # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3)) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2)) + # Create the ground truth poses of the robot trajectory. poses = createPoses() + # iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) parameters.setRelinearizeSkip(1) isam = gtsam.ISAM2(parameters) + # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() - initial_estimate = gtsam.Values() - # Add prior factor to the first pose + initial_estimate = gtsam.Values() + + # Add prior factor to the first pose with intentionally poor initial estimate graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], PRIOR_NOISE)) initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) @@ -108,6 +119,7 @@ def Pose3_ISAM2_example(): """ Simple brute force approach which iterates through previous states and checks for loop closure. + Author: Jerred Chen ### Parameters: odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation measurement in the body frame, [roll, pitch, yaw, x, y, z] @@ -131,13 +143,14 @@ def Pose3_ISAM2_example(): current_estimate = None for i in range(1, len(poses)): - # The odometry "ground truth" measurement between poses + # The "ground truth" change between poses odom_tf = poses[i-1].transformPoseTo(poses[i]) odom_xyz = odom_tf.x(), odom_tf.y(), odom_tf.z() odom_rpy = odom_tf.rotation().rpy() # Odometry measurement that is received by the robot and corrupted by gaussian noise measurement = mult_gauss(np.hstack((odom_rpy,odom_xyz)), ODOMETRY_NOISE.covariance()) loop = determine_loop_closure(measurement, current_estimate) + # If loop closure is detected, then adds a constraint between existing states in the factor graph if loop is not None: graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(loop), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) else: @@ -145,7 +158,9 @@ def Pose3_ISAM2_example(): # Intentionally insert poor initializations initial_estimate.insert(X(i), poses[i].compose(gtsam.Pose3( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + # Performs iSAM2 incremental update based on newly added factors isam.update(graph, initial_estimate) + # Additional iSAM2 optimization isam.update() current_estimate = isam.calculateEstimate() print("*"*50) @@ -153,6 +168,7 @@ def Pose3_ISAM2_example(): print(current_estimate) marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) initial_estimate.clear() + # Print the final covariance matrix for each pose after completing inference i = 0 while current_estimate.exists(X(i)): print(f"X{i} covariance:\n{marginals.marginalCovariance(X(i))}\n") From 35061ca1356b081f94712ec5f343fb9592cf0d21 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 13:46:33 -0400 Subject: [PATCH 100/169] simplify logic of biggest diagonal --- gtsam/geometry/SO3.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 0cb6c93d1..37ea3840b 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -262,20 +262,20 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (tr + 1.0 < 1e-4) { - std::vector diags = {R11, R22, R33}; - size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end())); - if (max_elem == 2) { + if (R33 > R22 && R33 > R11) { + // R33 is the largest diagonal const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R33); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12); omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); - } else if (max_elem == 1) { + } else if (R22 > R11) { + // R22 is the largest diagonal const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R22); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { - // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit + // R33 is the largest diagonal const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); @@ -283,8 +283,9 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { } } else { double magnitude; - const double tr_3 = tr - 3.0; // always negative + const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal if (tr_3 < -1e-6) { + // this is the normal case -1 < trace < 3 double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); } else { From 48cd9794daaa87517d217bd714118202bda1f427 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 13:48:41 -0400 Subject: [PATCH 101/169] Fix typo --- gtsam/geometry/SO3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 37ea3840b..890bcd206 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -275,7 +275,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { - // R33 is the largest diagonal + // R11 is the largest diagonal const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); From 80ebd5a63b23736d8cb81e8e55833a905baabe75 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 13 Oct 2021 19:41:04 -0400 Subject: [PATCH 102/169] Add specific examples to stress-test the log map --- python/gtsam/tests/test_Rot3.py | 1998 ++++++++++++++++++++++++++++++- 1 file changed, 1997 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index 90c80ab31..a1ce01ba2 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -17,6 +17,1990 @@ from gtsam import Rot3 from gtsam.utils.test_case import GtsamTestCase +R1_R2_pairs = [ + ( + [ + [0.994283, -0.10356, 0.0260251], + [0.102811, 0.994289, 0.0286205], + [-0.0288404, -0.0257812, 0.999251], + ], + [ + [-0.994235, 0.0918291, -0.0553602], + [-0.0987317, -0.582632, 0.806718], + [0.0418251, 0.807532, 0.588339], + ], + ), + ( + [ + [0.999823, -0.000724729, 0.0187896], + [0.00220672, 0.996874, -0.0789728], + [-0.0186736, 0.0790003, 0.9967], + ], + [ + [-0.99946, -0.0155217, -0.0289749], + [-0.0306159, 0.760422, 0.648707], + [0.0119641, 0.649244, -0.760487], + ], + ), + ( + [ + [0.999976, 0.00455542, -0.00529608], + [-0.00579633, 0.964214, -0.265062], + [0.00389908, 0.265086, 0.964217], + ], + [ + [-0.999912, -0.0123323, -0.00489179], + [-0.00739095, 0.21159, 0.977331], + [-0.0110179, 0.977281, -0.211663], + ], + ), + ( + [ + [0.998801, 0.0449026, 0.019479], + [-0.0448727, 0.998991, -0.00197348], + [-0.0195479, 0.00109704, 0.999808], + ], + [ + [-0.999144, -0.0406154, -0.00800012], + [0.0406017, -0.999174, 0.00185875], + [-0.00806909, 0.00153352, 0.999966], + ], + ), + ( + [ + [0.587202, 0.00034062, -0.80944], + [0.394859, 0.872825, 0.286815], + [0.706597, -0.488034, 0.51239], + ], + [ + [-0.999565, -0.028095, -0.00905389], + [0.0192863, -0.853838, 0.520182], + [-0.0223455, 0.519782, 0.854007], + ], + ), + ( + [ + [0.998798, 0.0370584, 0.0320815], + [-0.0355966, 0.998353, -0.0449943], + [-0.033696, 0.0437982, 0.998472], + ], + [ + [-0.999942, -0.010745, -0.00132538], + [-0.000998705, -0.0304045, 0.999538], + [-0.0107807, 0.999481, 0.0303914], + ], + ), + ( + [ + [0.998755, 0.00708291, -0.0493744], + [-0.00742097, 0.99995, -0.00666709], + [0.0493247, 0.0070252, 0.998758], + ], + [ + [-0.998434, 0.0104672, 0.0549825], + [0.0115323, 0.999751, 0.0190859], + [-0.0547691, 0.01969, -0.998307], + ], + ), + ( + [ + [0.990471, 0.0997485, -0.0949595], + [-0.117924, 0.970427, -0.210631], + [0.0711411, 0.219822, 0.972943], + ], + [ + [-0.99192, -0.125627, 0.0177888], + [0.126478, -0.967866, 0.217348], + [-0.0100874, 0.217839, 0.975933], + ], + ), + ( + [ + [-0.780894, -0.578319, -0.236116], + [0.34478, -0.0838381, -0.934932], + [0.520894, -0.811491, 0.264862], + ], + [ + [-0.99345, 0.00261746, -0.114244], + [-0.112503, 0.152922, 0.981815], + [0.0200403, 0.988236, -0.151626], + ], + ), + ( + [ + [0.968425, 0.0466097, 0.244911], + [-0.218867, 0.629346, 0.745668], + [-0.119378, -0.775726, 0.619676], + ], + [ + [-0.971208, 0.00666431, -0.238143], + [0.0937886, 0.929584, -0.35648], + [0.218998, -0.368551, -0.903444], + ], + ), + ( + [ + [0.998512, 0.0449168, -0.0309146], + [-0.0344032, 0.958823, 0.281914], + [0.0423043, -0.280431, 0.958941], + ], + [ + [-0.999713, 0.00732431, 0.0228168], + [-0.00759688, 0.806166, -0.59164], + [-0.0227275, -0.591644, -0.805879], + ], + ), + ( + [ + [0.981814, 0.00930728, 0.189617], + [-0.0084101, 0.999949, -0.00553563], + [-0.189659, 0.00384026, 0.981843], + ], + [ + [-0.981359, 0.00722349, -0.192051], + [0.00148564, 0.999549, 0.0300036], + [0.192182, 0.0291591, -0.980927], + ], + ), + ( + [ + [0.972544, -0.215591, 0.0876242], + [0.220661, 0.973915, -0.0529018], + [-0.0739333, 0.0707846, 0.994748], + ], + [ + [-0.971294, 0.215675, -0.100371], + [-0.23035, -0.747337, 0.62324], + [0.0594069, 0.628469, 0.775564], + ], + ), + ( + [ + [0.989488, 0.0152447, 0.143808], + [-0.0160974, 0.999859, 0.00476753], + [-0.143715, -0.00703235, 0.989594], + ], + [ + [-0.988492, 0.0124362, -0.150766], + [0.00992423, 0.999799, 0.0174037], + [0.150952, 0.0157072, -0.988417], + ], + ), + ( + [ + [0.99026, 0.109934, -0.0854388], + [-0.0973012, 0.985345, 0.140096], + [0.099588, -0.130418, 0.986445], + ], + [ + [-0.994239, 0.0206112, 0.1052], + [0.0227944, 0.999548, 0.0195944], + [-0.104748, 0.0218794, -0.994259], + ], + ), + ( + [ + [0.988981, 0.132742, -0.0655406], + [-0.113134, 0.963226, 0.243712], + [0.0954813, -0.233612, 0.96763], + ], + [ + [-0.989473, -0.144453, 0.00888153], + [0.112318, -0.727754, 0.67658], + [-0.0912697, 0.670455, 0.736317], + ], + ), + ( + [ + [0.13315, -0.722685, 0.678231], + [0.255831, 0.686195, 0.680946], + [-0.957508, 0.0828446, 0.276252], + ], + [ + [-0.233019, 0.0127274, -0.97239], + [-0.0143824, 0.99976, 0.0165321], + [0.972367, 0.0178377, -0.23278], + ], + ), + ( + [ + [0.906305, -0.0179617, -0.422243], + [0.0246095, 0.999644, 0.0102984], + [0.421908, -0.0197247, 0.906424], + ], + [ + [-0.90393, 0.0136293, 0.427466], + [0.0169755, 0.999848, 0.0040176], + [-0.427346, 0.0108879, -0.904024], + ], + ), + ( + [ + [0.999808, 0.0177784, -0.00826505], + [-0.0177075, 0.999806, 0.00856939], + [0.0084158, -0.00842139, 0.999929], + ], + [ + [-0.999901, -0.0141114, 0.00072392], + [0.00130602, -0.0413336, 0.999145], + [-0.0140699, 0.999047, 0.0413473], + ], + ), + ( + [ + [0.985811, -0.161425, 0.0460375], + [0.154776, 0.980269, 0.12295], + [-0.0649764, -0.11408, 0.991344], + ], + [ + [-0.985689, 0.137931, -0.09692], + [-0.162627, -0.626622, 0.762168], + [0.0443951, 0.767022, 0.640085], + ], + ), + ( + [ + [0.956652, -0.0116044, 0.291001], + [0.05108, 0.990402, -0.128428], + [-0.286718, 0.137726, 0.948064], + ], + [ + [-0.956189, 0.00996594, -0.292585], + [-0.0397033, 0.985772, 0.16333], + [0.29005, 0.167791, -0.942189], + ], + ), + ( + [ + [0.783763, -0.0181248, -0.620796], + [-0.0386421, 0.996214, -0.0778717], + [0.619857, 0.0850218, 0.780095], + ], + [ + [-0.780275, 0.0093644, 0.625368], + [-0.0221791, 0.998845, -0.0426297], + [-0.625045, -0.0471329, -0.779165], + ], + ), + ( + [ + [0.890984, 0.0232464, -0.453439], + [-0.0221215, 0.999725, 0.00778511], + [0.453495, 0.00309433, 0.891253], + ], + [ + [-0.890178, 0.0290103, 0.45469], + [0.0337152, 0.999429, 0.0022403], + [-0.454366, 0.0173244, -0.890648], + ], + ), + ( + [ + [0.998177, -0.0119546, 0.0591504], + [0.00277494, 0.988238, 0.152901], + [-0.0602825, -0.152458, 0.98647], + ], + [ + [-0.997444, 0.00871865, -0.0709414], + [0.0197108, 0.987598, -0.155762], + [0.0687035, -0.156762, -0.985246], + ], + ), + ( + [ + [0.985214, 0.164929, 0.0463837], + [-0.166966, 0.984975, 0.0441225], + [-0.0384096, -0.0512146, 0.997949], + ], + [ + [-0.999472, -0.000819214, -0.0325087], + [-0.00344291, 0.99673, 0.0807324], + [0.0323362, 0.0808019, -0.996206], + ], + ), + ( + [ + [0.998499, 0.0465241, 0.0288955], + [-0.0454764, 0.99832, -0.0359142], + [-0.0305178, 0.0345463, 0.998937], + ], + [ + [-0.999441, 0.00412484, -0.0332105], + [0.00374685, 0.999928, 0.0114307], + [0.0332552, 0.0112999, -0.999384], + ], + ), + ( + [ + [0.10101, -0.943239, -0.316381], + [0.841334, -0.0887423, 0.533182], + [-0.530994, -0.320039, 0.784615], + ], + [ + [-0.725665, 0.0153749, -0.687878], + [-0.304813, 0.889109, 0.34143], + [0.616848, 0.457438, -0.640509], + ], + ), + ( + [ + [0.843428, 0.174952, 0.507958], + [0.0435637, 0.920106, -0.389239], + [-0.535473, 0.350423, 0.768422], + ], + [ + [-0.835464, 0.0040872, -0.549533], + [0.00227251, 0.999989, 0.00398241], + [0.549543, 0.00207845, -0.835464], + ], + ), + ( + [ + [0.999897, -0.0142888, -0.00160177], + [0.0141561, 0.997826, -0.064364], + [0.00251798, 0.0643346, 0.997925], + ], + [ + [-0.999956, 0.00898988, 0.00296485], + [0.00900757, 0.999941, 0.00601779], + [-0.00291058, 0.00604429, -0.999978], + ], + ), + ( + [ + [0.999477, -0.0204859, 0.0250096], + [0.0126204, 0.959462, 0.281557], + [-0.0297637, -0.281094, 0.959219], + ], + [ + [-0.999384, 0.0172602, -0.0305795], + [-0.0254425, 0.24428, 0.969371], + [0.0242012, 0.969551, -0.24369], + ], + ), + ( + [ + [0.984597, 0.173474, -0.0218106], + [-0.15145, 0.783891, -0.602145], + [-0.0873592, 0.596173, 0.798089], + ], + [ + [-0.998608, -0.0432858, 0.0301827], + [-0.00287128, 0.615692, 0.787983], + [-0.0526917, 0.786797, -0.61496], + ], + ), + ( + [ + [0.917099, -0.3072, 0.254083], + [0.303902, 0.951219, 0.0531566], + [-0.258018, 0.0284665, 0.965721], + ], + [ + [-0.931191, 0.347008, -0.111675], + [-0.352102, -0.77686, 0.522032], + [0.0943935, 0.52543, 0.845586], + ], + ), + ( + [ + [0.991706, 0.0721037, -0.106393], + [-0.0995017, 0.954693, -0.280464], + [0.0813505, 0.288725, 0.95395], + ], + [ + [-0.995306, 0.00106317, 0.0967833], + [0.0167662, 0.986717, 0.161583], + [-0.0953259, 0.162447, -0.982103], + ], + ), + ( + [ + [0.997078, 0.0478273, -0.0595641], + [-0.0348316, 0.978617, 0.202719], + [0.067986, -0.200052, 0.977424], + ], + [ + [-0.997925, -0.0439691, 0.0470461], + [0.0643829, -0.695474, 0.715663], + [0.00125305, 0.717207, 0.696861], + ], + ), + ( + [ + [0.972749, -0.0233882, -0.230677], + [0.0255773, 0.999652, 0.00650349], + [0.230445, -0.0122264, 0.973009], + ], + [ + [-0.973286, 0.0147558, 0.229126], + [0.0145644, 0.999891, -0.00252631], + [-0.229138, 0.000878362, -0.973394], + ], + ), + ( + [ + [0.999271, 0.00700481, 0.0375381], + [-0.0348202, 0.57069, 0.820427], + [-0.0156757, -0.821136, 0.570517], + ], + [ + [-0.999805, -0.0198049, 0.000539906], + [0.0179848, -0.89584, 0.444015], + [-0.00831113, 0.443938, 0.89602], + ], + ), + ( + [ + [0.975255, -0.0207895, 0.220104], + [0.0252764, 0.999526, -0.0175888], + [-0.219634, 0.022717, 0.975318], + ], + [ + [-0.975573, 0.0128154, -0.219304], + [0.0106554, 0.999882, 0.0110292], + [0.219419, 0.00842303, -0.975594], + ], + ), + ( + [ + [-0.433961, -0.658151, -0.615236], + [0.610442, -0.717039, 0.336476], + [-0.6626, -0.229548, 0.71293], + ], + [ + [-0.998516, -0.00675119, -0.054067], + [-0.00405539, 0.99875, -0.0498174], + [0.0543358, -0.0495237, -0.997296], + ], + ), + ( + [ + [0.942764, -0.0126807, -0.333221], + [-0.0017175, 0.999079, -0.042879], + [0.333458, 0.0409971, 0.941873], + ], + [ + [-0.942228, -0.0109444, 0.334798], + [0.0110573, 0.997905, 0.0637396], + [-0.334794, 0.0637589, -0.940133], + ], + ), + ( + [ + [0.962038, 0.0147987, 0.272515], + [-0.0185974, 0.999762, 0.0113615], + [-0.272283, -0.0159982, 0.962084], + ], + [ + [-0.959802, 0.0113708, -0.280451], + [0.00982126, 0.999928, 0.00692958], + [0.280509, 0.00389678, -0.959845], + ], + ), + ( + [ + [0.998414, 0.0139348, 0.0545528], + [-0.0226877, 0.986318, 0.163283], + [-0.0515311, -0.164262, 0.98507], + ], + [ + [-0.998641, -0.000695993, -0.0521343], + [0.0182534, 0.931965, -0.362087], + [0.0488394, -0.362547, -0.930686], + ], + ), + ( + [ + [0.999705, -0.0234518, -0.00633743], + [0.0235916, 0.999458, 0.0229643], + [0.00579544, -0.023107, 0.999716], + ], + [ + [-0.999901, 0.004436, 0.0133471], + [-0.00306106, 0.85758, -0.514342], + [-0.0137278, -0.514332, -0.857481], + ], + ), + ( + [ + [0.998617, -0.051704, 0.0094837], + [0.0484292, 0.975079, 0.216506], + [-0.0204416, -0.215748, 0.976235], + ], + [ + [-0.999959, -0.00295958, -0.00862907], + [-0.00313279, 0.999792, 0.0201361], + [0.00856768, 0.0201625, -0.999761], + ], + ), + ( + [ + [0.999121, 0.0345472, -0.023733], + [-0.0333175, 0.998174, 0.0503881], + [0.0254304, -0.0495531, 0.998448], + ], + [ + [-0.999272, -0.0337466, 0.0178065], + [0.0200629, -0.0677624, 0.9975], + [-0.0324556, 0.997131, 0.0683898], + ], + ), + ( + [ + [0.989017, 0.139841, -0.0478525], + [-0.131355, 0.683201, -0.718319], + [-0.0677572, 0.716715, 0.694067], + ], + [ + [-0.995236, 0.00457798, 0.097401], + [0.097488, 0.0258334, 0.994902], + [0.00203912, 0.999657, -0.0261574], + ], + ), + ( + [ + [0.961528, 0.249402, 0.11516], + [-0.204522, 0.9298, -0.306009], + [-0.183395, 0.270684, 0.945038], + ], + [ + [-0.999566, -0.0233216, 0.0180679], + [0.012372, 0.224583, 0.974377], + [-0.0267822, 0.974177, -0.224197], + ], + ), + ( + [ + [0.865581, 0.0252563, -0.500131], + [0.0302583, 0.994265, 0.102578], + [0.499853, -0.103923, 0.859853], + ], + [ + [-0.866693, 0.0042288, 0.498824], + [0.0129331, 0.999818, 0.0139949], + [-0.498674, 0.0185807, -0.866591], + ], + ), + ( + [ + [0.998999, -0.0213419, -0.0393009], + [-0.0007582, 0.870578, -0.492031], + [0.0447153, 0.491568, 0.86969], + ], + [ + [-0.999207, -0.0184688, 0.0353073], + [0.00153266, 0.867625, 0.497218], + [-0.0398164, 0.496876, -0.866908], + ], + ), + ( + [ + [0.96567, -0.00482973, 0.259728], + [0.00349956, 0.999978, 0.00558359], + [-0.259749, -0.00448297, 0.965666], + ], + [ + [-0.962691, -0.00113074, -0.270609], + [-5.93716e-05, 0.999992, -0.00396767], + [0.270612, -0.00380337, -0.962683], + ], + ), + ( + [ + [0.948799, 0.287027, -0.131894], + [-0.300257, 0.949181, -0.0943405], + [0.0981135, 0.129112, 0.986764], + ], + [ + [-0.993593, -0.0406684, 0.105449], + [-0.0506857, 0.994269, -0.0941326], + [-0.101017, -0.0988741, -0.98996], + ], + ), + ( + [ + [0.998935, 0.0451118, 0.0097202], + [-0.0418086, 0.973879, -0.223183], + [-0.0195345, 0.222539, 0.974728], + ], + [ + [-0.999821, 0.00821522, -0.0170658], + [0.00742187, 0.998912, 0.046048], + [0.0174255, 0.0459131, -0.998794], + ], + ), + ( + [ + [0.99577, 0.00458459, 0.0917705], + [-0.00244288, 0.999722, -0.0234365], + [-0.0918524, 0.0231131, 0.995504], + ], + [ + [-0.995956, 0.0137721, -0.0887945], + [0.0122932, 0.999777, 0.0171801], + [0.0890113, 0.0160191, -0.995903], + ], + ), + ( + [ + [0.97931, 0.0219899, 0.201169], + [-0.0159226, 0.99937, -0.0317288], + [-0.201739, 0.0278692, 0.979043], + ], + [ + [-0.980952, 0.00507266, -0.19419], + [0.00310821, 0.999941, 0.010419], + [0.194231, 0.00961706, -0.98091], + ], + ), + ( + [ + [0.999616, 0.00550326, -0.0271537], + [-0.0048286, 0.99968, 0.0248495], + [0.0272817, -0.0247088, 0.999322], + ], + [ + [-0.999689, -0.00054899, 0.0249588], + [-0.00125497, 0.999599, -0.0282774], + [-0.0249333, -0.0282998, -0.999289], + ], + ), + ( + [ + [0.998036, -0.00755259, -0.0621791], + [0.0417502, 0.820234, 0.570502], + [0.0466927, -0.571978, 0.818939], + ], + [ + [-0.999135, -0.0278203, 0.0309173], + [-0.00855238, 0.864892, 0.501886], + [-0.0407029, 0.501187, -0.864382], + ], + ), + ( + [ + [0.958227, 0.00271545, 0.285997], + [-0.00426128, 0.999979, 0.00478282], + [-0.285979, -0.00580174, 0.958218], + ], + [ + [-0.958726, 0.011053, -0.284121], + [0.0138068, 0.999875, -0.00769161], + [0.284001, -0.0112968, -0.958759], + ], + ), + ( + [ + [-0.804547, -0.48558, -0.341929], + [0.517913, -0.855425, -0.00382581], + [-0.290637, -0.180168, 0.939718], + ], + [ + [0.993776, -0.0469383, -0.101033], + [-0.110087, -0.274676, -0.955214], + [0.0170842, 0.96039, -0.278134], + ], + ), + ( + [ + [0.991875, -0.0022313, -0.127195], + [-0.00198041, 0.999454, -0.0329762], + [0.127199, 0.0329602, 0.991329], + ], + [ + [-0.992632, -0.0090772, 0.120844], + [-0.00870494, 0.999956, 0.00360636], + [-0.120871, 0.00252786, -0.992667], + ], + ), + ( + [ + [0.999305, -0.0252534, 0.0274367], + [0.026144, 0.999126, -0.0326002], + [-0.0265895, 0.0332948, 0.999092], + ], + [ + [-0.999314, -0.0038532, -0.0368519], + [-0.00441323, 0.999876, 0.0151263], + [0.036789, 0.0152787, -0.999207], + ], + ), + ( + [ + [0.999843, -0.00958823, 0.0148803], + [0.00982469, 0.999825, -0.0159002], + [-0.0147253, 0.0160439, 0.999763], + ], + [ + [-0.999973, 0.00673608, -0.00308692], + [-0.0067409, -0.999977, 0.00116827], + [-0.00307934, 0.00119013, 0.999995], + ], + ), + ( + [ + [0.981558, -0.00727741, 0.191028], + [-0.00866166, 0.996556, 0.0824708], + [-0.190971, -0.0826044, 0.978114], + ], + [ + [-0.980202, 0.0179519, -0.197188], + [0.00957606, 0.999014, 0.0433472], + [0.197772, 0.0406008, -0.979408], + ], + ), + ( + [ + [0.966044, 0.0143709, 0.257977], + [-0.0157938, 0.999869, 0.00344404], + [-0.257894, -0.00740153, 0.966145], + ], + [ + [-0.965532, 0.0100318, -0.260094], + [0.00950897, 0.999949, 0.00326797], + [0.260113, 0.000682242, -0.965579], + ], + ), + ( + [ + [0.999965, 0.00727991, -0.00412134], + [-0.00802642, 0.973769, -0.227397], + [0.00235781, 0.227422, 0.973794], + ], + [ + [-0.999877, 0.00698241, 0.0141441], + [0.0103867, 0.966295, 0.257228], + [-0.0118713, 0.257343, -0.966248], + ], + ), + ( + [ + [0.951385, -0.0297966, 0.306561], + [-0.0314555, 0.980706, 0.19294], + [-0.306395, -0.193204, 0.932092], + ], + [ + [-0.99981, 0.00389172, -0.0191159], + [-0.00386326, -0.999991, -0.00150593], + [-0.0191215, -0.00143146, 0.999816], + ], + ), + ( + [ + [0.986772, -0.120673, 0.10825], + [0.0543962, 0.875511, 0.480126], + [-0.152713, -0.467887, 0.870495], + ], + [ + [-0.991246, 0.125848, -0.0399414], + [-0.129021, -0.85897, 0.495507], + [0.0280503, 0.496321, 0.867686], + ], + ), + ( + [ + [-0.804799, -0.588418, 0.0778637], + [-0.514399, 0.756902, 0.403104], + [-0.296129, 0.284365, -0.911836], + ], + [ + [0.98676, -0.0939473, 0.132227], + [0.162179, 0.557277, -0.814336], + [0.0028177, 0.824995, 0.565135], + ], + ), + ( + [ + [0.878935, 0.115231, 0.462813], + [0.0845639, 0.917349, -0.388998], + [-0.469386, 0.381041, 0.796546], + ], + [ + [-0.869533, 0.00193279, -0.493873], + [-0.00419575, 0.999927, 0.0113007], + [0.493859, 0.0118986, -0.869462], + ], + ), + ( + [ + [0.951881, 0.20828, 0.224816], + [-0.305582, 0.700797, 0.644595], + [-0.023294, -0.682277, 0.730722], + ], + [ + [-0.999787, 0.0141074, -0.0151097], + [-0.000971554, 0.698061, 0.716038], + [0.0206489, 0.7159, -0.697898], + ], + ), + ( + [ + [0.999538, 0.0192173, 0.0235334], + [-0.0189064, 0.999732, -0.0133635], + [-0.0237839, 0.0129124, 0.999634], + ], + [ + [-0.999807, 0.00286378, -0.0194776], + [0.0026258, 0.999922, 0.0122308], + [0.0195111, 0.0121774, -0.999736], + ], + ), + ( + [ + [0.998468, 0.041362, -0.0367422], + [-0.0364453, 0.991404, 0.125658], + [0.0416238, -0.124127, 0.991393], + ], + [ + [-0.997665, -0.0658235, 0.0183602], + [0.0216855, -0.0501652, 0.998507], + [-0.064804, 0.99657, 0.0514739], + ], + ), + ( + [ + [0.995563, 0.0493669, 0.0801057], + [-0.0272233, 0.966027, -0.257002], + [-0.0900717, 0.253681, 0.963085], + ], + [ + [-0.999228, -0.034399, -0.0190572], + [0.0250208, -0.929986, 0.366743], + [-0.0303386, 0.365984, 0.930127], + ], + ), + ( + [ + [0.952898, 0.0122933, 0.303043], + [-0.00568444, 0.999727, -0.0226807], + [-0.303239, 0.0198898, 0.952707], + ], + [ + [-0.951155, 0.0127759, -0.308452], + [0.000612627, 0.999219, 0.0394978], + [0.308716, 0.0373795, -0.95042], + ], + ), + ( + [ + [0.923096, -0.000313887, 0.38457], + [0.00948258, 0.999714, -0.0219453], + [-0.384454, 0.0239044, 0.922835], + ], + [ + [-0.922662, -0.00403523, -0.385589], + [-0.0119834, 0.999762, 0.0182116], + [0.385424, 0.0214239, -0.922491], + ], + ), + ( + [ + [0.991575, 0.0945042, -0.0885834], + [-0.10112, 0.99216, -0.0734349], + [0.080949, 0.0817738, 0.993358], + ], + [ + [-0.990948, -0.127974, 0.0405639], + [0.096351, -0.467557, 0.878697], + [-0.0934839, 0.874651, 0.475655], + ], + ), + ( + [ + [0.997148, 0.010521, 0.0747407], + [-0.0079726, 0.999379, -0.034313], + [-0.0750553, 0.0336192, 0.996612], + ], + [ + [-0.996543, 0.00988805, -0.0825019], + [0.00939476, 0.999936, 0.0063645], + [0.0825595, 0.00556751, -0.996572], + ], + ), + ( + [ + [0.991261, 0.00474444, -0.131831], + [-0.00205841, 0.999788, 0.0205036], + [0.131901, -0.020053, 0.99106], + ], + [ + [-0.990924, 4.45275e-05, 0.134427], + [0.00614714, 0.998969, 0.0449827], + [-0.134286, 0.0454008, -0.989903], + ], + ), + ( + [ + [0.992266, -0.0947916, 0.0801474], + [0.100889, 0.992006, -0.0757987], + [-0.0723216, 0.0832984, 0.993897], + ], + [ + [-0.992701, 0.0817686, -0.0886652], + [-0.114283, -0.40263, 0.908203], + [0.0385633, 0.911704, 0.409035], + ], + ), + ( + [ + [0.99696, -0.00808565, -0.0774951], + [0.0585083, 0.734519, 0.676061], + [0.0514552, -0.67854, 0.732759], + ], + [ + [-0.9998, 0.0053398, -0.0193164], + [-0.0162677, -0.779206, 0.626556], + [-0.0117055, 0.626745, 0.779137], + ], + ), + ( + [ + [0.961501, 0.0133645, -0.274475], + [-0.016255, 0.999834, -0.00825889], + [0.274319, 0.0124025, 0.961559], + ], + [ + [-0.963687, 0.000179203, 0.267042], + [0.00670194, 0.999701, 0.023515], + [-0.266958, 0.0244509, -0.9634], + ], + ), + ( + [ + [0.99877, 0.0413462, -0.0273572], + [-0.0263673, 0.91029, 0.413131], + [0.0419844, -0.411902, 0.910261], + ], + [ + [-0.998035, -0.0613039, 0.0130407], + [-0.00146496, 0.230815, 0.972998], + [-0.0626594, 0.971065, -0.230452], + ], + ), + ( + [ + [0.999657, 0.0261608, 0.00141675], + [-0.0261957, 0.998937, 0.0379393], + [-0.000422719, -0.0379634, 0.999279], + ], + [ + [-0.998896, -0.0310033, -0.0353275], + [0.0315452, -0.999392, -0.0148857], + [-0.0348445, -0.0159846, 0.999265], + ], + ), + ( + [ + [0.77369, 0.0137861, 0.633415], + [-0.0186509, 0.999826, 0.00102049], + [-0.63329, -0.0126033, 0.773812], + ], + [ + [-0.773069, 0.0156632, -0.634129], + [0.00418312, 0.999799, 0.0195956], + [0.634308, 0.0124961, -0.772979], + ], + ), + ( + [ + [0.952827, -0.024521, -0.302522], + [-0.00541318, 0.9952, -0.0977158], + [0.303465, 0.0947439, 0.94812], + ], + [ + [-0.952266, -0.00806089, 0.305165], + [0.00351941, 0.999295, 0.037378], + [-0.305252, 0.0366678, -0.951567], + ], + ), + ( + [ + [-0.172189, 0.949971, 0.260587], + [-0.86961, -0.0223234, -0.493235], + [-0.462741, -0.311539, 0.829948], + ], + [ + [-0.672964, 0.0127645, -0.739567], + [0.00429523, 0.999902, 0.0133494], + [0.739664, 0.00580721, -0.672953], + ], + ), + ( + [ + [0.637899, -0.440017, 0.632036], + [-0.52883, 0.346333, 0.774849], + [-0.559842, -0.828516, -0.0117683], + ], + [ + [-0.0627307, -0.0314554, -0.997536], + [-0.733537, 0.679201, 0.0247117], + [0.67675, 0.733279, -0.0656804], + ], + ), + ( + [ + [0.998402, 0.00284932, -0.0564372], + [0.000393713, 0.998353, 0.0573683], + [0.0565077, -0.0572989, 0.996757], + ], + [ + [-0.997878, 0.000941416, 0.0651252], + [-2.16756e-05, 0.999891, -0.0147853], + [-0.065132, -0.0147552, -0.997768], + ], + ), + ( + [ + [0.9999, 0.0141438, -0.000431687], + [-0.0140882, 0.9979, 0.063225], + [0.00132502, -0.0632125, 0.997999], + ], + [ + [-0.999515, -0.0308197, -0.00482715], + [-0.00160551, -0.103741, 0.994605], + [-0.0311554, 0.994128, 0.10364], + ], + ), + ( + [ + [-0.201909, 0.0267804, 0.979038], + [-0.0159062, 0.999405, -0.0306179], + [-0.979275, -0.0217548, -0.201363], + ], + [ + [0.261235, 0.951613, -0.161839], + [0.0758567, 0.146901, 0.986239], + [0.962292, -0.269916, -0.03381], + ], + ), + ( + [ + [0.998335, -0.0191576, -0.0544038], + [0.0163271, 0.998513, -0.0520045], + [0.0553192, 0.0510297, 0.997164], + ], + [ + [-0.998811, -0.00846127, 0.0480344], + [-0.0051736, 0.997661, 0.0681593], + [-0.0484988, 0.0678295, -0.996519], + ], + ), + ( + [ + [0.999973, 0.00227282, -0.00699658], + [-0.00137504, 0.992062, 0.125744], + [0.00722684, -0.125731, 0.992038], + ], + [ + [-0.999995, -0.00337061, 4.25756e-05], + [-0.00333677, 0.991528, 0.129853], + [-0.00047993, 0.129852, -0.991534], + ], + ), + ( + [ + [0.998908, 0.0216581, -0.041392], + [-0.0327304, 0.956678, -0.289302], + [0.0333331, 0.290341, 0.956342], + ], + [ + [-0.998254, -0.0377592, 0.0454422], + [0.00744647, 0.682591, 0.730764], + [-0.0586112, 0.729825, -0.681118], + ], + ), + ( + [ + [0.999387, -0.0042571, -0.0347599], + [0.00485203, 0.999843, 0.017049], + [0.0346819, -0.0172072, 0.99925], + ], + [ + [-0.999976, 0.00260242, -0.00669664], + [-0.00250352, -0.999889, -0.0147361], + [-0.00673422, -0.0147175, 0.99987], + ], + ), + ( + [ + [0.906103, -0.398828, -0.141112], + [0.381512, 0.914475, -0.13485], + [0.182826, 0.0683519, 0.980766], + ], + [ + [-0.996568, -0.0321282, -0.0763021], + [-0.0823787, 0.476597, 0.875254], + [0.00824509, 0.878535, -0.477609], + ], + ), + ( + [ + [0.908356, 0.316033, -0.273884], + [-0.231421, -0.165634, -0.95865], + [-0.34833, 0.934178, -0.0773183], + ], + [ + [-0.999889, -0.0146322, -0.00295739], + [-0.0149238, 0.974974, 0.221815], + [-0.000362257, 0.221835, -0.975085], + ], + ), + ( + [ + [0.999507, -0.00834631, 0.0302637], + [0.00899248, 0.999733, -0.0212785], + [-0.030078, 0.0215401, 0.999315], + ], + [ + [-0.999538, 0.00785187, -0.0293621], + [0.00739788, 0.999852, 0.0155394], + [0.0294797, 0.0153149, -0.999448], + ], + ), + ( + [ + [0.999951, -0.00729441, -0.00672921], + [0.00313753, 0.87564, -0.482954], + [0.00941523, 0.48291, 0.87562], + ], + [ + [-0.999984, -0.005202, -0.00277372], + [0.00340465, -0.893745, 0.448565], + [-0.00481353, 0.448548, 0.893747], + ], + ), + ( + [ + [0.998028, -0.0569885, 0.0263322], + [0.0489091, 0.968801, 0.242967], + [-0.039357, -0.2412, 0.969677], + ], + [ + [-0.997066, 0.0422415, -0.0638525], + [-0.0760293, -0.448184, 0.890703], + [0.00900662, 0.892944, 0.45008], + ], + ), + ( + [ + [0.999745, 0.00860777, 0.0208747], + [-0.00827114, 0.999835, -0.0161595], + [-0.0210103, 0.0159827, 0.999651], + ], + [ + [-0.999576, 0.0148733, -0.0251161], + [0.0151027, 0.999846, -0.00898035], + [0.0249787, -0.00935575, -0.999646], + ], + ), + ( + [ + [0.91924, 0.0372116, -0.391934], + [-0.00675798, 0.996868, 0.0787959], + [0.393639, -0.0697837, 0.916613], + ], + [ + [-0.921919, 0.00882585, 0.387286], + [0.00588498, 0.999944, -0.00877866], + [-0.387342, -0.00581387, -0.921919], + ], + ), + ( + [ + [0.998324, -0.0029024, 0.0577924], + [0.00236766, 0.999954, 0.00931901], + [-0.0578167, -0.00916657, 0.998285], + ], + [ + [-0.99892, -0.0025688, -0.0464413], + [-0.00203721, 0.999932, -0.0114927], + [0.0464676, -0.0113855, -0.998857], + ], + ), + ( + [ + [0.993986, 0.0163462, -0.108279], + [-0.0612924, 0.902447, -0.426418], + [0.090746, 0.43049, 0.898022], + ], + [ + [-0.994519, -0.0767804, 0.0709843], + [0.0579273, 0.160607, 0.985318], + [-0.0870543, 0.984028, -0.15528], + ], + ), + ( + [ + [0.997351, 0.0715122, -0.0132892], + [-0.0707087, 0.996067, 0.0533919], + [0.0170551, -0.0523108, 0.998485], + ], + [ + [-0.997704, -0.066002, 0.015281], + [0.064101, -0.846657, 0.528267], + [-0.0219278, 0.528033, 0.848942], + ], + ), + ( + [ + [0.999839, 0.00714662, -0.0164633], + [-0.00859425, 0.99594, -0.0896085], + [0.0157561, 0.0897356, 0.995841], + ], + [ + [-0.999773, 0.0079918, 0.0197854], + [0.00864136, 0.999419, 0.0329623], + [-0.0195105, 0.0331255, -0.999262], + ], + ), + ( + [ + [-0.773738, 0.630074, 0.0658454], + [-0.622848, -0.737618, -0.260731], + [-0.115711, -0.242749, 0.963163], + ], + [ + [-0.740005, 0.000855199, -0.672604], + [-0.0106008, 0.99986, 0.0129348], + [0.672521, 0.0167018, -0.739892], + ], + ), + ( + [ + [0.969039, -0.00110643, -0.246907], + [-0.121454, 0.868509, -0.480564], + [0.214973, 0.495673, 0.841484], + ], + [ + [-0.981168, -0.150714, 0.120811], + [0.172426, -0.401504, 0.89948], + [-0.0870583, 0.903372, 0.419929], + ], + ), + ( + [ + [0.589015, 0.80692, 0.0440651], + [-0.806467, 0.583447, 0.0959135], + [0.0516848, -0.0920316, 0.994414], + ], + [ + [-0.99998, 0.00434293, -0.00486489], + [0.00437139, 0.999973, -0.00588975], + [0.00483918, -0.00591087, -0.999972], + ], + ), + ( + [ + [0.999972, 0.000781564, 0.00750023], + [-0.0031568, 0.946655, 0.322235], + [-0.00684828, -0.322249, 0.94663], + ], + [ + [-0.999817, -0.0178453, -0.00691725], + [-0.0189272, 0.975556, 0.218934], + [0.00284118, 0.219025, -0.975716], + ], + ), + ( + [ + [-0.969668, 0.219101, -0.108345], + [0.172364, 0.298654, -0.938667], + [-0.173305, -0.928871, -0.32736], + ], + [ + [-0.999917, 0.0111423, -0.00656864], + [-0.00977865, -0.318874, 0.947748], + [0.00846644, 0.947733, 0.318955], + ], + ), + ( + [ + [-0.808574, -0.185515, -0.558383], + [0.174641, -0.981898, 0.0733309], + [-0.561879, -0.038223, 0.826336], + ], + [ + [-0.873416, 0.0121808, -0.486824], + [-0.00495714, 0.999413, 0.0338998], + [0.486951, 0.032022, -0.872843], + ], + ), + ( + [ + [0.999295, 0.0295658, -0.0231234], + [-0.0251771, 0.984904, 0.17126], + [0.0278378, -0.170557, 0.984954], + ], + [ + [-0.998834, -0.040128, 0.026921], + [0.0327412, -0.152276, 0.987798], + [-0.0355388, 0.987524, 0.153411], + ], + ), + ( + [ + [0.996021, -0.0050677, -0.0889802], + [0.0042919, 0.999951, -0.00890794], + [0.089021, 0.0084906, 0.995994], + ], + [ + [-0.995726, -0.00858132, 0.0919686], + [-0.00615004, 0.999625, 0.0266854], + [-0.0921631, 0.0260058, -0.995405], + ], + ), + ( + [ + [0.563325, 0.812296, 0.151129], + [-0.316559, 0.381143, -0.868632], + [-0.763188, 0.441481, 0.471847], + ], + [ + [-0.980048, -0.0115108, -0.198437], + [-0.168991, 0.573853, 0.801335], + [0.104649, 0.818877, -0.564348], + ], + ), + ( + [ + [0.984844, -0.0288271, 0.17103], + [0.0260588, 0.999491, 0.0184094], + [-0.171474, -0.0136736, 0.985094], + ], + [ + [-0.984637, -0.00367691, -0.174577], + [-0.00649229, 0.999858, 0.0155587], + [0.174495, 0.0164532, -0.984521], + ], + ), + ( + [ + [0.99985, 0.000720773, -0.0172841], + [-0.00075051, 0.999998, -0.0017141], + [0.0172828, 0.00172682, 0.999849], + ], + [ + [-0.999926, -0.00413456, 0.0114842], + [-0.00368343, 0.999231, 0.0390359], + [-0.0116368, 0.0389908, -0.999172], + ], + ), + ( + [ + [0.997976, 0.0603523, -0.0200139], + [-0.0558618, 0.982551, 0.177404], + [0.0303714, -0.175927, 0.983935], + ], + [ + [-0.996867, -0.0790953, 0.00217996], + [0.0318842, -0.376338, 0.925935], + [-0.0724181, 0.923101, 0.37768], + ], + ), + ( + [ + [0.94678, -0.00538407, -0.321837], + [0.00249113, 0.999953, -0.0094], + [0.321872, 0.008098, 0.946749], + ], + [ + [-0.945694, 0.0255694, 0.324053], + [0.0240377, 0.999673, -0.00872898], + [-0.32417, -0.000465377, -0.945999], + ], + ), + ( + [ + [0.846059, 0.435245, -0.307807], + [0.318073, 0.0512036, 0.946682], + [0.4278, -0.898855, -0.0951187], + ], + [ + [-0.217213, -0.0389124, 0.975352], + [0.742195, 0.642416, 0.190918], + [-0.634011, 0.765368, -0.11066], + ], + ), + ( + [ + [0.914988, -0.0538229, -0.399875], + [-0.0459455, 0.970717, -0.23579], + [0.400857, 0.234117, 0.885722], + ], + [ + [-0.919706, 0.00194642, 0.392606], + [0.105539, 0.964406, 0.242451], + [-0.378159, 0.264418, -0.887176], + ], + ), + ( + [ + [0.970915, -0.183858, 0.153365], + [0.209801, 0.96196, -0.174974], + [-0.115361, 0.202061, 0.972555], + ], + [ + [-0.975509, 0.21077, -0.0629391], + [-0.218082, -0.964089, 0.151576], + [-0.0287314, 0.161588, 0.986441], + ], + ), + ( + [ + [0.99369, -0.00515149, -0.112044], + [0.00366664, 0.999903, -0.0134545], + [0.112102, 0.0129588, 0.993612], + ], + [ + [-0.99406, 0.00631892, 0.108668], + [0.00878985, 0.999713, 0.022273], + [-0.108496, 0.0230956, -0.99383], + ], + ), + ( + [ + [0.995917, 0.0137529, 0.089215], + [-0.0145079, 0.999864, 0.00781912], + [-0.0890954, -0.00908151, 0.995982], + ], + [ + [-0.996188, 0.012059, -0.0864113], + [0.0126654, 0.999899, -0.00647346], + [0.0863245, -0.00754306, -0.99624], + ], + ), + ( + [ + [0.84563, -0.0032436, -0.533759], + [0.0040093, 0.999992, 0.000275049], + [0.533754, -0.00237259, 0.845636], + ], + [ + [-0.849818, -0.00755214, 0.527023], + [-0.00734806, 0.99997, 0.00248074], + [-0.527026, -0.00176415, -0.849848], + ], + ), + ( + [ + [0.736067, -0.212675, -0.642631], + [-0.447028, 0.560168, -0.697408], + [0.508303, 0.800613, 0.31725], + ], + [ + [-0.684029, 0.0061039, 0.729431], + [0.0260275, 0.999532, 0.0160434], + [-0.728992, 0.0299595, -0.683868], + ], + ), + ( + [ + [0.993949, 0.00461705, -0.109742], + [-0.00653155, 0.999833, -0.0170925], + [0.109644, 0.0177058, 0.993813], + ], + [ + [-0.994446, 0.0218439, 0.102965], + [0.0227578, 0.999711, 0.00770966], + [-0.102767, 0.0100102, -0.994656], + ], + ), + ( + [ + [0.996005, -0.0103388, 0.0886959], + [-0.0291635, 0.901147, 0.432531], + [-0.0843999, -0.43339, 0.897246], + ], + [ + [-0.999947, 0.00833193, -0.00598923], + [-0.0101526, -0.887864, 0.459993], + [-0.00148526, 0.46003, 0.887902], + ], + ), + ( + [ + [0.981518, 0.0114609, 0.191025], + [-0.0104683, 0.999926, -0.00620422], + [-0.191082, 0.00408984, 0.981565], + ], + [ + [-0.979556, 0.000134379, -0.201176], + [-0.00817302, 0.999148, 0.0404628], + [0.20101, 0.0412799, -0.97872], + ], + ), + ( + [ + [0.997665, -0.0372296, -0.0572574], + [0.0379027, 0.999224, 0.0107148], + [0.0568141, -0.01286, 0.998302], + ], + [ + [-0.997794, 0.00389749, 0.0662921], + [0.00639122, 0.999278, 0.0374446], + [-0.0660983, 0.0377856, -0.997099], + ], + ), + ( + [ + [0.981618, -0.0105643, -0.190564], + [0.00329498, 0.999256, -0.0384229], + [0.190828, 0.0370887, 0.980923], + ], + [ + [-0.981673, -0.000810695, 0.190576], + [0.00398375, 0.999685, 0.0247729], + [-0.190536, 0.0250779, -0.981361], + ], + ), + ( + [ + [-0.544941, -0.812151, -0.208446], + [0.812337, -0.449791, -0.37121], + [0.207722, -0.371617, 0.90485], + ], + [ + [-0.121327, -0.000366672, -0.992614], + [-0.955208, 0.271977, 0.116655], + [0.269926, 0.962303, -0.0333484], + ], + ), + ( + [ + [0.637701, -0.219537, 0.738336], + [0.735715, 0.457522, -0.499397], + [-0.228168, 0.861671, 0.453279], + ], + [ + [-0.741797, 0.0196167, -0.670339], + [-0.00209087, 0.9995, 0.0315629], + [0.670623, 0.0248149, -0.741385], + ], + ), + ( + [ + [0.99813, -0.0590625, -0.0157485], + [0.0589086, 0.998213, -0.0100649], + [0.0163148, 0.00911833, 0.999825], + ], + [ + [-0.99893, 0.0258783, -0.0383385], + [-0.0440455, -0.279068, 0.959261], + [0.014125, 0.959924, 0.279908], + ], + ), + ( + [ + [0.999558, 0.0028395, -0.0296019], + [-0.00492321, 0.997496, -0.0705578], + [0.0293274, 0.0706723, 0.997068], + ], + [ + [-0.999532, -0.0305627, -0.00231546], + [0.00957406, -0.38309, 0.923664], + [-0.0291167, 0.923206, 0.383202], + ], + ), + ( + [ + [0.99814, -0.0528437, -0.0303853], + [0.0590889, 0.96123, 0.269341], + [0.0149743, -0.270636, 0.962565], + ], + [ + [-0.999464, 0.00781117, 0.0318024], + [-0.000588355, 0.966696, -0.255928], + [-0.0327423, -0.255809, -0.966173], + ], + ), + ( + [ + [-0.936685, 0.234194, 0.260336], + [-0.233325, -0.97178, 0.034698], + [0.261116, -0.0282419, 0.964894], + ], + [ + [0.999511, 0.00582072, 0.0307461], + [0.0289012, 0.204922, -0.978352], + [-0.0119956, 0.978762, 0.204654], + ], + ), + ( + [ + [0.973616, -0.019218, -0.227384], + [0.0030011, 0.99744, -0.0714512], + [0.228175, 0.0688836, 0.97118], + ], + [ + [-0.974738, 0.0190271, 0.222547], + [0.0222378, 0.999682, 0.0119297], + [-0.222249, 0.0165771, -0.97485], + ], + ), + ( + [ + [0.997273, 0.0453471, -0.0582173], + [-0.0234007, 0.942529, 0.333303], + [0.0699858, -0.331032, 0.941021], + ], + [ + [-0.996269, -0.0613496, 0.0607196], + [-0.0100285, 0.780948, 0.624516], + [-0.0857328, 0.621576, -0.77865], + ], + ), + ( + [ + [0.999511, 0.0274482, -0.0149865], + [-0.0305945, 0.957511, -0.286769], + [0.00647846, 0.287087, 0.957883], + ], + [ + [-0.999443, -0.0260559, 0.0209038], + [0.0148505, 0.213942, 0.976734], + [-0.0299225, 0.976499, -0.213437], + ], + ), + ( + [ + [0.621123, 0.722893, 0.302708], + [-0.48353, 0.657448, -0.577894], + [-0.61677, 0.212574, 0.757896], + ], + [ + [-0.996888, -0.0217614, -0.0757776], + [-0.0783897, 0.376159, 0.923234], + [0.00841386, 0.926299, -0.376694], + ], + ), + ( + [ + [0.974426, 0.0128384, -0.224341], + [-0.0123842, 0.999917, 0.00343166], + [0.224367, -0.00056561, 0.974505], + ], + [ + [-0.973234, -0.00506667, 0.229763], + [-0.000498848, 0.999801, 0.0199346], + [-0.229818, 0.0192865, -0.973043], + ], + ), + ( + [ + [0.994721, -0.0881097, 0.0526082], + [0.0972904, 0.972774, -0.210345], + [-0.0326424, 0.214353, 0.976211], + ], + [ + [-0.994309, 0.0920529, -0.0536268], + [-0.105538, -0.782431, 0.613729], + [0.0145358, 0.615896, 0.787694], + ], + ), + ( + [ + [0.998677, -0.0372894, 0.0354002], + [0.0242326, 0.948589, 0.315583], + [-0.0453481, -0.314308, 0.948237], + ], + [ + [-0.999066, -0.00910724, -0.0422707], + [-0.024629, 0.923353, 0.383161], + [0.0355411, 0.383844, -0.922715], + ], + ), + ( + [ + [0.931525, 0.00831028, 0.363583], + [0.0192806, 0.997204, -0.0721909], + [-0.363167, 0.0742577, 0.92876], + ], + [ + [-0.930052, -0.00174384, -0.367425], + [-0.0268673, 0.997634, 0.0632737], + [0.366445, 0.0687194, -0.927899], + ], + ), + ( + [ + [-0.50483, -0.819216, 0.272087], + [0.775688, -0.568816, -0.273414], + [0.378753, 0.0730272, 0.922612], + ], + [ + [-0.981596, 0.00031926, 0.190974], + [0.00652401, 0.999471, 0.0318616], + [-0.190863, 0.0325211, -0.981079], + ], + ), + ( + [ + [0.990518, -0.00195099, -0.137368], + [-0.00164696, 0.999659, -0.0260735], + [0.137372, 0.0260526, 0.990177], + ], + [ + [-0.991078, 0.00934835, 0.132961], + [0.0106057, 0.999905, 0.00875176], + [-0.132866, 0.0100839, -0.991083], + ], + ), + ( + [ + [0.935049, -0.353081, 0.0318997], + [0.257018, 0.737114, 0.624984], + [-0.244184, -0.576192, 0.779985], + ], + [ + [-0.977342, -0.00167896, -0.211667], + [-0.0448634, 0.978894, 0.199386], + [0.206864, 0.204364, -0.956789], + ], + ), + ( + [ + [0.998464, 0.0501172, 0.0236119], + [-0.0498618, 0.998692, -0.0112844], + [-0.0241466, 0.0100898, 0.999658], + ], + [ + [-0.999931, -0.0037971, -0.0112195], + [-0.00640916, 0.970027, 0.242913], + [0.00996085, 0.242968, -0.969984], + ], + ), + ( + [ + [0.999893, -0.0108217, 0.00984537], + [0.011201, 0.999164, -0.0393194], + [-0.00941164, 0.0394255, 0.999178], + ], + [ + [-0.999886, 0.00730461, -0.0133396], + [-0.0118202, -0.925163, 0.379391], + [-0.00956982, 0.379504, 0.925142], + ], + ), + ( + [ + [0.990922, -0.086745, 0.102709], + [0.0847349, 0.99612, 0.0237834], + [-0.104373, -0.0148644, 0.994427], + ], + [ + [-0.994922, -0.00197458, -0.10064], + [-0.00242513, 0.999988, 0.00435525], + [0.10063, 0.00457739, -0.994914], + ], + ), + ( + [ + [0.999856, 0.00210734, -0.0168511], + [-0.00557165, 0.978053, -0.20828], + [0.0160424, 0.208344, 0.977924], + ], + [ + [-0.999698, 0.0048691, 0.0241226], + [-0.00154306, 0.965899, -0.258915], + [-0.0245606, -0.258874, -0.9656], + ], + ), + ( + [ + [0.992858, -0.0249864, -0.116659], + [0.0419872, 0.988447, 0.145634], + [0.111673, -0.149492, 0.982436], + ], + [ + [-0.992324, 0.0357741, 0.118384], + [-0.0419528, 0.803113, -0.594348], + [-0.116338, -0.594752, -0.795447], + ], + ), + ( + [ + [0.986821, -0.00531913, 0.161729], + [0.00797365, 0.999844, -0.0157688], + [-0.16162, 0.0168505, 0.986709], + ], + [ + [-0.985867, 0.0119402, -0.167109], + [0.0141227, 0.99983, -0.0118784], + [0.166939, -0.0140704, -0.985868], + ], + ), + ( + [ + [0.999693, -0.0158939, -0.0190113], + [0.0103599, 0.96501, -0.262007], + [0.0225104, 0.261729, 0.964879], + ], + [ + [-0.999344, -0.0314781, -0.0180051], + [-0.0250895, 0.241673, 0.970034], + [-0.0261833, 0.969847, -0.242305], + ], + ), + ( + [ + [0.977445, 0.0293661, 0.209138], + [-0.0723687, 0.976903, 0.201057], + [-0.198403, -0.211657, 0.956994], + ], + [ + [-0.976437, 0.00895131, -0.215624], + [0.0552894, 0.976169, -0.20985], + [0.208607, -0.216827, -0.953663], + ], + ), + ( + [ + [0.994593, 0.0974797, -0.0358119], + [-0.0822288, 0.949838, 0.301737], + [0.0634288, -0.297161, 0.952718], + ], + [ + [-0.994192, -0.10746, -0.00604622], + [0.078812, -0.7651, 0.639071], + [-0.0733003, 0.634882, 0.769124], + ], + ), + ( + [ + [0.365674, 0.282077, -0.88697], + [-0.609177, 0.793033, 0.00105565], + [0.703694, 0.539936, 0.461826], + ], + [ + [-0.469534, 0.0109062, 0.882848], + [0.0060785, 0.99994, -0.00911984], + [-0.882894, 0.00108445, -0.469572], + ], + ), + ( + [ + [0.999956, 0.00903085, 0.0025358], + [-0.00862738, 0.991574, -0.129252], + [-0.00368169, 0.129224, 0.991609], + ], + [ + [-0.999976, 0.00322491, -0.00637541], + [0.00379751, 0.995755, -0.0919687], + [0.00605176, -0.0919907, -0.995743], + ], + ), + ( + [ + [0.999982, -0.00398882, -0.00441072], + [0.00411881, 0.999545, 0.0298655], + [0.00428959, -0.0298832, 0.999544], + ], + [ + [-0.999931, -0.00315547, -0.0114491], + [0.00300966, -0.999914, 0.0128304], + [-0.0114875, 0.012796, 0.999853], + ], + ), + ( + [ + [0.996613, 0.0781452, -0.0256245], + [-0.0610516, 0.91178, 0.406116], + [0.0550999, -0.403175, 0.913462], + ], + [ + [-0.996368, -0.084671, 0.00909851], + [0.0540149, -0.545774, 0.83619], + [-0.0658352, 0.833644, 0.548365], + ], + ), + ( + [ + [0.961059, 0.139318, 0.238654], + [-0.117488, 0.987672, -0.103448], + [-0.250124, 0.0713812, 0.965579], + ], + [ + [-0.973397, 0.00782581, -0.228998], + [-0.0621109, 0.952986, 0.296581], + [0.220553, 0.302913, -0.927147], + ], + ), + ( + [ + [0.156415, -0.982138, 0.104589], + [-0.568896, -0.176149, -0.803323], + [0.807398, 0.0661518, -0.586287], + ], + [ + [-0.992155, 0.0934304, -0.0830664], + [-0.121171, -0.555137, 0.822887], + [0.0307694, 0.826496, 0.562102], + ], + ), + ( + [ + [0.997973, 0.0130328, -0.0622976], + [-0.011111, 0.999455, 0.0310968], + [0.0626689, -0.0303416, 0.997573], + ], + [ + [-0.997391, -0.00094697, 0.0722014], + [-0.00271076, 0.9997, -0.024334], + [-0.0721567, -0.024466, -0.997094], + ], + ), + ( + [ + [0.913504, -0.0125928, -0.406634], + [-0.108363, 0.95588, -0.27304], + [0.392132, 0.293487, 0.871836], + ], + [ + [-0.909813, 0.0115348, 0.414861], + [0.128636, 0.958223, 0.255464], + [-0.394582, 0.28579, -0.873287], + ], + ), + ( + [ + [0.932595, -0.0693644, 0.354197], + [0.0984415, 0.993036, -0.0647231], + [-0.347241, 0.0952281, 0.932928], + ], + [ + [-0.930498, 0.00578599, -0.366252], + [-0.106202, 0.952666, 0.284867], + [0.350564, 0.303964, -0.885839], + ], + ), + ( + [ + [0.995668, -0.00475737, 0.0928567], + [0.00890154, 0.99898, -0.0442667], + [-0.0925514, 0.0449015, 0.994695], + ], + [ + [-0.996077, -0.0107986, -0.0878355], + [0.00749423, 0.978669, -0.205305], + [0.0881789, -0.205158, -0.974749], + ], + ), + ( + [ + [0.99948, 0.0321999, 0.00146151], + [-0.0321302, 0.998886, -0.0345513], + [-0.00257243, 0.0344864, 0.999402], + ], + [ + [-0.999953, 0.00726142, -0.0065326], + [0.00488529, 0.950962, 0.30927], + [0.00845801, 0.309223, -0.950953], + ], + ), +] + + class TestRot3(GtsamTestCase): """Test selected Rot3 methods.""" @@ -30,12 +2014,24 @@ class TestRot3(GtsamTestCase): [ -0.0018374, 0.0209105, -0.999781] ]) # fmt: on - + # get back angle in radians _, actual_angle = Rot3(R).axisAngle() expected_angle = 3.1396582 np.testing.assert_almost_equal(actual_angle, expected_angle, 1e-7) + def test_axis_angle_stress_test(self) -> None: + """Test that .axisAngle() yields angles less than 180 degrees for specific inputs.""" + for (R1, R2) in R1_R2_pairs: + R1 = Rot3(np.array(R1)) + R2 = Rot3(np.array(R2)) + + i1Ri2 = R1.between(R2) + + axis, angle = i1Ri2.axisAngle() + angle_deg = np.rad2deg(angle) + assert angle_deg < 180 + if __name__ == "__main__": unittest.main() From 9ec3f791f55b9b49d285ddf32615e96e0650c6dd Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 13 Oct 2021 22:03:40 -0400 Subject: [PATCH 103/169] loosen tolerance on trace(R) == -1 --- gtsam/geometry/SO3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 890bcd206..5f1bf7460 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,7 +261,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (tr + 1.0 < 1e-4) { + if (tr + 1.0 < 1e-3) { if (R33 > R22 && R33 > R11) { // R33 is the largest diagonal const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; From 462fdb4ff0bf0d806a12ae157fc6d8cce793a89d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 22:05:23 -0400 Subject: [PATCH 104/169] Fix again with better approximation --- gtsam/geometry/SO3.cpp | 53 +++++++++++++++++++++---------- gtsam/geometry/tests/testRot3.cpp | 2 +- 2 files changed, 38 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 890bcd206..2585c37be 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,25 +261,46 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (tr + 1.0 < 1e-4) { + if (tr + 1.0 < 1e-3) { if (R33 > R22 && R33 > R11) { - // R33 is the largest diagonal - const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; - const double r = sqrt(2.0 + 2.0 * R33); - const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12); - omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); + // R33 is the largest diagonal, a=3, b=1, c=2 + const double W = R21 - R12; + const double Q1 = 2.0 + 2.0 * R33; + const double Q2 = R31 + R13; + const double Q3 = R23 + R32; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q2, Q3, Q1); } else if (R22 > R11) { - // R22 is the largest diagonal - const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; - const double r = sqrt(2.0 + 2.0 * R22); - const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); - omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); + // R22 is the largest diagonal, a=2, b=3, c=1 + const double W = R13 - R31; + const double Q1 = 2.0 + 2.0 * R22; + const double Q2 = R23 + R32; + const double Q3 = R12 + R21; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q3, Q1, Q2); } else { - // R11 is the largest diagonal - const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; - const double r = sqrt(2.0 + 2.0 * R11); - const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); - omega = sgn_w * scale * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); + // R11 is the largest diagonal, a=1, b=2, c=3 + const double W = R32 - R23; + const double Q1 = 2.0 + 2.0 * R11; + const double Q2 = R12 + R21; + const double Q3 = R31 + R13; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q1, Q2, Q3); } } else { double magnitude; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index bb6f1a77a..dc4b888b3 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -268,7 +268,7 @@ TEST( Rot3, log) { (Vector)Rot3::Logmap(Rlund), 1e-8)); #else // SO3 will be approximate because of the non-orthogonality - EXPECT(assert_equal(Vector3(0.264485272, -0.742291088, -3.04136444), + EXPECT(assert_equal(Vector3(0.264452, -0.742197708, -3.04098184), (Vector)Rot3::Logmap(Rlund), 1e-8)); #endif } From 00c541aca660268e79834b8ab90f3782896482be Mon Sep 17 00:00:00 2001 From: jerredchen Date: Thu, 14 Oct 2021 13:42:21 -0400 Subject: [PATCH 105/169] adjusted docstrings to match google style guide --- python/gtsam/examples/Pose2ISAM2Example.py | 33 ++++++++--------- python/gtsam/examples/Pose3ISAM2Example.py | 43 ++++++++++------------ 2 files changed, 35 insertions(+), 41 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index 6710dc706..5336bc34e 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -22,9 +22,8 @@ import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot def Pose2SLAM_ISAM2_plot(graph, current_estimate): - """ - Plots incremental state of the robot for 2D Pose SLAM using iSAM2 - Author: Jerred Chen + """Plots incremental state of the robot for 2D Pose SLAM using iSAM2 + Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) @@ -48,7 +47,8 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate): def Pose2SLAM_ISAM2_example(): - """ + """Perform 2D SLAM given the ground truth changes in pose as well as + simple loop closure detection. """ plt.ion() @@ -91,20 +91,19 @@ def Pose2SLAM_ISAM2_example(): graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - def determine_loop_closure(odom, current_estimate, xy_tol=0.6, theta_tol=0.3): - """ - Simple brute force approach which iterates through previous states + def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + xy_tol=0.6, theta_tol=0.3) -> int: + """Simple brute force approach which iterates through previous states and checks for loop closure. - Author: Jerred - ### Parameters: - odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) - measurement in the body frame. - current_estimate: (gtsam.Values) The current estimates computed by iSAM2. - xy_tol: (double) Optional argument for the x-y measurement tolerance. - theta_tol: (double) Optional argument for the theta measurement tolerance. - ### Returns: - k: (int) The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. + + Args: + odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. + current_estimate: The current estimates computed by iSAM2. + xy_tol: Optional argument for the x-y measurement tolerance. + theta_tol: Optional argument for the theta measurement tolerance. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. """ if current_estimate: prev_est = current_estimate.atPose2(i+1) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index a15424d04..3fcdcd7ec 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -24,9 +24,8 @@ import matplotlib.pyplot as plt def Pose3SLAM_ISAM2_plot(graph, current_estimate): - """ - Plots incremental state of the robot for 3D Pose SLAM using iSAM2 - Author: Jerred Chen + """Plots incremental state of the robot for 3D Pose SLAM using iSAM2 + Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) @@ -52,9 +51,7 @@ def Pose3SLAM_ISAM2_plot(graph, current_estimate): def createPoses(): - """ - Creates ground truth poses of the robot. - """ + """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], @@ -80,10 +77,8 @@ def createPoses(): return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)] def Pose3_ISAM2_example(): - """ - Perform 3D SLAM given ground truth poses as well as simple - loop closure detection. - """ + """Perform 3D SLAM given ground truth poses as well as simple + loop closure detection.""" plt.ion() def vector6(x, y, z, a, b, c): @@ -115,20 +110,20 @@ def Pose3_ISAM2_example(): initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) - def determine_loop_closure(odom, current_estimate, xyz_tol=0.6, rot_tol=0.3): - """ - Simple brute force approach which iterates through previous states + def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + xyz_tol=0.6, rot_tol=0.3) -> int: + """Simple brute force approach which iterates through previous states and checks for loop closure. - Author: Jerred Chen - ### Parameters: - odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation - measurement in the body frame, [roll, pitch, yaw, x, y, z] - current_estimate: (gtsam.Values) The current estimates computed by iSAM2. - xyz_tol: (double) Optional argument for the translational tolerance. - rot_tol: (double) Optional argument for the rotational tolerance. - ### Returns: - k: (int) The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. + + Args: + odom: Vector representing noisy odometry transformation measurement in the body frame, + where the vector represents [roll, pitch, yaw, x, y, z]. + current_estimate: The current estimates computed by iSAM2. + xyz_tol: Optional argument for the translational tolerance. + rot_tol: Optional argument for the rotational tolerance. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. """ if current_estimate: rot = Rot3.RzRyRx(odom[:3]) @@ -164,7 +159,7 @@ def Pose3_ISAM2_example(): isam.update() current_estimate = isam.calculateEstimate() print("*"*50) - print(f"Inference after State {i}:") + print(f"Inference after State {i}:\n") print(current_estimate) marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) initial_estimate.clear() From aac05f238996e952eefee5655e5eab6709ad7f28 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 15 Oct 2021 14:46:17 -0400 Subject: [PATCH 106/169] Revert "Revert "replace deprecated tbb functionality"" This reverts commit f819b1a03f74f289f50b96125610026618601a2f. --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 +++++++------------ 2 files changed, 29 insertions(+), 46 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7a88f72eb..30cec3b9a 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - tbb::task::spawn_root_and_wait( - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold)); + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 87d5b0d4c..dc1b45906 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task, tbb::task_list +#include // tbb::task_group #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask : public tbb::task + class PreOrderTask { public: const boost::shared_ptr& treeNode; @@ -42,28 +42,30 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; bool makeNewTasks; - bool isPostOrderPhase; + // Keep track of order phase across multiple calls to the same functor + mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) + tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), + tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() override + void operator()() const { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return nullptr; } else { @@ -71,14 +73,10 @@ namespace gtsam { { if(!treeNode->children.empty()) { - // Allocate post-order task as a continuation - isPostOrderPhase = true; - recycle_as_continuation(); - bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - tbb::task* firstChild = 0; - tbb::task_list childTasks; + // If we have child tasks, start subtasks and wait for them to complete + tbb::task_group ctg; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -86,37 +84,30 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - tbb::task* childTask = - new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if (firstChild) - childTasks.push_back(*childTask); - else - firstChild = childTask; + ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, ctg, overThreshold)); } + ctg.wait(); - // If we have child tasks, start subtasks and wait for them to complete - set_ref_count((int)treeNode->children.size()); - spawn(childTasks); - return firstChild; + // Allocate post-order task as a continuation + isPostOrderPhase = true; + tg.run(*this); } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const { for(const boost::shared_ptr& child: node->children) { @@ -131,7 +122,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask : public tbb::task + class RootTask { public: const ROOTS& roots; @@ -139,38 +130,31 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold) : + int problemSizeThreshold, tbb::task_group& tg) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold) {} + problemSizeThreshold(problemSizeThreshold), tg(tg) {} - tbb::task* execute() override + void operator()() const { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tasks.push_back(*new(allocate_child()) - PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } - // Set TBB ref count - set_ref_count(1 + (int) roots.size()); - // Spawn tasks - spawn_and_wait_for_all(tasks); - // Return nullptr - return nullptr; } }; template - RootTask& - CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); - } + tbb::task_group tg; + tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + } } From 33e16aa7d2ba47239ec59439da997238d642b8bd Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 01:01:02 -0400 Subject: [PATCH 107/169] correct jacobians --- gtsam/geometry/Pose3.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 484fb9ca9..5a3b20782 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -79,22 +79,23 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); - // Since we use the Point3 version of cross, the jacobian of pRw wrpt t - // (pRw_H_t) needs special treatment as detailed below. const Vector3 pRw = - cross(t_, Rw, boost::none, (H_this || H_xib) ? &pRw_H_Rw : nullptr); + cross(t_, Rw, pRw_H_t, (H_this || H_xib) ? &pRw_H_Rw : nullptr); result.tail<3>() = pRw + Rv; // Jacobians if (H_this) { - // By applying the chain rule to the matrix-matrix product of [t]R, we can - // compute a simplified derivative which is the same as Rw_H_R. Details: - // https://github.com/borglab/gtsam/pull/885#discussion_r726591370 - pRw_H_t = Rw_H_R; - *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) .finished(); + /* This is the "full" calculation: + Matrix36 R_H_this, t_H_this; + rotation(R_H_this); // I_3x3, Z_3x3 + translation(t_H_this); // Z_3x3, R + (*H_this) *= (Matrix6() << R_H_this, t_H_this).finished(); + */ + // But we can simplify those calculations since it's mostly I and Z: + H_this->bottomRightCorner<3, 3>() *= R_.matrix(); } if (H_xib) { *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // @@ -130,9 +131,11 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, // Jacobians if (H_this) { - *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rv_H_R, // -Rtv_H_tv * tv_H_t + *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, -Rtv_H_tv * tv_H_t, /* */ Rv_H_R, /* */ Z_3x3) .finished(); + // See Adjoint(xi) jacobian calculation for why we multiply by R + H_this->topRightCorner<3, 3>() *= R_.matrix(); } if (H_x) { *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // From 908ba707062fdbf698101ab1d4a29f84697ddf39 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 03:34:13 -0400 Subject: [PATCH 108/169] documentation --- doc/math.lyx | 179 +++++++++++++++++++++++++++++++++++++++++++++++++++ doc/math.pdf | Bin 261727 -> 264527 bytes 2 files changed, 179 insertions(+) diff --git a/doc/math.lyx b/doc/math.lyx index 2533822a7..f14ebc66f 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5084,6 +5084,185 @@ reference "ex:projection" \end_layout +\begin_layout Subsection +Derivative of Adjoint +\end_layout + +\begin_layout Standard +Consider +\begin_inset Formula $f:SE(3)\rightarrow\mathbb{R}^{6}$ +\end_inset + + is defined as +\begin_inset Formula $f(T)=Ad_{T}y$ +\end_inset + + for some constant +\begin_inset Formula $y=\begin{bmatrix}\omega_{y}\\ +v_{y} +\end{bmatrix}$ +\end_inset + +. + Defining +\begin_inset Formula $\xi=\begin{bmatrix}\omega\\ +v +\end{bmatrix}$ +\end_inset + + for the derivative notation (w.r.t. + pose +\begin_inset Formula $T$ +\end_inset + +): +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix} +\] + +\end_inset + +Then we can compute +\begin_inset Formula $f'(T)$ +\end_inset + + by considering the rotation and translation separately. + To reduce confusion with +\begin_inset Formula $\omega_{y},v_{y}$ +\end_inset + +, we will use +\begin_inset Formula $R,t$ +\end_inset + + to denote the rotation and translation of +\begin_inset Formula $T\exp\xi$ +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\frac{\partial}{\partial\omega}\begin{bmatrix}R & 0\\{} +[t]_{\times}R & R +\end{bmatrix}\begin{bmatrix}\omega_{y}\\ +v_{y} +\end{bmatrix}=\frac{\partial}{\partial\omega}\begin{bmatrix}R\omega_{y}\\{} +[t]_{\times}R\omega_{y}+Rv_{y} +\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times}\\ +-[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} +\end{bmatrix} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\frac{\partial}{\partial t}\begin{bmatrix}R & 0\\{} +[t]_{\times}R & R +\end{bmatrix}\begin{bmatrix}\omega\\ +v +\end{bmatrix}=\frac{\partial}{\partial t}\begin{bmatrix}R\omega_{y}\\{} +[t]_{\times}R\omega_{y}+Rv_{y} +\end{bmatrix}=\begin{bmatrix}0\\ +-[R\omega_{y}] +\end{bmatrix} +\] + +\end_inset + +Applying chain rule for the translation, +\begin_inset Formula $\frac{\partial}{\partial v}=\frac{\partial}{\partial t}\frac{\partial t}{\partial v}$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\frac{\partial}{\partial v}\begin{bmatrix}R & 0\\{} +[t]_{\times}R & R +\end{bmatrix}\begin{bmatrix}\omega_{y}\\ +v_{y} +\end{bmatrix}=\begin{bmatrix}0\\ +-[R\omega_{y}]_{\times} +\end{bmatrix}R=\begin{bmatrix}0\\ +-[R\omega_{y}]_{\times}R +\end{bmatrix}=\begin{bmatrix}0\\ +-R[\omega_{y}]_{\times} +\end{bmatrix} +\] + +\end_inset + +The simplification +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $[R\omega_{y}]_{\times}R=R[\omega_{y}]_{\times}$ +\end_inset + + can be derived by considering the result for when +\begin_inset Formula $\omega_{y}$ +\end_inset + + is each of the 3 standard basis vectors +\begin_inset Formula $\hat{e}_{i}$ +\end_inset + +: +\begin_inset Formula $-[R\hat{e}_{i}]_{\times}R$ +\end_inset + +. +\end_layout + +\begin_layout Standard +Now putting together the rotation and translation: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times} & 0\\ +-[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} & -R[\omega_{y}]_{\times} +\end{bmatrix} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +We can apply a similar procedure to compute the derivative of +\begin_inset Formula $Ad_{T}^{T}y$ +\end_inset + +. +\end_layout + \begin_layout Subsection Instantaneous Velocity \end_layout diff --git a/doc/math.pdf b/doc/math.pdf index 8dc7270f1139ce7e246a63925d29302f3afad2aa..06dec078d5fb590fe6dfe74dcdc13aace1a22328 100644 GIT binary patch delta 127232 zcmZshQ)8G7lx}0&YGXEbW81cECyn0Nwr$&Xlg74fJ2~H++nL|6Z`Rt++WtVEPC$th z0cA~fyalHQHZ^tS@x?KGw)L4}cOj3ap4Ak|l;DJ{)?gHJJskUBYx=|tB-Zt}ojDlK zZ>KIY4A8Y;%~CfXyN`36`A6VksjC$9I8h}Q0>U691qET}giw*hERG@m7?NE<8fJ|m z8WvY1p-tiBS2^@x!Li>IRfv{l&(x8eYgr%`;)x*uF|H_U@JelE;4=7VFU0%0raR+C zOjzQ`_7~`1hys81cjXlj<*}ZuB1u1_Jes4#_Hcsceo9DC2{$;wwb6o*@Zh5m#l8By zgWx4$=z)ZdP$S4jWxH6s zQ6JO;qC>602hx;-gbpBtI1sB1W~uHlB_T?|2IAFlCaa`F(T9d^aY!|h!V_l{V9dhg z4VIyLh6zw}>nM;2Lup19RZv|(i;2*@)B73(@zGb1P%<<4A$_V$67qwSqqoE~7B|4w zBVI$q)B?E!Bxc0aA^fW#O(D{&jDr{rdDoeMD##U5?>Q(&$+&T-FU}?>v_4{5xfSLG zr#MxV1E-l&`6Kov;=1|5sXf$RLJ20?gPIQ9-$iI|z1gyt!w97Fp zm?`IYJf!T?8z@Eh2>u#+kp3_{o|EZtk6mI*Xb1ODj{Tg(r291`zjcx;D@l8#EYYiTc<6_Kl|J67VV6_xTl;02my!Z9YWe_y z9lS@*p4OXlmfEZ>-!1(-NxFzI?|j$L!+hw0smRkjN!R|$*7{l2sIBE)pB zr?0$#oxRi9-Km2N?teC5S4Ni_iQ}}M+*>mL($#DIZ+`O5y(z|@mN1NIUL>X&N*+=-W=Uw2Y_X>{(6UPrJ78Nt3==|6?l)xh(Ot zJB~49FV++lx5W8k=Bk0|llQqW6DCdVb*tIyB9>I7J@@8)wGQIi^S@hPd`<6BZ(u`@ zmsU^?HFcj?e?WzroI0Akk|oXGvWtc{t_V{0Iz8oD6zSLH?)LXSRExne^9T!}ct1nWNR2DQjnnV|yMxeC}?_e4C4Ort3|A{18|k`Y^YH zW(|_-JxiGEneeG5&-BUe+)7~krQYv|ut##Q$MQ$&G1T;pa+=2eT8(JBYXJ~$sQ^HC z#T}~>Zz<8i*q9P)Au&NY69vMk5-%Y!0Cvla7Qp~*R&bYD7vl8?jqZjM2`96oueYB) zWKx6mrG2ad99VQe;zN&bv#%AlH-MKx_X-WgsGz*zv#&Z5DH56&DGc%gmFlbKN}#59 zQP)R2^0D@*6mK96!+LYu*1~^Tweva9{yfmfVyC;~f}RFy!Ir3Hirk%N zJzm7YGZvux+oCl?p`!>@Ns6gbLQ)9}31JA_NVY7nWTypy*(8VgOxbn`kpII<;Xh(G zB3DUz!Num}Knxuor+P@r4RK775q6S-7nyKE8(3|I2{FWYOUQ(3!iD6ERg=%QXiaJh z3DNW*3962kMl;$it-1%`f;mMHgI`}vhyo-wh#){1P(s3O(WKrpl0A9A8V;UDa1xp{ z@SIwH2QN^eJW!<38AU|Sn(~{mF`ycgMJ&XL%FtxGJUOwua=#E4bpdlnO^_mJb@ zSU=2en^%`9H=jVrRQHd0Rc(D3C%3*EN4$8JIt#x$SyEq2@T-908*gmCE9H$(-b8vh zutp8dnVteNkpL2Q^+F5-rq(_@x9uqmcRN!M9CANq@g0NjIb=3JgkNKC`7Ei1INMilbCrAX(ygn(Byz;NKw+oO>+!E) zJ{&ZfiQ@U4BjQf}0muO(*yMXx;_{oYLw!?S5|9i%PLnIvU>L&ao6f3iI1$Wm!qvZp zDSa@?hMLd=%m#VU{4vVXtN129ZB5*xNE`dR8i>9wR}VnX7NXBm7MwwRg=inGvtUdK zmkWhgK^CVT?AJ?;>a2Q2H^E-VlN!&Z-Q1Re zit_xT_-Ll^_NUM7G!N zyA`ygRb`tsUS)n|Zi7uxkh?hBS3-4M)aqYi_EpS6?KvT}a1~(nhq1CIVr`v~mk{Ip zdv-z3_&v?KWz2jWC!!v^?1$$QNZiKURocDB~uOL zUf-hnEmx-*w5O%5&n@SQ&`fZL85mSz#DJt))-XXV&nTSeD*mOFq}xYVe;2x`ORIRb z{>YG#_^7!=`y~ZM;mN-zONE?UmRN>;Hbe+ohDXFz`>w9^=i~`O% zSWhN3Z5gaHoM>*1(Q;ME_J!t?=mt*2f8X#ohiI&9Z*XiJnX~b!-CGtGO^pV-sT*PR&h=gAp}V5aWjNkPF?pupLmarY}8YK?)xzvxol*Dc_^9tXhJ{IibS zu?8e-R|n;dTKNv$C0+iqS$&I^H*xeNk@o2 zr7u2mjr*?7Z#l{DR|GeV;0`P^bg+P;=2~xFm`g_yhR=ap)gk_vFLW*b*9(fH@#>T) z{XR$X*bFX`7FaOwv~kLjrc;-E<-zT^;p4~VM``$3JpKoPo=6I7oU78n-YDQhRwhoL zc&Vi|^Xz=N75Cmclm0tXwo+6I)JKN>lVay3;aiO5M_V`bZ$KM^v=(6h!iURF=U$Kj zr}ehaYl|{ff29q=@3tIaF?#==dw=lTC5$bZ<*kC6WNqb{>T!3_H+ByU*X3svvPfOZ zMKT0+KJ~ne@Zn7L&n$i0<$QUbZ-n`tDW=vr??*fJnBRx$a_C?c_Oa6`(JGUIe^bbP zfNyX_VAo)b zr0rPuFtcxHNXTcl1dgIs#mz#}Oj!_!)NT$oZU%F&_&=bxP-^}DKjLsOC(a;Yg0lP% z;sBf4lFsX5SUuZ?VHwr8U7(gVxTFCA-Dl)alhw?V&+bd;oVVoMN6Lsau3 zCGtKRHbjF8WygK;NX3GPNJV5rmB=J^u0vr__a~pE{k~%6hl#BQ7B8!_n7y879)PV#Cov6q0Ss8fw}*4Ru6Ke)hpe} zizeGL3K%R9(SsN??miLL=qGhtZJvuFQ(``OHPt9K=2G?;GGUEg^2s#b;&alp*Eo;tjoapQXi_8_01#U=DQIj{dy+Bh3`wI zsy|gJ(%&u{%il-*c9eK7PmY?2bk$t^Hj$5f3XR`N)z`ec$<9190_Qh{Zake9dUO}+ zJKoXz#9ln-Bs(5mZoBIEod_>ZB2}js6a>p+9UC@Jhs+vFMvXdei*~^KB2fF#T6f2k zmd`DJV7Kn{tY8zk`Ewcmc(l?=6SJR>aFfF@a*!g;^%culEAwds%Ks?t(VwpN9ov>w zvH%1$+rRqHulu2j9WyO|KBtTnJk&Xy!QN^Tsa`%KXx~Q4dCxA+8gri*NVfnHYtP6H zH#pqkGfW02R0$HPMFkKvb^wGaQ6#VC*=pqv9^E@lE51fB)=`0n`n=}h6eRs{z08xd z73AN|`Iq-SAKsBr^1^B*IucH1H3w;)x$ zt}CBHyO;m$_CjrMsS-MQ$))?{f!{sLQJ{YgE>|-`GMXD1?&&}j)&V(`4c?KEQuv#<;bnB!R7I1~T5YFF^W*SU-v{{>UzWvKN&=!BMj<{U!y;H8| zcdyl_0FN5Z3QN#CaSG_P1+fk*#FsD3aYeM2gw)#eD-XCNsw;1^^r4>)^?obAk9M2z z9k_r~TJN*t&|ih~Ml@c%$~P4-%to{eMGwk8%lsfwB)EV_^iN${$&%gj2E6XK@hh!X zTtt4qpu9BiU;jy>=RiV|PMlR3*_+0^II&lmMK1tXx&C zv+7?eYz3lO3#@TOPypf2n0I5t%0H%F?VE72B}(nucC{#Ihjl}~C`Ytau0^ux6fKduI_{WaY7Skh&%BIGFo zL7uSE?f8m-=UizieKc=CJ&~3;Y#E&3L?f6C!3cvLuVcxgCKCXgt-x*Rq4C}q`d zUaIXG*9MpZnBPfAkFZx6zXzov6PLo9)7SEO^Cs8EtJ%G(ba2K*WrH%F1I#oN1EOrl zAfKvhjke8ZdhjPb`;5kkEetckU@!6|ET(l=?qAZsvuu|l_j~;3tg37@KSzJ~ygHH$ zw;@-D&9)WPR+b`IeYcQ)6R+2!*f>ok6d8uypaAIkAE-z3a}G3WnP`)$8~(ayNa|rH z3T<;EMOHiJz)JGF&stwLnJrQPz6;=^`rP~U$NC(BDt(%-e~XwtFt-k4sG}P3p+<+B zbsV&>NH7YV%A%KxXJO%(`fkw!^0wh|J80S~uk%;R;UNSBYb{2RqYQ64d6>4CG29x+9Yy z@tLWcyaR39cn^-;G)vybeG?nu+XW-zErJS9*APz4y}vNiyip85N?ga%jy7iT?S2Z~ z#Of4g4N@)vi{? zleV*b+Ma^aMXv^*{TTvF;I__1)5~QJc3EkKbUJb#(!w2r1dY8Gq8wC;Cqmj(57I`W z*rII*3i-n@Wi-dGdk)2A3<{zPb`HySFAN+lcAqDJ<@M0GPftTY#kaRlx?dUt(6GYI zhJ!{SZGZNKx1tR;(K{TaqX*-Qhw-rxU=9LTlKRrpZP%N}XPctCiAsNSg`tzD)tSOu z-*(N9QF%FsB`@`)%O(`Otf&9FhJ@u_gE2oL5T~@xW&a)!&3PM-u`VJqLwVZUqB#eO zukG@F&C8k*_rxR)h6TecQee6dJa1T{;eFN&u1gm|Da>SNm9TovAxhS2WGDlfz4hS; z*Ux`VXdf!{E`->h#dU^=^ia2-KNcvx?8kz?4HzyP$l}RiXU28^_~wJ~UPunk?yjL%!k7|pDD9icL$PYKQN~it^>Ic@lLaAd~5j+U|ElpBNLyG3?r8%S+BiYc7;R|x5tvJEVso5S+H z9)E0LF~eu$M`~)cmmpj7?Y;1?7x^VQFIO$O2-aiAv)55!#p=&qU}Z9(S6K5MFXnI4 zS$}tb;qdEk`WaRDDPW2K8(Cm{h5(yCwg`vA5`XsZ7K-3X-e0>ZFagdd8nhwn(x#nO zGpUL2akkKZ(_^dpx-kLU*Y|yxtO~Z)GrzdGTyWC4WyIa~@Dk3mZO%jxZEJt!(09aL zAns`$QjX=6rsI>$12K%Xoyns?!`PL!kGnbghjmO)&Cy3c3HGToA50d0X{kWxhT719 z`Ec;&s?5Dcze^wGnCh)(iE=(A;r_1aeIv}zcf3RFa8%Z^e9VfZql_W+lFI* zG^$xgIheP6(NtF3&@zcLp{Hxg7;B;0>wwSxo&Kn8?W!C>2ITAF+p9AbIrivBFFPcI z-tYPI)V#*|vy93QP0iOlb4etTg{?}#p`lm8uKgxDe!`kF)y#L0klOQ9ViHy|6jt(g z!fFs0iox#&x#Bgm1MR&i!*Oi48cWJ|vc#8==RhQ~3pChxs$Dw1JnsHZ{DrrLJwb|r z=lqR*{%7OH8|Wj`>i;vAiW5vgVqA+?1b2-HlKcDIX?bFB$#e&S=BZDFGAqp+&g3tK za;;pS19{}$M7Yw&Cpa0UKld}TBThrc9?mjoeIYeA{yENSPb!M4BmXO7i>Apf%dc`Q zweMou%w<$LFM1AMm`AqR711-f`C*xLmsLZ9vMBHAWI#zFkEDQDY_AamgQOA6mr*d+u7mN{y>AMpP$XQ3CUtS0*US94nQO@O`Ow+puXS1IlPaulqP;mE-jhfG9 zB34iseIU`uSQo)%CB^0D-#nt+Q0K*6o=QIJ`9V@rN>L6Kc+0EE&~;^y zrM+}+1pP_UI)s$h;*ZYDyC~EvQGc#lG8!bsdAFvGMV6m3e3v6;nTxv3HRxCE>*NER zBs$10!X$R`8FnzwEiyj4UEKkmXl~hZqpNcZJ;~*9R*)19+p;;Y%;x9p;nE9ywE}Gts+zsx@#O@ z55Ln?N#7s{viM-30eT!A02D`u9~+?t+R(v{KGJU-2^ZFV>n)p4OMCSjL>BrR>3^gP zo0FZ0k;vZ23XX>djzPxM&fLX<=usc!^UP4)QZB>?|eJcSi<#INx`h)o_W&U<4Z@RZA>N>I6b= zF7~**_&eKl?cALU%qJeb`>E0uS-%}_pF)>I-t-d*V8vK;d`{(QZ!#yd_E$^Ey>X|X zq}xOR8Zdjcdq{PcmWmS#)EPyA&_$Q2O*8^pL7gzq;cG;e|8%NdH!t3VXFu#WD!U!x zeYu4Q%mmhD1$MbRsz2k&F)Tyj?gT65*F=O*apbv?scR<~9TS_j9g?W;pVp@76WVPZ z^+>z>?oO7kVv+^V$6)+X^yiln9b!5*3uQ?_mFaPuZl3QPzHNw1+vD6ZX0&Jcp?`m60y7Ip;0 zK#um0hIV^_T(pbdj;v;`t9DmVm|QzV3HB2eL;RiSocN8}DF3=C?5m;I$(NJv!V93G~^B3Ja;r=N$7B#H1Avl_LbX-vi zC4ey-;M2yDgjkXiNqZC~#*!GzJ&r;`jv6C8fS3hGJ9llnIn$f1U&0md@t~s|<2PKV zOzmvXBfCAWGGLp3bCcfS6gX1gfLddwo1nb)AJ;CKU-R!=;Qj2GWHibvnLY&O*gk4e z)mC+%t}eDwAtCo9kOaYzl@!9LDv$Kg`hwV2DB~H!{ANbSZ=IQ?rB_T4G>cVs81(NR zKeLrdq^)$6>J~@*=+D|3Jy&#oMA$OhN}dTkfIB^Y)| zGVnjmSYYv#{5IsotkR%DVT%R?&O{@Sim(;FltthODig{kjabRuD3_xnkSwE$4=HgZ z1v}xw+*Rhxrln6%56WB1htlETb{N~e`&RpV8>Lf;OUId_CbO`KnGr+iMFZ;-V~-2DR86V1`5?^DQJuW zgqTp*m6C>3OXUbvEsU%qmhc2&_V8y+R*lNB{+Yqdfr}mj-CUBN4eGR^l|-)BUxV$vhNR zW0Vou5R8OjOFwM61u>(IP4mknaS-@~xBdpN+AOio^E zDk=`gGmm#9p;n~aSU@~byAr~FmuVu(mxW0_kR&@60S3S-s3l`gTBxxqU%cSluBx-| zXQWQ#Bw4&XM6FOZepUJ@*bCXX|F_sDy|uMs3Q`+|{8)}Sfp7oXdj2~m5AL;7NsySN z8JY~kVah?mc#hsG@7owr!?YS~Mq6$Rr%&n`(9xS9NzM!whl!=@by@4rm!7soS=i&4 zAVqoj6BIZWREAfdo^wWnt7R#vEiD$>2Q4LHr-BPOtADXSl=1fZV?Mfvg`D6r-#wQ! ze@f`QZURo!?inR@_Ti)*b0d|9+d4k*!;fsSWn4};&=Uc%g_UAJF!gaa(6ej!6(`P7 zI8EM^o@~#U>NjG1QF#+d!E1n^iTATUx4-VsnmwRpw1<=Dz|^=7uj=aVeSMFnx3V&5 zaEVuAsd>e`DjPg0;$$V6k=5+6+0UkU;l%_@Ek!H!+X!zw}S%&K`ghBQrB%q4(z% zQt;t<;wR-c=4-#>Biz4opB*}_D`hm}cQdlE#@7?JJ{mu&@6Sz@ zDSrd6420GsEpltDBKVMcwjAmc@4}pGZi%<=-tJU)olNXg%1BnGV7;R?30{X=+jf|u zyyCK)wtJh_()^p*AtzGnHJaBrGDtaEw_ZJg#}a2so{3k(Yf7WFm-C})0lpX4g}P^{ zmvZC}@JPQ@=0UHnROa54u31s|@pCt7JY#@(D!wwSGJFwu-UK#I9jP@@N*~Z8(~duu zBAQ4mN5w@HJS6+qS=6M_0PBG<9`ovHQma)mlD3DI3$WIX@bgACif8vjJ}J|3Rmrio z>(>&jEFB{=md=^enld6zqBuH_%>xOjN)vLg!?b^86jhL9-Wd$IgkJ{@7kt3H_9&2y zFcp9xv~~4Bq8Z-Q7>X93o66F52EI)AcL?E2LWZ*QJwh!^@vQNVskxcpt`0`yReUbf zU>WbaH#@E-mn7yVTB+cnqD|COqtHLUo6Il5D_jdyEIae?Qw0t#5t!kUYDJWq zsIO~kE(L3aAxO|Gl)LjdLl}f-Cn&(6!tdcnEiT2xcy$Xu zSt~0Gr~TILyh?i4H(l%hy2lzzkn!cI2a13Mz1n%l$pniv+2(G@!0yc-4|TZ9@npas z4CxSsP6aR*Y3Y1sLvEY9W>N>fUQaS~YEZ>UmvI&TCG!&zCFpRQ{%UmZ%LxQ#)wc%m z$Ly;=Orw`MBv@jGnqU-%gORY{^1WTYeSrz(hV|Y3={8^Dtkej&r)>GB9 zJMDD5(^zh3ghER6$tqK=JO?_{?^`58h!CP$!5iLmT3)RsFux?Nf{3+3JpmRx5i++- z#dp#N`dMI3i>=Y8_@j)PO{!J6@RK=NajZKUDii&>?*3G3=8M9Qas&hDcVnX4XJ?6d z%Hlm#C#t8NMcL~*-YF7c_wZk@?C|?dGxC@nmFuJvKX6xquj;N!4f+GEcGb+DMg{q= z2QpOMBT+rE8uSx>veT7EZ_RogFN-N;+N^f7l#$Njw6vvl1ZT5GTO`6z3o_77z$NoRAT8bKd7 z1BGVs%&gb56DyOhFsbI-m8)eqWcJbiiLx@XYLY{{=HzX%e4|-ZmHn%Q=JT6B^5(14 zb1=cx)zgNZfBR#wmNGmpWBu{O>K7I2LeL=$JUl&6s}>cnQhwz%6>j9O^Q~I2915)Y z?AA1L@z7SX^T68QRsCsEx7t+ulGHw4wNdaQ`>Al24NLGcZ75xAxk-&<6L@05Az zd&|Pj_Tz;1e$N5WUVK(n;?33B5Vee)c_LyMHz4<23$Uq z_Jqkc9AY4G#J5R=qwW=mo?>gNc$z7e6TC`suT-%zafyW659FB$r8YaXRnN99EBfv! zokhwp!bp6E<+b_}6ehB13Xu(71UMLX*gD~5vXH}+uP0F@hxqeM#nMp*zy^sLX)qg( z+c>gRRlhfDs_;gUWdYjL%(zul1J%wFTi+6*R5-nuSrVKe9OHk=VUl%F$P?QahHOT4 z+(2`N#H=5Wj*g{Q={@?x7K}b<5Cg-o7hm@ zw2nAj#GAlOjmfMzI=oq3&bVuH8sI7oZt>bK4sMC0Uv@$8>7HZT#w`& zypeo7yk)C#e9Rv?ZmNnCR;D^s-(Cg+l!(7+RzlMO4nPL&$Yd`qr0k=UG%*tbgwpRD zze7-)l#orIcX}@m7Z2s|X0rNn!%~^Y%HI0SXKw6l4Q{fE7iKhBR(db%Z^uU2rSBLf zaHL@cwmcY2#9F6+%&3{>0X7|F?o(`=(7w~Z&T6W8imf8UCXTeBUU4Y&$9qO&Rn%kBiq^scSrY=W6IK4Y!9 z{E%FU!}{1L4DKIZ6>W2NDL+}rxr5Y*uMykd8#DUF7W|!SUZ$D^Ya_F1(A-E}y;`#T zk6pGl{c`B77e=n_VyR}}mm{kohWM2F}Q$cW^^^KIZBIomK!<_($P7L>+h@~u)27Y^1Bqj?i%@_BOlw8u-^-%vL8mF3K7@Drm z!=bjhLeNVGDc-`6z>S^8uja~KDBLR8SoG=R4h;jb7t^O!^L4N^y<$GebDe-poc8NT z#LmAWS|>iVF5!`MpUgMx4N(GGlhcKmE0%e&pJ;K7Ct4E1O&P_gV+uBys1zkr zb4mHaP02#s99@`j`VY}0TWk>_YnR(%&DyzXZ0yj`9i6LfPBZ<#$H0s;5I>g34*pRop8uh(Z)B9gk|E0ia%His?>L z#H^90Xe5}KaPB&tN z+ClH?`*_`-Evqjr`;A~0g=qd8A5nxlY4-AXd@}D?+b0KOOCr|fZO=z$zmm2R-gbQ4 zul(+MTYIWAFU#_b8dtvT0`BV;VID(fK-ZfbAwaPqt0G18!h1ZXOrS|n9;05;Mp>Mj z-YhQ!cn1k4VqRRyD2%U#+nKdkR-M!xACrGCvN7gd-E+lKaJQ7x)u^OAG7cQ-@(2dJ z#JyK;{s$TzZHR{oI=p7Gi5#2r9b7-IQqKxAbgwqah7|q`e={;S0q$5N4R}(@wbq z2K0k2d)3?fPY|7&1I9jk+?0RLvH#%V)$~*Z9g@kH+PRzzjI6+7*FY0AswmBqwjSUw zSe|g;bDjFOb8thG0k=o&2)t}D8t8cxIZqNO~TIaw0b2F zHxC(%Gm40i`C6&!!yG4b3(jCJe^yZI<{0|ivd;Q}*CDPI0o-WLSAwJBazc?K87!5ExL@BVO{{;{BmaNV+)X5>XH>rc8k8tU$U@fC}+HQant!w>?dPtq3{ zbQa5v9M5Tf2T*&JJ0wQQjSu=|)EC06nKtvew!4dw{<*#!u;nxw5f=v>oHJYH+g-DZ zd#|TnBz!NUkfY)hJTD!DxsqNAL-F&@&bo$fw#-2gCG!+Wu@8KZ-Qq9Tlv@k?mnHb_ z*kpM=8-|g;+R1&h_02WU!t_HcEe@n3#XeR8@>p8(0LEa{RBh(~Qk(K;&Iz`q&RwGA zp0ayg&Sx{^zAeaS{a87}pQWM3?{%}HwA0!&mxuFL-YHub zzh0;Df#RiTAbAxX%XT*BsU6rM0BeSp%GA}};OYw*^?}GX+eEk(ij>B(EX?NypM89r zn5reElRaRSqqsi{V@oqB^Yj_Fc{L(2NDi%!Yy0i%0a=d#_H*@+H&qJ22zKK`N0zQF zUI!s4C)xPN8~woZ>czI%|xou`M4HNC|6NKEPh=nH27E)wGQ_2g#5J`OZinG?Y{+!uRWinSy!t0 z@-OTY+VZ_2_a4JWyhN?t->ml>Tm!YCF$iN*G^`ELc>ic*wf8qe=?GeZE|s$Y3j1Kk z*FalP7etc$I?~Sg?Wq3+VKW>J zv_`i$T^DzTtpuY_)PEajLLYyM9VlebIfW2Y|92R9@W4MHLvh04TEEJ5L^8nlW@Br6 z?N~o2F3w7krD7xkG)PV?S_mY$5;TajZf0$m-faJ&WXiyfczp1!AN|*a=}+dmZ1+sN zC2pJN8j|plKv+%zmm(gl#Tbqx6@R{dD>D9r(tXEfoaM5iN2K;V>+eTjGIEtKg_(}{Z z*!Q+AQxQJA441ypCB;o&<8#plq`^&oiv}R#8Yl7bHf&HpC)4=)4+MF!dA9}Ze~vco z5U?d@hjS|`1eU8&ewGq4cw4NW4JeN&Gr}e;B1APvNQ4(7SwH#b^FQdk=5+$^(0H~s zr?0avH;TRP>Ef7YsZgC*A-&|$%+ubvjlBtC)&Im7ub*|-H;(y|3(C}c7C-OcFuJWq z*|;LZ?}+rH3rPgXy2Kt|>kQnAZ3<>iwtS={a&u4swTkaVbr`2FQfyG~?}C@K-(h3o z{v!!7_EpBeEOEA6qr1w;;?)>LQ?|D@lh!*MblRZT?|L+bAjweTc^KJ%-^NxAKW{16 z;b{lrj3BnCAeh_A9?d6nxXgx7LR`rb7W!|or5yH`F9nP(q6T~7RK}g{0DZ710lai8 z3S>VZ;59{(&0fAwLZu2JWsx5Wx+JLl_WY!Vm6H$`Y%&@n#@Jb1qx*fC@|hvjuMw$V z^9`#E&CmS5s1{3ZUT}d7EWqtwUogmmwL&NC7^AP^q`~)aHP9a_4hpR-0>cl-Ip+{j z_%##{lQ+^T2qTKep#@-y;-Ns32IfJDcp(7RoUIzlu?=J(MGlF>48RxMKy z4X_`tDc4V(XYnksVbbd#+8B80L`x)HE1pSMiwS)Fk!OaflwzMA;)Nw4@kDP-B}YIu zVU_~Lr5$6m4B3vZF|{az<4MHm?T-l@f^E<;g&U7?Mv?*I6&n4~L1^>Ba-j$de$SVT zeVx#)vF`i9PES8&tp4$BjuEG!J{ZPwQ3140T?rgA2ut>pePOD2iZ0Tc!*GM55QswO z{tpqxx9u@>?@)hd$)$Q@f@593L~K9`OmpO$!VDM4ED>ox6kAHwMUa*C?lvr7#{CRzBLMSU;E;))x-JXcC9ay zW_K-ZMSm+Kt{_Pinfv+&X^F?!Gc1aZ&3&rxrF0U?j5YUJY#9oZ#Oeyy?iEmX?Ku!H z=e+~60M=z1q**g5(EPMoYKn}==HSHhZdfF^xgo+PDYKEIz!$QjJ8GNABbCT(79uHm zHphUoPWT4H0SR@dzq4luqj)AjcJIamAYwRL()Vp#EB$yc<-P!caCD0?oH%5PUrMguT_?fi{YhaH`T%xiQ8|BWLexNL z(Cbo|>V6T==3juR@Bak*m1SVF@SeWMVB#`}}hkuQG1FgjY%`ATig zi-45+nOI0+oJ_Bt2>0|x!r_Cj8Sg@ez>^cPa0Ed7wPKlAZ#&|&-f?1K49!nk__!bn z&8Op;G$1Q^tn%NWf~L3H!(}5_EcWO<8Ybw9L6 z4xSmmJOxEvZ^&fYwzuV8@AqQ|(C}+&CtPBR)t;g$oFsN*4?#z|)4J;u4uqiP<>^ln zf|ovZ)AbDnspu=FiND)wU8EekWj|q_`t^~88C|UxPm6+~u*&fIK`cr$#(1i0fGG;X z7UlcbLND8-rZtUcJ>i;DD@K1fMk zgw^)2Vu-5ht|DPu5in%Lb#7wAD#%qRd-uhx@_HR-szB|T{Ni*`G^M#^@R$Ct(dM~x z*@q&IY}#Kkprn_FVbB4{PjH$7L~j!2CyM|$4zwT(v5#EiO!@7KOqa|l|c zqCK?gx1EONsD*nLZU#0{B50*{b*6Dy(#AZPxK)iA4IMaI>iFgMM_}tFD5dzi(`ULs zPMw|YFpx7?%v~?MWk(51m`PA3)lZOn7Z?lefrlzN)sb~sTzEi6h-tpIwCdf&^~lH0 z&C)FLs3ebf&I+2Cnm@UHq|agw!?9hNfngd~YH`KA9!hmgqJb>R{sIp%50K3z|dSh!q>6#J8#)mZ?!O7RN|iPQEIMCp9eRX5w^aVyPKL59eW zrIt*8h-r=Onp}|5kkuH{uf*2m#F>2lF5!gzvHAWG-kD!DF*5?3$epsEX@))5<)eBJ z^bqY)gTTIUBh0Wdc?5r^BYcDHTZq{!qxT-FWu??FRR)xd_hU>#{To!v1j{7Lc<$(r+2sJ@8C&_8_F#Om<-&ZR`#T!29rq z6NuMMdEiR1#vwi8r*+O9E>RAH`Fhy7znk8wo`gz*`Z%vUS&HOsTse)x8Afj$18DSi zjSoyTv`-`HiU=9ZPLvfSLj=iVv00!D#DLOibObj6Ljjth!Lt)+boGnwj5vBQSXVO$ zCA50>eUFfFT^urtNOF3x{-bpm2&a`m0(bnK8>Ps;ia|uTzN-!^z{JBLl6NI?-RSyBL9KFaJw>#Rq4wF|pE3Dvog4az zk4N%{Hwv)@!A%|F9m=5}l!G(ce`6v;Oy;yQr*D*cu6B@*VlNm-G$UV(+Z*}VfyI$H z>Rx@dFPY7wvTAPco3phBIC5pK+e+VvP0iMCn-7xZrtNe#*~AcuE-#&V094T*0#grM zY>};p8h%}_wSuooo7#?>NKa+E`}IneVwBtOllIM;J0q>;i2}|vzH2gRK=qjB>6r7? zV9uK4q%sDkc;HU3%sM+O05TsR`Fu5c=xjx_?9^Bp4T&7~_6XG0pQ3m+zj{oP*XSt~ zl+OI-T@u%XA4(ktx=TeIc$bvTr#PW_v`LV((}tG4D+q2Rd83KQh9KBhfrpWm%Thx8 zU3`|4AS!41rn71)SI#Cg{Gr_NrF^$SE!ivB1)3w_@FFP@Y+?Hm$1){jR;+8>clmK61PuVd1tm)2)w@}a?@Kp6TCS@qEh8Zk=GgNj1bXP&@ffv z3hSPrWqDB%!*Umn0_2GN&j~k1fd9wVIYepJL~A;&O51i;+O}=mw(+HH+qP}nwkvJ3 z|9gA&tmkJI>#W%E;@R|4M4JvD50p7vuX^9znGihe9J5A)#o>^`nx+Rp;0_weQ6*Z5WI}ip2>%CFZ)U2ewofMdbQ>M zx^y^Se-ZtovndL@2tqY@3C3qx_%tQ0ao(>f^}-&1fpEp7&ywy-QGglK6s3VtL0MTi zk__sol3avP0mj^|)|F-rcCiTYV3N%pl%!}IzkGGTEdF+U4sFTP(?ko0V|u$EJtfbN z8%YFv8mtD<93^l2S5^jZ{R-0MDnU(C17io@9c zhvCa?3SUxaBddxc&}iXy4W_7$rCqR9;fKcBM%C~lfOpufh=GU@D3$<`;i~zY2rBV4 zLy>04OrtWxY!}Lh6{9cLWN6FKsw^K>Uw6%I^rk7(=OJ8v2IgUl90rPADkFF-)6B+$ z%euBGqYm7;+M!a>=NT3R*wTBxWR8PDYJJ98pa?^rcpg!D+wes@LjU) zLP0NaXyVQ)B4m@iUDtTUL0F$nM4HMy!045FkfLWxd~NE`AC(PGLnXT-4kws;u{+fa zvgt{O$mpsAjH^M2aN6tI(md@cSVnd_64WuQ;*(O^NmwI<%D#L6!)-clG)t=H=<}LP8i)B0 z$U!gH2l~b=iE5Yz9;oG6c-|SO2~`X{MI30C8Q#^X%+qJhK+E-&_ru54guOhb526|l z<-tM<_J6H9XolmFC&0v*yo&?$@0*`MgunAnTCwsQf`0>+3$7q{j>f2bCSR-)Y! zOsNP=GXR<+c*`gJhfK$k`E&Q^AJZ$NpXdd z$C2W?kD{+YcV#c#X_XInI~;gsDlodG!5o0$>{_7uufjeSGysTim2x;t(JbtDIQdUu zN`VzGNmSi&OBFEe;ePaAt<$hbv{zR8HHIo9L~)`X!k;FbAxaIb;v7wB&$wOG^-Y~8 zaGS}EJCe*#|8`YlhgT2mDIJmm`IulWYZSx{-%8+q+2RUg5DZEK63#OPukJVrRCw52 zbg(aSl!C{BQRF{{x4$sHUA$PXc&QVo3|%9;MKF zyU~G-`Aw{EI8!*2e@bsG{tu@^8EAt5!|X>lS_NY&SD_$G_&4;enN#n1>NdJgPB7HF zPeg?J7>!qk%QZ-Qpfe!>LfC~ELtKvylu}q+aFGfSNWh2iFFXjw7MLA!*+V_Zzhs3f z<$$&68qb(N-O@oY_=o`zW!S!U;_r|#O+qi@8cq@i+XYOirqNztSW)84R=MMtRdfUB zPV&;WBM6Yd^s^{IwzRHIpBfZgtJO!w!i)5igT~~9Epm!DflP_vbX3$a;PAD zduu)Alc-FB$46DZT(n*&v$wl1?dL6@j=!blZo2>#JnemEFO(f~M? zQ|tXG`A#gKl|y+YO-O}x)-+I@r-G3x!DBTtn-WCQ4z7f6c5n8lQ$loByqH=xxq@e;dDUZ0$z$J=9c75aIyLc1Z^~Lke2iK(#G%N zv_0(aYe#t}vQ&%4;JUF>L#2rG3`iYvXHs-!LP^msbpzr7n!Qx$749yiZ}qZ=<4_eB zBt1y0n@D3%wH2l}8OGQ5466|4GLi01l@tIbVnPDPhks-8QT;ngqP9AC&Y<72hY_Pp z*#{AJZ^xDxVAN0@p{Muu17gOx`}@#p`up(W_E0lf*u_Ux$44MT`7`FY4p4M;ySi6J z&!@6gxvn;kA9v~%=RQiy-PU1`N|toJMt!)LTG^={N-91XoQm}NKCt#77HX?_blyhk zY`M>W%H6(y)?|`Kp{7{Vk<@BY%;AWctxZvbV}Ch+-hs?mJgQf1BZ zBbctfMS9%)6BYiRrJdii0GKX`99=xUTQx9yu^GL=o}NY22bWN&vQuW8%=emXh2`&H zJ%bhAo}F%okeWCecXv~s#vm~Z8)-&PCtb(iWZ%q$a#`(9OHde+MTBt09~tDX-sh>s zAgKs_?Amg@4Wb6aKnwr5RfQ%!;!+{YEU~(2wK#vpBn$9Th*OrS2H^J*8Wf*1-hfBX zIv6a!G!o!r8ai`Qhw^rpf!9rbbQTyfmq%0*gss^r>iTiaY#1|;+1guv?6BaE7(_zt z2=NCT0bn901pVVzJ{dbJ*8re;lIcLryM@fKfz69Pu_McL@Mlrw55Wgn1g)BN-!A=5 z2#@A(=~~gifoYbwfM2FObRxd(UAB1Uy7SqnD$g0;{*>a}*1}YcJE{ z<$3jf>!CxGl}+WZeN>dHMJvla>VMW_IqfZ&je9?dMAdj2U{6d7PT9|p==;aa#}Oqg z`NRX}TtuJPK%3s(N<1DO+mp^6Z~ZZs8mK&tpzu1hi=PvOb*FHK*~On~l90|Df;Z-^ z3P?99vb9MC(4_pEmjZbV%k4llGn?~aXI`2$0?1dW?3Zw-OZ2RsCk^~AwC(Kqca(#lAzSMdi>>!lh#VIL#+NIfx-TM)wz!9l99KFSn*t& zQjwl0nFDjlra0JcF9(uD#oqvLRtU_MunjElC1M+X0E5+O3+?N<^2+vAGylk{jPN5NWaCo=aH(H|JSyYz8*@R1)Fd2JI}vWW z_S+Lezb2q!-QLu{elu(FuD;Md^qyqo&~p>Ot08+g%DfUzcgiE!_YFzoKrpRfMs)sah3pMl3sDhW5 zHQ%d_C^vI=xUHCLWJfL^avD&GRnb>h+?SgG3Ks?jB_TaI$5hu=94eW2nm5X?j6J2h zqS~6v9#h%i1q#`LzNNq~Jy(|Q|8T=`T^;ctUqMg`5`0GTP1KmQ(Zr@H6fN(JSGXZ5|;7q?~>mwX}qVqlmQR` z{%XJBt|i~PfIeaT*=4eY{0`Y4fm-=*bmdKGpD; zA#6BA6gy#PA(R=<2Mh~`xJ9W?!@X>P$@F|EBrz6E$r*0OwQYyr@DYsuCkiD6E{pV- zak0A{ZNc1SdF@YQ9Q!;DKf2brX~xw19;n6iCHx)I3 zZK4r@vBVV7t~GJb4IExa&zLM4eOY<$l8WeFdxdv;1x&+{O31Ej`f@Tu=mZ?!srJS$ z-C&xw3Bk;){n{j-c)xe(U_ylQlJ8aD*v6V?Wd(uTzS%XnSzH>Z8NUtrD=|J#AN*70b~N4C5$|L~I&QDcA6 z&9r?KR^I*i%$pkAt-q?Qi0k^c{%zLzyp(-Aa^)-I_%xxQ`qFyzw`N^1$0j-3iFlzS$^j+VEOg%Km9%+W7vl)v$F`tqCWD^h5 zB93zun;nKTY{dF&N<`+sNG`}b=Ma4TsE^c^t9kzx)$+7~*t>Zq*+&U46qg5*B>;0G zK;zU}LSJ&cZO-Wyg7nBeXcJHTWM1j5Wy$iE5qE8PXL5p88fJhag-7Kdre5lpOc^f$ z&Z)j!m=2I7h&EggZreOn2eYfz4&>KFPaxAE0X@J+lNU#3Cjh-x=bG<7rr<)fFNSvo zFKcm_OVr(t?eKLTN`Wb2Nl@~&({8NOsfQ%YV2q1Y;{?zVFeb^^(ocL{a2x@m{1?Cj zjLu3Lewm3Q0vu z+yH1y4JiWfb{M`^a^Bp)AZRW3ulve!u(&{0^TG8rhrili~$eK?#Y zI}iAd2o zYKI?>5p4x2a#2ZAS{>(@H3`0T9XF_ky=iHVH!Fj_kVCAAiQ&vQZg8Db>SlxxaO;~iS~YB*>BEY%`} zj+7Q}KZzT|;NO3p1#Qq;XjfUGs63~Tf*^WP?}{z32Mhm|*S*CV5`*6yZu_yCmzt!@VZ-$LB3}<7BlbFQ^26$k zV@Oo>ct@4P05QaB5)qaA&L{TUhO8+ddnqKVu9s+14-16#;ergr+9?I&k^Q+*1_dM=VZKgl^J1 zWN>|nYpqOZ8+3gzG}^~Qv(2sHl|7ah!hV#OR=1%Kc9b(5j6n^gpLaPkSm|(UeS$Mn zC1+N8+&87$Hk*U9gf-&R764pl{uJ*p)efx&Sl0W^qb^6HjRn?9!mNi%*tRR(^sPfpjWXj9B;EHHa@Gj20yl^I&so=)Y`T)`c>6&n zk7V*Qe3f_i#ijL+K0sJeE0xv2dj+tFzs`_ zrwnGW?K!fx+|Bakg~bkBwE42s-w`Yvn=|Mat6j#q41#Gj{Ff0kJTo_=u&mN`0(Qd= zt}n*dG+T3|yP`Iz)fbcf(N~inJ3P=iKUe^G5;Zp9((%Hq1t9MRhRM4BPaFpe11Zpr zWRZhTFw%X}Eky|x4cMB1F`;4@bo5DL?^c-=FCq(1CXgQNH6hXocRRiTJOia36OlbP zH37ktzHO)*!x(e9$08Ed8hraDCy&(LmV9+6 zwQS>V2nHC zXNCN`d(3Sxe54XgydDxG%l>P6_rf2>+|^PW$d=h^xMbm{JSG)SNQYmdWyu)+l+{6{ zYYbW5Q<}#+-D6Y!p3nAg;sRz|n9E29o(9n|R{b#39sr|`x9Xzu4iLj_cE`N`Jo2!w z4-Kcx&Cp$+o%9z$`JXIvbl8h#Bb8W(V>+r`RU~*rg#6&HLEAbbI@Qo7#DPc=68=*W z-7QD};^<}1iol-J@V;ZH(`K@2LRj*j=9Sl3!L%-y>PxL&Nq16GWHZ#_)d^w{VY#`S zA?F~6NkEEl4Db-fLVZ*Hj!z{@p?D0R6wk3J#lI2-q^D9|B~X&I#6(L#<9;sn7nWUgIGK0|oUtBrY>i9d=~FMBXYt*dSsSLCv(X;-VgLY$}$*D@y-S5;9ecBI^%}YTq~tka+30oW*-R?4v$X)zX-^Ak%Iv zLVTCKG}=;jYAfugY|HPi1aNdA?E^97pp&J>K~nrUM2Vp;u2INPE-wpQB1Hlk9#kSt zJ^)FyNs8$ris{lfkxB$%gvRql!G@ab3g%ZARx7IOMeE6__{;N)PYo;s9fCr4-pEwN z>)9x#v^tvzRzy*zy&KKu1myd;5lCz~S862XL>bhY>%~Q#c1lfw#jv+#J9w{a);N`y zr{wl(6WC~0REzSR_^cOOHmqo~U?$xc&4342x_t)4u;+7Iz7(QLJ=;M+D(^)B*p(C8 zhUqwOYo=|}-Iv8=b=M;#CbLG>==AnJZk5R$+en|2+ii<>{EN-LRyQaa6!Kky%r85- z5Zcr9;tc0Dzeb0es+pWV8k#U3+s(ZI|G~Mfm1nRERl{x7N(mQ2w!SAT7cdh_C_n_| zlD;0|ELSJe5h#thMrrFa!Mo4UvUy2gf1<^5mWt<}$`XR02bHA`eN@CUYRio+O)JJD zfquWto#LU0)lGu5osG<9oeqJ_2m0YBL!K}JA){RJ4B%WDPWz=dxZ4agQ#+r2`afaf zys6?ZMxiVlTR-a;QzqhZvR!i~Cjj2);8#&mi<+I=JVyuOZM>S(V7PRK3nanQA)Bc1 z%34D?#}9)@Ue%89uv5`OQ7}*jG6o%2+PC(pHH7AmF`YHZ7TNkO(uhSo@@3w2Fw(^2 zbL@FlKGJ&F5Vm_?v9sc&IF2$M^-2#7H9CbB`3HteOto??(;71(W6uWt1V#I7;?DHzi0IbNd0o7 zjpfd&?-YV*nqg&wO>FPaeW4v~-eq^{0BmeMU9_qrCLY)v@>Gf79l-KW<|jtg(aYj% ztj3ubb@>Dq_PAd)1AVL~o zxtAbMsqWKveN)|QYu;6SqB5Igv8q4p$WHxMIP0gUXCD*Ram@-UZ%Z7Wug$nFv*TOr zsQpjOR|Jp066ou(3xJkLjtE91=Yn_R;~KSaK5N3ekErioZ@d;b`V|EX$>2m0;@VQj z=1K=5mBk3Eh}DPOnlx!q!{(NbsSPDWE}?!A*i|f|H|`3t>!E zAA#qW5!zGwb3u8yi`rLfQp`H)k1~j+15nZLxvxNv7*&(mi*kPK;){(WnWK*e8;7@?wbJx~#(L0DEzT1wsjhndOd8 zCcT#yJP)sI0)VG|EQ|nFL=+1T-L$7l-YW3XoaI?^V!RJC&1wJNi9+LHvqLMVv$HSc zIor2pa($jYtgq+?R1wonbD8Kn#~;oW*Gz%(O#!q%hSzhJ*RqMjhY24XCt<|bl6={i z^g@JdphPB7d^a-C!yT`>-Y$>lF`7?oN-R`*Fez0-esvLjJN@quoZOoIcX?*Ybb|&Q zb}Q~C15mZ5nX92HK#ZPRJ!5U!ip@1;4B?aKpBt7Ygf)r6CaqG(hXC_7Hy8;yD zD@uH&%>VPQ$eVWO!!X73CsKe%+lfTiMNiSQBrPLAHl_tkluG@k3mu#~W@y%l9dIS9 z8(s0;20ojtB`r2(@KeHxD&}XxIh}whz5)x62k*mk7+&@0vS zq`MZFO6qDOx($N1mLd1#Vu#UK!k8`fwM4TZywDcB*NwbRoDIpqteIsu{#uhI@Ej6YCvn20V|&cckZVm)wAAXQ#N0(H~lXakVK>czIq zhw=-Y{RpMe1g;jT^Rr5qbO62g024(%^B^x2|8jFsdJ(!CGym<9v&e#0$9b1n(A<4cjJTm%w!+=is+72ZeV)BhH#j6zwPQle*$6|y zdG@L9r!L(g_;Lv)WLMyP$x^hTux~eQqxDe)9g}EEmr6o|sMDl9cmQ}qYMlL1t$4Va z-L3Ir7v9QzsF%LK}SkaL=}yGW0mP!(@n` z$0-fQ^S3l+2h9*u7Z)Wd#!$Iw1ZpJ)HV?iMKEg<#zsP-w59V?lrxDSWqi#cer!gF+f)44ma3rS>jF@!5-?i8je@V$Mp$;WQv_+Enn6iJ2WAgb-}0@ zJk?<$mn)HI?}( z9l=TM70(_{uTqSA{1P)u@EHggNKgl)dbzA*)$5274+?Js+_d~|a|7oYaSdtVzB^up|RxEkKZQdQyCn2R$J6enrEYjlY9E#cq7t@ZLOlpG<(o36wNTQHSI>K1B z|7*YDLkzk)Y9Kfl`74@EA+s3Fwbthm-_TN%)=I&%fKf0>#~2?XBPDD41TD$m7{k;l z(JLeDT3|51Wubt7hojGpT{Qw>+rJ%+Jf4FEoCV?FVw+ShcNT#sxhN;9o@qj@WXcaV z^kHYQt+74{-n#$KD5g(s)1EK&{_ce@nwBW$KSXf9ZF19fEVCy+DD0G@X6sIwq%UK_ z|9)~jlfS_4Nm3>Re;n^oUN_?q`D_fal3YxvfK!rkO|XG=lR8W&DRC>SIJzI}YiKcI zIA|iy4UUX?#+vcoVnIU#nE5d$-a?46qC^@UCz7B|=>e5+N|H3$??6CONCIMCLXl+;)M<8aVnj!dTStjT)asI#WSj+< z(t&^umjMqr+9ez|DdvJVLBxynDf1dtiUQv`eXEh9njZ{w(=6cJi*)HAhkQBE#bcZZ zl;t#9`jO`pkGP&{xg>cx|8%}Rod1hlZvXXoj6jy?17QhRp<>IZtK;)4O#=Ud?f^pS zkMw4D;T@3TTuE9v9$MiSXSP`9Ba~JL=V1zhAOncs`Z&O>gE^duGHa-p`O#tVvxs~0 z5)M-c0l_R4@O>W%LqsU`!;`~|D9LH0QT+sug9x(w5$=X?S)9}4xyndEAcoo4o^h2L zqPrBUGT?`pCl^<%)*&oqNd7qXCs4)L{Hi;LCefxg!Zgk#}MppxguA= zWtOi+|Fx|WItf7D+ zI4IG}Q$<{;N7x^hNmAjc`KM{}9YqZ3diFMfMX`KwKcQLub-b{(@W?bi-OlS}5XyNo@H5`rRGS~_Yhws{>ZBQ7rDFPFi zW*VixQyzU#)+i1l4$#jqU*n-f9ROfgij{1<{T_+)pz6FWH8o-2-BW&4Gvm|M_de-+ zjt|Xp9Vh(5eJG~qIz9-i%famdCW1;L!gsF(H;^};4YQV15O<(T6;=k4G=>W2>YB(2dSa6QTjFoZjzA4hzd<6`As#{*& ztiAp1whvA-nvdAb=@nsglh__GvVkesJrrg6+YNf+y{=5*%oda?P8aqC8MS-2prJc9f z5dSMK?5FDx8el|?$&fR9w&qBdW^jbH#v()(72DxbrJO9kzka7CS|lWkh^lP{Ng#gJ zud};RIhkY=VZe!RCsIwsyN@y--M}gL^X}6s=tb{~^Wi}Ie;Oe(h?pql10!W+Gv^jj zd59-RyKBh62GSXjdBZZLIsh1Uy{|@$y6r@=8KJn8qY*l(5g`O2P}e;c)Pbu=gHYxl zB3#I^^NIrR_X%M`s$pqL4}s(hoA4i?E~VJKrcC3X&;*d5!vu$;%DKfC^60>Gju~?L z4QqFeRjDE;LV1sgs!gQQCG&B8u*)|C1>6RD&Wz6FRSKY)K`@ht6akzd-nc0>#ms1{ zSaOjw{pjl<`L+8TaFj{VdL?rs3;<6%D51df9n^{!Nx^alz=nB zQ<8(A6%PFkod2$>+YAqUfB}!t94JK#INX^A8>&P7#hs{kz5^};9RzYQlz=82R7mp= z0p>~$KP7v{jN&7{VLt9DyJRDUa6Y>YDog7 zcOG-QZrwg?eA8yDO8b6WX4tzLyb&RnPxcpPB`i!L5de4?9<8-{dKFIYks0=}sZ*&u z+*gXN5{}Y`#>_08o0sI_Nu|y<6QE^Ua~zMGuzC8^bICD~2oOW;d%e*!cPnQ{9t=KA zIn!fF?`SYcb>jnf&u>;TUYi7iO02#OJeBM zZ&P;@6%QA)$s3OyoX^DQ|9D{#FO`-%XUuq+@U&4G{dt*eFQb2Z@oA@@c6l2Shbg9y+j@p%#><*=@9rb95 zd~$E$Tm?{<76GP;9HIT_T1=c;Qs-SVP52sp-#?I2Tq-6LX3O?Tms3+sH5_2@)ct(0 zlZH47h%X2J4ZzRea=1csFqpy6HZQ&gj%*caA`ROlun8Yrmd++c_Ku&pmL*F6oR?Qf zs{o|Mc>d!Lz`7ukTD%sI|E2ObF;Z57+=T4j_jOkUYQ_s5;+hA~hm1%V7=2i?a`U1M zT}?X6=4S3@-pqDq*{@O4%acHs2B;Y5l=XBn83@VRpj2F^lx znH=Z;4sDRzUL=If69V|TFJ2~YF)rb@Q#I@jjRaxE&qoTu?bxU9GvMI<@49sY#wNYF4QCjV{ z;+E3N1W$b?yB#U;Z3aRMhLMuJBBO;_bvw|3iECQnbjAP^q)5j)kZinLc#XM>y6t}BS?Xsv9t?p(@WsX{@ zL0p1=WQHKHk~oegE&$GBHPVz}?n1-SV;z$(e`_hHH4q3xpTA!^ zzfPWQ*gn(-6E2-Nbd$q}!12f?@nO8+2dRjqjBehwsCRh8oAYkj9VE3MbpDVSC}x%F zWh1QGP8wWt2rW);0*jE2_+^rE6n!e5Q8bHx{RDdmXteYOvl?C9BnML>j>l%PzMR-!rI zD}!WC9}oZqNwnNhzrOWyZMSy~>Z0Gh6lf4^tIY()N(*o4 zFclzDbQ?bEb4u=~qAbL#c(;u?HPO*No6KT~DospeI}CTYn1p5RoZeus7lLqCT+i4x z?5JH@&q2$%4b;OB&9MF6vj93}i;zYF4pGoKD3HYy&qL32?XW^H(Q8bh7{n)@Z0%T$ z5aBL7)7B8KFx$P3z-U*c-J2j+Zut#mrhH@3U@q8wn97hH5o`1gjaL=IMNhtxmGdDw zEY*Ec@o}g+P_m3b)r3OpRk$bQK(H9hqArNW+6DH(X6*8K!L6@PF#(}F$%Zkdj5D;N z48*7!;mMxT{j?tH#*wCCLW77aE==Z7#t9xQ@sja>aEnfJXm|@G`j;}#i0O&SfrBoa z8q7F*3YEmOl)>!=_lp%BIIHoV>6wX3vvQo-aYU|I!4hznG6gIyay{3(MUDJ>_GMVC z;bHX%W;g7cI&yzVtO2^QDo&dYaxC5!vckg;UMx~%VQfv&LWvNC-UKdokys;3{MDJ0 z*`z8YqFm;8(mPYpj`W6`KPzR};G;$YlwVePlmi`)2sVnaMk14++r1D-4A0Dg1m|2* z*vgn!6A6aY39M)Xgt7zHM7)U4N<9hEDxY+9#=&-n)2E;4kPACownJdHrU1KA9Spf> zR%uB6g%X_I8c>dXS7B3Zs^hg54`v8cJ0Qa20ur&2Z4h17w|i}0YqoqE^J&xuOQ)d! z+u5;W_hPEUeiLhG3S~1K)E$+DjEu0b^nk86;#8?9=VECebaq>ZYm1kA(D}C}sP3Eo zU!?|KNTdib7w1hfCL@~9Nf^)U0XgQa{X1#7BhATvFMwy9jK32;-U2uMUD*u{9#dje z)k+ujQ-s2)b7zKQ-Nv(xgM-Yrn-1X>$KCmw@}~91yo1_)>Up4Ui886!4;rFmU=DD_ z2`xC7l^u+YWEZ**VyGx(CvvLeX7sV1v-7nxmy!M6=&4hyQu~iBr*Vm~Ppnc4U5};DbyijqR5NcO z)^L4gTHu-l8>mMDjH&flW)%a53N@(NF?~0<1%?DwT4~;L@%eV`930k7(hT$!7i7)z zf(oqBH;&>5vrvUc>)x0t6_J&G-h z0eTQDNSFVGjnuwzc_G&2FRs@82|eqhZFd5K4djQV(;)%KjP`<(q?q?9$x531$9JLE z!_g7&F3}_xYtF;UVvug6m%vhKM(dU)iChD`Hpvpw`Gj34K#H&_- zJ)0kEX-LZWj2el{sv-H%RRO=YSNW(;fKNwn6XnLs7GqrS)=uf@nt)l>?#o4{M3?xh zwf>G;mC?Voz4kVru3f%|>yP=WwmF@;-rY@I^sMLYsq0{g{(9@9f0=3m)fQv%6-vl& z5ds{6Ww&cnjXGWq=QMR1rX{*UA%#-%Mtgc!l4312!otPlK@1r8pew?16xfXDfbOo^ zIst@~u=SUDXp!nAZ#XKi{l3@xD}G@wfz# zJuVa@61?@ea$tL6nNEmIIbE37e;ImsOXj}h)J}y+;VFyJ$+V->Hb?^r{S^*FDI#?% zPw^|KFomGmK^-+@!S<+VX_`c=fI|mjE!!z&OobQ-n4U~^63@tuE26rEe2*0+8lHhW zx_fG5W8AN-vwuuWr!IW+W)*Ppo7FfKTtHM9EL(q{%YpPhGfzFd0!?kud#=y9+L)ww z7`@ib`uGEaP(G@|x$o}79QYu*69whWp*`?Epf+CbZR99J{)%1N07f$~cU5yi z@T2Rn-=JYxKEjOq!3THdexJ*s_UGY%7IJ#fmOilr+GgxD*=$b{fQjVS@E`aOTSwRaup^;P}eXq~S_V zDvT5758msC3tQIYwz^}2wRj--1X~YjybuxhJ?*&x7%J8XPrp6xXbS!< z%WZgt}FIFKTErK-Wvd01u{~{4&m4 z=D~9~D53rgRybg?_+(pjX?QP9;q`sjw+rIA^Y!%oyO!$>1E%g{GAVham)d>Duqag! zzsjj)+r1Z0MS8u6i-&wkga5eiYjZZoXD3oD&c3M+7nh>{SNp$UYn-VZ%lnHH*;u1v zKD8G#_VU=metV=Cz$Xk%wqBoKy{yvm6c$st`v-jvmDez=6mo>2xIsoS+TyS4%j3w# zt@$eZKpBy>rUq1_9bm$_LO7!^TS z2x$V{0jq)3ogQf*6c7{71!M@Dx?I{ZPdB%8C{=cCuBIVGKp^xi0s>CG)ZGlj!Pr+Y z0g8#iAi@KI`OXH~Vvg=Xk??@$oK8?QV#rH7#>ydg7V@%0nR1|wNK~BjuspBFyfr2x zA5v}{o{SPT0WeTDG@5w!+^1M}gQc(z8FqLUR$26ay7E8Dxjqva_MD=x^rqab${~Gg zhT92&rbRA1fJN=|9=pHASyB+25ks~4Pkq5f%7e(#IE;h8#h;DoCm^TcXKdPiZZAwm z$hfOlLfhW^{XLrOg@S-nGXZ-=gtL&3oBd7Doq^lp#P-^&=DaAoONEh>u)iH=@9_ys z9%tqR>Yg?Q0CO(;xdWFljBUbL!9`pG;9wKlZSyB`9*7(QEE2Qn;xUdGh{n*rkLWv} zu-PUO7cQ`Yrlt#?Kyt}8Fl4$%)2xX_;j{T49}qljN5p8pr*ReQu>G&a2=@) zPMk)n>^3+AGgpqN9ZN+!{1l<%?RTOeKIvgXP#2!F`qYG*`9(+~C3z43ptl3CML>b$ zP5xV3wE~4)9o%do)_s#6v<7NMB?-nP<(-JQjx}_|UNI3an0+fbY``uy$TVYv&iEXR z$=+!jCh4K{%tK1swqTOn!aSoy4ihY%6b;S#!$E`|3J;1VBm$i^X&xvam2=;Bp-@%jt*!hShViE-E+PNYFWXdDkjR>d+HI@ZPwM^-5EO^2mkog}{I7AwDB48qo_%2nG-HjCtxYTP<*8RX7685VSdv z@H0OW_=4A;Pyal0CrSpuic)f4-Nttf)Cd6x&|}T$gC91~aKdO9S#+)?(Sf&4t@!m^ zLA9?yxsRjCYI)^AZSEa&OuDcgg)Rf^%cv%x?s)(GYTbbySh!ohLx2euw@1c6!zk4E zIUzzm2?(k{MjN19bIj8+evaCx8yie1y%Wp*C|)#Nn&zs|7C-@7$P0=a@@?DBx&7yO zppa?#J!NK66e)?J^Y=!@m<{+Oy$*8|4wpSIP+wL{dY%ov9~XCv*LJ<~dnOpXFa5lu z%(u7?k<6RmUZ^xpv0-jxk1qP-*hGq>K=re9GSY*S`rEr>9_mn-=NIca*hI%*b;S@R zfK*@Do7jy_=XQ52#lo}^x(wZtEcG1_WOjlN+EB`$tm3cO# z+wDBo`kC^D{LZ<{-_-rrs?MdiSF9up!jr6gc{^b2)225-HoC(UBU^w5Yn{Q89kJgt zxZSy(u}Zr=tvX>Vp*L+($Jtaq$rOTp96)>NLTv5i#m#ob|$tOv?do=6u8?QA@XoPL+D~qVm zCTBKPmuj)}q@vRgdEL(rk1TmVfE8jWep2O**M1;=bZRT59w6^>5&3)`>2^j9K?T`( z0_UT8f&jkSc2c${l!G>PNj&){@8NP~g-3tY)36PY!B39ki<)ZX`erR;g`uo9n#V6&z7_B}c@Bjnp@RsdhcjEx59uH4y>MFx=*p0hy3x2bbxcF6PnZf)(8 zGad{7M1}#)%@;7kY}nj*=)CFZ(!V;r6jhz)nRahlCT{`OnUFAKM6G8j6I2PDfn3<0 zVLIbsyKx@Fnke7rY2`!e(F(xViGf1Xcjm!{V7S_aSe>l@=&iZ)bKzh5es0>%yKVtQ z=bw!ohSdD`83d-cvM=PED#eq|>Sc#No&*P0ktsaA_^)+gO+4#&HBi?Sz0ear&J(3Y+u@xl)tu9SikNh=q_pE=IIWiR$!U#Gp|K3vNka$mgDw%fBy9&Pqg8!-QSxPB1+=Rk3LLh$v52_* zwS{t`VMLDe6%;@#m9qddI?%SEg;*jM`7=Z|s_-UN4}CE;KfNIpbwTv$ZX(1WDQ5X} zc@3MM!jmv+u4ye(4QqJytzo+8&M61Xg1o!Sxw!+<)3N`u!J4Ty;ZaMbTg_CRF9Q=< z;!HsrAsCCIjf<@$-cKxiru|avFX!ykR3kFgM@-h0uFb$ye@_8kYTIa(E8Qc6>16+h zt#fM61lYE9Y}>YN+qP}1<1gyiwr$(CZFFp_lkDDWpS!be>KD{gHJ>rZ81IMtnuezS z)~knt3IY6)%j`!7R3FG<_~(IhkIrTkaEqGY@ntP9aBBx`W)xJ_!=|t^WF0mi2i3$L z_HbsXdB+y)==EP*sn?p1uH?yO6>HYs4nrP@T<2I@sZl!(Pwgg}3ienQCEd&)R$#hc z1*@TZm8}4sQWWfA=S$MvdcwqtXwn_;8^a6yW8T-~y0s{;h8ls~c;}kvFcWWG-SG<$ z8x+_eU(LcXEi8HrTCC&QXFCJrm)g`lsy^??X`S^?;0Ej42X}R#8Rd;w$vet1F8n#D zQzRQzJ7B^!E2Zz2o+2FhUGsiq@Sk9^!O6OZ;bVZkSPrqudZ{z-Qv+#zn@i7y(`k<` zYmF^s9~^MNEwxtQgXi_hO{F>>nePIp{dSrK7ue1P&c6c6)6Y^Btzy*1a$`wt)f2w8e$O)v?ELeixooDW8b8`r)B)@I-;1jA}N{oI$fg@Nz( zkvRajx*3v&eAQ9h7aYKdl4XN1B^=8KFpR?D{CD*Q7Gg)2>FkH`lCX;8IPR>umn-ld z{VL~v{w-{!MzP{1n6fdx5$yVt2i|M5-8C7wefNk7yz>gCEy!HCJD z;{PbX99;htV6>!yO`0UDa5?}#tEY?9Mr!wE6c9Wv>(@;k?%VUn;YAEP>u9)YiE3+VpHyJ1^~+F%4EYFyK#`+>G?! zR|vFY>AIsp4^JZ1SSGDVxi9_^w4;#6*jv6EpJ^mMm}*r8EcYhxBrzHMJnBbyB;cOH zaxK7Dj2jIOfkn8=zq**Pu?qOOK4CAR=zd)M1YVMCTQ8rpX;!j=5SoR=vt*1jv2SQu zCt5985n+D@{Bc7&$JYaaDrL;Y**0}ljenMY%@Z*7!ge>cPgGb4hh2uMsgF1nm*QRxL(nVB1FHhhA|i{iV7ztd#XZ7(~4yj@pHTU-Q zV%%W+acKKW<$(azx!hZ=M{mCCY9dvc~;$Qo`(t6r>FzIS7bB|;l6 zG!JJ8Z{aC;$wiTZ3nif(?Mzr5gVZIg+@GV6n5{}Ci>KrH)u(U=ZUQ1WXJTt&4PQP2 z_ueW09{qcdcl0}Py0c!KaE^MHcoI!x=B7B|Ll|%q_`zLnh(~!g;g840gLIdr&dj6g zCFGeMHwqNS#k#oaXsv6zP;r>1RDHW9uG_}H8hU9T$W7i~52Ya}NCD;^fBjW~zbp07 zs`PL62O1DU+aJ$kfvER|Yvx>c)M-xUHuU)y_|my3j!qgKMzS$qT#^tzMDwvg_$K&c z1PlN*rP1)Pd!^3D0A&(AkFeVzExVl`dy~o#aIq5`ziWjpw@Z0o5`N<3UFU9jnUITB z_qqT0IDCaY;bmeXvjVM(Q-SJOw(IEd{7IPAxI5ay{=K_Cd&7TjVUbTQ-jmYw3-hPz z4IL69DVhi{VNgM)=pi4+^%(GLWNBD9e-VZkWMm%ztwn}MG*P}gZ!Mz3 zh<`SEP}5dpG{CSgq*Fl6bi=>h@JRVXAmqI_*_&;ft=JqH z*Up&fXK;8Hfzxm5v_OCIARAuw*Cgq;!B4g{w&+#qoOe>-;zCJQsf-0q{D>RWogaV) z*aoWNv_#*7<@%dG=bZa8c>i+>wRImq;5{AA(8vo6g@`iD7!zxl6T%Zze&7LNjMGch z38D1R3i-O-4q-hae<#0Z9dDBEn<@J?Hsdwm&(>$?w>!6n^%oI(XRnq3J6Eo^W?AZdDt>=Zo8z{9~A|I9qs~H(%|v9bw;;P zzl58Ki?m<<(QsgDyY6s>TIc|H83= zQPtKMoDS+LBIFEmf;CzKQ#}D{68nfeFUKqe(Y;Oojm}4-NTDm*`S;I{`?x9o7-Za6 z($9o3dujoW9~*b@h1e)zginEp?Of-$Y&lca_ROZ`1R@0ZU?+{6H(4L`cAHwU$)^?< z^FhHycWKy_vVv-9fN?oLG_Y%05>^drA^k`i{9ZEeulNI6%m68wnE`;O^phy`HSykC z%c3&-8S{MC)bE@hPV6)CP6^6Pd3VQf(diu`= z{Jnd$#0TTqev|O*4^7j!0|D4H`PJnS(Qd(H`3Bl3bGqtwkG|O>{T$N8e(l>AjtEGg zT4*O2-_JkZr|WqlopOLP(@XhQJnk3jNA6d7#wdhOr7hyf5t9J-L#0Y0?sxKxuLA3Z z3rSA;q=dWX+sDk)mbBrO8|*xt2&qoh@0 zE4m;6JL+5NmbbN+ncaAxflq&8CN-lWw+75Bd7QzGAPdnXK~(@7Wwe2)FSazok}rvl zXBgQ{Pp^>i(c~c&&!z}J{e7qohz5=$l`<} zSQaZV?zo*e>QDi=ClldY$_%^aF!efOc0YqGEIc^puE5~32@&#v^mh$MAwaK4;aM)A z#!4%yvBBtL4q{&CdQiM3v&Lk?zrc^17YFROf#!RqVSUlSE6MoBKpq%rW=KC8jX?=G zNVNR?`A1OkXQ{kd1n!5+^Obb2MX65<^!#=wkjQ%^o8|`;ImJpeMkBg@={MRVgZPaf zLh2JZKiSJx7XuwdE+WQ3 zu3h@Pfv7R47bs{e1AXA zMXNnuqciCJwoVk1nq#K-85wgF>r~_yROig=7Z;>HM!a)6&Cctr!<5hynMPfi151K0 z392k~UOvn>8&*`&(j@#r{6u<{`;Y9)$^HL_%~`n8^5cPNl0Fg4u82wSUVI)?=h#!;!xaD9`pG$Cv`&K zQ#F2xcDyo9>K}J;$e+Di;(H5RWdep;pL3=xM4pvqu_pxPDyBiaI_@25wqCpKIaVwT zKi|Pm_IjJ?Q+76P5q$Ri2nB6kpLybU)ty6UT=eO$6|PtNiHMW;Hh1cBQ>HF3Mb=Aq zeedSX=Vo*1Wln-Hum=4)vzACM-_Fjd4~7uQ=3q$=Astn#OZn0k#S%5ojR7vI^HL8< zl+2_sl`g<3&}1I8#tbZlBvuL?r92FgG;gK7KW!?#TZ0*%l zS=HW+zY;L{CuILgxK2O&x(Y3aB#?R9z!-@llJd1$^9v78D!jEyDB-c~Bs|c3kSNSX zQRJ(vDYod%C+b19SKkM8sxxADxEl<>&tqqD* z*G9dZ&*$i?4)3qI;}cN5-?vYUUM~eUsp`6WVF2XAix$#3*3{i(-T_+uL>M8b1YXy5<$w=Scc=3cUAA|Vn2iRW-vkYAAm6IJ!;EXU> zg;+l9R}-BIms?WINYU;C!)8?$r+dlY?{u)`A^yzozeDBNS7XNi?0k$WVxF)Y`7deo z-d5}ihNw?lueM_ag#kSJc(-p6yI``7B+U1=oDUmwuIN$-zx)XeoBy>p%XkTtO4rV9 zWk35Zeh7s7oc$f;{heWTlWj=W6y=d<+}QdQ z8|Ar8=l4T{@>U)xfvFB1M~AHKYB~0)#~F5avJA8N;oSA@{see-9+nrABLr{@iREa% zPZiHXRV97sAdby>as|$hS%6l$64btlAhsEah!OHPXeQZ?XJp6FgQoKRx(>ZfulEFR%C)YS9 z1mxHq72JnLcmP04uv8qgT5E2zN3|wToPE~QG-@W@ujpP4RzA+V&6OBjWd8BAtVI7X z>i%>&@StIM%UE?bfrd?kaFrKEHRTX`s2^ty6o8?ug%kG+u4Ek_^uiR0|6N-kFja7a zn5Ks0K^!m1(O?L9jw42^y&O{r?P(O9B8Q5?tK?f$6aWwwNBS}v_OK&1yR((<6j`P{ms3K{RhL0u$L$q){nN#0N&Y zb|RBV4v<^ku$L@S1c~5vdKD8lP~)&kB>;WKrH)}p$t*o383jh@!sc^eECqMCfCNHO zqJCy=Lj$igAv8Kn@l*X=l;8kSzlu}8Q)zTOplIG`qhHb1f0}LBv|B~8Tc`mJ(RkAS zI(R@g^9vH@W8yUI+yfFs=xB!;c@5=tst6vHiHByKKJ5M^Be~W#bl8l$e2q#K*Em6P$j@$Zl+id zhM7vJPez1uGAbLCoxU0)5eU;Tj0ZraDiR}x(pLfES+wP(8CE1}WSl4S13)cFYh{9p zJBJ-ClFJqZ6Y?mSc^3te5<0hr;l=>cV=K{=CR%MvBnenk>cmpX?COEf>Za=k1$TQ2%2O)UI@#^0lGZ^N`iTo|q3! zaJzcl82+=}%aq>Kfd;9FU_LdC6lN(Ci+xyB&y*F* zI0Cy)-xv|Mh(l$&990l=plZ!eQyYP^e-Em4dc0l(*nb^mPR`tC03!F7z7%O$BnV~A zb!rG|Pi>=CG&KWypSWTX-nkDjnlN}OBIGK0phAa5?7=ulpE@cLTFUhnZ6|r;HuD*O zbj|Ur@guHoD=zyHNqIJ3g=#t?F-h5K#4;x;Rm)cp z`@)ew5nLX{&0>h2jr-dKOLc2E{vek3R&QqrV2~u&5_Wb;Loss+q<28jWNe}UvkM5qk>u|< z^n~QWU`Kx2G)h<=Y5t?ThO3}SSR!HX3LcVY@D$1R8~D>qUY=QK7EOb6z5esMDktZQ z(c*AU3S{Fo`*w8h4wiEVo(rXaf%6R)l-;h2q&{J60K(Y4Q!Up3_YAeVr5EJ>vf+tm zib5hpNgkm>e@X96JcE3IU$&vU=&7Fb_#d5fV>iG>ZSbfp$jyUwxDgnMI1m5K?CX8` z=>)8;8V#Rwv|c-DAnY%G*r8GgH>;|WHFC2|tf3E91R*>`N2(~NZ^4UZYm)zm0x>tX zPx>r{#^m}hG!PbM&a{GfVCp32TnP|OmZXkcKH!I>m)ta9`J~vq2*4?bCms+YudBRJ z3Yzha*EcRYNob9PSOPLTW5Eduo^b&rG^m(e1{z-lL;<2fP{>rho7>z!ZW0!_z$m-1 z)(l&NS;wb?&+BOpGq8|UIm)U>Et{t>`TPCN+MaV`7y#r|nMVD=N<0o)RSMZuE2X>Z zb8d$=$pAyh2Hv6s8juFV`~en^!rUT8M0OU+(w_)&Kn*Ngj_P-$opAkHa(@Q0vX8i<#)fh)4K@|kW6 z6~^~Yml`dC=!|Jgo#)Y@9H$x~<-Ih7MLpGlOQ3)Zj3U%O2v}cKxfKrrMVD7YrlK}t zMhz|`!*8UeBH!r4Zq1kHP9%mG01_7FH*MtV;DS(%UZVnO$}XoR^lZ^xJRGBQ^e{iM zcAm~J^bL0oRDysukvj~&)s7#grr6gxc^zi%Q$bQpnjvFXCTk{@BHKtN&H6OA3F*{b zMPx2P4;V?~0fcS!(_&|&KQlKDR^=CsTX`xu%X!?PuysO6m~5Vgd2XQC$@L4o5wCE1 zWjv4gLp^@iGyS;TAjFgEJzKvn%z%r=yI_EO0aqCRq(F~<7cxsSAyYjUBNxLR^?bg$p32L1-t!xe1(aRrC`$a6-}boK8f24^6FPC7 z8&q>nH(33RJHCBX*T}nd2z~2MO7$4bW(V;*&$##FR_7ok&wE@>0pEudtDtV@QOj(n zgFaS#=NI!Cc5}LS;r<@7QQ+D3!3Aj{7*_ual|DI|OXhS4yfW5PepumiCAzkMyV7h6 z867x{D*(Hdtb~z#{^WYfw|Mi$G+PFTdEdK!(7sA*x{`_7iY!o<^}w-qxFD3CKRN7F zEv77Lj-!xQ(xt+uI4)4G6tq;*2U`>GuSJ=a9`j_-P9YLAD(~ZtuSBsK813{L&O$OB zrv`D_8`?u2m`3_Dv?tDB?u0RUG%0;=K-&U4831G$BE2M^QbnoLE1Xx~4NDD(AiH9# zd~r^kT}Qn@xPSh`o=U=-s-NM)#267)^?@6&3+fl$$w!8fInpM_R~F{WCg1zTvYY=f z+PE;8P6Sz^$#PA!2aeXIHgVWA%`%cx)PuCJ#>}NxE`Ah;5ArdJxJY7I5b;GTIUN#9 z8$hWdbDOgZBFmC!Fr!=jFBTf@b4Pe~X5$U9A}fRa9)5%d!VJ1HtQp3oNWq_?aZP59 zp@^`bzFA8%;{FL$Svl+D+jQmu996B*m^3k&Xo@LDlS?}lhO3cyTJ668m^se-!j1KF#)lE8Rm{66)dLfjguWk>BQIqB=Yf&+cAV>%?~{0en8KD zd~msL8Qgj{cY+(BJcXph$Eh?&-?bIoP8mMH z{0pSB;bgcRE`W%SUArq{60jg9oIqxx{r+z|<ZYo`VlMugVczPp5^_~o~$ULKS6UM8%kunJ$oO3XJpaEx6djR@3PB* zICk|%penFlK0K=W-~p;5lP_E&e$>42Abz$7Aq0L8_8U0BNy_w}l!W%5>I+a7Cf1~d zL~6i{&V&Q5BvQ{M?b#L*(pC>6vu}7H)f9CEG+6!dP&iITl9Mx6k$$cV|_V!M{cJZK-CRV>1?VP&ASon;#Z6kBS!Q^oW(W;79u3#2Sl@XHOAu1%Xpj zn#7+`F|v)xA2o+DFiP7Wa$U12J9UgB#U;R=!5-G8K^jv~b0Xv8? zyrGa;XDF+=<;|{ksN9~X_eb&%N}J@Y5-9z7OVfF&ALj` zYV&3KJZhyeOgB#finAO~Bv=a^_A>@79y=6N+1y|Yu}|QOg%RgBybyo0=8-U=35-fO zh%y@ti4a&anPU-xG?IhSyHaooJue`2baGTJh8K^!C`9?1jf^5q3=S9e_o_(L1s1mk!$PVpPJ%2c$%wYgMoPx9A(1U{XMZ7orzIdU21`XO z49rfg+dOinEQ92M9^=})SRnHU(qC&+znvahn4iBcwhU-X+eF#w=iQ2`PB2Y^4SV_ub!kQ@_wz?B#3QroD_F1 zPn>a_^m1&Op2%y^leJJzYD7Tx>BNK`D~Gr^E>>Hzn6!@w@FQIHx}0BRS!A~~-Mr^u zhKxTv%@#yViTHi{G!1*Hx#IN{9X$ajL*29DiSW2Zy3WMuzrNyVoBxKK z^XUfHeK4ME+J*+s3B>@|iBX_cnEeH^f-zl_(9T^CBnRGi#(4zSCj43_vNEgfQu_Q0 zi}p^yRTnnA!QB$dV1JH)C{=-r_ud{vv-{DEeWbBv&?*oUSTG)ls9{YZq_qtalj(o6 zvpW(r=k4jHq|ANeYLQ?BP=)BO2v!paTy33wGnuuh+`F$ck2e5jFrq#%!wBn<6Vt&w z5S8Yp24DHSjj=m^g^P%!-{5k-!+;DvYQcm#7A1Arr3!3H%@)5PH3s{oH_fdfE_{O~ z)R)!fT-BZniJd2tituvO%W`r!?P(k|!No<{wR$i>AGxto@Vx)EIx^v!Wv0A4NW>9S z*W_JI!g`JxjvWI~g}8Wl1HkdZSD_=~h8zfgnd#trS?I6W_c;tZ1^2Axm!^JBnKA>k z9JAHVPj-K_GAg4?E?rQVeRev!OaA-HmfBFjG5Kq( zer5b@J3KU2<8)ZZ^F9+!HE;h>H2&L8^~eZ&hf+2jP9{MF@@`R9@Y3kYu0PV%4v?b2 zN&Q716t@6q0m~P8cJEpGQ)THJS!@n>h|{vJ5qh15cL8tBTIg-EOp{L$O}3)%*HWdl zQzWps&h0J3c)-c~N8#^@(bej7cT^5ZvDtp*mh^qMo<%^(21D%)YuCu6w~LSwTRtro z>uFKpq&6b$0rjX`yqU|Y;{bf)wre(>x$1r|_xL2h@Q7?($8;RS49e-tElg>%b!5vb zYZ!reStXXqcA=?=IS35l86s{Iw|sFEH<1y?V3c{!1CxP%FLsn}sTckYMopGlwvt7s zD=S99;EtUF`8Q98w0F@UQZO7jr8KI1E~UtCkQVY~2hTJgrsF0IMzAS<=bNBa}R+q@c`(`23FAvBlGvNfKYFy2war~G*T5ETN zvis3)uG>=5lh9+zMp{kqv;Wf zAcq87g!b5E?hbs1^>2Z|J(|paCxoRcgOEXARCbC1M4?8CCuD&M>O)9FAT5zuN zti9|NXiopO0J<}{!t%9pKjv~}3#fkeixvZ5I zT^D*k_ARe@pFVkC*a9JAxzbWf=M;A@yH3X* z7U{`An)6xoIL*8WvO>&pu&KT5kr8Ncl{V-Hsk@-Ln5THFc8lRG35AJ#d!-Hpi13Rn z?VIe_F6jWwP^>FFxRghjDl_5G&Lz|TzJQQZmKNA{G$vtD0@LCK0l{_CIhIpx_CxJM z{VIyCmy|2FR9p8t+aKnCTxXaOX`3?qDJF5OsQIR`Fc-~I4>Pm`jSurU|LPI7C|(Tm zNOyf&9;V~{qCo1ttM3M}3Xx=2A-{eM3M5r~_CkO<`$z3|%?a3?e;dGP=%k|>Y<>HE zu1dUZ6+WdRuUanD)2bOU%H%e;d}kUr%mctU@ZJnP()Oq~=P!%F6pMyoxx$z^i&XTa zH8UR`PXUIb-p^*D!H~cDar~IYu*fc zWdYb&Q~JdOJjeL;9uxiAHS@SCh%swTorUUIT3V6)%bM5NT44s?0WJNLX9i5`vc*xw z2Z~JV=;!*hy*gN|OTS~!VRgz6JqGvZ{1IZZjdnp!s@Ui>|H)f6GWFb^v+9``y8}*I z8f1c4hPaCiw(ald2Bu2patbL7elQhfjTE2*<+&7DLPhvrTop9BTPMIVHC9M_i>I&u zbVl@_JO5DP>heO+R`IVM9mlOEJC*>tP@@3HTXK8?$hPuy(Ov8iI4u6r^_Pu;c*@C& z^{rru!gz_qWFjvOas}FH$5tvZYoa;@7tQQEopasTZI%qF+UD8W-BT)* zT1bp^FHTwm2r_C^J*@%-^j>mY>o;=i;9WgcBa;6<4#u30qAEQ-37F4QX$Gcn^Lq0MXpp&|_s0pF9q)J5J zdP&tO0vvP})S5V85&O3sr(>Ux=Aq~#UIu!YQYBBirVG17z(%HP+s-rr3+Q{jT$S2t zr`3QWii9YaLRoFhE>HGrWRZ7L3L)35b?X|#49Few<~Yn!5ptW8R;B^5j1{ogh0eYZ zT5~0wb`A`i%&Yqzcujja*CB5Wk=2atBNxXWZL|GVtG!SfwnsuKC2ZlIReDEu&30QY zv$>$Egy$B;4D{iF9H1?$ft&q6Fute+NjLsjf7np2iMpNb@o0PmmLi_juLjhB^Os5l zK%knaG)}(_2Bko2LFo?$1qwj2+n}Pz=O<||G66%_hqC?F7>QG0i)KEGH{+ZY!ydzd9i?fahn1xy_Bac1tTn^D8 zSmF+C4i{m+vFi+u=>zf?83F0^kc&fU7YT6-H1nl5VW`bf)>;i&;ypRT48E^y) zMYo(K%4f|&019-C6Ae;*LZk9v;#K6>kmB_$gjr4y9z^IAB7H|F<)%Ac{$A;l)^%yD~X^#*fw15IiV$Nv}xlu)RV<Y`?rmL3bLz5kfDw0mxA);x(am$hFQ zUyA#|hv!#(hA4Y5T?Oq%0D(bSLsq3t^kz!IeWkwvOurgOGpV_k$8{`Ox(cvIb&M41 zm3JAgWa$K`8>Y2P8ZgNo zRW8TXlrPw$&HDGRyAIb`NGmn$%LbfzSDBm@n*-jV#5;!|=KP2C*%o`2wl>u2&YZW` z$M1t#gg+czrVY9hnkap5C;pQ;!@7`_)IcFB6gby)o1v{Oh{n13Zk^?q{z1#y?pfW? zIM-irBqzUn3;}w!1%H2mdmlBNwBF8E^^~8XN5?=*s0RJ+y#4N~Z8>0DAJyiJD?oJE^9`8IU2D_p^2=yXhwECAqeuI%pJoIepPW#0s4`?-kr z7HWJases+>3%sXy4gQ`U>VCS&Qt_dW?vx;l>oPSqf|~xZz7^%MmfK#A3DFB6XF|vm zKK^Wqjj_yOv=WZbBVNI3h$W>iizMaUK1)R1H{F`)m6L#OfI4MJrwR$+j!2Zk6#tV9 zOU%j^SOchJVVl%}s4N!hX|mZgm}oehFmUI@6KA@2hDiloheJ(bmsQlW#2!OX zBm^_(J{S~RI03YHoV@>S@Nyj@T~Z57Hb{L1G6%qy&t*lnmPN5pUM@ok%;}DH9urOX#CO3w699(m#F}1g2<`tv&kERagZspjh0Hix)hxT5w$; z2|-P&GAv*+ydthWm3NGBeWN# zpe2cr;@jXJ>uowvBt);z?{3o<1T=!VsV;n`>oYHFPT(Wx=G}FLi1rYPLX@cRO%wnS z*JWMTf5tZ~W~To#zHu?L{4a3HYx172zZYQ^Ap?9}Y_6EwTK3AnLK=c*=^L<{~5iQN6 z%~Yx?_cw*(ClV?Yrunc{N3pWvDY2q>@hbdH4F*k@uw%{W#-1FP-VKuIs=> z9%`ZS+aYf!U(coIsc-yi&Md+vX*A}k z9QziSImA?C$9>q#2<1x)h16s6@(+6zfv>9gssW>N(_ti_cJQ8{!}d#uxll{y&LjE= z*#a}wt4ij)m|}*3g?Ad1Ri_!C!M~oK!78g(uy*VT7tV*#U9-yGj4?n|ok`Lg!hqQy zxNqsBLXOcUEK47IAn;^GiO&i&1bTe}g0)7gYDAE~k$j$uVlhknJI~B)fX%`;Hvth2 z(|&uG!Tx=JvwY0+jmMtHr?jYCC6+s@1eMmTqgc{=ylF^x_PA6^=~DyXARPOF1{mH> zI+v+o`Pbo*9s=BSEiSf z9B9(qfGn-X3A=@8x{?99Q3iP}%*khtyaA$KDkSG?tv_;eMaVHD;kS=$rN0B`0;o{B zw9e`X=qVMz{K5G4lx%3Mv^UNZUV5R>k$p5{%B>{@#Fv@SMEoQRRfeIvE5iIpfWc?s z7x95Qm_}!~tTBGCvmTNs@X1NI{D7P<%DuK{pExsLIP?EIZvf6pGp$1t?Z4z5BqbGK z=e-Ldrow~EiHpQp{KW=a7zb=vIH?WB-#LS(Xf6C&D}1So$t;H~7I>`Ty2^bG6i@0? zW5X@%#!w5HCz*70-z7yT(pRe-uver(2#^5n3S-)ttf(%Q;0G7<1pTenW!QoA4y^lW zE*BVSMBEZB9)Mv=l#8NQHC_(_YeCyTGLJ!K*R+53TIqFCFf#7;`N}dEhfjpL@qM#a zDiV-nEc6em3^lW=5;x7k%O-H(&pq5V7jrPKyR@@q4VXj{fr#e0gs}TMDy{Nbnn|;r z6seh@N3ST4y?nZHXZ<_)K0PsvvArn<0`tgtuqY1RssLS0oVT8h!+HLqDMk0srW|(+ z$}k_2mfIA4d0&@Tr|W_?x-w45H;WXTwxjw>YOciY=LhpO(Rem4PVx++Bx}nOLm{yU zMhBVOC&|d$bn{(4>|(XPcx1TzHM45h2IHPN6|v@&0f_e>(u2OX(u0`Ng9-{L42md+ z>QL)i7JxYC;^nKL#+_aQb6{)&Jp*y}NcdRU^_<0Qg$)RWjzBlbJVl&3mo>Xu@O}(tM6gR6LQs1&Ib~upjow0W?CY; zC7K`SDZNi8ItR;8J3>4fRSn(AQQSd(VjGR!s^;-Y2{@AB&wPxGFwJ4B!eFfCue|v0 zya3Qsao3jok$JKGa|}2ZSH&fIbIdB?+bsm?Ef77liotrqzK%;(SWBKqJ_r-trabKf z1Y|a0Jk94x z)>4T4J_sjnyKyw|jW?m_PuSx$WVo8fO&)j`-SZm z7YxP#91Z#pK}uxb>oFvhiT-Nnmi~R?d@=}U6i7KkUR~KT`raybu~Ro>+fiWiO=Pi- z0_rMTw#JeDRNI0Gdh}E4vHDWO91n(a_ogrNcoAqFZtUKlI+G%z?^+!2dqSR{7C@Jw zehp?@KFAaapGxOxbvokVR;R>#=7~=uF*@RQ&#HFt5dIS<4^k2*S8lx8C&{Qn6c#TI zv$xBz8|TRI@}@%;6ebpmXkoV*-Xa{ELbLg5`oO4kzIy%P_IACjDSd)RCHIs@;olkeX4K7b@Y|MB8> z-;sHw)LAtHjWOTf`}(MoHuZhQ4frUN{=*jzz*(7kmv4V(x^>^??)5_j%fVq)mP_j~ z;k|**cU;$qpV>R-> zqh=_4caf-_X}IuICP08qd{hCe&YkiRL@=0-f*5#n`K>4(n6zxhrZr@$HRlGM>!bzL z?T!*`JGxx3GYA=bB>e6<(zyS(sKPO7_ zg_g_nUMdHp{J|}dW)xJN+9?1EuMA2JJq~J!^(c86;y#Ry9Y9c8GwG#sPubY=7-9&87R zIYkw=0x2@%w9M&J|7AiR0LHsxqV6surAFm@4~}jd_O71I&?m8-eAGT6%7y*!=7e*S znaVji0|`7o0eJeJ;S7D4jl`HV@IQBCVxBvuBk0OWM(V!920qp4tdui!5IZ|IktiFD zXlqY5^-vj_NuB)02@=a0NGv5%D!ozGED6S>op9OaqmHWdpS0hDx+yV=_&T} zW4O+SmS+g@^1)Pyzl7QbvU&ffZZiKj>pUy-|G9yxhLM1ylDdZp0CF%Ao`0%3g-<%c*ZH~g&?D9NE6(6`WPUGarw&7A?IN& zP>34ix^u6gpV@&cv6^($#0_k6ahX)q#7u>#Xj43gDj$cLR^zj>H7`rQ*G14?XU8Z{ zE;PZUxWskqx>$*#q@g+}#*Tnz>}sqNz;m@7H)_TlgX&bG z=27VzZVNqSSAG?b^xE>LcuJ~V(&HZxiWvt4nqmvRi#fO=3a+*CqSMMly~82fpT?&~ zK0P^3K{OL-7-9~|l1@7y%$Wh7a;0=zG9?YUm|C#kz}GhrBs~sUH)WqX8J;=RW!&gCj;SR&Sk_VQm?0 zKjJJeMl~%e2EuPvoDB?u<>J!;9d491uXU9bRz}r) zF(?WNDS($gQlkXc=t9_v!2~lYXOR{Pd^RVFgr^|3r5tJT#67m1`prF}n3Kg5VQa0Ih_polqqpm9%eVQA)0Lv^Dh*Gx*+}gA_u`Grim@wOKv7 z+H(<^`~C4>lM93Wxx!Tt5B>zOah?COItLzHMPz8{lR2ULg@zsiFz?)T@DJx;+h36kXG z2Q+FY=7OV$(%f=b5idN$a*<>UBCe4?YRstnhxTlDP+Xpqz2YngGRPbTka4ESxO`Ki0KxrQ91dc6`F3saaGecA0Uua)3#!Vs7#_leIh~c=558PHLFmxO%@(+@I+6f zp7|@dAx=>xl-wssB?lO9UWV>#4g8uC&X+7eTNhRdn)lJPJvigvG37bV<^<4FJSTNhR(8?C9AInYIiKu4zcb(Rz`MH(qksCaDn{O7r zQo|JlB;8P=jVv?&N~D&ruk42HIad+j!5X&djT*fAQDy-a3|r`^}~siiEUdG+qP}n&Oc5j&cwED+qP}n{&LRwF5j!Z=<4d~?x(Bv zv)10f1-E@exC6Asz-ex3Uaf74m#0}zn*$YKGUPf%Wujf$g94Uj)@c*R7TTsBdT5v& z{TA-pjZF&(Q)R$t)6A`hW00Y@Z8WNLo#8@B3hjfhRp?=DQ28FrOxZ5+J7ZV`TtImg zDV+Xv?X7`PP9SAcd9GGmH^S&RQ2U%FMF-bolu=<#5%wb;Ji9p0LSAyxteo16fd0ME z*YMjzA7_AgsE`#U}D~ zYC^>j99SBMz$fHFo->qxT@{XuQGmc)37!o-zaz={?&yc*QNNvHr1H`MV5+BS(P#q? z9U5qN7E`&ItgUz5{NruL2xJSgz_gtrs+9(!s3#gzZC->7dMZOH=nw!e=Juh;&JR9D zA051?q|&%Y8}TWnz#i9@rXZ!%ct|uvA>(zpha&E(6r!VmCx)zok7T2O({ngJZ~BOm zRpN}}H6PWsvZ!C!Qk4<_u;;Se>!{)#>Wn!t0D+P-`-)^IXkh|#+XmiuIA-(jRA9uz zM~xBi9=L=XsCV;!>CHuq!o>@8S(h@g_hrOuvNXTSKhp%rgh3ns~=iNsj%(=93DK`O2zdAz^8@4 zoS%LgZ|S^ac8D!JeLrcetK7xO!Es+VysU3Pjhbxx_sGTsrl zodK=sq*_U3cQJupHQtfUAJG4}P%`Yeb-G0yx-{RjrWyJGt#v$vq&#suFw(r{y61sFK&ZGoqnCtPweQPcv z%(Xx<%)@(#?ei_o+9Y)fH~VhXk`UtB1%nzY)9ch)GWx@$=-3PuQevtg9m`aK;kHT+ zD2UX|=S|GF_d@xp7Bt6ZxO23Wq#jyvdNn~%l^PCT;f%1dB&nCsPHD}-<29MKOF#^? zsc)D_xPPRN)dH?u9wR&8)Kzg2`MSlTe` z{Ob1HB@u06OScmp;vkOlB%E~&@s!l{cPvfaxk??axl#OuTe(=ad2iCuM|2zrWlOp{R!zdLAy(G@NKeS)3ug~Py<+^CogFLK}9Ql zmfZ%8oG3}mIrb7Ie2R`Jf5GjffCxr4*NN3>HM8z)4XwUDL{uPBREcO{x>-Y~r&H$W zq?^T1NEBGur=4H6M3Ox)6~vYKdegjFpXN|o=$VqR10=)}aD7Fjp3)Qu{m?C1fgLDH z`05p)G@xWvr6~KcOt2$%WL{tZd;$@;jj8;l65rAR1x1~ZSQI7F*gP;^AV`PS0~yo- zPX5G^nYDumYp3ks+fmXvVb3^l`(u$LtO8Wld+3ag7oY_d9z?V%bixkH%NODzoSCMVu35N-Y6O;}KuvXOOPwmQzIzOryrmKgCvc;=UhTwZS8pglf<7%}!egm` z-Ck_Nh=H9U5R4%aVegUVb4z?Pc36mDdXA+tIqF>GiheM#ZeZk=V+UaXqjV!i5i#0< zIrPducj&9RuApm5{=drx%t5RKgzyqL#&IqrU#jZJ@ik=8sa9ZChLBbj%7YR$Lm0uM z12DCsa-ogS{-~H>qGz;#kpSOeAstk}f}4d!WW0I8*itATjD5Mdq4m4iUCDgg4XQ8%Pq80`G!xX1^`W0!iC) z0~CJpF@ZY%l7uPvO?PVYojeh*ydr(iHd?C*MM=6#%q!Tf(CKh9xaqo2qW<1 znhE2ZPMW3*ytTo{9L%j2N>S`H*P?rT@uYaHKIZf?G!O`?V-=Mxi32Tq(z6y{?(dQ< zIED=n1m-yB;09%QSZv2Ap;?T)4ge!KQ5IKBx>RIwLG>cz7;V_9@@?e!Uydj&B7xq=}@c16!nu!*AAaPttdwJkUG)<}7 z21F8Qf8P~srF#}49OB~EJ8l8R3z28?H3^a zD2_(9s7nV)UMXlHRv8h7=SjNtgr`c_?C)U~3fc((-rk2rJ+N>k+MxaNJ6N-J0cr(7 z3L=%a`el8d=c>uOD(${+`^h;s%*d>|Syl|sQL&8Moef5m?#&zJ#WI=l(7i6?XL~5_ zpGRxp-td-uaQ&&SKcRw~GxAsOzb{BoVo73`?xsrp5cBU4<#^|)Q899Q7vE>Bv*eF` zkQ|%<1!M3Wjq)9pV$N!yy3imAWL9w!EtbQvzs;}CjA_Tpo<3c(ujafFY-@beq;|Or ztoPA7pURZz!(}_YP8joWc}Bum`4yH-gzeJd17j%}?|*pqjfxv-XGGeYY{wMaw67;~ z)HVbkLa`&X-K@hwpo{`p1Y{$(@r$Y|&dCP=+?|8bq7P$#2gJL$6~{~)DUf`kuy4>V z!}_w2A&3^)$ll0?@^|2jQTQo%D?&?$r8G-Hi;vptKL_cGcWr+7$F@&r*hf*0^mHv zcrbr?Rz@eee|robKz?I{xh?^wU}vVm@$H46TtRqk;IQ>BDLb%)pDD}Zh}GV))F=Iq z?Q5qPvjjmT5y}YP%9hxd!N_hhpJ450beVR@ZibUU=vkDN2A?#_e1OA4kQK~IOc&&t z#wJ@TbXG0IpYF}U{?jaDbOx2)p(|iRFP56Q>tCCoe8*E+4liHp>^1q493>_M0vgHzqgijD$8wfmQ#-UZF_sBsQaP}#C+Oqmbo)}*np z_)?0*3x{_!IKMP@HPE&G44R2)fi}sGHU<$MB9t5Nv^_MOyr_| zA5cHk#O+pxodPZdO@9jE)}~}SMi><`OTWzZ#J?ES-?FJHVeur42PcP~y}U_%7(urU zD59Z`+5lDpg^dnxW``nuENo-}zlVkTxdi>je#V_JTo=3B67%=F6*uJ)9tnFsvNgP& z5P^ko(0_0T&OU&mUy6%&8ilI%qN9iwx~HDDvQw8^Bj>ZMikeiJO5N zx+<>O;r#_i=>=FdBTeJ%>$I0T3~XXe>xGY)xY;JX?Nfl?OF3XlteyT5NEc-He50HvNEys9V4bDpfG$Ou*K$p#(w zLl{U}UY53%o8>g0$5F9S5<8M1!cL>+Z!F(|#43!Q1<(=%u-DpICpa>7U-_e5sv%;p ze@cAmJ2Kuv#6vYn{f4uaVOYP4;xg`v6i!%rX0*s{27me#qd$TZKj4DoP*`B=me+Di zSXEjG+seAWA&5o@2>ydO0sa$*KmpIj3QaF401I}z3yLyXx|EHz&|Oqo$lM!i$R-smM!pD&*fN*A-J4%D4%p_qtB0_Scf&}oHXd(O4N+b zosbITwJR_xp)OLw7?lj1t09BD3XNsN(_{az+C^ zDFSd4OHVknt(!o@8fHHG8tg&HZ-X#0DUk+$W`0=yg(=@Y)Txi$61AQes&i-q>>_oe}ccb0y_~yORW{X zb!)@bu^E^!YMD53`}rA|XlsMVI!K!9u$E{3Cfsni!_rCrL*!h?{!oTxmM?=A4QbNM zfhGlCWGLHRf_6cAssmGk$Osw&)+?n3gF{kRRNp_?Go0&q1>9G;OxB+|H>Lck#`o29 zp61(-24w!xo4(p^ezpUJ{YRI6opzmWw}ph)cUa)Ds7t@GUX~?rqFi6yl$gN1Iuv9` zJCM~H0khoUI@w&$#=Ok!rq|HTqfKjTs+8^XG3WER2=<)?AJpnON}xrjyCcZ{cY5Wk zpcj3Xki^-S1aJe|^~+OOP=x#G(~i*NYHJ9C@q>E#<9z#@vAakk_jmhg|K;VO0EdwZ zJ6(UOH~1=vnchx&$gc!Ho~3=m)P+!JrNp!HlbsaC(wILq!$`Fue`t~errTJyB37Me zD_>xo8f!wXBG7AiLyC2Hm0xr{y!D;x&O^gO>wGDuP5@or1Q-Fuk4V7njIIT={sGDKit{d0%VD>+;cPBKNvDSH>QzGZK>W z%eOH%GFkG3cgQ1m>p`{~>TR7jdk$U0@M+<7T8}>2PdD+o&(l|m|_QRm*xL30obFFC%JNEb`pU+m?j z=X$u>#IseW1G?U}eD%E5X|BFz|6H%H52|>vb+=~99DgB90Nl2xj=wygYIlEI3N3(H zba7)cgtHzL#<(*pmV`^5k#sl#*%p7r0MhmBt|nYRF20h7veJ$(KPRu%x3|~b>a(|L zc^j_RCzjBYY$?x%VC%k2eyrCz^t{W#=i9eRIWE{Xq2 z!Z~-7=9w(71P+p^9ck(cTR)I|HgOy52x^MQg?7X8K)zt#Zb2J9jpwZav*SZPpeJO1 zd#dx@-~YJQZ&~DN5az)f+?I9vvvS|*GwFqNu1MJo=%gaocLhp zPC{-3{gOK@_=(BbsXpc)G2P4OC!EP;f83TsRW!+n#rhbEG>iF5jQWtaJ{YtCR1~A_ zkcA>>>`VUn^lX3Arrm2vUR0F_AQJN0LKXalhNuPH)c-4r~Qc-;p9v<9D;MTeT7=R2BYWV9W zrK9Ve*kiiH&iM%9-Nrzy`qE^4nW&P_ae;3YHD@Mei(hN1K2#kSun{iOHntnz4{s%LBnViK zn@H$N;v^WcBl4WuKk3T?++QYg%s4-dZ%N)9f_7Ne~s3xfX~$>s+5}@^6pjZ zhr{Zr#Qyyk-kd&w6F-f@2lI@YNM`A%JOF}VE>;hA81Gkav11r?l5va@m*4EQ$C6;~ z_9sqEX!l+~0_Z{iCAfi|&<$zBOKvU}-RFlr6qQY(;5>Sy}g*2)S8P)=i zDzABWgs2D@Qd!~o!x6%PEzX1{e&~l!=KHU_GC)KId27={QK- zeB^(E60L)K03}M9R z;d!}o%ssZF;>4Zn%_=Dw^&JE$PU?*_E+R85HTV%Z!TVzu7zloqSrK)69~}a%uOtl~ z)jU6kcXl^Zu`Pyd^SyDszWH=%qNZa5-sPDKeA1>PWLP-h$%`il1k-dnY|u*L;{Lrq zN(`B)0!FezirIP>tY?v46Kh{Tf3C{jPCCCM+deq#M5ZbUkG=V@2A(+5emhPwX$_1r z4&6XTq(Vf*3j2eU*a0X6D-LQW5KkAi#8(8l^(v+4dPD&m&I^@Nhl>-JQk9CnTL`k3 zU`eHgMamGae|lKFtYvL_=)jB~&Xk7-1fh&E=HiFa{?)+y>T-)Xu+#zFRv{z{gCHimS5`Gtb!MdEJXn*$5yK6O8*k z%=ggM?3k?t1Ai~u8);m*zWQp}s(N)t2%YjVzYAC|Am4vfX|F}K;?L2gG0>M#(qUEj zEQrAz=UY)(T@My%YEN3J%ey8=X6lOpytT0*F+7AEJI9w=8~4>1YW(%PHC{0te;?;U z!x-!0U|khB&%6N^?LUVTc(Dfxd5`)lz7fgavV$Z=su)V8Vdxl08%%V(mD*u1+O zxGBo*U>O0o>IZJ+;ScgcMQedI7`Z@3kXFkPr+${rgbHuBKs}e?auQ+)elk7)0bMc4 zON~uI=(HjIp|_``v&LaG;lSTLz&-ZrS+i1M61s-mGd$QqC%W$Wyb11OdW!8}sKhcs zzTy!{DWciFR?bzZVYmTpa9B%=ZPAOGdhl+VV=)p+xdgmrhEo3eQbF=g8OE@nn@9(k zv#ypkiS&5Za!4F_f`4lHHQ3kz5C%5ls(xuP6cx~!TYB+0yj~QyaXZ(QyQRLG@27aSJ4*Zq&LR=dIM3R20c>rhgP&R} zrd@J4Xuo^*_Mo_^@a6n|m(i`d@4vczJQUQAaoZU_+ePDP_lq{#ZBH}@$6#&_w9tU- z;7(}o{4QOo2sa64rWH5>*j0z?=qCQXP=k=XoIbz!rRv)~_UWS)T$eq*ZtWDGKOUUd zMAF!>2({9Rugr4pCtbL4XvK#D+A~h*)QBxvv8mF7LtSEzTH{A+09?@vU~9kgJ*Q8H zP7eM@E4xvE0{%M|Ark!J1aw7JARX}Va>_HL#jhz zGQe!$S5HnjBLl!iix3A7f@rXQDO=DBDnr6%AB!$Xp(^I}xK~8?dCpvk;|Al#;CcS> z^k&ITlY?LUkP5a0+=+p#O6f_SdrnB5t7Dxa=efhjI0pW)R>h`S!X!}hCNpwlrmr!3;(5{@L zYcP6ZuO(Fa{E9?y%B%5-0%^6C+TKsd#T97GpOzJ&@6e{t&#x+7bll460lrvt4#cG@ zp%wo=c0Uf^><2Sf8(c1$$$(99M``-1!i@GxgwLX1y@t00nS2cv`s-2Ro{}B=A$~vH zxOtj*AoJiCplK)HiDrn%hD329FMw+ZD*8wG{GgWz z@xO9b-C?zHFgaQt6*ttk^=gs=S8O}S^7^F@3N<2;ej7`}f6^z>P=vFWoiW0UN2*(c+0AU2|b_>CC4;GxDXReK^Nx?C_LQ#4Q?=um&0V=9UwC%r>~kT zO~q%>|2i7mLnQaGKLgyvMCA$)3$lw|k2ZKSUdN2};acD@-bVDnS3(r+mx&gAHlCOn zP03}z{tJOffcGfz0c&kyfWdRR;Y?!0^Lpa{Xqb>Tm@usJM&mCH_T^V-&AK>d}U^v}KzEOr@% zWpPsL4-p8LVG%##<3)RaznGfuczF1YLRc&6(pZWyYz2mx-0 zEAFiU2qU!3mp-Ko{BS8=og>pwjKF^o6?pP%j;q|iyw(mF{i6L=nw%MU0WFi43jXT= zx2BHAk_@9Lb+&p~G#s_+=5}ut8idy}fg(y3^p!_V&EHgmFGYLGWLqk11yt63;*7bz zUR;Gcoa%gxnxhr*D zAPgt5&RiZ(z1Rnl8uwe2$?F!Ku$;J7PxQWXuq?I7^CpWmv+KOETMr+UZCA(q#0;_ku&W2Ak0tYq?Y=95;2Sych3qht$$4p&=c6 zc=pXqeInbiD=PWIF{h<9Sxl&07G=|aOT8g4yRNnD%;Pm>)LY@vbF);k-` z#(MQzqkHWL$&<>&OG5*uv574Tcl6xXli z_PR3YW++5&!o}B#=a|g7*E`m7IXBm5w^wSwYb*pV)z=C6gt=nKP>+>WC^e)?r;^kf z2d*viEc5=|);O)$wv7ULlktNkj%G_GvT5wVJ}JB(ylq!6Qs^u^1~POWsi9^g1|6yc zI5gI|tr~xj7L&E7%E=`l)+77ZGaq*&;Ye1D(A7_{BM?i+Ei4-t6!c3ZsXBYk-_Ap~ zw}@E9314~QA%y+>URlL8^oU_j#2C3HyF41A{dg3d4nK_Rd{)1VNy zJ$BHnAn2?Lu>iSPrKY@%to_WH7jS7l<(4=D?O}e(LG|kfe(iDqCTR`bJT3lHjz_Tu z(C-}63W3OqeLTU+40&vB{DUk!;da<_xkdjtBphJ0t4qbgKr%>uP)x%>jwpobF$mpA z1l&f2FG$dZR5wj7?bhuHG-z(Jba!EuW)>X2+_#%!PB=qkxxf>CEurtm@DtdN6~x@D zY{WeUU~A-w`^RPgIZ2pV#0E(v1~jgR$9v2i40j2-9y#B#-_pC4^!(SzsWESfSjo5Um?dj6qvM9V`V*Xg-&k__69);mqcV||{ zqD+}gZ*87H$G6l+C*zb_r2$g`HJ@8bcc7O7uS%9CZVf3wv>$)iX&8J90y4#W)@!3o z2g)+@axF!^nhf0Imi8OzI@un$Rp}bpSCS-Q=9e4vE`Obmom)CafA@TvXb}n2V!?Vvcy65rt-bAf`7GC8j+nj z1506)f0f0@us(g@XAoHVo$DzT@m3F`gyRUXo?Wh5dxE>VxZmg9mZVci1TA@6J5n)t z85fYK77rX*U)`s`cG1D}6S4qGRY&NNS>G1`X}0y{Ly=xo9=$E)?(J#7S9=CMD>|Q! zZ_Mgq5aV6~bp5XWf-l!#x z75Mi|)gV%FW5vOeplkQPXg+YKhKiUcIj&yg}8zbo%>WdK8CY^L%G^qft}a?U&_if&KtHA|LB5?Bwl` z>w=T+U)2*;m~oZDy2rb&3SFIkf42Fd<$ZE>u|GDXUQ^>!F69RRO+!(m?E|p06b6|J zw5WeA6=wOh;)aUsKNISFCJliQimH@iL+9a1y5@!neeALgfMJ9W4(3cEkaAkr#uP~Z zksZ%IRK}r3rjm@lKv*4vQ4zvIkSDO>OnTl!614zxj84^qW9vvIq6XY07s5o+5D#!D z{s_<9wstvESx-k{SV`G=x8z<00n4PzNM;+6`x{Q&^F8cep_#hXwY?xC> zkugdzQxr~7Y($SV2yB9UhDCxjW~O2HQ^sms)*vZYdb(2pSTehn1EnLlZ2wtYOd9)3 zoK>b*6=C0+8dy!e9sTr|+fh9P8vE`q43ZULQ1({l`1AMr7{1w2SbSf;-n6Jp?%>>K zWQ-muFSgg+3q3QvAG&s!d7S)*G66(nT66+GRB8-Iv~jU89H$V*albJ5sLUm3b7fj{ z+AQz~j4>Yo?5-*)-(OX%m0C68H&U-rE}9frpEp0P9~>?$8nx*=4WVD6dnqB9Sw$c? z@)TsCdNf8^v{p6K0AXskbLXZRRFq4=naKI1(Lc7A0lv8fJl1J&&bu6c@a*{$gG=z^g4~F}u1b;!)=~-sP1+M~3Zi(+TJ}LXaz5MN|ILvZpMA zM;Aio4JRK|RCM&0Rf~sqGynIW?Tw?Gaek#H)SMXh8|@+pk(dX{xLvONuZ{f2Cj*Gt z@Ps%3Il_BcQk``{XM4FW&(VDfn($qM#V`nPbu&(t7zW`wN1aFvp|zH-b&^boC_G(! zLudG$qkZI!$1>xYB=vYSW;hIMN|J(itLk&18CW*HBc6XvGL4CHsd^?N37Z6A0rV?; zWEMtx!4SrNiX4AU+rN>mo?^ntd13IU+4Ks)+Gr5a;ekb+=a?CxFr)P(Pu~dc8M>7w zun_UnGhs0yynd+N9B=_u5D}~VrR;+51P^kgbNQ8wEuF+lllm)VU}gX$8bmGmqDLB$ zzh|aDzC1U~Y*?x}{F2Jd9lRGUMt>M%m)2MSTSEYfC1|yjvPW(B#07K`3clCD`HL zgw2nGGJ!dYotfFAR6{{HozcVdv4R zq(5-oLKKzfI`8h2-Jh8>Ho{%Y8p}u6@h;Bc^1r{0OOX43ro_hB+=r*X)KK%8z`1ajsvjv1!Ri3mS|W_m|5 z5Ui`4l}^j3aMwiD>@kw+^1)u?(mKFPq<1@rNgWe5>GQ$qfFH$1PMYU7Vk0M&^Nmp^ z3MkO=|IOGR?J&O8{Qbuaaps_#-4}AHOl*_Fw6U^@YH)KK4YzILK{gKn5$_kZ;gQ5_ zLM=cMzTfSD1#{^au1}@OFs}oZ1$`e07kPWnwDk2Tdo60G@b#~VbUv4ReJV1E_`Tbm z+2JiMQ}I@j_|o`*XjLwC(pkxfYK*_~KS^r3TP; zcpWr)0ADY2yl$fUP8Xa#t?gz8l3zP~P=R(cm1 zOP4~X@_yA$d5YNto+9c*4&^dms)Q?|Gw99v#4$0~!m~2!MOe4OwVwm-;^8PMIV`s@ zg#d~u-p}b?MG9^;@hJN6tSV)X4bMiRn@;rVWU+N`0NRD*&^b>9wac+-uv2bKH_O&_ zB_zGj!$E*(Vae1;7Lrhyec*|)%f1%-CdX0B7u$MW&d_{1{c7FsbHCo9jYx$)?1;mB z$$!LSdj0)6jeTwpW1bHHZ)(rUhBC{ccpAWsa(6}Q=ZCK^TV?5Lzom!H6rS4TU!_g> zO>Jm)zy+347XsFZT%_syOJx2gS3)xCWr2S;N|#1F!VC@y)(Dt<<31><;^CS@sI&Ka z)6GCIVde|%b5e}HLtdBDd-Iv^uCoFi{<-JLHu}p!M~9Z~G~2XKvNeZ71+T^H6gAC4 z@LFc|QT9<581{nBwz3^KQ)J&Q@6E1DSDl~>fKX;k+6dukxsV{WB-2QNG6DH-FiO{H z@zR5eoJU1!u&9mG?Tcw+M>QII<&sxp29KKEMn%!{YKU*BbGJ=n_gO11dgGqmv7R(N zd)ee`am0>Qt~rJ$Mn1G~olCvQ^x*I10I`;2KZs3L1j~<=^qnSOMua0eszs>4x4v3- zK(J_4jc<=&@gr&Imz^eXp()~j0-L^(4$rd?b!UPXoRvgh-ma}+`K zdF9bXE8Im!)#u-aWfyjjER5flmOFd|FP_B@GM-~)I==*9>)(T|%WFCXIWh5Jl=Rl* z8R9%Q*jD?QJ7C55IFB;_4tbYCT!2T}0CJP-tPQSXZ~`MIN4jp5Fn7IQMzvoGomcJc z_~`T$bC2u^ntwL}2uBKK$zcpqY1AtF^%T`C-lD6<^5+(4ZEHV1Sr+i-0=sfjs-{xc zD+7j1j@!kxiRa`m7!M&AKXP{3*WLqu$p z$Y{R3wYRt2vzoMdWYp?iKB@)2;9c4EyftHTXs?%VZMu^5mztMU4?Q#4UGHPW$|}3( z5Gc_%;d)q6#uk0*>Zk&&=-RX=0Y?$1McU+S(D@UYn=&&fe_GEn7#85ZL4mgqEIg`K zb0VCx1jFdK51Ikx0D>L4r$`ltMpDXu$Bv!@$5O zU`)(^IDWPyG=P0pq^@_h8;a$M-#}4jWsc%yOo@{w)X{eJPBY>en2T%wh)0d4yXDw` z!U@0?8-}N@R1Xc6jFE-ey1w3kKb(DTH?@kX2~(CeRhRLpsj0_t_a)Vq%g;71l0EAy zz8yG{)27khU%m}|Uc6tw-j=VoJXbx?Qx4^CUyiPmod9g_YX6*CGvq0Xv0DL7lljVS zWZ{_hQNI(0W+$|g;fSZ;<54_H;sx8CYgct8dA!2-meL>oe6_D%Er&j`ZgN}ie}BF` zT^(7G#o+;DiL1^w(}q3-0cfGlv=1~q+rHBAjjzUq`_ke07WQP}^H$!f*H11IJp9Ds zi)ZMQ0|2ME);}Q@$EN`j4eAZ(TeNgUpl&}4{h!|$qg^Bs@Nm$qxhl}MdR8;c${1!x z-6X$(1)XnK*UP@(4o`+2X`!f&iAjaQ(1@jn*Xq3FKsW$F2-^obS*OBCxY z>1If@89~09){?Q&fb|mNJ(w6S3Z_cU#ZaPuS#5d6fQ~bo#DiWZ7 zXOzm)yoyv3I;eo*84o^YM?EA0IRfS^0?Vk1W}F4?bWYQITG*pqD545e{PJ(N(RUzW zg#pWD=CB%~O@ce70cs=iKv8Kt;}Q*lzrfJ$r*LSCrF6L@Xp#Akjiefi; zK%KAS7hS8wsVlz0ovS)Zk|;S6pIdTMEs3yzrNbfy_26Mj2>`afK0kD=PlL1>H39hT zZO@tcyVD?i?dVgMnWVGqD?2a1lRjcqfC+#!y3DKIlU`srun-rwh>0M25~1NhP>`qIteeWP-)8}$fOoo}34x-k~*xC`=_hi6umnngKT{{bR0#I=LY zA(v-rXspy-ETykNZPj1oB@s1lMUJzCJ8C&ANA zOOHA=to&T6uq_+&<|?_cOpoz62_QV{S6?XZh%PDwN_j8JiCHq%eUp@^ARmx2Z6`=Q zR-)nn3r|9>*KzoF`42rU;Zk$5#SzjzGG0a|XnwH-wol(x1>ZG-lm$TZOxn#KBSL&a zrruqcbl}RyU;?y$di2FTGnoi5VO~C-)GnM&d=B*#s4DkcE8G44^zD2VPNr%!zTGl# z(;L=evG}R=Hgf{2(b;ZqFZX^30lIXOmCv_qERI(QFifX=WDnlAyO*xXA(ecJcF7|? ztwXnS%}ol{D%Z=|tN_OB9|8?&yG||bl&u>F#Hm@k_3l8V&Qv2)J^p^Za{NSdy9$b1 z{ov-N?R!g?&cN0miCM8zg_#uFj1vjAz$}ucwWW7=t9qUREx<-K!s+IrPg}Dnd`941 z?kBz-n2)BH#342dl?q0i{UVZr7ckZ*}yZ*kf$DKS++^3=4a zehaC)MFp8zO0MB0qut9zZ+Jl<=iwV`VgmYZ`aO z$*){;Ro!v{#$8J!_ksxcLLCKBVu-xuE-FX+Fn`7N2Ov+-4L>`}PRe@WZJ8{}WnnYa z8R*F!fAF))t~;^^BC{x9@^X`+Lx)&P&f7x^v7_o_7(QK$?GWj*?a{O?=ta==N%o$l zaIXN@Q}+isfpRJNs8i~L{J?f9^f@cqRCKa`%7A#yJF}XcX?;S+q^z)DwV(j(O?T^h z|12G107T?vsA1IN5rf^YHJmRD#p~3kXGsi3AkOoYfeLedt_BE;2Zz#H zl!XilamI&a_klcH1i_0Fdm;8XgSo%3JX&NnKj$2w$^%0)aG~%s6OH?53 zEC}51lTM+-|7zHbVF!65r{K}G#+W!Te-C>t%Dwa12c0ody4vgg1+)aKX_=V|GZ!to z3n&rWE*JbdSR2TiYmapCYR`>)R)5ZI0aND`SZ?l4z|{K@1&RwOQ5O1oqiMt=A5I=0 z%!#~ajk%@#kX80dfRzt>aT^zRa zU2S4kRCS(^BRfoAsnpH_USQ7_?!ao3VD#(V=O2y6Ke49ymO-?W(Qpwb_L!KM zjM4VjgUR<3lsMU`+BATIzY2o__W8nxWTp%stzFVGdigoPY)j@l1@&U$VSKa9x21%y ziQNh};}4#hBY@Tq!9M9qd~V8;N0OKRCi{e|*Y@2MHX zd|G5;H?1^nbRN^iA37uYOcqAHv>XJq40i~dns(<*?~c9irVBQBwW*#&;~&Q$ig|p! zJ(xMT*v_Eo`Ka=3!p=&WIu(?e$4jgpE$)^NHeJDM&}pZrXb>D*r}BjUjF|rol3$fi zpA1JW3sc3`*Ru!AWi8un<4I#MIiw#RxIE0!3}oD+xrj@k>dYfxN`o^1dyr7Rt&JT~ z9?_OYFG{d-?*L`|+BL!5i}P2HYpA!9pK@H_|5Y-0KshbEhVtd1RlP}@L{~EyyOtx3 z#*E_1U&fgf!(obU?Xm}yydy#!W2zeKWMEm>!{3EtnIHlv{v|-L@AwV{nep~_d>K9P z)Pza~nZ{;0kU$wUqw`TecT)vHmi(hJA4gNq&;_&%Hx=pNDMrn$yNbqaB5c$eGwJkUO`ihup2@P7St;H8iMHd@tPXQ!H zlp&MA=lB%ELTVRnC|sk~PPBn&`TZ@>g0&e2hjE`l0!E?GL|zVF6eq&lZHBmR(qI^T zJBx-8PbZr;v8Jnd;lE71m(${$ z6vPkpM{qz_o4#tX5e_M6n8LU^$I!+F`43M|DG=h)7ux=R%$2q&UeN}dIijrL z>DwVw1y{%v<;-~YG3=V1S;=6_i8-+uo_g&7h4rrPHP|ocF=e$FN;YL>C3U)6zU_KH zvEqQ`X9EPiWptzR!Tz(ox;6$adYV9H0kDpKc~)>-4~8`FZp=xs5 z>~w6~w$rg~?_O)|v+q5ae_>YLRacGikbZ)A@-cdtWS}mwhj^p@SQxcS;0)NG9aSUz zPLvWYXy3L2S|{tMDVoM=OWKV0sa^KZZ@~o>O4;&w2=)kV@LVOBz&FKiEqm<7P%{g% zW@+E;XqqQi&xeh9(4{dQe@EUg2J){B=iV%+n|y(ZTuz{Zy>`82lN@MpeMTTPrE3yQ z2(#!L*tLI8cU&w7g*L7V)FXl_8YQMaSxExH+0p8^4-~*8!Y*!Ye|G#?za<3BfY1If zyU5OwJ`M~V0&1H|k*){=%mgUiyMZ=`8l+hJ`pnfW5)*-15K(HBC`g$UBbjK{75}2i z4<#Kb(Pt-(B@$O-F-3tUw(&u_L+hRpG?L>KS5L|Izr@R#q7o05dRaV%@O2Fde1FCS zarjNJSd*BxV4yf~ar7vmJprpi9;NXkY_rY*%9Ge4E2!AAC;DTsb_s+}+==YF<4^>;>-r0M`+R zOpydq@s7T^MiC#;MhkeXA!14v%Oi~l*TdkgKo)DfL~Mi;NQY~Y%=k$7;dMhznhG|b zzkr&XV{6Ie>2YgYkI6W9!>CspF^E3nP^ijWKsqG0*PwQ*9Tl4jyKx z4K(Vb2TXNoqtIZK!6ky1WVx@4ey|Wp*V)KPD+&b(FZ5Nj0Mfref!RPR`qEj!fYkty zF>ENe>|FNeU++~|l|aHgnbNmF){tsY8j|H8IVO>%0ecogh~+_iTzaPwl^V0AK>N~P zgLA*}ioWV&sWki{(I+;5?dqoa;<2)zNr7Ytr>NPdDvWRD{-VWCY$y{(cf@``$rA?x zEnC7sU#b7RnAfv7h)j%^vZXPq#?%0AUuwgC_8l|{T1MJb_}XW;v$E|Ddi2`378u?0 zN|ClfYy#HPa07pl=(aPiYBx?c)Db}v>Rbl>^AoGm_-!4xGJHPd{m$ zyxAP%Oyp38x}XUE{y?&0!?~MBP4$+!c7b0(`$X zI(}I~I$>UK+^&|@UcW6K-$p2cczie9Dz?-%P!E46oHz*wQ%+WH2=w-Z*V4Fk3g-Jc zx5Z-v4T2%BoQ^_HIbC+(C@-ICLzwB|>in?gxiH@q8~j*sU@{%Zs z&{*-A?7y@8;>jq?O!_7CMaytN5vhuo@iZ2dMauwE>q9I|j|`|;(^#PJ zW&GtgDkq5q30rey4f%mYxLZQueRp(3bAz+>R+R^NM3r$4ptaEnRi(AvZBL*vpVoX*%fmCfuo*r{0smD~qmk`gN|Q zA9y$9vq1&FsBi$fx=4{h+p!Z0I>RAmn&*q-Mxv{HJ*mYt`?`!Dd>J_<(!L~#68lm^ zs@Tcl`|AFQlo73Zpk%RkLs6^rqClP;}P9NMXQsyjdamA6B@KDELAb0VDB|&k93p+3cb(wgy8ru=}s=_1eF=puY z%O)$ygVUx2X5_J!!c%n~9i4_-G_sWpgd^HQdiV9K742r)9=?m?Z*C~A-S+A%=iBz7 zP%p53)EhuERMt!zhkbjtOE>+Ca;gG2$!q!Y!o#abnSa8rr%^|o>P}bO&Sa3QlIGD_ zMshNTwJ+;mq);0(z3WQzc4|*sSoJzr2qM%AsX*Y?xgU@6fOQUfyCZ^USE>Gz{&@FT zS&{JW_?PqS3+bsxQz^`9mJ;4-dzu3q+eAKRT@I*d+mM|wk({EZJlN0^=MaQe zaz>DWf<>znwr3}zOR86A1FdH6lb#u*n=&v(zhe*88ib3E2z?3M!BEuDJ1&+s+C8FU zV7>G&XkY~WgcgnMr^Q62rVRhg-Dn!=#`9qzp9(H~d%qV}7#u`35X$7-Guk2H86c*C?}*y#~B{XLk8n*U-!7%yCjV zId(23%P_F>8JrsE$fJ5(kGsq2jl{4^@fFSHA>}5_w)Wv^0v`5n!Bs`(oIHbY^~OlM3~TE&biM4$6TXSfRv;kE_+&M+^^i zJxAMfrPiUZ4M0Q|7ebKmiB9HMp4EWKIcxP4|6-4arZM*KlW8{LuP#H2-!<)d2#1Fis`~@#f2ClXn{sX+DzHxv_Z^iMf+iA=LI-bE;V zROq5&o`;fMiDyTsQJ2giOh-4?@Z<0E%s)s+Csn>5Bg8cI_;69{`|RWK^gyU19!f+C z7H83#RG2h?W6V&vPV&y?-J0dX_r^o+m5j0JhNBupNw}b&H07z?lNFHJ9YHS!fW;gB zcY|Zr4H*2RkA@sxeuyj@eR>`yxjs5%4{v%j<8_{6hqD9JRi(C2)Vyn9Z* zqz&RI{L5$Dy;F>oR2Zv@kwr$FS$Lw(W|@Z!RZ?PbX1mgncv_fb2k$xM>ROanIo^s= zr3edGBNhUEP%7B@oSaa{_&u7LPRL;-*Iw@*a{pO*i+*E!m7vtIYSsHUW{h3x<@s!* z`qKu(7I{^tTXWU7Hsde#uQ&OI=@#N~>Y%m0L%O`S0o}aB9>pCsNx@~3ElO3L@S2R` zelAitQjng=Ofj)>%W#~KZYeXQlg~t(X3dNWjOmF(AV-n?VHjntE3pFC@IX)GFu~>l zTtMgP`OEt9@dOT9Qw){{6fCkdu=bEO8=&p|5nK}Xb%9_oHU9TR2_iD{m=SL6_=RLwYKV(12?kS`MmecxXBP zoh9>;N%Bc6)uA6nqEY4D*sd!4t6IB%HlXw;d`q5;wj>rBl5jbKjF_9` z1D;Y1i*Jim>(c_lyPkWSn9Rc<3^UZYQ9zwiKpReC zNC6uZig)4J4^8GygJM66t#_S#od!x7A{cCdsuahWmBS1Xdqrp=6s~I8cq?ep|1UIo zn3o54%`Gz#1wTz`Xo{*ob6%3imY$p(;jkfbJ^LrAy@~GpIh>3}3=jB%_kEju2u5>Y z{-~D8zhe?#If5rGD0}{WbI__d06_1|932iBDTIn!vy3(R(fS%b)W&f+vf2mp*t>AK zO*mISNxmY(Z#vmLxKfp?44380;P3;lvY zgjuP(yBVMSG0_{j34)B8VuW?nU9?Ndj#u|lIK3Dz0N z%4rHlvIkKOi4q5T-T3Cuy629ilePN-i}L?n}(2tev&piQ85U`B6-rSD4pl(T?+66^m_zZXZtL z(Z>?^Piu8gyh~0#TLlEfwR=8uHT3H|Y#DU4RhyaICpkWsK@)OKK9h#iG+J)OyJZ7@ z$)6^cFn5ZvV*mCs6+pD({3{`7y^3y>O46d%X->eqZXuo6s(HQUW|8ENz?7qmPU=yD zuB^l5>TaW3`dr? ziygE#0(s-5NVRx01>B@F>8Mg4mosJ11<7`)_^_@&oz~GXC?GgDuwmxkYT9~Ra9yy@ z`>7{~UGvT#cS)1g=nuVU2YiuEtu|J$c8twP_D`pmfxO>2dM|=-GY?mxo6P`HUvPSb zOAY_IhEb=(i9;vf7zdU~kStv5kjkPBvm#Ewwj1)BDerJOHK4=08i?4*Kn^WbiW}-o znT9iXdx9qM8ZhN|9lP>gpvb)i`o)o0mo#NMw4DA(DuyUARxad=xgh~knmi-HZ0vJS zU5+Q&lwBbZHeWVs*3*=~pp8lrt~8pJ$6RWB#g$@hX^h%SiZxjgmtc8yB7w#!$-f$n z*VNp+=&br+T1p{W`b>c;$0OWX6PV@%llFkKzBfJ$4saQ=JTVAz>4MnUbNVI!gnfP6 z5NX0sV51oC=f`xPN8?GA@2u6n%1hY?i~CzMj%AygwCcNU0v;R(Dr{%%eDq22FOP!g z3fp&OguP-K3dL7UOZ{*f(EQ2NqVvtS91WllqfNq7Uh)rgz)oR z9JO_DvJobCK~|a~)_5t)GfXV!V4!CrZ=}=%2sqE9P&mBXzbQ8l+URk|X(75!0$I~1 zQvo}}!K55naZ?22uZ2qf$P#gF;ysw&yqG z&twtl|LQ&0|EnlrW#Q)jPrXk!!vIDCktR>~zyRh1EZGQindmJHPj8FNhyAB`V{mQ` zGs=3AYuE-e8+^~P(H5q=H|nScrMHT|sGy~G8t-=Q-pFp2uDO}(H&^XCd_#anf)i7r z4j=?#+R7vmEj<0yrjAUSG+U%F7;1nqUYu;NY+q08PJ@0V({*m}Z)5bD$kF~tUlXR; zr)vUme{Snfvs?}!W>1}*qR~J`_((bI zl88mDtb`YniE^H@{I_SdisXVj^25Zm>caqcYvtKmw0P!ObSobkR?Qe{484DHIG0;# zNzxoA-`CGUbxfROa_Lw8-sbq8Z1F9XIMW=gn=9eP4^3l5q6(m*j*wx^2N>)Yn{lMi zg*i4hH@iIS#0f{8Y%+;&kE~xhUtY$4ZOk8xw@Tp0i73$Zm41jF+>1KAGYjY zw-h5J)?5ptPy6{42NMZF{r0?K-DyW5GU=awl|(FG5Iiq5crA}U=1WU_l3TiPa8qyfuvPdW}bK4mhmYfgjL;Igdr8?mo|Y<-Gq z;u((DI)gpE?mA8s@yfSVDEGOyaqmzd$+Qdf=(nR@Q>W06>7;f~;YGamyjgeAawA z;Cy3wW-uk@yqUQlY#!5w%)sVZa5CFFX}fTtzlDE?@j8a?v3}#%V3|14@Pox4cn7`% z_X6|HvM>*O_vsMTK?seREVxSTt;J%C0@NC$;ck37`a%)CP=dk~;8hDH>*>uY2p_=H zf$H+N!CC<3bHU@%V%ski|IABrsBC|U>;8t5_Z}e63n~1Lj6`v+Wb zYacb2_*R6 z7DA?G6~swH5r_od--v%-E;FXz7Jric`{=M=(t2wNAttC(z#?Z20m;CU1s{GPZL*cC!%U zOZ6SF+g6jrOjg1HjzEO5gY;7l`sb0C2>q7Wh>R*vIp|sZG3|wE zHt>MHtPY;qpay??jUV#gPZ(7KK<(KnWY`VRA#{0|PfUA6^VDt0?l_+~Z@=6Dq>e<^ zc`<$U?sXJRgcWtXP9`sbMoTAg)3GSk8{+fXS!Y$dim$y#D5Jw&=jDBB zHh)ZSD|U{1hS77H|DJZ?p0?QQwruoB>9auWyk4(HWi}V7L^eL;?N>6R|}!MiExdF@^&a6_^N594h@vt((9K1*+U1QWsE zWnc%F&r$?@7G+$EoxP)+p^TO}fk{~_Y&dc6@W+BPM{-FZ7hPfAQpQza`fR3k&-VNd$@~6RJgmLmx``n? zs~_7uq42gEkuF+xa%$T|MBw~F=5}>m4TPg8wCSE&JO3T zInMk=8KUuH@JHU7eZB~Oo$lOae{3^9Ra!LjaU+!b3C7zd+P>Y{G92OYz_SVv>muT- z)Y~a_Q>H|cbjd27nE@gi+mN6%;a&@Z?~yWrO0$WD5nx|*$4&CliI^=rqv37K1B+F;E7l(J2pPVki_ z!H!As^Fkhnjy4R6`PW>ZY?lb&S=T#g?eGR9rRqkacEV;6EkS%a%zvg%Z<9I1CR)Gu zEt62YB2duFqM&oE0oI}RMn8NQ7HkCjl6Q9QFrtY7Jn>!*UOFP|r9xW7Gg;xf)~5K` zvNX3eQ=0c>wzJEblVSDh{2xI6XJZ|zhECPOkKM`bKazqgf5(;|#n@H%<6Xrx| zP#<8Ku6YnV76rc@V#Pw$9U5s`zU``;B|3YIfdY%Ib}vB*9ayz)Rhl@EewZ|Gaq<*l z3eG?*AldKIWL_y(u>d!XJg#7Wack!?%VvsX!y+YMS6!Wcc%}#tdJ@ay1M>0?;2MBK zv*0$OX^fugw;_Fy{8yPs2=M5?ej(Kc2zaSQrlMpNuS6_qCm`-rHda2KP&vsBLhqAg z?=|!-vUZA5Xtha%F0M1k!uMsKHS8x-e0!x&_hd{OI7lAw@Y@!#Fb*<~HwHoZnTE1U zSjH!z`N!;{j1B_yF$YtPxd1*FVnSf011ne!PN8SD;-<-n&?QD=3etb+nF>uJ(SMm@ zfo)dhwg(XZa2|EKonbIDR~ZTHkg#E>>@gx(3yuSoWac_xcpxX}1paR7Glv73U;r)b&@{#jV9dw<)`{i=Ruq<0-%ODvJd0NOV8L*Lg7u zxw7n>a;6~Nxh*B?e_3E)!#73es8u=$jF)^ zcXyxb+T%*RyRG+i#z~GTMj``dxV1%3rY}{EbZp|>sO@KfQZMrlKji(8Sc^h)d+)D* z@=-e0a(!2;t%|<8@!Zp<3sA17Yt3Fx&qicw(?SYvLRx~0$Ap$oDs(s%)PP}}&ex$Q zY^2#dk~cvAd3L-v!tvV5v|jPH|NY2PwY^#W)lt6XrxW&|yp|yYJ!MPzNr|!MS!B!^ zuTr|Hu-XF&?JG6JFOIBdzRkpI@Vz6Mu0H&>^>ZV{Qo;NE+J5nmm9nZ$XWTBTY;@$3uXbC%CoijiS#n zAuA^^oe6g#4b^Y@@Suk|V{67pBB9Gny)nwTYhmxpdF<}G%z~_)QT!-$?$8y)@g`xN zATg#9M={F)>gZ~22h1$pkM7rRS)}@IfTw9JT?@wcdj9zsz}i_wN1l9MzIx%Z!?C9S zOsUAe_gCA;qF#~S!^BESK02X_t^*XzRQB5W`kjR?A50;U0uc+VJZNw`sac>b5zO82 z8jOmW5DB3`s7W=A;mF%K#|Ymq3(u7x*#t#MJyP_thy zxcm0Xkj2mf0ovBkm^siKPHctc8z(p?IyF-BDmQvT;UPRKv;7tY65uRA1va!HKF72A zWVUW13ca1^O$pvQ?sk0}sjEP?c>FZqgW^>9mviBfqor=p*bqER`wz+MaY=){q5B+6 zO+eyQ&q+oF$qCHeP&?pe=uiMLey-jIx=0i+cNQXJ`|{-YP=9HHKOHR5u&<0#6Li-gk21*JS~oQUfc_Zx&sXChWXMp2mJ5fZEJ@ZakGr$;8_gRz zKkgJ_WsnRdhESxyEQHVuq>1zhDmpag3VS&r9YeH64l5UC$sC+hn{cj92SK8lc7_|R z9Ivr+0QFEk4mG)<)Q#^zK#PeIG_B$p-f%vWtCP13bGaH){A~vdrwkIpzYvb1!KK^zxbMb(A=($gGd#+ogCyk)nun+j$=S78U#4;y@h43``P1 zm^9h=C)a_>J(f5a9ry!kqPfy-%^4Slz(M^g0CD|WATJss8b*L=h7act4A?Q>%6-r} zWpt2>0%F1pdlo6&TWa7E1Nmq1B4k31f(q|Yzev9p zmYMup#zwEWGO!j`?#aWVWjzN0Nm(RFu{17Iub$o;gMeXQ^R7u(sr@=|9!6V*fzuZV z;J03;N^%aVJu!0XXG)u}ZW0(?!m$5Laqq~=EYuMA`_Ixi4y0$P5#=@V%`Uwln8xBv4P49XZvg(Pz9}<6s%{LDtZf4Z%|k}U8s5@XnKt}@br7V zs3DNu82hvc+*r59et9!6Wm){H?2epRlXb~RGX!LZ*fAy)ch{#<(z?f(_Nw86vwj6^Y$ThO9!6r?8g?{8P66 zh31XAclW4NV}ts5)V*a>Ar}H}Hcro5Z?Zp#>Rw7-FYz zA{QXyOfi&{M$T2L*#Qin0ZPWerpDS}jj zouP@-7;Ytw+v#2$p*)QgKw#NwwJ0&8Y$$vn#8eBAW9fccL*rSr04XQAbl*c8l~T03 zy4f&uLBWOdxEp6nB@wmd%w*MqA|O4`l#3fmk?p!}ZZ!BqT91Z8tU z{3O!YE{u^DvF9~Dls`dh7$bPY0pN$x|U_|m^*p+3>RFyfx@ON*& zvcX7Q`%mgS)wnL0vT=$V3DLiZJ%ANqWjkhXB~PipTClzZ0PS@Wjo3xc?UXGcl=oM| z&~tw_J-ck{>r#)x@EhA-=z2P?y)@B(cfbs9^F11A^4bH)_2AxI{h1R=%a@S`#}}#? z3xEAs85&}1l?geRx*WIDz;d%XdbeiKxx@r6JeG2r~#NNG|s^Sd%Jstwdagi-#NH7LO;GLW9=3AIoCWB#=*hc8NY z5Azm^5e3k!n1ruXDA5u{Jh7UaZAQ;e0(&(izhUl)0o$kZnXc6+qH~xvqEO>v6*-7R ze(v8y2Oh_vZfR4W>ark}!y_QCy4FVg)hfo~vc(51t`{w94n;K+Tww^i{@C4B5USSO znUzK2)!j8nuJIi;E~>KiVFs_{Mr>Y{tB7wk`Vl`E5bJIKa3?0O6<_56ptCZJjt}m~ z$L9{e0U|HI2{6N-lTchTPV};CRnvv-`VHbnsOL-hCM z-KQNmbc6Tm-F*+*md|+V$+aH1$3gdNYag!``-o9=O({;qaGNM>P!ZS zYe~)Li=;3|W5}q zNM@l<2Y>CvsZqqYluEH4OZ(V=(Ro-d8HhwI#^FCwb_Y5y5B$~!%BMxq4 zsWBWcVH?M0o2|Upef8ns-3#C_3cyYe-QL(sYDFY6B}fFXR1T0r;11&E7}#Tl)jS5( znn&L5^xoC=p2gb~TGrANjzpbl0C_V{9g__k_zG_&T$^{m;^auEG`c}aT(x_2H~gRJ zC*=Jx3xc021PbYmy^M;@f!VX)i@5n~Z<4T|xBy$s3y0@nYh78v)m8tVVk{t16J;`F zWEAN^A=!AUzb7fYE6F^`W>VQJV1}`MvU?0IC~yRkQf9>tWCBKVzYdI+0Mkvf)Qu)E zcpyaTk*v(RC4{;$wsWE`H=J;=SkTt`o8cws={aUWYWZCR>ffLkTqPKi$<0iM)XhK+ zmDa_W((;y1g#`vbUD2l&$A05S&Sq|ikiuO>xNE^F+jc2@>~YP}UC_gt8)@-t9zdj= zt5PZ0*sS!TQ%L_jH3Xx8Yf^#luw4{8`kd9oOkXSQ)Uff3XG+e(Y*o0`A{yCxXHwsl&-g3Lz{F7NiTo ziG)D=#Z~y1K$N_1o`{9L-gyFdDyAza!Dn#xYz_tMtmIV{ypoC?5&*@#^)}7*jT4M4 zcB^}93AO+)4cHTmJ^eBxoUKXnit~Rh_y6X5#7@ESBX|Yde?)s`!c<$m@S3$s;y{cs;cThik2f!&n?hmL5fj#+e#B_i8f#?TD zCMSOV^=yt(tAw`_p3k(VZm=Qq!ZCbU^$_re+RoD8UfvwlO;7PuF&K}jXdI%$OVlI1 z#lad(|3u?UnQ#>l0W;c z?fQ6aXTYo`a?6#-WVE)PoJ11Qe(!yFU?SPL0~#(n<706DNQ8P0?t zhX0tAd|dS}kl6-@`wlAZ8u5QTfI{D8%fY2p*lXRx8vg&4<O$%VG*ufwFLdwbRk8`Tlphf8ll#x}B~)p0ANc z)C<~+$H}F}^vp7pR2c5%u3uHmZDQsBY%Z?Zs|uE1rBfe+k)RZzD>cPYVzRAQgrhH7UvVE_`BFFq|j zLD*))kI>OtQo_%^p6t6f$z(Re%-VBL^wy)hvpY9}g+GCB$WEZ#>t ?f;U-l&Az z4lqV6;FSs8^a-rv?0V3^nH z+YSE`(Sr?tA(tj_h(G^MeGx*XJ|K-0*ts-&xkbYtiHtUE`S(pz99n9L7|-*}`0M!ZpsPCmT91+iq+r@_1Z<=$y*bCb?STu!lZl#i%YHQ zhH!y^)5cA$OTtfX>imlt>XMYw3llHg?~=W>>ITme{X{{?(QF`$sb+n(ntV61$&72A zY!cZB#+XG`K4%2(yHQ3lUA!ip!c*S-8hYS9JToh-!$o>Jos3HS)ItYC(R#MWZ3L+^ zCVIM08Tlf~qLF-H7VxFfC|%j9UR)l*uWf`VjnzJNUHN`qRe8OTJ^9aRprg(tk)zVp z)ddX>2}`+^Ak{Q?>3E@O^f_4VZPr zPuLyHppZdl>$=fL3KFMx){$i_fGVk>!mihV{5SI()+L=mOPbrq&{seq)$ZJ)eKkrcX>>g8l^kLMTRxF!zwV?D&$g6t8*$yog>u`vhRIn&&BA2TY zK_S*_L}pY=$&o6JUO+WHI{v>yBE!#2i(2RdD%Ft=d!ZD z#oM=p#`QVSS6@Z$QPku4;Y9_PHQ6@5OQ8?%P)1>Rq@UvqUp-K#Gy%IQLRb&oqwX- z$kzE7j=0mP#BYj#@RJ2cKNUL(dv~7V+(nSdoo*4n`2;#EFIv%t0VFxdabd0=y7A6@ zd*lz2Zh8v~9J*O{f|I4zx#E7OD+!E2Bm5Ke<)GuW)jYBE>F$XQ5S7W!x+g0!bQE5f zxt37KOTRSb*hiYL__cZ2+2pD}nRn#WR7KqE{j|oIdzF{km&~h1aZ7Tsx7%w+Lp$p6 zB;S^;(W0e5B*;Um7UR?*7~tome|>G#CepQMk<@YOn64(HT_~x3nf#y>WZ)Fii(uCfi{Sz- zU7E2HkE($Ux&t&(+)Qc}iv3ZNA<@9S1u3i!i%he6LxbZ%LvbWkKK6+l6aKP548iht zMhGnUv3*5*0>lN##Y_xE6^GfFf@Me_&LN|wM$rA3*p7ZgAKpYC&b>t+goNydOCyo* z28Spp)i`;3aU!RsE^TM)VVgAi_@tA5HZLF2Bi658)<0|b&Fni$I&J)e+T*ab7;n&c zPRxfvT62S~r12}mihLh;qO%Y+mTr<(?f0TgB$UKN0l%X2R^h-03}`n}B0n>|yGV2x zA#5)PUm`SPOw^jWO7RD(hKKe`7WccQe4D2r@oOjoef`QZ{hRwBGR zq;4BX=6~?lV|NLW{0UwE%m0zNuS#5Y8kV- z#anef0gbLTfg_n&YE_ptH9Z&a&*s{HG#ZEfnz>sCII91(5GK9P&e0B;VS_JP4gn#5 z#;G4$KEWh5ikpeVXJq?N4;$EreD28k_@<7hC07@{z1nO9+Whr`_p(^K)pIZS`qenC zY#z!j-ytXMKbzX=p_BC6r++iFt~<9+Uys>;0!rdV3^>}EPxG)iCCyru(Z%iP3`i+D z@0vE7Vp8#bSh)n-!g0CIKOgGfwnp8w_rOa6F}mlrrlY^YHzvKgz1$HDc;I%55HFAF z3i9(z`EruRFo%Ef!T&yl-5Zbekt6y$o*Q^Rm(qetuMIu6S%KyojB{IuKwFE9-W3%= z0#IJG&~@~GS~XDdVBf3kFlchMsnt(e zt85BoqHeiGxCrNcs~evKGx1nyD5}-j0SHgK;u6kNU!OTZfUr0^Q`)d~`(RkP^^Dz< z*|o)9xs)rROTU_m%KeV_v{=lY|IU31<0470&#o>t3$GDJ<8MBxa{s(sz84 zNS4pQIq3ojge1P0T+*8Q!+`U&F zTBHYW&!z>`30)`rdX3ef^-AKal%74{^WL!gju7@J5F8*!9d@9) zM#X`M%dAwSc%MGd^7*6weU0{wGXP+Ft1E~9bzj`9|HuBBg@gIOFLyPq{(&DVbssmCU>EcQ&b8V3=ep-Gl8zmf&zDPtez%fS}qH zPQg#{s=)u4Z~ET_l-QKCNTcED~#QEEwonGpaR$9d6RuT9bAeZ;VZOey5_YgaAvDcL(Vvc6d^%>PDj!Ejo1Cx)4k{22HkWC zIGWX!5EmVHyM?z;Pe@oUm|=@nIs~g z78}iO5ttT-((WR9Dz=HB;O=kx%;(22ZExnr5gfg=WeUjOmH>99cv*rPUT3oZkF9eG z4m4`gaLh?2wkEbYv2EM-#Qb90PA0Z(O>EoA#5Q;K-`cC)n^V=-r@GGR?zf+p_Ubda zNY)PT4-h3hqD3qwg3H}DKKv#e@7gYg2$4Wc%WA_Xs4=wUpH9I6=xx8hh4czJ2!Qpw zbIVhPW2p!WYG&;k;BIWa?;ok2F>KzQm`eKlIc3XPwPb2f zkNsqXlM%=}SOV(5N5K=$!3cx-!g*ufTd~g4x8Y}u#FM`g%NjYRY{4;2q|RvT)oaUM zbc@(``E4D*zsTxj#4h~_3vj80q20X8xd?)2$RBM?)eB$|;RHRwntDewDvR@j@GKckpalq^UYqNU^u;-xMpG4-b_UNr zLEtLG8M@#n*1v^N zd<~8`zc6BU9#{-QE1(23Z@imz{tX-@q!t3Ugh=d3z(~Y%V&E0an~W&haBz>W%xk24 zqD4gv&k2)|4dJ#esG?VKqUcsh2Zbxq-TTdj6m zKvk0wdMTZ}=p`}LJ^(iC7yb+NpA<+Q4dRAah8e)%rZ~LBbwJw?VNEwnSYv;xXeh=$ zRZ0ZQoZhY&Zy!*kVx}K&!^$|;zhQ$Gi+RNsPL0Gt7LpdNf$m{aZ>h?#->Nl#A z{z*`baeYV6=gi-v;#P3xMKsBgvFu+}mG6*jD`4>9-^^-%_V68QZf`tDoK}?dkaHc7 zcgF)DU@T#8{SLe|5?+cZaQqc@&m=`5p0J`R!Y+QIG(Z?Ulj?|0!@j?8cH2*!=meq&(M_Y{Y+lz=FxRJ=@O-1~dZLBk!kLfU2Wi0H&8 zviBdDl0U)ucVfC{Y*&*y85`Bw0uQ zU3Y1sguFd}bt}9nQc1_<7_1TA7hHM@=f2AdJiXC?{UnsduQ7X0-MP_;eb6DK*;9a% zm?t(+fRkF1{mATENg?*$eI#=Ah`$1Ew~weaf2n7X56)OJaiz%4o(^;&n^h;ySLR4c z=aA2Rqd%qP?zYHz=HfUUMAF}XRLTLzvc-xiM66IP93Y*1S8Wx+xjm`#_dGqfWpK!P zUUIV8Je)b;S^5ltE$-)PUe;S@&1dXmx)>~~1gSBouEBPfn__st@ShS^{(Jz+x<4d( zQb`U=B&moqAdo|FVy7her}dFT3FXam_W#s-7F{J2eQpH~jae3FJFy&iI^*AGmz*uo z?jgI0EI)*2I(_@$fy24xTY-@hsTE?Wsk+*1-`1a~JE!uJ*p^1l{N84P_|nMG{;@I7 zbJT^zgr-JRbFtFjvsOP&eU=1_)lz2`T5h8-VI*yuzs*H-9YF;KgMQveM-O+}_WmT( zO~$nBmOnou996c~(jTiL{B_1TH>el0{SZUpGt%J5^f6AGdy&a=bxy;ZNlWMURzEd- z_E3L~5=>Kw=2U7_$uYiDU2-!ILqyoX6@7GJ%#Jbkr6bA&Z?B#ea>WDGT1W$@jtt_L zy=Wyv(Si5f61-LvUgNF-*XMqnqeLQHlHE3jSsu#}nj*COWY+y*ig%~raoQ_vU0Y_7 zY(`5ST%(r1Ih%MuCJDUit8A~!Kv0cm%FV)PpJcnid0v|*bl9DIckxk$pr7L1x6L9* zp>ti(x;y1W@C~hwFgXDfEq?vCQ(S5j`FN)9xqHMII|_%i`W>G3vp(iXa*$cB>(e2m z>e}XSiH>Ynmwo4)=r8!hd*_s2~5x|>6 z=1-*`%F2~7Upq1@?De@>Py;-S56;cl?gAuF%&^^}tXVoVr?#em_&FVh#=m#D%ZIj( zAD2g=_WEAffGm8mDf>RK3tz!Vn$2aLZVAOx1AXo8XPR|pHVCY%;@vG-;q9cUniZ!j z@qME{6QtH1*5c9@8R-74hKgg>pgex!-iF{p+h@2=u4(VnswYwKcYF_X%2bo~Mx0GQ zb@!%j#r_m|9i2?3j}5hW6Su3x+k7>_`1(HJsZDUaiZCztbV<$C zX1MYCd|+(_pWwle5Xe7pIQj0bpR_OrCy@U@DS?No!%m{bRjs}}H=+KAK(2Lan_mSf zi!MoZLW*0vSJ%pRAoZ5S!vaa2+g`5b+!lVvQ&1;oI`aK7XOPEy>6wb_XHo$Ha)IHN zs*(*9eonB<-7jo_&yQyyl<;=_;>|@bB$xfgvFB4FN6qobgyv*J9;yCjDLNza7hR9T}rJgR7zH&aEDALNF@UjK^mm7vm z+^-9KR3*hIFG5|c<3yLcH&Dof^e6_q9?=OO7qiakI;yb1>dVdu@JGuLpNFPHX+T^F#H8KGacjAtksLzy?@$MNs z?9V(=cZ$8TDxaFpC94C}hKH$x>*EZv}dB>rjGRjs)ij{adgui08VG6cV|9 zA3`9-{)%)v>Bvp28YIS$p)9D=a-e7F;OW0xCK_=7{$NwBa0t5ecE$ETa9kbt=Y`|N*YZF~o4Z{_4f#oG?MJker0DrgGP_tmYEEZPZTV*baU!Xl z(bVG+He*$}y^?o#yLfbQ6SZ+)#C%iJS338iWyAcrYU{_WZ8!^6ErBLlDdMpr>*=@j zPiv6lU!Giy&l35vwaY^C1%f#Vk!T2W;?CMLK)yIPFJ##HH-YCbRDmp)k!gqPq{@&I z4;)Q=zY?6l5{O&|@kz1kW1w$0G)Yv3(Dz4_VV(*~42LMJ!~hi(nSc>x-@1AMMcJ7W z9x-85XlpF&<08L(wQrXYXux!~WKCLdAkL1MnQKRz=UdEl$~8@%U~lQ>xiFgpOX3a< z0NGv;x;m}?)%J?W64R9>$dU~9ME}WrAtMyU7-_VYMtN{DoutZxw(5+J)kW5z=Rkc! zYVM^e#?3tGxB44h{-G)+tZGU@L>-&J1$)5hWMBTDMacka{ zFDu=xrkAqXE#__fNYBYd-wxPZy$e^-HXCNEn$xR=*){0#bAw%dO6mHST)w>07Aah%wGNDM(Gv{i@H=>~5e0=v2Y z{0;@9q~{v2@-FDgr_#9j$(^p6&l0||HkuAKZLcXtz?ik8o+(f#yCnzgZnlwLOyNTG zhi+E~r(?DZU+{I;o*FCnN-$_U5X?s(G#fG#kaNc&*WYGoUQJ7#k}SRLVTTWBz@&vD zou*CPpX7jEU)7N1ifb(gmO;m+0xpcYFzK_uLY}9b{bHO>7DI&8f=-MO$}788gohJ=ZJML!X0iza@9NF3F4!ZqyJKE%^SI6?mwxk>Cxl zP|7w6vL2d~5X8mX@B3(0_}EtP=ue;R^vFtoiKksO;419$vzI`ZQKJ8A%+HUv2?pM+R60we*84IqKAg?7!vk~kao%P7(C$XwG?TD+2Qz0Kn59$-mp2$A! z!p@K+g+HT^inGX3;V)H&36u{b$=9;Q&}uuz!CL%lg3Zh@%N#fNKF#}W*WpQo~PrvWlLP^c*k>EZ_#oK;duXH9gcz4X{<=% z3#5vMksP$T{Vu9;#$H3ndI7b26aV;&5w@m$4i=5r1vR-gY;)yXCEy=K_HCo(T z;64!qh1bzoKm^@2VPh`aKcy}IT1y`pCmtJrO;F#P|%<1;GzgeE;L|=^5D)2VQ((SuQ6YkkclZ}EfJU;Y?@y)vkn;1}bDx+^k zHs8%FAm{Nu-|$^I008F=AMtPP#XuLyWe-9A){J&O56SOy$$A#NS{C3K-)J1_>aWN< zsBi#Q3GPgXIOoEw@ER0mdf08Fz3#08H|OE-u+4Yt2rX{L(8Sg615wN+;=D8!Y)N>= zCPQF@QxI`a!E>r=)i|#ouHe96YT;*W!Tr5XB4H70S-`_Z2j% zghy^3SAtJa;I6e|PW%}h>|y&%3&id1V!!$*eT2T2hLN4~36u}r87V^-Pp>&*LB$BZ zk(qEg)EEbsC*@3^P(F)0Hp@#1Hf3a^lhBrJP^__9ygH==$%s;{fx5L-ZyQl|&{O;5*{%^(BFeUPJ|+;VKOa^EoX^ zG_$ExGB)FK$XCgs^6JMz$I7nlZlA%|rTkjFr#5FFMheW`NJhYj`8{A5IOzCLjL$2D z_0QGZuvmU`H{n+M<{z(RYWye8mAUFiY13rBUnD3vNjW7Lu>m~_?!5h8K=LHEZKw;k z3P)`o;00vGoT-b~2N7qJF=1^M8swjed~(hVo3{QTj#T`>apk{C@IVR_CV{YCF8CfR zE1`?rMQx2QGcWW@?K?_TQk9w9y>ugHiZwCwPUv-2_2^+kk(g`G!%ArHFY%U|RADZE zdNc+tR19IEo7Q;`(HSP?bj~qnvfvha5_VT#z{RR{Pm?%=am5hY0U6J6=chUuwrQ?1 zX{AIs_fapRFBpnl>EuhRm1{Mmerbxsa0pUo`>x}FNlO#KJF(aELGc)>1{IvzD}>tq zIaamA7{wx#J=CKy-R{Wq4r}1la`wLWMQ~OPdG?hG*OL&6N_dS3Xb$9$^}rd+UDlHf zfY8pbsW^hXR;D+zTFRx{*Hehsm??~1>ew>p1SL_vgOADGhruE*EG9puYT@$j2{g-j z;tLBz zkKkQ|Ao2dQ7mWRa>l^T)o%N4;Pw}u!ATFR+{~Lr5SoD|VzB=JeH4U@zOz2$`DnsN) zJA7^IgSiakVy3pD>*($oYN9H{=EXNB>e2QyvgSPkpr;uMzGW`*rNYzRgDw?`<|uPh zSTVdHa-7P1pC~Qyw!isy;z*Ox%-2uL&{8UAD9tjSC6kQq-n`?=_Z7O!?X_PIKyR{U zY|^>Ue>BF+3H{!({?W~O-h5bpL4xfMt*O|jGc7?})#fZkf$xyt>MjR07omPtWkfW| za&!20TSFGtoao<}4Od8pB`E*(4CK! z?mH|GcbCpn?Mp&ZVREC|hosC1U~Ke?8m!2$g9sA?%5hH^`Ddl5X-@UMXl}Z?C=MUv zm|bqj`iP`O1&xAWO{~Ft=Ov~zZ$oP?ZGk;FR>huOG+KYhTh-Y-YHlMtmV@gxdkf!6623Z?v-V#w+ zesv=$kVES?*1m1sZdG5NEnc*gizp}|D&@WUNySL1DUy?xYu`t?JsR-(!>xDMk$9B# zK!=Ecs7Q~J1pW@7(Ygr&LNJwJCa|viqt-Zw7_1^ z)IET59Xg)uV{{%{%#WQL;l(2NNwaxx8)7OTl$rG&G+(HFSJS?XQ#ylm55V+D7dE8F zuzTwmw~q+@X@@9x4u(Tgp*42=IFR)Fez75AcP!aQHZUJ&MEyY^l$ykuxuttFT`ijh6+nwyaI%=4Ns_)Q;5Za|D zyR!YVnxgNyofpf#I=Phq5dsHZ@%$1jK@=3JK+z+Rn8r*WdqOm{c^4|aVjx;(Q~k9- z(LlN4kth&2x!@RNOzq5_Er^&n6L%-6feWoZJ3NkWU03SID|(H@WC&=i}^VoLU>m5~QmZ=9|P*}BtH?z~)$lm_PtEO_C>goI?#@Rkz48KK9}m+@90 zmQmo4tJx$h{R<(>TCEVrK6L`9t2Kp_qpabQl(dp${M&PwJxzo;xS**=UEp8)0C>n) z$>?{AI76)45cC0(C_{81!O-N77y-t69Rsa?&LRne08kV9Qg}K>ihIT11CXwmAqu*+ z|B?sFtZ!3ghWkxNYPNUGMKOrcGSDy$R((Z@6w;0lh>t@h*B=xjzZZ@sV#T};^VzBr zWW*w8JCLE@kxOaZQ6XaO0q@HORM2W@>@eM#Y4qqK#6vus`p?XKDS~xIA-}Dmg|t;9 z%8O?X!||dD`x)K%x3$H1Ep49N&|Hf!J$ApxaO^Z2IapMk+rbyral_VdU`-L+ES(1M z9*mNN?MO0XBE4U7(IUz4F$U1AZYt^eQF!y*)bEH=HjB zr>687Qob)?_EgjBk(y9s)Anfl_IN)B%t%l~9>!LvXI_K(MMMylY2B}asw<9`WEznN zZgK2xb=7)fq-@riPNXwYFx954tnId=1=nEdF_*x% z@m$|e$T-MPTD3z5-Y=)VcC(72LVsG0 zuyQ+;KL6Bm6YGbkr>DPLUy*asvi~l=m{mcF|m#%*~0i?v#*lhdHizIWsz5+FTRztCRC!I8g4Kx#yR=8(E&2UH+ zn-?>N$}J%G^W|}pdX9eJApBb*1>-~vqD=&Zdh|)%di3HS!LBvNmM9b)TnkIFxpFxz zo+)MF16Nw6KFMlqXMt%; zR7`sD4zVX)Eo>);Ubl_l9-Wr0LrHDoetH2Q{K?SD8$#M)n^T+CLX4Uw5hMp*5&U#?*{%SzPq# zE%5QPX-FnsA}>@CZRMMn;kB9gf2O@fsgBq6RWi@-c|NN+ARTh^b#PB;Z{5ih7`FkP zdg&**RPEN%4Zl34;y;|U%4Xj)<;B&H^rMZWNGN<3%nt8`kk5WjQN0dx&!GhW(|fDv z57D*F8xjuMN>hq{p-AK`?C2A7mi&%BEJe)v+5L09a<8Gq9LVif^JvrS*{WiSE}2#R zcZb!@E-=!}R}$1eWe#hZ8~m$R6wv^urrBB8B0kjUGgynb9;^xkJC~RV1xc?=+^;oP zlaOZDmZP?L-;!Y{LJ)|8DIj!pQTCtJiUd1ZpXC|+whJX6L2{0WC{>t+s__?$8S?(-Os}cU# zw?^)-T6&JA^_9W{3(Lz}@*5-b$dPvemA?pYUK!MQJ{xAgmYa!~5&QG&Bla_gs9z`l zD|h6p=WA{|@eh9kdNKKbzC~axEU7=xz))MbOhBn=!C2V-CvuMiy~71#;rO4(uM$)U zAC&7q0SXfb2kZa7$jn5Xtn5sw$lTy);9Tr1|J|mh23jFi=6!HEV*HZk5Wv7KgOI`I z$Tl~*pk$y1Y*{5rB;4TUh<^NtP5_&mo1@%>k?5XZ|Lc4C_VP9DvEAU*y#3_#=P6>^ z#nzlQN~?W?xtS5S7VMv+=j8AJ4P2b^GTYHUzfH~lGW<~GqQ%AXBX`Z!u{J0 zLQ@#Doe>El>i#_y-qjJBbWRiv#@Ws7Z=I{qHPE~Bn1B}xa*0$3E&v?`GO*d(?59r! z&RUt>3n=aP8G{~k2_ONm&W5fMZq}Sweqeb>NGsU>%#JS2kOQEl>!9spRv@yNwRies zRsar*z(HA1m);QZ#>IjxO*Qub zXjQ*qu*SC5CUL-;ngyxe5jS!2hzbyD#Y~?m_#t2r9R-Qqk&upS@cer=^2TVdY-pgJ zet~h5AiMYMdBOPy3YNBefqk(q&te~4o`2*f;t#GaU8>_WSJMmln2$KA*WS+a|D=Su z>Ph3pf<-z&M@B(80QbuU?5H?s;^3dxComRS>(k=ovBn@Tn1M83qxfUVHDv zG&=l2C0c1#mfvy%fZXfnXOLzjBoqVi=1|W4k7aH+sPzvQu?IrL8<4cy{C@!TosWmt zw^6)5jM`}|3wJ%IJ=)(?lsFU@Sax0$Z?}asHPfJbGc)xedg+J<;dl1;QNdsUcz7`H zuM6>UoLh<*V1=4{y$^&K2#zeD2ZUVT20$PA!TUj~U1=bPHb)W!ia`GuH^yx;Q6XG|>4amBcU>s3skH#Hl^_TTl9+G)2 z#y8NHCPM|a&dx6qVFc_ErM9Lz=ZJvtu_RAlvQ4FoWL_Xf%PsQG{o`QnCGLH3hdqK6?pw}&yk zgmC-}6;_dvt5zT#1RzysdoBpT{iY1wwlfJqo5bEp6q6uAv|`vP_58o$!9k7#;0Vnk z*Ft;pYd^Gav64gJ@dEaix7)o-X?B;IdnCWy&?tLgU?L&`tLGZ>K%HHOr0wnbFr!{z zs%M!7>l_qncT%Qbz^HF}3$i6keRQVX3BVoH(y@ySW4lBIFh%O+x*lB>2ISQ7yy&e> z-%_lIM6KINot@`GQ=G26M+VM~&3c9El@L*6yS-E&{kU7M{@#dtF?P{K4f$$tk{Knt z^UGz%%#5MUC(-pe+)|6Q3LA9od40Oz8f}W0;O;0hSXZol)l; z>?9H7{{K|M?Wa^{-}MW6f<$shn?)4A?seFr<<4?*zmitxvxyy&O4c-~?%9))GA5*h znY@U)Q!{wzl@EErqE*$dRjNs;Qo`GNsghhk2c+D0fAQ3m{2OESHcjaX)9>UQMNU6d zvoZEbhM&g-7eJ?2V|()ZKdT_zAS1?>E2WugQrpe~a&O8{G>~~**=cC^Ae9--ZQ2ix z8j3gRbvUm!K4_1TM4*vZBr1Q^YUcKzpC=PZ^W;5Kw?^PB1y6LL&cB)03?_mUveFi6 zjbY(LO{eHW6Y%Z5+!W&~nGMG6c-PNfmlgGpoP!K?ii`7MAXzVDbZ<7HKj~>+-^15| zmUJQk^;#})OLn8kiTEs+C5egAWs4idlxDX72-TaCCxD@<^I;RIK=z> zav}n!t?EMROxpDKFHIr_fg*FLz!s(x3A9^|)zu)9YR%UC<+H~!@e-6T$iZ-(!lWgO zV=eWZfwCZ9Oz?vJu%7J*PAnuIj6D6p$ks9NrkQ}z!#G~&<%0cT{84DfKP*v9z5#&~ zzruIaV)b&wqFW&;e;>H;orF8P%0}{xK*|NPBn`Td{o~*KOr{TVrv4SHkG$=L;1NYI%Dev>+gCXmu;0}ZO~zP6YR?1kNyRc*)pyf1KVqm`<(grAzMKUAh62x)Yxmw$ z+_lp(p5S~PC9~eMqD~UrP3fPIkH*p+_OA)h`>}phdwo&MppJ#6f4PCDpuxD@7lZoV zJ81`oF=)ymlU&x_HLkGl1(oaxvZn)MzUF1UTcZ5A*||XF9DNL5NU71J(qIqJW+=Ys zEp*(trJkwqJBQiR#_xw1SzhFRSN75^S|v;&=HkbUkJ+9FvQI7=UiU+rYnpmFf{iG} zl-6%hzfb(qI)7&}=}h=7<2byIUWYTJ85kUo>)lrDTTcY8Av>0(t^6q0sUj{*9H?Pr z)->3wJ}TbFTOn#{YbImLf0hW$QHi^_UbUZ$kjY{im|5|=^cIj~r8avfZAvr6Nv@nq z2;GbgA-2;OcvqYH>uDt(@g>S>SyXJ492|}y{zQejZH+35J%?YJy;xpWs;;|J!>7g; znF-q1#$~Ox8Qp}uZ1su4W}md}%qq55FrJ-A-jY*>gOepf)}JwoRPz8>82|po;J7Ib zZA%#y?2*ttEi}`%;_3@_V%AuRHi%F%l8gMl+fgo&MwDHH%wXY26o>Zvsp=?PSDDgp zNV$WNwp1~Zq!ptlOWpnb4GUfn#;$hn<2}+HzlNi5c-S>_%a8esqok;6 zgdD*}_EpdOIDJ&?dc6)9#rLAGRMC7?i&B@!3_Q*yRpFk8;WvlR|MV4Sdq1r!D26z^ zluf@c54sDgQ-mJD}xsB!q`SG{+{9Ut<6(pv*b)X z+zS2-+L>~J9q}Cb(`Iki$k+&R2eUi-1=CxVc=?8 z%2B%6PP+MSB5Vch&HCJLlHi)qv;J80m!P~=` z-_hY6HoIQLFC`gTf4-d;Ft?%T(*wcZ>fn$+wQwi4w>ct!t!CfdW9#S?fU`@{U|enM zr7An6qwe#Q!k3lEmc#FmagD>KIGf$-mFCc=#6M0dktU;>hb@3&fKbj%&mSu0EX2@+ z<@hij+e!*jKSQM_2XEQjsqnMcH)oQK4Vw8;=9IvOeq^*YP~58Qlk0RXLc1L*Gj$N; zEFYS&)bbWyY9aTSuQRJg z^-?rX^>H;?2>>#J!!UJ^PCbXITCwS@!W>Xf_t7A5zZ#Wq)3n~SQD4?~bf#&Qa)Z(| zIiM1Jsxj+cxbAhsZ(Z_^8LrG2_ZGdHOZ@ERPbh)r`1v1L>QX!RGuU)(YqA{CT`uIJ zKepe3xu3mOxK@u`yxW+Q>Im~u+Ps%4R+e!P$PqC~&?Cox_xqQ-Hinq!!C&Kz>xN5< zlG``W4TJv{JM|NKbSmj-(>@E-1xi?E&hi!)N{-1ISC%8dj{=@W60QR zR~Ej)=sV_gQx?hh+!;U}V{kH@rhaf!m6znrIb^@t;p@lY`CGqRk>Ty;-lNO~{;FcD zzYZZxv{R`UxsL5R_iJH{oX=>ZJq+?_YZp8RRpbx`TEuY-o=>I@fqNoUl)@`>JXs20 z-Zr4z@mytz33BT8XZsz!?St_Wti8`{;8tsJO?Q4QDbrHk(fsjFlPV*}2d@+$kTW`R zUHLoa;`Gf53YkHUZ&NRudpA+AD)f3%bHvp>4X(;4Q<|2haa)++dA736oAV;3_-c4a zY_I^{^A-#8j{y%BZsiW=`zMh+gBvDLb-j=x&qgs_S9$Jq(BRQi&x5CBuE#K@^arlp zG~_Sxm2q>G^tUDs&CY<~GL zFBrIE+oAX|QuS90wA)8)A^4UB;x$x-s7KefRxksylWMPRySpnD{0Uq-^aRK;HR> zdF!Eyp0mJqrXDHz)ko9XKXoLh1O_!rQ&v6i9Af;{8rsOzHTpH*gmzvwy_ju1U9PUsloW zE$T6M`PZ>{#5+?MSErT@nAp!;=XAbu+Iuq^s~NC`3$zT+D?hz&6@V>oA-uq%x?*%d zY)rW|0A1d!YbSQjVXc#%p6HZ$>W5})lybAGrRFX&neK^aHF`ftg5Wz(B?wSL!rEev z#vY-v%`)eG*^}Txc2nQD<53NS~9A4{AbLv1nS*XS5(S`!e~OeLbMy7qYM=n^>54#B4p4?elE&wC1PzGIrW zAC!}J=%W`)GF0lsVLMeom-{8WJbMX#kbB0bnO_)ufhn$MyzBUREL*cs^tV52ABP=f z2@uDde*y_}(w7qePC|Gr$?^kF$w{8ghtwI)?``u|`%&^wV{cqV(M%nr^Ao63(W$mR zfiZt_!2Z@Ik2p2ffPC%46fGjYrXR`*9FfxDljcZKZDe`U1dMl8o>V<&Bomu8SYIBoxYjfNt`9+qkr#dumWSDMC z>j^p^sZ@(_{P)vMX&I`aLc(ohT0>Fmz5Rtsou=oKC7ejwT5HZtT1;NzHu15f=R;!~ zb&3i=MFgRjks9j3*|w>eLz0L!jUEFo&Ftzm*+La7&4VXLEA+#u23BmbGAd8M*JP!_ zJDKLm$wWM8bkaC-^Ir=olJq68$N&*119AIU8?2?13CrsI)5a9xN%&~3c_^7f-|;F3 zfGZ1}7p^9liF?CE=jP^Mu(ZODTP;>`d+ZkQPNcS>TOPlgQ0xBLsuASB(%@_M=W~z- z=RL;NV`-RC9o^8MmOsC0jilwU>JgQHkxTeW=HUhsw5%*mo$OTnh+h<;?9WKpCYF(K zn9>zaRoEs%ifPzzG$k`}^zN*d8b;!n;llWL=<5xy*u0fK&P5LW3YdsgEVhWwHaC2L zLF4Vm9N}e6OwxT)>9<22gxdH43ok?e?TGW@WPkO)LaV3K9q@_OF;{8XS+alcc@^LV zwh_k`r3}!;h+${tb7KIKNIYb1_(q?0r$)w2Afa;yE*Rf9@yZ_Oj0Q7D=KT4Os* zFF)Rt6W_)pBO&wElGvAx1bRLfzBuZoNH6LG`RfH}b&MtMp#-&z@cZh(6(@&{|CuhG zx4mQuEMM3QGZH~iTG^YaKl!R?NgvaqQpu0s6@}!Mu_vkn7Xysa?HsqWt3bl%eR?A|_AD6vx|t zkYetM;CmC6XeQZiX&Pw_K{zqCjR%=X1(U;LI^`kpH;C1d%F^GL*i~J}s7#vh7g&=C z5dOIrS@lm4RslNMnM`TGHUsq&lFjCkhLw@OeP#C!Qz0{;(Fk{ci1A|?$`##Pxd!yj zTv`@5zgWkqDDM!KZ``j9F(lBmaGHtn>RRnoHEa9Bz~{-AwlNSa*%;_Oqo`&2GfNX+ zt}4MxIiD(fI-j5fueJ6czoRnKbpQ@7o3X$WVy!a9m7ollGVTeugWE!MN~$2%?6V zJeln;4Ww`R4J!$J109NkLtlR@Z}1J@YpS`#*{-*${9TlfJn=UpAX3Y^?=m}u1o^fz zKB^oZ44DL624hE9$dGYR9`D&wLH5XS#tx!enLcQSKP)ylV-UVO=Yl zLU=_Bjgi*zh_3)DcUeBcUyKVSdJ}MowQt?)WaC@|Hbprn^cE=N-uy^W1fzv%`SN$? zi$>ULwdMz)bCbqG3yW&pG5w2!^9kqKIdsZGC%CxP53w1)kLG8jV#(NlJQb+^DELQS zxsX>qVXFhm_(N7rYuBYaGS|^@3K5*s>|VN7V)m4u+a&D;DM@zJV%9tI(|6g`Uh;Cj zV%B|lY9j4D5#TYwJ6Y~b*uTrOT&9`1dms+WGa3iHjQz!$S2=@2gLGA6t9 z#F1(y462dT8&s|Kk^-)}Z5Q;rdtW|Mg-Q}M7)Ai@7l94sC9zhBv5`~cd1nXpcGB~4 zUlha^TlyoNinFy8Y^`CUSGQu79v1fHE?>f5VxwIC(pzilN!fJvwB*z%Wt{&I2VL9N zL#%3+a(dUOCunX9D4yPHD?^4b%a=o3j;T$A;o6pMOI>zTQwAB-!DcIXS)So+Y9-WO za%sS*{N@K|E&47PgR4rwMasIqlH{;#! zr0|HIDRc%^0z3xysYN#BU`l2#qk_I46?U|*@$HRvsMQ}aM+bRQm{JLMm2YTB(z5%( z7h=(3jXOQJ0l}dvi{mnZv>eRtA65@hLcRd}+bs2p9}uM>l#L=bCOCwj2X@M)F@}j> zFq^weXd8FF51;#*)GM-5ZM$;+Q{wJ**kZ$2PPS&8oIqjVhDE_Bo#||GZ~5fJa4s#I zF5K2ILBzPcac%xi9$Gur1S0z_855m2(Jx&|J(pxY0k%o&XIa2c>J zLJ}3!`>xNJ9StKFLCtx1?7^#T03fdcIwh+o81^Z{vF7$%um-B`TEJp+}Av+W|)r;(Qj-+WV_mAbC1l};*PuT(#vF_)pw2Xdi-c>6* z63VYhA))i-8q`zQnjf^?8ln(#vD{rJiG{Hz%;(tOhz8BgQiJIYH@99F$f^PPVk?1d zezmj6+X2^U*BS6DMejYA{m>F-YnUsF0e4pU zt>y6BXb~^x9O~Wb$DC!_3t^Ub@H%|D{DSG* z4n~6WO-KaV^^7w7QSaZj;I44vi-5pi>W$KeRb}1o%IMkOubX3!n0tV1HaJKtMjgx> zr#OmZ!X9StO55vsszT@M1SUGNo<@<~L7~o$J%@@B+6&O&i)3?PqaS_4;H%EuPP7KG z@Pv$npSz(nIc`0)%_H;v<%w=db3^@G%2HhH=EbC0jfc|rJ>3Fu>dbNrDX_cX7>Zz4 zzmfM16RN1&x^nAJ_6*N78*9yj9_haC2c08`KI>PrBH3#Q zN-m$(`b5tUA1)|~ z>lGPeiA9}9T5FXzaY62zCFb(eh_FpdGsEtQ8P}uQZFyQkZ>NSWXyo}hdFDrG9B(Z3 z2}gc8gllKe*&9IZ$|{v|_}E|xp294v<7CWnBRz2)J0;D!eTPCz7+5;Vf!erT$#+qI81XEyx4lqZ7@EFj7D1)FPluoI&3Cb3#Aa}$2#!G+)4eTpOeQlx79|Yo!n23 zGb@#<;}-1l@t?f|hyv17A(ggqzFK345DW#nvbUEbG%+C{UAy6EclOvPNr{O^avZG; z(2$;5uiYMu{bW!b#+6f(G0raHBxfF)jX{rO^BjmXsr;b~j%ZzW^paf1LgG+1 z6WOb(69gpQFa~K34$BvZU2TxDxZ04$QL74lP2D=|Knlv{P7+>=_9?$ z!$74YM3GlZ;Trhm%4eDkYkwCMA;0Y}GNv3FoI}9Q|5LU@BV$r912|T1ax1eD?qf-z zp2aKX=NBjY@S%DL7bI^dV z!;fP|9jahDgWWW9$Bh9C`~dR6j;_O&@{mLLV|9)aH%<8PTf5r#u57d`+x^_KB( z_|mn)g^fs8T{2m@xr!ht`1C7vIzwmQEL*&26FK-};N|8T)G?)~aH-dM5Y1qZZTNyQ zg#ovPW>g@pWry(1uBSVUd0~z8L}0%Z#!aDu9{p>h8wK*0fbua5TY#pn6tikKvBeG6 z)zcm2uiXSYsMul83N79;V;s8R;VO7kIU`sOzs+&aNk@logLbr0v+~0(V z-cYB7X?eR$ZRyb~2 z>l=dqRxd+*uvuVlD{QzvEvJV?`~VQP=Re46dy*6uFriMEbl7d4;OXHBs_vg?t|u1~dsz#;M?5cFFDZ?DRt{*2EN7mMZ_yZ3#M_`|rx8wW<-TUbS>FH#G^@4pb zv!EcZ&ukDb%Mh=z)S;T?u*aR*4N2#u>hWkOjPzebonv#DUAsWTCTZ-(R%6?ZZQHhS zH@0otw$a#j(%3dmXWnP#{DAB0p4t0ado9NxOoZ}ba_5Jqw`0J5#B#g0LZ(`gI|!U> zRW53!>QKDhnfbwYiw-r+XT3mw*vdwA2ysBUwS7Klm94Q;S0Apzctzvco9$05g(&*{ z==0LxcHiQDH6^G72&~$QaI=x0hd$(X=)vNOaT+qvxZ1k#U9AEOL8e0>fuhd(gxxM& zMf)x$%8bfkA|HT3JIw!aF;;oHZJ%7qbM8a8vL=1|)sjXc^E+P_z2Uh zA=ezaz5Qa4WIm<+J7Frtboy>;WXkq->xCPWb7L~?TMHobclk8$Uuu9+g#mHOxY;+N z3vI}fcGsT`S$p%9ZXgSLt!(qg*9RvjFr07z1EEI{F3ApTyQ^e(=6N+h0KwXnVmK+r zsjTMXi4&5v9WUay&L|?5Cmhs=O~!1HhxcykKM(WwXa>yQ`LdT(&WsgH|J>*-)z8Ol!H^^z@Z@2U*7CogMb++$@9Tq#3v(>!6I-;M9pVwqEyuA$oZJAiDk3; z7Q1QZDMg_EF`30G@W0Tjq0p=9i{X5=;Xahd7<@(UPO;iO&j&NH%y$S z7$nP!X~KU?f(|y&-dwMTT6#J(>fX}G)-|78Ru%$VuwxzcxHKMiXcE$jVEya)cw%d2 z*iVuK-R7phq}&rlH_dRH^JhayB#+*s9Mo^w{#C7;CGNQAa=UMv$gXjFmJ&pKb}zs* zu4wNwvU z^M9E7eM@)tNE^Qz(3?Jv((#cKDo9l~ml~fQaA%WaHOypR`zv7PhE5@N3T`I7i=tyx za2E8=7{+Y3h7)fiod((8HOoCAFo)%`%9%5D%AgTFLzCGs9ElXURmLOl5}7sd1)9&- z2l_vHjD_)kgc!~>s0l0>%YQ6c(i^A)3>eFQEEx_Y7z8313wz5y&WzITe;652i&X%a zy%rejfAiN{VCvt&SpS>XIRT?Z1!MhhKIa`wiVuwSKNjta3!Izu zWE#W&U^GZZ4ptWC1i?@Wpt9Y_*~F%W9Cuw-gd2UmIZN64YNgp`o$Y7k3LAB^Mjf?Y z_S40D$)A_@FXtQruSgd;ZPew?BTarbtQ$lVE~4D5xUkl&DrP)XUJ`bJt^VPWA&Jo` z(XD8yWZBWd3CL?Qvj7qjR0kGD=jPf5bXI4!-EXBCAfWl=!yu-n00=53CMGVHz~I!@ zz-<4-29`isd7-R`u<%<>JRl7sivN}Oa_d~r&;S_=@V2I_udlABu5bZ@?|R%nGg3h^ zCuKm2F_y?dkb?-)6>m{MBw;*Meg}qCg%#wn?u10dH3cyWEcLGpE{z6$*S9=^uKj!h z)wec-ZG0|ap?IYSXox6&6ZMX64X*5sPY!Ob41N-z)Anu+&JOpFo}6Ln??Ny*u{64T zp87T5_4Y5VtU*4@f$2FabpT|>SLRP926shoU~dLT`{%~z<_1@A&*87a%J17ATp2iA zU#RP&ds6VV%pdg-YgircN^?EVdJKA+X2P&q6u%q7I_~&^x=3jd0aywzE8&=$6+QjO zXpAq(fuYg9z_qkH_yb7FOPR|}y0O2aCzA%*jE zZC=6$$qfUja05EC?$pFV4!-cl@4tLVzu4v9xBp>!uw7qv`=4sjpD9xBU!FN%)QznX zO--G7lgl9QUVlKklOywJcdVvCj6uI>u&spueC5RCMFlne`5Haz(X7aL=idARPfcvt zRnrQzp6o6)C^UO-ndz6<=--5R9_U|P!PU1k|5yYXfKe42Kl11$jAnAZjn9fI5H51j z;jerp#ukR=`j3!*O}GkaYrn51fBN>%B>n*Z`pA7m^z^Su|Bsa2m>NNQ)tlO{u?s+% z#0=^^Cj`(B47%6=)peFW&jjHzl`XD$drCsWWUiS0BH!=%o%(r4V|ILQzKbyPS)LJh zq6aK~#cTuF&R?*Rsa17&*EzEau568vzB21yXzvAVt`5%c#4mcVI~Av29A8$KGSV9| zk-MrX7m*(RwDS#2nP+u(!4x7*Z(eD=A;Rb5*ec<$A66Hh?{fogt>tiAXF|sr#cf~B z9yD320Is+aLG+`chSGgux;FaGTde#8I*@N0?`A)P`;F;1iO4jwLd;8te#}X&y%EXVPt8yT8@5geubBWR*YApSiUuq7wi(&K?B!(7Qfs@%&e>giYy*pMY!lF zIK+nyfk(WZ+5O?Z_?sVwZ^fC&6x?1vIJVfazkMz0?su0EyGyS&433xgq5s=UU1Ar|HbIw&PPM35t!nS#fUl$ zPER*)Dv)({Ex)c`4^nFIBwl@r_kgYkiCL~x_u8)pD~Bxm8=7|+rz#FxC;VVOXOxTh z3TGO*R5I6uT?-GAjZj62Ycn5wAImO18ykjDB561_$5=SbI@%mQ z13p|vz`V^j^i7U}HBn6MiO>tKIHaF|Mu?CfgjV|rn=*gIO(+TaAx}Mlgv{4 zju}1_Oc|Ne5#a-A*!<2K>z-?L+ul}EPsH_aM~@&9({KCq^Hx=;3FBbB^j9nflh^!1 zp$o@J-Om$iDEHQLS-?-a5C_{=s}`2pv}9RJ@k1~ppMd6pX~;||1vKgptXvnTrKUim z6|zN!3MPlzJG9SL^DiXtC@+@;4q)1nJQb{{|IEd(&scMt z5&{Y*qIR$(Yv`jHWr#^@T6DlPEAg_!as)IrZ}kfXmyXUbr037H@Fs~;xf@4< zR`D$bLE!t&*$@#UO-t~6h*K>1U&1`y72GO&vx!&{Q?5HtQGt_shizev-z;7URWh&Y zS1KsY2QtXJq5vaXX2Jb49_Pkz#9y~DGAwaQ4l^WXg6NDYZL$nLib?u%X%`DN54&yvd-~Ng*;uKSgZO;p7@}IJW$)aDRt3&sUHm_neISK2Ltd zFOKwyD)Kna=rG3qTNl0>d*L5KrJ2L)n{i8jRx-qdP6F4Y561$fw$ZjdpSjyGhz=&G zD0!2nR3@$QH%PfWy3{9We+v+Qv=)4_5DQV@5B6)4bM9V6klSaX`f7fgW*&rFBJnK< zpyWLy>TYI??5yCI96H`c^s%zM#S0?bO$@GOb=;N97!LVU$h?`ff%pArigrdB>zl1K z|4WHX#S5SpTlH8O#`5pV!d{UsPk%WEBba)aHy;W~aEG!j8>;ZFu_HQa!!oPKLrz=h z7N_tECXlMec|Phlnf~GVnR4|cZw)+s(20z z_0FNJ)9*xonlML#9GP_6?JlhRLVql?IXNmy3Jl=w;(hoVv(#YHaxeCuay7#Qu~*{R zM^RdnX7?>n?tiYD*>;$4!{<9q;bo%FW<4Ti)Cl3^MJDWX?Hjbi6a{4 zR3?CyM}9hrvydb9xB9@g^N^QIb;}9@d=!i!nF$e8iiwUwWm$P^BdPdQZRBTUpE8l} z+0r*jEi*CFHaRf<%l7rIw5_s&$Mzt6Xm19!p(99DTAi!tUsWpfP0tnU5ck^W@e#H# zbttH#f+-N>KIk48yf;tI5lkNHOdcDCC#1kOmVp(HU6FT@@r71mEQ=oNKyic`{)7u! zM73DGY9Dr~T|{j>j~1pVxZQ@Y^6ImoApP3HFiTgZ<60<7Me)(7{ZN(r!&lVn?r()7 z&}f&Qm&?#%y}b)=4ojn!9Th*dEPkg)No}ji2fqWD`B4`h*9aFVRaq3W)@xpzqj>=7 zBwEcPTpfWa>vbi|H{XnpLx>*Icb5ZR*COpgj;%h%@0|Xom-sxVM@lAJSl;94X{}vN zVHz@Ud$4R89fqc^@T2P_{II8R@; zK0?b`;%jPp5Oqinyn%XEg1E^Hv;>g-xFs_y0!LNMkA6iH5TiAjBH-WtAdG(uWm)|7 z?N8qa$X}spX6Cv-Q^y4>VA$WSLUMYJtmFV^?}W(QHA!-0n&3AoCk{hJZ7}xFKV=G8 zr6<1`Y7@DYsslJg+^O{Xcyp+oqG@e~@paDZqhJI~v&039pJXNDP0_ti;->*~o-pU* ztR=E2?cG$T%ishLyOv_Y?z?5n13x?}65C%r-_7>0(wV;3Wm2m@h~IUWBV6fYB5sgG z3oj*w%H|e_Sf&PU(^R|B2=u`g4@5;o4ho3~{CZ9C`Fso&`jD_|+w6ne_isE>ol0XU z>%FbS!%n*?7i`%Y(qIZ`LrVuntOoVqEgN1vG2Y?*drARl27`v}^lu|^U}1xNuZ|q> z8e9m4`;DSakqJ*6dQh8nyP7YG{^lre^DQT0=Qej6qIV{V^&xy*8|p{AdUq0QovU3n zRF!y5S@=(-104`b4qJ1bZfX%oH4`d@EZKxl?Uh*Y(G=mP10tF6$wa^wuKDaoWqEq1 zEjjW5q)mct)D|g8$}8o8W&C-Pj?=jCrAayF9)DpvReFf|Je#c!&Sz_*TG5I5Iq)qW zX;Igc8lRMiC!*aHx=HJr!H0*yP-T0c;!70w(ue{CwvT_t(+TBK_m9{o9`C=uRLDay zv74SEW-8|HYEaOGOR<2}dl8p~MEiI=+%w+T&+?eSg8hp5(eLp$1CxihVk-L&KP_`3 z?dAE$OB?#*AAX0@ifh`RC4X2MeuWUCF$Sk<8mFEX#fT(S`ZSdZS?;(D&^osPIY;Ki zNPngyxx(-QGihC60@rq0u%h0R))GGb=}bXO%v8w*N9^2{=}ZB-78VG$eh$ywA&q8A z+2=I~THMRl>^Kze`Lt1Wut|F)y6WQNTN`Tx(HZpe2ES_pr>)lYQ~$(5S&JJsJ~3o0 z7lHu!4V2!vcSXx}#Im?_^_U*@CBgUBi1rd{?+1aBSDEU}+{hoMt-asC7lm<7T8Ckm z24FM{X1Mlq&<+1>Lq?=n1ILpl)n)N!46uM?9ihfvtQLqP@QVG)o&AxNl{YBK?nxtW zcMLU&Cf=P5>YkV0%+K^}6%*N)ZdXfItIy^Il!%diE&l^r80)eFGarWgoqqNUQaL5& zlZURq51U`!R=-$MqeAEau$|+8ga_`}^uOx?s+sig~%VA4xwLpOWQo<{%OSQJfd@ zH^vR0(?*rsKQ&}-?r5t6hQa9{Zv1w+A6ze(`<|)Sb%HP7e!A(q zEd)FfTx|pPvaaH7MXVS(=Er}M57^6nwyjzW7V;L(j`m>L^Lm1R7KxR*)XAGgh#Xj4 z+P=uy(%;2UF)Atqvk}BXtLY{K*#S#b3dA#S`B-5&!V;{C);mY15p*PBKHyF$_?-X#@u=(Z>KMY{tby~mP%Iob5r8{I*va3`6{D@?Xi`s5}gpSf9 zl?8l0g@0Ex4APgeT6AGgVcv#Y(&BMWtFj4N-e@f(y+spElY_aT1{8T_Gxoldr-Fbg zF?PkB%4E``bSa&~i$M*fhe+2`3i!cqdwP%BU-YpB-c6Gjc=bnd+62BPpmTHbSs&!^ zc(MV+4;TsKSDF;&!p*C>Y!4tqWNc+=+JN0BH>GBC1GGR5+_R$K@k6boUR$>twjS!8JOq$ZM9L1&|BC2` z>l;13Z`Ob3kCZv074&+s4-KmZ3!y=Fu?z@KqE%a3w-$kXIPc3|_;75;HXOAWMG z^phTQ8vbKz`pP9ntk@Cpt6*K;_RK}3tR|nHyihQe?ZaIWWi?UJJy*{bB+O{PRq?cb zpW7N7RH5{Mc%;Ic@<8(ux1FiPRf>+ninUe{7sq zXb-+v3R^uYs-Y8llhk#*tw2gl(H$@8 z14q9V{Wk^WbW};2i{Dh&dlgqBs8OsSoU7{IiqoSL)(8g?L3<}=hYy0PM1xoUqTt)P zI|Y(ESJTVCA^o>UREbR$;*D@AlKtzj5f#`4k(UK}52Ox3w(aRTK!JyNI2DyB-9;E{Ihw$G>tt$euR&2qeMIb?69IP5xAq6_j1Iy*oBC>CHPwi4hHB!1HxcDfj22M)L zq>`dtg_&o~&pC9qGVnyDe<*##=h7AB%@hUYwJos)y&wIG7_1}tX)aJK(XCEbdZRZ$ zny4$5o#|Na2JH_{?tql`lVlBXXG%%O!hS1A_vcen4WF5~$j^AIzh|8BmzYrG=pqC@ zj`WzI^A0w8n+JSx+_UE!gM2^yn!74+_)5dLGlH%vF3%VupZ5C`u;M95I@`f#C3zkZ zrN=@g>p1!-K|jU>Hu08GINX_8MEwACg)#`4PQw+uv(Gms!>V(3uNp(>sOd@K670P2Mg*ZegBlhHG*c?Vz zBR3^9>Fkh#<&=b!ybD))1oWgc{#NCem2D% zRI!OeKcIS3R2;4=_73uH=5<59e4zr?snPk<{*%m#L4mJXjMjTHe6Jl%(3&6KphIak zk6{~DUO_J9N**=C>4h<4Ab|?g4P)Z3K_)ZbPaE?&qfN{+>$1NKt4re z)KG~ydTmYE#kWL6C1-f$b0><}0t#5IoPtpqgjYLm9rXJ6wV0ZG(Oa;$XMA)`)a5>BAaXGOOlh8A zC*(QHCW7c<7GxB9Ej~Ob+lL=IVCpG;TeGl~pT!!CmsQafw1MZ+oJ$jBbO`HLCLGLo zteSZ8&uc%haxZ#w5XG82KhzFAIxZTxRDdgXHtuh@`SMX*T#F-m=+p+r0I%u`2Hzqj zzl)(vj9QZkk(YJ40Gg9}(ZBP}Z^eZB!cg7G5IgG9yaNDpaKPPTQm1G2XRr2z8T6{&4M5o_33h z6$JNsv)iS@ZuHD(*v=g?7f^VBd)GH}dBBqaM5RmMmGYf)<$8JhyUzaEzglp}jUaif z4}at=N6XD>omYK_dx`PFu?1IaNlK_Cz#e|@kTbW{-&XfZnxt=~yFHpYQ1Iapqsrb+ zI=b6;j;@yL;WMLM?~gRX(VkoB&lD?;U@m_)dAe2@iJ{@KV%O`?02G7$mt`#Oh*yFB zG*r2jT~$sGR?8Zyhm!XALAcFMZ_-`Q^nB6uu?e47)A^ zyCPj!z32;X3uU$)@*6DkFOxg%XG3+WarR=w`q&+Y%Irtr2c(`e->vyfPK9D)aA0vA zZr<43Zzdsa0m=1U?eOq{h8DZy0Zk(d^r+1h?no+Dh=9rIX^-I4#iSRJ^tT#FxEeba zHZOX4HI4*c?RJXfpLV~Pv|k5Hge`>|@D!TjaTcVikND+3>N(}X(y3|?q(!dMDx`It zxx5H#a7Y2(g->0AHfa|#b*QjF7mPFa1o?mI=O7mmAaC_;KEJIvic0iEAxmkG>YE{J zcgCg8uXTafIk%d$G}mI7bw4jp(f#cu+iFkS_;rG3x-^g1Lh-zub))x#lwk+%vUH_QL?F%8wdGU41(mr}XzYYHWTg z?}ypRyFMBzRl#a9Y3BhM6t7chjLcjAPPB>|DjQAIOTcep}cw~aaLwZ4>92_f~IPB#$8(q?B+bcui=XW50b@IrOFQEl(}{3s9fD6S10^K=g2H5UB_{Z;7$*3l*$ax#3)JdOTI@C8wDYU z6iluC+uZ4a6|$fpbikzCzF9`VCvFj5M(*9^jfFR=cNwxg((CRn$4Cc_i0sE@1&@2s zX814XWbVMWx1d9NP>LEwX?osHfY|!NUlH`ihvSvxukTG7?s);|;!1K!Ts#6eGv6eK z{3jSu|DQ}AF)G3L+ij?HK?}3CS+U0$1aS0IOhOdeJLVzG<THQ318q{@JrTUNvRG!VE) z`}Ll>Gw?k2>6!HT{E;5!rVNo{5mZ8@)SK!n#pYrR`o>fxT?*Y}M~L|H{ztG1gd3OQ z7Bz4qJycnTcQT4T#Y{vkIq zF>9$Kg+q1M&l6Qy>;BwwiqCBPAdVr4+E{GxW7MSnF2ERE85M7jmkS-l($znI>G+__ zSmO4sjyTK>wM#b8+Ov3@JBSWjEJ{qD#7opkVjX_(=Ds~uAF2N$x%*f} zmM)i-w$%WJKOBqz;871w2g68y7b=UV<$TiC#)BV}EMZ)mSFbMa%-t{>jim@>sF`5H z_6Z{?H~DpjNsJ@Y9{%cqEo{pGHCLm3efCV)Yem7#+VmUQOG~6Zli=U`n)62dChnbA z{Mh=beOt6+H*_$OGWQZ8VWB)-5jPP;w1F8KbEo4e1#xRAkXpvO>%&jlWt6j6o%oFe z6Z%J=rG|f!qC-p#4p{dyX{DoNv=85Z? zt+w&tT{%ujOh+MCm-L3y@mS_SLF)Y;fKfA0@e&W zQO{AmBBIlI?=3#*skm4tQ3=zES=J;-cseflksbWKBtD_UwrZH0+DQZaPDS}Xk_-=% zU15t=@J=dsc}Xc7Ts|Irvet3;ZkTegj&^l$7PSL6U}-MbWg|1oIIQw|-}5^GDg0e^^f(B%K!K$JGbD0E^q^*q)k%Q6$x_bjm|>^c?)X)!WW_bzYcrdZ!6 zAeMN0?xAhIe^aP(P5#|mr269f=)VYU`Kx=6{)G1q4WW_`MKOt(Zi5aOWk%93t8JCz z2#dOBY(M;Zw8U6pn`H`2CY0sqph%fl_S5Sc(E=0~hM^SC+P1}1{)_1&&yV1En@ z*)WfVZoY+kDMEjt(yZSAD!83+NMXCtDfu&%nRoOd!5s(x!wv^*vrEneUPJJ4d+n;& zB;zLN&|T4YtHQ@o6sT3YBQ1VfJ!!9yoaw@h=J)4!9aai2PT7cYHT=qHnfsc zV+?yuZ?W$B^dr@n;Jj4li&w-Duo5PqfEVLhNz+7fE)ZoVNf`#W)w-p6m2LJ_Z05tn zO2Ppj$Ly10mRk@VYKU#hMnj2?kR7BOR1eQ(&lmv@Qub7t8)NH`rbm;8-Ao_oXIJIK zGeS%+Oh`c6!=?AeF`&A=w3;{hUaT$p&-5|haCbE$ZYtQKB|I; z^7#kxExfPle%oYfKV2l{bn^$Q8`3_h*Jla|on^XrYHeCTZB}g%foK3+ozg!_Z;=-5 znH2;lDpQL4olIsbfRQrmHx1$}9&PL%+VMa(@sh!vg!#8eD;C!f!fU#FJjZ=PTpC%@E5yrHmLjLBDs(c_uSpqTFz&mkM+!p{GixCy;4HfS%ODF1|MGJy zk`>XejO)jQ5&;8islLa09436f@+l@+!a!Ydxc^XRnt!E)a+ZsAy6vxfYE$cV7WWb@ zr18IrxJl6AGleoQ8ofEji>+?acF8Xiks8(rNR#2@=9J|tVm%!93zQ$wq-;N)q2YfI zF&BwW6C*Hy$(d8OMcDM8BdJ*xC_MTf1(z1r3Z~#hs~Pr%`&eFyhAuw1?P?3{^JDm0 ztrX(g5s{Am8yRI4CHDtpzyp<`)U0KSIaK@~OzdsQ+ELma{N zRtxkrs1(Y_l`&>88nCd^?VnP5sa>G0CXZ^2W1{%v`9SmvA~G=L_TY3@FIHVqs>0f{ zAtU4s>oBytv1=YB8|AukqiH1h~@Vso>ebkba=NiU!XkJHdWQxT%4lH zt7<-bwlEVr1Lz934rN35%J3r9M3?x*J6$H$uTNES1Ea=$J)g^diFI9+*IBX{*rF!i0OXwUA29^H_RP`xE0C>1s)#B=+_}QO z{nC3=P%KVip2W8GZ9_{zHx=cNteB@ndNq%n2BQ;`br1+G2@0oVQ z03(7p%Mx2#RHJJMf+4RfCMZdN)AoIyW1@Nvuz`ZV{|{O<7ZUn$JTPSq8nHtPn8!v3uNME0s z1ATN7`j=I=B&cE*L3g)3DM>n-OihWHL5#&ulbwoc`AFs*^|K~%QU-&j6w|dmfbj#i zIy$Z^;PgOxC9Gy>FQAex=+PJ{mhkkN8eaP#Lrw*QzCosRO1lO_Y4K++EX>^ucDJC{ zb{@N>+fhDm6R8Ev)68}FU#86fM~^UtJC_ggpjT_!XVPNfWl8Q`qh$R(26QWn1yXWn z)41n=$R^8jmYjdr+`<`pba(Y_!0d}|&r0;VSm#M@nf`l|GkOtT9Wv*Czm=%w%y(LwJ<|0OeVs`g_0*{HcXfnixp zLR+$1$A(dce%!$WYY`9r)`PP8`};>>oOI({DsBoQ(^?^Nvr-BR&J)-ikl$<~{FDc~ z`DX3xq;RyL^BX1+MBzIbOpBEVd=$OW{FbHIO7PG@cNga7nVs$=Prc4u_!;Sy$qQR8 zYp9}Q>$zvfhG$EcHEP}TV&TZ{LD-+RPJ0al^p$$+)W)vgewT2?k?C7K$r_~y`gkcY7r@LxIq{@n#*9d948@e~!)P$(Qi+J7E7F?d9k z!LktY4Y@ZkbtO6ljF#N}9s85Ck`Qj+FQJkaba8Y;vc2JQdqXLt2Aps0AU|aHo@XFE zAsEjU4gJVxPazmla3dkw&tOS%c}_F8)N0*>3N>8h<&)tQq+J@}^OP?=n&BtGvI1Y5 zYCqb4Ol?>MH?x&wgiM)#!plCU*1T`2|7H;BT<&W6n2sYw2x=4$3H}7eMo5$mN9s5D zFd`=`oqH2QJl=&d0+bVrjUS`=k;by~t~bBDBN?7mLtYU;xDd)uXnx)Cp# z3@pse zh4+=1q{Iwb8_?PvVb^FYU-8A0b46(PPQ)NK89%HQM% z_|DId7z<_%{d6Q8l#G&RGtd$kZu}#V-=W^V%2f|CIk9}e>q(8=!Z;prN%F^J19^P- zbYfKWCf{qdq?rfi6a7v@qC0skkB@+L!2ysT~a2Tgpt;&f$*Gi8i6{8@W4X zwP7VE!4qm8H$<7M8iM3t*i?gqHBmPKwx7YA`vEV|gGR-secw7krC?Wehw0f@-^8XC zUC>7D2VjG9rK!oVe<{-~qvK)EKaECb=wdBFTX3(}*{`!<^d6JL^zOWz*I4Qyjw?4(|pp`sr2l6Heiy zRT%#(%tS1{0ndLkj9~vbmJ952yD^oPpQ>l8GJuxrngg%$U(yKPF~&Cgiv{DTEvls)*x8rn{YN5HMF zc6XeN&|V4^hr^%^25;+(!oQHI6SZ@f0!WnLY(9HfEd;^#q1T@u+&Y*xuqNysn6AoZ zQ7QA82+fMV(b@B9PuRnj@7pWV&%d(Ns9_W3>l5uM#OyV3)Y?|fL0{+oeNPh97=A{U>s3Dm~0bt_fI<DzrE~mMto1f zw8Le3LBI;jeF%7P<-TR9pbD~VFhFj!wXohdLLI+z)^`tuxwumq@RSI&f~yP1>Z7ef z3PEPQ8dCA8LNvNWW|CoPmd8!U7@YBsn9$a0%s=}Y>EHh`TaejwxJdpha)Lo-bLrW^ zUL4SuT+nSo9}r7P-HAa}60(k{?SF?aTUZrOItpL*%xf>U8GJAt77@zT2!!osmvZy+ zq=c292;9gdDZ(?lQ-YJzA2RRPT*b|Dag|@_?{GBAZ|8>v$rQAn(7hn?7%whQfNNMJ zBP{egI+J0NVbr+!M)(-1&?6XtanJd==iKhOj*fd%y;jC z(vgE%*KT)V$Ox;GJ=i!S%ZnV7w;hxOnM>h*zi!LtKnM^sHyeAj0_1*TXN`{D!uK&b zUCZaF!&eQo(Xrv`GFk5bt?bk8>3wut+G4kXp*Kp;ehQ z-JDi&+c&_kPp&mc%Vd3o9xY8*DmbA;rEgKhOL_=l^)Vyo0pIj$?FP5b8A!DZJ<7WJ z@qJIo9-PTpkNQ%(&>(7C3)pZ-VM)Y3`NqnLlZVa<&N7^=m2>e>SvH7D%OX1x0+Vc5 zXG?wP?KN`6mjo6}@iUKU1^n9(hZsz-QPdneXt^J&eWj}Cj$gqtDGTz76e@>}jRok) zgA3&*>tRVrfQSQQp+f=pEeCcCsK{+WVHXbKZpc7S$FXd~Gl#1=Bdl$qfkW(lFzm+F zj-Zc|NbPs07fV%Y1>mPBuAgsyvZ!ONxh-2 zkB`q)5qTUxhu3`IQ05i5EABcKc?s-rx_b^F4mn|m?2EY!xZ`md7?~ZH^YunG7*Tb? zvBR%-Iwz=q#>U2!p4=uk51r$OF6DJXMBTuSf?d}Z)%rZSRXbL0Li6mO33}7CheqF- z1~b3509S}7ma6;R7E5?fZBRdoMd#n{{|fEAs3!YoxFqe0ngpK(GuwQ0Z$wNo9x=L| zzcy%CsG&b@N4I}0Ntv%u9|^M+O{vE5h8IU*Eg>^7#uwu zEN}QPu0YX|#G5hu8wu6=TrOtpc`D&ZA}gibXu$Ilr+n@@#qh{0{C0G208^`x! zzI)CzTW@&djKFkg^2XMapz3wv@5x5+y$n5`;;CZHyu3T(kBjtCAEG@3@%L4SV~7`H zz_l<5uA1%WK8gB-G8gjIJ$POX{E6+|5L@)rKXluPzWCXBhE#!mrmF}_ISc-)9!<0R zsFT)^O64%ly8>xz(&EHULP^6W>$%!DugCJIq!!FUor=!A18;m`l%c8UH^GUiu#ue& zLG62E$dGsX9wI&DFDipK0!m%(1@sdt;Fn6sl3=#&pPXem&NNK7DJS{TL3v3aO+b$i zL-INMeA?FJPlEb<)@JS$`1Pl(5d`hY0MA5Ko7v$=wujdpwYeWd)!fO0l%@Np<9Cfm zTLb+cb%>i3C|z-r6pA&}Xlfq=AiX_D=Bb0g(tt~hl8v`Y3A%U5!}GM zqMH2AQ#iKEe%*$3MiIqdwz zJ{GctT=bH}5;|pZ#M&J}dJUQS_W_o7 z6=ylsu_~nEk%PWuzcSlX>%dMbH%+E3pUsXs8j2Rp#?^@*wCe+mmmO;rlCY)esG5{0 zny9VKnXWVrb&>g&9}*6T2LXDOmLGdbeZi+EDJ}>tm4epD9F}Y{6n2xzyQBWEv%(UW z7ZeW7?~ymUFmDdcLVms*z#?6L$u+mv-F-ruCRsZzbu)&0qlq)ECsVU2%aa^6qh`sI z96jUrHR)Gj++V10p}$ObOwzC{W|Ab2v^QRVEfx3DeWd%w`KKmw1vK82L%g}TiA_Uv zWXk_Q&Tvdr6e+iqh$@i#Ivswn^$LqDUHC&{2ldVW(7J2JZIi1J2@piF%ub@Na{z~F zb|f;3ytk9T#2d_fHW{(#?%5=rDRl~b@Tz!jH5fS^RUV0#&hd90GohvLLw6ji^4BISeh&-3u0WqOF30dkfYq0S z+Q=0mazmEC>Xx#zi^ecT^VY!%J30Z&5YBtN{C;S0WAbJz?k3iK%mp0*XE>VHb zl`K0YIB83}AVsrHG-HEAoA7Ahs#QiT7F%zbKJ5$zInd~@9{tuX|9Dt(;z-nNUVT4j^U0IsX{ot{gYHMoA`Gd zzVS3hQrvEq0LsFD50j{Qcdk(wdEadlIhbowk{=R9BL^VqAX|*cem9U2CZbpn{GR+d z)YPZpoO)yhlKEfTuCs2Oc8wL^mwN4#0Aa%ndl*>0LX~1zodoC!E^#0E zHD~G>zhNdrHU|~%M#K_K-*3z#N{dC@io=<|Y$5%|j$g-XB|*+G33vDD<-TQ9!ST9Z#DRHb*=*ad| z4D4yyC(0zYGRWdF0VzG^NuY^X5Zo*ZzrJ@ z|M@X(=Sbht>hGRpbHVMh&Y|laK`oJR3)5Jxuf!uO81TqmF0(&oM(Gass)tv;DX1}dx6@d>S1VWordrEW|GJa~`m14K&S@fV zGu#<;;<#(2RP6@&mW3EhL8 zy@8GjLj!EF1@yqQ_)F7x4CAc7HnJe{yT_6JPPx~-sBLZ-mP2;7wv&o*vh#e^n@dhji*aql|xS!GJ@2WiRn1TJPe{RG5_4!?Gf6BXw-LUE{?CI#)E0-0IR%TH69-t^QW=x4d@;X6 zdn8b?^J96&mmy_e;_|mxmUp1Iwy&5;M8|Q*8aW{D(iSf-TnCT?=Q1_khNB zSe$ug4?+%2EPG2UjBrtk|ICIYd4I>Wv>z}VPLu<)Q6|^20OG^#LPDVynu0b3f>`!k z1eaM^EIoqeOBNh>(#uhq?>;a^ z@&MuN7;v%NtIZBSI_0rw+@QQxK_-mTEsTdE)U?({j(^k#(9VEn@u*(j)+e?rkitb) zA)LRlkZm(cStI1=U0}I?|0GIaHCCe0RMel};+awIrLFdgx&Cef@a1c%8>Ed~euP62 zS4vc^xc)^R-2yvvSC)S8FrYbTlLJh|?@RclQgl5b)x$OVNqG(hW1GY^5Vm?UjSeC+ z`h@}-=|l7~u2xh?X&zVbC=7-#7QzjsF^Y4_5_|ykS(~5r1PuMa<7=n4bL(MIjpGk-e@a3Aguk#wXSHp(~ZaELb_&l*X~}!FXRvnb2AknfX-Bs>IIV3zXQR| zVe_An43mbwag9P`HO34UA=tBJxyei9P;KPe5?)M0ejdUtkma3RZ;K(Hy*Cl#>S7`6 zU$bme{5B-?Le69yB>Ck~Oxvk`=o>8-Ao2O&1+ZmCAA`+6&xQKtvR0Qr&*Cq#Ls=J3 z4ued&DqXY38cN!iv#LDHWC@IfLz@E!_mOg}j}LWB<%IVhpW)9`^HkfTq5Jlx@sTV=_*0}uY!UAcoI4>r^&a|c6UnD-;lq?b6;46+_^K!oa=;Nv&n1rheW zD134PzGe+V_YBXgTpb<8LF=%MKBEfg&lmA&B4OPsmZo7P2a1lBgENqtn7KHhlH0?P zZXKK|VRuSWSof`}k7@r5-u&K5RyZhHqswr2I|uQIz5-?C3lH~Q$lR|QslsS|(g*K! z`N(zv)dchTm$_b-mNgY%w3_LdlPn#Hr&cX$==a_1`vvktMklG)i#A+3?r3HoK%dsn zC3tvrQYZk{fJPTRnY#9(sHjh6ewsApNUXm$#ojL}z&!3<9u8szAn3Pfj>(sX1IJ z*lR)*-mB;>vS%MCPv8udQ^H4CGoI&0xuJ+Kx&)gWI2r@1@(8Swr+`2-dw8F_A{?}e z{rS8_>p<&lp1(9JaPMjN0t{vf4~CD6P)ciNa+iRujX`$I&YSBOZJOvBU{I!cD1_kNUlUf!XX++%F?5ZB zm6E>D(AV$lwg5tn4l?^3(~_}5#m6RU)42XX!J4no)TX+V*dHH{sF=2GD$DD9JtdRf z!@0jxTgwC?Ykjj))>8MS`8Y%jO6##tW#@@X+j_-`9K-6rb}S6LCB7AU}}!%ALBT3}^RGJW5uB9zLEV zN=0=4zZrd7RmP>!RWEQ8d9Fu-$d%vly`^CvuWYe-xDVM!24u@UQG*mm)z{#!)J>Bw zVBJ}Z-9g8`1l5XbGGBZqIP6AFlWB&gb{S)YlNk8|LS9J2NV!c9!s~M4Q*uVcl~Cf? z?9v|qcvc__Doq$sU+1CiR^&DsREIWtZ6W_EW1MN;)W7Shkh59|z70GjO7u(F^j!-E z$1<2$iJGi5+j7M$WOm((vgGHY?kQVKS~#6Bg4K0xTRPb|N8VDi5}?w$emGBbKXPZN z#d7RWv+na|B9#LEYp#a^8jRh^exTvaIRt*96$S9$sRuC@A`z9cJUA|^y`_fPh=itU z|N6kQ1g|VXru+h(;oZ&+ZZ?aDS0mlP92@g=0DLNSY{ueM`R&0M?m0bmAK(FqjZNnM zCFY)u8K>TU{ybz@4w}O$gK@PF9{sV%1rX;tg$|*TGX`;nIsJWa^)F)_}zpT%) zC1*+sPj{$b&?_|7UBrI|aNi^)MYw^TQNM|*K)U&e@$5;pHjiaLXpvuAHj}dyt zkxhq-vwHsn$uu)mc#&W;2IJ2b2=^pZ)6)*Du@f$|%QNZ zVrrrh#%`b3V3TAuEqov4axmU|39<7VNKp5E>`uDTi`GDOX&&RY z_0d0ZgtB3~c)&vACme`NgAa1r^F-D&5VYDVYqq6 z5wqjLWu{4kcY@l*%_l{mD~|wZs*JldFjYmSg@Yfbk0vl?w^=E>sk!)Q$nbQriHz%# zRsn|qs^7##pkQxC`XP*RM=Ze2>OxaYB97EX5@^!bZgr}}Y#pD1OH?+%x+x;OVr8{# zszs4#;T=Wz;rsw{w;8nY6hqP}BQ_dF%_DAf@Ng5MG$8R|CNa1J^C8HFd>n%|A-iWhz{LhTB(SpWTE*~hCuS9!bc&SG4oXVqw2d* zi{R8Gi_v692}DTnNodnVBhaOYg;i*fMRf4&5IP~N={(@sk;b&*S4X$|M{}z1jv?;b zMse-Bo{T4pf%IcBaSuTH=`sdDL{LF7F1<@7v`J`Ym|vgBXgQ?=%CD~`An05q57fLW zLFfh>4j)zQ?Q+aMu{10`d`2w-2GKCA+{6x=EHWOv)@$!_6EXrDf4|1zW86ISHrGX3 zxz10*g$UQOVGG$w9_AI$;h*8I+F1$E#ZAm0POx*I*J23u3Mj<9uS5y>EU>~WM1p{V zy-$jg{NefATlO4$MJUKhgK)_uGL~c=Jd9rZS_G7R2?<7ks+~?aHR1wq4(*Jj2kYLR z!lKUZHq;zCMBu`{OK*v+0?>x5Bex_O33dhzHTSaL_C`B2q}g ziY3~tMTOTkZ6pxj)8qi%vXP>P1Wy19E{nYCRfLDIIu0qkVfi6P-X^0CxWp#7v}4d# z_EpUjrr{4NmZFOJaFRMrkP^75-=4$SJuyG)@D}v=Q zmAP5ZFY|vQ(w=#E{0z7D+tArbgDk=X#k(wLhJOFdSEF@8J#7cJl2+}!=CD=Tbm2zB z;4i8$-h%xmK9^pY;K5VoiFyg~rXH1jus&`I7YYpn>xJYJ2*o~P+L0PV(}_yAKJ0el z6A$#*^22m<)&KgZjzvZPQ7=4gMw<|Fl7Dn*dkEEhS^nJm7k$q?f*#R8eTbA{3bavg z0ky#dVVI-qo!|~E)4B=dMtqML5n*e+N{I%J)lgIno-b}^pCPZ9@&kC6hvvpIK>V2>wQ3GCy4($|8Xy=w_#4_H1a-KCTV}CF(b6fAJ0S?6A(GL9hSJ zj|j1`pBEO&CYNz5=c_c0z*L2sNn2vVf|=cp`TA(!WN)W_)QhKr8sBM=UWJ^WZPP(8 zU8RMAzbM)YW^Kh2GUwDdd^-8=>qW00Ul*Y$oU4%0amNX*^hub& zQOF`A`5t!lPH2SdwYHM*VENqvMY1v*x6yGXZQ`B){;R=ILtJ6-DQWzLd?BxL){-S2?R-Ml`RGerw?EMU0z%j=Tn3 zW$XDXN@1m*nQ_F$I%X$p4MizMl z@`W)6f3vGzvtJ4QJ>)i-AGpb2*4BQki=f%1H`a{X^mO;%#AXp8eKs>j!|k`Nm34n~ z^Wv|*JN4C979QDlYJA6}fvOw=HRyfcbi5NjJ#Fc!p?*~K-`f2fHrp{+$h8UlC^CG0 zqrAVA*`-`cxc7JZFX^mNRW64#a28#y&3R2uQRJsqE1_N|tvK1o z{`cA-o2ws>kMEyuYLm+~!#dBvJe0;`m=UoaKc9}LB%lDT0m=bUVS$lI*Z003m<{=& zU9XFJPLKQ2wJl^w+Lk`(O_;_N;JwO{h`RG!k=N@1eDyQX;T!Kg28uQ7hE4^$<2`>+ ze&SdU276lP#bZ$5OJ4)c46+htoaQG`cXVko9}Az>%aY0{h-Su5f-)e1Mc&|`$LTDf zQ2-R!g0OR)RL7S0_%|TOlHT8MU<8Zq%v#MzxEgWeZl55ehRoR)ZfC1@yW!(n;R;QE zZ6hstfd*#kKwG&>Mc;LXMk-D+^^soMD)oJ)D|jBCfmOR^yPv0RIm7W6A7zf%X}?eF z2I}>usBzR!15_88zErk>JxI-wTHs+k4_Y?Q@1E-fqKJrHjIWL9P#1 zchgchxQJrE1|pBBOF6#TovZkYM&r28{sl=Fl-?{RjA zyYb-j{pjS~rBv5sPRPjzcak_;Cskhz$!+$pm7{~ZtqSe;ZI6u%;O%Zv0o`F#sW34D>UNq#a`w#sU8L3FkE@OEz&LiCNZ zJ$8c|9at?l0{I!jW}?=!s{5 z>$ARzYf6uI?|T;)%Z)^|9eA|ZDd^9Caz32N9%}-$jna+58s(#Ky~HKvzCdzAbL1N# zoEw}39*=Ez!La8u&b%u8n$3&Gi3feJw?}>TR+}ohi-XDsz=W^}f4XzpF1N|T4>-jY zqg2uDD@4Xt@On&!)F@3s+uHk4OgVpFzs>ZY^Nd65b56Ost3k}#Pr*bR?{hBQ>IH8P zSBt{Lh8B^(3zchh-v*liAIqpNf;2c@P+^E6b}e0DV6Hs7TDT1EQ7};AI~r+MXJ-`# zU~Dc+Zp$J+fQ_bS;%Ve9>cs;5?{1%Wf8w6zAMS%Zxe5mgcf8-IA3a;aks>^$!WZ+K zR%Ckh2Zow1BBN3i_Y%(PN%W2T7*I$fg6NWFj_(UkukICJ*eTXzUk2V)zKfOeM4h~D zQQ}xhr!z>)#-G88!^@)bfg}U5^|rj~cG< zvXq#;RG@$$84!;3g_aZJqbL)$={eclr&?%22fw@<&g>N7C6FW_|9lsAQw4^_mIp zD`J~gxx>$WoYGzar_5UHH1xRlUbX_whR6RHb&B{;HSrAJ-qKmljOwj?1A03 zI`~Dl(E_8-cY-%d)b^-V9O8KL0V5ALasnL(1U0QNG3(XR3b8R;h6Rp!{l#?l?Qxbb zX*MGldQC#5MFJf^@h&?vL2{0BRBz;%yB&;pG?g1_g7ORn6xi0nVx5y4!{Q%Xs|E5w z-@L*Wn@81}upQ_U-WaaedmdIVJAg*RCH_H4anFLd%zK-7r+IcS9o>-`Xn&w9O>$qq z4&Tnu3@4ukSe|%&Z>+>B6Hf zb=ks=dxmOQr*4VOOt}dxp#8%SgankOhQ!W?s4G}uJv*y@P2(349V3QcQ3)zcS3I`T1JmFPHrG7|~{Me9u;%!MkplWv{PA~JW z#Jtb1a%q>AuzfQwURTn;616{r&mEo+0cE9BaZAgH;*_%nr#Jubk* z2D98N>fi~vSkION1X!HGWN^gLlI3NAGGlnRb9VErkUN!Y9x3YpYZ2??Kt;gNWB$_ zFSncYtzx70qBt(E8jO%Qx~#wRT+4ZxL2*lGcWEEV!ybVYsJpV%Tt?cjJ{4I8w`T4{ z(hq=P)T`0JNCC%Lzn`bK5(P^{PPx*5Dss0@*h$3Qr*h2yOu1 z{_>v>;N{f;F_Jis22g4lt7FgK?Ra8-TZTvT*lsvov-5kL7G;2hYXAO2R_&A4@<0o>|4s*}~J@ z(v5`1#llL%QiqY1iGziSjg1zbS=7_R#@US~>)S0PbC%96Bq=|^;eprvxS7Ihb0M(1Rom<+y9r6Re1}kD9+Bv#>DmCSxIQVmLO5E^|B=SPcAKq zy^E=bjk~2OiI=6DyREYm2^SMP6FV)ukkEg(%gM!=f(pRn;DKkBvUPL!AmL#7fAr!c ztlV5#u6L01KzU~ZZp48%?QH>qsHWA_ah2oktA(c@S^qMTxui@Xc|&O;TUOPaVuIn zNH}V6fHo0A8q;uC%oZ)EHTVrU+R9)Qq743_D3UT!pnC93IcGEvZJBf}2-9xk6(S#7 zPBxcK%=Y&!csN9uAHk~HGbkA|$4$mDR|ZB_eh4-Jc9Ecq7Qppp5hFN z@8kkJJcYqLV)+~7HoI5qbMSEVh>ee$mWW8X+#wC9ZFoy$Nch_s1PjDDoEvhP*pYHk zh{SL(&^4C|pV#zM!cRcxOTs5yQ)^W~$jANe;bMHf@a@zKyNHz?XkZ{Hy>oZ*wC6gY z&obufkuR)n03~ip-hO2KoO|P548kW)2pkb}01Py)+YiFTV`q6>! zD9;JWwOwyFhsRl?!H%?6C%V3>g4DPcCQ`!*OrCM75YPNeiWKx!rZO|O8+)TTRkWFr zld2BkI4U-#YWePU0~J1o1lwWsUw>t^Aa>wG={F2#T2)as5TGX82b_8>XBIEGCW+3> z@yNvJBBwSm)?eUSg?UnUthaEswdhGr>NdNuQPLYH!`F~mw*KAhRsIw9{MMTlbw%wA z%-nPB9^-Ih8S@qn0KMogIV_OhJMfJa7_*8`3G` z@XvOTW1@9yCzIqjGP75V7&V!}=y%F$?c}lR6KA$})TlH?{Y*Fa)7xTi682I4ea!3Q zAugf9@zb9ueh7_+E{a@Pi=0HtgL80a4y3!E-=~z`0D{Xi;;H8mIcpw|h2iEj#Oo+rm-| z<47)CZdr>B^E=&K?8tGA=1DU#Yp>n(aQUhWXi>u|Kr)NZ z{@_M18RuYC%u2s6VHxQooc>ZYUnG~>hT%awykoNNYVbUTe;+cdX(j^eidbr}CrGrv zH$fy5Q9aQ`DkJKX-FOl3`u!kigM%bP{6<_WnU1XMyP1W`j3>t?Z%r`>Fup6;j>!r*nsl295!`*u=Tff>jRX#(Ti~F(WI+mk}MKAM0wY&J$9N;U5v5eLWXg_cBWG+KtP>4&2kV`b7sW zXdHWUG&CXeKL|m<6UV|ApNy2B1ZG}e*sISme^f*V)k16-9`6ar{M=Nby#ROoHE!dA z>+YJ>UrwRU4paj*TWHW_0$WT0A@~m11ESq%mI65aL?9n7Kko@Fo2SKFe9bxi*lm+I zomkm@l3C&AJ(o|83iyZdZ0FWhTZQ@#0qG1z>g7bz;k7Uzt^bM)W42c%tEY+7_ldP& z(c$1%=AC}Gv@?ok{7=h&MAjFr0`*bp2CsC)|0ZQb>~e;t-3v1(vbHS9j6$Hxf{0gs zGRZuO65k#JpAzV;H?^%HW8CQ&WU|O)xRqs$m4>cxHPswoDe z{QFa2;MidSqV-zpKam(Aw4X=Nc zVE)3{C*)({*6sgP&)G<_o}VCzfnTNB5T04x*3XjU%YOJhQBmmGhVPpL@W6JP8tg0UkCfaV|D4F$oDCPBB&>1n z9wCzdH3dcJYas^ft-ZYWuuU3alSp`E)+{s06;&ga}Pb6 zi?J&agtn!_Of@;!k{DIUi-~f8psj&N@p{Da9*$ubE34~i$<73BDVNm2 z@5QsHg(rV+pWi{F-K62S#@J%m4B=;&(zk@VQ)c7~B9VYGM{0%;{9W9JkgsRNDDctr_K_~Gf-6~H0;VKVdNt_$EaNIGSE(XoQf3pAs<`0biySCmv&)vg|dX2ccpddhffZ3Bodr*!&wi(?ON_4jii}i zdqaB{++y-1kUPJ?AL6-ScSh?|3BaDYoG=bb>45)q?R3$GV-Ak%(%eNq1X8jjsqbUM zF^AE2y>&s0nW8R2ltHV^pfVIGgQq?D7@+xV`GK%6F<34#e0D?Ukv5)dKan zymtF-)pj}U`ifE7A#H}O8f&I*y4Hm8ieXK`I6+lTmL2&81I7y|HB5y@eob7UsjnG1 z{?!&L`iY^KUpTRh_5BbT1e{7)RW%CZkI$nl0|WxTG**HNl>QP_mhg(owE9DS`HnpE zHwj(3yBwtRH|62?gqDO0B)Q1=!y0JPE(-9JYU~FZ433 z1(hF4@%Mf|;lU-}owkEO3?prVW$AXyT|Kpt-6ZA6PJ#&RgLCw3AYB}`9ny{Gx&%OpW*#JH_;vf zQ*jv@pRw;n!seAPx}jlpk}z z>xlw@g zu+F-47dKxs5vjaZ%idB;IV`yzq6P$^+5+vy6SJpG_@GOEl%-~U@!9^6tDLvOKluv5 z%IK1*UWDqNrVLQ}Ua*^d|4y@OVUpE%E+qKtON!D`B=5`qLT0$nmfCH*ynxPfGy zJA%?KqAi=i5t~YW{E>sh7E zS+HYDq&VlIKA{SG?9E*sYY#( z?8m*g;Rp2aP{wkLF=z;i+?EΜP;Ylfg!-0$oBWO0wJ!oPx7E2~8oUNu1Dd;f7-W zppUc#IkHG4D{7%fB#2=i*=Wn0fys)Hzp@6Ihj3Cl;S+Om0C4YGL*!!cbhv$p+N(QY z+tF{~k{Tg=ut|k+_2dxESgA4-W&~v7Fqb^>#sI@${s@&lND7+D<{(jo#!nFL&{0^a za2=%PhEfC%##wBJ7{jHdr?HHsui&f z%dhCn@Wu1fa?yUzE90CzYeZO}#yje~f+;xzb(1OPF5cAF(ejkvfiJmSFq+`7yW$@Z zO`XkbcvNtN_#&8d=})v&9Nm7!37lx`K7KC!<)JWs_~UIBlh+z2WN2;O7r#zvjeJ9Q zXB+)qtm@a{nQ=NST-4o&XHzjPLEm*fWw^Zh+B;x7vpoCY*%}X;fxMDwO)ACmD+A^R zD6aGem2mx-*QVPA5q$xJ*?-~c0S)bsxl4$=qird~;?(SFe*T!o6~^N10S14RUpVrA z=E({W7_6`^ck~uo=s72rTiIU8u_$OBX#4&3^D1-cO1q$LQ-Lr1oHZei+FB@Y^L8(S z(=8v4XNgJ6O|!TcgUmX2#?P48rIuP3C}}m2X(6P#*72q>r}jf7+ z*#3VzM-v-3){J|6Fm!NYR@RK7EKu5%4@e9EOJ=zh5pAcKWkF&&%x=-t-E1-G&)m${ zlYrkj2fiMvbzhk?{{&)NX5e2tS16! zs56>)=_C}PhKJkjR)oLsyX)b1pPyGT#l)vp!)ZfX3THJ>+pGJmz84K?%DHP(>tL<| z&35WrNxZ{Z^XoWeNoYz7eiL~@sD=NxTL2=1QhXVTD1X%9WF8e-`Yr*RWx)y!5D^P^ zc-531#3Aekp}da^HGe4GEDLr}5pN)Uk$XTm`p7F*jnuOp#^<|y<-UE! z3inq^h*v#Mzs%+)1n;wqWwyK?SHJZesEc8=YyWstX~t^BcIQw3(L`v0I3#%GqSeGXWiY%4Eax zTFVH%K-4@KyOc_f%s>vYQc zB4aFG)L(03Jg`l7rc2&gq5ytz_%A;{b`%kE2NbYCOPJFgilWvA?W%FILRlNfSQ43* zG7`%iRcS;pk=tB)(dd{VQz3H9c~n#KXthLfdDBuGaLvM18-=$h218_Na17$k*rKc8LH3K!H;pfgJu=mLQ&KLE|ih@y|+^16OGN zFtxEob51jym06HD0b)A!J5z+8Q*`%Qe@|;sb5>{)r%9jAQJ;X}*B;};X|`OtU0#ag zT*0^>FWKC=6zmWkfEhPIOucm{s(J}OFsTrt2Ffvgvb|G6i5G)wRlLR zVE&~h>;+Dc2nu5V*O{LqwNGR%LaxPgWl-QILE={GR3l@xAGl3kQwxkzV>mR=f^}hD zE#jE0R-nLDzM_c}YS&YIkYOIbLo0_ipoGv)jmsEySwpR`>a{!sSRWP=7I` z+R2R7Wk*}W#S8(~f+~zQ8tX;xCmf_Ob!>5$dtt<(?#@@qn_w+#Y5dnTm{SI(bhCZ@ zP>?MlaIp|W24IVWS#zfSl4XsrcX^u!M{E_j>@q#S{Z_({>9(3-91}9d$Blrw9f~wx zeS%C}r*PiIiy`aJv!bI)YD8=Dml32+HACN%6an*Ahk4CGW6?CXrD+d3?uzZ*tR7vB z_3s={*fPYsrsndL*xO|mw7%YGGV^j{_xO?UU0yNs6L1k760r#f(J&GY#3*Q*l?})- zntCD0{M4XGp~mYv`biYS7aj3EA3}7;p5wz7qdBM2ln^LVQuA>%*D7V2t1rV`?5Owq zkO6C((sTN;8Y4C+g#~ZI**Qg;1V)d3eIIZ6{C#tNFVFoRnd-qrkyQC5FQcp6k2Q zZ;Bvqlh|>OmDtx%lwnOC?g_yK|Ie1HYoK7YF5U zJ)_@A6>98qc;v-ME33`NM;a`VQ93b@^%x~x0T#F>a^aqMPTzub6^54pcU)hSp_TW9 zb0{KEuia?a^jO%Z=5P7ZLA? zfG>H5z5=cl9&FF5m||?&O_60Cy08o77qtiTK_NZXv6CPt>zRa-lW%6`KvhnrlmuKj zrE-#kK5>OqN}qX?*(n&g@{$BhxN5%zU<|dMx*cRY)sqmzW}x{zNH8C>Zlb#@flEn| z>`LVKpocQm`2D7mHy>|kjG9Ffj{pZX`nHo@ae>hVBCP;2bRizfv~g#4dx4kF(&NmU z#D6}5j&|}r>ut0`YX@n*8NA(&yD=;nijo+NadjNoNK!NP7Y8Ke(1z=Tgfsa8D8F=9 z4oK2rh$tMl3Fv=FwWG6JeYaOM6!)mqwLgk>I&9KIWcWtLm@0ZA%~Xo0!Kn~=6T;!0j(H*SURTsiM2 z>W@X(XPwd%GM^O+kjIS1xv4Jz+-<=~1vR@FE#(l1BFiI|jqrF&rH*8?0C48UyJO0< z=Prp0+wllyXo)QQu8Ms_m7(;Wmg}gacF;AD{LrV2snei=KRFQx% z&FwXZ+6Kct%9#dx-hUK1STU)=cqs}4u-t+b1M{(`_Kvc1`&gqBHU9sAa8c8fIra)r zEJ04C-0|ibKdfJIb1TEJO<%ydpU>dkyp-xCl17L@wl1ONK3^wO)5qROIT1FAo7|;h zL}OrAYo5obApZ2=tg1V3_4Tn_0`qU7j7G0!@8QgA`3S1lse8rWy(eYlGNoZD%#I9qhNm(Qon+V{Mn0_vHuLn%q$p4^+aB2dLs{abfjeYBF;*qnR`^o37 z+D)bB$uOG41QQbOeGB$}Up${U?o+dnz$ufy{_)^Zb}Md_%#4}5HhGv%ojZXr<^MWp z<>h1xt{B)PTJ5ijf3o`tncr`hKJ6n!=A#2R#^1TO?9K%AeeI#2L^{hb81rYb#bRp& zF_H#7O#2X_#&cF72O;I41jnK<5XdGG2Mst{h|72Q&dVMP1+?t1zKlO#4%c@#2ryKc z>TD4YvnRTh20X~I=UbVTx*aY^vkEp<+}Uzcq;s=o|2+AsX8PabNC}zpCyU)oax>dfjN6Dc zXjP%N^9|;_t3)u1#C3@LDB@0ywKhOY<}A+fa#>o7ztejw@~T6}e!u*X=reC_f(3HMeKBcwI7m`;G`7OnTcc{kl(^jsf87;>WwK z)*JTBLLQY%hkhSW=`rM>m{az6^bo~n7%$7 zBbZID92ht^xYnEIZKB}BRwqnEp&%Cra~^xJuYuZTNYn&Au$f{lpr6nAA{Gzn5iIWL zGri;ihx1BW>!lnmjHX;Y1_mTHf<|VMgB|bHv^k+x>OX5+b2Y*-%&6bjm$fkF!Rtkv zW?h}Gz$N$8$Xyh}IZrSdA4z1h+-SHfdtoT=$~5p|r3s^^lft9W;Dm4&T}NA-cR?ub zL96@T+9HjIta~31K<)2wIm-FYWd#od{k_kZ zv1(lG0wp9thyVueFy;xWm&+W(fYB7lPgGfCCvO37H@S6UQXo#RUt1xd|80F%vwc^Z z%M4pgwitZ~#>G$~K9MkqL^9G7!e7MwAoHu5i^5nMudA*KZ7BJjZQ51l>%0{jH8e0y zMA38`hw%Gj@i+yHFP4P)28{3ZVKaIJqE)e5JhnUuM-)$OP25mBXu1`Q%2KjkUvw~N z?`DS_CNtE;1|X#lf0xbAg0Nf6f-obpoN1CN4Om4RhEk?b@h(-~iVhJmN5$GrH=|1` z)zJQG?$)EM6wKVDjHN+?3-ID5_?cFBl6HOkY|EY_H(I{WFs%Te*^KrBUMvKn{LMjI z0NETBV-KH?lM`0#Rn%LwRCsDaksxE%N>3xKbLOGa7obf2uxYFQ@pTSAN--fV##&}A z^4wQNN=5_YN)K9`1E7;LfnN0~6xvpTef40)!Gy5rUG*@il7bhgzhFH=9N7Y63`K*;+AMu|#CgY-wG(2+2;Q+}mG5OF3smvF-;VLbAS@!uuq$?dy zukc8uFuH5`YRCYjaesgAQp6qYr4!;#<(KvOi|={=y)|BN*^?HKD@+mQXpC^`Iz zQUGJ2n?#WgyLyt47>Xs2U@=p%2TFqxQ)5Kw;cHdq+W8*zj3jBczfz0EZDMl_`-^q)^HEBx zHg&3amku#Vqf78Qq_a|&$ZL?zpjJ6#{#}w`u16#I_6Nz2>0xdm+!nxMw1ZmlLfEs? z+4x9weYDeLk!TF6lKR~C$bqb~VPSOT?(h2DJ3|Ni-S@eFOZ!o)5iaJC>i>@4SpiTM zLyRU*8)kC(^u3iYj>Zm%bRZ0;s~^Qri2Ah(avlalzTw{9cQ-)7e~ZC4KnnZi21Bn< zrzc&Ysk32U<8pTjOHmd{pF=ck-O%uD3kj=2Gn$609ri-^bXTbn)lx9oBqtK)o&2i< znhR%RqhuQ(DqIX}^`t;+6i#G80|f8}GJd8kV0#ek8IrmO2G6);FI~xSk%yR^|NJt4 z8>3|H-H-V5V4RF7eTUw~opI$wW2Y4nBjN{uKTQtdxf}~^$bHb1_t>}aq_?v?vnqUh zUnL1OEa*9x>ik9`Bo^zh51G+eE&nEU3za3cGvn<#FczxhCaM;LGV$9N19C{iLRzlK zSw_I2Yhgr^Cz2toj_dh_R00WKO7G9^CfRu_Qd1j3D*Nt~v7p$BUdh8QS;UOw1dWwq zLueUy;@cQ`z0gGwpEU7vORyy2ESa_ai$sW4C^zeA%l&>!#su%3LPg!cxXx<5#b={h zK}>8&RDvd--~^XXg;?;G0u5omwpkAIlfr#!QuE?EyYki2o+vwHs;=N~K*1sqz7Swb`+MK+G<^7avC3{t!!+ z26!dcYfC5pC}j6V(HnBs;UTCbPR`+BhBL;|i5(g5=m$w8F8k)JHVDkPfq1ajbu8BB z+N>3ia@Sho8!XR9&9u_p$0e&BW7=o2=?wHRQ4*|H8=d$#|7ri#wRLxm2FQ!L;$CX7mpXUq#%w!j zg}v{&3D^B*2D6T+Oirycd~uK{U`^T+gGWYmf<53Sy80m9w3Vsxlasv!t1?-t8d<7@ zS>UyaO~w%oKwb!0I-Crhm0~;fI4q`=y8d35ROCRRbcirJd}v-iK0mhoOQFLJsL`OJ zX2<$wT6^p~0FDD?T7w@G6Ift)B*zTcr7`z#K?LR2ju|t?>!t^B)UJY}esZ%MV@xFD zl#ApA9>`*iX0VmLZbM7|1hecwcE)bbKE&<-Wm!&*iL;Nr+Lw*2?j-WZ-e!zB%Suy2 zuKuUDzB>(bN|2F55c-~Nbw%pF=EAbvp{IePg!2|p2b2{GONfibj{KmZ7T1Ghkp~M= z?Y^YkA`WRIXL(k?ddM62ErEZ7R(sCO5`i0ye1wJ^IdM7%eA zQ-ihzqh}AaMuX!$wZT{^%{%Ds$PIKj7P_fXckMuS+}emk9{17QnzVw+=9PPSYs0*R zFZy9V0BEwn{G+EnXfslulUeaV0xt-SJ)rS;`O)}@%`l0ZilcFpVfIFu?=B@0%Rz40bIT!6vNl2J;BsSSk0^-6&!c(#K zpqI&M82A* zF<;o9t0sy4EK?|K)_j|(n?wXdHWA*ujEokWY5k)2T#bcJp7(9~xPGtSY(0Nonng3l zN3ECgAwlJ$vS&&~lf^ABGTJi~5`gTOqdc-Rw%MOhlYRn2_d3Ix_seZLu2cSNpoErk z1XORUbU{gd7 z1N#G;Y{Ux!gO;g`e;2@ktrLNQt^aQ|Md9XJ9~Ckmj|t^-_~0b6x4#0!sXn^@4+i6A zC1xi6&wkF}h6Y7|VCClK&S)|RrAg_*M*$Ah7`4Z%I&Es2n>?7}|4aO2@1jf7OYJWA z9g&$qETBT5Wj61P{h3LOHZpI{$Jdd+{k-po@hDhD6H$XLGHT=(<&A?74mOV!o&!9* zJ9chP#OKp5--4A{ORT;R9I9QhHB)J#*Ovu&iJ1evH- zyOn02bYBciXdCabIk#U{`7c1bslm)ywYJtylbE91VtTe{qTwdv87J>JC&y+{zu(hy zY(OnkrJ>*H@>#skU$s6F%7Q{g1yIlNxlWe>F5x>z$aTFg17w?|xr+wf^3`YZXPE}WRqH@O2tupe&0f2XaCL(&d z0kxYKh(?~<+o``$=OFQoFhu}^Vx({~*wdej%JiSQC3+lw80%Z~Kj3t)Ol2W2{X5IS zU^VI1N3DnoCRNlb6&{=Ht)7Ruh^CCdDN+Z@WBs*dYS@a#PqYz9yMIvUB5M<@{)~n( zOkAZ<651*?E=>1hZ;C5w7LXBd1SL!)j}Oz>xu#u^oK0?aSZ5wdJCE)zfRVW$H(Dn=zTQFxjo%f^GlO;0vQ7@(hm0CFFzo?@n~3~=ImRQgbAlOizY z6{S^4fYG+0#b-y4DB7OYO2)Q`_Th{~Jq!a=x$%Y@Mj`%z&2T41UON1qvh#C8tVH`R zbq{u!MpHsGywVyQP+?MWBkl~mQ#6T!TwHKj8Dq{i=|Ml2)Q%t?Tlz$gEiKds9igJN zc0HlEgDGCwR@;#0fo%M7$qf=fTxWJBa$E?VcSx*DaIcT+F#j?R0eRuOP%&blRa!$% zA*p>R;5Z-_6fQle6-QOhPvP8I`kaV}Rzw?$hDdsyK~7Qw94N>u+E9e*i<5{#z?{Pt zLVVUlg~uv|P^W-k3v)Ibi-XUpxghhJX0)>mX3bj1mMdDbAPj zQ#CkRf1yzTI8?d(p>S7KWftyy;`jpd=_V-W|EAQJWs2lC_zz%2@~nUYAq7PYzSRv} zRl3sy9n8=V{3nkp947(ty-F(lCmx%0#|EL^Rb*+jT`lObB#Sta3k^q1v|dMA>%TAv z9qP75##nl#3Za$a?-?$vQ~8R;5r4dV(r`#wH=Mi~uz~h!ZT)p0K4`KXzYJGinULW$ zRmC98f!LwEaHBuIjT8oQub9K4I7sCaOv&5gcm>NV4Qld4OrdO%JU{aMSm0Z^1hv*2 zYeGH(BW~Q@3L9ia!klZ{_Szu^1C{>P-w9OL6Btn2Wfj)frjBNYP7n{YP$=a<*_Qos z5vJw==ybSg%S9;=gMQSWK2ztIixqJ&5{y_4@y#qcdAhq4jZ?Op$->_8XeYp{p;k9; zKT``7oSqlL;o5nt&Aef(zLra_K2ea!^+gwfjcO^$F0r7}r?T;2q2myPakDil$xM2) zEGB_Xw?^~eqo1#2T1DQqv=zC2QnDl=kBdhE%;xAO#lm1_>Z9UpxahMO9D?Eg&uML) zd(-NQ!t8!N{8vZX|J0c)nef}8Vee6z{dRQ23NcuQ<7-6!^633L`_TGxN+Q6l9xo$X zGfW?v(~O2{^^{dXHK5J8ZgnNfmY&KP)_^J?vSTP!f_4B>2^Uk__e&E9l9y*nR5s)Q zWGIpzLXGxCIN&wsldn$o-mG=-d5YMw$rfL>XX7AB0yHxCg#9{eGp*U*(pgye%u{|`1r<%u>>B5 zGZ%DsB}O6_{9$KKE?;VnJb{ODxZec;yKQ&x&iG4$kHfpn49ubGobG9&o9N5W{+}pF zx-UlBmZ&-Y&c2?)bt>AIsr8~ZU+EC9AX&6jNE$(>koyQomd`KO?+5ovb`EqZ|GavI zFgy$ku7J1t31kP&29dli=)K}&!BLiWN6BFByaWSR(~|sG>AcEmaU}Mg9Hc;o9fN9r|Urjs#42Mvx8=ZI9KY2I=GvUNy-b8uYw$NJ%Kq z`@vSkwfLHzS4=Pc*0I)Qyu1#W98$&{*x=*lSD{+WaYJ3R;0e$u$XADjYXV}^D-7GI z(h`}lYn2qvJA74SuI2ic1OEYbb}l^^Sp>>{N-K%1kyxJO%Z{C?t?&x(_%qXPt4oor zn2qXkG78*3jtX*V1mc8{A6Vit>{O?8+hltf_-A_xIAcXZ{Lv$$Dv-a#i?Y&JF;8ip#M=p&fvgX=yStmU1o z;_mkqBG)P!OCmO`QM~`)#$5%QgS;plAiw-=f`M0Tx!n;d!?a9d;}we-AtzW$af|e` zC*V~)QznQ2|C4fOjbH*qBV)Qm8duuw`lRJnwohT85xZ3n#aR(vP+Z26ZI*d7*ZzzV z1h?q9w0?8&i7lTd^r|pz4MPNUotfZ*Y&kA|WDG}8*X*Dr&e$wts>16lL(%a>3 z(~OBF02ReATcJ*M2Xt(xzd#-tY?yWxiOAlkxf*M_H49BWq{>~kMfi7f(O-@wB!1=u z!@qwI!4SF2pV?{n&etqj3kq4`k~)%%*nT5X70iKMD}n%C#XP9^KbBc(JtcADQ2CCf zh*+4EFAvn`OH0Mqyn{Atc-?uX#-9*>DP}5`Hg?m9t*)xc3Wtu=G;ij>t>&@pfrHqQ zO4mlL2E9x2#IyMMJ+B-Da1`_yR2K%i7J$yD$uvwZP?shd8!l>W-!fFG*AR?$Zl z5`!(lXQ}|8pzQWPC?N1&>y#OKH=YNi4kU`umO#`!kYUisL8uCY+O3$zZdf|W=<7M6 zKVE{Mi>LPaLJP-x8hp>OZdaowWHA}}YvNf63o2xeO%bO3DA{jZPP3q-lK z)$^v!PrpaxfqZgN8;A9hnz}-9CG>PMdiu`KJ}nkWm9p+z9;)cD>-Pq2HrDo z^}txW?LfvW#d5Z#Pvx@xL2-+B-Xdh{kS)t8kwrEUtBp5#iLxwb;#aX>zhrl z0xvK5LY*7x$Bcm8SDdb~m)y_+C*Ql}M=#^&-hE2jsBi>Ou&eRx@Oeh_`R{fm6mp7x zKZ4@d*v04lh8)BWp|c9M-g0Y-G}c(-J+@e?bT_JyvSf%fWwCiGd2<19sxNPY{Y!NZ zkw$i)tDv$t7Ur`_Sb1>|+C|`qqD-7(GWCA8%Bp3%q|6}3{Rw7H*YvT&!GQrh;eN12 zh%xtl9DAsBRb=)0rl)hudhxcvt{qI{^`-HTsYb`>joZuxqSI`(}54ReF`uw@8<-*1BOD`WFsOPj;X<}*h1HNuX-H8aRdh0T0U@L!aQ0hCnI zn~?mQRw^{$s;P+_vW?ROx^81M+_cq2a1f)fzWPxd>T7HApg7pCrk65MT8rnBa~*Kc z#>YYMtbH>;5(2yC!vvzNW?5LHCO(fDMk}~Nb_jawl;B#MB5J)@o8qVJ#f-3ac(I294TA* zk!6IE_>L;#_|@swOfDB`<~Is)Fy^M_f}6ui5m~vNN}U!S#6K6$KotxtE@7g@dNAo` z&qiPOQKqfN6H5-{>JG5t)KUl6xk~MQNK073Rl)rvpV^Od5u|OFVT??k)U&XYWX{-y zA&g0;DWz=~mG)+k1nV|RSB~v%Imjy8;mwbHEozl<%sozq{oCZ*!$kPd9HiNT^y}xP zAH7`GOOsOq{<>itSvp$|rfT*Ej#RK8kX7)gHsb{s*f;_#4}+B(eMJ`Ev-3bZSi_A3 zd5?<)=vsb5Er7{eea+TaU-kwO?dvk(5ra!BSSAkj^e}Q$bA&6R5?cL?HU2?RqxrVS zWvl{Da{F~f#-=YLhMh(<{^nXx?^^~#el8;q9{XU7IFA})xAVv1%YPN>VcQp){=U64 zM#=)}WU>Yb5M(hu=UD;-+~Gf*cskf2X-CI%aF@UWx00%YkjwUe)%$qWW}|qx-*Wv2 z4W0IOJ8O+SX1|1Rhadz4$6r%Ow-8YIrauVc!NQ2|f)>!nOkIeeNHwqr9i{il&mo&C zq(fUvv`S6$mysf9d1vA5y!LP5BZ5s>%d;0+U~+-8(ZaWkvdAg}2O4syz(#c>W+QKq zoU)*9$9;3;1SC7!rgU;nj;qwFfG4hWFU792k*y)6(F|j&V&_fWkAxii=5Nnf94G4V zYEaQiE2lKm^r#JfbggB`KG*tl!3(~>a4I8Q-vg4*OO{=J#81ttrg}-CIW3cBze^5p1bMM55(PK@uM6@xuz`qcAR+CD-pb64;LO9cdOI zp<4?kI=kt+->&(zQi^3J?42QUMb>ZPq?{g{@h9b2+Xzcu093d{N~FE0OtE%h63{Ji z3^^t$%ZU^p-e=eB39qNXPNXRLqZxR_;MO8OI&U^WRrSE++G9WF{6#&-G%{CLh)rX+ zg`klIQnH6Jft4_iSI3*MLbg}Dv+Cc-7ZCyCAZb{o65ym@e#3lL=r)~}JRjJZ@`L1t zc#CA$&ceLaWo(fVI4jgiC5Pwt=2Z;akpHXGM-(Bn@Xyw=`tRsyHPJ&rdJ+I*GzNnP zKE5ZpF&`!Whk|cS9C_0dPg|0>?Buiw?uvb3JOFk=+pVvJL}OM3#*C`n4+dS;l4fcF zcT2V?@5dfo7}Glv>MgIh*y6KeNpYRx6mAXxzPW3sM{)Lh>c{ge5cE&-PO%i zzCmUU9ojj4DX0z!#s4R5G#VIMGIh6rP$==4>-hS~Ty|L+$Hl5jr022!g|)6KSP7Nz zkzyR36S)xdXLV%PqDh-vCMBvPE`u`h$HS2O=vNMbDPy_;sAL>A?Piyyx-<&zyn9BD zyE@%o+MieIn#e(g@)0exh)xp~K}Kd+TCnt1S+&z5mFQQoy6J>AQZV3drkM0NRn!7& zfkK>?Gwr(bFpqSwDxuPA-Vf7GQ%6h27h|$59Mkzf(+vyH$gGMV`p&fvdui?Ru6_8n zp})`j>5}f!hItcCbeP`4e3%;iZcCuo=YviE`X0?0w>z;me`f(2?}?%<>9+IzeibnA zzxG*ImHsIpdR*l(=M4~ZH2aF31=?*<2Ly(IEl9vMh#PWPHiw`@dQ1Av9A&ZSJh6Ke z=99#mbA9rnC5gN%RmN(_wePIz_$&N$kBh1J=$$KxioZ6Gq2n+Yh;`!mp_qs#_~+Zk zv-(4W(Lsy^Lo-2p`H#X&YyHoNkFu#siws>(SQa&dd%;)){X8Jdt2VrNGM9dc5pRRN7|yx_Q#sm8I4Eh~S$V`gNfJ#Yy=6RcO|{a9c4kf10g^rzJ$5;OPj zY{q8>B^>gvE5}+nzf*F@T~bA{g4Oku#@s*WD=|p3>)3n;Lr;Mw>Cx6cf71L6c@ga` z#Qw8sUo1|I$oGJq@S=(_`!$HdD5?_1mLh?Ha$`cCwDVcCN}~d<_P{?H+^b(*S^PQx zTWF0zMRjArbHMPLv4Wu_VruHBzB__0esTOMO(MLfKlahDN=M;wbXIY9?>g1e@bZTY z76p|==mK2~UGdHnMP>I;59G8|#%bv%`su*0?RL8*f>40Rvyc;?Hi6Eab#^nE0{Ptm z6sl6Dz|=tFqNU54X=VT~WY*j9H{?_0BbZAz%(`ucSaci4(nw{8Q&`do8?y`6HU~ap zx|FCi#NFSrjrhvbpQrR=9@{4JA_Q`(M$%7&o1rmz57Kk|~4W@aOcE0ACh*t&V0o4iTsy`F9-y!f>HMQ5U&os(--hcAos+a6be zWJA7naCGui1HPq1q_nTIBq*zH-Ui0rmo6bOG=6Lz;#+TFiFR8>0*R1{B)>n_*z#B-IF(yw|d{q~@#LD|VYB&hDkGW1hv< zIeUDrDE8RWK@Ml!rAntqEd)$Keq!F@fD<$YJT+vW{6!hl^-$@rj}dEnQFN&JdMx0Q zt-}NU(Y8R-Z50b#yZ#&e@2LejiL)7=U&;Ui#<{m265hmKyeG#Ky|_~(Y2G;1I!Ut7%Q`k7d_f9Sd6VVd4Br7bO&&o z(r72P%PY)?IKFJ#wjbV4%{JD$t+yRdU!vAPq$1voha}w@ooh}i!t29jS&EO1UXyXV z>n^{}5k7G`((Ru&X{t&jrg%2eS8C z?gc;lXqM!ld5emYW0k+AUtlovUU3|0-wH}eiX4TGl?$(6i7TEaT20F7#?wrk*YgWt zaHJ}#H=QlclNp{n%2HVa-SQfxlC0$w+PLJdl#Z|({?oVldl(^2?r{HRku=GGVlww5-%Z1inRQ4hEA+`j*~n-(M7=*?BbgBXN}twUB&p+9=hYC_ zKS;cnGW+yMop91M+jmd>J0k3z7BrQQ@hix1fT%Kf zHh=2=$+eoJ&ZDU5(B8bm_&~bL^`W%uuT+Hk1NLhC=E&Br0fGz;+C%JO^ueTmc1^p< z^O1Jv3rSwi`YWvrPec;V8nk8zB))NhjBL{e0q`&l?cl@Ck1fBhoXLc4(_9hVw?HOV-buWuj!pwvVYp!&^8-LCN9_NTjFywdDUz#6+XyurgX~U{ zBymWhPRRCog@r7fxt1)c6e(UvH5hNe0UB4#6#qG_R3+c_;{(kIVrfE+4d)%e2+Xj| zQhR)ch!r25GSdF?cQTUA#2tvgkqvoFB3amxKH%o`3|s91{9XRJz;IupRj@qsay)`%*+a7Biss%H-@{$a6D1VfeFKLY0D!M2V}7a86`1gYh0iQ>ItY_fvE&|FNcY#q!xdRv$`j`d7Bsv$ z4Ac%iRxgm6qFTGxW+p()lqh3oP8@lFR4&wDW zneVTH=Qg+Ab;LE{vowOr+rTD{++aOG!m?x|zQ3Z#lt$e|+Io{>RgpBE>Cp?J#(d*i z2j4O|F;4PY#2&T68$AG`-ob*ise`>R>KPQW1VJg85}LYf;?#?ZO=mR&^Y!1ItQ*Kw zL?WDU#`*D`5bi-WGwYfI-{Dg5UNGn&!Ig_41l>?L;F-GWwUPsRUym15Da#+-#kl;s ziKwMR(k>md`=&k-b_F1Pqny6yn*HLipl$6X0SQwmA&GIxFrGk!|2BH8+u<;j5mq4s z*$k^aRCe3EAfvBzrAW$A7cv>*$q8k-j7jlx=mU%R4X;hkigI=+3mdlNrl%_yi9>k-_v!4jmi{7TH*4BdpVe3+6Z%x~X0%1q6{@-+w z5|6P&D>9>o#pzH4roF$oeTBpJgl$yzF$1c!fq%5;VD5nkE{flO%wRjwjJn*yur;gR zno@)b>yYMa$Ca(^{{(mz)YBT63(B4{HkDQD4EkyGaW^*II(8iUui!+5g15h4%0qY1^le=pZ)#lu4|V&R=r4zDkH z1wS7WapncYQ-9(vrR2#~|2#hBPM*RqS9oV(g`t5~HQ;D$z#kl`YI~*eRzwHQ1yd=a zQ7#m+EJ1$zW~8-4uu2Fc!e&ebW_F(46012|K2g^3EfMt`pvsw3ld}s5N|{^3DsW;) zTJt#)@s0;26!=TROS%zmGkP;aJlTxFYMdD;?>Ye=im{|wIaS+cG3TQ?Y{uX(TO7D0n#P)2t;8%^YhFqk2`gq3oiCJSID3)s6ViRr=MuB9UfS%YEqW!Ef} zgv6!j>ReL>Nc=~c+!l|q69Bs%y4qtB(G&aYpM;>TW!J$a|f`sRaNZi`nLhAN9L zs^ei6`A~z%A=c(9GcSTLMg96yPbT3t`_O>CY9V#O6wdtIGR$|N4u4&?fom~k&#k$Z z88Wt2lZ+5Xa3d!S+ek%xtNEk*rJrYJD_i$oqJJ)$Aau|z?RMZzTbj)(59_U2Z`3io zOfojQWHYFV1x+TmdnjGRg)#14&8%_GX6i!6O5eV;KLg$_MI_h3O`yi}%lcP$8z$h3 zlyNhH_+a}D^yCi9XigUaVdClXxB^W)+>b-w$u7!`B%LkKC#=BiYnK0)JMz zkLfUco!;!<8{}J(yEbccB!C=S3p^r{B(Z1Q_KzMl#iMVc0Z#_*$^D+(xY7`=v>v8H{0>40i z+W_q^ie!sAh~<`Kn4lu-EMt_zvH_oAKeMMQM^n|Ikjo1yKDhZK?6?puk`kkG!stEc zufF+?o-cRlig!9R7KWiAYQS64BcK#!&u5);+rGI=HDEUZ`-~1$ISjMO=^N?W^2f@Gru97}rEhaQ(4qF5O`2 z9nNC%z1#e(ZO0oH`CBnTfa%}WZnxc_<=_AkyhGmxTTI?uq%uEC2vB>7DJZEB&XL-` zLHd3#8ZHQ#W`?cy;6-x8Zh>$8;2k`KL=7h$=B0(>?|2y9r;5HLL0mK!D=bU8W`Q@a z)CL~{Vc#?&LA>FI7xWvX-C0ddGxd&jKrTktlXH7;k5EI05k$GuCwog}=(_K1ROYV% zIKF`mst(5`WY1-F6mT%a@NC3nAJexjrd?;_sDm}s-`fuj^-FEa@d|(A<&NC_P9(I@ zd2kqNLM#;=cW_o_(5A7U+d)uq@s-8E-vRVl;^|+ABsP&$&L^69W3x~MO2XB{V@P0Q z?2^Z8%|H7uo8QJyDM)x*nF_0X5pjlx*=u|)<<<)*QJxXD-2f47V&N~_{>t}JE^}ku zbB)yqcf3)1=#iL<9*moJoh9uDGFiLuT+^p)GLX&j`(GqH6UGjrO0^ z^6@nNlT@8ywHYCbOCgGc+@*1xed0Y3Me`JvxtU6N^|S z%z4Wk?JmIuo}P9OJ&+`UB@MHBTkEklkPju~7g3kj6R@8D`{)T#Nbuz`cz)cACEEq< zB!C_!6vGbg7fIApfiG?4dp4igJlv9Hd0jsFJ`(ED=<-AemVAuXFhrU-B<3IL+AkVa zMRD*$sX? z5hxP@HJ7ht0V|V)LKJ@!J5FkP-^S5s;?b2~YB$Xpt zQN624beF^tscoS6g(`}zqlgkJ5Zz(RDB?_a8CDU+$F{`4r)qzqe2(qJ;KV`%nn1$h zRcQCbl%wk~BH9DpVy9u8bSU!ma{76a32Dabn|xh;cUjyO^UGEB!va4rGsI|}FelzA zVh)*PnzHem&v(mWwVKYa!=^>`&21jmu<<^WIN`!7pE1q=iD5jDJV!5Wh|+E(fY8<) z|K<|}`U@`rqWS$*?nI$x~|e`){I>i!ngXu^NS)5W}8!Hyq_>c4LC38&-V zCrpj2%X3MIcH%feM0Yhrg6z|~jxtIZTsJesit6{DF#FYDR9-EltL5+tRd!C8l-oFt zoC2&7d{t%%vof3_<7)gXc3}g=7S3XWd)w){zha^_V0k7IZ9bbVCb08EBij2~s_R9q zmd*jMG^h=w1z%LN<07A3+|TlLlb}Qve=d@t)F2&DWK9&)!98+{tp;8n#NmiiC%k}5 zF5r3*!i<<8+_Z;dsAQQieq-lUWVLC;(V!(6_&jc7olX3cBwV?#U}z3bnHrib4b8=d z7BNZ^2A_t@DG`U$cniU!Ek5BKumA+I4%w@LQ0S!E#b}co9b~jznmt(e>j2u|e>S?; z5Rb%8JYpD+V&yD(q*Lugu~4m^UDDI>G&L#?#2udQqbuT5rOE7jkMGO8MvFNq*MQkjkZBp3@oyN?YN>LBN6Y#8_J^-RwI0L`Rx*d}>JbMOkjl1SIrzvU%p~RNSztBzRGv zskR27XgOS4yOc30f1s46%(MgH#-fSrf?JB=TGzw1%x5hsd=$=s<8XaTHa87>F+xv-{imsy^veb;flNWT%Q)e;xXK;ekzsZanZYX8dJZ z%r5;k&{(g5FFw^~@KuXHNi`k(18+P?@!NeqtFYU8fqIGz0E?#9>tcY>lGY1+hkdWF_dkf_OCpals?jEp;>XFnc>VJOTD zpuG($!4R6$e*(1w#l@dUTqV>psBN)rFiM>UMxmGt$8t#kGDKSO$bu4hsflV632La& zCmf8$*M+22kaAe|456sn2gehy3O(l0<95XjyU5!B8V!b`?V!*Jowdwq(nN@6Z*PLKxXi=zXB;LGlzWXfVsp zd57B_Iyr7^zdj=rczUUO^-5iKa%)P|M7`ngf1NB;oyWI?usYMBHG5W*5s;NO5P;~$FUbi&HYHy!8e z_WPo73FX(|E`FL%WT05CYNFQ{zMFvAjrYF1?asfxyPJKfa_6o`%B#-@1B(B18g^8j ze;o%rl?7Bv8$~-C7#Xsh@6o*YaUdht%_3zgUAu(5`f_zY@9-pPu<){eUVr!+pMB76 z!u%5VgRVCSl-`p&-k%!NHlcDdbtJt~jp_txbV9AF+;w8~b96HZ0E`{5S%1W(yVqsm zZ&y`nhCMK|z9YEe_9&LxZN7c%$}{+T|?O!;;H}!YSnGQM#*YkM@#6lj?|o2(;G#m%|Gg@Kq+5roM6*=Q-4-kT9%Cd>59(2hiPhsre*}s& z9mTz1(HRbl+RpXk!y;q`N5!Cr5A@qrSyMMuY=$&Z-Q)0rEvCwks5rY%q4fG$ntnBf z#}JqzLbte2;MkvnT5^ydB=mk@uIlUGfKL)8z^r%Zyn-ODFkfO3wTl9V;;B4g@aBhY zHfTK^q&&d;IX;mVBY@yz8n?g7e{YKr9q#TLz?s){=f%~c=02cGQcqS#_+sP3) zZp6+Dx~00_I%ZowtEfhViq&0lu?`KECYoY+Rui@5J%H^Fa|CG>`RdC=f2(oIFD#%x z+)OWS;()T6-a;}llm^(=$(`B{q(eoX%U%EVa-aeL>gC5}yTR^Hjz3MeVDkbUR*Le6 z%Ym6rBmOEdSEHisEH&KV)cm3k$r?C{lj>yh0|Nf^>q#KL78FxS%!b2En>~ba9ixBNX3*)s#I8`i4w+rTN0Q|`H6Kf6AjRba`26py-!DLNxq-nPG|Wt#`I+_ z6nccyt9UBD%#S|bRnI?%FnUe2m>2W)G|Xg$6|RdlDp6fMd(OB44$6ZBJDfDW9#S*0 z*gnTA(s;AlJ6{UktuJ^^cfnX#c)Ow8l40W!kroU)BH*iEOS;wjxWTCww0RXSdg zTosaX3$gYzR~4Y1(#BU6&(X$Y@2W4bC>5#2OF&MHR$o}ie;X4^=GMW&N$+^Iz*X1P zV5&3JwArZaQ+Z3fi?_6=^OiOaKx{Ao{XzvBc)!!}3pDllDQwEuir0$3Z zrp!-S*<;?9fBG}{>GUJsm+pHF{Cz2-z&w#WFJ(@S4gS2862)xqd8z+^e_qO*+Wfo} z!m?Fj%kxqp&9(^*+760sd0r}ad0y&{e_pD~j!>wi7ZoqkccneIL&5Bj*R}|}JEYZ? zP7HGym8yq4st_B;G#VHt+=lXlCYtl)ugu^MrIq&Mf4(uvn4t|1STasZ(E&0&*lqoQ zM-|@tGbbmUFiQtxq<3yDq0j25uKs$DH_#hD4qEADnaTOzHPfKgpMp`M%={w=xqU=b znzGJy+8{hQAu6A2hGHLMNQ)ufYa(s#4c)lC$4nJ3=cm5XJjKwMse7Dx3bETfh2C|D z)RZ8GK*rE(|d23B&y1l zh%^ImqD+m|biiPL0Ge_Cgp)CR69F-kv62%6Gc`CflR=&+ldw=P8xBbiE!lA#w18n! z(s1U?xu5S0-(I}>+xJS$llo8~f3K*I`}u+joc(7GzhuRxUY3Wu`EM6L!CY)c2r@!Y zI~SfUs6?9jnZBPBk=2cTmS)@h(3H3S{^zo=H!Qnq>f>(z+J9TF{q-jNc%9EF&OX^m z3ja$!n98lMaesY^B_ayQaO+=mLo_l{y4`9-q?mEUxF1v5F+erqM9dcOf2$5ZT!x2b ze}09Sku!!g52HR}fm4LI1p*m|)Jz-^CfgA(3rVq4At~ZC*ZINke^u;?YPI(t>dJE~ z@Mf1c`L;L|O^Pa55*d*5kg(yJmavY4ul?L5D$wgBza^6FugUIpz1h@rDzclsr^*oQkt$8nHM$XPlafN%?|xQg&82f4t%?C?%7kBoYbDpDIiKyt33!l;x2BV8<}WVmQ@j z_&fC^fAs6lrXdCI^MfD(kM;RcwG5%X z#|cCBj$kAS@#Eg5lm_NAHdT;FN%?TtqOgx=4_o8yO^Vwj!WMsQQp6$#<8+?R6-;Q5 z)L)JqYkF!-PRjNIH53{1xyGl!oiHmnX?Tn{mTF270p5Q>UA&CCXkJ1Whunr+q4lgM z(uagEUqblug@tci{Z7PJF~t!nupQ$n#6V5n6y8(r|7TH?!r?TF{oQt3ScmDIr$-rK zP`MkFmZ0#Qzr(F0s@qg zv9KDzrgoN3j>c)!ltA2;x=R>-C9iE&DzaUE6;`&Whn;C{(NufW)Q9{~*45q(<=s%4 zabgBzN%rA8n=ddMs(*uemrK#ryLQ}7x!JTuok3cUn9+YY=6)*o4yST;Vj1e;wa0e* zMPx_UsCsP5t1{fOcVV6;ul6v-)<1wEYa=#|K?gAeT;^#J0h)eSuGd8~2hoBg%Y3tc zWzto4) zO4Nzf5kG(0?7PCja;5ue0wt&5*TT#fF4}96<(IVu7CIymzS>{67^M3cKKTz01BC0c z-tLZv@W7#+Y*jR+lbrnrRQmgNX6Ye8?IA(9tdG?)(mO&0F-5v_7VC7RFrq8n-lQgx zAk+>je&4ySkH9rBGIBK0Um&=FAichb|?+CP6nQQH)?;QZ78p0s8GtogySw}`xI z<{Uc|-0YxPTC}8}o-zb1`6)wSTAwlm-VeFkbYxY;2l{rH_hJl# zx))86Vy%GN*!4;B*5+XmvOLX(A12sfqJ)3GNh*tFgBme&99&&$GO!)&OoEzh-v$i$ zs%IV$+M0Un@4BErUKwg!*;#GU&wE@aj99Ena0GjBvH8Z&?5z%{a8{SL1<*F5MqC>0 zkJ@P2p~LlqR69N9U|s`9wRBu z+Ow*z3jb@rTLvEkR$nX+{*lluwFAQg%Q?yNFm>KE!+2=FOE$zPHx7@&ZB4NtHt|W= z8|)T4#v)}{+Z>!|GD4maM_t|p0wIjod#i!I-H5^C4CS;bt0E667ig>Rn}aTQ=~&mX ze3R}*NF>ZCNUf7a1cscRJBQ~6;go+XhZ+NhFM7ky!gcehE{GmA`6%&l7oD7!LPZcw zdm>cymY$9ky+OaHA?#kPcn~!{himL?u9=fuQz@>cTcb1vruQ8ih5>`RdXCUBEBbXK z5hU5KienfQp$W?Stm2v_+EoLoFt@t|Ib=Yz70lOF)g-=$H4@U6QUJFP&!4Tz*u=IMKXW!RLf)(CcP-kC3i)sk3*0#=uyJ3tQ)8^c6GJtD(q2g zN=pxFXlY_BO>l0I5d5CR2`M(`L|F@##z8G)vCKD{_T0k+oFg&9rjy-j1T`d6B@3H$ zRM$;vR*;n8-K@Lg{(5agb=}==LM=t&r0Y?WzRgxs;Y>irt4@C_U*`f26LnZj zZ}X`PQAM>0hotN&12I&Wx5Y9)?ybpimwDSuQ(o1LN647TR(-_8{q9gHceg3>mA`YN z;d-cR|Jd3gFin_ve`xZ%xfHgTW0Gdh-TKMGwXQaI{;sSX^0a9`2LcK|Z&%k7s8@w? zSE+iU1F`v~SHt5UBSC*iM4A5bf^gQ+jTj$RIm0rwt^dg98>T)h{RNivM!iXux1d`3 zF(^+po%{6y;#4Iqqn)J|GY$ld)9fZ4PH(OKaoL@#Nz0WBq$&SGOdrN`2qdJ>^OzY7 z*Bhjqw`u%_M`raBVLITKg~NLtmdADl^%w<+M^;}ZJ9=!|eI|b^?_nf778RkfcE5YZ z!HIBpteO)xV8XCP1c?u`9U`i_UOC0-Gb#2!<)S|{FA^Oh7=S5(;i~Jt_gj2%pjAKS znog>1f!(GklC+3y?&zyQrNRrKL4; zPZ$yGn#-yc|3`nq_(Z4Gz{3j$l?OXzE>NQhaoS#b!=9gadUBfEbQ3heG?G3^V8IwV zRw02Q0d3c}nXQ0K&)b$Xp=<0A;04u?@BMMiH~xSpd|G#DogPI_EXE#_j0DK;bv5oC ze9$p~W1E3f9?zs|h76w8plA0hq(r6bn8_ZpIEIM{y(j&;jV)|m2TeOWpT4+PSG z31l?8f+~>>GH2bMsc{b`Yc_jS*PP1LB$p?WHCv&|lWDW=S6fg9ye%wpCw*tnc+3(= zZM%P=$)rJ>1s(Pv0?^4=*6UhT{+po7tiw{_yEgIlpN4t2d#3PLe+n8NT@|b2GJF}j zOe~QEyQzQuHTMrz$L*!>8?n%dVvxZX>*J(9kHG@*Yv zZVu&cQ!ab`*RWmT(S5l5qxQY{lzQFo!PdUY+lG!iN>=0?k*ES6j=NpG4=wul_JaFB zh`Uxp=-htgpmRI+s+rq&Gmg8^#Gl9kC^c|TPS<}C;uM-d)TzFct=&$c3DHd6`wjU$ z$0BMKSg@ZciU=o2XQlE=GrCkQ44R0n~T(`#-k`)5M48n_k5@Iq=3)6^6S4WmBi-5E*JZ$^ND<%|| zD~o??g0Vx!NzTD=)p;1M`g*|mk4d;&+LxVrysOeDrEIjem@p-f0|y}bpWX-5Ym_o; zW?up7C~eP`w6M=;+q|C(+@-5G6G7Ig?t3qverAYnEGDA|RcShy1FP5=+h|{2>7{g` z7j_erb@1+JD%wNYiCJ1VpXQ66lomZOO_6^*w8w`p3dD+RAOYZgboEJ1aQ=)YUnS9F z?HBZUymjPIvfQ*#wmEZiv3qozejQ5K*Di*>B7J;O8F$drB7B^Moa#DFAQWw&ieHxd z_kB;~{xn%!KACSrZV2Mu5VWZgQ}m%7KvJg2!>(jNyj$uh6hz-1T@81u7JhrcALI9)=@ORu&Ew}y;DjeEqlQDb~0y8j^z+M#rHJ7nv z0V z$Yus2lQQ*V7KM|`ZC=3^f#XPGm929>yP^HrwInocCXY(QkW}bq#2mSR6MjKaa^ZMX z1 zaD!+oj?knDw`!t@V(dafqJ(PJ7ZPtY45=%t;;z`BwzR@{6C-iPj3JC#Sl19|fu01k zo_5(Dxtv(afEvxy2p!!(WQb5mjW%{b#&G?VP^Ceuhy{`N*=Ja9HbpGV-W&7@{6S=U zDYRP6VPT?lD2G&-!Fqq+9WVhaTP@9}OeGInKhO^5W56}Wof_zdBb3vq;u8jI&iX^Y zwMy9$Zz80yD0@;#n3h9gK2b##;qYLMt5Af3U!8FA$r1!9K^yoF)WDrW|^0Mw`<_~&gS}Ana z8=?Yr>i|gH%WXB4fNBD#T-b+azH$-Yzl4#yGAy&J{gxPG!-!>x6si{%JQh+O3&jF} zMDeA37|@S7VIk~EC!CI*@L)|~9GY=&sy9LXh;y~%O-z6A9E4`nNbp1NZV2zJ4gFDk zb@UD+!%s(^L7Uz*45KJ(BE?7xEa8$p^&>;R=`;DJPx6h$_y%e6!*D*GO>Ou4eg17`osZUF-b;eOT z5H_+4P~l{?vp4D?t|Ox3SrhZdjz~Ds({Xqn&IFXncF=bRe36x^TZ~3I1d3?v>G&yP z92jmpB}&;o2YOo{p+}-RONrxD!295_`bG(?i-Clm68wxVP@1F!b*_B#9wBL2EoxAC zp)om=Xk#9KS(Tg2{;olkbtcApJ=D0J#}sca7M6!MXJnTPE_vVnBbp+DU? zzjilOi4WX>wiPCiI5Y($58>+E1WumL4Lp#eor|zC$yTMyd<=&2-O{(Ru#*mJzzfD0 ze6F+KeH!52>U|YjMHuH01%BKnWY`&I^nM5$mE*JnYq~6x2xK4un3E}FECKhER%9Ow zQSlS^+K&)EY+(tLfMh8FRg{)4V*shxWngW`p##Mmnc6dnGh!6h$DjrD zVnEy>md^}Wm@=RU6d0n15{wsl%%yO0JJq)m^{80nMQtI0Jr3m*Y%QJ4x`7vvryR-C zK9Oe%zgqEjYb+`xw=)h2wtA(MW|{+zLHG}NQ_FvEE2t6R9@ykrfcv0?^h*Ua)@c+_ zf9J2Yp>d8rj0!VN!HTpCrGU3j=K<+)&I1PO%GH-*`pGOuo|WauXJk2mu zBPU8A{R*%I)~48GzP|eSnNAk)Cu9OdSTnhIL)R1BDAkkIA;HKHy4M13WT= z1|@$--+XWi*aUFsMKf4eqJAzJKQxDUJs89%LI4KSp)~#DLk4M5Kn^Af#8OUJmnZ&& zxB?+Jejwh3Dro8Jo^?!02JnV#ZRk3tVHR|xRWKd}XOyI@Rj&`znMwX}z>#RGo`&Qj z5L&z#iEEPb)~_9gUn8XjuWD6*U+MGAX;wQ3MSeODp>_*jURzikeq#j~?9Kbrh=$n5K8&bQ7nNYdboZa&&%-(h1&F=`o#VC~*A&kvL*-9dPBmVetAd^>;a zN$~|c*Mr0SVU1VaL*7q>x}7p!))HU%{G-C^B3` zxehfgyjZed?TYQHm^XDjSIKboHE4fj{_z(GS+4pKIX?5n3dPdMxfy3H(eZOLPN>!6 zbF*0S$)mTUr)7|raeMT*3^O=({J4w~lb$**>m`-%J$0`5!+8I=47;;Bs=yH8Q*q%; z`R=@0!i*Vpz_tw$9d^H4Vwr^5MhSqJWsG0or;O#Nut*0*0>3iPu6a->f)#&MjY_le z|Hlt_D7f&aCuc7@;8FBhCp=tWb?LwIh=-kZ#3Ro+;z?h0#6u5XI5~RzK@cWGJnmEr zz@EmO57&~Qj^I)(i==?vIB;jo98CdJLJ=;5?LYq*2rqqrOOR$L`v2Erzez0?ask%X zV$+Laqvsqj9GT{L-ABK&?t_0g;j%73=Gg@>-Sd9X191||y?vs-ZW*zsPUdU|EstCc`45)7L8xD; z9=VL$ODe`bIC9leMXy?#r&le-vuy9Raa5ccN9|XOqc6fFGlqG%z}!Av6Dj0ti??S6 z()53a?eY39BvLOg?P}}Fp|7ayYP=!)s><#&2=GPSAv~jkP{uH)WGvLgOv9~z1FI@| z=aYPCM*}i2F_V!sDSyRUOLH4H629|S;Fw%MpdWZtxl-lCak5o$#bx=hiL8Sx(_XC= z(o0c^^Y8cTAvxx7NDfUi$w4z1@I#~D=$;V;a=}qZjuUR#5#cXlDlzWSNi4XF6REhX zOBfU8k|^%gnuahFr`EU||<)00#nE0Z-JhQ@~3@B0?Brm=%HbyO6?+8m4aO`o0Be;P^!&{X@Mb=!hA#pd03@Bg(> zliWddHjyzfRTPlXVuRo$8MXqG@jC=_QezlV4NilB!IDPMg5A`N2y-bVC=4o44mKzl z2zUgNjYzFd1$Yi1B9NV+YgLdnyryD{Kqgnih|>xlV1Ht*V3%RS6oQF^J1z|Er%-H= z;cDdZ;cj$%vuxB&O6z=7x|0SgAGvGRig(44^oME>dc+7-Z;_&4(+9nvdiF z|6w?0j)g`VT{c&Sg)%7wg?F34O!$Fe6L>>r2*dzo!7_8)EC~Yww%LcUIIGQod?|VK z=#d*=xPS2vvzIeBe&W8me|UXAxmn!Krap?7uiZD_jJ|rsIpHtA3L(6(%gindyIi-U z+xCa4U0wwVE#Rh$Y5?yxQG3>{AJ{HWHfNr+#O8LH?AT>V^XmZ-wJ+lLE!lZXXTG`l z>vqd#w?Ep)Nl%O~;M^|vJJ?_IzJv3ZcKJ_Bc7MHPBhY<&`NZX?EgN;;Z7zD&HSfHI ze%y_kwt%;-3=P3I2V_Wn8K|eRpSq6I5$&R#Y5eK89rE38;$H=A?mxF`zi!v9#MgL$ z;oN6<7kiD_e{ZBe=kDdm-48o`@v%?mdDq|nyU#cKv3n8+)aT&w!AR(2inl!%_7-6O>(67Ra9(brx-L0oj5V^1Eqv;ri!7jFFYc5!8FJbE-5 zzx?yV#EqX{znhH4-_53r$#ijV1%)9;;}?_r*~9#1a^Dma&D68Wo7?MeXCK|AQNx*r zl8LTHATh^UXUA)Udpw=aKp?Jj&q1IK;JAE8}bRB@Tf#_-tO1)boHaMCR-8 zFeos^EpDG8tu*R2UPQW35Pdm0l(+YW!-MRPXc7HMP7awka zzkZ+e^=$V3eC$Mq1EWWcNW{|g<;7`qW0uYdb+H#ukR!4yyt#+C~ltifKu9l-@}XMqxr%K{x-lLb7d zL0CdR{?qTnrhHw#Ogh)Ipz&l}lAlf@e{(s(>-U#DPfS(%E zReCX9>C~7m%NVxTk7qpHrDUJgVmbQ+r7I58RPB`^YPekg#*#Uf;H#KzDjITa+KJK_ zA$p;KHiEmMY+hCQxFG7__8g^Km;{fHIUmMRioVutuWr58GDuVdO7Y5rIL7&xYaQAg z=0x2bk*J4dj)FcZqd2mZ-<|~9zQYsYCLOE9pORHQ7X}^eU(A# zj=C(;_H>AECU2)bFXuoXD|dTgQDxb*6(jL9&P}gMTQ2IYz<~9A3F?sA$~d`{J;2cbU5^;O#@UIk>Ir1s#_f zx4C3OAx?PB=C3J9;Fc3nlN5Wm$FkhymU=Shj3TMzWKKBZTspGDYFFOf09#aGC$+t~ z&9>WObK`f>(JmNPA*9~sKb~9Jrhn0h;z02U3!D;(Bgu44^ftAOLJW)6G6a~TWcntx z8Zn3l^}J2%2X+ixojXCZ&C=cMLG}Aj7}I>8VugbG4WG=ZL>xq`miU-76k?5X-lY_% zL2lYrABM+9O76s_*npLgAuQHTw39#vjM<`kNHH!DI6=MJXHP*|yE%qoH-BYVt!YeC zOA(9+rIQwA+7fUJ-c_4onUIh$L^Ykv8NygZ9t~s?97Ef*{lSBNJLEV~XIIDg}0OrBsO5PubFNJf#8G2&c; z>rpFXARAVHUpSy6!+`;h;=$bm3P~kp38!4^ zPF8sW4@fw=*`X(NH__y6auJMZqnBczMhKuJ*3EEj-8p=~A>uk3oJ_eu&aI(ps#gRO zqs~&zSMn$YH>;^?@P8?6q-9)s*N_BZ3a-IKbLa#TvpK;SPk*`FudZj*=8gMow+t>& zUH@L*Zp(e%tlR94VPyzx#Dl!cRSt=V=xKfnuVOjXC?&eIw?Xvx*N(Od&v0Gl*1UT@m0p*J`_kRqIEuTi`(t6Lv0s8gY4GzO zdrm@-aFvo!Bf`xPTp+Ab)44AlghX)0qROE_xlPsRJY{+c+HP#}Wko_@IH?zbw0OQ& z1|ZAW@Nly_4}Xz+(~03*dv>dHJ7a3<&+dJ(UbhO4>&HsJ02iThjeyPg(m_5g9c+b0 z6N+tTFAJ^fWDxY)5P+dIf2!7ofC51{D_03#c$=UBg0bpZrTRX8`yAmR?FcOZ|Ks2q zk3B^I)-UpecOkm9)0{OIKm@J*nIS2w0u-^Qw4wT9%YT|7%d#QM@|Rc^AGPd5Ix}M& z_WEm2Kr&6t8mDIZf2?K~QRa+T)tWe{R1K(V5fj&bfIg@c7Op5ELmtIF=@_ua+yLG* zukQNn6>KsX)cAOt#GM)Qq~X9OEj@z9NxJ6Nw(M(+O*Yf1rkLt$&WPdW46_v; zsoSVm0e?$0)w9!Ot9ou62H20P?56!d(oh-OssjnZo`@GQ5AGzItOW&;V8qu^N+sQp z!yC2_GbT3BF@C6D&ks)-8`zPY2N+$7VW*K%;sp1wEm5kgqLyRtn7=q!TV5wy?GR-| zQ>p#4EdOf@2Plu3tw*b?Lr;cQFn+i9PqV6cxqrE{7}<8)F>eUByRt&pKxSLs_Uc+l zyGQqHU)&b!Z0FZ6-CMp{Zy)()edyHNXRcFUZS5|Lax?dGJ0T({?3(zgmK6{y+A(CT zln8tx5RTyK`^_@XmmXUBx+~OJogSe=jY>~q-(KWNMm4QNJ0wd(Fc8M3d9`K%1pDKx(sYr0M{qRM2d*)T07`Vqeo7PfxIHT19Vq zdd{uPQuT`-w#lzg;NXNlE8-4(`XQW;0k32byqZDqN?rn9hdx2}eL9;HN+FBUDu0`C zCWd#Fr+^3JYlai4{VUZBiM0ULcRzgs1GWC)7;u4b_v3#xC;~XN1R%Pzw_W+s!+#q9s2+)z?#bO|ncfXcBV2^ZapF9Q+HJSAF40@@!w^ zyY8q_%OH`@#ZqgkvU(%*e;<4(SAMWe?z*j)dI=ho$I;-piho9YhZRk*Uq4r;s%hp77*o9Z63-MpVTYy%#W19?j8TlJ zEp_(O<=G!+);D2Y$XXT_3J@1q16~%lXCFV~$rAqf0fH?vdU9Wp-cGF0SWVWG-_HKs zSwVp+h{0g#qbpb~*s{BK31C8K@vyh#P~#bF?K9TsdNMYa3)kU;C#_H;EYWh;wn)?+{7?tGsiV{it^%1hIHd8%8x1Q zxtTirwZ`(Ej58~(<9~7HO8v?#h_;aRL=PZGT&>Xp zzN1GGQ}D=s57Oq_j{UvH866sDuE+Bh)p%a0?+&g#&>j1ZDv6uI&ic9Nwh2{Sdc=~8 zo(~x*jfZ~rag{eM^zTgP?J1nj=z^QVF-V%@^M8=5h5L$LhdiK*BQwN|nK+uo!9Z4s zW!|EDpFoogNgWSCu#^aHhTs18j%j3I$=;tJ0BQhVYnzC@txS#Y{s(%RC{2?wd=!^4 z2LTfUGC4GtKrR6)e_Km)8@Um_>sN43AuA?=_aiPJ?6RGTYd6_sRh5J7!$=&7+~q4I zWl8@&jlnzs5W^W#q&It!C<+5Ky1(xJx`BCjef5`*N(=}`j7vJWz8feyP*@|(#o&4} z_&gLyj>ZIIJpB1#JG~oGFrXHv8xVqw5XFpm zFs2jD5y3;Z zJbVeuyA_4MBGDXiDq!Ve#hZ57I6<0K<4SQRd9i+83XpBf2etp8P)HG7J^8B}qNP7X zBZ^5i;tLkIe;kAZ0gz<%H2397CRhn+QIB}tG6$~)OjD8wWR&(KA+R%shi_7o5ra67 zCDH_TVZC6Y8Iotj@u~rb}C;*ZmWoWma5-|v^s@QEi&NMhk zE&7{UqE=UI)2cyKn~o3y2t84mF$QBF`|g-``GkPqX`@5``<>!vKchuHgOP;w`uU9X zeP-ICaMe@cJ!wH~V3UZEf%Gk&pfUHQZ^I|)e~V9c(AgnvgHMId-Y*nQUx?#Nt2GW) zqHn0$6D$I?&C-6XY6rF02X)963s8G(26*DrI>~c2;)GTM-%e7J5ny`G_qE^&s?qGy zG7SiTNO2Y05)`0df5jjU*nv{03Kqd8nsK08V)kv>tj55Uh4Mw5_M^08*Bg6MHB=(h zf0~41#$vYFrVoFga}XpNX7ZGpYfPbhBmjOJ7fp}(B*xHpaK0zcXikuD2-ZE|HI`VC+B#^p_n zf1Q|t7KX*KDOzS|GlD<@NV2z~7dRb}e*_95>F-=1S*;9+<^o}GMksKRl4WALo|e_` zHosjh%Z5%+Hy7VL&+_?X>pt(QM`jcF|7`c{{#s<~^=x^MQq36b2vWJn4^P=*J%>q% z!9y68L?@8fCTG;OzI}qdcfU8QUACL8mfMuRl+9jEXipVt3Ng%k*VgY+$Og=se_BYI zMm2n@M*K1w>)}6>Q3tp}BTNI@s8i)Kj2pz-ibmB1I*2;8e>|3Em}Vo2hu^H8^He#` zA*Zr)m*cHw5l5Rvwp!Npx`>o5y)MWtghdhsp6&!{I#kzbhAAoa`&jQ%nhylCtwv0T z*>aMakXn+qpQ~^p9x{Y+xGD?_e^LwB(n}oIiMa%#kc0j7^0pmc-h!V~Zo#1NBng(L zTjTv>YB7U|skQYrK|tL)V_SMPJe#Jc;sRE02EK8SNQw{6C=5F)U%hL=4L=$;Z&SJo ze4t4MOQIMIgl-H;Hxr!D6qaQJ^kkCG+*B4QC|3z-L7tC*|N$Df4j}>DcRzq zuvDYNUlUThn9^tmr}j=^IQMmU;o!qeXY>(WF&GLSS_kUt0Hr#laM`s?xA!x9-r^#2 z`LCPjlsd=O#5&!ZPU*C+0S_zHt2*1YYA%pzJCr)6l(aMg!8JJv>(>J5MK*?Y5LR-} zxyix0{6G}ATo5n`6?8W{e+UR~i|J~2hr5LOd|g5vmS#Are|A-~v%3@moPtj1 z5FYHRbJKI19i6tOtxjXFh+7g01Dc>VW*D)`(Xy)KSv<^lv-P~JWgcg{X=;z!0IFo9 zt{4bdn)Bc(LF9u#?q=zA2phMz<78x1a0pu+gWjw_53W3fVy`+Aje-|;PXl3PSpB{o z@NR6M==}O5=O=sJet zzEkfgJ(uDzBy?%GS=##n%goxCLF%n&<&SyUmVDS&9=`gmY+de>IZ7kmxNNT$c7)oX zg^f#>uGzynf8Rj7xCz6`WVXp~ZG$qIoS^mz1rGfilW91mH)u^O^&%l1%}MAwal;I8|qDe<{<^lxcmCd09A_qw>Wh9gc-D zaU8w+aDDaPE9((38)N_tXp{qp>xff1xLsU*{)z_^_~$1m;4PJWEQT%ykdKA3>YERK zx%$`DyW&1;*k}R@fwdXHz7b{e2rX3NQ$IXH2<1ibYnfUx?0ssN;B^K(d@PZY@=$DH zKzN&se>hvDLoEd@jJ4&)fn;IsF*SeqAraWMpL1|}^Oo8``lZzY(aU&)IUWabD+Xlo z2c?r6fg*&PPVDNVPFQen4iI9OGVb8kja>mnlaL{HBUUKv;kOR4o75((%2?UmI|JH8 z541HZ?+drR^AMr~xFyuds6a&a3I!FQTD4ivf0fW?D4Icp2HNaFRb@}AD*xZ8>US+k zeHDDcv?UG^s4)X=`E=FDh^~T3-OcP`=xqz8?fI4kTxs>OpdM*z4$R;oX2)|*0?uUO z2063sSXJM%pd`&Il|@;> z<-+3Ox7_ZyPEJ!^3vESm87TmQX_GsY<1)k;`#^%j_PxbYd~Mo$eEH&yb|+acX0%pm z*od#KhSt~__$d@zu$gAoh&J;00WH#z!kxVY%9Cf}CVGcg+;r+H%6GLf!deHNpGDJuhT870{^n)GJZhU*h3?Ur{&AhO*ui2`LNdtRVg zETdNg zT8w1cPuy>~$Vl5`5vI#inyUWO@bUmqdm?LiD5q`scjwrLm&4-tcK7dU2}gPa!BQA( zf#sZ_L-!=xEoo&arIM<<@jJrZ`Cui(S)H;uC4Rc+&>k-9#z0YuuLqiqLrHhYAKRiLhEhTFP{5-q}m7^KWc#6NW4Z|J!-Dh0!pMRS|;e#(zQAD<{`AZTP z!OtYVL4^A1Dymp}enve~V-uK$D%}D&Lq^J;VtNGK1>oduNgTxko$%}sy4;6J6BWp~ zk+bGOvPSt0pl!h~Bq&E`ia_5YuCBIs%-GI_*58^fKgPRPFUK=;%Qrs={4TrAm$PkM zS8MthUrx0&y3U*5wDSt}ZsqZ#+h#h*Ulu5pN}3P0EQ*@*@G^!&KvhG7;narh4@9M8rTqZESln_-zEG8K zYFqlY^qoVtMvx3QkQqlHXf7 zlJ0O=5pydy>EE`#4(mFL_ucBBP+7x1WKNs+91_L%&QR zc}kXv(#F)AFluTTe0ub4UciIFZ1nFGVeXRC={PWM_~+MPe7txHW-&I_yrtXIu*@YC z?T#+DhQ8^2h79^hNuYcQT#9pn9IAaPYZJyxH3g#D?mGbc7W;$>=0QW8IB#-Q=Ms%T5ne?&7%!xy~Bb!I%8b;!~nefj>@ zF`QtXJXG!@y@)oZ=_n8m@Qd!?GKQAi8_F4PLemQewtV!IMcMfvDDI+Ik6X{`hjt zBGKO1J0@^|iR)9#pv^^39Q)cpt^e|fn97^%M>v>^M$oO!3t{Ah2SPl)G~YBc#w7g# zZkd_9o!)F1 zO%?=QtgINgTL0D#l;tKD9xH$N5KSvC#A`*idV{Qmi1TIU--VkY=a2$SAsV&mc3MP_KC$Q6nRSGwBEPRXnOv-2mDAH z7O5*hSM*lcMU5|$u_e8k&gm*)!m?q)l7AI+pOFR5Ody2U7+^}tv^YF&a}oqYvHWI^ zc|>H}>fZ*s3O>}lQm@&p3)?bLUjAo@-Rj<(+esoAF5`YzmLI26kX3l!+{|kilpgL)jP8W_0(%m%95P2t+^?;&`MsEr zHiJGH^r+2(n{{`W^222KtRAmqg5NkDA6)RoQDTT1JXcz==wL;-hJe{?4FpQEj0+RM z0PRDW2f81jONsX<02rjJLwofNkl^rZD5Jc@V^M-M4uWiY)ovcH8xSzR`42=&`|6(C zYJT2#b-tcp>jp1|kc!|0jy8DI;$7)-ZpZAaek{G+4eBMNp323f4lW37<$A@vwZaJu zoAYFAyLcDKmkpTDuO@4A7w27>GX)ypZQbcMGFl~%HPdG2A^CUpeH@;N7M&y)1wsjY zY>Z)S69F$mjlbO}O(vk^bNOY`ix7kAWs>UG?=&g=dAf0^6UZSCH#WIEV$~2d5~YiS zl-!f>p47s_*jB6CWt}FSr{`;}>M?01Ls-BG(D93z=HIZXICcPMJaLWpzhC^BXV0QUr=_`E(-3dEru5N2lt( ze(Zj$o3annx|`xfwacwFB#Yu^Rz>7TSSP8r}&$uM5J-v6iFW zCR&GA(7;Tj%F`7Zz4n_%ynx6I6J1L75%XCWj z&Spshzjr*?QE9$$->@#9>VAHZp6v0q(BkfB*ui`5e&_R9yuNTI?5fy@%Q$M+T+dps z@)K0D>}l%I`%ROw#E^I|+x51aGm)M3TdR>c{s^<*yFH_U_u}>R43@p{kN;s%PP`k zh62i?ch6!hmaEQ8I0~Oas*VweL7eM(_)b#we*Y+w<5&ExWLr_21xcUJOTXFmpvD?> z>>%CH*E$N{%qd09-~qC-AUYgbliTlg=j_vWX9_p^!_}@kuew$T*KM0#z8SY!Uqz#Q zDpU2A7w?Qh0PU?WA}>^L4JoC~!@#FV=?lg|v5u!=v}6ZEH+d(^6>_E6H=!f<=Nosq zhVhz>H};P0-6HCfF?8Y?gCNJt*;=v-wkoZHHv#(P_jzJ1HlFX|$C(q(Uev~j3Wi2Q z(^~66(wkWhpJqlI;AIY*qxbCdPhs`JOp~Yxd^htKK*zx6S8YWvl3H(ch7yMCxUE45Y`EnE{eLNYlsmz~W zhSPT!dJ$cnBpV3%pw`yWf8~y`<73;Um~+gA0f?TOEjIp5Q$-e8 zuvO(u<3OOqGmig|m2vWY*8c4{FZ9*SDhthJVR1^J`wF7rWnO*MR#5N2eUZktiamaR zm2J}^yr^Q*f%HT_V_N64_{y>=xaZOxG&&MhdBQo^>+riUUnb#cRtuz}l-^75ulY*+ z0wC>1$n^_<2da@ml`F}xW`Z2R1`%xU;bGaVBAObvYejaHEEDoRl9rFwKx$e9`2K!D zeA2=hbo&jS*9C3#n-yzQtoO!>7tI&*y#QBLXc>Q+$d0l86RD~dU<=(qlR)crm zql^<|g9T#dDwbOQvv4Z4S)$EQZRB9^q8pXHlQ-g-?Lv02nlZ?@Vr3c!MgWO| zH_UcS38$?J3QpsohPac zZc~sypO>#JOa@L+%(yBqEqr$k-ma@|Csxtj(qPLA5rx}BNeDN2dPuXSIPh?Et<^y9 zl&!{9|HYNd3hzpciGa%Ul_fJ$F<`Cn0(i)vgZ4#r9QX*GOS;6UZq%s=qg^%xNS)yx zsp>B(>&a^ZRRgFV6|J=*B3kBo1JYOPEr8e#s`urzjJJdt>II=E!4uFW?mw5#2 z@aY+vU?e*YP)k}^9D}0~6cAYaXm|cSy&oxOocoD-HgbTkpttjh_lXFbHGvDBSS`*l ztEDDoI;|E-?>cV|Y7lmqwWD}&oU9WP)R2Nb(qlS8Yw9fGrA!*Xm|*a| zTLv=JqecL53K?;?Ch@&y*pH77fwW%U(?j`h-j)1VoS0~DqdFO{@*lO+p>e0NdBdeu z=;lQ9i&o+;LVRM znA41M4iaiqJtaFInW;jZZN^fL9w}mWaz|pIWOOxX3xTj2VMYE$vOG~{=#9VdsdF~1 z6`gfN*|qXT-(mgI8vqRBkty&sCi#1UBwq zf{mfTCr06d;f&>CW2e+pWaSYDm6xe&Z=$9#4a^sVaw0aTO~xrIMVRUL;5tY{EqdQg zY%x~hY$IlrN&uM`{;Gb|V|D6;`$u~&J*}+n_S1)=5IgJMX{v@+wpmF$W2G@c(n54& zl5({9%!)(Ch!M1o;>3ZZsMqrz)$zXdxbClPP54bl!vb%FUa7yG|8{HjsZk*6r)R86 zqcDe9OJ#)UNZMCs7&x^E>`HFY{IbXQL?ySPLRG+k6#z3lSL@om3OckGBbE#8mJtLT z{Q|!FRG)Fm_pCWWuRRO)#jD~Mb9K`dsQgs}$$H+ZL@!idMHMjPc;8vzVBmur0Vp%= zTWb{r>BtFst^a@zP*MnE1f<9q@bOOq4hW1yo?}$&PfJ70nkpJ0(0^CfY{aVQvm{

=o_-B*C(($?3rI=t8R`Z%^wwqKLfNpaeRuh(HBf zr~I0e>f0QyO+1q$X2QmtX>+DfS(xdCj8)I}E`q56Z}LKx-d!%>&>aD1tqv>dD;edR zL2_CkK?g9-^NVQszya|pF@E<}TAcZ7)JNK+{eYmhOYINA#X2Xf4lmD~^Ui@1;XpIp z%+gVb@-KPBHN|cUak<{T_tM)s)OaPg+bXlGdt)++GBXhRWIY9UXw*^qB+93)#mrZg#;)8AU+{hohZ?lH&) zeujGd=_Im1{Xiz~WLE*%&!wn8XJ8fzk8gWmYUk4MXXRk46oaZO4%10NFoum-!_Otx z%VwR(yVt3~P+~+%l}71lHwSCe2^hgIn2#iU>Z!bt-&i~xCV9ZP$oIh+PTaXmqgt}~A;QEB?>=fXj zgy5VofEiS$3h-?$%LQD87xvM<3B@!xLK z{2ntc-qyoY5}TM`jDswu8jCEg#&!3`_NTj#y&Qn zHaVFed6X%wi-=XHE2b^jel%gmaE$U1(X}r5-U0*Xi8Cp*kmGk`TzzK8-X1CvAut#% zAd-&mNq5mNn7lMph7xmfFqc9yTacF0;P4$i_J7z*YbI$@}7h)LZrQw&M{ zNJQtUw_+w$8RLMa3yvo$HW1)%6d#b1JIBU| zyuimlSdpx6^z~GSkk&mmQOWQuocIzF^j6~J>WjJ;F5eMjOONRv*-+VYetvyJ@))2C zYo(l5WXf@xqM1BhIJ@dGwcaF4O?c-wNo}d=lyFA?XE~Ya3V+7lCHp1n&?fG9-67 z?s$uo7z0`+*0T~(uHRG-)ZSMf1t6;%EK}@SKzWjfBr!(+eFfPTTZx4NAmZpJ`zK2X z9vuT;!(!WufbaIDwMS8ZYO55eqWuGf%i0MxEDQm;%%?|xu4N?48LBgBHDX$s0iy|+ zZ9d!StX(-C`%_Z#i&x;19*g|LE7uyyDkXkzIUSV711x1=ZtlA&RRWF+nJzPw5MsQDeu-LekXmm8M z4%H-VP{)8FBM|Klufu}p{rLXN3!B0?mc!dLn5gy@jfGOl6lAjMCRsRWUjISdJWYPo zzYW_KKlEL4>xCl!rdb|jlk)n(6Bq#c(~8|H@`}7`QV7j0HatrOi0mqfc*^rOp9EiY z>+R%UdC;?Qg45!D570h?pYroEhxbnY)6M0j`WPds2(Y`cy2pqAA%+NzMqSU!)R$

T!Q8|%2=CqJy1kbZ1qp%#^LqbOr-oZEK ztVU3{ccxw54LSMD0baAmht7N+gJR}gM$hsfSJ&SnFpot5j4hYWrXl6bPsgHTS@#iq zgd)YsVGIJH_?mlDyPd&aQI=%YH>|&O?E;w5Fg6$TDXm0Kw9%7GHrz>B@?j)w@;j-` zIx09!W%#%PuVkI@DmK?CLNbt}44k0bKJ1d-284?1=7}(_H@DT?ZfnH)zvA0#gk^%6b@f2=@o18*1Mcx(N;8q-&g$jbZ zeO*GaS(eHKEr&rk+e*s#-O=7`<`G}ke88!qG7GBS8?0B|l~sF#$!mJSs-ulEge6L2WdoyNrY%f>uIUl1(A&ndKG2Oj9nPfF%QePHDZ z5aNsqt^V-6W4b$lHl@)yhbqI*vos+w4S2oNQuj?JMxJWl^0~B8#44a<{i`nh_-(4@ zJT)t8rmBjuCEvfsvNU5%zaXJh*C~NJF9Agbzxb?-X^BslKA!He6F@Lmql&rU>NA3Q;FXn9YlDTtji7A(yD@)PR!qJM>~ zbQ9P8PddWnV*Ed(BSx10T|@#jHSBOW5Pe>16t>lThi>_>5A%T2@#Squas~EzE03Hz ztD;ppsxLqO!H;#Z(Tqid91@`y9mNl%*>!WJay&$&*hQr>ie)N!G52I)@-ZzUQ!g3& zu)M{7#8+Xg9sM0fljS>7u8#jMmLXf?{!+OvjwH``CDSsk_}MKl#5@3Ybi11xJLpj& z)e9(C>3L$D^zEcGm*?db2PxUp`gki2XqP9?r6e-b{*o76;8+XGL5Ix6eR^y-So26` znrwa2AJ7h~yQYJbS$Az@(4Lb`EZddHk)7plTF*6@8&;O-P2_v*$+vgPPr#`bF6#XD z=~rtKfz_CK1=a#9vbX+V=O2lvoZ|RcwSY6adR4t5FOw4s8ArVSDG3vMd27CMd;yc%sv#28*1( z1`g*%6i)JHHOiZ_>x3`ZZa;9eCXnF?-C>}%jgGI%&X+Kxvg;_aKsWV?1j_5LmakcD8(xe8EZwt&ZD+;nx_~AgGlGk zlzYJopH*<+51PQpQ=4dxz;n0n3Ogr01W4aJs4bG*Z6av-wl;@DmT0pp=VwaC+hSY; z65z;k7vY(I%(WK%&M2_>VoDHs*dELbwcfq|mdDt4{2lD>dhzchZ;6n|%q6xOPi~4d}c+Ca2%88{ICdB932n zcfUyf!}|IS7w>FHjesKxS%7<@``NU4k=@tjO*@QShf$BvTX*o2PDAT$uN$0g?ijC) z_=`uUu^-{qS;t)TPx?DvS}uRh=df z+kPvOwXL_=MmIJod5%H>{cL7vO`m4B%8lUF872_tOrs(ETu!lb0AZ2AB{eCPZ~QGb zu5wmTSec1KX5uZU^o+WU-sxWS`7vBk=@q%ti>wZ`k7zq}TkG*rTQm1*K#5&6KVdl{ z6|Dnk)@JVk`|ynlEv__tfjM{A7z`dEq)oVmXAz-V0UX$L2m9V~E>Z7ptQ~S|6~6Gr z!`hGpe(WM1e>_alt$XQXKQ~4t=(JoxCd!c+bee(o{Iv?WwbAiTUOFJwvi6tm=iHJ@ zljYlbZ&k-TNkkd6AH&-#uED#J^dAweJ%9$l=V|kHwSmA7W(1NB@72n4GS|d(-d1Bb z?=%_)_6b_quZ(`ab+>kGSqG?`J(GW_XOzbn-n>ZFKP0+eH;zkZsT`FxsJNd?B#m@< z2HnF)y<{OTgI10V>wYIiKR6#1AsTbK`7l6wrqGRhDL+x=DUmLzjeAm>QA@~mMW6sk zXr>((umWl`KJfFUyXnEn34T?POq5HVkF*XMc2H64zIJYHZZb&8BtiH$+Q6q6X4zLg+wM#yV8f$p|Y$$aOe+t7c)q9;HcnJVjlzd@W7N2xOYSQkIBqzWI*{WH1RDO z;;Xi0?ipJ5bPN(;&Beit*exiZ)`+3m12*E8U@pRZ+!@xmMxv13LT{~!uD=IhWPOd^tz{8WkCc9ojs?PBMt##bR0qvMvzk;e`2!r?3?okR<+ zGKcXJ|3oV!R$Ib*>!^&5g5*ZH<^7Al9?O&L_Dq9tjCqLDeGgJo?Md`U+v-DCUj1E% z$_F6}gMW(QtLs(BBv7oRDzeh@?RC@Qaiyx=Jb&A=mIVpoEZ)IaP||5F#}}6Fs6iU z8BsxwtbaDMaPiR!XI3CsXflHz9y> z2UYrgdknCDZ%->xZ`aTJ41gs^kaWgaLq|{kIqUo~B9+mxD@R8Ta1Sm}sk20H0GCm` zoE>|}n?aMt4Fmt?7pe~(6h?TP-ojWUK)JuH+D9h$gX=3Y=$G2X?OV&n%L;!=#w|2Z zoswDxqZnx-vbNqd=fZ-HyX|Z9ddSSDK?67jP z%w9;rK!u`WhMB&Lj+B!9aR#t3O-8hui-t}KSr8QAN~QezP=O@6@=J`l%q`Mv&a@`A zt_iH^-}wTMUUB>03f$Av>!uo3fMrcX=%aQA_Nifp;`AhveV75~#!;hwr?d@lo$yqB z#uzH5#Y}?BGPAaqO8i?|u$A+rdTejK zOSKe%l5ZD){lLFAO7PXqkeLh7j^(GMvL2o^S}^m!2oZ1F{7fwxx4&(^b@LuH@WO8x z%uN5ySOcSoD7&kD8ot5Rt-~$B2?L!YmZ}Oaj9S$7YT|U?v^`&DFzS zX8bE8ulO~7ahvXhGnQ#-0*HdV^jPCiccsidN!k0*91ht!Ho?I^*|y-y0Oe!R`ZN9B z`QtP!IgaKCpY%>J1L?e5Z)y8Y8rS*!{cNSZER?@hy=upCanv0-Ec*)Q9&oggVor}X z*XpF=5kO`#@YtNO^e;9_TQ1RG!ZA{5l78rb?%VBoC^5L)ku=W;`ll)@ly#wgowZMb z+>a8LNlcevmP1-TH`As|@Al!jP5?y9k>pGEEhmY>C!K+v;13!m-%Ombcl8|=35js4 zkqYTvRxoT5`Orql5?O*tdP<4+M?8-~G2C4ACX;AxdE-l3IbJ8PN)uK!hT_tf9z0Y9 zGq6DzbMvt&t!woV1z>R=S%bdwjS2RVp6X4(l#}exN`VC0jFmEIFgMH}E-fvc)v&;a zm*6hhBQV<`=&@YeTN@9*K`x>x@Z7;XvU>^ve=ns|_?fJwq^ zs+|95+7Vp6b_H*_(m}H&Q(D86pfP)15=p)xh3k0^ zS@RIG2Vpl&^bO)gaP0*}e7yP$(pE#@{N4WI_yg1If2S}lw**q>1s(QM(!^rOIf_vo zc%*zDG7Z{x2lkkIpXwFL)Tf)xx~9^^`&oXZ<;v9ziEmr=JYy1m*@x9DHYrAde-W;R zN>T(MJ~$}Gy#NF=rarec)ag-LZ2{Hy2WYe|)z>5|D~Qu=6BfZ4tXQ6t2yB(rTc3Hl zbA{roh7ckoK~^m?capxuBULHs!36!#0>CYkuJ$}5!2_<+BEMR%iP>>HYJ%9)61>jMf!oR;G`es^YT~cArA4+!tqG2s`2~~2-BGfh1@HtQS&Vk$$+Fs!bw}krI(5);xw+y^9{0Z=Q?*E zA1+iAJug~}nVe^n_Yck<X(AH_2n*gEM()1WurL+XEKx|7wHBG~X_Y z{cI*dd1vF*R{kzbg=jBm-id4}je6(_1flaR4ZClf=!sPm1WA$k% z)Gsp>UNkG>v6rj0#}-}B=u190(Z5?K$~;(cW_)xqn;J0Rw~v2AULJ}%mo#l)a3OJV zCxn?E$1$x=Vv3L+QKjsu7P1u8E93RcYO3ZF!j^4LR@-i+M-bRD@aeRg_f6GGSpoPZ z!3OFIvT3s!uK8xikiTb`82sk(KF3d>K~+|gN=wGxBlH-$A8P7TTlcj9P7vvA>W9YS z#gzBkBY&AuKN5c#q61-t7MLNvURNFrXkdhEpdOk@u!)o|NTf5-g=UyGT0k?`U76|` z7l7gRzg>_96Y0z6lg^0-U^{&py#vUrO+<&US4$<5aEn{iqBdxF#CX>#Ed3Iof)Pv1 zxehbDkuGaW-$^h7v^h_5Dau{T<%l`w!I9*+S4`j%?&${l`Y6~uSa+`I@G_w)77oSN zYqmPr{mijMnF=n9bTLOb=~9s1V#9?pBdofQG^*`xel^nlef=%mg$&i50dX~HmI_Bm zcNmtZ`UJ&Zn?d{KvKmbAOyjIqBU8K-i5bSmZNiJzj+?=ehkANBZfqCb`KVsk-sX(< zy;y}+kwinkO4>V|gkR;=33XE_n2hZIhnB<1$dMvQ4@~tx;m;AIAB5N?5-ntcJOAZ< zUp41ZD|K0*sVaOtJR)$My#y_|Z+@a)j@@%t#5SZvtP=eoa%R@q*~p6C3ml-E_x(XH zOES<%2{TLEMq5KI@P z6>W430erG-T~2Q30TU%lG8Qxkjl5|z)2OaTm6e$<_ohiR&Du6AC;l^hLj-w{M%eQ|N0ncN!xW^&G$Lx-IZvr40NphjwO8e4eG5 zrrAdf}?Q}>j(6QIA*0>~o z0s}ehT5I^jw9$?j`R>&1+B8+>y&%G60?MiOU@s3r2oXQ5`z z$h2o0Fs3gFGlaVsnA7j-dZ4+>l_CQTE_socaTuUb4^z=1?&RGwWH9XvT9sZ zEWZ9qtb%%}!SJ4{tY>6VSe0DWLU z9r?aMA;q$_>6qR&RND5G+BVeM*2lQ9OzyqSMum69PRDrRfJI9JuN~|NKpT5SYFq9G z${33*bR#<2fdZ*XebV54QxcP$4(6QWwt(v>rPY-`u1$_sH?`h?C!Btw9I&oBAb?KG zRj+ApQilBcjZVI7ks;+|Dr8$usExM;jGDokh7@X>LDZe7T%hH@g}DS1B2PLjeo)qY zz#Ff&JQ{ZlF1fxk+R@ztXeCC$lQ~gQPG4o9ED11am}f;69IK>}dAhK4QdMfN9A?$9 zBigFImP%j9_vDCh8iOU5Aopp-g2X*Ys0hE-kjCkUekn}9kEME-eA~je3vSS(pW(|E zl^FgjX}?Pr1|KYm`E`DMImMGrdsnifuwmZ$nfj|!Ii8?|*1G=_FglDsyTMiC#TsA$Qg{mEMU`PNZ~raJzbH zF;ryf$}bin?MzDp>=wErk9e#ljo2-L=wj(_@9vLSBiK3Y=<%1h$P^>78Lbj2;>b>X z3*J&0@;tde7~k0wKwi6-Q1QMRJnPpEZ6hoZAoB>b9{fzGW|S54w}zCFekjk$)55~$ z!Z*w_aVuNPj{%fWWxk>8yGbEhO@gW+w# zP%oNRPx9s=#0EQqGp1udYIP>Ia|-1itKJk5KYiXLqCFD=uq*#3LiX#Qi*u5$-SW4N zvzkq_Z@X=Y2`^{a{1DBepobD(A>;)i*`z)m>k2tC_T02U&S`6cv#h+Cec0BRoZ?cF z#A4bzk5tH9rI2f)+w5<4{VP8|)kL}qe3)FpnvJ38?82R{(6yXHJ1{>CMj?}Gb#+2Tn11?nf={kZ5l z!{SI8eYP}mat_U^t_gUJs9T7#C(uNEq~+pbv%?_`z#nT<<-_ymY^E^u203dW+PV&< z49QXHH3B z7=;>uU+G!ZP#w%;ZV8rOb{819e!}RIKCr47GM9i?jDxZ;Z`q+ttgSx^6Luf(U>EE` zw$5xdsb51XaHmE(es@{uBUK|Zv?%;6B?3sPnb8DRM;6oalW!*!BokXdn+1o zPs9fX;qcR&A)!lV=izGXyHMDDm zC42uMHIVb$Vuh!n6!UYz*s8qb@KUZ}%F;6aN{9CxSzPYO=8LW~4Ls}e77kv^#gt_o z-Hn!UGKD(~wL1Zgdr@ycY|1|ijbBv&VaZb56YP=F6x1N!YLkoFBpHpnV*g(U9B|ol zZ-u@nB5Umh%)5WClrLxrSeMrpqLgPHh(d>{Kw^0jfqbB_;62?F5SUA{jn;pTOGc75 z2svnT_>y#?ZsMP|;_THhLYA6{!)7_*r#QTu#y(y_{a8h9-YXjASR--;u2KMgh?m+< zJlQD;2w#Gf;~_DV1OZVCesRn^F+_egsF%20VGNVUC8xKDHm1AEEibGDp}js)b17S- z-rE^$W-YtXGS(cietr+=0_GA2#@NxN!9l+0QR#yEATvv@dOuK!AZ_rpQJ+W@&R^ug z%ut-!{O&jVUDB$lMKPXPnGQgcX__*7fa^_gdpDgK_9Mz}sp6+W=wqnI45NcFBKbol zET=eTN@GZOA5>-NZ^{d+#XMV3sNSpyj(ZwTHwXEwuAtw-@z|~lYGb@r#>$Vs3ftmhx#i+QoT_7n}({+QUWTa zZg&tz79ne@ne-iEi-`xNW5S$Rw#`SOFN6is_6|V5pB}k;R^YTenQ}#DRq--5rJ|Zz zCc{`uiMwjPLUuo?m+JwPQdN*^+BTsi>h@?WPJ-3Y=<7(Oe1!;-D(G=c1&rm8p8}O+ z8FXnz&iu7%dcSe@K&`vg))n$2ml9VDalvgw1PO{=B0nY$a?^qX_FlbkC=D?UX5spB zg%nBp9_4`p(mjh68u?%$#=LOs=5fC3p<8sAO=I!D#Su4LOvqu7npJkcfwoVCJO5`` zX8ON8^DInU{|(DD3FU*ofD;m02k`;4Qi*hCyj18=#B0UyGfzAi=OG;#J)fl_La6y7 zw};n6imZx13=~nv7P)J)z-f#h1d374F)a1%W%H)lyR63I!;EWBYYCU3w$cNu96B6H zu<8RN@zHVWK!;Dyxsn+)5hYrFo*WhOV8gSMK{kM};ZynA zR!!sT#y8bl_u%@cZ!fc0V3_ag!Yj}0`~Je%6%VSg)_nOh8a&!zAkLVM3B7d zE-X$_K#U_oQ&E7;FuCU0uVE{EZY8JJhgE6Y2#lZM)#|50r<)f*`Bdi=r?Ld4+}&8#y9#vg8b%uLKM zBv93riajRP{v;c2VW~1ZyHV7HD=TRE`d$MD$%&LmCLZUB?or4m!Un9{2$DH$%QEfa zI3R3JvFXPIWUD@Ra`T$K^TOa`mNjmjlwfY?Z$4nN&PO!N%lrLWFF)<;1Ixz41=?RL zwqNNe>8!l3de>Js)XloR8>-nd{caN2BvBmh3M9$nJe~FL-uT*P!307Uj+_ndOKEMF z7*9vb`GlDlq!l+56ad^+&OsD26&?WP!>bK6}|KJ(Vqj*vyaT7BJX>^KGA>;v7|PKF2NP9xv}|r(nNjm5YPqO0fzFe+UG0 zolc)}MUkL^!~jPrA-RGzJB5Y#JlL&|gX`mos4+_c);wHf5>_C8sldLvKN}hEGn^U& zF+HT14sKsV^jqX`)Sf20pImQG$2%PSt762liHqZeY0Db#`%)0ZM_9Da1y7AQ6ApY4ZAi2LO=I`D~rTpE$te(A%R0a0Lj} zscS8+vHAU@{eZuGJFr&|wL46{H($p=_AgD8Ir`G7a;VI)KP{|7FgrXWI@;z3DvAZbJnDWg6-fUmOMVY+n$dUnvK20>FN~h|n0QMKbdK zAV8Q9USheszsQsj9qncoepljgD333q@-$7*3*)!Wa?+-rh3emo@2N!Yfkr|@h{Ni` zI`sOS#>H96z#}e1=J?Q`d#m;8>Ee5BenHlP_X3XEv~8YiP>x0U)1f%Bkd+ss8z!4z zMl`ybh1R`T1#=?b?CXsX8ybS+M`61nnas$6QCSN!BsYXrZSmyO+G^M8wR_qb@XM`n zY^0P|k5V1--vw-jW3ux5)~C@$*-L!mGDg~l79HgTfZiuglLrwKZAQ^fURb@0*B-`f zjsZ*?)`oTKA1XKh528R_zhHM$rCHvJN9e*UKQaLzFtmaph@%l0J^&oR8%77M4DJ=n ztOKadYzt_CQtE8yty0953QR$ub*#6|v-(iw>#$gClTlyM{&%b857GJsLr*JAt^ua$ z#f^4M))sn)!tzkqAvOxwhBoTIw44aq6i^+e<_jTz&$KDMCM8;#AdOjKQbjMD^}Tjs z>TPW>4Ie zlT5&;%8&c<)2{rJ$S)i6(5|E>Fl|9oilk<61L)G^Gd~yaM7l$5*F`!Uvmeeu7U94X zy#%sXXl zQF(Sz@YAjW&IZ2-f`5>}H8hwD2F1U;a#&v~nHyeqB%3l5n8Lx-$cLYyx)V_2(RWwH z2@Envna+YwxovMHoqah#jl9`!?4c@rUfD^17ThfTfZvRn@s|1iCC|M`ZOv$; zDVuT;W298CT$}>Qe#WrNi{oIT>_eg}iUYWOdD^U=!t<1t59C znmDhkk8$rKW|lz0vzb354hXIc!QFSjc-(cHz;IC9pN(_}#bXd3aeE8#>|G#!aAZVd zM@FHhjMm?xsjC;(|fb`M=oq516{mmTYHwfGu;Owic zs2|E|+X8t5%ynlny{KQe+k9VbR?UI9xo=)fa&wszO?-p#y@I)&Bs-SM;*W!Wuy~BO z{2G!w6r687TfcTW&8J{39hvU+hZ*1_&hh!V8cs}Wj?dO`vEG=%XLeP+7VoWpR~$bH z<=>{pyH=7?wVrkg4ImHd5tvyivkPm@1Ac zxcXu6_v?so1axWY;SQ$Wj*Y5i7kU4ZcUImu2OAvMPi}t(r;5CHs`wYavNJfvzv#Fk zHmVw}RC}rls)pP6t0J8P*y}5AVvx>K_@LZjMT1W-{Zy6@4k@=~*gUidSOzeKWmcaf;(;P!tWN}j2cvCR~daZnTkF*PxhL7pgo*b}LzEC!8R`SpBOv*UcD>jFjAMf+4EeEFnl4R*w6B5EBv}w4**&QtIv~ zh8UrZl%aV*x2BQkQ#FEg1S+TK%JUn>G(45II%)^L2CZOHOV%^0Ko~a){16fzRE)*T z3GdsJU_`X&N}q18{{$OgbiUe_=G|wzjYT+BCBorRy1o6pPIu+Q((vnix!SL?;v%?h zsS%+QAV5P9@bzPVA)vYv2XzD=B_{QPuY*Ft5HoHI-m`_TKx>O;1X?5v)A`@B?J}#B zxW98QF-th8nj6?2N1RvQ|1MqTSF3be(#9s;?^a)YDye>6Qd8G#1_{~74wmB10r5QB z6>9p$Hv12M^&qy}Nr5j`Q4hhYWWtwCOjsShmD1C{|3 zH&lvoNErryN5b0qvZ+IxGx1WK@$6w3P4pf_;#xT`kn4VaWqbXoZ;wiXK z`x3sV043gp%@Epvgg7K|kVQ6;_*-F3Ov0KtX08#ei6N}9N);!;8eh^N+IZpCbPZD_77RM4bmwGIyGH5^#i($G9-`! zQL%t-0-yzFFvC#=C>l1Bv;$+lDf+>G&^3eDmPk?x$`^_}fk#aU+fcu#hDdP$+(Z&Y zIS{F>5hWW0w?Hljiej56hBSyFFabNTQ}U>PbwOC%VOyUzff+LB@njpaXoqZL7UknA zP>w-6qfVuOtIYNT_KQU7dFz$_;>z_gO&zPCtr?*)- ztI(h92RgrA70b^>krn0h{e!LA^YR}kj*8XQy86kkvjg|jn{cC$LT%5OoY*N8Q#ytE z`JF-y;B3h;Ewbx!S2hE&o3Wh@_u&11hw|@rR+fh=m!-I#iOq~Or+(7}lmN!Z)db3F z&9$QJGk*^Et?i_E!#4{uFFP$EU1Etqn0{1~(FR?#|42q#czmpdM<%uK6ynnB-~skH zCi5#>I)C1`yN4qBvHUOHRl0BwzuafrVr8ROS!g4OVes9XZ4STMl>=KWmNTq>iqb+9 zyw70eTd1-&m8G?*tgLMx{FFYfYf5B_@_AZX zvF#^G6ggObOBMGFmw09n0#Hf74laEBD0{yqJdK;R}6Y4xB5jRJFs_c+YOU2t- zME5n|r5`|NvJsht>vastfab13)}ktC-){HWE-Uun1?$I!6wX1a27cLPuY2^O?9sc2 z9vj-xLu_bc6J>oi^mgeW2uTt%&)j5LCMBJzF2Z$vdw}xY@{T^a(iXu#gjn2?YkNF6 z5D(<@WBC?J1cccezs5X5~8@KI)*7*Xd7{SCtWF zU;bU^%XGc2B1*bm-Bvn%SncmTmELCgbylpF7h#vApW4N;{+we2mRsI@|{Zbs28}0Avdn$)n(sG>_*>&O)XZN7$Yd}I? zG$G|GTbHC-^_GlPYr-hfJnkHXK?+RSj1r(Q2yp|>p*X^V4S7d183uB#50rTPlwxvx z{}PCG*3Jxn(9Z=Qa6<`p0y^ys8(-})yc)#zDXWnqSNj@R&4K4|G1JqbfTDwH2)(K^ z%yoVVe5XDAA2gK=cnSQmRlsxY)RwO?Lv`;w(Qp>(?9`4L%0#&tH`U9m7c&+)T(1{6 zD8i5$B!^<*XrJoKB4PCod)XlEUH~14jxEf1Y&m^@xVJ8i0leCV-$a?e#x5+jUCQ&A zz^$&wC4LNF-@+6LLtI--D*7aC%ec1BUrN>i)AsV!H}%81 z2RY(@8_b+6HuhJP!z-RY(-(WonSMP8C9lmLRrf@^pU8-is0owsbNHMHK$tdO@!f-< zEy>JCu}HwTux)}04TYY0Wd#^%E{PEh84=8L>gMPMmcO zBv_yZ*I&BeB7XP(fJ-EiFs-WDmQQKhk9T-~r}R7%!gRQ40H~t!J9iD7bmt)>5QQk$ z_L3!Lp|V5`P;<8|5o4v_DN_n@k_?6%y<8OPWBH3g7*ESzbSS~~X9Ehw?aMY3XA0)9 z2@wP+PdzC|36?q>1CH2HHx-Wxwa*WHN5$EbMD}As?E!8W#H=JB*+4=UGc|B3=2Q)yqwLbARrS-7JiUL6MU`LcYwP}uv)?m=PiCc97NDN_4QS`S#}+v_CX zUE9lFZjOAn%bLSEKF@=fn<1(}`K3;O66jSo&T|}(^ZI0Ao=*+)eEgP_?!FL8S$)h| zX|lKn{Z?;Zg4XZvclnpqCT*`hQlm>63SEF|n%?XDb|L2JZneMLxVu#DK=8hPr}H9w z6T=`ydvBqB|C1Mcsz<5+4)y3Atk7ws^=r%~9$5Epz<39Z31qeQv|772SK8kY@6PbXCeEGu%i5dpkJM2(Rn>x}EU6vXnFHB`_XLM*FGBGkYlR=&-lg`=_e}z#XZIEk$J0M6>pluKYjYWKy( z6rGZbC1R>BF|J%(k~oWro!C59r^!fzD+SA>kTDQ;iYxBmhG39vfSsw>pg3YM ze-%^3KpCqVZ-hq~gOtHWj*c?^C`fX}h8Y_OgVBtIMWX|Eq=c&pl#K!#X-66ym55~p zvV{QdvO-vsQi5Zy;9JOXibh-@zd}EF%nBZ(LNgU|MaeFb(Fk53wE-S8K@9>H+4vMw zor!5nn06v$kFnqlwT%nbP}{ErSIHK~f7R|~qp;n;O|7PEnME^9F>>3i5(dy$3Z)nw z3=wb}+6LpM?udcf05(blp@}en^s(5W+5m5fq*=uFNueq%?^2~=%ex%1*(767@u;%P zWcG&=GZlNixXcW8@i<3=M@=qgh9D3kONNrceAa5aV^AUnz(+378Eorv4Pv&%f56}Z zVRMCYQpiqVX$%4YWikbFlF04=Vle$IK|qme3Q#8ns}eveMQl(pz|PQ(lNxP+I~D7Y zfj1ajVR#KNR07P^@MbzvpktPxC{3m$s8|KSBN$#G4#0*-QtU0m+1adlfAyd1)-_M&^Tle`yu5w4>gNBvoc}RvzFXY9Z*N`) zI3&GkerSGdzJE=agg3M1MSHPwuK`al9h9T5!B8qbLkU#GLu}AJac5_)dFh&eEM6^K z^VEIS{`>ZPb-9@P?CERw?YA@hGftkm*AXrg+zZ$I{L8QA)SA^7Ky-e4e|7a{kPh+z zrw`I_CSRdzn93KxY-~XZ*#kunl4VjK3}I4z6&+m@A;rTnN9kB1bV%cLDfp~A>pfe{ zSDnV6A*4a-^&8LB;%2y;Ac)A_1dhXqN1F*Oh|sl}2qt+mfh(}ykpaqFbu%$PHV?NM zm}s8gEG}NQz=ms{KYiw!e^>3NmAmI}4Y%j#zqhmId+^%MSIaJh<8=eoa&dce(b_Fq z-}+bk{__00#V6On(9o}yfve}|H+UCuZS!h};3amoU~jxwu(#{Z){~Jg#WPtf5O0x> z3*ZYEgfn>|`7ESAl9NE*(gT<1saUQkz@a6?>=UOkPI%YnkOq$Of4Lr=`5gOn2&Ox9 zS=1vekJF{(GXms;bay_74DJOX^C)6w@yz3#l0qWj>9ZZG=RqgE~1z|;5VfZQPQ7x@QAXFm(xQd5E-w(en7AtJmo-_snz!jx$DhL_YC6M@P#a2yT zc`O8%(=n1c_+ZuchLIeh39CzwU?lc)L64C`J7b8+-OfbOe_I!X#sp=EH9>191ZF*o z?>G0tbh0#0&d$0WnkN^wdGoUQ>BW!s|EmwH)%7<`^S-@WowwJ^%d5qF>Hot1*Ej8Q z=`R)^8}voBA!>p&Ur%Nj-3e;adww&@r)my2h2JF@Zs-25%Rer!&3F9b=J&>C&C~Yg z@=xgZ&-U%&f48@*5AECb)AeEwH@ZB(di(MGdb$8g-ugq1!84jsPT6Pc6%%S(%Kr}m zEOtsfE?4yE9w;BS>AHZeR<@HzLqA5$zU%YsopA=xE+V;%&=t6Ocji#MCO#@73>Vb> znqfOV^b$ue32epvow~c9MYeN=b*fS6QOQE{Ub4_Qf3=*@bR0x_bOYtY4vkpRD|wU- zG^jXqlrCB$t4Giw;FC_M6{{ga55c2n>IX{7ZBq~RXj9Ez_8YNsWFuB&w?@!<%G@8V zg}0M61V5`UyqyoBDWg&jr@2q(AFr?4{WFGgWWQ5*9F03P$(qC%N9ke|Z|6k+t4?F6 zx!6J6e|GC`VZ+Yz@}uxm!#;k>RQ!}pZCUh`ILb($0g*r=>Hzp#j%c6l%q}CXhQl@F z>cdB0Pgk^20D5g};qFw5(4eZ7wQA+;K@OI;@0RVwrhb$?NFRA@R_acFl|9kJWDFkJ zaKZ#0$7ls|Sy^6uq2UH<5Yc@=J`f!T!9JWxf47`!GP=tix~oh@cU?i0! zcW4HyhaI{s$xDf&bbz(jDMzxy)!^`9LCgE7ffQsH>h^_V8r1EpiSc3|P1aZ5 ze_Z29p^xM%0Sy2mRbl(iCTR39kI83>Uj+J!78A`lT46ybvlCf@+L8dWY2<4N(J2oo zP=$kqa?eV%4>OUEW`$43Ng$PmvQN2W=t;(MgXH(asARIti$^Ag`@)_uAGr=a6{>mD7$o9#yccJP=qjV_vEhV$ev1N0$m?{v{8O30VI z*ElDp$*AAtM=G1^y2eg?(l44Z#kCh8C8XN@+@wntQi6S_9y`Q7r87iA=C)(}WG$HE zR6KeU=nDPB=#L2QUE;sC3WXb!vCR_!Gm~*p6azFjH%MDXnDI#V=LSR>3ucD2lYnGtq5 zCm7=y`{8m<1)jZK-K>`W^6FCY*)HGguP>>XmAiF*Se2XoOI37&nle_*8My4ZVy^4foXw+N?U86n6BQBOlD5~NzZA6v73 zS?(UO*W3={p42#eQYvH^g8|%w?=I(h_NJnfAsy_Aikj?KEO2TW97$$sB9@)Mq}j#WW`PBTkT#9fKBNTV_fk=O z>nThudANkd5Q(&8wcsdE%?FoMlWcB~;3S%~r><^`wXG$m#c*jr9LQ+dOe@lVdoTic z9j@@(t*gEV-^a7o+LTILuQ zp{M8$K^z7=tL}I!=m?#4G}dl^10S_X#YIQ*oqaN+Zw{-&qw@w5 z*3C!;icNHPUKRPZIYo?sBm>e<1G~V2pJ5jUTwlYDMY-D*`)#>d?)@$frT@(B2eAQ9 z%!<2mVK?S}NPB0(yr8CdXE60q_bi-yuBoiCuQt`;Z8~it?O3}*MCguvq4By z8cIJNV7J}sQ_{-=R#RVplGmY9dseq-f~Mh@v7Wu9dhKBa(I5nH05BYuXY>+cl9w2d za?iydvKQN^4tBg(2m zhxgayAfbyH5HyegTI>iqC1BVc_?NTuRy@y^V%|o!1gymaTp{`dv^!*RWU9q+$R7jB zpAsIGA*r1O>zcVNjm2ObVZ{acbzqi8lC;z~(9`1xxTDChuB0Bv8w26dSaCikrA!fJ z^abJXp8|O}foU&)KwFHlr2i@pfFl?ekbB$O&4c~IL-e__RToQ;?+X88zRB;(8=IrM z`xdy`PghB}B(s|Q;6GOztCkxFW9`!x<2dqN`FpWAtn93QI{*7kQLc-_?(y~ITwsrL z;K%CW7B#y-j$_%om1qUnvao|`Sq;26l)JtEX|-9d78SmKv;?`oyv3VYj0o!X#|3xxxmaKP@*B>U@FTDsad`g0k*+hw5O#&W zyX@V?zc0RbCD7OjK~ke3-Q<9Lm5>3m!1?jo%yc*3S72lsQA|$Ao?$?h_98@ufbe;4 zM7>b;B#Mg1iYKfG>BfsE>^P#6qoL!g3DbOXnC4%9C`^+-4!9myqa@yBxUq#nxzb5; z6<?LEFj{9x!mSijvulN zSLNlHy3Y=?`k6mF!zQiM1!?vp$61Z)#eH)!!&3rDVo!gB(+Edh7rUxMABQLH_uza7 z5EcV}PybW8@+k-+?UBKhbE%}KRG?g8jp%S&W2pDV0lS?&}FYu=gL^a?o)k^pIX z?t#Qf{qto38Wgm?UlO?JLhJO=oXLI;n8gC=+G_H9fd7Z~2ORO^+I| zRc%jZgiCXRGlKd|`4o5!+R1T6vfCFC&`vUc16$M)d&7va8WAb7sJ@PIf?*P_8sAIO zSWklt!&3b=UC@Ui>w#*Oz6o|-*`GUy6}Zh-TvhpXGKv_O+n(Z5ft2pLMU#jjASP|b zgRuaa3n$Q;a)^C$BKu^AeO-H(l*cC>r#Nksu%~#Jsk8*5uLRR%j6;kc3r<`j>FUUT z!@&sxjy)=6;Ccm{l$5}%LYY)jLWYx)A$dh^=*NMRYGqXH`i_X+95)=p^vQ-ZAo+Hv zGf2K8_&$~*jD~w0i|WZ}A(PQUVKx#8A{adCj`$gv%EI`0(h2EEj(YMb1L;L4mBp0D z_4ynjOa#7wrNOuY9khKdOe3tsaYrhDCmNHSYD`~P>?R@hzA1GOJ}^rXlTibr1?D}^ zi3*Oi&7HJ_lv11v;g72~l?YQg4dIJZ5x(r?%{UOJa(irLb(ORj37X3zkxsFgQ|5~y zv}@%Efp@xWO%OV!(5v0N<<#j?SpDNPsH^-@~s zDaH7v;`}AvRX3U{Y{DQhp;&u6rMfy;F)`hs94;PEwdkxp(Mv}mc93w<)BEW^dop`yt{obG4f)&}*>?T!L_ z;e)CUfJU}S(5Hk85wNDPK8Oe0t&CvNHv@LnDA8kjDy*kW#ZH>)b7CqV0((<+uxNKz zwg2G*F%bNZkuQ`{_Ul_wMRq&TNlC#SblN|h<{MdQzNxzs+&x!+aE4EBlK$tQ*!;?( zFYV6$ll~W=HkG3dzK|zy(y+k{n$G66Ik1=@DjzN3U8?St(wcG z*`};a;QD^kl(KDqS3N)<)_ITMdRJDp;5H$GFQht?`Tns}Fkjr}^@+~z{;qiCFCU7a z9mOB_gc(iB&=`1a?QjL5s&`I_JIBFoB6?n2-hX}N=-Vsm#Quw3g+mLsb;1mic)Y*? z(80|&Sl}?)0b@czv_wp+62qk;t~@~_-R%j8a7%_wull@y9Ww9J;QoiFcK_HDamf8A za1fw|H=g0{A3*`1u>=bTG_kw5)_frX7%_hQ9u0Om`3UIaNbf0y7*Eu3ARZk{j45YRA|Kbzppt%O^&A;y?EJ7<&{bc2ie?)4q`y^X1Yn*)|N%SxeP6>$g-2>Gc0f z2nUkwd%4@h82+8rROa8(NW?@FLKk_JS?70q`wlrbv-fpZ+xr6~pnCd3V3w1f6%hIz5c;G^GXU zzuW%|vdjIuJZ>Ll^~Uo3-C?!8d-PNftHW*GNV?vZo5KHl6ZplB%N2=F@Y`il32sxU z@qa`>neE)L9dO2Z4G)`5t)Bk_z05YVld;Vc7&I^-Fd%PYY6?6&3NK7$ZfA68ATlyC zGncVt0V;o6kJ~mDexF~VM;oZPyo%%keMr$3+X9Q-O$Ymu&4V)5OtctVQ^}J_{(V1Q zWQ!8*agu2s77GL!nWD*a4!`rA3;o0O)n9(rs$h}`S@Gifw$QxL)DS9FalI+tte7Bt zT{24P>X+THuXp&@bdSTj55D;qNK*ke#Ps<97AkX zNjJq1R#k=5Da_fqIS_e#Co-d?Vx|zp$T;n`@3uj{I*9wO+k6TGw|%{B za$!Z1g^C zfy0(HbH-~b{8e|YO1`cG3-tKo!?sQC-!~T zWRg@u6_8xps2!y&2Qz3_S+Cl!F=>!=;OQ!PY2p!xs!n386t7ArNAJKhgR=4Gt9P2 zB94r7&i9zIgjW0_=M#2fn1D-jsrL!*ed;3bJBsZc?yr731Plgwg2Rayb~v+8>`!qx zJvto1A3F(QRuP4b=_Sr&z7E6H0>giFhGFY5TqUwBBcpV-W?a84Q(UNAnySEE1U@*4 zfEdjV35$pc%Re87_qS{AGLEOZ-$YmGZo}Wd*AEZv?ru1jwnP-aieG(-u)V1xkQ)~{ zKR)!$FwBtsYV+T&-R;ANzHw6;xa={)9DJeaw=>Ct3x}P$uHUq~dfyD+r80jcq1&rq zs}x%l?o$Di6=7_QTa?QzH!#A1_|hV^iWx$N>0AmSCb2n-=@MS6T&4gbHxW%AGK(o9 zs97$SDq)E+5Yz&92LN-G4;xl#udEq4w8Kb9h1+iETRw)hQNJMI!581QhgbThDAs0#oWkdp7rNSwW z3Bcc)-Axnys2+W~-M`No2O)(Xj^a01X7!KUc|dAS4~hPf=~H=V8aVJI8rXDb3X3&qFt~M-K93hfChu?Zf@2$&4A9ZJNGyYkqM5fy2UcEE)oF z9r$^I7z5L@gJ~d%^n?Z4?1v_&9^AxodDJf(@GBQIU2v%gKV+;J%_CfxMUKcaA!uZw zi*vWDpRTWdzj7rPHbj5QqM#}=4CE{nJan_Ydh?bR8@&9rphQB4CqH#tV1}aZd-ug} zSO2>D!I$8PiX7RWLWn~U1WDi8*Bn`$5|!`_Z|wv)UEG;7}^Q z+GHZP>k62ZWUg~!S${;6S*N8^OQ~kwAL{bYWp4J8)UCc5b<2NCsat+Ib!U7Qyoq3o zw5yaB$y)^9O}7OtWL;LmgshtQcwn7PA}|0fZbr+n{4&--_BiX*QY=}E=TR_QYb5&_rBq-grVD{U zWUM(yDZ--c(r14uMPpZ>Q&b$$%ABB-M4O|FFHDoVB{mnnG%_ajaeA70&XS4*mFhWV zgMqax6C4*a-yrbcT5$Gpox;d5W@`920wFeJ-i1#CpSjkNYmO+*Kf{T5&KwT-ykL$sj+J%> z`Ah=>R3d*}GYH`F9P*V5?&pBdjPZ>oF2QPAz9gq3n^aE04DkQdbW)K@iRYLfM*c$H z2gb&Z4$nyei@O*JCslJm9G91&;Rru9?2HK=>}}!pl^1p+7A>08*6>P^bZ9Oz_v>sk zRM2V7lY>2VHnWV)tu@&PQIvDGH{*XXO|(?g$dT_Y*$V5$R)OW`Rg}|4 z_8A?|1OU87*qf+$GV$lL#DB*Ag3D~UVKaluS@g`Gu&acQw|%E}6wmv4q^;wuQ$-1L zU0*5m`J%-5I&U)Z6e99z37fT5EK0ZwtZ_NsKN}1$i1&G4)1rn2&01!kYh)p1H^Tz7hV<9<^hjr(zdzcCnZZ)Ci^dEM;$`eyPwkc)m7H-?EWwr;S{x{IzaVJKvQe+y#H(@8i(!?%bn{?O2TM`^P2>#rvmj2M-Uw zzQF5k{SY2}9~ZOQA$)dioX2nnRp0x5FJ5lDJ}%q%yD~@aH*MpuuH0T-n6_z#_AcJf zNkfd}X-lkwbp5&%+^+h`!Lb!tmr7QvSK(c(NXo9?gz08gKRoo^$95a<#@sBose3%- zHFg6FJ>8(!(@ooS%@Brm-98Nc{@Cv0cW)l!SPK#?g|xo)9|q!mv+>uvrDNKW7y`37 zSN;R^dy3GLvCR{c{O}Y6F*rFhm$795Dt}9J8#fHT>sRbi8dnSnzB`jkW|C=g>D1QE z^+Bd89M2cC`_ULQA5PN`HM*L7dH2bQ>i3jo%BhH_-};ktNTur91!rFQTz^WX z`fjJ2b^Me*Ki%16^-SA^3XljCrfi5?wIkiE> zj4bviDK(mTYb$ERPBp($+a4RooPlTzmKkkQ$qA*)Gv#HjYd#Ws_;eTKBH+w|i6pba zW!nLfpUTx1115!v#ud>g>Jo)z(tlQLBn4@4kA{R?YC;*wF<}T>*YyM=xm@TorN-bw zB9}NL0VNq2+1mz^v_Wl6#IbW1+Pj za>^`HGmT&b$$V};w2K~z@Igk3u|VaLOl(ExMuUge}R-8L1?R$C<=A4LO5;E=2}8T|iC=BWTE3$A19X5DmE|p3%WU zYs`V1fy{E}Z>tgL;p05~d~pzU zrBBoJV?E!jr`MZ!m3{5&bNc!HK3@KnNiM@`&$Rq(9`0;8pQXQkTdgUJO3RrmQ&K`1 zfA64-miOX}ZYO7SeSbJ(A(YuMUS2Q0&+zQo@6-E~jVO-X&x(4C`8r>umohRsRlrNb z@!{rXo<3w8a_qO#PGtLxyV*&&Y{rGni>NmKG)8LoLqXhhJgie>4aFTqRx@rrL1Znb zt%&r*aA&Q8AG6KVX-XtG9k@U`4hB3WY0E;I)7yw#2=##0>^BFcm%uLz9u3~ zC%T;wFI}`&7^JP2(&H019DJM%{uzYhqtzlnrW?v1g=LUIYIc1)xV+-g4FCBB(}}`$ z9~05r5I|&tm_>9k_+#*)-RlH{oF8jeB_W{<<)W=z$(c-w1>n%^3^~Pk(#Z47fOvzQ zA=$4pB!6D1GbCQWH3a?Zci6Kvqz`Bfk)@+IqJC|(a{Qi-tg zR%gg7kWODHba5p7zU~XY5!5ut!y@f4ss{?79}3=F7w4b`;Wl<^2zL7REY#Oe%v`?h93cO>SWVni(7F#BhXT=e5Q?%QhKm*qldW z%b2;XB|NujF8mH1v)B2Gm$S{-6Kf(nl09}{;o zxPKszbaZqHBPEZu3C3P!Sr`dv$fTlIh7bYXiAdTQK zL0=*HlSV()H~Oi=Tn*4ajMbP`F-Vw0Jgv~AFq$a)gItY{e)))NOZbDP!96vtfdmq{ zBwf*KCZw-n@PL_3`JT(}S=uEzrl=;7&VP{Y%8eutuS_Xi<8a!!=B z@+GfIWX*4L_%Pyxt#4@^HH`R=$D?TM&@{ST1S_EojT>*1ckz-wJ@g;H9MdJBj==8m z3!G#iw#WFz^&7wZpPYY|oFUCrP4n9b%?e2t(#&EsbEFjwiGJo!MLBxb zBXUvffH2GTnlgXCU7pYO+w3NP9^>vz5QG)6Qc0y4=Q1-S!No#0E7@hUgzsm;vKM|C z>2L^MLESuqZexT=q(G{E#WuEy<00;UP}36xMv~}r+-cK8}>Gmis?X2S5F(g$-&4s&YQIW#bZW1@D% zhmtPi9zs(HA3*V$LPC4R|9x4kjFXUZvMG(-%hX~kfBgMx;lH@sm4~e{trSo0bGPOu z+qf?mE{gO^-KQzUwt$&mw&e!Eq{vq8%XQZ5vMemb!G{)k;1_qrlnNiU`K=58{HG@< zuh4YHgr2w!)5-fb?hS-Z6vDcRp#MGu8jV7H^h9NzlKUJ2H3PacZhR?(q7XE5?xW!B zC}^w0f774;{o7mr*#Vq|yEgpw>8pGGv^o6YQ-GbnEc}!0t87ZK|KmAakl@L(si--R=)4UmUJu`p+;ygf;5%0W%!Cu0~WfYL)>R*M`>r%%@yTJgL{e8O~?BilXh} z7Pa@6fge}GlMFTtPCiqvTjVZVVS3BF9(SszCb1#&p>fkd3a3m)Tz?#1++P;-T}&J7<% zD9A@L>po0mU-N>*z*QqAjhhVEwjEA4ya@g_;EN19>L{pA*m_bDpv*s9Na}1MvA>9r ze|oG1tHav(E-oKrUkuRdQ9pasOAFa^TWDiOw&0}(gttM?yqvVbptu_92(y{J0)*S65M*f1k9`f2V*FvwEt)0%;R?e~X9sHsy9Js+wf~ zyxo=SXbA~qB~RpS;Bta(<5>f3PX2r{0*F^zC^!TLAOop2Tvc^0+J`oV)dgYPs=SLi zP@qW==RnhughPU&Cc&c5!?lMVNgai!CXXYD36@x}QP<3=#=rv#=`@Gcbkxi#e^NG~ z49vXCgl03wK*MPd=DXB6n8zbGGzlyyAjk$ch#_!1{15c-b1aVP_$G*Fquao%;^V4m z6yZ3`=-teP8Dm!iaxibn5oDlLCC5|a84v?0i?$k!h1cNwqFP4v0kWi4MTb~U0%cs7U|7&?uryD<)#^ z;faTTm(#(E&FB{1+4v+T+ZX$2_y<|DZY2IP!q?QYB2q_*RQ-cRx8-ike?1LWF$o6n z7xXVx#%$^oz3xD3i`|nM7#d{{Vrnet4pzN@gDI&q=N{((b4BCQKSeI+NWrDSj&+PnLWUiV_pzOgR?^n#%m^V9Tt6aPLKr35;>yQ}f2q11Tic(<)w6ah zyu}J{&`m$s}#elV0HPqv7 zqNelG-t!xpN1~Qwbc)E72E?V4PYoE;S2g<9$z8t-70Z#ft89~>$loYvyTIfL61byu zDW1x5&{k8s>Zh{Ir?PMJYM0sZsB>r17{$j8NAux2n-ZYaf4(2~rb24oOjW|Z*x1Uv z?28TH4`XTjk!k8Nq9^Ne=TE4X#)!<@z5isB=L@&W=BnnSa$6Pa;#+3O2hSzoyLX4J zjqB^5`jT)M{w#e5&=g#KhtoX<$6KU8ipWuj(uj!@N=ra0?9r1P+D#AylN^?b?Jd}> zTT4z-Q$$yff5e`?i5j&#|F(}T3()UY7GO$Fe)_a8R*rWAEpC)5bwP*Q?N#Ng)!w}x z{PW)Hn{^Q8WnZkftFK;si-j9_;;F5uJmvHe+yILcMSQT3@d++Fyh7T>_QJbG0oq^? zhCtF^vv!xe%(M8GcSD<|4dTlJfS9cV9W+3~m9^ioe;z}pQBn4h+D*|S*YKY=Qv~(% zewlf?mW>m);&Y@%w1T9@a?h;bK6b$6(cL2ku*KC9c2z(|=1##71FkwvtX_wn9yJ5J*4^G!*TFPDaby+=`Z&8lZs%`reVqXU0aCK3DbV6zC@N#DU?uWKh>vJ>%2vPdHbhKxfg zEx91A_0{9G%E&Q`tn|F!J4e_8%29r0n-)8Pj8Mj=Du+lYSI+Hk$JbXK@R0|EduzgF zbE7TA+7?}vxcd`h5!mA2)IMro6$j8tx9HVo7cppPkf4xCj2~I)naV#N0uYcfN4;(X zK?m4|+%~BQjdMyrxVQZH@BVNQTx^E_PQY<(ihrg_aQhW3rFttlNVT#fS93fYOlx9O zx*-RgDmh9{^hTl@BQL(>v)78}?Oa~I!X+|*76&Yn`sE~hXu+JAN6-iYCE$W{3H1k< zYsxhzQ*XEhL`fU=@*AdQUhThV351<3{SOxK53(DN8nE=k?B3u&>V4O^p^_6tyO!kb z@tmI~E901xMU+Np7Td5jQ!YH@dVSWrAqN2=|JBMjHJn5awR+OF!{Eu^_4bNLPV&>b z4w7ivkeZV+ieYgaWQ|Oh4&MiKo$Qe=H9S?Wap}4nGKU`pJdaeh*a0o2Q zP$v8Ny6){Q(If*%seHy#$a;X=Oq9(J{bvB7$`Dla{`S1XBM<%i1(bv)4p*)AolX-N zugosJh77Ar>#hK`v<_Xj3qrR9;3eUh#gLHZ3%!rGDeTRyJHl2X>Y|D~zH2o1QzqWwly2@A2ryS3mx zGFC=wqdN4ZNHDB?9NSTWe^Kr7OD_Eegl)=^RTalTLlUxwlM!>X+tP2;tQ2_8O0oEJ z;Ze;S`d_Zgc8*hOr4INM$j= zA^2840x%_>l&DT}+MBk?bciA;fkc5!Q56!KG4N}mVeAQP#Dmr@SRT8sgq|YO#JPHL zHokBX((zH$#7C+IlNQDJoS3K!lD8UCw(#Fl*{kZ!6v0WpbHzeD3Iv>1vxOK)4_GoW za;tvv<3$9p1vQf0wuWxE@iskx>L6keVO0dUj+z5V=q&$rVC)X~TjlaJ_vd7cT z%zH(kMj><)pHN}%a3rJ$G{B-vjKNJx-j;Jc+4?e#t|VAj0PBhg z7|9+)H6%(6=)a~{Ki)lWP5K49^LzANs;Sd{{Cc5&NfD7uaw1ZIqqRfdz@&Yjamz?0 zc3nF!RXunap6xK=RX&nZb@nV+D-dyW@rj)g5-=05TKt8(xvd}RMkX_8b>WrbFoI?& zb3c}X0NO_Jz851{y!C? zk5e*#Y4mW5IB=x553=LbV??M#JHV z|0lmpdbHf9TN3C&z3+rSbGblhgh~)To*nlaBDz=0U=rejW@J5eWX;SSyRFuN+e})* zi6ToC!;p5d?O;6)r!yRQ45|l8JI#We_Hy^qXizs5A<`uPEX8fvzf~&xYy2y4>f(8+ zx+Cs-BfmEb*yajr-VSTYdujk_Qd^bv>tm95UhUb-;O6s z?ZBzZ>@K@#q7B0ca0cd06dAd)VLq!@w+(T$_${=ef{(Me28fV8FJ>hqk|m;I$%;)V zciJi;Z;NAqi$(J?qTk;=y!I6c6pkdXnD)1%TqnP`&FBQBz$b?0STI>~Ufki3v+=Tg zXS}&B*+=Ra0e*`6642Bfc4A{Fx9V4H6P)EZ(M)%Y)?417hTrQip3DCBVoolLE;(bu zIy$`goa}<;)-0yh4lMn}sLfS4me6H}+;xfgxw^UlB1vEOf9O=BHu?AnoNKCi{}2SD zsg@54o)*VE;F~dD6)kIYdKLPQ*F9}nIr9r4SE zZL|UGgRPV@(8*uo8>_}L0jDm{zFOU?j`H(I<Teem~JpLZ$Q6avA$JPA1 zss5%Zy#_$4)$FOp%eNf3woAV&#p8p2)dq(w_T+m5=q;*p^%({lp<_n`-D9NvyX}jn zG^JVCN_RKJSexIFY-3=x$YdJ|bX)kGh-3NcVnY48pD!5Y36Rw zH;qjww6ybkc4XLg5N`v{7zmo*(wI-cq3(r#a{~X{upe#(0@^+=1Vr4)hs*4${$4P5 zy#V@u|91|iWMnE-5Ekxap8c4A;85OIohNn&75z~oSn<4?UHI=f3;0ux&u%B7)w zjlo}QfVM#!HRkp%yi^Ohr8;{l3#Pl9zB9eBd=vhC`_tcGonyF^_M%en)BTtf0w~J;W#Rq zMriiZUszNjT1NI$r|MVt?{Pm*cX*Y{9H@>oP3%t5plGlnQTkF)#7Hw0es=dtkJ~a7 z!0wo;wDmwdCsP`NJMv`^XimI?k4V#7t!*su#>IgpMIfJ2v3fX3Hi+lrgN3$8pe(Wi zToEl>31?oYaJzV} z-aBysUk{3G!&OUJ3$|K?AZs$v@Ij7%e=B~*zmZ4v+tU+75Cz)%TXc`@bR7P6)3*NG zlCd&n)!2%nBmBU$|EJ)lrmB~DZAi`}o6foC*6Y_*Xq>5`=YLIz9axUeeEpHk;E!5( z!vP1(-_3r2Hua7vXz+LJ&RzWWu0uv{!rcl(y=5OuAS~>0`f;w4^NgGboy58x@HL|e zRAvGe9C5}5df*UU>Wa$xcJ}v^Y|E$4zTGk^6O!p6f zIslPM5am&5JZo~ibl-)H(5pEfkRV^@-`?TvVn!CS-G>53JsYElgvD%NOHk+k3eVVj zgYtpt^zt>#;iu~uRGs7zy`mv#;AfGjY6}suq$+BFy&8Pe(79bAW{QSv-i-wo$twQa z4U1&68k+r*0~&#a#KJr!UQ;151mskDa*C35UK+`oCp$!aVcaxDMj|a8fCWu`qDcUP zgz7v0Km(e&s#SUd;#7%R8hSGx(FQSAZl)t3 za0Mgkm$_}iS*g0#fvU+$r^ z^D2{v$iEdd8T{Ln|^^W9aOKB|4tf`k5vx$ z<|TwfwrMzjZmh=vutSN-7oM53+=xTm)P2|E-cc3NOsb~pcK@P=(!{)fiY&yzjf_iF zZuAtYxZc5zNQ6a3ch9BLS4`DM1l20`M3xpI#K9uQf*~j$BA+pj)6K#(b-?X7X-7ON z1!_v9OB03#P?XKO?qmYd!?MUt1qAgua^z^I-;sP7vOXl#0 zge;Vt$L-Tbk_&Uqq3dIjn&G=^LQ_%=W2)eWr3H1!PaZjY2RRa7I+)AAkzzG(%?Uyl zm?9Oi=xYj@E#nuNzJv1bQ>m6@IzhHxCk>#Ta*9L)#431(NO^@2c!dBFQV_K#r7sAf zkkoS{?cssr)kZRxD-lhkr@n}t1t^#(X#NgX-ziQEEio@$pHU!XA8|#g$n@UKETGnp z+)S|!bj;EJM3T+Osz!PAgP)&Na-CKnMii))`3<3OKq$=kJ~day8U=M=k9N?SB>tW9 z#aU|(una3!Ev*Lj9l+fubR$akM;+3C42^w9-#u-Nx;?%n8HJiL1u8#e8_Mp`TjlI8 z2ulnEiHNbm62vJde7efp;13}z^x3^?d$sGJ-I!kHi1`S!JI+QBIBK^j`d3DOZIlcA zM~A@(SGil4H><%1!J8_JBIZl^pL<}lcZWc>&Z~I*}WnMX|vEpYPH*tLo+=#)9A_mcNU3^(|cK3ie-K-iJoZ z`_z^~*SJS0J&V-B=pAA~Pd*>55kz%l!5m^({h|8#Ay-?p9P&Rh@t*Xx>xs@f++)Tuh^PVZy7ny`stNv`F1_Lc0!fV7}h#so9$ni;)NJBm(x5C7Slm zsKMmV5kR@^v+$U&c6Kw8>9{n&gaAVHkXA3O&j!nn7Fx$_kFVgouP=qex__J3(S&CW zV;d(FUh*T-MU8vAj%|c^_HV3C$H%q6*s@}3)|q9`uq?A%{!LcNWWSHwiLeyG5h>0x zED*Bcv_E}XILMd8;yFy4$|oXO?Ss`R;H}PDpk2<pdBAy4`K z9*NuH1!5#?7EvtB;9*#1gas)Vsun<@_0Cdp;c?z zE5uFpxqX!&K)pusnd^?V#35}RXN9JZeToB_Z13{jQk6)>R! z_>gAGI@7RfBS+x#Jr7OCoG6e9-Qiyf z4%+Fc)}*&|!y3HKnC$&U*LvHo?9>KOWvwH=bhfTfq&#2RsKgGz*&WP?O?^z_y8%F#=awgzeKAZUj%EZ7 zutCV0sV8!4+L?ZhS&zyL1uql>Q1+L63UJVkOTZ{97Kz=`4w_omwA9{Q(6~zXz?=#! z+Uagw?b648{nyW*T0ng=<-u`!+DSw^-(OBIF}7n%Bhc5Am+QYpnP1b3{8pEsnn z`8rg!acTfw)gr-_B2fA7uX90gtc8{Ib&+Aw;BLEy)MsBINUn3ZL)44oY$sPmfHrOS`J4 z|9%MW$t~+Z5O}y}x~Z>GHp$X{bJ+Dx%lyer`+TIrla&Fv=mh`n1A%1Oq&$#fSwPYY zkIfbc6@j}C+b?^;i)n*hYM62q%eb@)9pYkJ{d=J{0Hser(8v;90~m+fckXtQdm{rQ zt=#aNTcr~+?qnCNMd|rK+7R56GIE-nH~>8jix@se9;5P$L&TYj+dK0|-QXIQQe-{m z#{L_NTc**UThrw05q(YmH^ft>FDb|02zR5$>$FCZbn*?w5h5M2d7qS(-Wet5XMNn5fAo6qFbSUPA?{zA6h|Hesp`SVd!iV4;=6IR6F~v8d!f0cqp!xyh2H zflKv=;6#QssVC#~4r~5SHB*wmYZ9myDh>r`Vwz>D#Si-@1Fo+>t42w7R@72X28hHh zt7e^2A?1H-c+sp|*_V}Cz5bT;NJdJa1I3az6rc}+F?BLA)12v7@WT)s9)98GXdv*H zpCleej-SHvS?u6`iTHD}JCQj&qtwI{;a>JjCrSS7Gpx_MkE8o)^f*yU*?O*o+^6jq zS~M+CyJjR7E&!(Ou_eFM3L9crujsFW8Txi01SpT1TdP&BBt#Q1;T1jTSFzj7uaE2S zZvPun8-le?gdNBx>c4oKJiZzsku}QvYBli4QBpJ1r(ff68QmoR48`h+fVu2sW%Hf1 z4Gv$*hKg$JLDq!$SZH$7=wOoI_7Fg(!=Y5Lr1>QpI{;YNP<2J?vRFfqu9vZO+grMy z?|*3+COe3dxO-t|Y2_x3d%E5puo*dS4&K2f@VHzZi@QRb>$W>zoQXsW z!HEjAp@;))N-2jz6npu^)Q4-}ca2am7uZ7z3ZynchoVIRMQtSO-d&mQ`VE|O;voa$$pFd?*5 z1j3%`lxl%=xN=VJk6aXe+Qq>UWI@ZMQJMbe>b`RH>Lc3x8F8FyZv+*fchu?GJqG9N zXa$szu}iLt5LdoXI|a9rfcx@cJaZ?1p==0}B4K8Ht5ELTr@Iv24;+90oN#0Sqkt#C z60-3wb69%BTl z;Fr)Cw$UwB-Z~T|YWksOBGO|kaPX^3`2ikdStNTDF}amWSq^i5UoPL|E+4?| zY~t*O;R?ZI4R~gx4#3w5hN)9f?+2%$BZBFcbK_v6z)YJciQ;~Q8)v%W9lCj-xUcMW8*1mSy5uXtB^vs8fHT0B%bh;3UJL>R&AIN}>T`B%&7)U;F4c4zB{` zhohJN>1*0nRT+fKXZE~H@K{f=zFwQ zfZibJq6dSH%2_OnFx}3#AIGfMFF9l8;IYZF*d9&2l3vsI zA=98bEEQ(?7+F#JyaUt=6fR+B)|2^muiLyFaqRoHhpTj!OwAjGST{gSLI9>Bxd6FB zkp1@fUfW!7M#Ad9taQY#ULhgvRq7ia%jqn1)j6w@4utNe@#fzvh-SC0-@6s%fRl-X zi%zO4u8&}TIPJlc#{(H%L~gUDl0T7FqiwVBxbASp-V&?Rr(1BnMTgww}{7%~Ft?~!p3FMw`2N{y#)91(5 z^2W-4Ef9kq2RKn`Bs&NYQy2qqkKdd=_U7>yT!TYWjP&23`XVVkb^oHAPk5-)=pJ_( z3_iZ|E1B*r&Jtf*%{#VUM%|QBvVHTGGs`-gG*|~HV7>)?p(IjMtpFLc=a@;Wi413? z2uZ9v5|=a#)9+gM;9{1>DWa1sWJ4b2!h4x|-r+@mdKs5oF9`Q3OjpjALQ5y1j14Lw zW-Vc@#ZgIzyPm~FLc}Gu-Wlh_miImD%-Z9^Z=b|Ir}tx6CQ>Y4=A898*5TGd6{5_S zjS5xBTcd!tA?C@4Z~|LE{pC64nM+_goX!C)(L0xoEIjgHgj{weT`kk z%v{}-W0LjXtk`bq&htkGpP`KNLG7O{ra8d5yX)exNysZNmOY;G5MI@2E;mQ7ka$OC za2xLiEMn6ABoY54r71y3P+addE(||=u>O5Oqffq~YGeg?^a5V`vXekPtiWa;>>pe3 z0lk}iW?vT9EU$NUr~G#g_LuCT4;vU52dhgOA)2jipc#4Tj1z@j!hKmjN^`5CL{q0P z)_>cyifK=q?6&gj)F*lCYA%sJ8_Ct5=dqiXor`U9S<5s%LgmvV)7V{LB+MOh{_ zlPmjAO%+Dc9s$h!sBuQ3kp9pVHFsMFPuYYkQ(51?=Igk{z5_=zAbtMua(7S>66j(f zTWfLuoxPyK6!K}vr-U7Vp(gLtv2v1X1wSQih~H8ZNmzDv)VO`oCe(W<&q~@6?VXYc z=f@ADnAG>(Es~${Af8E#<^Z_uQn4UK_c~`pSd?f^KrwoG zcE;LsI*4+}n;e&t0<0SvfWDM8TM5*ax#m!g8YZ_pr`k5ks~Y7Cj;?znw$*}5Vyxtr z21z)CH6pHxY9}U_m30d+{m+PT_|z`KZC5TyePNSrKTfBGnLieNReJ(0?9$rScphz9 zI28iBg8`$Vp?<}oILF*8Pc~<)5%jB7OI-9>DU#u`69^z9r$%vxd7^QKe_YD9zbn)A zn~T%dD7X+j+h!VXZ0_COPWMLSS7jGZw4w6f8ZUi9rLxqHpAp@ZkFqc2@BqL8?6~q? z_|(w^;+&6$11TrU&(vz^fZ{%M^O-@^M(@w~cI9wN{W`O=)H8o|iL(@Qn z$INhM?9!&_T2L>M5j||)=J|&VCvhpuFeZIxg|;w#3GnyI3(##)Q_(l!@oqA%PRRMQ z*>zeCLNX0;JlGx1`dPI<4Eh>C{wcE6T7@+r*gg3g?zq!io!>*B~yErB%Z7sBnr94!c#iOvf1^ zqIowumpIsWto)#vMl@@eZ}lxFsCvHxX!7S|xoN2NY!F7fcr_k?M*Sk9STu&0*zXQ( z>b$?n%hLd3r2W2;X%Wm-`0C~10I?T%S8xb$J|U#ParmgwtM2lmir*x?bqm2S) zB1tntK_*UQgKIvF0P&aa5GAE1l@4n@lf4m#(1-D8NM(Z-Cu31=Lm^};9d}{^oLj7y zq-e2%BLFAR3}t52%*W>pv!4=oJmZFe$AmQ7+YZi0$sjO#R|t7!s(pcCaFt+4Cb==8 zQMUluS6UTgO3PV178dA#cSoFFO}-{g9E_a;BLsO$a@K=Wcdb*2TjSWEd!a|wHPcV6 zx`UDaz~QBii`pqVg3o zxMzMWn^2cUsw`x&gvBH>FtaXHyWZD3D2L6oJGyL81bPDm(x)a06KOt5YNv<2cFZ$5 z@X>`yypzL_;2@rYLSj;XOJSlwv*yt4rcDIeWLUz%Z~w(e$Yi=#Mlc!$$U>oLzNUZw zS__97YcV8>Vk%j{04bZLSl5nbzeHuoilHxLVkeO{QXj2Q3!p*K#X~ZoVY_z3Hz>7Z zU?v#}_~mF;x`dMSwtyWojHc(P&$Ku1Fb2B4wkRVxXL$E3Zy{S(RH{z!yZU$2VI|;6t!>x}dX0mWHv# z#o>QHfV-5XqXfYiem@6jB6+yE3P294T55W3EM{8E89D|71b!1LUj$@G@T$&gO@H#df)pXNeS*TGO>8q z^K_KU%#;LxcA5cgFZs#+Uo0?S$nU8dL&0R??nSp5GACXv*}LOB`{PBmu<2l6iI%Va z3{MHNm)P)9RPZJi zy5{f#HaatUC!tSi?X%liDyE~T=D6@|5QXF`=0G8U5Z|3_R3MqBIrL&$AzS>8hzVo1 z#NhyYs%_o%JPW#ENV^hEzi>lZr*(|*LV=PCW}R~7 zt8?bc@iK-n{Vcq`Q$>4fLjyEy!`!-G2x$T~ec`}s3!btjPVTYdr6u;Dm(h14-4yFY zfi4k%8jeQMfq@*>udCN=k-w=4iy2!Zs@=h|igbQk-comJUP4Vju<2b1)7^)qV1d~ zdV2g4FK?(*Z}s=_pI+sF-m2))pv;m7V->9f$4QJiYmHM_g$Q{lPYcepRN1=p1o(!c9kWez9)ymp@4wCTz+m8=BVE^ChBB}l z82)7<#AZ$#_NfQB0cs0 zqxE0*_Hb8q&XG>6{;6EWU z<+gL!BLi>p`xZA6V49WI7!Q9vFG;R^613}w)U+P2ZBP4ty2HNjqgr^_Mk~^}jWdKy zrDC{Hzw(x`j>2pQfQCto`=%KGv-MK}d{tmGRGxDe%bmacgL+R`tn`b;&mp+{ zlOTz>vXc}fY9W?vpn6BLPy?@1;cogQp)@(N(|%~7D4s&(pNSfhlp!~poh(dWNZ{fG zE5i4my^D%{4%d#g*9QO?@!L2u3v~Jtl6n`vv^0;ZBlSN16UP=U+Mqh5P!2Gm+7)wz ztxh8V$S)4;@BgAsPOkqa>SX2O{1066pN883cO3br>N{yi&9$&`9gYFf2w_YnfkdPa z>{IWTUz{i=M1h~wc=`EQk(-O{&S?`i7~YJK5NsAjBd?(bcvMsKxScv$n{kMx!XlNY z{-OC`gkgqhilW2LobVI)dmH(b_?mfQs!Z2>j)nRHhoqc=&7LK~| zcC>K>6Pe{wXe>RqyNE!dwfCFT^hJ<==_#=o*B~fYCr7D~PNOBx?qlf5qmXKKZPGsx zF)3uK?_0I@B1+IzEv27#GJEul$v(IfK^9mLE z3ls$Y!PX5tAvuj>78`pAG{*I-WYl#{`zt^bz_5F7JaM-< zktLOStF4!hha&}A(Hsd3L zO1>;pug!SCgGxQ>M;wTcB0TKP(w^U~=Nfw%P9}TmD|q(^v|nfrm{W&AwEe{ax8C;b ze7L<}hY|nuMT>K`jC;UO6`3+sI(Ls!3CS%ycO{h26a>1^k_~sN&K1ivb~?|RLzWU< zTl{{eTOqA7%TI5CtDVX2z(A1w%RM%;$hKf8d+ZMcs=DV z*x^j7GtW3)Q4wt&pclVt`iFK>Zk&~Yi7plHW%V>BK0IhGwqJFmmko)L7rmez6&Z$? zxnN8$WFU?RE$!~_aX*lxiD4c3l9#@0Hr$wa;-U{7CbuVz2}68WKPFFT{o#;%x$UkB$GX&lNU5i2vpaJ02Z+&awC{6v@JtyAx`f} zC2SR>jtY}EV1{Thq?ruzGB_Nx^0lkl-{v^~7f*dw3EbrKJcIQ4hB=^5+Qf7v9LxT=9q&xsui~5iC8m>o=rZd0MaSRg*s1#LlqISCw5m=Sy0k)HkLa7gfZLJAjGiJ4}hfrz*3*z!IA(f zqGlD1nZ!S1^)TdI;=U?yHD;*y39nPg2K<^_{L7(f#(==QZO7ui>nWn;EbU+ABY3Ob zWS_TJ*{q$uI-TPNc0}4@fi_pC_;pSBnW6Y6f!8JmAeZVGOq`9a;>Bu8k?69eP(E@j zFJqLp@X`2YM8Fr|*+M0u7HzUl+^sFkr_TcqcfuN5>1C3@j^*G@;|q!ni@&(ec>Gr9}EVY5%1Ao5g)(SeHdP z59a+1P&h~5xzU-O;{wh$y;!Vm#_9Gbx6;NqVcn8Ne_S=yfnHjVqL>!>xjsr=$pR~R z-LUxqp_8m~YWE8LyII;sIO=zL(A>D$Ma1L2jOQ&=Hm8H0!pYvS+WpAY0SeyA@3We7 z-4p22PGOa7N&W>vP5XngDLd(i%fEI{t{qcxfC)QyN&5pvH?+IP@mS&Sp>}SPuGmT{ zV~blH<~6^v?NYts!xnOcKUkU@#~VZs#P4gWaSP%XY%vPP ze5jfLDC7LZ8CYLpZ?-Lie(V10PT;}hNUI<@uU~ZD zwK}lF97gL`XQo+{`AdsoVKOr39pAC>1 z?Yf)`Ea=9HtunM>L`R&3Bu=ak!7O-dcDejH@UN`o0Ift2BK>TEVOr}R(FOdHMJ2R8 zAm8&Jy~j`;l`@#ob}PVGlRiWb7ba{Mzu5dFr%2jLy1mIjziv7Cx;Z99)CwI9uS}5qhSaYMXgvU1fI;=k zUQfh@D6Hh12;>|K7s-Y_V3q0F)p#ZznSqKqCbmD~ni39o04g0p|DuP9Bp~5@`IspN z&)L5Zp~Ll54%J;YtNx&O)=+`@6%(KAk`C8Zm&1{cS%$7jxW~#Rl zp%5a~vKj6Q$N`w|?_6Qzz1;)y2LQnn@O0@E_cyqk%o*eVhA^)GPw&ja&iy~9;g+^c z@BO<63|Ol=acBxGed6bLt(W2Hn+g8%SX}WPTCbmfV7J2Mw7Z@&Mh*Oqn)mgeI`nuw;#C*6(Yl^^S z+l9*UXW?-XV$hB2$n8f~b{1t0NQML@>;6Qe%AAx=X=gfl;(1)pGbxKex6_|O~~lJdBN*(c_}8^|GqUDmuLuTLfqb@ z0&yA)wB@dbuq~qvhVj{TfF?U+gD;0+{~JL=1B!6VEjOdHj&A><>78p| zc)YVckqU3!0Op>1QW-Se2`U350f?GdmUDo21sa45&IlX@K^Sg25KD|_Xz&ipmz*fp zXh=4F9>8y^hM`SO8sN@tQ4-?=`^Zh2P%_DMW@h{$Nzsjv8{S)&@hzv`%W%$Y3v3y`&%<2L@Scy2gCIcU`dY0o;j5nA^Zhj%AAt*C&QRKq0SeU|(SQDNr)ub>GD>Py z4oLgT8cNvMmB2ts5#GcMJC|l&k4#d0;Eo+8q^4hV!D1n-XbN^sAf8{b@mODhEr8dh zgwCT!E7wNUL!wa~8^bb2%*&(~j6Q(Uv^NaPGHd_H`kG1s5mKrt<+VwM#E~MPO4&zf zmpeyt5tm>eP9Vd?pBnwI~UZ(-Y_}pDe&5M z=GteVyQv-9Z1@?0Nw$S|`Q@yH6Gv^6pVigYD7zxN{ch~J5xppzJOZyT3Ja1$v)nPu zDEJ3^84!>^W<(G=d6V7Am_rva>^FJ>vxVt&r2X`|>b2>I{Ea!aggu4HTt|SB4lt(c z!7}jzt+fGaO?NZvBaqZ`zWdnYirAll*(}n-5XHW+o0B)_t_1p1JL$xPzz~4rQqi2< z-zoSvb>#l4Gbyv-Xd7 zZ8W+I6?m~75)&0TxlP&El%BN=GN9*%7ipt=tnJldA%(7k)(*Dtx@i?}qQvrjum|0| zE@{D5FfolUwpdH|vgz_A$?NdsBL!8|mwhUf50a^q2~P%JZ9-F0Itx77F_Nu#LG=D% zajr{Zm+97ScX8dPV9u#};0i@H)WfpkH5?^(Onb0pOe#B=g<< z<3a`hJJZMCmi!bdeJ*>_LP2OO-9UHHgtJgynNT26MPqR z^le?vd5D-7X2@hP8dB89O2gU?>HU6!sHMn4+x673r8OoTDdXR|BY-gRHW~RM4`5BU zP(gUlbW#sP8I_A6VQ-B>f|jIUab96hXQ6&oB;(&?2CYofK{i2n15&IOa#+mP2Hrr< zux+I)*>n{UliqdYUhrsjNs##+CJG!rnBFvfLr9I4&?FOjIS#<_1@uiTlfMl-yFzBS z{mA;CGmOXkE%3qX@&Twyk{MYq;Bg_#Gb`P1VQ*ZD#RT0Pn_#xW>Zi*{QA=)D~J%>lzh@vj0+V5-Is^ZC}$U`YbJs8!W%Ob`EzJ`>u>ZtG|MA#V8q z#4*v6nx_{oM~@AtLu0}R0--{goUjp>^tc}kXviw1{+oe7xY>SsLTJEj|J8&5N4h!= zxSSaN&vlC1rnWx`=4>(EI_jL_Smu@hK3OIbTUP5(xs*g%cF^2V;PoL;wgOcS(ys*uf_!=U(WrhzZs)nIQX@!o48@JU;+Nx*JVW);vxI0_~+Bj`ghi}?0Z^)^shBIBA7j$w1QnIb%XKEwZ z8a*_|$I%PcM<^K^=pBviod1>CyRi#yqAvl76{{9uF^4E^$5;+))-rTt)kWds)lcs1 z_yA2~h`<(h$}>j%u{>*k~ZYb{?H|k2(CS=O!oDUZZBlp_Jez z5f`t%Rknq=$_sP5x5gF#1n>=p=iO&dy|dH}I9z^*>vQZ+QD+OfMD-G<(EIi6dFtxfJj9#x^|o(~1Rjg7ifL8lE=58B90y7=v&IVcR4yCY)rAXNb0^fy67SSmgP6Lfr=AIL-9vflEWG0K1 z+6>-ll3W1$EbC|;V~S2K36NWuT?{$_tym>Z-6av$k0X?)Aj*W25G zMSvmF@+{8|&otN)O#Z@#>eF(~2oy*1MLPyY9;8S$>VjbI&&dVzJ`!@s-~xZ3;Eld) zqMl{MB3gfEmq4ou6~;k^<@%lF=?

OD6BFkSWdV4+FmmNZ^mcuWn#x$4jMr$mex% zh8G@1S5lSR1hEuiySyx=1W$B}5eAWP^}7HO^$#O5F!FckrzeL?)084wjL8$~)S326 z-Z|;o)$rCGOYFW0-x~C50JwJ`#qhiZt&vnj=1YD|Jh7GG-}s{Jx3wEA8~;A>8F7KR zPCKA8AnUdQBpkAC>s)3?-K#IZ)(U}NNm&@0{iKvQyP2^llVGk2Iyh>3Z#3|ljAsp{ zRvb0;|3}n21qaf7@!GL%+qP|66Wf_goQXP^m=oK!ZA@(2b~3TPIq&~Fb#A(9^~LX| zt9I>P``PR1tf>_=nDF$r1Mtc8%ROqS)2hieg(lNR6SC4?5UiH?3tTKOB(xZ$u^>Oe z?k~rH0q3~T zR-*)eudDz^=1$!f=p)$Q-DE%3@;vCArh1v&%a)v5YEjJr(87?W6trS zLsnnWearfL@d`2}SZqj-S84JSWO4g~p4gD>j3L0OUdi?JL~5d`)#5NkSJ^Vr5}I%5 z(zISVP?EClpvsuRus;6E@AiBfV-RYcXd&ble_&pdZl8xPS$Mc0Uzz-#Cx~2h-^DMK zPGO89(X`zr)<+jGHQdrfNWydK@DT_For$xh8!}0E!GtWl^iwksC#BR%wRc@lBa6ku zPX;*J`aT91%|Q~TUi`6yr7poTpQRHzSJ6mMgm67YQRcZuf+FU4153VCPKR|?m%ih0G}bYv?cg*(Q}SQxUu(Xt(MzNn zWn%81-AJ6&L0;)n*-TkVy}_JeHj~8CXYX%oc=@5E$^3?AJL^90nzqrH(GNJW{9*QSQJKp>V7@t__cf{hZI0{!v{_U-6yVMY*s#8_zh8s;SIT z3w%g@Tq2f)$i$A|+nRbDto7;C?hY`c_G`5H=;_>leU7lj^+*E-nVNLPr)SF^$zykx zZ#yJ3U`L|h&j#N-)CDf_Q)bL|+B4Z^<li}hQfqAiUpRF-toSg$J7h_n zbE+C*4n&yUTTl;Y7T5>PBnY_Q?X1iU=G=VXcE)7xzxd57>S=Fa@X+%>d!~Q~nh{ZE z_RVhzwf9TEw^B4w<4W1jgkxFJ2f}p;A1lwcR zUAeEdTbc2zQD7{(s_%Pl)`xno>;K^Bj`S5&uT*w@K*7a{xcX(~J?rjANC(ykExd6^ zc=aT`6-sG8E=)t;`3(OmIh+`4$hRsRhlw;W(YZJ>GMiZQ=gm~`qsjlpCv;|JH!1r)Di;U%8h z0HYyS_zZoct~qn;9?;Lx=w8qPZm&xcFCkj)t2*?AD>Rm;nXuJPUOr7a+rCtmX2)` z$7J$Hx5}x(9>uh+-Xdu(XDl^ME;_fk(}=YX?Zag~6lE*@hLZ7Eu_U#0Za?CyXX|HU zkEfjso0Ea+lwR0D>k!S%!-DLLJ8S+#<)#%gJz{srbv8gMF-Hb&pvuiOPug9t$2pS) zawXVKby+Gk75BGTf1*1%o}yC>@H8Ws_n!+=ukpP z$^DMj6k8iD{1vCg9SOz+H35Im`sC}Yl# z0#z6(>`|C=yaZujugW08R@+E5->CQMSf34EjAaU>Z|>pbLmXYnJN_7bsgHq?hw_0$ zN@Y~eRHsMC+pvrZ4BQqb&v1GBu8mE6eb=iqsEtp$r3zbo)t?E5S@#}uUv#Eb+A$b~ z^>7m3vN=9NF9jZ-znh0k_m%;b8^^v4yq4um#h8zJ2-XtFP$#Hl3BWhq>7(A+fm_dr zuCOSfs+Zd@cvaO|{(v9`Zgf)I=VR zv{DV&t_&hZ&jK#okDO^o+YjfOc8ESZ7seLt+AQu<9=7gunMkE3me=3yGGJ)V3TnfeygmLivsopgwV zHtYfS4%41Zjqzq*a|j}9%ArF~DKzWJA~QZ4f3zV^7mi<83q2cWV%F((j@u~!GihTs zLf(T0%w#}F2X~DAkxqcM5dE+3RP4!NN0-eMfme`D)uZukLR4m}Pw0gEHE&MELJ32Q z*CytM7~7aI24|8%=sfT+8KY1lVFwOyf42ZJbHb)CwTR@TJ*^6ty(-|Payjk zPh*IwCS!jrUFm}OT~EInKHs6lK`!m;EA8e7jY@T;zKMIeRxaOEkF9Rw+1}ur&o*7- z9cLvEBN$J5Qr#@}X)m7}M&tK2r|1ke7iM%Y^%_O&mgvh@Vx&J&`YEbupy;41|NM~X z;2bP0DYa&x^njwvItfbGoA!@iX<_v+aXeJne`m_PYdPm-*?Co9mj~+)MAd{peMxf_ zkh-l+p1KaZ3cC(%D~4P{6<2E3QEqpoFC;Q~BZWjggbxq+{+_-7`H#6Q9epy6M9?C)~D zHB8z_;Z8hhyY@bGL=zluv99c{xXGZf4$gOkN(oo#z6%DnxnkloUsUfxznLtBR&SiD zPRZdhEg&^c@i0kBb%yssRue#oJBqS4dEx@)LylR8G+uc_m}s3?gF2YHeg`9~n~vxoXO|qCMeNp$)@o3@6gnz@%ocuv3NRaEV;jE>^63-q!hKQco4R!K zA{&d?QEDOT&9}(+D1`U$MM=e&ZEwD#`)g{{*0ZhRRTaFKx#vjBVxcn!Q)_I zI-oWo_#}r8qteqIg3NkS;;p=|;W6iQ>odhC;IaZvfxhvAprUNdVSEDskRxS@3G;SOB4P-Hn zwzGZ=4^+c1kO#Ph&TUN`%@fU)!xD2F{dd$uk%kJ*NeRM&Qi%cIH1k$}Q6E-WuXzTy zk42Y<#$pS77O9bd3gQgKyqe4M-i9z5gz3XvXh=&jLyNi%FN{y4-oa^{sNzEXhIg#tmO zcm(&q15LrA6Q;D<%r_RtDg&!E4YwCF3c1p5!v&eMffOMJ2~39Z&rO2+BU%e%M+Rbn z=nqA%Is$WN84U))cBli9nCAjQ_zi>uF1PoNzwZoAQH-)xPOvnrg2P8L4=X|<-fjW2 z5Hc54+5n)J)Kbo_Tw(@%dz{W|9KSbvR6In+3U5B#8EtKJ8dUYHZhdyP`C_v09@>6v zzD}Pz8o|Qt+BSWf+`mqBMJ)N`sr(b2w&_e_!;Skm^uDeM{@hz)-gs4BPtj}fJNvAC zSsp&`;9keCG5k8|ezgt$iai#eaU87WfX-nVFa=}`jk(K5T)yoM{>c<@t6@VRZzbM% z9q#!=B=eArtYIn5h`ru2nB0>bwrT$bUho&wlB>_VxvT9QK`2O3GAVC}J&etE1=DyD z72Qv}_n^0MJ5Q4@_5-^WezF5BiY?hUL+lIn$tXiU)zi8adok+x8FzXr0oYIR9#!!+s=u}Z06WY?T6=^*L|M%qT$2G(J}Y( zw!Ls$wbBHgw(c*#Y@_Xb#+xz!WyW1_uc8cUX4iXhLyw>McN|ggXJW~T1TIC_9H z=L?k8KYusmy@_%2LE#a>dYd0@s9BFj&j@ogDZgmAIss=TszX|`^D zM%^4ZL6CQ;CiX3we!K~Ma~O0=#0N0KmM!7=b;v5>Aw=BXnzqWCg<^J}+FJes%PJgx zWPhlCKePX{z-k&_u_SF6BK8PX@JU486P7hd49(~ww|Fm8#+OZ;nl5#)CDud5y;Ij( zkUL_N_CrjyJme_p7#+5b^hpr*)n9`mBZUN6x-3;l8Z|t-HT4uWaupP5e+gK)$j8sF zD?TpSrL@l_SoJ@(;WECQfwJETrl!Gegk&h}&IvI#>%A28d@Q>jh5T*&#-`_K9v9^~ z=vf{@vdR{8c*J-U1?8Rm%0hj)LNlqvJ_frLrAoAOklL9kQc)vogUcLpp-y9k^HS-^ zKRd!)YruRvG9v#>MotV{JObFrW$aASI7E>vcHa4;uJ7vJgE@~18Dr<*(&-pn2x?ut z&PyU&5b=bjA!0J22qf?C)UB+xGaO4VmO#$kO9`Q_LPm9-{WiAcTTWOG0Ek$Se|DBi zBloF$uE0L`2i;#)dI~TL{R(EMvB7>g`PM1QNc)T+&6F(2%w!r;Fb#}n3Y_7+UPj#q!2x_$zCUtGo-b4@~~!Nt~y$axXoN=J(pr8%Ou44i+9w z=?z~ARd2v(OLD_9UjfA5!*i^T-rGQDSJ!9L;hTL;R_lZ9JY$m7#E1^|ty;fE*R}sO z2(whzSMLpovhG73!#sNBbkzZGLk1LEgC#d0!oxX|ws=3&N!<}RokD1pULI2LhPXAE z)iQDicZ0#7x;G2Wxy``1Z^F9!GK-c?yU`>trYFEKrA=2W13Y=Z0_ldL1FYWm691H? z+Uh4WDAsqwkKyF^c2l8!(GK5c3ne*HBWP7kpksue*ZVA)AwLZ zv(VFJBM8q7P}N4)GXEP87h3@Sy$nu4BAlqxE{8M%k?Kb|HTMz4!BU2lYhNvva5BG! zx51?ZX=^3Pgc4fDEh~(Go8wq0UFR6Nuu01Cl2@60!Sp1Az0BZ!6I}R_VNU+$bpL5m!j^gs+QWiI zs5|#exr?Z)dN{i{-w4V?Ve9Rb;Krq`Vc1h7K>Dpk$SYP{w}U~_%Y^!oX~iJvBu-ER zu1oYBh{uyb&^T5b4YG%(Rp{_0v3qKS^TFVb~kOT@77QbHtY62&34Yvw8JBdlh^vE4}7wQgQxJ){l+v3nXf)j zwe%Y0`)>q8H^20t7#~d}5VghlLPYkKDJuIFQs}9nbxSmQ2x9 z@yR&%z8HGpmYpiY+V1R!f5eI1HHos zW8?W>71#tSN(jpGUxC8S%l?0NUTh>hoGd9KQDB%5yjLd#a>L2ItKL}u;(#ZPPcTr;? z%j(2$Z&y{!}E?!bwPia!4Ej}ctM-HT{L zHAe==Ar(=9Svxjj;wj3SwJxMUIXOHWa=bp9iWT7}ckV0<#;@82To%s{|rVg8(+8zIY_r4CwiKHdg z+Ag+Z?a^;^ODnF5f>L}X)GIvc7C_#)Vw@KG^!+gz@?37X+wCi4i?+;H!seYCDL* zK=_?#qumKFxA7&1hIz*iaMo8E}~`PO&S)@MsN{ZuJI5UvLaAGHaf&FOxV@?yU zyXwiab+!PBl#elCy~#(xs^hnV4=hEotx|;hQI)Kfrg;UFSr(!tffoD#aM$icg9Fm) zMhGwLMukdQbd0sOav$yF87m&_AQY==up&4rhk;SAloTL%^7SmHszCh5pTL;6tzcs|Q;>F<2So;7q@WKD{Q{qS9%X#HycGe&j z;%o!5dGQ9F>4|O?u0#nVpzZVh-8|RCkSWGbFEl-|sB`cF$ECCECd z0`n}N!DN;;3q9bxnK|UtNbgawXxx*%dHExq+-+X3Yj+)Y3bkz@H{D#=MF+JqpVhEY z)28g&uQ6ORr_24#hFz%KZl&(_Z@kRRnZBGkIgU-kVy<`CyWv%FDMG7vs2P{Om}i$9 zb(ldvawyFhhf3Nra30h#_-KYmDmMSakHKnKsvj{mb4ft5uNu?N-UZ>?CO7B7>^JUB zw4Zj2genc&*sgvJG!hSe9?HJEL$M+*f^{B3aA8wxLBkyxOP1do$NqivR#hxAdP-{4 zNpnc7TlE3RqgOiVHEpr;W--Q=3f(c3vogP5fr(q_3r|q)01w}~VCly?${@DY_Jk|$ zDPn_$T1?~5b2{Low;6!9&Ty+9klPe%3DW_`OfSgN4SVX?RaqIz# zDx1w_BgUH(csYBcTGSS=x6}Jun5JQQe3KI8hx}71*_q?%2kgs%L&7ny-{-ke4<5t{ zJfOxHys2=>^K&O!T{cpiCqPM!Tb<^zTsNWIES=kcsDnz&mar4sHlkGC=*E6>Q$GH@ zT;3#6ZnP{3S2ytx@m!C&?8I|K-)S{9U6S{ED=4tfs}Ko2zXlbGPJ} z(i~6OrF38@6A+kWz~N!Pa;+S?r5U<>I~X#K26Z#SR+?%1QTp@*5t16PBUm7c?iDaUU#nhT~ zhTSOwXo+pgUR^DekiKgCuKk@8cT|v&>PnVPoVFpC`Glyp+MW7aw+grH!jTLQ=i0OS zCiw6Y3`lgWsFS&MB$2j`pi`0m^T9DJSNxAa7S8;BN7;~W<`HEjf|d`mVf&H9ENtZ> zZn$?g_GJw9Q1@c+?yl2}aW`z|0~Nm8Z-y0hz$L)@qW16V)D1}+mJ<2ZA5^)Imk_N5 zWK?WH+_8^yxEC9A5w%mYC=XfUJhTplmsQ9&aVqX`p6+$Uil0;)UH6WPO@KSq#V5L?!nB%3IZO}~F8nQm%h6XDIl9BQH zrT{hK64(OUc+5yscmLR;Ch3-JqnG82pWT3f#v~zRu1t%!kl+)i#xVxYe(rxDz3>*u zYZfmZVzQJZzlzjtE9X36Px={GM8;fPwbuA#svHu5@X(?o%-tmO>GKg_xAUM5U?#6t zk+DcyJe4Tvb|*{adx+_XtQmr=r<8#HcFAgVYKTmfs21N{Ocu8Gf@_o|it6Ug@&?1> zv%!(v$QY=O8eP(2ew`_A^7NWZ9Z$N&oMue1vII5XO;~&DuA*UngCcFKn)=M>)s8zlA5;#w(M9$n~yI>nMKt*!Dl{d`kMS!~5 ztTVa6Z+g;)yk*|27jTI+*cXDWzzJ7B>JO~d?nzzQ54;xjz>Pos zl)B0x_{W@jKE)k7iJ1_ID@VwIpPYtjyKpDCzPelaYg+xq=r~!u$q-%uNWY=FyvjYS zXSYLVOxT)&N(^}qLVNyfTS5$cflGvS+*9*>=eFbF)>SC!t{f~)(&`@+ZzQmpyn10O zxU+w<=&oRWb4rXvkW4)XiQwfgYge};M=J9Th;)l~=|*4MOg8ysoE_uFd6js+(yxXq zy&%Uovu23y=FFk_TO*|f>`5-evxZA-ifb&=@VV8YK2lnJ=ZmW1Am0u zs!JIc>h@+Mp;prOc~X4eaZa%l7=1WoHwbY06wDw4n5i=_uLnKXvPiYO1m`(?@|pBPq{CyTwR zSYRa~lgi4NH_>@Wo-VH)B20@XZc%tbFd_9A$Z$E$tCA&lq`Ob+-`Ncp+XLRo6i;s! z4qnn1Ps&&SD0hmwb!x%*y}YN6Z7LMmUQVu>xPt|Qf}t*S`|8EuF5|V4U4E;dY?UIy z(~=;}o}CT_2#j6zbVBj2DSpi>3=2bl54Y-N54gYV zD><+>~cKq*&(LdRQ_v=}DtlA}jhxdmLr2 z>HS|CFnd+IzV!QFrD8IP3N|DX0&nLSs&gi~ZsM;Dgf=8!Tq;e8$?mxpGPxv(4yg@m zctMlX3dfM7&c%6G6tl=)NUN#tl0?MTQeppI7!+u1agGL}8L1i-Skthnza?;huHq+1mt$ilT#-X&8d$qMkI> z7=F}-c+L2WQ$U7ctKe)1vM%r8@sVkRPq^r0&0&YzDyROuON!x0I(wIkVz3WxB5{Cf zT^Zuc&T4bc#z2wPR4H)%G$Te@7W6n(NVqhHrATmqb`KYQdtXM3G2OIp zdL3BQ!>y-S=_;wdRZeG55$whN}`b`4ZX;cIX@daf@}J^AMq z=#!THaIbfY8u2dtpzZ|Z5*f(}g|2T`B#+^$UDaWKJ4@IJr$hgT!HtV>QsG;WlcF|vmdy;WRX0;56I109S;yj&Q;bN=@L|^rg=?G?E|Rry`jNHZZimABq0q-Wj&GSu5j7F(c{LPm81H)V`W2 z-M5i{@H;!x?F&YRKlW-OT<C?P9h{Y|P58Qk2`bn77>~@{gIpVWu+i4lNzGs2BwSP~axXjn5 zrM!t;jM4+AIj}p!>+_DSzS9`Bmf1`+ z2diGAol16Gl|*cDbJc~#KkEaIt9ISM?r8PG|FSYxp3!xOF4UiM_xVTH})8UM22QdNuX zpIMy7&2Tuieqpxa8-wg06e|>Q*$VREZ>T;4N)Y!NTBw;z2i85hfPrZfJPSo{7kVH! z@pCOln7yL*$WUGHC&X?ZEz>(goNLMFii=yxJ%@^2-vqO?mGd{9?^K_%X;XC%MTNLc z3xaIQe}4U#i_$x?Q)}tRz)gV3m!BBje|ga6EIhbNje;)#d2h+spnd$Fou=d1iJ%yd z%N^U+?VCiQ`$a_s17H>gB|rQwi`g+mc2{X%y%uPeea&N&>~r|(8x5sEG{kF zO($5LaK3=x08UlL7MXUl3^Ve5&ot+b!4_8`1XhI|SxGBu9Jqi2oVE?OkRPP1b!(;) z*%7@zQ0Ddaki#Ys#jcScBjO1$Q>zh^7H>7;F*!B0x?V(?iPWw3h~gid+bgH7?&pn_ z*|MDsGviN?ITPp8VZS4nov_&Xn0pa)0E#~1RtTT_3zSWF(W}sHr~HF2i>-XwIa)l# zs`>I_%i17=fRPmZj?rU7m2{;yVr}STY5#*Ix=p^m?DM{%KouVuOXFEC!MMnYoUrLj z6flWezdFyPBPk?*Cq?IfJEi(8p4DrWYoj#!O~$8c7Qz*z!3e@OL$C0zX;%XgKvb)GgLbg}%z zEI7N>zBY1|J0(A`etAXwXptbV2I(Y{*7@I%=6EjJpPU69HzN3;evLaq;$#YVwDO6g zV)FNtfCIs#Nj6+Jb6)|Op?6_NKx7VVjQ7s6oiD-h+8qLptZ&CPyv_IZiTO84V?594fba)LnH#^c|ggyBv5bVnC0N~001(|b~0$A6Kt zv*l>l{t;nGl<-FG&h%SY)$kAgEt}>Jy0a(b!0JnyRYfR#Tl1U9NNV4!_Ks5!yo=!_-SAtdQ62<>$Te#~ZmvAKQh>-ksDA2Lrf zt_FvkS_QmKstf5a%NAaD!S;ONc%1-h(U`(|_ryuk7#?rzDGPv*piC;r+oyKoud zerD*$!rs*fU-h?NrLE&1N$2KR%_0I1g`ql~Nn7NfC0C1ATW^wQZFA+i=I8jo8(Oxm z`BOH&*J0onzzsVe8W4GK?Xfp>;wwA501^oi1!YG*D#fm5v+_^#4Se1@ZRQ;3!PzGf zpHivXEtfit5h4f)OZ7kguAnqbmfXryx7fi3=t?IEDbZ3iOg!Y0TZag*OHTR%ayHi7 zo`NAF*LUO9o13EVXy$4mM)z`MAvi^9=}Sp-in-d8?91Msn2|wJ#5zWJK2|u|z?={x zkF0(1&D0Iq^UA;OQuSJArf+1{Gpox=h+O@}8_bs!Eq^9|mqhmVDih9eMRe7p=zQx^ zC@IPXj%AZ8db+f-7Tu2lAJdqb>@D?pBI6(lIAnR^b?=&}zKPL8W&%&H?K4T8>HhLA zVdt=)gf)?AJ!)i^8cU`#0|kEgz-j8_DaEzLrH*|%?}9}W`m-mulFEZ~M&D6PV67FU z=vTU7^RWb{LBR~7GMU)6V@^~?N)ICJ1mmWQG4FV{Nz&yxdpgHZ1UPD?{*N>?MQD7w zi&dw`knuOlk&QdngiC>lwYZODovu^zueK+v$k8+I_icdIwL<p7|0=xCp%Q2zaGV+`MGA zFE%VzaZ>Grp!YRg$o~V$G zv|Zgo+6BHU3S-GB&7@AiMstrN!AMPZ%3F5N+ZZps3TRxPoqOiPv}9jDY5A$#kCLm% z1w+2YCJn8ws@^?!5vCd3cF}5f8AYRSYgYUcEoG z_u3VeaoMAB9PT9nUaje+{vIH|wnV{%H^b6D*hROIu#Gg)`CfFR-$6&a4H<6N*L8@IZVsJjTjfg!{pv7I`8@Kt@&~#D#Wt!v z-)KJ6<@6phcelL!;7f0YoSpmk_7Y?EU1;<`zQcxB$g~iPm%qdMHlr8NCSJVnKAD=E z*W|#ur40uGX?!!sElpJMVqqWmf){b?i8hDy9er&v^n1TnFqWT}*&=tPF`AY!9dG;d z;!=Ehc2=6P5&s$FB?m8$w_0nqN5U1md?g$6y}MX~D}K;n$rEM#5IX&eief~wnuIyX zW2118y=ldp+L=bN5~(rFH|OaLlKe3$FD{}@ow%b6Y_q_8lgB0*lbBb%Pu1^-DUo4x zpxuzJ`1WBFmuvz(NsVQ+y`zg`q|rGjy~xXa&0Fq1var?lOTRBI2=uoy)HqZg{xsjh zH~JN}@!R8A7yDm9&%G;l$y~vDLV^*lT~lg=28*+N(=fUE0ePXY_EWSqkvOG%+fNd) zvFwy9fEPUH^Pa_Tu44)!1l4X=-M=5CVV8eHJUtWA)nI+g8-&#Q61KwLRL~6?Ja_p; z^cRVx|Xp?nj` zXaoX%2S~}rXeQPzzIW{3;2Y#LI1rsda|+(;0vwt>7RPDHFdyo)&GBtJ)?YZ42J~@7*u#}EnIrSD@Rs!4!7hRdbwXp;{3eLwl2y(PLUS#8IV5rn}{ZZrmLkuR z42X+k1GLMaJv^ju$6 z7iD)O$P#o~HeCBx1Qeu51Z@phQezu#MYxXX;A|O~*~c>m+aeC2-&`JQjywEr0B1q1 z&8qTBlRWROEuEFGQN^WQMP>ZA%FJte!s8|pnqH+yKKmM^+&!6fqwB#UG4|jM;xr|W zdYZr9jcC54pCOI&R65MRJ5{B6$!VXR`H?7k=Wac3Pm{B@Ykf@j7c@{p+dVO@;TNj} zuIOlnvmNLt!aI(r{*Y#7niH5)016LKbg$+zKEQ4F_Pj*VFXBpJJ@B9zPk5s}=)0F7 zT$=u*1z5@mssf2ctE)L;pcnKtiE`B$n^PnCeMh3I9mQgCcIv9r_elpo#kF_QFt*nH zVMH9`hH}#VX~ro_y)vZY&ltAGX0$*Tm5&iOn zFcHx8m{oQ`17uL6s~6bo=Ra5O zy!vk^yocZm1SEFQz`ninLku!73|VTk>G~_fE0wK{y-gFyM!oM2hG^9Mj`EBbH2Szz zbmGM!U84`eKVqFC%uge+pfzx{%szDGdihkA*beO7~0;h|XLVBbbml7g#St4@|(VNPsXdz$yvYt9&c<9Y|mXS32jCPz&c4=$em~g0Nc)23xEB-Wk>WH>Q2ajYNTj3w~W?gpO^(d=WB>-HUB=EL~r#V@k|o?C?s+XQ0r zYYEo+Q_P329qJsp%vqYo-n{!4yzW|=qsDkJBJ*ZiVmEDI^GEYA54jDAz&q%lzX$u) zbaH&V8~ct+1ZXF)hvYnrkjl)_(ur%GXkbIsO#4m@<6PYz+HYD5@WzeU6-ZhJ*UvPy zO2m5@(rM<=uqD27d#yW`ThgOo8WGIcNUPksaDCh89$`yZQyvLPs4vknR{nczM=&Gc z^waD9QyVJ8$M2>4Jj6h;(lUMs~;DU-!(>Eusw z#^)p4j7KJtypkbv7(XtfQZkzNt0HC~nq5DNe@&~CB|K`oKE+l*Q@M3Djnrmc z1zR!~DS@Bxru~q@EusQ;CqIuxIliL)u@r6y7(>0Z>yG0Qe;VkSF6GBzI}E8TrR?zG zm=!V*Fx<9L+%V)b+oRKX2eN z4~6w-7w!7ZLT5~D4Y|q1yAQu_m-BNGRP%m^W)9V80A_nYpj0$EV=67{OQ*0Ie)@0Q zkxx_4VMpy<)a$q zl~@}G`=_7N(Rs|%N-tbq#PAUl&Oxcc?~Uv1WE9EZWD)sbEd!xv0%^V!8{1))&*3HN zak77Be$Ri}0qQ=gu`Eew2TjgEs%EiQ4C8IWyt>VOAG`50- z3M!Dr(Ba)mUetV2Wsu^E=zgjRZjy{9`RI17l56;JF4@fy$YAReF3j(4&yT`9^H<|@!z+gn+aSz4VdjLQu7 zLdG@Jvbs8{)i?ZZ*iX#UWMg#!eNSrWkBf)nPsicWTUm|3;mP;`x}XpRN=Eh1bU+0{ zPE8F^BhZShOl=L!_D^i!isY3SO35lJ-si*v3LqgwUwJR*r{*S>@FZ_o*<*AkIJexEujkRSKdO)}VxO;od*}Yx`dZ z4@Oms3ZGe5U!V$et2>T*5Dt@{Y6Bv}H;#G138}#y;dOz*4ZnGN7kXbEf$2|5?Iunx zr^e=5&X@7ao;A2!3v=UVW;07U2K3FHb&VKfap>o{poqSb`;B0U=aGsi2DLF zeV=Zq3}`3_<`+44+(d{1zREuVfv=x)FU&0Y%~I}vzM^948v?3sw{6~X0)Yi7rkEH& z_H!BRS?>YJ{Nj9~qPn7zVyPH+m)ht{wn;mka!YL+NFdYw=!(i6Abl)~rjvyHUM6O;3%YDQD-5Ko3iWgnE7NdqetB=CYS7+lI*En=l;~pDFm4kQgtzyLVfEHLExF-wa;d$xud4z|)L zbUp7#rkvJZSz^L_Zk?-S)8;W_G>%QrPac{Ni_~f9jsw(SWDuAvZA1Rx%fFlNh{$O# zJmU`W4@{}W4k*)&g`!ZZ91CmxZlqTsC|Eze5C<7rN7MkS;<;3Ee5*zHC+tAw@CQ5( zuwHh02S`YAO>pJhh&~`*iZ!Ot#(Rc?z~k#zf01giT72Ua?u0`c%~>LN!*5$se6`I< zd_m+#_+~m^93Drn%a-zY>rLoeOOy1uhetY%9mzXb&*y{_kl;GXlqkJl8zS*UsglOQ zVN|J~%`6YpAH7ib=bK!rsG~<~ErdTRQGVA7Rgtq)rdT&btRjjt2WU>?|>`Y`x z)uOqjy{UVu#f^z8)rb5f8j5AIMXFhCkZb}%%k)Cr>z#fgJ-NCE*`qN&7AP_NPDGES<80X2Y`GU38UOpwr)89wYF?{bX_T`J(@>PT50*}e*{cDE|vEs4Z(@~`^o(juznzG7ryC}=Yj&-Usv6@Mhk)4gZ;AW%a zQ{XM`&3{e&JhK^p3YZeN*NGXOaed>=r$HtI6l5&I5|3sU8)qeodA5#~n$j^mMQl)J4WkF+APd4yOtr8%TJFw} zE`!K;=Jf)H^xMXL4`~71>|LfN@pHXbNywHbS$~)d$;e=i!sAcma61o{-zld=Ac4S~ ze$0G0c9wFj)ypqExG=R4(Y#LoDaw~s$ zn(f5#4pjXBNTh>KHIe9DSLV}izD-6w8xukC$U$dCU!pbP=$rLrm=9>8HGqxvFzLJ8 zJbxlK-IK(l6RxT`y5stjZYMm%l~qufCX2a*{z}?|pW=9}xMz-+E@0rE>f4YIm1qzH zW10AbuP++kaZc!Y88p`bJy@6MoffR*n49R@#vt;(?kEMVe?RoSsmXT~jF>|T%+}C_ zFLfFZ7M<2~S7LW4oGT|^LuJ@_WBS=Yb$|M&{{`u4&zf*!y7_#wstMKNuB;%KX)Y?Y zXz(!)(cOe7ML>t^*vOsc`MdYTB{-jc+8%$H@=7=D2uM8UeC|zQN>Ndzbi?$(%5W%t z#HyTypU3j3-m0!-T+xFR0{%qWOjte_SE>WJBzSR@`}l0)bjR-P5CtXFIHO*2et#K} z=pW~IR;XD*&fEr;I2PSvn_QAt@%DErNwdZCPUv~oCid1%U?hY3YjqumUw8fT85tPt zGAx$SAm@swQ0#VID(UpupMmgc>89gzdVP8KmoA^e`t&LYF+Oz9U2KI72h|GXztH_^n&(D{Y+}5$8PfE8KT?cr$)bMeTJ9KH~eJuZqp{ zuUfj^ABkA2UTX4Hwe#z`*d-O-5nv=FkO438Px>lyFM8tb=@e#gGiPwS#edRDd@w-Q zG~P-2O3K)`C?ROPX@pG|tvs4*D;8dlWUhiVP~8<`Bzm?kj0WDiDOB3{K|n6r)WkHs zu5#E2H>;-QN^{1oc=dlpDLA0jdVpd)Y<@VR=oNeW4vyN`EqImVR^`o{y%E(|bADoY zLUnN-kKJM1gOm|R2`v1`!hiZUK)B~onj(?~Pop|^r<>@R5021HhFv&Lg;iQ+y~#rK z(U)lx3oOK}|5IY}5`xS#zt`TqihRy5^ge#I*io6aM)R1+^$GH+O3Ej+QYBG5c>Nn? z$-oqz3D2X6Hrwa%l_UR~BES>m8R~GmR;iD%S*T+8#Q=)MI*~*gg@1tfdGEs%RoH-h z`djhdGWfXFdx2D0fZAZ*UZEoGrf;|%h*6jEByQ;Piy+K6GtL~ zaw~~gpa`a3`f$Xgs2FDa#;H+_u<3vxu!Ji^wv#AXXem$LJ&!YEjx}yW4psATjS)a< zoj^{i`$0mSY88Cxl7GiwG>LHNo%&k}vD`8H+zl(^!EQ}nZ0E~r<&g~`J#Iv=5^LfG zk|GMej5ApENaLZh7WZHm7t|w;UB;D=mcP1(a+sLUEUtGfK;8?g(~Xmq*g}P0SgOrF zZ+k?(?xeTO?qCLdLCSM0`%H+0e|gpph16(Ozgzd>EQ#J+5`RVbE#ga^%B-%A8E*ss z3;6~g9Xjj1X@CqNkWloqFPp=Fx${6$R&!L-C~1%5ZO|PPrj<82h&rJ^ZJTby-Zs~5 zG*4P7Yv{Anjb+~REswa}o6Y;0+}csg>1mHD{I@^~)_bb=?rI_YmAQ(f`ZM_6Z)*nq zO^~G*PODD2kEMXU!RiA z}Ygf*AmiU)~sFCwn#f*lszjGr%u7*)4S``@F*8&2F)PH3g+f*klNp_i&0DnSRMD!4_ zlzg?jSb)+M+-xotDkHB~kprcLg8G?9o#mIT2ZU{bQiEH!?B5dlKLsF&&_sVmhN)iq zSTxDlSDDJE1YS4XL6-UVd5$I+WBHj;C9(mLvBoX|QR)9Nqn5k+`K-5~gPzafnVEqf z@&^`~lYfcREftQHAdXIGah5fT1GXk!6N@XZXQbH9bY?OZUn3ldld~zhY#WoE0eFS- zY#>}=&KrAMO@M;LDfO3)&rw8r?dKo_m$>mrUI~@$r{qX=tj8CIbr2u+qR#0GZ=JDI z-6m&tPY9k~sbB%fw}Jh} zsZY*#J&8Bw-r_1=8i3+Z{LUJr2N^U!GwBM~o7H4L-=bPbT+k99A1K~aZ`VE(v@epq)~S0qcqZ+J95y3$j2kXgsj&GQV_~@RSrfqt zYkT2ofQu;-=Z~bM+!8!el}t3NaKJJb?0;tO>`B-!a{=eEKKOzR{`xLFc-{2nrn<-h zH{(26Z?I5Qs*f>|`L%8B5&CP9D1YVU1|M5{iBCdv(t0(VH{=c3S}Jm$wRD2S;*W?c z*FAl%O$7EP8gtb=LxA~hFg_f*Vp=2RyBKdIYM1CcmNuF-H>pHnfIKUn_4ty(+M{g< z$ITN#NBpfhO@!LB6&o2Ibj3|B9jiZ~T8qi$56u&lA=@lqn>v8v6077)j(<1bJp>Kw zSp=Y08gwZ?^m|5G`HsSerd>Aq;hv6@S~*Vd8Y`G0^g(AXUUC#?zcy==8=B(oNSRvw)BsqdmDr9jFa75Ra z7NQmoqOEy#6c`&Op6I=q58=DSkb3AGE#ZhdViuurfJE*k{$Dp z<`g({mbRJEGKr{(T8;>`Cn2?`X9nxfOLcKFe_K*v8l?`6WHX*Nns6jo*pPM3ykzXM zbxta8?l<$8lK!5VC z&`)>p2Xy{dUp+_eXp8~y$;P6SIu0f# za|mAyBu_sxuTRo)K|U80YS;#OA60yRf8GWRAiPG_Vo{6YY@;~QWVF>^V6_wEaSiyK zzN8MY4G#hn5Pz>p5S3M^mA&T2uB;Rhr*LqAiw$HnW@G4_@(_4WQ?a1j^@nwx%4BBj?!$hvNe-Tda#P#1)FiqZoosV)I_CJ_L}rlJ9@<_5317Rga~2 zUW11{I!a|3c-p}5_CVA{Z&uq0W}{=0rc2a7B#C`=V%Y2koX)QJuIY3;gW9uJtKPYC zMy>~hf`8>fAgmsN+Y(a1UwGn;5_2-%$1S#nc2;6ATrxI9#Tsh(%A$*acol<8@92Hi z(R&Qx+oewjFm=vwmu32I772nQB@aGN#+rG222IBQZw! zAm|nXE-v-xBaGx&-J^w$rtHLQIjVe%d>h+sMZ1u8?-l&v{)DbZKxFb%#`Af(-?Ee> z{4x&1x61AwWX5(9eMHW^qg_YJRTl6~grE+pG1Gt4yf1E~pIZOSS}Z7=rrWJVNvBUC zD1YNOeg@-fc(A=>tc*GZR3BqEc0WbV=y^DLeK~m<+F|$VP+ozJMdQnV@IZ=-6;?0| z{)Lsi0OLd}^CSUM_JjN~qV<$~E39m>^1ceUzXohd_16?1kPtL}`J}q$&3Z~`F52qO z(Q$6*c1OTdT(q<9j^M2orlH!L7G$_qownwbDNC* zMA_ILmC!W{lIv0gk^MB7?M`{8{CqD!MJ-LP^QY*Xi^6O+cZO`-SunSUQTF(#lz~Z; zmHf9gd)U1ZpB?9++PzI>u-(pU&(v2921J~6>zZq!IQnQ23fyU-`O~(vaqJ!{%YV~L zeiSv*OsN;x`;J-`ro)!r`vGU$Bn6r{bbQU1v^TSV{b zY{~&NadvC-1leoX1F!VKK9_OX`hROI7uQ60D6{PTqO$Sk1Qm9?oP*!pKEHXuUTE1k zYA;aRLtM)5sE(}zTqUH9PpS!o+dLkZ1ywAimFzs7j`2u5n~x81D#&+68J<#(Oo>(9-n4mSMY-c)EX9sA zM9aRn)-J(Gqmv9dhT5tA9)CLz^+a6v=9=#a z`KzwQk@`x&>glYS$fzLH<+isd8z4v_tVihNxJ3B7r1aiRPC)c7NCiI>0csHx<=* zQY#1u7cxQ*2tl_UY6pGM__$!UUht@b_$fSnz=s&@L5AI=iMI)K$>YFfry*1(kDjiY z)lQCOBgEJ!r6PEnVumAKkYndzr*wi{M*k7($|^CdYy8(|kZ+M+;Y3g7A+GeVLh37f z2n>GmzSYuJQTD8IGk-Qr4=VR0oKILJ8Y`a5U{9mbW)CqNO(wGvE|d=CY2s?*AHcrV zmrTqG12)B3y2zr7R#TlitDmJ;wc}IN7JI9F1HR+lhP%MkZRJGF$A&Tg?i1=; z=y#T0W%CQpi@Z5hIbQSz4$<@sD$?W60{zrVAV?O{f-4KAsyVa_AdMd_7%GJAMP zWJeaBHJfkU%zw#4!Rw|pxsh!3Fy=%PcG#6pUX5C7O8q6a2`5=p^3?&~IptUd(Tp1R z7KLh^tV*^!Pv`hneU%}0L=kdOPTbn6UjoYO@t59-#-vQAH``U1yW3=sF>WZ7{e?yG zqAFwx2!b&7?tT5k)qJ=mG&D>7bVa11vz;TM6Tu)fR)4aPUeBROPeneFnr(^NqGB^0 z*Y~?*YaqIJD|kPG$yIbWvRqhiTR>(ab*%QQu{$~XKKbQg9G5|B@LqI8C33A^-Bw<< zC<`AZ>@Rqu^AJaaZmvpD8seMAvIQ$j0!j(6k-64gi9ac*zd3SoLE=})k2>@3USqqg z8TIc(gnuhO7rw_XEse8hpATnIi%2Gr3!6uDv`D7fT5Y1l6V_r|l&Sg?tXXrmYVenc z-nt=2p9yqOo%txApjq8~0G>25rY-gM`h;{Px~(O#Jkr1;zc&R`)18KG65JD#!4Ddx z!lB@3ZDH04Jwm|`mTI+a;?#dI#@(eqrS#{AD}QZ=b1Eth&)&VG4r`93mN#}|?l2@* z;}15!yyP07S5@Q{YV5z|)4#n_+w8qUTwzW2aO#{`s=O0QbLtVD4*eXO?To6C*q~Z+ zJy$^X%VI|gXr{P2Fco=Vsx*1k?&<-*ZjfkutogG(a+%^zzYY`$?JU~)F0QilNCWMP zM1Q?^)1tAag)U0rQI%cnDGFpYzMJM9-HCmAM(E$(VXk^^lG7jf zA>$qG}tRO z?)Ae&RVc499^>9vrc}8jR}?-H*s7!3mxEd6%={cnsOW~=H;I9O0%zlXN`8m~!i0q%H{%H8E5s7nj`yYDqI;`Me?Kce1{6W-Ie!(= zJxUSlmWN7t{Ak$`-O0|6j~wiaOs69o=zp=oMf;1~Cg!ePS~o6_ za%*FbP!lwIMp`a4y_$D?%ZwKKC-#mrCzJ|s%UCh`tXtwhIzlDKB!)eA{Xj?j%cF!| zl%aK^-`Oz>jS6B;0Cd=)R+`4YWGh9d54ycONePKRRnp=%UQF#21@`{%oZko1pXV@o790 zJjoGYAiCkFekZRIbT3`pLVrxH{1Se#eHgkU zqkgfOY|Ntp)!3Hjk>XB$GhU52V7`1of~V|UM@a#^vOfnJL8)gs(MR@A4g=+C7eL}I z^D{*k*YLdpuf|xLaBCIIet2W}Z$5FYgaU#jU7WtYiqaOpNsH%z3g%iMKCacvMyNqB zRrbCaCLx2|atF}OY=2KX%?9ByslN3xx44401#)M@f;H5tph_QgwK!XRRNh{)5bD5(Nv0{@}mXCYsyFBPEQNX*1;Ap*M`qnyap z2s7QNguxwqFo16avg2u=IHDC;jhNA>VkMu8cqevf*-x5fJAXy5N!j>FOXF>hYEt$P zRVDsQf;B9oD{FOInQ@9DQzyKm=OCh|cprv0wzsRarytjhJfp(cj4Rwt8T>|cTQVsl z3z(OwrGkZceFM3O=L!ck{A@5Bbayy0s{l?rKc&_sJ_Vx6H7Vy@ObrFd zO1m-I(@3_uEWgZW$$&eEAh*;u8g+?1s%1p84W5-cO-1cIl|Cn!9ic4;^!is2;H`^^ zii+Qmt-U-=)(;y!A2M#qZjR|AQRRJgxJ+9)PZ-Srq61?P5KuAlZsoU&4Nj8-k+;Xl@56u@@FnHHAT6hAML2W+(L_kD_jwFak(0HuEgc2 zu75T4zBNV55Le;T!D-L^vm)z~k?g;a9)yT>)fWxQ!hjb=LDsgsW-&`$2 z973>4dM`{E&FY~R9oG(}d^)U|1hXQ6d*e2uf4xK!oy{>h8Z@ky#SeCs!OP`7cEp^C zty9H1B?aVUu{;r5Y+~^=NNAt$q7jB=R)6HQIlrlV%McRb%(ho0+$m($H)bb+$u`NhRO&AnlBREH`DHQgt;o|4Sk zgYX;%Tu{d+FOTsV6p0-RnW*fL@bPLinenv+J6ONZlfI&I5ZmJvf6xXxZLPZeR)5xi zc;!PKBIsXI#Po21aB;UxZ_8a?!fx~n32NqiE}|{KUP01HnO?V^b&o16W)!9IT9czZ ztCBeX=nyxS$xNiN|6SmVZn5|lff}+D6rW9H*C(m(;o=+dARMQ%A@;=4lsAQ3 z9h2~EXd`+)&D4;hbZo=`<$5Uf7Bf<`Qe|Iz5%mQ5!-0X{EHXqbKUSv7?te1l%_(wA z{!{Y7Xa}dEZ-!KmYs2qq7}taJQ^GEe1ZIerxiCNa;x>XXxSe152s4cdJa5tj0(8)* zhD2v`Gjg2%A+~Ac@`6+ls3}I1<|K8S_=@nhN|SARmp9$$DfzfzLBvjHb8m{>CPI)2 z^NeY=v!gczwnrv1?rGrqn5SQQ}0}^|2M4 zp%GUI{R+=Y*sM`z7G{ikWcA5X=FtACwuM*k=Oo);XU+PbDPmh2x_>w+7?Q!Rl>DIE zpVTw5sobEp#orvNG&IWf?@5j@oMgzc2v3z_SQA>r7lglvKgC2+d43EU6} zC0E6*th38>g26P9t=Y`HV-?de_P~kS*@+>}zl820EQLROZGM}AD3{V1iD&ohy!#85 zIYk69bz0hZW#WW$RDVM#HtNn?Ni!T|Q)rU&%`U(gmX~0A} zlMjA`ifW@O?OjQK(VT?-m&BRX!gLJ3Tby~x%#uo7yxfm2$!setI7?~5=B67lg#1yS zC3NKsbt!i3(SLRkXy<{)`>)%v=HV?>2mN`f1I7jw3~xKK!a=ACy}~3i7?uHmd2ETiP(xk2^jp27b z%5n6xYkjX>e0~0e27MpF0gzveDbcg=0}RF|)D&DrHeq(~t2NO`d%<%f0HTNS#YI?gcE+KNC3xCvYl8?8K5r6xd<${*&9ZYd0buKs-Zyd?xGPZx?=A_V_fKbO`%o=X$ zqqE=4?hm=AkzrnRSV0rs_?&ZdamBf`nuXPq!d_o_VECiu4;w4j;F0U}LwG*^>nSN7 zxXPO{)&7i?%wWPV?-REViCAbTRU4wmihls^fbW~AS@s%GL>EP16p_MR=hjD-)PNMT zF|(RE%fy0P551irp?CS>7v_YfW;x-?n*P?o+V=6(0kz6`9jllL_Y=bAwxtmeRC7fc zSuOAn<`?!~7oV|{2Q=`Px*i&T8n+OHZbN8{%7Gh~0UO&0L}h#!B~<_n1qwI?p7Z34L8N+Fh;Q(?ubI zJLYhM_86k5D0QM2R+!;arsB+fBgE_JP`E~OQz);1!oNrP1kJ@ff7C!9jy$+mPj9Xd zs;;F+C)9v$PE)?yQx&{%^#J;n7C{_sk z^m0dQ@`Y{!&t6A)phuZ9sdOyUrAe$rXokmB5t4kn&=+}CD!O({EN4z=om*HuHt#H1 zfIa>N3Jm4waOE@UwXB>oRD{5!e76`NS#d%2C7RVN4q_xek}vx{n7`N=lYg1^YdlZm zw8tf#)20=SWEg%^8v9EHE`|TeUx&YC(jPpFDyS~nyt%nOSKz~B-C$+?QR7cy7~FtM zi7c_NApDE%3O_GrXF~KC2*?apc~UbR;hL{ffMnOFuP+Sohz5Dn+v!o*Q>scS=&CCO z!^}6vdJO_GR9MZWnU+{g|9^t3V`T53S{Wl|!dsEpAd5?ZJt-JCkQ(et183RhbEU=W zY!k9G=bOb;l?w1e{5+((Z_wTk$c>uaxoA&K2^xYu6x^b?4ELZM9wOvC9luGzK3fZ3 z@#)aDk}lYoC8#)b#Tx3L;uG4l2z)_Cn$?vW&mud8lBk#R+$>T4D$e=-omFijRXcGpd(KlF&uac) z59N=RFNtaN+y0FTv$b6WIX+5al5!nC7gKdJ9haAp-BG|_hTqxWFaN|_Ymr_!1(t;NC z7ki!_0DTTeuYctj0d6YbUe0+Ga^?R`gJ$Rq-vq4ohR~=|@%7z)YGIPmPFQ%^VxNc= zW(HIOSyYY%pMQZ;Y)k^*DS4)=7>P3y8HgmN?1XPV4N~o&BgR<3Y?_0gE1Hvky2^tG z9wR3cdm1SDlR(+9A*$fv(q$M(OQSt`0QmUqoF1ig2-mEfuvWfM>9Dng1hkm8MV@l_dRl&!%LQUvO zq#-4pu5v#ip=L8eQk3`8)}-cJ$P)Wp-T*jD7`@CdQaphYrh|O zDeW9Qvw!x|z6j-+djh;+@F@H2RLfNIx-_Vae!hy_uvt4`X@d+YnJ~Qk(v!S03s0-2 z8%ns3VI$Sc8Khlxc4$Oa$sJePqkk&SSdI?iJ0Bt{7MfVuvAkSz-j^B?`lt&iwu-8e zHB&-hILy$l{n%utTY;cJr+Zt-xDXfhc7+#_RE|I9+GbuP}=G#KUJU zvS1@3ZAn+DL&Z3l?xh0W=!(DvWki-lNjQ^i``93;`l-Bun(w5kQfIj`#x;gp;NWo9 zW`B@)-3i=%pSyYJ4q3+DW}GGNu*{Y6V&O=>`H+y7rFf%Drd;^cU;Ye1LkCa7#q>M2 z!37&HDaN#(VY8ZwM%xqdW0=h#1_JKPjxOoW1aTzH)}yXVw3}1JnR7cLwQOa1jm29P zmCVF2dWu^#Y;nW~8y#z+${DDJS{qjrEq^(9lf$eM5SqnD=Y`9YJWGZm-;F_O{Vegb zj<;}H1coPr?XZ#QD4iUJ#L`j`AG@>$Ioeo1Rap3KHGy047b1~_Z_%3jr|67kcFEml zW#|Hwwcwr=h=*_0a)mJM#G1E|6ar~JdJR?vi5(ni2r~6JH@1pa;Tf|^RT%<8 z)S_wl%0c;kJJtW&8SG_^XE1}lm5}DS3ii6bMqd7_Ny|MogfsvPUc{6p1+nw(ZK8Yt zv!OACFENpb`5mSyX2bx(CCWb%l7Hla^7rvumY>xo2H4FTKC&$ZcEyjC+P)!ItspB_ z6i@VtVyU6CU=Hig?kB^9d?od7~M zee|=MBFM&KS4%ow%zXRVI2}2Ki+{s;to9Y&l)*d| zsIZM1 z#TRHx&0;P|r6SECBu+@(D}q*AjCsoJyBt;{I{^oZ@29_0zxg(N0@t|NTqTGi4X1-7 z6z)2&8o8P3C91={Ehy>)wlpP1D%UIYiS>x5&pp`0uPT*sHh(0dxaxQ-zdH~tdS@qw zSLRN(n7Euc5eS%M&@=3@ydTGqDPk{UVzzXc;~Rfi!k^1rzW_aBP2cRDi4|x5B1>10 z-Nkvhc>8lI%%)f{hGj8#%$CL8=87t)CXF*nhAT2@TrAUd79h?lUW%nw#z4(^FWVyQ z4-2}58$+$GMSsN~)5IqQ|MW+8i7ZOa?@P+C!P=Q#qQD{g;xsLLT|20#={>LLlu1x| zPqu)p1TO}Y+nb!>!ugz05jRrQvFO0#SsTs$P&N9EgG0_B37YSRo7h&>G-aFyDkUeg zPq|v2|Jx?cJECUc^%+cjs3Gu97{5T! z4lBR0I$9Kor|Q{`&vViUuwDY|j*lZx=@<c5%-&pH8V-_75l?alLMsfp; zC|*dbW2vviNLPN5(K0BW#mrQEiV5oOA079=wLX;qt)xPeD$4zHK2S`~%{gi+N^?N;V zoy3DK4!W527RZ^0GDLsU9?nu|M(u-@G|Aa52vQ^0Xx%?2AC#GGBqGv~X_LwLPL$Mn z>Cwoy6&dWw++6x()XiiFMRH~*Fsl$_#P?f+iBZB(vOCjc1`S>ABU*Rt2a(7^wIao|1ycV<)CBW zPq;gxJV_WXKm{r5KH>O=^lDH*$GG*a?Pc_=WB!9g*u6B9i67h z+I=aljVjW|*e|`R?_jVk5Ty`hZQoWbeR=E2X@6m; z!gIt1FPrm0eE-OJf@o1^SXZ9rG`QadiHc;$bYWItHSu1-EH;X~=>p`XLHPT#`_Gzb zEVs|2@5%6wpLn15E8sGMbibV}Ul4+l%fm`eFH=msqn0gzyE{*&6PDzkgf>Ewn#@Yy?31p_Y0yMKSec_+9aEiF&+ z3yh;&v7ve%%p#C_bu_~Cfs@g`+BDsg(~fA$(2SV$9RdfgLzg+<>F*v7(nCH_f0WG5{882P0OX^ndBPY!AL}O`m zpZp=m;enCx$1t$5&42q1smaP9g7l!dvSKx+jvV=l21&0oW+RYafO`06(P-&i=TR}& ziC0IuU-^jXukpTmRANt{Bc9i%1H4G6BD84Ka7-lL8D=`9d9Stj;rn^Q&WhPAfkN;z zY{$li8Z)5gQN#*NQw_q~!q?9sd5)F9&66!3LImRqi}%`2>VF?(fnP_M<5=2cGgY6B zR@tM@a-=Zncfam>4|g&6u=2w3oKEqdUK@gjupM7GH7>GAK6Q!240rpGj*TEn;&3YH zyB&RtwY35bRa6-*$8Q(cUMv&Fw)h2T*|nnU(e{iJCD1#vN_apUA8iJSPY0HwW!~y4 z%xMEJTzCC4^nVOr!p}${E*tdmYd!q#(H_6C*C% zLh2M^#?@y91DXG}Mk(Xm?Brapk0)ApL{ju8X(@!IsCxfa8AO^AXW7wsCGP_s9aA1+F4MTM?Y6 zq)G53j(vassPK2kK-6yS&T3Ylp=o}WdRz_rshD&S_Yc)>E%w7Xmv|>{pz7ixjVI!Q zUP-b>aimzrz1_GmMXZ%YW#m`l!L_T$CCS(&Tn~J#W$;ttavF~Wgu^$st~r?|#ea7Q zWa}SrL60epEvn!Z(u*qx3AwVcYm(1owIj7*A{x7WbzQ5S#umd>kBX0T?_1w5%AP(Y zV~Pv5ik!{<`XLK{!Y^o0$mC9){L2amoK-R3-VroaZpV3OBEkVNEI-oAzLdZHMGN`9 zez6=nfl7M7h7uH&@!-T7J>QAAUw@C?65C}x2IU}incLMR(Q(KuGD)`FxG1vQPcA-T z9a+JM$_Ub+~G^2BF~L+$vJg;)>31@SuWWeH0C-JaRrq>rBl>sb$`B-Ak>F! zgmi8iQTd|~zz$7U3?7AypI#`DIJhZz3DO}*vre|(&QrJrrW`5sQSpIj6BkV{DCbV; z1>7BDAva|mi&g7R|#5lk*4pBkE>|x$X0z9VT~4FlL!$U zeab{Mz$+!x0;qM$KEr+=+&rhwd)hq^YhXFEkxtSjY$DnM5>|!oihnSX!A*yf*CY|L zFd#+=6p?y4h6Ph?f%bLklP|565>E2fVSukMFw;=+0o5ymh_;i`Iom&0o6>@yPZOcY zk23)oh7h?_%%Kyni{s3hLTR=-6-O!1cAj zp6mXJyzRvOV4w|0M$2kAh~WvSvP%+w)gBgDIFYmFw|sk3&4xiS^tNblch2X%UwXN^ zcLBsS&7WlW_^R?ig<;*d0y9pW=I4sF8#Mi%lH8t_U_C<=T;oTf;B+HMMO4y9F#3o; z4bHn$A@J!*H-A{YNx=X!s-{+{)&@ccY~GE8yLrsaU+5m5Rc7pxcRFSu%Uaew1DOt4zZxz$s7!JD3N`5V%a zhlZJJMuQW)aD5nQAcya3vzi()Uuw@`Ecr)P@4HNPEJ*fHpyK1i42czQD&NHgoiNh! z4MlzO?)q;w$2Es9UW9>ArVVJd$ZKgC&ZR!@Dd16aL9+oZ$v6w+2JR0abi|K3pQ zfY=DL`~P$9giZo@Dt;W@8Mw^WJ=!(8Rh zp41CP>V4qbJQKJHlP2(J8a8eREz_OWn{{7j7q*l(nhuTEU8MwcVCKqttACwqH&y4y zn|~Nr!*mAr8oF`zP}?c$D)LTe#frVpIx{$R$ruP}=l~=@*DoahmhIFkyvVf>=aJ2F znQ%1@(|ve$D;Jvsjf26X%RIaZdh;$~IdC|O!~GRBCW=PSsB#cT$MZLGSJaA{ZkFbJy>nWYhg-0mFctAiXI+@SlYjcsK)EDHRng}PUW2*=Mlj_6q<+_T%9o@6 zeR1wpmRrmm$qz6FXSR8o5j;KTr>_P#<)Qd*WaEpd&_dmL4ukGoiYOMh%P zQk~?bpvnsfelmk(aL*1}=o>-DD4(Hr5r?BUj_o0~;xt-K3Fkt1B429&v(2G3EWcVI zm0&W6x=!y}mI&6{+ui!JD88ewT5^&m>8yJzoD1qT!xUV$D6+KhV!Gcwz6WKea*W8~ z>p@Ol#%Fl~_)6wAE=S#)FBd1k`T#)75Lhd|Y!aZWB z*2R!^UXg=b<8LX$Bwmi}QtP4qW`_H(tcP>W^wP0uJCMERs2Hg;OQ-lCL4O-V4DU@D z&6kPVc3Rj#;Y=3&`kdueMfP;sm;sOF!==Y*VnSL9Gm!VqdD~fkjs1Pzry?cZ$a`Wd z_@J(xtrBS5FyV-GA#tIo06_NoU0@!ZxChfZ(klvO`_%V@hO`>yL!6CantO zOTun7_ug4~-X=y%=!|ST+vlf;*KCygP*C%|hC{*)ZARF(pS*S&sfJ#p+XH}RwktZ> z)zN=NgsG-XhW7rrg?&|296+-sE`t+f@F5T^3^4fM3>Ms7g1fs9?t$R$4uRmoH8=!< zy9Rd%5G=X;d-v|{+rD*I^{MVY-RG2ab$xCq48pM*K7I08sX_@3Fosv(X+#e%;S?>= z#(xPuOeZ(=xrld#5G0Zb%Jz?ok3!tVf=SJ`wTD7eAYEBf?_>4_B6K{(O`c-@x{o5P zxGn4UE4Fl6k+u4k{UK*0%k!H}L5pG<`BB=J7d0aDVOkI+7SphTb>eT(GLS3Q=$A34 zfV2$f*xS*W_g-AiapMPvmUXJ<;?tzPO(0fyXc%eN;#$v+i-ZD;Lxb*T8ZT76SpaXp z9H~=9B{pO@D9T^+8v*V7^HZ6UKLz;2?hYZ` zV*r{*4EFxL*i_0>To$}$m zl|iY8)RJ!`MD<#Irrn60xchLc8WXHVnBu!!f;Se}>|HBZ>B zc!d%OB3?|o<&s?#EG9SW>k}^^Y@6#lFi1Rfb9{NjYAl*?HW#~bX-Pt^x&Uu1DQ|LI zxH#u|MB9DCV%$l0QJQn7Xr_VaW^nnbs`!oI8|>QPhVC;ZI@GtddiHaMcm>ZBLqv{| zt&}bucc|&bTSupSXdGbioA#j%`LG*<4?;i zK{U;Us8N(Oy%GP7=Ew8D>m%WQL9P%)g7CdUB{SedxjFs^u@A(}1>r~*&trl&>)Sc4 zwI;ose=JPsz_7nk1hlq_<vQTlSD|vd z_k9Y`APxZrf}mg&QjXjkm^B>Ntd&*roiSpq34u*PFjdaGEfqpvq_oPCkO)>Hx`OcO za6)pLLOp5953*Q_P=)l)>9Jm8HR6a5Iw2MP7#4ss9}|sm94+7g%Hew~by*}FRSn6| zoh%Y!IwGQUsljllaVndbIHnl_X83*xODYLS1`#-<(u!=2k361 zz7xYuCf5V}P=Hw5^oDWy+hWUnDbN&`M*tEz!sIoli>8wb-j+_Z&^Ph3{y61FWHUuz zYH~J=;3`27CW}{>q~!QbGQTAu?GhpF#`J^3n4ONl7)gOZe+=XIb{H5TSgU}zcne=I z_{T)84R%Wq>0FSOf(qOvh*v^>z*-1F4_M<8ValPmjjh=S4ClhGEpd)e1u5^u0iG3H zNEi#4fuUfrbU;R*cmgtDu8@)naetsM^K5{Y-!}f9z-k&CLtPFm!kjy+;56P zsh$f{1Wdyhb}%r$9wx$!B=i#jE2&jmmBGB%8aul&u9PJY1Zp#%={S<7>2$4Y6Igz6 z3>-vIW%hr#euc?FSRxANQ19#=S;PlWT9<<=>OYBFDOjxDAs}!MaY#Gv?Y~7tLE#rs z$DjNl*_q&qJ<@#5ff@AXQfwKx>n)a5=LQ+ZNI?~F26arr0^?BlmMDoHyFZzo8hGQ{ z^dGZSIit1#`Rk~ST!bDl|B06&q1(Jq2w8MCY?3vLfXm?)P=|6C)g*1RM5&|T;0zTY zRDXH0TvE@9a&Wz*iry={b* zfnMth2Sk3~fiMhJd*OZiO8iTo=O!79=JKl!lP+&YGRmhPzFyQ+V+FvDL_nXXl^OHy z51=WorI#n&mr7jEwAQt@(b}^2$hu0h;oh9#a5Cjtx(@xm#JPeLxh(-0<+c&>)YvGZ z07#QXA?q`{Gd>+t5$tOq@+DGO?<#8r6jEvP||2JLSS{qrTY6aDYyWl=m~h zvF)))$dFEueyesHzPX)kph?h9`M1Q1|EnO*c`IOPrtL8lecyEGF|a~gOO&bS59OJ} z%S`>Y?P5~Z#j4BPA~Yd(5T_Pt2&F3|zE!%lti^;P!TIA;TV_C1TRM$-LA>`Ma$X^0 z7eik%zv*nU+A7PQmFQSME$h-^3-9lFuM&{PwP&x_Y3 z?>i(?-76-jmCcX4oywBDd#(JITgQPI!hK)*nsxCvYvbbJPs`QWWoQ1b8IzEF0Pa-1 zRz!KFC^OX!<0%)_IL*n_WtM5C@&q2;8*T3@LF5iqX?~Tdn4gFJ^eeEi@R47Ea4Q=+ zwq=XE#KU6VPlp?4fkwxQ{`G(j1v*ZH*{Cy*dk0}tuW87j`PO6~TQWl)23-T8t)Zll z*(vVwFTq)nV++Da{yNT*auSx{&~+Z3YCd*de{@P#i@gDvuHF%o|C6^ zr)Rih%7GDEL?~ud)YH3XIsaW6`agyY#@_P_7dXbn1`)pw%56*HuQ_khZL$xs+eo z*T;bZ4(-?NhK9-%C>3F}T+>yZhCDnNt~*w@uIu12UBO6##wVLaw(`w!A2)q3e62LS zJ=5ihE`Av(H!h087<{iPmxmTO?jdMbe#>(6T1eA-XKy@ts>eUj^LqRDq?R)9*W1AO zkgie^?!C8@Lc!~YnCcGOMOx7`O$sm0`XN_)?WgBjig}{^tHWP5je2|^9v7{g*b=^F zp$<{*_~Gp$iN0f?Wtanfud-fPuuXTBznUwH6-hooJ4?mi%!TI!bb7zoH+Sng-Kx8c zoE?}1cL3g;jrL2uX_(KF7z+_*l?{)xy6a@xOd)M!_Os|oC{K6F|M&Ow`uB_98kqwG z?XD~G4>;EqY%c2nLFxqT1#>u`;0Fed~r|BExGln{l1- z3?IPAa;K?-B5-KHYh1r^yf#l6B=IpYKlf8;_*X3zJ-9P+0{jNVjp-+gX7ZJ*X@aCl zt?q+TwdKnEUI$sp-Su-Vn0-}0s5*DE>89n$sP+O;c;caL+i$N;nI`b%_E&)ApY6S- zJgDH8^4}>r{&e?H#F)&?tfP>~5Z)}4KtF9G5Q_$c?A7EdHdN3B(d1U#Ib{@S}Ch__F`e+{Qc z+oTVm3#metQ$~#v|198(2i__W{I*eefT!5wUsGD<3Ll%sHPf`s4VosUxpv9`4o07S zAvOo!jD=Iky@Rtp@}2D9Wldj|f%)&vS-n$TO%*7_23L0#KZs)L5bAioRi(6Gtp@ji zb6M*R5Og@vyM6YFW9}!yFR78KjPhHg4;d+nW&KHrr(jJL=CIA88QHOpqiWe>coMFwrYpN` zT=~WvuJgAm*WK#`HUrilHmt@Ig*V}7FAmI?%BBskEA|L%!f*Q`>$Z31om~=Fj7rP2 zpV&p7F?e)xXV-fhTLtKzzIH#52pPaxq z*IPaTt6xmMb+rz!KOz3szJvS4xznn3J2KXBnVwhc1dH-EYfG#*H&_$paKM|<0t3d# z*WO`1y%F~#HYzid>`VP9A6x#_q*Gycu_8w}{DM?A&>B6^@>WTUnsPswH;~{wyc!a|xeoVIU%IFjM#lQe zNI3mBYUOTC?Mwc0zaXsdneXTt${lcbw}1C$N_%qi&g+EP<;9-BYS!KpNMosO)+j3~ zXwMVbxXFumr;Jb6`0~;4%)VRQEo)THtXX@)gqY-7hYGh3kcyP{+wTWhUM-gm3ZVMb zJDc14RO9hkYS>(gPX*pCLGqU!DVp``o|u+9U^I7T%F{OyQvJalU?OOhtnr3Z8V?rA zs+URCaB20Y@(V$ib!jncy*kObc_KttXzcB7!|7>(w#cf+i+!=@gWO)XoV=07?9YV( zQtS7T@?)*PdYM_cHsUhviYS_vdGyO23#8*lW`5%6ou&+O>YBlQj}Flkhb-EbeJ5~O z+QrY@Lr2U}(f#-i=WzOdZJ|L7INQZ>)&Q29@Th@w9zC26%DIw@wY=Ir=P3}OF$=UcjR{+=83G6pZO z_^70*`&$e}*125@S~R>*GTo-0bI7#(RNp{)a#8TS&ov9=prggdx+=i6GHqZHDpUN@ z7fP5DE4xiy-BbUJTjj*SQ2pI3oPWWj8Cf%RMlYdGU_q-H*)$dQMH&<@r4=u6Y8hWZ zE`MB5WC{NzFOC!Dsi0w_gx7(sEXk!)C1d1x0F@7>dEZrkjSFCx_d+n!WF?svf0?i< z(}^YcV)l-TXhuOnOJlex`kjZ0svCs)+ALf+ZB#sUBIp;L*bpJK;N2Nc2w;e@@svHoAe)g!W^ESQsx_kYBJm|kUr6l^># zK%5*9K6Xh9QyUXUW{{n;iMzF%g$c;h!qx3{0uV17CmRS0X&YBJcMumh>c2^6#J$VkJQ3A9f7(6b6$pKq$x+ z<~N|sK7ucbY$qq~iNDdfc3syuj@1A>Wu?>mV21pU!#=%09_Cy65J}AG-C_#QRZ(k{ z9sf~8LtPMOLdAz6C4Pm+oHFY!t`rQQaMwHRlNiRR$v}h>t}wL>#5bo-%m#n8EdPPU z1&j*eh+)Q`<%yt5O7GVh&gmi?YJ`ORL^Pq*fYw>mr3B~t;>x1_J;(*&R8d!As8~XZ zN`MIdh|RnPLVlhux5I($WKg(*(!~X-HakD+(V|nwt~Vd;^3g4Sp!nb2@$9RU?>7=rf(n)vsaY z$N_h+jG>FY(rL#ERS)L47R(nLa;HK|YbFHi&#VZOVo{wNkouRiz^a@n+Y8$e0zM!$ zz|VaYoGBc*#8tkSFuZ%N&vAhlIi(;l|8{(1zu*cLlpToVccl z>E3!B4|gI#$x$>*I=$*MV1GLau#O5z>L81p{g8Cl^=)r=Fn9oNPQFw)tWQT>w;2a= z)jM%Q$G5AJSnr2m3LL(caarHkjI)Uu1yJCtRfqxKRf%OZnIwCCm~@+Q)Scp_UvC=| zN+UOXkQ{CW46yztww%@t{8XqV(O8f|37A)`WvbDz8?UJ5w!@anP2VTO`ru&c+tZQ?OlqzD(oGC^ZbcM1KnA_!LW7g}$Weu8MwOc#j zyXiD)aq~+_*I1*ZbV{l?={Kn4fXw8|0I@n_w&UZ5c}CBsLA;@{caqx(*Zn#Hk?t}ObAX5Pq|*5AtK`}V>QS|^NiF!wU}aTM}Xdkj(@ zOHA+6qs30D3lU5=)5+_PNTr9F*;EGKTfuPK#~GXb=4_5A@h+TZ;$^_PV>-{z2RlM> zGwr4%{xt0y`UB(s7`4FPJg0IWryO@KzwDK z8#;@jkEFRPjtQqGfwP8&jY*Guw&aUNW!q@78$L@1?reeUg?PgZR_}g`w?F(CnLARW zol(4Q%pq}zwfa_n|2=9`uh6Kx>>9fqm2c9LJQdv6h3~!W9X(KDE;$I1$Ifn`fy{%K zBRoWYm6q&V-OOkn3O&^=J?IFgOjs^IoDdxtB1EcVz8oh&kr7Rgd`lT(jv$#%*5YUX^Yz^kI6Z8|v{@n?oG~MJh`D3b(WJ z8AiB?@Gk?VdPao=auKrHzFnUv7`oM1E=hbj#%kk0;CfrctVfUPJ(l)ZrmL0ocelh7 z)&0S#d!`$tL;W$5La(glx4)fhSyDs-PG}XGW;bp0A^9a&)Zd5({e+5FW(9et)$%v$ z$JLI1$A3z8^n@;}FB}r~p~-h7)1H1+*apH^tD9;naXQ9z%^=S=f9d31bYresSrw-= z-Wa5nl}{*poaa@*-%hf*qVKFggqcMh0xbbnKe~>dm+X_Q&@glBu4XuFe1L`Fd>o!<{Vua%@!xX$i|_tWi9evMlcr2v z$ZI6O>!RAeo18``dik}T5*BG zf~x}&hv(_>Vq9MMyJ}G2DLZH`{;q0hV=n0cHn^k?B8$tSb7Yui0l62|eOw*d+zyG0 zGUkbx@HzE)P#q%oj+XONm|_80`gR_E9wNAvYK_|DJ%8-#;2B{{u;&`JV`Lq&`_7$9 z0%k{VTff5u;To?A-(g?s7B#_Lh=>7>W9+3+Ymi{*D*Y)ssf!idg(3{u7+)s-8V@s4 zEQX@bB5c4Ig)hcWj2|%MyxZ#0@4^wB-B&}P6vDj5dJgWlamMr_`zFVaZ0R!gGoXLm z824M4K5Zw`zKfU3c$jt{PtLVx!li&tFL?Kya04Nhf6pQMlA$%+FRDYrn_{icVVlVI z#_)AetjfoxI6rvEtjv+j#xT_u3Fa+2*1t7V-17Xw?8f-_{b2m#AeLTDFCmLaXt+;EHg#d4w#pqcF?JLnik8MlLdH%>& z>KrxcS)an?&4{lsRlZEp4tjs1JX{O&$m|dAJ5r%ejTj*ERA8~soglcQ%h1_Z}>@3uw&&RC;ZtB zhjV3lUO1p2QibnINCoTY7~0D{KKs4aPwu4soc`0HFMG{NKoHuEBi20Q-Y=7ap_7AM zJ`X)>>P1Z52f?{%?^ZSIFd5W*`GD2s0JQ#PJEVlH2>Ud`jP)_zCjU_)yb`Yqziwg-M!a>HI^>+vS*DW3;%eTSx10lpf`$4^lZi6qz&O<68hwbK>vsCzg#g zCoIgde2C1kC9JMX-=!J{X&q=S5<^)wkSUcDzerPwl`z(3)qJtx!u%up@bI@GH@pOP(!%}letEZ_uvs9_s{Eh}}qiHr; zovt^pcl|gwuX}w_t~ammc;2jqlLlYST&()(`thYIRY^V9cp%M>D^mWU%?7dsWd);J z6i{je$@Mblx!RWtWtYyG>;2~%Y#}H=#HY{+?D>El5V2oUcIoGD8mTjmj$T`P^s-Q^ z8OL6ioI50?vXmR9`Ca#(y}+kp=Zk-jT+evn721IR>sSFS(m63v2w&YEer~|KcT&pI Gfd2ygcqJ_W From f2ac90d5b9682f4b78d8ffe99be378ed32df11c5 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 03:35:44 -0400 Subject: [PATCH 109/169] explicitly define simpified jacobian expressions for efficiency --- gtsam/geometry/Pose3.cpp | 62 +++++++++++++++++++++------------------- 1 file changed, 32 insertions(+), 30 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 5a3b20782..ab3afe37c 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -69,36 +69,33 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, OptionalJacobian<6, 6> H_xib) const { // Ad * xi = [ R 0 . [w // [t]R R ] v] - // Declarations and aliases - Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw; + // Declarations, aliases, and intermediate Jacobians easy to compute now Vector6 result; auto Rw = result.head<3>(); const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); + Matrix3 Rw_H_R, Rv_H_R; + const Matrix3 &Rw_H_w = R_.matrix(); + const Matrix3 &Rv_H_v = R_.matrix(); + const Matrix3 pRw_H_Rw = skewSymmetric(t_); // Calculations - Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); - const Vector3 Rv = - R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); - const Vector3 pRw = - cross(t_, Rw, pRw_H_t, (H_this || H_xib) ? &pRw_H_Rw : nullptr); + Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr /*, Rw_H_w */); + const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr /*, Rv_H_v */); + const Vector3 pRw = cross(t_, Rw /*, pRw_H_t, pRw_H_Rw */); result.tail<3>() = pRw + Rv; // Jacobians if (H_this) { + // pRw_H_thisv = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R + // where [ ]x denotes the skew-symmetric operator. + // See docs/math.pdf for more details. + const Matrix3 &pRw_H_thisv = Rw_H_R; *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // - /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) + /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_thisv) .finished(); - /* This is the "full" calculation: - Matrix36 R_H_this, t_H_this; - rotation(R_H_this); // I_3x3, Z_3x3 - translation(t_H_this); // Z_3x3, R - (*H_this) *= (Matrix6() << R_H_this, t_H_this).finished(); - */ - // But we can simplify those calculations since it's mostly I and Z: - H_this->bottomRightCorner<3, 3>() *= R_.matrix(); } if (H_xib) { - *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // + *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // note: this is Adjoint /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) .finished(); } @@ -113,32 +110,37 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, OptionalJacobian<6, 6> H_x) const { // Ad^T * xi = [ R^T R^T.[-t] . [w // 0 R^T ] v] - // Declarations and aliases - Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, tv_H_t, tv_H_v, Rtv_H_R, Rtv_H_tv; + // Declarations, aliases, and intermediate Jacobians easy to compute now Vector6 result; const Vector3 &w = x.head<3>(), &v = x.tail<3>(); auto Rv = result.tail<3>(); + Matrix3 Rw_H_R, Rv_H_R, Rtv_H_R; + const Matrix3 Rtranspose = R_.matrix().transpose(); + const Matrix3 &Rw_H_w = Rtranspose; + const Matrix3 &Rv_H_v = Rtranspose; + const Matrix3 &Rtv_H_tv = Rtranspose; + const Matrix3 tv_H_v = skewSymmetric(t_); // Calculations - const Vector3 Rw = - R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr); - Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr); - const Vector3 tv = - cross(t_, v, H_this ? &tv_H_t : nullptr, H_x ? &tv_H_v : nullptr); - const Vector3 Rtv = R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr, - (H_this || H_x) ? &Rtv_H_tv : nullptr); + const Vector3 Rw = R_.unrotate(w, H_this ? &Rw_H_R : nullptr /*, Rw_H_w */); + Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr /*, Rv_H_v */); + const Vector3 tv = cross(t_, v /*, tv_H_t, tv_H_v */); + const Vector3 Rtv = + R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); result.head<3>() = Rw - Rtv; // Jacobians if (H_this) { - *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, -Rtv_H_tv * tv_H_t, + // Rtv_H_thisv = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R + // where [ ]x denotes the skew-symmetric operator. + // See docs/math.pdf for more details. + const Matrix3 &Rtv_H_thisv = Rv_H_R; + *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_thisv, /* */ Rv_H_R, /* */ Z_3x3) .finished(); - // See Adjoint(xi) jacobian calculation for why we multiply by R - H_this->topRightCorner<3, 3>() *= R_.matrix(); } if (H_x) { - *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // + *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // note: this is AdjointT /* */ Z_3x3, Rv_H_v) .finished(); } From 761987a14ced11637d31efc43a3d81250eea0c0a Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 03:37:34 -0400 Subject: [PATCH 110/169] stricter tolerances --- gtsam/geometry/tests/testPose3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f0f2c0ccd..4ea3519d2 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -204,19 +204,19 @@ TEST(Pose3, AdjointTranspose) T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); - EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH2, actualH2)); T2.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi); - EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH2, actualH2)); T3.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); - EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH2, actualH2)); } From bc1104c8076beaa29824eeeb6fb40e9d511a47ac Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 11:20:55 -0400 Subject: [PATCH 111/169] Revert "stricter tolerances" This reverts commit 761987a14ced11637d31efc43a3d81250eea0c0a. --- gtsam/geometry/tests/testPose3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 4ea3519d2..f0f2c0ccd 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -204,19 +204,19 @@ TEST(Pose3, AdjointTranspose) T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); T2.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); T3.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); } From 31e5cbb81aac00bbc0d51cb6df26598c90500187 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 13:49:04 -0400 Subject: [PATCH 112/169] address frank review comments --- gtsam/geometry/Pose3.cpp | 60 +++++++++++++++++++++------------------- 1 file changed, 31 insertions(+), 29 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ab3afe37c..ed605d8b6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -65,53 +65,54 @@ Matrix6 Pose3::AdjointMap() const { /* ************************************************************************* */ // Calculate AdjointMap applied to xi_b, with Jacobians -Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, +Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_xib) const { // Ad * xi = [ R 0 . [w // [t]R R ] v] // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; + Vector6 result; // = AdjointMap() * xi auto Rw = result.head<3>(); - const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); - Matrix3 Rw_H_R, Rv_H_R; - const Matrix3 &Rw_H_w = R_.matrix(); - const Matrix3 &Rv_H_v = R_.matrix(); - const Matrix3 pRw_H_Rw = skewSymmetric(t_); + const auto &w = xi_b.head<3>(), &v = xi_b.tail<3>(); + Matrix3 Rw_H_R, Rv_H_R, pRw_H_Rw; + const Matrix3 R = R_.matrix(); + const Matrix3 &Rw_H_w = R; + const Matrix3 &Rv_H_v = R; // Calculations - Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr /*, Rw_H_w */); - const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 pRw = cross(t_, Rw /*, pRw_H_t, pRw_H_Rw */); + Rw = R_.rotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); + const Vector3 Rv = R_.rotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); + const Vector3 pRw = cross(t_, Rw, boost::none /* pRw_H_t */, pRw_H_Rw); result.tail<3>() = pRw + Rv; // Jacobians - if (H_this) { - // pRw_H_thisv = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R + if (H_pose) { + // pRw_H_posev = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R // where [ ]x denotes the skew-symmetric operator. // See docs/math.pdf for more details. - const Matrix3 &pRw_H_thisv = Rw_H_R; - *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // - /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_thisv) + const Matrix3 &pRw_H_posev = Rw_H_R; + *H_pose = (Matrix6() << Rw_H_R, /* */ Z_3x3, // + /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_posev) .finished(); } if (H_xib) { - *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // note: this is Adjoint + // This is just equal to AdjointMap() but we can reuse pRw_H_Rw = [t]x + *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) .finished(); } - // Return + // Return - we computed result manually but it should be = AdjointMap() * xi return result; } /* ************************************************************************* */ /// The dual version of Adjoint -Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, +Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_x) const { // Ad^T * xi = [ R^T R^T.[-t] . [w // 0 R^T ] v] // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; + Vector6 result; // = AdjointMap().transpose() * x const Vector3 &w = x.head<3>(), &v = x.tail<3>(); auto Rv = result.tail<3>(); Matrix3 Rw_H_R, Rv_H_R, Rtv_H_R; @@ -122,30 +123,31 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, const Matrix3 tv_H_v = skewSymmetric(t_); // Calculations - const Vector3 Rw = R_.unrotate(w, H_this ? &Rw_H_R : nullptr /*, Rw_H_w */); - Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 tv = cross(t_, v /*, tv_H_t, tv_H_v */); + const Vector3 Rw = R_.unrotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); + Rv = R_.unrotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); + const Vector3 tv = cross(t_, v, boost::none /* tv_H_t */, tv_H_v); const Vector3 Rtv = - R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); + R_.unrotate(tv, H_pose ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); result.head<3>() = Rw - Rtv; // Jacobians - if (H_this) { - // Rtv_H_thisv = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R + if (H_pose) { + // Rtv_H_posev = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R // where [ ]x denotes the skew-symmetric operator. // See docs/math.pdf for more details. - const Matrix3 &Rtv_H_thisv = Rv_H_R; - *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_thisv, + const Matrix3 &Rtv_H_posev = Rv_H_R; + *H_pose = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_posev, /* */ Rv_H_R, /* */ Z_3x3) .finished(); } if (H_x) { - *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // note: this is AdjointT + // This is just equal to AdjointMap().transpose() but we can reuse [t]x + *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, /* */ Z_3x3, Rv_H_v) .finished(); } - // Return + // Return - this should be equivalent to AdjointMap().transpose() * xi return result; } From 108c77b57a48c322ad2b70201d4e1d2d652b923c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Oct 2021 08:52:00 -0400 Subject: [PATCH 113/169] use variables to store targets --- python/CMakeLists.txt | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index c3524adad..eabf5872a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -65,8 +65,10 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) +set(GTSAM_TARGET gtsam_py) +set(GTSAM_UNSTABLE_TARGET gtsam_unstable_py) -pybind_wrap(gtsam_py # target +pybind_wrap(${GTSAM_TARGET} # target "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp "gtsam" # module_name @@ -78,7 +80,7 @@ pybind_wrap(gtsam_py # target ON # use_boost ) -set_target_properties(gtsam_py PROPERTIES +set_target_properties(${GTSAM_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam" @@ -98,7 +100,7 @@ create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") # Add gtsam as a dependency to the install target -set(GTSAM_PYTHON_DEPENDENCIES gtsam_py) +set(GTSAM_PYTHON_DEPENDENCIES ${GTSAM_TARGET}) if(GTSAM_UNSTABLE_BUILD_PYTHON) @@ -122,7 +124,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) - pybind_wrap(gtsam_unstable_py # target + pybind_wrap(${GTSAM_UNSTABLE_TARGET} # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp "gtsam_unstable" # module_name @@ -134,7 +136,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) ON # use_boost ) - set_target_properties(gtsam_unstable_py PROPERTIES + set_target_properties(${GTSAM_UNSTABLE_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam_unstable" @@ -150,7 +152,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) "${GTSAM_UNSTABLE_MODULE_PATH}") # Add gtsam_unstable to the install target - list(APPEND GTSAM_PYTHON_DEPENDENCIES gtsam_unstable_py) + list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_UNSTABLE_TARGET}) endif() From 7793a2ddc1476ebf4f81b6f9c84f79bca2107e64 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Oct 2021 09:16:49 -0400 Subject: [PATCH 114/169] clean up the __init__ file --- python/gtsam/__init__.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 70a00c3dc..d00e47b65 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1,6 +1,12 @@ -from . import utils -from .gtsam import * -from .utils import findExampleDataFile +"""Module definition file for GTSAM""" + +# pylint: disable=import-outside-toplevel, global-variable-not-assigned, possibly-unused-variable, import-error, import-self + +import sys + +from gtsam import gtsam, utils +from gtsam.gtsam import * +from gtsam.utils import findExampleDataFile def _init(): @@ -13,7 +19,7 @@ def _init(): def Point2(x=np.nan, y=np.nan): """Shim for the deleted Point2 type.""" if isinstance(x, np.ndarray): - assert x.shape == (2,), "Point2 takes 2-vector" + assert x.shape == (2, ), "Point2 takes 2-vector" return x # "copy constructor" return np.array([x, y], dtype=float) @@ -22,7 +28,7 @@ def _init(): def Point3(x=np.nan, y=np.nan, z=np.nan): """Shim for the deleted Point3 type.""" if isinstance(x, np.ndarray): - assert x.shape == (3,), "Point3 takes 3-vector" + assert x.shape == (3, ), "Point3 takes 3-vector" return x # "copy constructor" return np.array([x, y, z], dtype=float) From 21c19453346598c679992673b70b72e777242bfa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Oct 2021 12:23:38 -0400 Subject: [PATCH 115/169] address review comments --- python/CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index eabf5872a..2b3ed3852 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -65,10 +65,10 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) -set(GTSAM_TARGET gtsam_py) -set(GTSAM_UNSTABLE_TARGET gtsam_unstable_py) +set(GTSAM_PYTHON_TARGET gtsam_py) +set(GTSAM_PYTHON_UNSTABLE_TARGET gtsam_unstable_py) -pybind_wrap(${GTSAM_TARGET} # target +pybind_wrap(${GTSAM_PYTHON_TARGET} # target "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp "gtsam" # module_name @@ -80,7 +80,7 @@ pybind_wrap(${GTSAM_TARGET} # target ON # use_boost ) -set_target_properties(${GTSAM_TARGET} PROPERTIES +set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam" @@ -100,7 +100,7 @@ create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") # Add gtsam as a dependency to the install target -set(GTSAM_PYTHON_DEPENDENCIES ${GTSAM_TARGET}) +set(GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_TARGET}) if(GTSAM_UNSTABLE_BUILD_PYTHON) @@ -124,7 +124,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) - pybind_wrap(${GTSAM_UNSTABLE_TARGET} # target + pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp "gtsam_unstable" # module_name @@ -136,7 +136,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) ON # use_boost ) - set_target_properties(${GTSAM_UNSTABLE_TARGET} PROPERTIES + set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam_unstable" @@ -152,7 +152,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) "${GTSAM_UNSTABLE_MODULE_PATH}") # Add gtsam_unstable to the install target - list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_UNSTABLE_TARGET}) + list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) endif() From e0425f3548f7340184d47fb9a461215e428880e5 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 20 Oct 2021 14:27:42 -0400 Subject: [PATCH 116/169] add logmap expression --- gtsam/slam/expressions.h | 17 +++++++++++++++++ gtsam/slam/tests/testSlamExpressions.cpp | 6 ++++++ 2 files changed, 23 insertions(+) diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index c6aa02774..3b8ea86d3 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -138,4 +138,21 @@ Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } + +/// logmap +// TODO(dellaert): Should work but fails because of a type deduction conflict. +// template +// gtsam::Expression::TangentVector> logmap( +// const gtsam::Expression &x1, const gtsam::Expression &x2) { +// return gtsam::Expression::TangentVector>( +// x1, &T::logmap, x2); +// } + +template +gtsam::Expression::TangentVector> logmap( + const gtsam::Expression &x1, const gtsam::Expression &x2) { + return Expression::TangentVector>( + gtsam::traits::Logmap, between(x1, x2)); +} + } // \namespace gtsam diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp index 294b821d3..186c32ac3 100644 --- a/gtsam/slam/tests/testSlamExpressions.cpp +++ b/gtsam/slam/tests/testSlamExpressions.cpp @@ -58,6 +58,12 @@ TEST(SlamExpressions, unrotate) { const Point3_ q_ = unrotate(R_, p_); } +TEST(SlamExpressions, logmap) { + Pose3_ T1_(0); + Pose3_ T2_(1); + const Vector6_ l = logmap(T1_, T2_); +} + /* ************************************************************************* */ int main() { TestResult tr; From 60d7514e15b39eb6751456e11793a7eb12ee6955 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 20 Oct 2021 14:27:56 -0400 Subject: [PATCH 117/169] add logmap expression --- gtsam/slam/tests/testSlamExpressions.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp index 186c32ac3..b5298989f 100644 --- a/gtsam/slam/tests/testSlamExpressions.cpp +++ b/gtsam/slam/tests/testSlamExpressions.cpp @@ -58,6 +58,7 @@ TEST(SlamExpressions, unrotate) { const Point3_ q_ = unrotate(R_, p_); } +/* ************************************************************************* */ TEST(SlamExpressions, logmap) { Pose3_ T1_(0); Pose3_ T2_(1); From 4dbd006d7d6160b4f83f08268cf03788d3df4cad Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 10:18:39 -0400 Subject: [PATCH 118/169] add LAGO (for Pose2) to python wrapper --- gtsam/slam/slam.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 1c04fd14c..e41dee433 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -334,5 +334,9 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { Vector evaluateError(const T& R1, const T& R2); }; + +#include +gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); +gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath); } // namespace gtsam From 2a9ac7c166560957ecaaab2b8608eec37ad9dff0 Mon Sep 17 00:00:00 2001 From: Ivor Wanders Date: Thu, 21 Oct 2021 10:23:32 -0400 Subject: [PATCH 119/169] Fix dangling reference in static allocation. --- gtsam/slam/lago.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 70caa424f..f8b092f86 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -36,7 +36,7 @@ static const Matrix I = I_1x1; static const Matrix I3 = I_3x3; static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = - noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished()); + noiseModel::Diagonal::Sigmas(Vector1(0)); static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); From c5e24dbae4828800904bf533e93b02a10c7dff43 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 10:37:00 -0400 Subject: [PATCH 120/169] add LAGO example to Python --- .../gtsam/examples/Pose2SLAMExample_lago.py | 71 +++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 python/gtsam/examples/Pose2SLAMExample_lago.py diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py new file mode 100644 index 000000000..beac0f8e0 --- /dev/null +++ b/python/gtsam/examples/Pose2SLAMExample_lago.py @@ -0,0 +1,71 @@ +""" +GTSAM Copyright 2010, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) +See LICENSE for the license information + +A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem +using LAGO (Linear Approximation for Graph Optimization). +Output is written to a file, in g2o format + +Reference: +L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate +approximation for planar pose graph optimization, IJRR, 2014. + +L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation +for graph-based simultaneous localization and mapping, RSS, 2011. + +Author: Luca Carlone (C++), John Lambert (Python) +""" + +import argparse +from argparse import Namespace + +import numpy as np + +import gtsam +from gtsam import Pose2, PriorFactorPose2, Values + + +def vector3(x: float, y: float, z: float) -> np.ndarray: + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=float) + + +def run(args: Namespace) -> None: + """Run LAGO on input data stored in g2o file.""" + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None else args.input + + graph = gtsam.NonlinearFactorGraph() + graph, initial = gtsam.readG2o(g2oFile) + + # Add prior on the pose having index (key) = 0 + priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) + graph.add(PriorFactorPose2(0, Pose2(), priorModel)) + graph.print() + + print("Computing LAGO estimate") + estimateLago: Values = lago.initialize(graph) + print("done!") + + if args.output is None: + estimateLago.print("estimateLago") + else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel = gtsam.NonlinearFactorGraph() + graphNoKernel, initial2 = gtsam.readG2o(g2oFile) + gtsam.writeG2o(graphNoKernel, estimateLago, outputFile) + print("Done! ") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="A 2D Pose SLAM example that reads input from g2o, " + "converts it to a factor graph and does the optimization. " + "Output is written on a file, in g2o format" + ) + parser.add_argument("-i", "--input", help="input file g2o format") + parser.add_argument("-o", "--output", help="the path to the output file with optimized graph") + run(args) From 769c75c01f14ccf73be3f77da7f21b81e9c8bf47 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 11:03:29 -0600 Subject: [PATCH 121/169] use nested namespace in wrapper from Varun's suggestion --- gtsam/slam/slam.i | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index e41dee433..24139444c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -336,7 +336,9 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { }; #include -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath); - +namespace lago { + gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); + gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath); +} + } // namespace gtsam From b9f10cdb153eed5863421cc30cf51602c632d333 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 11:04:06 -0600 Subject: [PATCH 122/169] use nested namespace --- python/gtsam/examples/Pose2SLAMExample_lago.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py index beac0f8e0..92a219d21 100644 --- a/python/gtsam/examples/Pose2SLAMExample_lago.py +++ b/python/gtsam/examples/Pose2SLAMExample_lago.py @@ -46,7 +46,7 @@ def run(args: Namespace) -> None: graph.print() print("Computing LAGO estimate") - estimateLago: Values = lago.initialize(graph) + estimateLago: Values = gtsam.lago.initialize(graph) print("done!") if args.output is None: From 6145466decfe06eaf311dd651ce244cfcc8f92a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 15:02:24 -0400 Subject: [PATCH 123/169] add type annotations --- python/gtsam/examples/IMUKittiExampleGPS.py | 30 +++++++++++---------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index a23f98186..5a9811522 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -4,11 +4,11 @@ Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BEN Author: Varun Agrawal """ import argparse - -import numpy as np +from typing import List import gtsam -from gtsam import Pose3, noiseModel +import numpy as np +from gtsam import ISAM2, Point3, Pose3, noiseModel from gtsam.symbol_shorthand import B, V, X @@ -31,7 +31,8 @@ class KittiCalibration: class ImuMeasurement: """An instance of an IMU measurement.""" - def __init__(self, time, dt, accelerometer, gyroscope): + def __init__(self, time: float, dt: float, accelerometer: gtsam.Point3, + gyroscope: gtsam.Point3): self.time = time self.dt = dt self.accelerometer = accelerometer @@ -40,14 +41,14 @@ class ImuMeasurement: class GpsMeasurement: """An instance of a GPS measurement.""" - def __init__(self, time, position: gtsam.Point3): + def __init__(self, time: float, position: gtsam.Point3): self.time = time self.position = position -def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", - gps_data_file="KittiGps_converted.txt", - imu_metadata_file="KittiEquivBiasedImu_metadata.txt"): +def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", + gps_data_file: str = "KittiGps_converted.txt", + imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt"): """ Load the KITTI Dataset. """ @@ -56,7 +57,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma # AverageDeltaT imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) - with open(imu_metadata_file) as imu_metadata: + with open(imu_metadata_file, encoding='UTF-8') as imu_metadata: print("-- Reading sensor metadata") line = imu_metadata.readline() # Ignore the first line line = imu_metadata.readline().strip() @@ -70,7 +71,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", imu_measurements = [] print("-- Reading IMU measurements from file") - with open(imu_data_file) as imu_data: + with open(imu_data_file, encoding='UTF-8') as imu_data: data = imu_data.readlines() for i in range(1, len(data)): # ignore the first line time, dt, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = map( @@ -86,7 +87,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", gps_measurements = [] print("-- Reading GPS measurements from file") - with open(gps_data_file) as gps_data: + with open(gps_data_file, encoding='UTF-8') as gps_data: data = gps_data.readlines() for i in range(1, len(data)): time, x, y, z = map(float, data[i].split(',')) @@ -96,7 +97,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", return kitti_calibration, imu_measurements, gps_measurements -def getImuParams(kitti_calibration): +def getImuParams(kitti_calibration: KittiCalibration): """Get the IMU parameters from the KITTI calibration data.""" GRAVITY = 9.8 w_coriolis = np.zeros(3) @@ -122,11 +123,12 @@ def getImuParams(kitti_calibration): return imu_params -def save_results(isam, output_filename, first_gps_pose, gps_measurements): +def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, + gps_measurements: List): """Write the results from `isam` to `output_filename`.""" # Save results to file print("Writing results to file...") - with open(output_filename, 'w') as fp_out: + with open(output_filename, 'w', encoding='UTF-8') as fp_out: fp_out.write( "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n") From 279c6450280e6512892849384a9794a708d01487 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 15:08:21 -0400 Subject: [PATCH 124/169] fix type annotation --- python/gtsam/examples/IMUKittiExampleGPS.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 5a9811522..a9fb17ecb 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -124,7 +124,7 @@ def getImuParams(kitti_calibration: KittiCalibration): def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, - gps_measurements: List): + gps_measurements: List[GpsMeasurement]): """Write the results from `isam` to `output_filename`.""" # Save results to file print("Writing results to file...") From 03ac36c8c3694ec0cfa414be04c58a72c4cdacc3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 15:12:31 -0400 Subject: [PATCH 125/169] use python f-strings --- python/gtsam/examples/IMUKittiExampleGPS.py | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index a9fb17ecb..68c93bf19 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -145,10 +145,10 @@ def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, pose_quat = pose.rotation().toQuaternion() gps = gps_measurements[i].position - print("State at #{}".format(i)) - print("Pose:\n", pose) - print("Velocity:\n", velocity) - print("Bias:\n", bias) + print(f"State at #{i}") + print(f"Pose:\n{pose}") + print(f"Velocity:\n{velocity}") + print(f"Bias:\n{bias}") fp_out.write("{},{},{},{},{},{},{},{},{},{},{}\n".format( gps_measurements[i].time, pose.x(), pose.y(), pose.z(), @@ -290,9 +290,7 @@ def main(): noise_model_gps) new_values.insert(current_pose_key, gps_pose) - print( - "################ POSE INCLUDED AT TIME {} ################" - .format(t)) + print(f"############ POSE INCLUDED AT TIME {t} ############") print(gps_pose.translation(), "\n") else: new_values.insert(current_pose_key, current_pose_global) @@ -307,9 +305,7 @@ def main(): # We accumulate 2*GPSskip GPS measurements before updating the solver at # first so that the heading becomes observable. if i > (first_gps_pose + 2 * gps_skip): - print( - "################ NEW FACTORS AT TIME {:.6f} ################" - .format(t)) + print(f"############ NEW FACTORS AT TIME {t:.6f} ############") new_factors.print() isam.update(new_factors, new_values) @@ -325,9 +321,7 @@ def main(): current_velocity_global = result.atVector(current_vel_key) current_bias = result.atConstantBias(current_bias_key) - print( - "################ POSE AT TIME {} ################".format( - t)) + print(f"############ POSE AT TIME {t} ############") current_pose_global.print() print("\n") From 3ce02ba21e58039f2c75e6223d3553a6fe1443cb Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 16:08:33 -0400 Subject: [PATCH 126/169] fix typos in python example file --- python/gtsam/examples/Pose2SLAMExample_lago.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py index 92a219d21..b50b38759 100644 --- a/python/gtsam/examples/Pose2SLAMExample_lago.py +++ b/python/gtsam/examples/Pose2SLAMExample_lago.py @@ -43,7 +43,7 @@ def run(args: Namespace) -> None: # Add prior on the pose having index (key) = 0 priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) graph.add(PriorFactorPose2(0, Pose2(), priorModel)) - graph.print() + print(graph) print("Computing LAGO estimate") estimateLago: Values = gtsam.lago.initialize(graph) @@ -68,4 +68,5 @@ if __name__ == "__main__": ) parser.add_argument("-i", "--input", help="input file g2o format") parser.add_argument("-o", "--output", help="the path to the output file with optimized graph") + args = parser.parse_args() run(args) From 84d291003f954d5682771cc3b0dc22452b3f0a83 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 21 Oct 2021 16:14:44 -0400 Subject: [PATCH 127/169] add lago unit test, since lago namespace cannot be imported properly be wrapper --- python/gtsam/tests/test_lago.py | 34 +++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 python/gtsam/tests/test_lago.py diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py new file mode 100644 index 000000000..f1e62ca92 --- /dev/null +++ b/python/gtsam/tests/test_lago.py @@ -0,0 +1,34 @@ +""" +GTSAM Copyright 2010, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) +See LICENSE for the license information + +Author: John Lambert (Python) +""" + +import numpy as np + +import gtsam +from gtsam import Pose2, PriorFactorPose2, Values + + +def vector3(x: float, y: float, z: float) -> np.ndarray: + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=float) + + +def test_lago() -> None: + """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file.""" + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") + + graph = gtsam.NonlinearFactorGraph() + graph, initial = gtsam.readG2o(g2oFile) + + # Add prior on the pose having index (key) = 0 + priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) + graph.add(PriorFactorPose2(0, Pose2(), priorModel)) + + estimateLago: Values = gtsam.lago.initialize(graph) + assert isinstance(estimateLago, Values) From 1e84fd9cc41f14aab04e7dfc6ee75917c3a2ad15 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 16:33:32 -0400 Subject: [PATCH 128/169] refactor the example to make it cleaner --- python/gtsam/examples/IMUKittiExampleGPS.py | 126 ++++++++++++-------- 1 file changed, 79 insertions(+), 47 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 68c93bf19..071065260 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -4,13 +4,15 @@ Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BEN Author: Varun Agrawal """ import argparse -from typing import List +from typing import List, Tuple import gtsam import numpy as np -from gtsam import ISAM2, Point3, Pose3, noiseModel +from gtsam import ISAM2, Pose3, noiseModel from gtsam.symbol_shorthand import B, V, X +GRAVITY = 9.8 + class KittiCalibration: """Class to hold KITTI calibration info.""" @@ -46,25 +48,8 @@ class GpsMeasurement: self.position = position -def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", - gps_data_file: str = "KittiGps_converted.txt", - imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt"): - """ - Load the KITTI Dataset. - """ - # Read IMU metadata and compute relative sensor pose transforms - # BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma - # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma - # AverageDeltaT - imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) - with open(imu_metadata_file, encoding='UTF-8') as imu_metadata: - print("-- Reading sensor metadata") - line = imu_metadata.readline() # Ignore the first line - line = imu_metadata.readline().strip() - data = list(map(float, line.split(' '))) - kitti_calibration = KittiCalibration(*data) - print("IMU metadata:", data) - +def loadImuData(imu_data_file: str) -> List[ImuMeasurement]: + """Helper to load the IMU data.""" # Read IMU data # Time dt accelX accelY accelZ omegaX omegaY omegaZ imu_data_file = gtsam.findExampleDataFile(imu_data_file) @@ -81,6 +66,11 @@ def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", np.asarray([gyro_x, gyro_y, gyro_z])) imu_measurements.append(imu_measurement) + return imu_measurements + + +def loadGpsData(gps_data_file: str) -> List[GpsMeasurement]: + """Helper to load the GPS data.""" # Read GPS data # Time,X,Y,Z gps_data_file = gtsam.findExampleDataFile(gps_data_file) @@ -94,12 +84,38 @@ def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", gps_measurement = GpsMeasurement(time, np.asarray([x, y, z])) gps_measurements.append(gps_measurement) + return gps_measurements + + +def loadKittiData( + imu_data_file: str = "KittiEquivBiasedImu.txt", + gps_data_file: str = "KittiGps_converted.txt", + imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt" +) -> Tuple[KittiCalibration, List[ImuMeasurement], List[GpsMeasurement]]: + """ + Load the KITTI Dataset. + """ + # Read IMU metadata and compute relative sensor pose transforms + # BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma + # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma + # AverageDeltaT + imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) + with open(imu_metadata_file, encoding='UTF-8') as imu_metadata: + print("-- Reading sensor metadata") + line = imu_metadata.readline() # Ignore the first line + line = imu_metadata.readline().strip() + data = list(map(float, line.split(' '))) + kitti_calibration = KittiCalibration(*data) + print("IMU metadata:", data) + + imu_measurements = loadImuData(imu_data_file) + gps_measurements = loadGpsData(gps_data_file) + return kitti_calibration, imu_measurements, gps_measurements def getImuParams(kitti_calibration: KittiCalibration): """Get the IMU parameters from the KITTI calibration data.""" - GRAVITY = 9.8 w_coriolis = np.zeros(3) # Set IMU preintegration parameters @@ -156,7 +172,7 @@ def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, gps[0], gps[1], gps[2])) -def parse_args(): +def parse_args() -> argparse.Namespace: """Parse command line arguments.""" parser = argparse.ArgumentParser() parser.add_argument("--output_filename", @@ -164,24 +180,15 @@ def parse_args(): return parser.parse_args() -def main(): - """Main runner.""" - args = parse_args() - kitti_calibration, imu_measurements, gps_measurements = loadKittiData() - - if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8): - raise ValueError( - "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" - ) - - # Configure different variables - first_gps_pose = 1 - gps_skip = 10 - - # Configure noise models - noise_model_gps = noiseModel.Diagonal.Precisions( - np.asarray([0, 0, 0] + [1.0 / 0.07] * 3)) - +def optimize(gps_measurements: List[GpsMeasurement], + imu_measurements: List[ImuMeasurement], + sigma_init_x: gtsam.noiseModel.Diagonal, + sigma_init_v: gtsam.noiseModel.Diagonal, + sigma_init_b: gtsam.noiseModel.Diagonal, + noise_model_gps: gtsam.noiseModel.Diagonal, + kitti_calibration: KittiCalibration, first_gps_pose: int, + gps_skip: int) -> gtsam.ISAM2: + """Run ISAM2 optimization on the measurements.""" # Set initial conditions for the estimated trajectory # initial pose is the reference frame (navigation frame) current_pose_global = Pose3(gtsam.Rot3(), @@ -191,12 +198,6 @@ def main(): current_velocity_global = np.zeros(3) current_bias = gtsam.imuBias.ConstantBias() # init with zero bias - sigma_init_x = noiseModel.Diagonal.Precisions( - np.asarray([0, 0, 0, 1, 1, 1])) - sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) - sigma_init_b = noiseModel.Diagonal.Sigmas( - np.asarray([0.1] * 3 + [5.00e-05] * 3)) - imu_params = getImuParams(kitti_calibration) # Set ISAM2 parameters and create ISAM2 solver object @@ -325,6 +326,37 @@ def main(): current_pose_global.print() print("\n") + return isam + + +def main(): + """Main runner.""" + args = parse_args() + kitti_calibration, imu_measurements, gps_measurements = loadKittiData() + + if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8): + raise ValueError( + "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" + ) + + # Configure different variables + first_gps_pose = 1 + gps_skip = 10 + + # Configure noise models + noise_model_gps = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0] + [1.0 / 0.07] * 3)) + + sigma_init_x = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0, 1, 1, 1])) + sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) + sigma_init_b = noiseModel.Diagonal.Sigmas( + np.asarray([0.1] * 3 + [5.00e-05] * 3)) + + isam = optimize(gps_measurements, imu_measurements, sigma_init_x, + sigma_init_v, sigma_init_b, noise_model_gps, + kitti_calibration, first_gps_pose, gps_skip) + save_results(isam, args.output_filename, first_gps_pose, gps_measurements) From 9aa0dbf49394d0cabca4eaa45e269d84f8ff80a3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 18:42:46 -0400 Subject: [PATCH 129/169] replace static variable with variable of greater scope in cpp example --- examples/IMUKittiExampleGPS.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index a2c82575f..cb60b2516 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -241,6 +241,8 @@ int main(int argc, char* argv[]) { "-- Starting main loop: inference is performed at each time step, but we " "plot trajectory every 10 steps\n"); size_t j = 0; + size_t included_imu_measurement_count = 0; + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { // At each non=IMU measurement we initialize a new node in the graph auto current_pose_key = X(i); @@ -266,7 +268,7 @@ int main(int argc, char* argv[]) { current_summarized_measurement = std::make_shared(imu_params, current_bias); - static size_t included_imu_measurement_count = 0; + while (j < imu_measurements.size() && imu_measurements[j].time <= t) { if (imu_measurements[j].time >= t_previous) { current_summarized_measurement->integrateMeasurement( @@ -306,7 +308,7 @@ int main(int argc, char* argv[]) { current_pose_key, gps_pose, noise_model_gps); new_values.insert(current_pose_key, gps_pose); - printf("################ POSE INCLUDED AT TIME %lf ################\n", + printf("############ POSE INCLUDED AT TIME %.6lf ############\n", t); cout << gps_pose.translation(); printf("\n\n"); @@ -324,7 +326,7 @@ int main(int argc, char* argv[]) { // We accumulate 2*GPSskip GPS measurements before updating the solver at // first so that the heading becomes observable. if (i > (first_gps_pose + 2 * gps_skip)) { - printf("################ NEW FACTORS AT TIME %lf ################\n", + printf("############ NEW FACTORS AT TIME %.6lf ############\n", t); new_factors.print(); @@ -341,7 +343,7 @@ int main(int argc, char* argv[]) { current_velocity_global = result.at(current_vel_key); current_bias = result.at(current_bias_key); - printf("\n################ POSE AT TIME %lf ################\n", t); + printf("\n############ POSE AT TIME %lf ############\n", t); current_pose_global.print(); printf("\n\n"); } From b3e8bf2325848f4600a57b977201cbeed38d1090 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 18:45:57 -0400 Subject: [PATCH 130/169] fix the included_imu_measurement_count scope --- python/gtsam/examples/IMUKittiExampleGPS.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 071065260..8b6b4fdf0 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -219,7 +219,10 @@ def optimize(gps_measurements: List[GpsMeasurement], # (3) we solve the graph to obtain and optimal estimate of robot trajectory print("-- Starting main loop: inference is performed at each time step, " "but we plot trajectory every 10 steps") + j = 0 + included_imu_measurement_count = 0 + for i in range(first_gps_pose, len(gps_measurements)): # At each non=IMU measurement we initialize a new node in the graph current_pose_key = X(i) @@ -246,7 +249,6 @@ def optimize(gps_measurements: List[GpsMeasurement], current_summarized_measurement = gtsam.PreintegratedImuMeasurements( imu_params, current_bias) - included_imu_measurement_count = 0 while (j < len(imu_measurements) and imu_measurements[j].time <= t): if imu_measurements[j].time >= t_previous: From 7a9d89539c3c8924c9f8b8dca6a0ed63734f4d85 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 21 Oct 2021 19:23:44 -0400 Subject: [PATCH 131/169] TBB revival --- cmake/HandleTBB.cmake | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index 118dc4dac..52ee75494 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -7,9 +7,9 @@ if (GTSAM_WITH_TBB) if(TBB_FOUND) set(GTSAM_USE_TBB 1) # This will go into config.h - if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) - message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") - endif() +# if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) +# message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") +# endif() if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) set(TBB_GREATER_EQUAL_2020 1) From bc68ecb5ab2a01566de479eea741e14edcfcc43e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 01:18:51 -0400 Subject: [PATCH 132/169] use unittest framework instead of pytest --- python/gtsam/tests/test_lago.py | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py index f1e62ca92..5b836ccdf 100644 --- a/python/gtsam/tests/test_lago.py +++ b/python/gtsam/tests/test_lago.py @@ -8,6 +8,8 @@ See LICENSE for the license information Author: John Lambert (Python) """ +import unittest + import numpy as np import gtsam @@ -19,16 +21,23 @@ def vector3(x: float, y: float, z: float) -> np.ndarray: return np.array([x, y, z], dtype=float) -def test_lago() -> None: - """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file.""" - g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") +class TestLago(unittest.TestCase): + """Test selected LAGO methods.""" - graph = gtsam.NonlinearFactorGraph() - graph, initial = gtsam.readG2o(g2oFile) + def test_initialize(self) -> None: + """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file.""" + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") - # Add prior on the pose having index (key) = 0 - priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) - graph.add(PriorFactorPose2(0, Pose2(), priorModel)) + graph = gtsam.NonlinearFactorGraph() + graph, initial = gtsam.readG2o(g2oFile) - estimateLago: Values = gtsam.lago.initialize(graph) - assert isinstance(estimateLago, Values) + # Add prior on the pose having index (key) = 0 + priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) + graph.add(PriorFactorPose2(0, Pose2(), priorModel)) + + estimateLago: Values = gtsam.lago.initialize(graph) + assert isinstance(estimateLago, Values) + + +if __name__ == "__main__": + unittest.main() From 97fee355ee52077753b2a343245425381e8799bc Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 09:13:17 -0400 Subject: [PATCH 133/169] add missing default arg on useOdometricPath --- gtsam/slam/slam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 24139444c..60000dbab 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -337,8 +337,8 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { #include namespace lago { + gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); - gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath); } } // namespace gtsam From 430530ca544be50001252ec1209bf1c95cf40d0a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Oct 2021 14:40:30 -0400 Subject: [PATCH 134/169] put all example scripts inside main() functions and apply formatting --- python/gtsam/examples/CustomFactorExample.py | 224 ++++++++++-------- python/gtsam/examples/GPSFactorExample.py | 51 ++-- python/gtsam/examples/OdometryExample.py | 81 ++++--- python/gtsam/examples/PlanarSLAMExample.py | 116 +++++---- python/gtsam/examples/Pose2SLAMExample.py | 134 ++++++----- python/gtsam/examples/Pose2SLAMExample_g2o.py | 141 +++++------ python/gtsam/examples/Pose3SLAMExample_g2o.py | 97 ++++---- ...Pose3SLAMExample_initializePose3Chordal.py | 32 ++- 8 files changed, 477 insertions(+), 399 deletions(-) diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index c7fe1e202..03b138410 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -9,15 +9,17 @@ CustomFactor demo that simulates a 1-D sensor fusion task. Author: Fan Jiang, Frank Dellaert """ +from functools import partial +from typing import List, Optional + import gtsam import numpy as np -from typing import List, Optional -from functools import partial +I = np.eye(1) def simulate_car(): - # Simulate a car for one second + """Simulate a car for one second""" x0 = 0 dt = 0.25 # 4 Hz, typical GPS v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast @@ -26,46 +28,8 @@ def simulate_car(): return x -x = simulate_car() -print(f"Simulated car trajectory: {x}") - -# %% -add_noise = True # set this to False to run with "perfect" measurements - -# GPS measurements -sigma_gps = 3.0 # assume GPS is +/- 3m -g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) - for k in range(5)] - -# Odometry measurements -sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz -o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0) - for k in range(4)] - -# Landmark measurements: -sigma_lm = 1 # assume landmark measurement is accurate up to 1m - -# Assume first landmark is at x=5, we measure it at time k=0 -lm_0 = 5.0 -z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) - -# Assume other landmark is at x=28, we measure it at time k=3 -lm_3 = 28.0 -z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) - -unknown = [gtsam.symbol('x', k) for k in range(5)] - -print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) - -# We now can use nonlinear factor graphs -factor_graph = gtsam.NonlinearFactorGraph() - -# Add factors for GPS measurements -I = np.eye(1) -gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) - - -def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, + jacobians: Optional[List[np.ndarray]]): """GPS Factor error function :param measurement: GPS measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -82,36 +46,8 @@ def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobia return error -# Add the GPS factors -for k in range(5): - gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]]))) - factor_graph.add(gf) - -# New Values container -v = gtsam.Values() - -# Add initial estimates to the Values container -for i in range(5): - v.insert(unknown[i], np.array([0.0])) - -# Initialize optimizer -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) - -# Optimize the factor graph -result = optimizer.optimize() - -# calculate the error from ground truth -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) - -print("Result with only GPS") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") - -# Adding odometry will improve things a lot -odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) - - -def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, + jacobians: Optional[List[np.ndarray]]): """Odometry Factor error function :param measurement: Odometry measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -130,25 +66,8 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobi return error -for k in range(4): - odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]]))) - factor_graph.add(odof) - -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) - -result = optimizer.optimize() - -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) - -print("Result with GPS+Odometry") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") - -# This is great, but GPS noise is still apparent, so now we add the two landmarks -lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) - - -def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, + jacobians: Optional[List[np.ndarray]]): """Landmark Factor error function :param measurement: Landmark measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -165,15 +84,120 @@ def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobian return error -factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0])))) -factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3])))) +def main(): + """Main runner.""" -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + x = simulate_car() + print(f"Simulated car trajectory: {x}") -result = optimizer.optimize() + add_noise = True # set this to False to run with "perfect" measurements -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) + # GPS measurements + sigma_gps = 3.0 # assume GPS is +/- 3m + g = [ + x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) + for k in range(5) + ] -print("Result with GPS+Odometry+Landmark") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") + # Odometry measurements + sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz + o = [ + x[k + 1] - x[k] + + (np.random.normal(scale=sigma_odo) if add_noise else 0) + for k in range(4) + ] + + # Landmark measurements: + sigma_lm = 1 # assume landmark measurement is accurate up to 1m + + # Assume first landmark is at x=5, we measure it at time k=0 + lm_0 = 5.0 + z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) + + # Assume other landmark is at x=28, we measure it at time k=3 + lm_3 = 28.0 + z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) + + unknown = [gtsam.symbol('x', k) for k in range(5)] + + print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) + + # We now can use nonlinear factor graphs + factor_graph = gtsam.NonlinearFactorGraph() + + # Add factors for GPS measurements + gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) + + # Add the GPS factors + for k in range(5): + gf = gtsam.CustomFactor(gps_model, [unknown[k]], + partial(error_gps, np.array([g[k]]))) + factor_graph.add(gf) + + # New Values container + v = gtsam.Values() + + # Add initial estimates to the Values container + for i in range(5): + v.insert(unknown[i], np.array([0.0])) + + # Initialize optimizer + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + # Optimize the factor graph + result = optimizer.optimize() + + # calculate the error from ground truth + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) + + print("Result with only GPS") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") + + # Adding odometry will improve things a lot + odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) + + for k in range(4): + odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], + partial(error_odom, np.array([o[k]]))) + factor_graph.add(odof) + + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + result = optimizer.optimize() + + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) + + print("Result with GPS+Odometry") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") + + # This is great, but GPS noise is still apparent, so now we add the two landmarks + lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) + + factor_graph.add( + gtsam.CustomFactor(lm_model, [unknown[0]], + partial(error_lm, np.array([lm_0 + z_0])))) + factor_graph.add( + gtsam.CustomFactor(lm_model, [unknown[3]], + partial(error_lm, np.array([lm_3 + z_3])))) + + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + result = optimizer.optimize() + + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) + + print("Result with GPS+Odometry+Landmark") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/GPSFactorExample.py b/python/gtsam/examples/GPSFactorExample.py index 0bc0d1bf3..8eb663cb4 100644 --- a/python/gtsam/examples/GPSFactorExample.py +++ b/python/gtsam/examples/GPSFactorExample.py @@ -13,13 +13,8 @@ Author: Mandy Xie from __future__ import print_function -import numpy as np - import gtsam -import matplotlib.pyplot as plt -import gtsam.utils.plot as gtsam_plot - # ENU Origin is where the plane was in hold next to runway lat0 = 33.86998 lon0 = -84.30626 @@ -29,28 +24,34 @@ h0 = 274 GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() -# Add a prior on the first point, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorMean = gtsam.Pose3() # prior at origin -graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) +def main(): + """Main runner.""" + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() -# Add GPS factors -gps = gtsam.Point3(lat0, lon0, h0) -graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) + # Add a prior on the first point, setting it to the origin + # A prior factor consists of a mean and a noise model (covariance matrix) + priorMean = gtsam.Pose3() # prior at origin + graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) -# Create the data structure to hold the initialEstimate estimate to the solution -# For illustrative purposes, these have been deliberately set to incorrect values -initial = gtsam.Values() -initial.insert(1, gtsam.Pose3()) -print("\nInitial Estimate:\n{}".format(initial)) + # Add GPS factors + gps = gtsam.Point3(lat0, lon0, h0) + graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) -# optimize using Levenberg-Marquardt optimization -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) + # Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initial = gtsam.Values() + initial.insert(1, gtsam.Pose3()) + print("\nInitial Estimate:\n{}".format(initial)) + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/OdometryExample.py b/python/gtsam/examples/OdometryExample.py index 8b519ce9a..210aeb808 100644 --- a/python/gtsam/examples/OdometryExample.py +++ b/python/gtsam/examples/OdometryExample.py @@ -13,57 +13,60 @@ Author: Frank Dellaert from __future__ import print_function -import numpy as np - import gtsam - -import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np # Create noise models ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() -# Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin -graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) +def main(): + """Main runner""" + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() -# Add odometry factors -odometry = gtsam.Pose2(2.0, 0.0, 0.0) -# For simplicity, we will use the same noise model for each odometry factor -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) + # Add a prior on the first pose, setting it to the origin + # A prior factor consists of a mean and a noise model (covariance matrix) + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin + graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) -# Create the data structure to hold the initialEstimate estimate to the solution -# For illustrative purposes, these have been deliberately set to incorrect values -initial = gtsam.Values() -initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) -print("\nInitial Estimate:\n{}".format(initial)) + # Add odometry factors + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + # For simplicity, we will use the same noise model for each odometry factor + # Create odometry (Between) factors between consecutive poses + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) + graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) -# optimize using Levenberg-Marquardt optimization -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) + # Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initial = gtsam.Values() + initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) + print("\nInitial Estimate:\n{}".format(initial)) -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for i in range(1, 4): - print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) - -fig = plt.figure(0) -for i in range(1, 4): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) -plt.axis('equal') -plt.show() + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for i in range(1, 4): + print("X{} covariance:\n{}\n".format(i, + marginals.marginalCovariance(i))) + + for i in range(1, 4): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, + marginals.marginalCovariance(i)) + plt.axis('equal') + plt.show() +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py index 5ffdf048d..d2ee92c95 100644 --- a/python/gtsam/examples/PlanarSLAMExample.py +++ b/python/gtsam/examples/PlanarSLAMExample.py @@ -13,69 +13,85 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) from __future__ import print_function -import numpy as np - import gtsam -from gtsam.symbol_shorthand import X, L +import numpy as np +from gtsam.symbol_shorthand import L, X # Create noise models PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() -# Create the keys corresponding to unknown variables in the factor graph -X1 = X(1) -X2 = X(2) -X3 = X(3) -L1 = L(4) -L2 = L(5) +def main(): + """Main runner""" -# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model -graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() -# Add odometry factors between X1,X2 and X2,X3, respectively -graph.add(gtsam.BetweenFactorPose2( - X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) + # Create the keys corresponding to unknown variables in the factor graph + X1 = X(1) + X2 = X(2) + X3 = X(3) + L1 = L(4) + L2 = L(5) -# Add Range-Bearing measurements to two different landmarks L1 and L2 -graph.add(gtsam.BearingRangeFactor2D( - X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) -graph.add(gtsam.BearingRangeFactor2D( - X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) -graph.add(gtsam.BearingRangeFactor2D( - X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) + # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model + graph.add( + gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) -# Print graph -print("Factor Graph:\n{}".format(graph)) + # Add odometry factors between X1,X2 and X2,X3, respectively + graph.add( + gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), + ODOMETRY_NOISE)) -# Create (deliberately inaccurate) initial estimate -initial_estimate = gtsam.Values() -initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) -initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) -initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) -initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) -initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) + # Add Range-Bearing measurements to two different landmarks L1 and L2 + graph.add( + gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45), + np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE)) + graph.add( + gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, + MEASUREMENT_NOISE)) + graph.add( + gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, + MEASUREMENT_NOISE)) -# Print -print("Initial Estimate:\n{}".format(initial_estimate)) + # Print graph + print("Factor Graph:\n{}".format(graph)) -# Optimize using Levenberg-Marquardt optimization. The optimizer -# accepts an optional set of configuration parameters, controlling -# things like convergence criteria, the type of linear system solver -# to use, and the amount of information displayed during optimization. -# Here we will use the default set of parameters. See the -# documentation for the full set of parameters. -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) + # Create (deliberately inaccurate) initial estimate + initial_estimate = gtsam.Values() + initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) + initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) + initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) + initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) + initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) -# Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]: - print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key))) + # Print + print("Initial Estimate:\n{}".format(initial_estimate)) + + # Optimize using Levenberg-Marquardt optimization. The optimizer + # accepts an optional set of configuration parameters, controlling + # things like convergence criteria, the type of linear system solver + # to use, and the amount of information displayed during optimization. + # Here we will use the default set of parameters. See the + # documentation for the full set of parameters. + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, + params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) + + # Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), + (L2, "L2")]: + print("{} covariance:\n{}\n".format(s, + marginals.marginalCovariance(key))) + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py index 2ec190d73..300a70fbd 100644 --- a/python/gtsam/examples/Pose2SLAMExample.py +++ b/python/gtsam/examples/Pose2SLAMExample.py @@ -15,82 +15,88 @@ from __future__ import print_function import math -import numpy as np - import gtsam - -import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) +def main(): + """Main runner.""" + # Create noise models + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(gtsam.Point3(0.3, 0.3, 0.1)) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas( + gtsam.Point3(0.2, 0.2, 0.1)) -# Create noise models -PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) -ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) + # 1. Create a factor graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() -# 1. Create a factor graph container and add factors to it -graph = gtsam.NonlinearFactorGraph() + # 2a. Add a prior on the first pose, setting it to the origin + # A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) + graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) -# 2a. Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) -graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) + # 2b. Add odometry factors + # Create odometry (Between) factors between consecutive poses + graph.add( + gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) -# 2b. Add odometry factors -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) + # 2c. Add the loop closure constraint + # This factor encodes the fact that we have returned to the same pose. In real + # systems, these constraints may be identified in many ways, such as appearance-based + # techniques with camera images. We will use another Between Factor to enforce this constraint: + graph.add( + gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) # print -# 2c. Add the loop closure constraint -# This factor encodes the fact that we have returned to the same pose. In real -# systems, these constraints may be identified in many ways, such as appearance-based -# techniques with camera images. We will use another Between Factor to enforce this constraint: -graph.add(gtsam.BetweenFactorPose2( - 5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) # print + # 3. Create the data structure to hold the initial_estimate estimate to the + # solution. For illustrative purposes, these have been deliberately set to incorrect values + initial_estimate = gtsam.Values() + initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) + initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) + initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) + print("\nInitial Estimate:\n{}".format(initial_estimate)) # print -# 3. Create the data structure to hold the initial_estimate estimate to the -# solution. For illustrative purposes, these have been deliberately set to incorrect values -initial_estimate = gtsam.Values() -initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) -initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) -initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) -print("\nInitial Estimate:\n{}".format(initial_estimate)) # print + # 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer + # The optimizer accepts an optional set of configuration parameters, + # controlling things like convergence criteria, the type of linear + # system solver to use, and the amount of information displayed during + # optimization. We will set a few parameters as a demonstration. + parameters = gtsam.GaussNewtonParams() -# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer -# The optimizer accepts an optional set of configuration parameters, -# controlling things like convergence criteria, the type of linear -# system solver to use, and the amount of information displayed during -# optimization. We will set a few parameters as a demonstration. -parameters = gtsam.GaussNewtonParams() + # Stop iterating once the change in error between steps is less than this value + parameters.setRelativeErrorTol(1e-5) + # Do not perform more than N iteration steps + parameters.setMaxIterations(100) + # Create the optimizer ... + optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) + # ... and optimize + result = optimizer.optimize() + print("Final Result:\n{}".format(result)) -# Stop iterating once the change in error between steps is less than this value -parameters.setRelativeErrorTol(1e-5) -# Do not perform more than N iteration steps -parameters.setMaxIterations(100) -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) -# ... and optimize -result = optimizer.optimize() -print("Final Result:\n{}".format(result)) + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for i in range(1, 6): + print("X{} covariance:\n{}\n".format(i, + marginals.marginalCovariance(i))) -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for i in range(1, 6): - print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) + for i in range(1, 6): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, + marginals.marginalCovariance(i)) -fig = plt.figure(0) -for i in range(1, 6): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) + plt.axis('equal') + plt.show() -plt.axis('equal') -plt.show() + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose2SLAMExample_g2o.py b/python/gtsam/examples/Pose2SLAMExample_g2o.py index 97fb46c5f..cf029c049 100644 --- a/python/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/python/gtsam/examples/Pose2SLAMExample_g2o.py @@ -12,77 +12,86 @@ and does the optimization. Output is written on a file, in g2o format # pylint: disable=invalid-name, E1101 from __future__ import print_function + import argparse -import math -import numpy as np -import matplotlib.pyplot as plt import gtsam +import matplotlib.pyplot as plt from gtsam.utils import plot -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) +def main(): + """Main runner.""" + + parser = argparse.ArgumentParser( + description="A 2D Pose SLAM example that reads input from g2o, " + "converts it to a factor graph and does the optimization. " + "Output is written on a file, in g2o format") + parser.add_argument('-i', '--input', help='input file g2o format') + parser.add_argument( + '-o', + '--output', + help="the path to the output file with optimized graph") + parser.add_argument('-m', + '--maxiter', + type=int, + help="maximum number of iterations for optimizer") + parser.add_argument('-k', + '--kernel', + choices=['none', 'huber', 'tukey'], + default="none", + help="Type of kernel used") + parser.add_argument("-p", + "--plot", + action="store_true", + help="Flag to plot results") + args = parser.parse_args() + + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ + else args.input + + maxIterations = 100 if args.maxiter is None else args.maxiter + + is3D = False + + graph, initial = gtsam.readG2o(g2oFile, is3D) + + assert args.kernel == "none", "Supplied kernel type is not yet implemented" + + # Add prior on the pose having index (key) = 0 + priorModel = gtsam.noiseModel.Diagonal.Variances(gtsam.Point3(1e-6, 1e-6, 1e-8)) + graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) + + params = gtsam.GaussNewtonParams() + params.setVerbosity("Termination") + params.setMaxIterations(maxIterations) + # parameters.setRelativeErrorTol(1e-5) + # Create the optimizer ... + optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) + # ... and optimize + result = optimizer.optimize() + + print("Optimization complete") + print("initial error = ", graph.error(initial)) + print("final error = ", graph.error(result)) + + if args.output is None: + print("\nFactor Graph:\n{}".format(graph)) + print("\nInitial Estimate:\n{}".format(initial)) + print("Final Result:\n{}".format(result)) + else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) + gtsam.writeG2o(graphNoKernel, result, outputFile) + print("Done!") + + if args.plot: + resultPoses = gtsam.utilities.extractPose2(result) + for i in range(resultPoses.shape[0]): + plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) + plt.show() -parser = argparse.ArgumentParser( - description="A 2D Pose SLAM example that reads input from g2o, " - "converts it to a factor graph and does the optimization. " - "Output is written on a file, in g2o format") -parser.add_argument('-i', '--input', help='input file g2o format') -parser.add_argument('-o', '--output', - help="the path to the output file with optimized graph") -parser.add_argument('-m', '--maxiter', type=int, - help="maximum number of iterations for optimizer") -parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'], - default="none", help="Type of kernel used") -parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") -args = parser.parse_args() - -g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ - else args.input - -maxIterations = 100 if args.maxiter is None else args.maxiter - -is3D = False - -graph, initial = gtsam.readG2o(g2oFile, is3D) - -assert args.kernel == "none", "Supplied kernel type is not yet implemented" - -# Add prior on the pose having index (key) = 0 -priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) -graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) - -params = gtsam.GaussNewtonParams() -params.setVerbosity("Termination") -params.setMaxIterations(maxIterations) -# parameters.setRelativeErrorTol(1e-5) -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) -# ... and optimize -result = optimizer.optimize() - -print("Optimization complete") -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) - - -if args.output is None: - print("\nFactor Graph:\n{}".format(graph)) - print("\nInitial Estimate:\n{}".format(initial)) - print("Final Result:\n{}".format(result)) -else: - outputFile = args.output - print("Writing results to file: ", outputFile) - graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) - gtsam.writeG2o(graphNoKernel, result, outputFile) - print ("Done!") - -if args.plot: - resultPoses = gtsam.utilities.extractPose2(result) - for i in range(resultPoses.shape[0]): - plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) - plt.show() +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose3SLAMExample_g2o.py b/python/gtsam/examples/Pose3SLAMExample_g2o.py index 501a75dc1..dcdfc34a3 100644 --- a/python/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/python/gtsam/examples/Pose3SLAMExample_g2o.py @@ -8,13 +8,14 @@ # pylint: disable=invalid-name, E1101 from __future__ import print_function + import argparse -import numpy as np -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D import gtsam +import matplotlib.pyplot as plt +import numpy as np from gtsam.utils import plot +from mpl_toolkits.mplot3d import Axes3D def vector6(x, y, z, a, b, c): @@ -22,50 +23,62 @@ def vector6(x, y, z, a, b, c): return np.array([x, y, z, a, b, c], dtype=float) -parser = argparse.ArgumentParser( - description="A 3D Pose SLAM example that reads input from g2o, and " - "initializes Pose3") -parser.add_argument('-i', '--input', help='input file g2o format') -parser.add_argument('-o', '--output', - help="the path to the output file with optimized graph") -parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") -args = parser.parse_args() +def main(): + """Main runner.""" -g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ - else args.input + parser = argparse.ArgumentParser( + description="A 3D Pose SLAM example that reads input from g2o, and " + "initializes Pose3") + parser.add_argument('-i', '--input', help='input file g2o format') + parser.add_argument( + '-o', + '--output', + help="the path to the output file with optimized graph") + parser.add_argument("-p", + "--plot", + action="store_true", + help="Flag to plot results") + args = parser.parse_args() -is3D = True -graph, initial = gtsam.readG2o(g2oFile, is3D) + g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ + else args.input -# Add Prior on the first key -priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, - 1e-4, 1e-4, 1e-4)) + is3D = True + graph, initial = gtsam.readG2o(g2oFile, is3D) -print("Adding prior to g2o file ") -firstKey = initial.keys()[0] -graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) + # Add Prior on the first key + priorModel = gtsam.noiseModel.Diagonal.Variances( + vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) -params = gtsam.GaussNewtonParams() -params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) -result = optimizer.optimize() -print("Optimization complete") + print("Adding prior to g2o file ") + firstKey = initial.keys()[0] + graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) + params = gtsam.GaussNewtonParams() + params.setVerbosity( + "Termination") # this will show info about stopping conds + optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) + result = optimizer.optimize() + print("Optimization complete") -if args.output is None: - print("Final Result:\n{}".format(result)) -else: - outputFile = args.output - print("Writing results to file: ", outputFile) - graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) - gtsam.writeG2o(graphNoKernel, result, outputFile) - print ("Done!") + print("initial error = ", graph.error(initial)) + print("final error = ", graph.error(result)) -if args.plot: - resultPoses = gtsam.utilities.allPose3s(result) - for i in range(resultPoses.size()): - plot.plot_pose3(1, resultPoses.atPose3(i)) - plt.show() + if args.output is None: + print("Final Result:\n{}".format(result)) + else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) + gtsam.writeG2o(graphNoKernel, result, outputFile) + print("Done!") + + if args.plot: + resultPoses = gtsam.utilities.allPose3s(result) + for i in range(resultPoses.size()): + plot.plot_pose3(1, resultPoses.atPose3(i)) + plt.show() + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py index 2b2c5f991..e439bbaeb 100644 --- a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py +++ b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -13,23 +13,29 @@ Author: Luca Carlone, Frank Dellaert (python port) from __future__ import print_function +import gtsam import numpy as np -import gtsam -# Read graph from file -g2oFile = gtsam.findExampleDataFile("pose3example.txt") +def main(): + """Main runner.""" + # Read graph from file + g2oFile = gtsam.findExampleDataFile("pose3example.txt") -is3D = True -graph, initial = gtsam.readG2o(g2oFile, is3D) + is3D = True + graph, initial = gtsam.readG2o(g2oFile, is3D) -# Add prior on the first key. TODO: assumes first key ios z -priorModel = gtsam.noiseModel.Diagonal.Variances( - np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) -firstKey = initial.keys()[0] -graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + # Add prior on the first key. TODO: assumes first key ios z + priorModel = gtsam.noiseModel.Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) + firstKey = initial.keys()[0] + graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) -# Initializing Pose3 - chordal relaxation" -initialization = gtsam.InitializePose3.initialize(graph) + # Initializing Pose3 - chordal relaxation" + initialization = gtsam.InitializePose3.initialize(graph) -print(initialization) + print(initialization) + + +if __name__ == "__main__": + main() From d48b7371bb4a0f019168f54b98079110b4500076 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 15:05:05 -0400 Subject: [PATCH 135/169] use Point3 instead of artificial vector3 --- python/gtsam/tests/test_lago.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py index 5b836ccdf..8ed5dd943 100644 --- a/python/gtsam/tests/test_lago.py +++ b/python/gtsam/tests/test_lago.py @@ -13,12 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Pose2, PriorFactorPose2, Values - - -def vector3(x: float, y: float, z: float) -> np.ndarray: - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) +from gtsam import Point3, Pose2, PriorFactorPose2, Values class TestLago(unittest.TestCase): @@ -32,7 +27,7 @@ class TestLago(unittest.TestCase): graph, initial = gtsam.readG2o(g2oFile) # Add prior on the pose having index (key) = 0 - priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) + priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8)) graph.add(PriorFactorPose2(0, Pose2(), priorModel)) estimateLago: Values = gtsam.lago.initialize(graph) From a93c58abd6e510f22528f0fb380fd4e97e33a986 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 15:05:37 -0400 Subject: [PATCH 136/169] use Point3 instead of artificial vector3 --- python/gtsam/examples/Pose2SLAMExample_lago.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py index b50b38759..d8cddde0b 100644 --- a/python/gtsam/examples/Pose2SLAMExample_lago.py +++ b/python/gtsam/examples/Pose2SLAMExample_lago.py @@ -25,12 +25,7 @@ from argparse import Namespace import numpy as np import gtsam -from gtsam import Pose2, PriorFactorPose2, Values - - -def vector3(x: float, y: float, z: float) -> np.ndarray: - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) +from gtsam import Point3, Pose2, PriorFactorPose2, Values def run(args: Namespace) -> None: @@ -41,7 +36,7 @@ def run(args: Namespace) -> None: graph, initial = gtsam.readG2o(g2oFile) # Add prior on the pose having index (key) = 0 - priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) + priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8)) graph.add(PriorFactorPose2(0, Pose2(), priorModel)) print(graph) From f8f93cab21128efd163af0abfc8a3dc6824694f8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Oct 2021 19:28:11 -0400 Subject: [PATCH 137/169] add type annotations --- python/gtsam/examples/CustomFactorExample.py | 17 ++++++++++------- .../Pose3SLAMExample_initializePose3Chordal.py | 2 +- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index 03b138410..36c1e003d 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -18,7 +18,7 @@ import numpy as np I = np.eye(1) -def simulate_car(): +def simulate_car() -> List[float]: """Simulate a car for one second""" x0 = 0 dt = 0.25 # 4 Hz, typical GPS @@ -28,8 +28,9 @@ def simulate_car(): return x -def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, - jacobians: Optional[List[np.ndarray]]): +def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """GPS Factor error function :param measurement: GPS measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -46,8 +47,9 @@ def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, return error -def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, - jacobians: Optional[List[np.ndarray]]): +def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """Odometry Factor error function :param measurement: Odometry measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -66,8 +68,9 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, return error -def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, - jacobians: Optional[List[np.ndarray]]): +def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """Landmark Factor error function :param measurement: Landmark measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle diff --git a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py index e439bbaeb..a96da0774 100644 --- a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py +++ b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -31,7 +31,7 @@ def main(): firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) - # Initializing Pose3 - chordal relaxation" + # Initializing Pose3 - chordal relaxation initialization = gtsam.InitializePose3.initialize(graph) print(initialization) From 361f9f4fbd6ff54a163561f33d26ec8b4e9d0069 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 23 Oct 2021 01:14:27 -0400 Subject: [PATCH 138/169] add conjoining constructor for VectorValues --- gtsam/linear/linear.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 8635c55f8..d93b14d3e 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -221,6 +221,7 @@ class VectorValues { //Constructors VectorValues(); VectorValues(const gtsam::VectorValues& other); + VectorValues(const gtsam::VectorValues& first, const gtsam::VectorValues& second); //Named Constructors static gtsam::VectorValues Zero(const gtsam::VectorValues& model); From 48cc70c5068e4fef0ed6a489a2739001e01a7534 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 23 Oct 2021 12:45:21 -0400 Subject: [PATCH 139/169] generate GTSAM_UNSTABLE Cmake exports --- CMakeLists.txt | 5 +++++ gtsam_unstable/CMakeLists.txt | 6 +++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d2559705d..b8480867e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,6 +105,11 @@ endif() GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) +if (GTSAM_BUILD_UNSTABLE) + GtsamMakeConfigFile(GTSAM_UNSTABLE "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") + export(TARGETS ${GTSAM_UNSTABLE_EXPORTED_TARGETS} FILE GTSAM_UNSTABLE-exports.cmake) +endif() + # Check for doxygen availability - optional dependency find_package(Doxygen) diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 13c061b9b..98a1b4ef9 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -100,12 +100,12 @@ endif() install( TARGETS gtsam_unstable - EXPORT GTSAM-exports + EXPORT GTSAM_UNSTABLE-exports LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) -list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable) -set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) +list(APPEND GTSAM_UNSTABLE_EXPORTED_TARGETS gtsam_unstable) +set(GTSAM_UNSTABLE_EXPORTED_TARGETS "${GTSAM_UNSTABLE_EXPORTED_TARGETS}" PARENT_SCOPE) # Build examples add_subdirectory(examples) From f06d66df69babf0634fddbdd7783979ac46684a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 23 Oct 2021 12:45:44 -0400 Subject: [PATCH 140/169] update cmake function to set default package version number --- cmake/GtsamMakeConfigFile.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/GtsamMakeConfigFile.cmake b/cmake/GtsamMakeConfigFile.cmake index 0479a2524..91cb98a8c 100644 --- a/cmake/GtsamMakeConfigFile.cmake +++ b/cmake/GtsamMakeConfigFile.cmake @@ -27,6 +27,8 @@ function(GtsamMakeConfigFile PACKAGE_NAME) # here. if(NOT DEFINED ${PACKAGE_NAME}_VERSION AND DEFINED ${PACKAGE_NAME}_VERSION_STRING) set(${PACKAGE_NAME}_VERSION ${${PACKAGE_NAME}_VERSION_STRING}) + elseif(NOT DEFINED ${PACKAGE_NAME}_VERSION_STRING) + set(${PACKAGE_NAME}_VERSION ${GTSAM_VERSION_STRING}) endif() # Version file From 79f7861f0c14a0c3a502be0ff3447a1dc737ea28 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Sun, 24 Oct 2021 15:34:49 -0400 Subject: [PATCH 141/169] made changes according to Frank --- python/gtsam/examples/Pose2ISAM2Example.py | 158 ++++++++++-------- python/gtsam/examples/Pose3ISAM2Example.py | 182 ++++++++++++--------- 2 files changed, 189 insertions(+), 151 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index 5336bc34e..fac7ecc1a 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -13,23 +13,28 @@ Modelled after: - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ -from __future__ import print_function import math import numpy as np -from numpy.random import multivariate_normal as mult_gauss import gtsam import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot -def Pose2SLAM_ISAM2_plot(graph, current_estimate): - """Plots incremental state of the robot for 2D Pose SLAM using iSAM2 +def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, + key: int): + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2. Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) """ + # Print the current estimates computed using iSAM2. + print("*"*50 + f"\nInference after State {key+1}:\n") + print(current_estimate) + + # Compute the marginals for all states in the graph. marginals = gtsam.Marginals(graph, current_estimate) + # Plot the newly updated iSAM2 inference. fig = plt.figure(0) axes = fig.gca() plt.cla() @@ -43,19 +48,47 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate): axes.set_xlim(-1, 5) axes.set_ylim(-1, 3) plt.pause(1) + return marginals +def vector3(x, y, z): + """Create a 3D double numpy array.""" + return np.array([x, y, z], dtype=float) + +def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + key: int, xy_tol=0.6, theta_tol=0.3) -> int: + """Simple brute force approach which iterates through previous states + and checks for loop closure. + + Args: + odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. + current_estimate: The current estimates computed by iSAM2. + key: Key corresponding to the current state estimate of the robot. + xy_tol: Optional argument for the x-y measurement tolerance. + theta_tol: Optional argument for the theta measurement tolerance. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + prev_est = current_estimate.atPose2(key+1) + rotated_odom = prev_est.rotation().matrix() @ odom[:2] + curr_xy = np.array([prev_est.x() + rotated_odom[0], + prev_est.y() + rotated_odom[1]]) + curr_theta = prev_est.theta() + odom[2] + for k in range(1, key+1): + pose_xy = np.array([current_estimate.atPose2(k).x(), + current_estimate.atPose2(k).y()]) + pose_theta = current_estimate.atPose2(k).theta() + if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ + (abs(pose_theta - curr_theta) <= theta_tol): + return k def Pose2SLAM_ISAM2_example(): """Perform 2D SLAM given the ground truth changes in pose as well as - simple loop closure detection. - """ + simple loop closure detection.""" plt.ion() - def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) - # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. @@ -66,85 +99,68 @@ def Pose2SLAM_ISAM2_example(): graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() - # iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) parameters.setRelinearizeSkip(1) isam = gtsam.ISAM2(parameters) - # The ground truth odometry measurements (without noise) of the robot during the trajectory. - odom_arr = [(2, 0, 0), - (2, 0, math.pi/2), - (2, 0, math.pi/2), - (2, 0, math.pi/2), - (2, 0, math.pi/2)] + # Create the ground truth odometry measurements of the robot during the trajectory. + true_odometry = [(2, 0, 0), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2)] - # The initial estimates for robot poses 2-5. Pose 1 is initialized by the prior. - # To demonstrate iSAM2 incremental optimization, poor initializations were intentionally inserted. - pose_est = [gtsam.Pose2(2.3, 0.1, -0.2), - gtsam.Pose2(4.1, 0.1, math.pi/2), - gtsam.Pose2(4.0, 2.0, math.pi), - gtsam.Pose2(2.1, 2.1, -math.pi/2), - gtsam.Pose2(1.9, -0.2, 0.2)] + # Corrupt the odometry measurements with gaussian noise to create noisy odometry measurements. + odometry_measurements = [np.random.multivariate_normal(true_odom, ODOMETRY_NOISE.covariance()) + for true_odom in true_odometry] + # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate + # iSAM2 incremental optimization. graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, - xy_tol=0.6, theta_tol=0.3) -> int: - """Simple brute force approach which iterates through previous states - and checks for loop closure. + # Initialize the current estimate which is used during the incremental inference loop. + current_estimate = initial_estimate - Args: - odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. - current_estimate: The current estimates computed by iSAM2. - xy_tol: Optional argument for the x-y measurement tolerance. - theta_tol: Optional argument for the theta measurement tolerance. - Returns: - k: The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. - """ - if current_estimate: - prev_est = current_estimate.atPose2(i+1) - rotated_odom = prev_est.rotation().matrix() @ odom[:2] - curr_xy = np.array([prev_est.x() + rotated_odom[0], - prev_est.y() + rotated_odom[1]]) - curr_theta = prev_est.theta() + odom[2] - for k in range(1, i+1): - pose_xy = np.array([current_estimate.atPose2(k).x(), - current_estimate.atPose2(k).y()]) - pose_theta = current_estimate.atPose2(k).theta() - if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ - (abs(pose_theta - curr_theta) <= theta_tol): - return k + for i in range(len(true_odometry)): - current_estimate = None - for i in range(len(odom_arr)): - # The "ground truth" change between poses - odom_x, odom_y, odom_theta = odom_arr[i] - # Odometry measurement that is received by the robot and corrupted by gaussian noise - odom = mult_gauss(odom_arr[i], ODOMETRY_NOISE.covariance()) - # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state - loop = determine_loop_closure(odom, current_estimate) + # Obtain "ground truth" change between the current pose and the previous pose. + true_odom_x, true_odom_y, true_odom_theta = true_odometry[i] + + # Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise. + noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i] + + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. + loop = determine_loop_closure(odometry_measurements[i], current_estimate, i) + + # Add a binary factor in between two existing states if loop closure is detected. + # Otherwise, add a binary factor between a newly observed state and the previous state. + # Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement. + # This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used. if loop: - graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE)) + graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, + gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE)) else: - graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE)) - initial_estimate.insert(i + 2, pose_est[i]) - # Incremental update to iSAM2's internal Baye's tree, which only optimizes upon the added variables - # as well as any affected variables + graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, + gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE)) + + # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. + computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta)) + initial_estimate.insert(i + 2, computed_estimate) + + # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. isam.update(graph, initial_estimate) - # Another iSAM2 update can be performed for additional optimization - isam.update() current_estimate = isam.calculateEstimate() - print("*"*50) - print(f"Inference after State {i+1}:") - print(current_estimate) - marginals = Pose2SLAM_ISAM2_plot(graph, current_estimate) + + # Report all current state estimates from the iSAM2 optimzation. + marginals = report_on_progress(graph, current_estimate, i) initial_estimate.clear() - # Print the final covariance matrix for each pose after completing inference - for i in range(1, len(odom_arr)+1): + + # Print the final covariance matrix for each pose after completing inference on the trajectory. + for i in range(1, len(true_odometry)+1): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") plt.ioff() diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 3fcdcd7ec..3e78339bd 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -12,34 +12,36 @@ Modelled after version by: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ -from __future__ import print_function + import gtsam -from gtsam import Pose3, Rot3 -from gtsam.symbol_shorthand import X import gtsam.utils.plot as gtsam_plot import numpy as np -from numpy import cos, sin, pi -from numpy.random import multivariate_normal as mult_gauss import matplotlib.pyplot as plt - -def Pose3SLAM_ISAM2_plot(graph, current_estimate): - """Plots incremental state of the robot for 3D Pose SLAM using iSAM2 +def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, + key: int): + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2. Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) """ + # Print the current estimates computed using iSAM2. + print("*"*50 + f"\nInference after State {key+1}:\n") + print(current_estimate) + + # Compute the marginals for all states in the graph. marginals = gtsam.Marginals(graph, current_estimate) + # Plot the newly updated iSAM2 inference. fig = plt.figure(0) axes = fig.gca(projection='3d') plt.cla() - i = 0 - while current_estimate.exists(X(i)): - gtsam_plot.plot_pose3(0, current_estimate.atPose3(X(i)), 10, - marginals.marginalCovariance(X(i))) + i = 1 + while current_estimate.exists(i): + gtsam_plot.plot_pose3(0, current_estimate.atPose3(i), 10, + marginals.marginalCovariance(i)) i += 1 axes.set_xlim3d(-30, 45) @@ -49,7 +51,6 @@ def Pose3SLAM_ISAM2_plot(graph, current_estimate): return marginals - def createPoses(): """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], @@ -60,9 +61,9 @@ def createPoses(): [1, 0, 0, 15], [0, 0, 1, 20], [0, 0, 0, 1]]) - P2 = np.array([[cos(pi/4), 0, sin(pi/4), 30], + P2 = np.array([[np.cos(np.pi/4), 0, np.sin(np.pi/4), 30], [0, 1, 0, 30], - [-sin(pi/4), 0, cos(pi/4), 30], + [-np.sin(np.pi/4), 0, np.cos(np.pi/4), 30], [0, 0, 0, 1]]) P3 = np.array([[0, 1, 0, 30], [0, 0, -1, 0], @@ -74,99 +75,120 @@ def createPoses(): [0, 0, 0, 1]]) P5 = P0[:] - return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)] + return [gtsam.Pose3(P0), gtsam.Pose3(P1), gtsam.Pose3(P2), + gtsam.Pose3(P3), gtsam.Pose3(P4), gtsam.Pose3(P5)] + +def vector6(x, y, z, a, b, c): + """Create a 6D double numpy array.""" + return np.array([x, y, z, a, b, c], dtype=float) + +def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + key: int, xyz_tol=0.6, rot_tol=0.3) -> int: + """Simple brute force approach which iterates through previous states + and checks for loop closure. + + Args: + odom: Vector representing noisy odometry transformation measurement in the body frame, + where the vector represents [roll, pitch, yaw, x, y, z]. + current_estimate: The current estimates computed by iSAM2. + key: Key corresponding to the current state estimate of the robot. + xyz_tol: Optional argument for the translational tolerance. + rot_tol: Optional argument for the rotational tolerance. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + rot = gtsam.Rot3.RzRyRx(odom[:3]) + odom_tf = gtsam.Pose3(rot, odom[3:6].reshape(-1,1)) + prev_est = current_estimate.atPose3(key+1) + curr_est = prev_est.transformPoseFrom(odom_tf) + for k in range(1, key+1): + pose = current_estimate.atPose3(k) + if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \ + (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): + return k def Pose3_ISAM2_example(): """Perform 3D SLAM given ground truth poses as well as simple loop closure detection.""" plt.ion() - def vector6(x, y, z, a, b, c): - """Create 6d double numpy array.""" - return np.array([x, y, z, a, b, c], dtype=float) - # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3)) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2)) - # Create the ground truth poses of the robot trajectory. - poses = createPoses() + # Create a Nonlinear factor graph as well as the data structure to hold state estimates. + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() - # iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) parameters.setRelinearizeSkip(1) isam = gtsam.ISAM2(parameters) - # Create a Nonlinear factor graph as well as the data structure to hold state estimates. - graph = gtsam.NonlinearFactorGraph() - initial_estimate = gtsam.Values() + # Create the ground truth poses of the robot trajectory. + true_poses = createPoses() - # Add prior factor to the first pose with intentionally poor initial estimate - graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], PRIOR_NOISE)) - initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3( + # Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations + # between each robot pose in the trajectory. + odometry_tf = [true_poses[i-1].transformPoseTo(true_poses[i]) for i in range(1, len(true_poses))] + odometry_xyz = [(odometry_tf[i].x(), odometry_tf[i].y(), odometry_tf[i].z()) for i in range(len(odometry_tf))] + odometry_rpy = [odometry_tf[i].rotation().rpy() for i in range(len(odometry_tf))] + + # Corrupt the xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements. + noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), ODOMETRY_NOISE.covariance()) + for i in range(len(odometry_tf))] + + # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate + # iSAM2 incremental optimization. + graph.push_back(gtsam.PriorFactorPose3(1, true_poses[0], PRIOR_NOISE)) + initial_estimate.insert(1, true_poses[0].compose(gtsam.Pose3( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) - def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, - xyz_tol=0.6, rot_tol=0.3) -> int: - """Simple brute force approach which iterates through previous states - and checks for loop closure. + # Initialize the current estimate which is used during the incremental inference loop. + current_estimate = initial_estimate + for i in range(len(odometry_tf)): - Args: - odom: Vector representing noisy odometry transformation measurement in the body frame, - where the vector represents [roll, pitch, yaw, x, y, z]. - current_estimate: The current estimates computed by iSAM2. - xyz_tol: Optional argument for the translational tolerance. - rot_tol: Optional argument for the rotational tolerance. - Returns: - k: The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. - """ - if current_estimate: - rot = Rot3.RzRyRx(odom[:3]) - odom_tf = Pose3(rot, odom[3:6].reshape(-1,1)) - prev_est = current_estimate.atPose3(X(i-1)) - curr_est = prev_est.transformPoseFrom(odom_tf) - for k in range(i): - pose = current_estimate.atPose3(X(k)) - if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \ - (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): - return k + # Obtain "ground truth" transformation between the current pose and the previous pose. + true_odometry = odometry_tf[i] - current_estimate = None - for i in range(1, len(poses)): - # The "ground truth" change between poses - odom_tf = poses[i-1].transformPoseTo(poses[i]) - odom_xyz = odom_tf.x(), odom_tf.y(), odom_tf.z() - odom_rpy = odom_tf.rotation().rpy() - # Odometry measurement that is received by the robot and corrupted by gaussian noise - measurement = mult_gauss(np.hstack((odom_rpy,odom_xyz)), ODOMETRY_NOISE.covariance()) - loop = determine_loop_closure(measurement, current_estimate) - # If loop closure is detected, then adds a constraint between existing states in the factor graph - if loop is not None: - graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(loop), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) + # Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise. + noisy_odometry = noisy_measurements[i] + + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. + loop = determine_loop_closure(noisy_odometry, current_estimate, i) + + # Add a binary factor in between two existing states if loop closure is detected. + # Otherwise, add a binary factor between a newly observed state and the previous state. + # Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement. + # This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used. + if loop: + graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, true_odometry, ODOMETRY_NOISE)) else: - graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(i), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) - # Intentionally insert poor initializations - initial_estimate.insert(X(i), poses[i].compose(gtsam.Pose3( - gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) - # Performs iSAM2 incremental update based on newly added factors + graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, true_odometry, ODOMETRY_NOISE)) + + # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. + noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1)) + computed_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) + initial_estimate.insert(i + 2, computed_estimate) + + # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. isam.update(graph, initial_estimate) - # Additional iSAM2 optimization - isam.update() current_estimate = isam.calculateEstimate() - print("*"*50) - print(f"Inference after State {i}:\n") - print(current_estimate) - marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) + + # Report all current state estimates from the iSAM2 optimization. + marginals = report_on_progress(graph, current_estimate, i) initial_estimate.clear() - # Print the final covariance matrix for each pose after completing inference - i = 0 - while current_estimate.exists(X(i)): - print(f"X{i} covariance:\n{marginals.marginalCovariance(X(i))}\n") + + # Print the final covariance matrix for each pose after completing inference. + i = 1 + while current_estimate.exists(i): + print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") i += 1 plt.ioff() From c195bb562c060214d5a09fb225f3a47bd5127758 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Oct 2021 12:46:06 -0400 Subject: [PATCH 142/169] Squashed 'wrap/' changes from d6350c214..0ab10c359 0ab10c359 Fix pyparsing version and replace `create_symlinks` with `copy_directory` (#128) 230a3c967 Merge pull request #127 from borglab/feature/template-instantiator cc7d9a8b4 breakdown template instantiator into smaller submodules 59536220a Merge pull request #126 from borglab/feature/instantiation-tests 7ee715ecc add type info to template_instantiator b367ed93d tests for InstantiationHelper e41cfd50e tests for InstantiatedGlobalFunction 1837982a7 tests for InstantiatedClass a7e3678a3 tests for InstantiatedStaticMethod da06c53f7 tests for InstantiatedMethod c645c143c tests for InstantiatedConstructor b8a046267 tests for InstantiatedDeclaration af80c9d04 finish all tests for namespace level instantiation d6085c37a add tests for high level template instantiation f7ae91346 add docs and utility method d90abb52b Merge pull request #125 from borglab/feature/templated-static 58cdab20d clean up 247cea727 add helper for multilevel instantiation 761f588e4 update tests 81e5d5d19 update pybind wrapper to use new way 96d1575d8 streamlined way of instantiating template for class methods 1e4e88799 add to_cpp method for Method 485d43138 add the test fixtures 8cb943635 support instantiation of static method templates 84ef6679b add template to static method parser git-subtree-dir: wrap git-subtree-split: 0ab10c359a6528def20eddc60aced74a04250419 --- .github/workflows/macos-ci.yml | 2 + cmake/PybindWrap.cmake | 30 +- gtwrap/interface_parser/classes.py | 13 +- gtwrap/interface_parser/type.py | 5 + gtwrap/matlab_wrapper/mixins.py | 4 +- gtwrap/pybind_wrapper.py | 7 +- gtwrap/template_instantiator.py | 714 ------------------ gtwrap/template_instantiator/__init__.py | 14 + gtwrap/template_instantiator/classes.py | 206 +++++ gtwrap/template_instantiator/constructor.py | 64 ++ gtwrap/template_instantiator/declaration.py | 45 ++ gtwrap/template_instantiator/function.py | 68 ++ gtwrap/template_instantiator/helpers.py | 293 +++++++ gtwrap/template_instantiator/method.py | 124 +++ gtwrap/template_instantiator/namespace.py | 88 +++ requirements.txt | 4 +- tests/expected/matlab/FunDouble.m | 12 + .../matlab/MultipleTemplatesIntDouble.m | 4 +- .../matlab/MultipleTemplatesIntFloat.m | 4 +- tests/expected/matlab/MyFactorPosePoint2.m | 8 +- tests/expected/matlab/MyVector12.m | 6 +- tests/expected/matlab/MyVector3.m | 6 +- tests/expected/matlab/PrimitiveRefDouble.m | 8 +- tests/expected/matlab/Test.m | 64 +- tests/expected/matlab/class_wrapper.cpp | 230 +++--- tests/expected/python/class_pybind.cpp | 3 +- tests/fixtures/class.i | 3 + tests/test_interface_parser.py | 24 +- tests/test_matlab_wrapper.py | 33 +- tests/test_template_instantiator.py | 606 +++++++++++++++ 30 files changed, 1766 insertions(+), 926 deletions(-) delete mode 100644 gtwrap/template_instantiator.py create mode 100644 gtwrap/template_instantiator/__init__.py create mode 100644 gtwrap/template_instantiator/classes.py create mode 100644 gtwrap/template_instantiator/constructor.py create mode 100644 gtwrap/template_instantiator/declaration.py create mode 100644 gtwrap/template_instantiator/function.py create mode 100644 gtwrap/template_instantiator/helpers.py create mode 100644 gtwrap/template_instantiator/method.py create mode 100644 gtwrap/template_instantiator/namespace.py create mode 100644 tests/test_template_instantiator.py diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml index 3910d28d8..8119a3acb 100644 --- a/.github/workflows/macos-ci.yml +++ b/.github/workflows/macos-ci.yml @@ -27,10 +27,12 @@ jobs: - name: Python Dependencies run: | + pip3 install -U pip setuptools pip3 install -r requirements.txt - name: Build and Test run: | + # Build cmake . cd tests # Use Pytest to run all the tests. diff --git a/cmake/PybindWrap.cmake b/cmake/PybindWrap.cmake index f341c2f98..2149c7195 100644 --- a/cmake/PybindWrap.cmake +++ b/cmake/PybindWrap.cmake @@ -192,9 +192,9 @@ function(install_python_files source_files dest_directory) endfunction() # ~~~ -# https://stackoverflow.com/questions/13959434/cmake-out-of-source-build-python-files +# Copy over the directory from source_folder to dest_foler # ~~~ -function(create_symlinks source_folder dest_folder) +function(copy_directory source_folder dest_folder) if(${source_folder} STREQUAL ${dest_folder}) return() endif() @@ -215,31 +215,13 @@ function(create_symlinks source_folder dest_folder) # Create REAL folder file(MAKE_DIRECTORY "${dest_folder}") - # Delete symlink if it exists + # Delete if it exists file(REMOVE "${dest_folder}/${path_file}") - # Get OS dependent path to use in `execute_process` - file(TO_NATIVE_PATH "${dest_folder}/${path_file}" link) + # Get OS dependent path to use in copy file(TO_NATIVE_PATH "${source_folder}/${path_file}" target) - # cmake-format: off - if(UNIX) - set(command ln -s ${target} ${link}) - else() - set(command cmd.exe /c mklink ${link} ${target}) - endif() - # cmake-format: on - - execute_process( - COMMAND ${command} - RESULT_VARIABLE result - ERROR_VARIABLE output) - - if(NOT ${result} EQUAL 0) - message( - FATAL_ERROR - "Could not create symbolic link for: ${target} --> ${output}") - endif() + file(COPY ${target} DESTINATION ${dest_folder}) endforeach(path_file) -endfunction(create_symlinks) +endfunction(copy_directory) diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index 841c963c2..54beb86c1 100644 --- a/gtwrap/interface_parser/classes.py +++ b/gtwrap/interface_parser/classes.py @@ -62,6 +62,10 @@ class Method: self.parent = parent + def to_cpp(self) -> str: + """Generate the C++ code for wrapping.""" + return self.name + def __repr__(self) -> str: return "Method: {} {} {}({}){}".format( self.template, @@ -84,7 +88,8 @@ class StaticMethod: ``` """ rule = ( - STATIC # + Optional(Template.rule("template")) # + + STATIC # + ReturnType.rule("return_type") # + IDENT("name") # + LPAREN # @@ -92,16 +97,18 @@ class StaticMethod: + RPAREN # + SEMI_COLON # BR ).setParseAction( - lambda t: StaticMethod(t.name, t.return_type, t.args_list)) + lambda t: StaticMethod(t.name, t.return_type, t.args_list, t.template)) def __init__(self, name: str, return_type: ReturnType, args: ArgumentList, + template: Union[Template, Any] = None, parent: Union["Class", Any] = ''): self.name = name self.return_type = return_type self.args = args + self.template = template self.parent = parent @@ -221,8 +228,8 @@ class Class: Rule for all the members within a class. """ rule = ZeroOrMore(Constructor.rule # - ^ StaticMethod.rule # ^ Method.rule # + ^ StaticMethod.rule # ^ Variable.rule # ^ Operator.rule # ^ Enum.rule # diff --git a/gtwrap/interface_parser/type.py b/gtwrap/interface_parser/type.py index 49315cc56..e94db4ff2 100644 --- a/gtwrap/interface_parser/type.py +++ b/gtwrap/interface_parser/type.py @@ -158,6 +158,8 @@ class Type: """ Parsed datatype, can be either a fundamental type or a custom datatype. E.g. void, double, size_t, Matrix. + Think of this as a high-level type which encodes the typename and other + characteristics of the type. The type can optionally be a raw pointer, shared pointer or reference. Can also be optionally qualified with a `const`, e.g. `const int`. @@ -240,6 +242,9 @@ class Type: or self.typename.name in ["Matrix", "Vector"]) else "", typename=typename)) + def get_typename(self): + """Convenience method to get the typename of this type.""" + return self.typename.name class TemplatedType: """ diff --git a/gtwrap/matlab_wrapper/mixins.py b/gtwrap/matlab_wrapper/mixins.py index 217801ff3..2d7c75b39 100644 --- a/gtwrap/matlab_wrapper/mixins.py +++ b/gtwrap/matlab_wrapper/mixins.py @@ -112,7 +112,7 @@ class FormatMixin: if separator == "::": # C++ templates = [] - for idx in range(len(type_name.instantiations)): + for idx, _ in enumerate(type_name.instantiations): template = '{}'.format( self._format_type_name(type_name.instantiations[idx], include_namespace=include_namespace, @@ -124,7 +124,7 @@ class FormatMixin: formatted_type_name += '<{}>'.format(','.join(templates)) else: - for idx in range(len(type_name.instantiations)): + for idx, _ in enumerate(type_name.instantiations): formatted_type_name += '{}'.format( self._format_type_name(type_name.instantiations[idx], separator=separator, diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index 40571263a..809c69b56 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -119,8 +119,11 @@ class PybindWrapper: if py_method in self.python_keywords: py_method = py_method + "_" - is_method = isinstance(method, instantiator.InstantiatedMethod) - is_static = isinstance(method, parser.StaticMethod) + is_method = isinstance( + method, (parser.Method, instantiator.InstantiatedMethod)) + is_static = isinstance( + method, + (parser.StaticMethod, instantiator.InstantiatedStaticMethod)) return_void = method.return_type.is_void() args_names = method.args.names() py_args_names = self._py_args_names(method.args) diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py deleted file mode 100644 index b4d79655d..000000000 --- a/gtwrap/template_instantiator.py +++ /dev/null @@ -1,714 +0,0 @@ -"""Code to help instantiate templated classes, methods and functions.""" - -# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, unused-variable - -import itertools -from copy import deepcopy -from typing import Any, Iterable, List, Sequence - -import gtwrap.interface_parser as parser - - -def is_scoped_template(template_typenames, str_arg_typename): - """ - Check if the template given by `str_arg_typename` is a scoped template, - and if so, return what template and index matches the scoped template correctly. - """ - for idx, template in enumerate(template_typenames): - if template in str_arg_typename.split("::"): - return template, idx - return False, -1 - - -def instantiate_type(ctype: parser.Type, - template_typenames: List[str], - instantiations: List[parser.Typename], - cpp_typename: parser.Typename, - instantiated_class=None): - """ - Instantiate template typename for @p ctype. - - Args: - instiated_class (InstantiatedClass): - - @return If ctype's name is in the @p template_typenames, return the - corresponding type to replace in @p instantiations. - If ctype name is `This`, return the new typename @p `cpp_typename`. - Otherwise, return the original ctype. - """ - # make a deep copy so that there is no overwriting of original template params - ctype = deepcopy(ctype) - - # Check if the return type has template parameters - if ctype.typename.instantiations: - for idx, instantiation in enumerate(ctype.typename.instantiations): - if instantiation.name in template_typenames: - template_idx = template_typenames.index(instantiation.name) - ctype.typename.instantiations[ - idx] = instantiations[ # type: ignore - template_idx] - - return ctype - - str_arg_typename = str(ctype.typename) - - # Check if template is a scoped template e.g. T::Value where T is the template - scoped_template, scoped_idx = is_scoped_template(template_typenames, - str_arg_typename) - - # Instantiate templates which have enumerated instantiations in the template. - # E.g. `template`. - - # Instantiate scoped templates, e.g. T::Value. - if scoped_template: - # Create a copy of the instantiation so we can modify it. - instantiation = deepcopy(instantiations[scoped_idx]) - # Replace the part of the template with the instantiation - instantiation.name = str_arg_typename.replace(scoped_template, - instantiation.name) - return parser.Type( - typename=instantiation, - is_const=ctype.is_const, - is_shared_ptr=ctype.is_shared_ptr, - is_ptr=ctype.is_ptr, - is_ref=ctype.is_ref, - is_basic=ctype.is_basic, - ) - # Check for exact template match. - elif str_arg_typename in template_typenames: - idx = template_typenames.index(str_arg_typename) - return parser.Type( - typename=instantiations[idx], - is_const=ctype.is_const, - is_shared_ptr=ctype.is_shared_ptr, - is_ptr=ctype.is_ptr, - is_ref=ctype.is_ref, - is_basic=ctype.is_basic, - ) - - # If a method has the keyword `This`, we replace it with the (instantiated) class. - elif str_arg_typename == 'This': - # Check if the class is template instantiated - # so we can replace it with the instantiated version. - if instantiated_class: - name = instantiated_class.original.name - namespaces_name = instantiated_class.namespaces() - namespaces_name.append(name) - cpp_typename = parser.Typename( - namespaces_name, - instantiations=instantiated_class.instantiations) - - return parser.Type( - typename=cpp_typename, - is_const=ctype.is_const, - is_shared_ptr=ctype.is_shared_ptr, - is_ptr=ctype.is_ptr, - is_ref=ctype.is_ref, - is_basic=ctype.is_basic, - ) - - # Case when 'This' is present in the type namespace, e.g `This::Subclass`. - elif 'This' in str_arg_typename: - # Simply get the index of `This` in the namespace and replace it with the instantiated name. - namespace_idx = ctype.typename.namespaces.index('This') - ctype.typename.namespaces[namespace_idx] = cpp_typename.name - return ctype - - else: - return ctype - - -def instantiate_args_list(args_list, template_typenames, instantiations, - cpp_typename): - """ - Instantiate template typenames in an argument list. - Type with name `This` will be replaced by @p `cpp_typename`. - - @param[in] args_list A list of `parser.Argument` to instantiate. - @param[in] template_typenames List of template typenames to instantiate, - e.g. ['T', 'U', 'V']. - @param[in] instantiations List of specific types to instantiate, each - associated with each template typename. Each type is a parser.Typename, - including its name and full namespaces. - @param[in] cpp_typename Full-namespace cpp class name of this instantiation - to replace for arguments of type named `This`. - @return A new list of parser.Argument which types are replaced with their - instantiations. - """ - instantiated_args = [] - for arg in args_list: - new_type = instantiate_type(arg.ctype, template_typenames, - instantiations, cpp_typename) - instantiated_args.append( - parser.Argument(name=arg.name, ctype=new_type, - default=arg.default)) - return instantiated_args - - -def instantiate_return_type(return_type, - template_typenames, - instantiations, - cpp_typename, - instantiated_class=None): - """Instantiate the return type.""" - new_type1 = instantiate_type(return_type.type1, - template_typenames, - instantiations, - cpp_typename, - instantiated_class=instantiated_class) - if return_type.type2: - new_type2 = instantiate_type(return_type.type2, - template_typenames, - instantiations, - cpp_typename, - instantiated_class=instantiated_class) - else: - new_type2 = '' - return parser.ReturnType(new_type1, new_type2) - - -def instantiate_name(original_name, instantiations): - """ - Concatenate instantiated types with an @p original name to form a new - instantiated name. - TODO(duy): To avoid conflicts, we should include the instantiation's - namespaces, but I find that too verbose. - """ - instantiated_names = [] - for inst in instantiations: - # Ensure the first character of the type is capitalized - name = inst.instantiated_name() - # Using `capitalize` on the complete name causes other caps to be lower case - instantiated_names.append(name.replace(name[0], name[0].capitalize())) - - return "{}{}".format(original_name, "".join(instantiated_names)) - - -class InstantiatedGlobalFunction(parser.GlobalFunction): - """ - Instantiate global functions. - - E.g. - template - T add(const T& x, const T& y); - """ - def __init__(self, original, instantiations=(), new_name=''): - self.original = original - self.instantiations = instantiations - self.template = '' - self.parent = original.parent - - if not original.template: - self.name = original.name - self.return_type = original.return_type - self.args = original.args - else: - self.name = instantiate_name( - original.name, instantiations) if not new_name else new_name - self.return_type = instantiate_return_type( - original.return_type, - self.original.template.typenames, - self.instantiations, - # Keyword type name `This` should already be replaced in the - # previous class template instantiation round. - cpp_typename='', - ) - instantiated_args = instantiate_args_list( - original.args.list(), - self.original.template.typenames, - self.instantiations, - # Keyword type name `This` should already be replaced in the - # previous class template instantiation round. - cpp_typename='', - ) - self.args = parser.ArgumentList(instantiated_args) - - super().__init__(self.name, - self.return_type, - self.args, - self.template, - parent=self.parent) - - def to_cpp(self): - """Generate the C++ code for wrapping.""" - if self.original.template: - instantiated_names = [ - inst.instantiated_name() for inst in self.instantiations - ] - ret = "{}<{}>".format(self.original.name, - ",".join(instantiated_names)) - else: - ret = self.original.name - return ret - - def __repr__(self): - return "Instantiated {}".format( - super(InstantiatedGlobalFunction, self).__repr__()) - - -class InstantiatedMethod(parser.Method): - """ - Instantiate method with template parameters. - - E.g. - class A { - template - void func(X x, Y y); - } - """ - def __init__(self, - original: parser.Method, - instantiations: Iterable[parser.Typename] = ()): - self.original = original - self.instantiations = instantiations - self.template: Any = '' - self.is_const = original.is_const - self.parent = original.parent - - # Check for typenames if templated. - # This way, we can gracefully handle both templated and non-templated methods. - typenames: Sequence = self.original.template.typenames if self.original.template else [] - self.name = instantiate_name(original.name, self.instantiations) - self.return_type = instantiate_return_type( - original.return_type, - typenames, - self.instantiations, - # Keyword type name `This` should already be replaced in the - # previous class template instantiation round. - cpp_typename='', - ) - - instantiated_args = instantiate_args_list( - original.args.list(), - typenames, - self.instantiations, - # Keyword type name `This` should already be replaced in the - # previous class template instantiation round. - cpp_typename='', - ) - self.args = parser.ArgumentList(instantiated_args) - - super().__init__(self.template, - self.name, - self.return_type, - self.args, - self.is_const, - parent=self.parent) - - def to_cpp(self): - """Generate the C++ code for wrapping.""" - if self.original.template: - # to_cpp will handle all the namespacing and templating - instantiation_list = [x.to_cpp() for x in self.instantiations] - # now can simply combine the instantiations, separated by commas - ret = "{}<{}>".format(self.original.name, - ",".join(instantiation_list)) - else: - ret = self.original.name - return ret - - def __repr__(self): - return "Instantiated {}".format( - super(InstantiatedMethod, self).__repr__()) - - -class InstantiatedClass(parser.Class): - """ - Instantiate the class defined in the interface file. - """ - def __init__(self, original: parser.Class, instantiations=(), new_name=''): - """ - Template - Instantiations: [T1, U1] - """ - self.original = original - self.instantiations = instantiations - - self.template = None - self.is_virtual = original.is_virtual - self.parent_class = original.parent_class - self.parent = original.parent - - # If the class is templated, check if the number of provided instantiations - # match the number of templates, else it's only a partial instantiation which is bad. - if original.template: - assert len(original.template.typenames) == len( - instantiations), "Typenames and instantiations mismatch!" - - # Get the instantiated name of the class. E.g. FuncDouble - self.name = instantiate_name( - original.name, instantiations) if not new_name else new_name - - # Check for typenames if templated. - # By passing in typenames, we can gracefully handle both templated and non-templated classes - # This will allow the `This` keyword to be used in both templated and non-templated classes. - typenames = self.original.template.typenames if self.original.template else [] - - # Instantiate the constructors, static methods, properties, respectively. - self.ctors = self.instantiate_ctors(typenames) - self.static_methods = self.instantiate_static_methods(typenames) - self.properties = self.instantiate_properties(typenames) - - # Instantiate all operator overloads - self.operators = self.instantiate_operators(typenames) - - # Set enums - self.enums = original.enums - - # Instantiate all instance methods - instantiated_methods = \ - self.instantiate_class_templates_in_methods(typenames) - - # Second instantiation round to instantiate templated methods. - # This is done in case both the class and the method are templated. - self.methods = [] - for method in instantiated_methods: - if not method.template: - self.methods.append(InstantiatedMethod(method, ())) - else: - instantiations = [] - # Get all combinations of template parameters - for instantiations in itertools.product( - *method.template.instantiations): - self.methods.append( - InstantiatedMethod(method, instantiations)) - - super().__init__( - self.template, - self.is_virtual, - self.name, - [self.parent_class], - self.ctors, - self.methods, - self.static_methods, - self.properties, - self.operators, - self.enums, - parent=self.parent, - ) - - def __repr__(self): - return "{virtual}Class {cpp_class} : {parent_class}\n"\ - "{ctors}\n{static_methods}\n{methods}\n{operators}".format( - virtual="virtual " if self.is_virtual else '', - cpp_class=self.to_cpp(), - parent_class=self.parent, - ctors="\n".join([repr(ctor) for ctor in self.ctors]), - static_methods="\n".join([repr(m) - for m in self.static_methods]), - methods="\n".join([repr(m) for m in self.methods]), - operators="\n".join([repr(op) for op in self.operators]) - ) - - def instantiate_ctors(self, typenames): - """ - Instantiate the class constructors. - - Args: - typenames: List of template types to instantiate. - - Return: List of constructors instantiated with provided template args. - """ - instantiated_ctors = [] - - def instantiate(instantiated_ctors, ctor, typenames, instantiations): - instantiated_args = instantiate_args_list( - ctor.args.list(), - typenames, - instantiations, - self.cpp_typename(), - ) - instantiated_ctors.append( - parser.Constructor( - name=self.name, - args=parser.ArgumentList(instantiated_args), - template=self.original.template, - parent=self, - )) - return instantiated_ctors - - for ctor in self.original.ctors: - # Add constructor templates to the typenames and instantiations - if isinstance(ctor.template, parser.template.Template): - typenames.extend(ctor.template.typenames) - - # Get all combinations of template args - for instantiations in itertools.product( - *ctor.template.instantiations): - instantiations = self.instantiations + list(instantiations) - - instantiated_ctors = instantiate( - instantiated_ctors, - ctor, - typenames=typenames, - instantiations=instantiations) - - else: - # If no constructor level templates, just use the class templates - instantiated_ctors = instantiate( - instantiated_ctors, - ctor, - typenames=typenames, - instantiations=self.instantiations) - - return instantiated_ctors - - def instantiate_static_methods(self, typenames): - """ - Instantiate static methods in the class. - - Args: - typenames: List of template types to instantiate. - - Return: List of static methods instantiated with provided template args. - """ - instantiated_static_methods = [] - for static_method in self.original.static_methods: - instantiated_args = instantiate_args_list( - static_method.args.list(), typenames, self.instantiations, - self.cpp_typename()) - instantiated_static_methods.append( - parser.StaticMethod( - name=static_method.name, - return_type=instantiate_return_type( - static_method.return_type, - typenames, - self.instantiations, - self.cpp_typename(), - instantiated_class=self), - args=parser.ArgumentList(instantiated_args), - parent=self, - )) - return instantiated_static_methods - - def instantiate_class_templates_in_methods(self, typenames): - """ - This function only instantiates the class-level templates in the methods. - Template methods are instantiated in InstantiatedMethod in the second - round. - - E.g. - ``` - template - class Greeter{ - void sayHello(T& name); - }; - - Args: - typenames: List of template types to instantiate. - - Return: List of methods instantiated with provided template args on the class. - """ - class_instantiated_methods = [] - for method in self.original.methods: - instantiated_args = instantiate_args_list( - method.args.list(), - typenames, - self.instantiations, - self.cpp_typename(), - ) - class_instantiated_methods.append( - parser.Method( - template=method.template, - name=method.name, - return_type=instantiate_return_type( - method.return_type, - typenames, - self.instantiations, - self.cpp_typename(), - ), - args=parser.ArgumentList(instantiated_args), - is_const=method.is_const, - parent=self, - )) - return class_instantiated_methods - - def instantiate_operators(self, typenames): - """ - Instantiate the class-level template in the operator overload. - - Args: - typenames: List of template types to instantiate. - - Return: List of methods instantiated with provided template args on the class. - """ - instantiated_operators = [] - for operator in self.original.operators: - instantiated_args = instantiate_args_list( - operator.args.list(), - typenames, - self.instantiations, - self.cpp_typename(), - ) - instantiated_operators.append( - parser.Operator( - name=operator.name, - operator=operator.operator, - return_type=instantiate_return_type( - operator.return_type, - typenames, - self.instantiations, - self.cpp_typename(), - ), - args=parser.ArgumentList(instantiated_args), - is_const=operator.is_const, - parent=self, - )) - return instantiated_operators - - def instantiate_properties(self, typenames): - """ - Instantiate the class properties. - - Args: - typenames: List of template types to instantiate. - - Return: List of properties instantiated with provided template args. - """ - instantiated_properties = instantiate_args_list( - self.original.properties, - typenames, - self.instantiations, - self.cpp_typename(), - ) - return instantiated_properties - - def cpp_typename(self): - """ - Return a parser.Typename including namespaces and cpp name of this - class. - """ - if self.original.template: - name = "{}<{}>".format( - self.original.name, - ", ".join([inst.to_cpp() for inst in self.instantiations])) - else: - name = self.original.name - namespaces_name = self.namespaces() - namespaces_name.append(name) - return parser.Typename(namespaces_name) - - def to_cpp(self): - """Generate the C++ code for wrapping.""" - return self.cpp_typename().to_cpp() - - -class InstantiatedDeclaration(parser.ForwardDeclaration): - """ - Instantiate typedefs of forward declarations. - This is useful when we wish to typedef a templated class - which is not defined in the current project. - - E.g. - class FactorFromAnotherMother; - - typedef FactorFromAnotherMother FactorWeCanUse; - """ - def __init__(self, original, instantiations=(), new_name=''): - super().__init__(original.typename, - original.parent_type, - original.is_virtual, - parent=original.parent) - - self.original = original - self.instantiations = instantiations - self.parent = original.parent - - self.name = instantiate_name( - original.name, instantiations) if not new_name else new_name - - def to_cpp(self): - """Generate the C++ code for wrapping.""" - instantiated_names = [ - inst.qualified_name() for inst in self.instantiations - ] - name = "{}<{}>".format(self.original.name, - ",".join(instantiated_names)) - namespaces_name = self.namespaces() - namespaces_name.append(name) - # Leverage Typename to generate the fully qualified C++ name - return parser.Typename(namespaces_name).to_cpp() - - def __repr__(self): - return "Instantiated {}".format( - super(InstantiatedDeclaration, self).__repr__()) - - -def instantiate_namespace(namespace): - """ - Instantiate the classes and other elements in the `namespace` content and - assign it back to the namespace content attribute. - - @param[in/out] namespace The namespace whose content will be replaced with - the instantiated content. - """ - instantiated_content = [] - typedef_content = [] - - for element in namespace.content: - if isinstance(element, parser.Class): - original_class = element - if not original_class.template: - instantiated_content.append( - InstantiatedClass(original_class, [])) - else: - # This case is for when the templates have enumerated instantiations. - - # Use itertools to get all possible combinations of instantiations - # Works even if one template does not have an instantiation list - for instantiations in itertools.product( - *original_class.template.instantiations): - instantiated_content.append( - InstantiatedClass(original_class, - list(instantiations))) - - elif isinstance(element, parser.GlobalFunction): - original_func = element - if not original_func.template: - instantiated_content.append( - InstantiatedGlobalFunction(original_func, [])) - else: - # Use itertools to get all possible combinations of instantiations - # Works even if one template does not have an instantiation list - for instantiations in itertools.product( - *original_func.template.instantiations): - instantiated_content.append( - InstantiatedGlobalFunction(original_func, - list(instantiations))) - - elif isinstance(element, parser.TypedefTemplateInstantiation): - # This is for the case where `typedef` statements are used - # to specify the template parameters. - typedef_inst = element - top_level = namespace.top_level() - original_element = top_level.find_class_or_function( - typedef_inst.typename) - - # Check if element is a typedef'd class, function or - # forward declaration from another project. - if isinstance(original_element, parser.Class): - typedef_content.append( - InstantiatedClass(original_element, - typedef_inst.typename.instantiations, - typedef_inst.new_name)) - elif isinstance(original_element, parser.GlobalFunction): - typedef_content.append( - InstantiatedGlobalFunction( - original_element, typedef_inst.typename.instantiations, - typedef_inst.new_name)) - elif isinstance(original_element, parser.ForwardDeclaration): - typedef_content.append( - InstantiatedDeclaration( - original_element, typedef_inst.typename.instantiations, - typedef_inst.new_name)) - - elif isinstance(element, parser.Namespace): - element = instantiate_namespace(element) - instantiated_content.append(element) - else: - instantiated_content.append(element) - - instantiated_content.extend(typedef_content) - namespace.content = instantiated_content - - return namespace diff --git a/gtwrap/template_instantiator/__init__.py b/gtwrap/template_instantiator/__init__.py new file mode 100644 index 000000000..6a30bb3c3 --- /dev/null +++ b/gtwrap/template_instantiator/__init__.py @@ -0,0 +1,14 @@ +"""Code to help instantiate templated classes, methods and functions.""" + +# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, unused-variable. unused-argument, too-many-branches + +from typing import Iterable, Sequence, Union + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.classes import * +from gtwrap.template_instantiator.constructor import * +from gtwrap.template_instantiator.declaration import * +from gtwrap.template_instantiator.function import * +from gtwrap.template_instantiator.helpers import * +from gtwrap.template_instantiator.method import * +from gtwrap.template_instantiator.namespace import * diff --git a/gtwrap/template_instantiator/classes.py b/gtwrap/template_instantiator/classes.py new file mode 100644 index 000000000..af366f80f --- /dev/null +++ b/gtwrap/template_instantiator/classes.py @@ -0,0 +1,206 @@ +"""Instantiate a class and its members.""" + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.constructor import InstantiatedConstructor +from gtwrap.template_instantiator.helpers import (InstantiationHelper, + instantiate_args_list, + instantiate_name, + instantiate_return_type) +from gtwrap.template_instantiator.method import (InstantiatedMethod, + InstantiatedStaticMethod) + + +class InstantiatedClass(parser.Class): + """ + Instantiate the class defined in the interface file. + """ + def __init__(self, original: parser.Class, instantiations=(), new_name=''): + """ + Template + Instantiations: [T1, U1] + """ + self.original = original + self.instantiations = instantiations + + self.template = None + self.is_virtual = original.is_virtual + self.parent_class = original.parent_class + self.parent = original.parent + + # If the class is templated, check if the number of provided instantiations + # match the number of templates, else it's only a partial instantiation which is bad. + if original.template: + assert len(original.template.typenames) == len( + instantiations), "Typenames and instantiations mismatch!" + + # Get the instantiated name of the class. E.g. FuncDouble + self.name = instantiate_name( + original.name, instantiations) if not new_name else new_name + + # Check for typenames if templated. + # By passing in typenames, we can gracefully handle both templated and non-templated classes + # This will allow the `This` keyword to be used in both templated and non-templated classes. + typenames = self.original.template.typenames if self.original.template else [] + + # Instantiate the constructors, static methods, properties, respectively. + self.ctors = self.instantiate_ctors(typenames) + self.static_methods = self.instantiate_static_methods(typenames) + self.properties = self.instantiate_properties(typenames) + + # Instantiate all operator overloads + self.operators = self.instantiate_operators(typenames) + + # Set enums + self.enums = original.enums + + # Instantiate all instance methods + self.methods = self.instantiate_methods(typenames) + + super().__init__( + self.template, + self.is_virtual, + self.name, + [self.parent_class], + self.ctors, + self.methods, + self.static_methods, + self.properties, + self.operators, + self.enums, + parent=self.parent, + ) + + def __repr__(self): + return "{virtual}Class {cpp_class} : {parent_class}\n"\ + "{ctors}\n{static_methods}\n{methods}\n{operators}".format( + virtual="virtual " if self.is_virtual else '', + cpp_class=self.to_cpp(), + parent_class=self.parent, + ctors="\n".join([repr(ctor) for ctor in self.ctors]), + static_methods="\n".join([repr(m) + for m in self.static_methods]), + methods="\n".join([repr(m) for m in self.methods]), + operators="\n".join([repr(op) for op in self.operators]) + ) + + def instantiate_ctors(self, typenames): + """ + Instantiate the class constructors. + + Args: + typenames: List of template types to instantiate. + + Return: List of constructors instantiated with provided template args. + """ + + helper = InstantiationHelper( + instantiation_type=InstantiatedConstructor) + + instantiated_ctors = helper.multilevel_instantiation( + self.original.ctors, typenames, self) + + return instantiated_ctors + + def instantiate_static_methods(self, typenames): + """ + Instantiate static methods in the class. + + Args: + typenames: List of template types to instantiate. + + Return: List of static methods instantiated with provided template args. + """ + helper = InstantiationHelper( + instantiation_type=InstantiatedStaticMethod) + + instantiated_static_methods = helper.multilevel_instantiation( + self.original.static_methods, typenames, self) + + return instantiated_static_methods + + def instantiate_methods(self, typenames): + """ + Instantiate regular methods in the class. + + Args: + typenames: List of template types to instantiate. + + Return: List of methods instantiated with provided template args. + """ + instantiated_methods = [] + + helper = InstantiationHelper(instantiation_type=InstantiatedMethod) + + instantiated_methods = helper.multilevel_instantiation( + self.original.methods, typenames, self) + + return instantiated_methods + + def instantiate_operators(self, typenames): + """ + Instantiate the class-level template in the operator overload. + + Args: + typenames: List of template types to instantiate. + + Return: List of methods instantiated with provided template args on the class. + """ + instantiated_operators = [] + for operator in self.original.operators: + instantiated_args = instantiate_args_list( + operator.args.list(), + typenames, + self.instantiations, + self.cpp_typename(), + ) + instantiated_operators.append( + parser.Operator( + name=operator.name, + operator=operator.operator, + return_type=instantiate_return_type( + operator.return_type, + typenames, + self.instantiations, + self.cpp_typename(), + ), + args=parser.ArgumentList(instantiated_args), + is_const=operator.is_const, + parent=self, + )) + return instantiated_operators + + def instantiate_properties(self, typenames): + """ + Instantiate the class properties. + + Args: + typenames: List of template types to instantiate. + + Return: List of properties instantiated with provided template args. + """ + instantiated_properties = instantiate_args_list( + self.original.properties, + typenames, + self.instantiations, + self.cpp_typename(), + ) + return instantiated_properties + + def cpp_typename(self): + """ + Return a parser.Typename including namespaces and cpp name of this + class. + """ + if self.original.template: + name = "{}<{}>".format( + self.original.name, + ", ".join([inst.to_cpp() for inst in self.instantiations])) + else: + name = self.original.name + namespaces_name = self.namespaces() + namespaces_name.append(name) + return parser.Typename(namespaces_name) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + return self.cpp_typename().to_cpp() diff --git a/gtwrap/template_instantiator/constructor.py b/gtwrap/template_instantiator/constructor.py new file mode 100644 index 000000000..1ea7d22d5 --- /dev/null +++ b/gtwrap/template_instantiator/constructor.py @@ -0,0 +1,64 @@ +"""Class constructor instantiator.""" + +# pylint: disable=unused-argument + +from typing import Iterable, List + +import gtwrap.interface_parser as parser + + +class InstantiatedConstructor(parser.Constructor): + """ + Instantiate constructor with template parameters. + + E.g. + class A { + template + A(X x, Y y); + } + """ + def __init__(self, + original: parser.Constructor, + instantiations: Iterable[parser.Typename] = ()): + self.original = original + self.instantiations = instantiations + self.name = original.name + self.args = original.args + self.template = original.template + self.parent = original.parent + + super().__init__(self.name, + self.args, + self.template, + parent=self.parent) + + @classmethod + def construct(cls, original: parser.Constructor, typenames: List[str], + class_instantiations: List[parser.Typename], + method_instantiations: List[parser.Typename], + instantiated_args: List[parser.Argument], + parent: 'InstantiatedClass'): + """Class method to construct object as required by InstantiationHelper.""" + method = parser.Constructor( + name=parent.name, + args=parser.ArgumentList(instantiated_args), + template=original.template, + parent=parent, + ) + return InstantiatedConstructor(method, + instantiations=method_instantiations) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + if self.original.template: + # to_cpp will handle all the namespacing and templating + instantiation_list = [x.to_cpp() for x in self.instantiations] + # now can simply combine the instantiations, separated by commas + ret = "{}<{}>".format(self.original.name, + ",".join(instantiation_list)) + else: + ret = self.original.name + return ret + + def __repr__(self): + return "Instantiated {}".format(super().__repr__()) diff --git a/gtwrap/template_instantiator/declaration.py b/gtwrap/template_instantiator/declaration.py new file mode 100644 index 000000000..4fa6b75d8 --- /dev/null +++ b/gtwrap/template_instantiator/declaration.py @@ -0,0 +1,45 @@ +"""Instantiate a forward declaration.""" + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.helpers import instantiate_name + + +class InstantiatedDeclaration(parser.ForwardDeclaration): + """ + Instantiate typedefs of forward declarations. + This is useful when we wish to typedef a templated class + which is not defined in the current project. + + E.g. + class FactorFromAnotherMother; + + typedef FactorFromAnotherMother FactorWeCanUse; + """ + def __init__(self, original, instantiations=(), new_name=''): + super().__init__(original.typename, + original.parent_type, + original.is_virtual, + parent=original.parent) + + self.original = original + self.instantiations = instantiations + self.parent = original.parent + + self.name = instantiate_name( + original.name, instantiations) if not new_name else new_name + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + instantiated_names = [ + inst.qualified_name() for inst in self.instantiations + ] + name = "{}<{}>".format(self.original.name, + ",".join(instantiated_names)) + namespaces_name = self.namespaces() + namespaces_name.append(name) + # Leverage Typename to generate the fully qualified C++ name + return parser.Typename(namespaces_name).to_cpp() + + def __repr__(self): + return "Instantiated {}".format( + super(InstantiatedDeclaration, self).__repr__()) diff --git a/gtwrap/template_instantiator/function.py b/gtwrap/template_instantiator/function.py new file mode 100644 index 000000000..3ad5da3f4 --- /dev/null +++ b/gtwrap/template_instantiator/function.py @@ -0,0 +1,68 @@ +"""Instantiate global function.""" + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.helpers import (instantiate_args_list, + instantiate_name, + instantiate_return_type) + + +class InstantiatedGlobalFunction(parser.GlobalFunction): + """ + Instantiate global functions. + + E.g. + template + T add(const T& x, const T& y); + """ + def __init__(self, original, instantiations=(), new_name=''): + self.original = original + self.instantiations = instantiations + self.template = '' + self.parent = original.parent + + if not original.template: + self.name = original.name + self.return_type = original.return_type + self.args = original.args + else: + self.name = instantiate_name( + original.name, instantiations) if not new_name else new_name + self.return_type = instantiate_return_type( + original.return_type, + self.original.template.typenames, + self.instantiations, + # Keyword type name `This` should already be replaced in the + # previous class template instantiation round. + cpp_typename='', + ) + instantiated_args = instantiate_args_list( + original.args.list(), + self.original.template.typenames, + self.instantiations, + # Keyword type name `This` should already be replaced in the + # previous class template instantiation round. + cpp_typename='', + ) + self.args = parser.ArgumentList(instantiated_args) + + super().__init__(self.name, + self.return_type, + self.args, + self.template, + parent=self.parent) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + if self.original.template: + instantiated_names = [ + inst.instantiated_name() for inst in self.instantiations + ] + ret = "{}<{}>".format(self.original.name, + ",".join(instantiated_names)) + else: + ret = self.original.name + return ret + + def __repr__(self): + return "Instantiated {}".format( + super(InstantiatedGlobalFunction, self).__repr__()) diff --git a/gtwrap/template_instantiator/helpers.py b/gtwrap/template_instantiator/helpers.py new file mode 100644 index 000000000..b55515dba --- /dev/null +++ b/gtwrap/template_instantiator/helpers.py @@ -0,0 +1,293 @@ +"""Various helpers for instantiation.""" + +import itertools +from copy import deepcopy +from typing import List, Sequence, Union + +import gtwrap.interface_parser as parser + +ClassMembers = Union[parser.Constructor, parser.Method, parser.StaticMethod, + parser.GlobalFunction, parser.Operator, parser.Variable, + parser.Enum] +InstantiatedMembers = Union['InstantiatedConstructor', 'InstantiatedMethod', + 'InstantiatedStaticMethod', + 'InstantiatedGlobalFunction'] + + +def is_scoped_template(template_typenames: Sequence[str], + str_arg_typename: str): + """ + Check if the template given by `str_arg_typename` is a scoped template e.g. T::Value, + and if so, return what template from `template_typenames` and + the corresponding index matches the scoped template correctly. + """ + for idx, template in enumerate(template_typenames): + if "::" in str_arg_typename and \ + template in str_arg_typename.split("::"): + return template, idx + return False, -1 + + +def instantiate_type( + ctype: parser.Type, + template_typenames: Sequence[str], + instantiations: Sequence[parser.Typename], + cpp_typename: parser.Typename, + instantiated_class: 'InstantiatedClass' = None) -> parser.Type: + """ + Instantiate template typename for `ctype`. + + Args: + ctype: The original argument type. + template_typenames: List of strings representing the templates. + instantiations: List of the instantiations of the templates in `template_typenames`. + cpp_typename: Full-namespace cpp class name of this instantiation + to replace for arguments of type named `This`. + instiated_class: The instantiated class which, if provided, + will be used for instantiating `This`. + + Returns: + If `ctype`'s name is in the `template_typenames`, return the + corresponding type to replace in `instantiations`. + If ctype name is `This`, return the new typename `cpp_typename`. + Otherwise, return the original ctype. + """ + # make a deep copy so that there is no overwriting of original template params + ctype = deepcopy(ctype) + + # Check if the return type has template parameters + if ctype.typename.instantiations: + for idx, instantiation in enumerate(ctype.typename.instantiations): + if instantiation.name in template_typenames: + template_idx = template_typenames.index(instantiation.name) + ctype.typename.instantiations[ + idx] = instantiations[ # type: ignore + template_idx] + + return ctype + + str_arg_typename = str(ctype.typename) + + # Check if template is a scoped template e.g. T::Value where T is the template + scoped_template, scoped_idx = is_scoped_template(template_typenames, + str_arg_typename) + + # Instantiate templates which have enumerated instantiations in the template. + # E.g. `template`. + + # Instantiate scoped templates, e.g. T::Value. + if scoped_template: + # Create a copy of the instantiation so we can modify it. + instantiation = deepcopy(instantiations[scoped_idx]) + # Replace the part of the template with the instantiation + instantiation.name = str_arg_typename.replace(scoped_template, + instantiation.name) + return parser.Type( + typename=instantiation, + is_const=ctype.is_const, + is_shared_ptr=ctype.is_shared_ptr, + is_ptr=ctype.is_ptr, + is_ref=ctype.is_ref, + is_basic=ctype.is_basic, + ) + # Check for exact template match. + elif str_arg_typename in template_typenames: + idx = template_typenames.index(str_arg_typename) + return parser.Type( + typename=instantiations[idx], + is_const=ctype.is_const, + is_shared_ptr=ctype.is_shared_ptr, + is_ptr=ctype.is_ptr, + is_ref=ctype.is_ref, + is_basic=ctype.is_basic, + ) + + # If a method has the keyword `This`, we replace it with the (instantiated) class. + elif str_arg_typename == 'This': + # Check if the class is template instantiated + # so we can replace it with the instantiated version. + if instantiated_class: + name = instantiated_class.original.name + namespaces_name = instantiated_class.namespaces() + namespaces_name.append(name) + cpp_typename = parser.Typename( + namespaces_name, + instantiations=instantiated_class.instantiations) + + return parser.Type( + typename=cpp_typename, + is_const=ctype.is_const, + is_shared_ptr=ctype.is_shared_ptr, + is_ptr=ctype.is_ptr, + is_ref=ctype.is_ref, + is_basic=ctype.is_basic, + ) + + # Case when 'This' is present in the type namespace, e.g `This::Subclass`. + elif 'This' in str_arg_typename: + # Simply get the index of `This` in the namespace and replace it with the instantiated name. + namespace_idx = ctype.typename.namespaces.index('This') + ctype.typename.namespaces[namespace_idx] = cpp_typename.name + return ctype + + else: + return ctype + + +def instantiate_args_list( + args_list: Sequence[parser.Argument], + template_typenames: Sequence[parser.template.Typename], + instantiations: Sequence, cpp_typename: parser.Typename): + """ + Instantiate template typenames in an argument list. + Type with name `This` will be replaced by @p `cpp_typename`. + + @param[in] args_list A list of `parser.Argument` to instantiate. + @param[in] template_typenames List of template typenames to instantiate, + e.g. ['T', 'U', 'V']. + @param[in] instantiations List of specific types to instantiate, each + associated with each template typename. Each type is a parser.Typename, + including its name and full namespaces. + @param[in] cpp_typename Full-namespace cpp class name of this instantiation + to replace for arguments of type named `This`. + @return A new list of parser.Argument which types are replaced with their + instantiations. + """ + instantiated_args = [] + for arg in args_list: + new_type = instantiate_type(arg.ctype, template_typenames, + instantiations, cpp_typename) + instantiated_args.append( + parser.Argument(name=arg.name, ctype=new_type, + default=arg.default)) + return instantiated_args + + +def instantiate_return_type( + return_type: parser.ReturnType, + template_typenames: Sequence[parser.template.Typename], + instantiations: Sequence[parser.Typename], + cpp_typename: parser.Typename, + instantiated_class: 'InstantiatedClass' = None): + """Instantiate the return type.""" + new_type1 = instantiate_type(return_type.type1, + template_typenames, + instantiations, + cpp_typename, + instantiated_class=instantiated_class) + if return_type.type2: + new_type2 = instantiate_type(return_type.type2, + template_typenames, + instantiations, + cpp_typename, + instantiated_class=instantiated_class) + else: + new_type2 = '' + return parser.ReturnType(new_type1, new_type2) + + +def instantiate_name(original_name: str, + instantiations: Sequence[parser.Typename]): + """ + Concatenate instantiated types with `original_name` to form a new + instantiated name. + + NOTE: To avoid conflicts, we should include the instantiation's + namespaces, but that is too verbose. + """ + instantiated_names = [] + for inst in instantiations: + # Ensure the first character of the type is capitalized + name = inst.instantiated_name() + # Using `capitalize` on the complete name causes other caps to be lower case + instantiated_names.append(name.replace(name[0], name[0].capitalize())) + + return "{}{}".format(original_name, "".join(instantiated_names)) + + +class InstantiationHelper: + """ + Helper class for instantiation templates. + Requires that `instantiation_type` defines a class method called + `construct` to generate the appropriate object type. + + Signature for `construct` should be + ``` + construct(method, + typenames, + class_instantiations, + method_instantiations, + instantiated_args, + parent=parent) + ``` + """ + def __init__(self, instantiation_type: InstantiatedMembers): + self.instantiation_type = instantiation_type + + def instantiate(self, instantiated_methods: List[InstantiatedMembers], + method: ClassMembers, typenames: Sequence[str], + class_instantiations: Sequence[parser.Typename], + method_instantiations: Sequence[parser.Typename], + parent: 'InstantiatedClass'): + """ + Instantiate both the class and method level templates. + """ + instantiations = class_instantiations + method_instantiations + + instantiated_args = instantiate_args_list(method.args.list(), + typenames, instantiations, + parent.cpp_typename()) + + instantiated_methods.append( + self.instantiation_type.construct(method, + typenames, + class_instantiations, + method_instantiations, + instantiated_args, + parent=parent)) + + return instantiated_methods + + def multilevel_instantiation(self, methods_list: Sequence[ClassMembers], + typenames: Sequence[str], + parent: 'InstantiatedClass'): + """ + Helper to instantiate methods at both the class and method level. + + Args: + methods_list: The list of methods in the class to instantiated. + typenames: List of class level template parameters, e.g. ['T']. + parent: The instantiated class to which `methods_list` belongs. + """ + instantiated_methods = [] + + for method in methods_list: + # We creare a copy since we will modify the typenames list. + method_typenames = deepcopy(typenames) + + if isinstance(method.template, parser.template.Template): + method_typenames.extend(method.template.typenames) + + # Get all combinations of template args + for instantiations in itertools.product( + *method.template.instantiations): + + instantiated_methods = self.instantiate( + instantiated_methods, + method, + typenames=method_typenames, + class_instantiations=parent.instantiations, + method_instantiations=list(instantiations), + parent=parent) + + else: + # If no constructor level templates, just use the class templates + instantiated_methods = self.instantiate( + instantiated_methods, + method, + typenames=method_typenames, + class_instantiations=parent.instantiations, + method_instantiations=[], + parent=parent) + + return instantiated_methods diff --git a/gtwrap/template_instantiator/method.py b/gtwrap/template_instantiator/method.py new file mode 100644 index 000000000..cd0a09c90 --- /dev/null +++ b/gtwrap/template_instantiator/method.py @@ -0,0 +1,124 @@ +"""Class method and static method instantiators.""" + +from typing import Iterable + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.helpers import (instantiate_name, + instantiate_return_type) + + +class InstantiatedMethod(parser.Method): + """ + Instantiate method with template parameters. + + E.g. + class A { + template + void func(X x, Y y); + } + """ + def __init__(self, + original: parser.Method, + instantiations: Iterable[parser.Typename] = ()): + self.original = original + self.instantiations = instantiations + self.template = original.template + self.is_const = original.is_const + self.parent = original.parent + + self.name = instantiate_name(original.name, self.instantiations) + self.return_type = original.return_type + self.args = original.args + + super().__init__(self.template, + self.name, + self.return_type, + self.args, + self.is_const, + parent=self.parent) + + @classmethod + def construct(cls, original, typenames, class_instantiations, + method_instantiations, instantiated_args, parent): + """Class method to construct object as required by InstantiationHelper.""" + method = parser.Method( + template=original.template, + name=original.name, + return_type=instantiate_return_type( + original.return_type, typenames, + class_instantiations + method_instantiations, + parent.cpp_typename()), + args=parser.ArgumentList(instantiated_args), + is_const=original.is_const, + parent=parent, + ) + return InstantiatedMethod(method, instantiations=method_instantiations) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + if self.original.template: + # to_cpp will handle all the namespacing and templating + instantiation_list = [x.to_cpp() for x in self.instantiations] + # now can simply combine the instantiations, separated by commas + ret = "{}<{}>".format(self.original.name, + ",".join(instantiation_list)) + else: + ret = self.original.name + return ret + + def __repr__(self): + return "Instantiated {}".format(super().__repr__()) + + +class InstantiatedStaticMethod(parser.StaticMethod): + """ + Instantiate static method with template parameters. + """ + def __init__(self, + original: parser.StaticMethod, + instantiations: Iterable[parser.Typename] = ()): + self.original = original + self.instantiations = instantiations + + self.name = instantiate_name(original.name, self.instantiations) + self.return_type = original.return_type + self.args = original.args + self.template = original.template + self.parent = original.parent + + super().__init__(self.name, self.return_type, self.args, self.template, + self.parent) + + @classmethod + def construct(cls, original, typenames, class_instantiations, + method_instantiations, instantiated_args, parent): + """Class method to construct object as required by InstantiationHelper.""" + method = parser.StaticMethod( + name=original.name, + return_type=instantiate_return_type(original.return_type, + typenames, + class_instantiations + + method_instantiations, + parent.cpp_typename(), + instantiated_class=parent), + args=parser.ArgumentList(instantiated_args), + template=original.template, + parent=parent, + ) + return InstantiatedStaticMethod(method, + instantiations=method_instantiations) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + if self.original.template: + # to_cpp will handle all the namespacing and templating + instantiation_list = [x.to_cpp() for x in self.instantiations] + # now can simply combine the instantiations, separated by commas + ret = "{}<{}>".format(self.original.name, + ",".join(instantiation_list)) + else: + ret = self.original.name + return ret + + def __repr__(self): + return "Instantiated {}".format(super().__repr__()) diff --git a/gtwrap/template_instantiator/namespace.py b/gtwrap/template_instantiator/namespace.py new file mode 100644 index 000000000..32ba0b95d --- /dev/null +++ b/gtwrap/template_instantiator/namespace.py @@ -0,0 +1,88 @@ +"""Instantiate a namespace.""" + +import itertools + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.classes import InstantiatedClass +from gtwrap.template_instantiator.declaration import InstantiatedDeclaration +from gtwrap.template_instantiator.function import InstantiatedGlobalFunction + + +def instantiate_namespace(namespace): + """ + Instantiate the classes and other elements in the `namespace` content and + assign it back to the namespace content attribute. + + @param[in/out] namespace The namespace whose content will be replaced with + the instantiated content. + """ + instantiated_content = [] + typedef_content = [] + + for element in namespace.content: + if isinstance(element, parser.Class): + original_class = element + if not original_class.template: + instantiated_content.append( + InstantiatedClass(original_class, [])) + else: + # This case is for when the templates have enumerated instantiations. + + # Use itertools to get all possible combinations of instantiations + # Works even if one template does not have an instantiation list + for instantiations in itertools.product( + *original_class.template.instantiations): + instantiated_content.append( + InstantiatedClass(original_class, + list(instantiations))) + + elif isinstance(element, parser.GlobalFunction): + original_func = element + if not original_func.template: + instantiated_content.append( + InstantiatedGlobalFunction(original_func, [])) + else: + # Use itertools to get all possible combinations of instantiations + # Works even if one template does not have an instantiation list + for instantiations in itertools.product( + *original_func.template.instantiations): + instantiated_content.append( + InstantiatedGlobalFunction(original_func, + list(instantiations))) + + elif isinstance(element, parser.TypedefTemplateInstantiation): + # This is for the case where `typedef` statements are used + # to specify the template parameters. + typedef_inst = element + top_level = namespace.top_level() + original_element = top_level.find_class_or_function( + typedef_inst.typename) + + # Check if element is a typedef'd class, function or + # forward declaration from another project. + if isinstance(original_element, parser.Class): + typedef_content.append( + InstantiatedClass(original_element, + typedef_inst.typename.instantiations, + typedef_inst.new_name)) + elif isinstance(original_element, parser.GlobalFunction): + typedef_content.append( + InstantiatedGlobalFunction( + original_element, typedef_inst.typename.instantiations, + typedef_inst.new_name)) + elif isinstance(original_element, parser.ForwardDeclaration): + typedef_content.append( + InstantiatedDeclaration( + original_element, typedef_inst.typename.instantiations, + typedef_inst.new_name)) + + elif isinstance(element, parser.Namespace): + element = instantiate_namespace(element) + instantiated_content.append(element) + else: + instantiated_content.append(element) + + instantiated_content.extend(typedef_content) + namespace.content = instantiated_content + + return namespace diff --git a/requirements.txt b/requirements.txt index a7181b271..0aac9302e 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,2 +1,2 @@ -pyparsing -pytest +pyparsing==2.4.7 +pytest>=6.2.4 diff --git a/tests/expected/matlab/FunDouble.m b/tests/expected/matlab/FunDouble.m index ca0efcacf..78609c7f6 100644 --- a/tests/expected/matlab/FunDouble.m +++ b/tests/expected/matlab/FunDouble.m @@ -7,6 +7,7 @@ % %-------Static Methods------- %staticMethodWithThis() : returns Fun +%templatedStaticMethodInt(int m) : returns double % %-------Serialization Interface------- %string_serialize() : returns string @@ -69,5 +70,16 @@ classdef FunDouble < handle error('Arguments do not match any overload of function FunDouble.staticMethodWithThis'); end + function varargout = TemplatedStaticMethodInt(varargin) + % TEMPLATEDSTATICMETHODINT usage: templatedStaticMethodInt(int m) : returns double + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = class_wrapper(10, varargin{:}); + return + end + + error('Arguments do not match any overload of function FunDouble.templatedStaticMethodInt'); + end + end end diff --git a/tests/expected/matlab/MultipleTemplatesIntDouble.m b/tests/expected/matlab/MultipleTemplatesIntDouble.m index 1a00572e0..863d30ee8 100644 --- a/tests/expected/matlab/MultipleTemplatesIntDouble.m +++ b/tests/expected/matlab/MultipleTemplatesIntDouble.m @@ -9,7 +9,7 @@ classdef MultipleTemplatesIntDouble < handle function obj = MultipleTemplatesIntDouble(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(49, my_ptr); + class_wrapper(50, my_ptr); else error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor'); end @@ -17,7 +17,7 @@ classdef MultipleTemplatesIntDouble < handle end function delete(obj) - class_wrapper(50, obj.ptr_MultipleTemplatesIntDouble); + class_wrapper(51, obj.ptr_MultipleTemplatesIntDouble); end function display(obj), obj.print(''); end diff --git a/tests/expected/matlab/MultipleTemplatesIntFloat.m b/tests/expected/matlab/MultipleTemplatesIntFloat.m index 6239b1bd1..b7f1fdac5 100644 --- a/tests/expected/matlab/MultipleTemplatesIntFloat.m +++ b/tests/expected/matlab/MultipleTemplatesIntFloat.m @@ -9,7 +9,7 @@ classdef MultipleTemplatesIntFloat < handle function obj = MultipleTemplatesIntFloat(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(51, my_ptr); + class_wrapper(52, my_ptr); else error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor'); end @@ -17,7 +17,7 @@ classdef MultipleTemplatesIntFloat < handle end function delete(obj) - class_wrapper(52, obj.ptr_MultipleTemplatesIntFloat); + class_wrapper(53, obj.ptr_MultipleTemplatesIntFloat); end function display(obj), obj.print(''); end diff --git a/tests/expected/matlab/MyFactorPosePoint2.m b/tests/expected/matlab/MyFactorPosePoint2.m index e4cd92196..7634ae2cb 100644 --- a/tests/expected/matlab/MyFactorPosePoint2.m +++ b/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(62, my_ptr); + class_wrapper(63, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = class_wrapper(63, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(64, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - class_wrapper(64, obj.ptr_MyFactorPosePoint2); + class_wrapper(65, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,7 +36,7 @@ classdef MyFactorPosePoint2 < handle % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(65, this, varargin{:}); + class_wrapper(66, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/tests/expected/matlab/MyVector12.m b/tests/expected/matlab/MyVector12.m index 00a8f1965..291d0d71b 100644 --- a/tests/expected/matlab/MyVector12.m +++ b/tests/expected/matlab/MyVector12.m @@ -12,9 +12,9 @@ classdef MyVector12 < handle function obj = MyVector12(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(46, my_ptr); + class_wrapper(47, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(47); + my_ptr = class_wrapper(48); else error('Arguments do not match any overload of MyVector12 constructor'); end @@ -22,7 +22,7 @@ classdef MyVector12 < handle end function delete(obj) - class_wrapper(48, obj.ptr_MyVector12); + class_wrapper(49, obj.ptr_MyVector12); end function display(obj), obj.print(''); end diff --git a/tests/expected/matlab/MyVector3.m b/tests/expected/matlab/MyVector3.m index 5d4a80cd8..3051dc2e2 100644 --- a/tests/expected/matlab/MyVector3.m +++ b/tests/expected/matlab/MyVector3.m @@ -12,9 +12,9 @@ classdef MyVector3 < handle function obj = MyVector3(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(43, my_ptr); + class_wrapper(44, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(44); + my_ptr = class_wrapper(45); else error('Arguments do not match any overload of MyVector3 constructor'); end @@ -22,7 +22,7 @@ classdef MyVector3 < handle end function delete(obj) - class_wrapper(45, obj.ptr_MyVector3); + class_wrapper(46, obj.ptr_MyVector3); end function display(obj), obj.print(''); end diff --git a/tests/expected/matlab/PrimitiveRefDouble.m b/tests/expected/matlab/PrimitiveRefDouble.m index 14f04a825..dd0a6d2da 100644 --- a/tests/expected/matlab/PrimitiveRefDouble.m +++ b/tests/expected/matlab/PrimitiveRefDouble.m @@ -19,9 +19,9 @@ classdef PrimitiveRefDouble < handle function obj = PrimitiveRefDouble(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(39, my_ptr); + class_wrapper(40, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(40); + my_ptr = class_wrapper(41); else error('Arguments do not match any overload of PrimitiveRefDouble constructor'); end @@ -29,7 +29,7 @@ classdef PrimitiveRefDouble < handle end function delete(obj) - class_wrapper(41, obj.ptr_PrimitiveRefDouble); + class_wrapper(42, obj.ptr_PrimitiveRefDouble); end function display(obj), obj.print(''); end @@ -43,7 +43,7 @@ classdef PrimitiveRefDouble < handle % BRUTAL usage: Brutal(double t) : returns PrimitiveRefdouble % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(42, varargin{:}); + varargout{1} = class_wrapper(43, varargin{:}); return end diff --git a/tests/expected/matlab/Test.m b/tests/expected/matlab/Test.m index c39882a93..8569120c5 100644 --- a/tests/expected/matlab/Test.m +++ b/tests/expected/matlab/Test.m @@ -40,11 +40,11 @@ classdef Test < handle function obj = Test(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(10, my_ptr); + class_wrapper(11, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(11); + my_ptr = class_wrapper(12); elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') - my_ptr = class_wrapper(12, varargin{1}, varargin{2}); + my_ptr = class_wrapper(13, varargin{1}, varargin{2}); else error('Arguments do not match any overload of Test constructor'); end @@ -52,7 +52,7 @@ classdef Test < handle end function delete(obj) - class_wrapper(13, obj.ptr_Test); + class_wrapper(14, obj.ptr_Test); end function display(obj), obj.print(''); end @@ -63,7 +63,7 @@ classdef Test < handle % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - class_wrapper(14, this, varargin{:}); + class_wrapper(15, this, varargin{:}); return end error('Arguments do not match any overload of function Test.arg_EigenConstRef'); @@ -73,7 +73,7 @@ classdef Test < handle % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - [ varargout{1} varargout{2} ] = class_wrapper(15, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(16, this, varargin{:}); return end error('Arguments do not match any overload of function Test.create_MixedPtrs'); @@ -83,7 +83,7 @@ classdef Test < handle % CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - [ varargout{1} varargout{2} ] = class_wrapper(16, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(17, this, varargin{:}); return end error('Arguments do not match any overload of function Test.create_ptrs'); @@ -93,7 +93,7 @@ classdef Test < handle % GET_CONTAINER usage: get_container() : returns std.vectorTest % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - varargout{1} = class_wrapper(17, this, varargin{:}); + varargout{1} = class_wrapper(18, this, varargin{:}); return end error('Arguments do not match any overload of function Test.get_container'); @@ -103,7 +103,7 @@ classdef Test < handle % LAMBDA usage: lambda() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(18, this, varargin{:}); + class_wrapper(19, this, varargin{:}); return end error('Arguments do not match any overload of function Test.lambda'); @@ -113,7 +113,7 @@ classdef Test < handle % PRINT usage: print() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(19, this, varargin{:}); + class_wrapper(20, this, varargin{:}); return end error('Arguments do not match any overload of function Test.print'); @@ -123,7 +123,7 @@ classdef Test < handle % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = class_wrapper(20, this, varargin{:}); + varargout{1} = class_wrapper(21, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Point2Ptr'); @@ -133,7 +133,7 @@ classdef Test < handle % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(21, this, varargin{:}); + varargout{1} = class_wrapper(22, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Test'); @@ -143,7 +143,7 @@ classdef Test < handle % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(22, this, varargin{:}); + varargout{1} = class_wrapper(23, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_TestPtr'); @@ -153,7 +153,7 @@ classdef Test < handle % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = class_wrapper(23, this, varargin{:}); + varargout{1} = class_wrapper(24, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_bool'); @@ -163,7 +163,7 @@ classdef Test < handle % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(24, this, varargin{:}); + varargout{1} = class_wrapper(25, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_double'); @@ -173,7 +173,7 @@ classdef Test < handle % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(25, this, varargin{:}); + varargout{1} = class_wrapper(26, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_field'); @@ -183,7 +183,7 @@ classdef Test < handle % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(26, this, varargin{:}); + varargout{1} = class_wrapper(27, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_int'); @@ -193,7 +193,7 @@ classdef Test < handle % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(27, this, varargin{:}); + varargout{1} = class_wrapper(28, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix1'); @@ -203,7 +203,7 @@ classdef Test < handle % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(28, this, varargin{:}); + varargout{1} = class_wrapper(29, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix2'); @@ -213,13 +213,13 @@ classdef Test < handle % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double') - [ varargout{1} varargout{2} ] = class_wrapper(29, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:}); return end % RETURN_PAIR usage: return_pair(Vector v) : returns pair< Vector, Matrix > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_pair'); @@ -229,7 +229,7 @@ classdef Test < handle % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') - [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(32, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_ptrs'); @@ -239,7 +239,7 @@ classdef Test < handle % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(32, this, varargin{:}); + varargout{1} = class_wrapper(33, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_size_t'); @@ -249,7 +249,7 @@ classdef Test < handle % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'char') - varargout{1} = class_wrapper(33, this, varargin{:}); + varargout{1} = class_wrapper(34, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_string'); @@ -259,7 +259,7 @@ classdef Test < handle % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - varargout{1} = class_wrapper(34, this, varargin{:}); + varargout{1} = class_wrapper(35, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector1'); @@ -269,19 +269,13 @@ classdef Test < handle % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - varargout{1} = class_wrapper(35, this, varargin{:}); + varargout{1} = class_wrapper(36, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector2'); end function varargout = set_container(this, varargin) - % SET_CONTAINER usage: set_container(vector container) : returns void - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') - class_wrapper(36, this, varargin{:}); - return - end % SET_CONTAINER usage: set_container(vector container) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') @@ -294,6 +288,12 @@ classdef Test < handle class_wrapper(38, this, varargin{:}); return end + % SET_CONTAINER usage: set_container(vector container) : returns void + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') + class_wrapper(39, this, varargin{:}); + return + end error('Arguments do not match any overload of function Test.set_container'); end diff --git a/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp index 48d6b2dce..df6cb3307 100644 --- a/tests/expected/matlab/class_wrapper.cpp +++ b/tests/expected/matlab/class_wrapper.cpp @@ -246,7 +246,14 @@ void FunDouble_staticMethodWithThis_9(int nargout, mxArray *out[], int nargin, c out[0] = wrap_shared_ptr(boost::make_shared>(Fun::staticMethodWithThis()),"Fundouble", false); } -void Test_collectorInsertAndMakeBase_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void FunDouble_templatedStaticMethodInt_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("FunDouble.templatedStaticMethodInt",nargout,nargin,1); + int m = unwrap< int >(in[0]); + out[0] = wrap< double >(Fun::templatedStaticMethodInt(m)); +} + +void Test_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -255,7 +262,7 @@ void Test_collectorInsertAndMakeBase_10(int nargout, mxArray *out[], int nargin, collector_Test.insert(self); } -void Test_constructor_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -266,7 +273,7 @@ void Test_constructor_11(int nargout, mxArray *out[], int nargin, const mxArray *reinterpret_cast (mxGetData(out[0])) = self; } -void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -279,7 +286,7 @@ void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *reinterpret_cast (mxGetData(out[0])) = self; } -void Test_deconstructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_Test",nargout,nargin,1); @@ -292,7 +299,7 @@ void Test_deconstructor_13(int nargout, mxArray *out[], int nargin, const mxArra } } -void Test_arg_EigenConstRef_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_arg_EigenConstRef_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("arg_EigenConstRef",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -300,7 +307,7 @@ void Test_arg_EigenConstRef_14(int nargout, mxArray *out[], int nargin, const mx obj->arg_EigenConstRef(value); } -void Test_create_MixedPtrs_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_create_MixedPtrs_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_MixedPtrs",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -309,7 +316,7 @@ void Test_create_MixedPtrs_15(int nargout, mxArray *out[], int nargin, const mxA out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_create_ptrs_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_create_ptrs_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -318,28 +325,28 @@ void Test_create_ptrs_16(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_get_container_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_get_container_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("get_container",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); out[0] = wrap_shared_ptr(boost::make_shared>(obj->get_container()),"std.vectorTest", false); } -void Test_lambda_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_lambda_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("lambda",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->lambda(); } -void Test_print_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_print_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->print(); } -void Test_return_Point2Ptr_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Point2Ptr_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -350,7 +357,7 @@ void Test_return_Point2Ptr_20(int nargout, mxArray *out[], int nargin, const mxA } } -void Test_return_Test_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Test_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Test",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -358,7 +365,7 @@ void Test_return_Test_21(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap_shared_ptr(boost::make_shared(obj->return_Test(value)),"Test", false); } -void Test_return_TestPtr_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_TestPtr_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -366,7 +373,7 @@ void Test_return_TestPtr_22(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); } -void Test_return_bool_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_bool_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -374,7 +381,7 @@ void Test_return_bool_23(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_bool(value)); } -void Test_return_double_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_double_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -382,7 +389,7 @@ void Test_return_double_24(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< double >(obj->return_double(value)); } -void Test_return_field_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_field_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -390,7 +397,7 @@ void Test_return_field_25(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_field(t)); } -void Test_return_int_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_int_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -398,7 +405,7 @@ void Test_return_int_26(int nargout, mxArray *out[], int nargin, const mxArray * out[0] = wrap< int >(obj->return_int(value)); } -void Test_return_matrix1_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix1_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -406,7 +413,7 @@ void Test_return_matrix1_27(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix1(value)); } -void Test_return_matrix2_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix2_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -414,7 +421,7 @@ void Test_return_matrix2_28(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix2(value)); } -void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -425,7 +432,7 @@ void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -435,7 +442,7 @@ void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_ptrs_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_ptrs_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -446,7 +453,7 @@ void Test_return_ptrs_31(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_return_size_t_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_size_t_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -454,7 +461,7 @@ void Test_return_size_t_32(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< size_t >(obj->return_size_t(value)); } -void Test_return_string_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_string_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -462,7 +469,7 @@ void Test_return_string_33(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< string >(obj->return_string(value)); } -void Test_return_vector1_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector1_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -470,7 +477,7 @@ void Test_return_vector1_34(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector1(value)); } -void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector2_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -478,14 +485,6 @@ void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector2(value)); } -void Test_set_container_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("set_container",nargout,nargin-1,1); - auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); - boost::shared_ptr> container = unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorTest"); - obj->set_container(*container); -} - void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("set_container",nargout,nargin-1,1); @@ -502,7 +501,15 @@ void Test_set_container_38(int nargout, mxArray *out[], int nargin, const mxArra obj->set_container(*container); } -void PrimitiveRefDouble_collectorInsertAndMakeBase_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_set_container_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("set_container",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr> container = unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorTest"); + obj->set_container(*container); +} + +void PrimitiveRefDouble_collectorInsertAndMakeBase_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -511,7 +518,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_39(int nargout, mxArray *out[ collector_PrimitiveRefDouble.insert(self); } -void PrimitiveRefDouble_constructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_constructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -522,7 +529,7 @@ void PrimitiveRefDouble_constructor_40(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void PrimitiveRefDouble_deconstructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_deconstructor_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1); @@ -535,14 +542,14 @@ void PrimitiveRefDouble_deconstructor_41(int nargout, mxArray *out[], int nargin } } -void PrimitiveRefDouble_Brutal_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_Brutal_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1); double t = unwrap< double >(in[0]); out[0] = wrap_shared_ptr(boost::make_shared>(PrimitiveRef::Brutal(t)),"PrimitiveRefdouble", false); } -void MyVector3_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -551,7 +558,7 @@ void MyVector3_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int na collector_MyVector3.insert(self); } -void MyVector3_constructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_constructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -562,7 +569,7 @@ void MyVector3_constructor_44(int nargout, mxArray *out[], int nargin, const mxA *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector3_deconstructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_deconstructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector3",nargout,nargin,1); @@ -575,7 +582,7 @@ void MyVector3_deconstructor_45(int nargout, mxArray *out[], int nargin, const m } } -void MyVector12_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_collectorInsertAndMakeBase_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -584,7 +591,7 @@ void MyVector12_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int n collector_MyVector12.insert(self); } -void MyVector12_constructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_constructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -595,7 +602,7 @@ void MyVector12_constructor_47(int nargout, mxArray *out[], int nargin, const mx *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector12_deconstructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector12",nargout,nargin,1); @@ -608,7 +615,7 @@ void MyVector12_deconstructor_48(int nargout, mxArray *out[], int nargin, const } } -void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -617,7 +624,7 @@ void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(int nargout, mxArr collector_MultipleTemplatesIntDouble.insert(self); } -void MultipleTemplatesIntDouble_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_deconstructor_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1); @@ -630,7 +637,7 @@ void MultipleTemplatesIntDouble_deconstructor_50(int nargout, mxArray *out[], in } } -void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -639,7 +646,7 @@ void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(int nargout, mxArra collector_MultipleTemplatesIntFloat.insert(self); } -void MultipleTemplatesIntFloat_deconstructor_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1); @@ -652,7 +659,7 @@ void MultipleTemplatesIntFloat_deconstructor_52(int nargout, mxArray *out[], int } } -void ForwardKinematics_collectorInsertAndMakeBase_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_collectorInsertAndMakeBase_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -661,7 +668,7 @@ void ForwardKinematics_collectorInsertAndMakeBase_53(int nargout, mxArray *out[] collector_ForwardKinematics.insert(self); } -void ForwardKinematics_constructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_constructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -677,7 +684,7 @@ void ForwardKinematics_constructor_54(int nargout, mxArray *out[], int nargin, c *reinterpret_cast (mxGetData(out[0])) = self; } -void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_deconstructor_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_ForwardKinematics",nargout,nargin,1); @@ -690,7 +697,7 @@ void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, } } -void TemplatedConstructor_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_collectorInsertAndMakeBase_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -699,7 +706,7 @@ void TemplatedConstructor_collectorInsertAndMakeBase_56(int nargout, mxArray *ou collector_TemplatedConstructor.insert(self); } -void TemplatedConstructor_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -710,7 +717,7 @@ void TemplatedConstructor_constructor_57(int nargout, mxArray *out[], int nargin *reinterpret_cast (mxGetData(out[0])) = self; } -void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -722,7 +729,7 @@ void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin *reinterpret_cast (mxGetData(out[0])) = self; } -void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -734,7 +741,7 @@ void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin *reinterpret_cast (mxGetData(out[0])) = self; } -void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -746,7 +753,7 @@ void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin *reinterpret_cast (mxGetData(out[0])) = self; } -void TemplatedConstructor_deconstructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_deconstructor_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_TemplatedConstructor",nargout,nargin,1); @@ -759,7 +766,7 @@ void TemplatedConstructor_deconstructor_61(int nargout, mxArray *out[], int narg } } -void MyFactorPosePoint2_collectorInsertAndMakeBase_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_collectorInsertAndMakeBase_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -768,7 +775,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_62(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -783,7 +790,7 @@ void MyFactorPosePoint2_constructor_63(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -796,7 +803,7 @@ void MyFactorPosePoint2_deconstructor_64(int nargout, mxArray *out[], int nargin } } -void MyFactorPosePoint2_print_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -848,85 +855,85 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) FunDouble_staticMethodWithThis_9(nargout, out, nargin-1, in+1); break; case 10: - Test_collectorInsertAndMakeBase_10(nargout, out, nargin-1, in+1); + FunDouble_templatedStaticMethodInt_10(nargout, out, nargin-1, in+1); break; case 11: - Test_constructor_11(nargout, out, nargin-1, in+1); + Test_collectorInsertAndMakeBase_11(nargout, out, nargin-1, in+1); break; case 12: Test_constructor_12(nargout, out, nargin-1, in+1); break; case 13: - Test_deconstructor_13(nargout, out, nargin-1, in+1); + Test_constructor_13(nargout, out, nargin-1, in+1); break; case 14: - Test_arg_EigenConstRef_14(nargout, out, nargin-1, in+1); + Test_deconstructor_14(nargout, out, nargin-1, in+1); break; case 15: - Test_create_MixedPtrs_15(nargout, out, nargin-1, in+1); + Test_arg_EigenConstRef_15(nargout, out, nargin-1, in+1); break; case 16: - Test_create_ptrs_16(nargout, out, nargin-1, in+1); + Test_create_MixedPtrs_16(nargout, out, nargin-1, in+1); break; case 17: - Test_get_container_17(nargout, out, nargin-1, in+1); + Test_create_ptrs_17(nargout, out, nargin-1, in+1); break; case 18: - Test_lambda_18(nargout, out, nargin-1, in+1); + Test_get_container_18(nargout, out, nargin-1, in+1); break; case 19: - Test_print_19(nargout, out, nargin-1, in+1); + Test_lambda_19(nargout, out, nargin-1, in+1); break; case 20: - Test_return_Point2Ptr_20(nargout, out, nargin-1, in+1); + Test_print_20(nargout, out, nargin-1, in+1); break; case 21: - Test_return_Test_21(nargout, out, nargin-1, in+1); + Test_return_Point2Ptr_21(nargout, out, nargin-1, in+1); break; case 22: - Test_return_TestPtr_22(nargout, out, nargin-1, in+1); + Test_return_Test_22(nargout, out, nargin-1, in+1); break; case 23: - Test_return_bool_23(nargout, out, nargin-1, in+1); + Test_return_TestPtr_23(nargout, out, nargin-1, in+1); break; case 24: - Test_return_double_24(nargout, out, nargin-1, in+1); + Test_return_bool_24(nargout, out, nargin-1, in+1); break; case 25: - Test_return_field_25(nargout, out, nargin-1, in+1); + Test_return_double_25(nargout, out, nargin-1, in+1); break; case 26: - Test_return_int_26(nargout, out, nargin-1, in+1); + Test_return_field_26(nargout, out, nargin-1, in+1); break; case 27: - Test_return_matrix1_27(nargout, out, nargin-1, in+1); + Test_return_int_27(nargout, out, nargin-1, in+1); break; case 28: - Test_return_matrix2_28(nargout, out, nargin-1, in+1); + Test_return_matrix1_28(nargout, out, nargin-1, in+1); break; case 29: - Test_return_pair_29(nargout, out, nargin-1, in+1); + Test_return_matrix2_29(nargout, out, nargin-1, in+1); break; case 30: Test_return_pair_30(nargout, out, nargin-1, in+1); break; case 31: - Test_return_ptrs_31(nargout, out, nargin-1, in+1); + Test_return_pair_31(nargout, out, nargin-1, in+1); break; case 32: - Test_return_size_t_32(nargout, out, nargin-1, in+1); + Test_return_ptrs_32(nargout, out, nargin-1, in+1); break; case 33: - Test_return_string_33(nargout, out, nargin-1, in+1); + Test_return_size_t_33(nargout, out, nargin-1, in+1); break; case 34: - Test_return_vector1_34(nargout, out, nargin-1, in+1); + Test_return_string_34(nargout, out, nargin-1, in+1); break; case 35: - Test_return_vector2_35(nargout, out, nargin-1, in+1); + Test_return_vector1_35(nargout, out, nargin-1, in+1); break; case 36: - Test_set_container_36(nargout, out, nargin-1, in+1); + Test_return_vector2_36(nargout, out, nargin-1, in+1); break; case 37: Test_set_container_37(nargout, out, nargin-1, in+1); @@ -935,61 +942,61 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) Test_set_container_38(nargout, out, nargin-1, in+1); break; case 39: - PrimitiveRefDouble_collectorInsertAndMakeBase_39(nargout, out, nargin-1, in+1); + Test_set_container_39(nargout, out, nargin-1, in+1); break; case 40: - PrimitiveRefDouble_constructor_40(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_collectorInsertAndMakeBase_40(nargout, out, nargin-1, in+1); break; case 41: - PrimitiveRefDouble_deconstructor_41(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_constructor_41(nargout, out, nargin-1, in+1); break; case 42: - PrimitiveRefDouble_Brutal_42(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_deconstructor_42(nargout, out, nargin-1, in+1); break; case 43: - MyVector3_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_Brutal_43(nargout, out, nargin-1, in+1); break; case 44: - MyVector3_constructor_44(nargout, out, nargin-1, in+1); + MyVector3_collectorInsertAndMakeBase_44(nargout, out, nargin-1, in+1); break; case 45: - MyVector3_deconstructor_45(nargout, out, nargin-1, in+1); + MyVector3_constructor_45(nargout, out, nargin-1, in+1); break; case 46: - MyVector12_collectorInsertAndMakeBase_46(nargout, out, nargin-1, in+1); + MyVector3_deconstructor_46(nargout, out, nargin-1, in+1); break; case 47: - MyVector12_constructor_47(nargout, out, nargin-1, in+1); + MyVector12_collectorInsertAndMakeBase_47(nargout, out, nargin-1, in+1); break; case 48: - MyVector12_deconstructor_48(nargout, out, nargin-1, in+1); + MyVector12_constructor_48(nargout, out, nargin-1, in+1); break; case 49: - MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(nargout, out, nargin-1, in+1); + MyVector12_deconstructor_49(nargout, out, nargin-1, in+1); break; case 50: - MultipleTemplatesIntDouble_deconstructor_50(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1); break; case 51: - MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_deconstructor_51(nargout, out, nargin-1, in+1); break; case 52: - MultipleTemplatesIntFloat_deconstructor_52(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1); break; case 53: - ForwardKinematics_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_deconstructor_53(nargout, out, nargin-1, in+1); break; case 54: - ForwardKinematics_constructor_54(nargout, out, nargin-1, in+1); + ForwardKinematics_collectorInsertAndMakeBase_54(nargout, out, nargin-1, in+1); break; case 55: - ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1); + ForwardKinematics_constructor_55(nargout, out, nargin-1, in+1); break; case 56: - TemplatedConstructor_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + ForwardKinematics_deconstructor_56(nargout, out, nargin-1, in+1); break; case 57: - TemplatedConstructor_constructor_57(nargout, out, nargin-1, in+1); + TemplatedConstructor_collectorInsertAndMakeBase_57(nargout, out, nargin-1, in+1); break; case 58: TemplatedConstructor_constructor_58(nargout, out, nargin-1, in+1); @@ -1001,19 +1008,22 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) TemplatedConstructor_constructor_60(nargout, out, nargin-1, in+1); break; case 61: - TemplatedConstructor_deconstructor_61(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_61(nargout, out, nargin-1, in+1); break; case 62: - MyFactorPosePoint2_collectorInsertAndMakeBase_62(nargout, out, nargin-1, in+1); + TemplatedConstructor_deconstructor_62(nargout, out, nargin-1, in+1); break; case 63: - MyFactorPosePoint2_constructor_63(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_collectorInsertAndMakeBase_63(nargout, out, nargin-1, in+1); break; case 64: - MyFactorPosePoint2_deconstructor_64(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_constructor_64(nargout, out, nargin-1, in+1); break; case 65: - MyFactorPosePoint2_print_65(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_deconstructor_65(nargout, out, nargin-1, in+1); + break; + case 66: + MyFactorPosePoint2_print_66(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/tests/expected/python/class_pybind.cpp b/tests/expected/python/class_pybind.cpp index f36c0a84c..a54d9135b 100644 --- a/tests/expected/python/class_pybind.cpp +++ b/tests/expected/python/class_pybind.cpp @@ -31,7 +31,8 @@ PYBIND11_MODULE(class_py, m_) { py::class_, std::shared_ptr>>(m_, "FunDouble") .def("templatedMethodString",[](Fun* self, double d, string t){return self->templatedMethod(d, t);}, py::arg("d"), py::arg("t")) .def("multiTemplatedMethodStringSize_t",[](Fun* self, double d, string t, size_t u){return self->multiTemplatedMethod(d, t, u);}, py::arg("d"), py::arg("t"), py::arg("u")) - .def_static("staticMethodWithThis",[](){return Fun::staticMethodWithThis();}); + .def_static("staticMethodWithThis",[](){return Fun::staticMethodWithThis();}) + .def_static("templatedStaticMethodInt",[](const int& m){return Fun::templatedStaticMethod(m);}, py::arg("m")); py::class_>(m_, "Test") .def(py::init<>()) diff --git a/tests/fixtures/class.i b/tests/fixtures/class.i index 9923ffce7..40a565506 100644 --- a/tests/fixtures/class.i +++ b/tests/fixtures/class.i @@ -10,6 +10,9 @@ class Fun { static This staticMethodWithThis(); + template + static double templatedStaticMethod(const T& m); + template This templatedMethod(double d, T t); diff --git a/tests/test_interface_parser.py b/tests/test_interface_parser.py index 25e4178a6..49165328c 100644 --- a/tests/test_interface_parser.py +++ b/tests/test_interface_parser.py @@ -18,11 +18,13 @@ import unittest sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from gtwrap.interface_parser import ( - ArgumentList, Class, Constructor, Enum, Enumerator, ForwardDeclaration, - GlobalFunction, Include, Method, Module, Namespace, Operator, ReturnType, - StaticMethod, TemplatedType, Type, TypedefTemplateInstantiation, Typename, - Variable) +from gtwrap.interface_parser import (ArgumentList, Class, Constructor, Enum, + Enumerator, ForwardDeclaration, + GlobalFunction, Include, Method, Module, + Namespace, Operator, ReturnType, + StaticMethod, TemplatedType, Type, + TypedefTemplateInstantiation, Typename, + Variable) class TestInterfaceParser(unittest.TestCase): @@ -266,6 +268,11 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual("char", return_type.type1.typename.name) self.assertEqual("int", return_type.type2.typename.name) + return_type = ReturnType.rule.parseString("pair")[0] + self.assertEqual("Test", return_type.type1.typename.name) + self.assertEqual("Test", return_type.type2.typename.name) + self.assertTrue(return_type.type2.is_shared_ptr) + def test_method(self): """Test for a class method.""" ret = Method.rule.parseString("int f();")[0] @@ -283,6 +290,13 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual("f", ret.name) self.assertEqual(3, len(ret.args)) + ret = Method.rule.parseString( + "pair create_MixedPtrs();")[0] + self.assertEqual("create_MixedPtrs", ret.name) + self.assertEqual(0, len(ret.args)) + self.assertEqual("First", ret.return_type.type1.typename.name) + self.assertEqual("Second", ret.return_type.type2.typename.name) + def test_static_method(self): """Test for static methods.""" ret = StaticMethod.rule.parseString("static int f();")[0] diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index 89e53ab21..34940d62e 100644 --- a/tests/test_matlab_wrapper.py +++ b/tests/test_matlab_wrapper.py @@ -42,17 +42,16 @@ class TestWrap(unittest.TestCase): # Create the `actual/matlab` directory os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True) - def compare_and_diff(self, file): + def compare_and_diff(self, file, actual): """ Compute the comparison between the expected and actual file, and assert if diff is zero. """ - output = osp.join(self.MATLAB_ACTUAL_DIR, file) expected = osp.join(self.MATLAB_TEST_DIR, file) - success = filecmp.cmp(output, expected) + success = filecmp.cmp(actual, expected) + if not success: - print("Differ in file: {}".format(file)) - os.system("diff {} {}".format(output, expected)) + os.system("diff {} {}".format(actual, expected)) self.assertTrue(success, "Mismatch for file {0}".format(file)) def test_geometry(self): @@ -77,7 +76,8 @@ class TestWrap(unittest.TestCase): self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam'))) for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_functions(self): """Test interface file with function info.""" @@ -99,7 +99,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_class(self): """Test interface file with only class info.""" @@ -121,7 +122,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_templates(self): """Test interface file with template info.""" @@ -138,7 +140,8 @@ class TestWrap(unittest.TestCase): files = ['template_wrapper.cpp'] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_inheritance(self): """Test interface file with class inheritance definitions.""" @@ -157,7 +160,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_namespaces(self): """ @@ -181,7 +185,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_special_cases(self): """ @@ -203,7 +208,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_multiple_files(self): """ @@ -228,7 +234,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) if __name__ == '__main__': diff --git a/tests/test_template_instantiator.py b/tests/test_template_instantiator.py new file mode 100644 index 000000000..4faf01aa9 --- /dev/null +++ b/tests/test_template_instantiator.py @@ -0,0 +1,606 @@ +""" +GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Tests for template_instantiator. + +Author: Varun Agrawal +""" + +# pylint: disable=import-error,wrong-import-position + +import os +import sys +import unittest + +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from gtwrap.interface_parser import (Argument, ArgumentList, Class, + Constructor, ForwardDeclaration, + GlobalFunction, Include, Method, + Namespace, ReturnType, StaticMethod, + Typename) +from gtwrap.template_instantiator import ( + InstantiatedClass, InstantiatedConstructor, InstantiatedDeclaration, + InstantiatedGlobalFunction, InstantiatedMethod, InstantiatedStaticMethod, + InstantiationHelper, instantiate_args_list, instantiate_name, + instantiate_namespace, instantiate_return_type, instantiate_type, + is_scoped_template) + + +class TestInstantiationHelper(unittest.TestCase): + """Tests for the InstantiationHelper class.""" + def test_constructor(self): + """Test constructor.""" + helper = InstantiationHelper(InstantiatedClass) + self.assertEqual(helper.instantiation_type, InstantiatedClass) + helper = InstantiationHelper(InstantiatedConstructor) + self.assertEqual(helper.instantiation_type, InstantiatedConstructor) + helper = InstantiationHelper(InstantiatedDeclaration) + self.assertEqual(helper.instantiation_type, InstantiatedDeclaration) + helper = InstantiationHelper(InstantiatedGlobalFunction) + self.assertEqual(helper.instantiation_type, InstantiatedGlobalFunction) + helper = InstantiationHelper(InstantiatedMethod) + self.assertEqual(helper.instantiation_type, InstantiatedMethod) + helper = InstantiationHelper(InstantiatedStaticMethod) + self.assertEqual(helper.instantiation_type, InstantiatedStaticMethod) + + def test_instantiate(self): + """Test instantiate method.""" + method = Method.rule.parseString(""" + template + double method(const T x, const U& param); + """)[0] + cls = Class.rule.parseString(""" + template + class Foo {}; + """)[0] + typenames = ['T', 'U'] + class_instantiations = [Typename.rule.parseString("string")[0]] + method_instantiations = [Typename.rule.parseString("double")[0]] + + parent = InstantiatedClass(cls, class_instantiations) + + helper = InstantiationHelper(InstantiatedMethod) + instantiated_methods = helper.instantiate([], method, typenames, + class_instantiations, + method_instantiations, + parent) + + self.assertEqual(len(instantiated_methods), 1) + args_list = instantiated_methods[0].args.list() + self.assertEqual(args_list[0].ctype.get_typename(), 'string') + self.assertEqual(args_list[1].ctype.get_typename(), 'double') + + def test_multilevel_instantiation(self): + """ + Test method for multilevel instantiation + i.e. instantiation at both the class and method level. + """ + cls = Class.rule.parseString(""" + template + class Foo { + template + double method1(const T x, const U& param); + + template + V method2(const T x); + }; + """)[0] + + typenames = ['T'] + class_instantiations = [Typename.rule.parseString("string")[0]] + parent = InstantiatedClass(cls, class_instantiations) + + helper = InstantiationHelper(InstantiatedMethod) + + instantiated_methods = helper.multilevel_instantiation( + cls.methods, typenames, parent) + self.assertEqual(len(instantiated_methods), 2) + self.assertEqual( + instantiated_methods[0].args.list()[0].ctype.get_typename(), + 'string') + self.assertEqual( + instantiated_methods[0].args.list()[1].ctype.get_typename(), + 'double') + self.assertEqual( + instantiated_methods[1].args.list()[0].ctype.get_typename(), + 'string') + self.assertEqual( + instantiated_methods[1].return_type.type1.get_typename(), 'int') + + +class TestInstantiatedGlobalFunction(unittest.TestCase): + """Tests for the InstantiatedGlobalFunction class.""" + def setUp(self): + original = GlobalFunction.rule.parseString(""" + template + R function(const T& x); + """)[0] + instantiations = [ + Typename.rule.parseString("int")[0], + Typename.rule.parseString("double")[0] + ] + self.func = InstantiatedGlobalFunction(original, instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.func, InstantiatedGlobalFunction) + self.assertIsInstance(self.func.original, GlobalFunction) + self.assertEqual(self.func.name, "functionIntDouble") + self.assertEqual(len(self.func.args.list()), 1) + self.assertEqual(self.func.args.list()[0].ctype.get_typename(), "int") + self.assertEqual(self.func.return_type.type1.get_typename(), "double") + + def test_to_cpp(self): + """Test to_cpp method.""" + actual = self.func.to_cpp() + self.assertEqual(actual, "function") + + +class TestInstantiatedConstructor(unittest.TestCase): + """Tests for the InstantiatedConstructor class.""" + def setUp(self): + constructor = Constructor.rule.parseString(""" + template + Class(C x, const U& param); + """)[0] + instantiations = [ + Typename.rule.parseString("double")[0], + Typename.rule.parseString("string")[0] + ] + self.constructor = InstantiatedConstructor(constructor, instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.constructor, InstantiatedConstructor) + self.assertIsInstance(self.constructor.original, Constructor) + + def test_construct(self): + """Test the construct classmethod.""" + constructor = Constructor.rule.parseString(""" + template + Class(C x, const U& param); + """)[0] + c = Class.rule.parseString(""" + template + class Class {}; + """)[0] + class_instantiations = [Typename.rule.parseString("double")[0]] + method_instantiations = [Typename.rule.parseString("string")[0]] + typenames = ['C', 'U'] + parent = InstantiatedClass(c, class_instantiations) + instantiated_args = instantiate_args_list( + constructor.args.list(), + typenames, class_instantiations + method_instantiations, + parent.cpp_typename()) + + instantiated_constructor = InstantiatedConstructor.construct( + constructor, typenames, class_instantiations, + method_instantiations, instantiated_args, parent) + self.assertEqual(instantiated_constructor.name, "ClassDouble") + self.assertEqual( + instantiated_constructor.args.list()[0].ctype.get_typename(), + "double") + self.assertEqual( + instantiated_constructor.args.list()[1].ctype.get_typename(), + "string") + + def test_to_cpp(self): + """Test the to_cpp method.""" + actual = self.constructor.to_cpp() + self.assertEqual(actual, "Class") + + +class TestInstantiatedMethod(unittest.TestCase): + """Tests for the InstantiatedMethod class.""" + def setUp(self): + method = Method.rule.parseString(""" + template + double method(const U& param); + """)[0] + instantiations = [Typename.rule.parseString("double")[0]] + self.method = InstantiatedMethod(method, instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.method, InstantiatedMethod) + self.assertIsInstance(self.method.original, Method) + self.assertEqual(self.method.name, "methodDouble") + + def test_construct(self): + """Test the construct classmethod.""" + method = Method.rule.parseString(""" + template + T method(U& param); + """)[0] + method_instantiations = [Typename.rule.parseString("double")[0]] + c = Class.rule.parseString(""" + template + class Class {}; + """)[0] + class_instantiations = [Typename.rule.parseString("string")[0]] + + typenames = ['T', 'U'] + parent = InstantiatedClass(c, class_instantiations) + instantiated_args = instantiate_args_list( + method.args.list(), + typenames, class_instantiations + method_instantiations, + parent.cpp_typename()) + + instantiated_method = InstantiatedMethod.construct( + method, typenames, class_instantiations, method_instantiations, + instantiated_args, parent) + self.assertEqual(instantiated_method.name, "methodDouble") + self.assertEqual( + instantiated_method.args.list()[0].ctype.get_typename(), "double") + self.assertEqual(instantiated_method.return_type.type1.get_typename(), + "string") + + def test_to_cpp(self): + """Test the to_cpp method.""" + actual = self.method.to_cpp() + self.assertEqual(actual, "method") + + +class TestInstantiatedStaticMethod(unittest.TestCase): + """Tests for the InstantiatedStaticMethod class.""" + def setUp(self): + static_method = StaticMethod.rule.parseString(""" + template + static T staticMethod(const U& param); + """)[0] + instantiations = [Typename.rule.parseString("double")[0]] + self.static_method = InstantiatedStaticMethod(static_method, + instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.static_method, InstantiatedStaticMethod) + self.assertIsInstance(self.static_method.original, StaticMethod) + self.assertEqual(self.static_method.name, "staticMethodDouble") + + def test_construct(self): + """Test the construct classmethod.""" + static_method = StaticMethod.rule.parseString(""" + template + static T staticMethod(U& param); + """)[0] + method_instantiations = [Typename.rule.parseString("double")[0]] + c = Class.rule.parseString(""" + template + class Class {}; + """)[0] + class_instantiations = [Typename.rule.parseString("string")[0]] + + typenames = ['T', 'U'] + parent = InstantiatedClass(c, class_instantiations) + instantiated_args = instantiate_args_list( + static_method.args.list(), + typenames, class_instantiations + method_instantiations, + parent.cpp_typename()) + + instantiated_static_method = InstantiatedStaticMethod.construct( + static_method, typenames, class_instantiations, + method_instantiations, instantiated_args, parent) + self.assertEqual(instantiated_static_method.name, "staticMethodDouble") + self.assertEqual( + instantiated_static_method.args.list()[0].ctype.get_typename(), + "double") + self.assertEqual( + instantiated_static_method.return_type.type1.get_typename(), + "string") + + def test_to_cpp(self): + """Test the to_cpp method.""" + actual = self.static_method.to_cpp() + self.assertEqual(actual, "staticMethod") + + +class TestInstantiatedClass(unittest.TestCase): + """Tests for the InstantiatedClass class.""" + def setUp(self): + cl = Class.rule.parseString(""" + template + class Foo { + template + Foo(C& c); + + template + static T staticMethod(const S& s); + + template + T method(const M& m); + + T operator*(T other) const; + + T prop; + }; + """)[0] + class_instantiations = [Typename.rule.parseString('string')[0]] + self.member_instantiations = [ + Typename.rule.parseString('int')[0], + Typename.rule.parseString('char')[0], + Typename.rule.parseString('double')[0], + ] + self.cl = InstantiatedClass(cl, class_instantiations) + self.typenames = self.cl.original.template.typenames + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.cl, InstantiatedClass) + self.assertIsInstance(self.cl.original, Class) + self.assertEqual(self.cl.name, "FooString") + + def test_instantiate_ctors(self): + """Test instantiate_ctors method.""" + ctors = self.cl.instantiate_ctors(self.typenames) + self.assertEqual(len(ctors), 1) + self.assertEqual(ctors[0].name, "FooString") + self.assertEqual(ctors[0].args.list()[0].ctype.get_typename(), "int") + + def test_instantiate_static_methods(self): + """Test instantiate_static_methods method.""" + static_methods = self.cl.instantiate_static_methods(self.typenames) + self.assertEqual(len(static_methods), 1) + self.assertEqual(static_methods[0].name, "staticMethodChar") + self.assertEqual(static_methods[0].args.list()[0].ctype.get_typename(), + "char") + self.assertEqual(static_methods[0].return_type.type1.get_typename(), + "string") + + def test_instantiate_methods(self): + """Test instantiate_methods method.""" + methods = self.cl.instantiate_methods(self.typenames) + self.assertEqual(len(methods), 1) + self.assertEqual(methods[0].name, "methodDouble") + self.assertEqual(methods[0].args.list()[0].ctype.get_typename(), + "double") + self.assertEqual(methods[0].return_type.type1.get_typename(), "string") + + def test_instantiate_operators(self): + """Test instantiate_operators method.""" + operators = self.cl.instantiate_operators(self.typenames) + self.assertEqual(len(operators), 1) + self.assertEqual(operators[0].operator, "*") + self.assertEqual(operators[0].args.list()[0].ctype.get_typename(), + "string") + self.assertEqual(operators[0].return_type.type1.get_typename(), + "string") + + def test_instantiate_properties(self): + """Test instantiate_properties method.""" + properties = self.cl.instantiate_properties(self.typenames) + self.assertEqual(len(properties), 1) + self.assertEqual(properties[0].name, "prop") + self.assertEqual(properties[0].ctype.get_typename(), "string") + + def test_cpp_typename(self): + """Test cpp_typename method.""" + actual = self.cl.cpp_typename() + self.assertEqual(actual.name, "Foo") + + def test_to_cpp(self): + """Test to_cpp method.""" + actual = self.cl.to_cpp() + self.assertEqual(actual, "Foo") + + +class TestInstantiatedDeclaration(unittest.TestCase): + """Tests for the InstantiatedDeclaration class.""" + def setUp(self): + #TODO(Varun) Need to support templated class forward declaration. + forward_declaration = ForwardDeclaration.rule.parseString(""" + class FooBar; + """)[0] + instantiations = [Typename.rule.parseString("double")[0]] + self.declaration = InstantiatedDeclaration( + forward_declaration, instantiations=instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.declaration, InstantiatedDeclaration) + self.assertIsInstance(self.declaration.original, ForwardDeclaration) + self.assertEqual(self.declaration.instantiations[0].name, "double") + + def test_to_cpp(self): + """Test to_cpp method.""" + cpp = self.declaration.to_cpp() + self.assertEqual(cpp, "FooBar") + + +class TestTemplateInstantiator(unittest.TestCase): + """ + Test overall template instantiation and the functions in the module. + """ + def test_scoped_template(self): + """Test is_scoped_template.""" + # Test if not scoped template. + template_typenames = ['T'] + str_arg_typename = "double" + scoped_template, scoped_idx = is_scoped_template( + template_typenames, str_arg_typename) + self.assertFalse(scoped_template) + self.assertEqual(scoped_idx, -1) + + # Check for correct template match. + template_typenames = ['T'] + str_arg_typename = "gtsam::Matrix" + scoped_template, scoped_idx = is_scoped_template( + template_typenames, str_arg_typename) + self.assertFalse(scoped_template) + self.assertEqual(scoped_idx, -1) + + # Test scoped templatte + template_typenames = ['T'] + str_arg_typename = "T::Value" + scoped_template, scoped_idx = is_scoped_template( + template_typenames, str_arg_typename) + self.assertEqual(scoped_template, "T") + self.assertEqual(scoped_idx, 0) + + template_typenames = ['U', 'T'] + str_arg_typename = "T::Value" + scoped_template, scoped_idx = is_scoped_template( + template_typenames, str_arg_typename) + self.assertEqual(scoped_template, "T") + self.assertEqual(scoped_idx, 1) + + def test_instantiate_type(self): + """Test for instantiate_type.""" + arg = Argument.rule.parseString("const T x")[0] + template_typenames = ["T"] + instantiations = [Typename.rule.parseString("double")[0]] + cpp_typename = "ExampleClass" + new_type = instantiate_type(arg.ctype, + template_typenames, + instantiations=instantiations, + cpp_typename=cpp_typename, + instantiated_class=None) + + new_typename = new_type.typename + self.assertEqual(new_typename.name, "double") + self.assertEqual(new_typename.instantiated_name(), "double") + + def test_instantiate_args_list(self): + """Test for instantiate_args_list.""" + args = ArgumentList.rule.parseString("T x, double y, string z")[0] + args_list = args.list() + template_typenames = ['T'] + instantiations = [Typename.rule.parseString("double")[0]] + instantiated_args_list = instantiate_args_list( + args_list, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + + self.assertEqual(instantiated_args_list[0].ctype.get_typename(), + "double") + + args = ArgumentList.rule.parseString("T x, U y, string z")[0] + args_list = args.list() + template_typenames = ['T', 'U'] + instantiations = [ + Typename.rule.parseString("double")[0], + Typename.rule.parseString("Matrix")[0] + ] + instantiated_args_list = instantiate_args_list( + args_list, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + self.assertEqual(instantiated_args_list[0].ctype.get_typename(), + "double") + self.assertEqual(instantiated_args_list[1].ctype.get_typename(), + "Matrix") + + args = ArgumentList.rule.parseString("T x, U y, T z")[0] + args_list = args.list() + template_typenames = ['T', 'U'] + instantiations = [ + Typename.rule.parseString("double")[0], + Typename.rule.parseString("Matrix")[0] + ] + instantiated_args_list = instantiate_args_list( + args_list, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + self.assertEqual(instantiated_args_list[0].ctype.get_typename(), + "double") + self.assertEqual(instantiated_args_list[1].ctype.get_typename(), + "Matrix") + self.assertEqual(instantiated_args_list[2].ctype.get_typename(), + "double") + + def test_instantiate_return_type(self): + """Test for instantiate_return_type.""" + return_type = ReturnType.rule.parseString("T")[0] + template_typenames = ['T'] + instantiations = [Typename.rule.parseString("double")[0]] + instantiated_return_type = instantiate_return_type( + return_type, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + + self.assertEqual(instantiated_return_type.type1.get_typename(), + "double") + + return_type = ReturnType.rule.parseString("pair")[0] + template_typenames = ['T', 'U'] + instantiations = [ + Typename.rule.parseString("double")[0], + Typename.rule.parseString("char")[0], + ] + instantiated_return_type = instantiate_return_type( + return_type, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + + self.assertEqual(instantiated_return_type.type1.get_typename(), + "double") + self.assertEqual(instantiated_return_type.type2.get_typename(), "char") + + def test_instantiate_name(self): + """Test for instantiate_name.""" + instantiations = [Typename.rule.parseString("Man")[0]] + instantiated_name = instantiate_name("Iron", instantiations) + self.assertEqual(instantiated_name, "IronMan") + + def test_instantiate_namespace(self): + """Test for instantiate_namespace.""" + namespace = Namespace.rule.parseString(""" + namespace gtsam { + #include + template + class Values { + Values(const T& other); + + template + void insert(size_t j, V x); + + template + static S staticMethod(const S& s); + }; + } + """)[0] + instantiated_namespace = instantiate_namespace(namespace) + + self.assertEqual(instantiated_namespace.name, "gtsam") + self.assertEqual(instantiated_namespace.parent, "") + + self.assertEqual(instantiated_namespace.content[0].header, + "gtsam/nonlinear/Values.h") + self.assertIsInstance(instantiated_namespace.content[0], Include) + + self.assertEqual(instantiated_namespace.content[1].name, "ValuesBasis") + self.assertIsInstance(instantiated_namespace.content[1], Class) + + self.assertIsInstance(instantiated_namespace.content[1].ctors[0], + Constructor) + self.assertEqual(instantiated_namespace.content[1].ctors[0].name, + "ValuesBasis") + + self.assertIsInstance(instantiated_namespace.content[1].methods[0], + Method) + self.assertIsInstance(instantiated_namespace.content[1].methods[1], + Method) + self.assertEqual(instantiated_namespace.content[1].methods[0].name, + "insertVector") + self.assertEqual(instantiated_namespace.content[1].methods[1].name, + "insertMatrix") + + self.assertIsInstance( + instantiated_namespace.content[1].static_methods[0], StaticMethod) + self.assertEqual( + instantiated_namespace.content[1].static_methods[0].name, + "staticMethodDouble") + + +if __name__ == '__main__': + unittest.main() From d852c7e79bb9dd5781899db802ce6bd7ae2feb06 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Oct 2021 13:01:58 -0400 Subject: [PATCH 143/169] update CMake --- python/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2b3ed3852..b703f5900 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -92,7 +92,7 @@ set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) # Symlink all tests .py files to build folder. -create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" +copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" "${GTSAM_MODULE_PATH}") # Common directory for data/datasets stored with the package. @@ -148,7 +148,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable) # Symlink all tests .py files to build folder. - create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" + copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" "${GTSAM_UNSTABLE_MODULE_PATH}") # Add gtsam_unstable to the install target From 27143fc724876f55514d745ab45dc94fe8c03e17 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Oct 2021 17:15:18 -0400 Subject: [PATCH 144/169] Add tests for Lie::interpolate jacobians --- tests/testLie.cpp | 40 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/tests/testLie.cpp b/tests/testLie.cpp index 0ef12198b..fe1173f22 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -129,6 +129,46 @@ TEST( testProduct, Logmap ) { EXPECT(assert_equal(numericH, actH, tol)); } +/* ************************************************************************* */ +Product interpolate_proxy(const Product& x, const Product& y, double t) { + return interpolate(x, y, t); +} + +TEST(Lie, Interpolate) { + Product x(Point2(1, 2), Pose2(3, 4, 5)); + Product y(Point2(6, 7), Pose2(8, 9, 0)); + + double t; + Matrix actH1, numericH1, actH2, numericH2; + + t = 0.0; + interpolate(x, y, t, actH1, actH2); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, tol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, tol)); + + t = 0.5; + interpolate(x, y, t, actH1, actH2); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, tol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, tol)); + + t = 1.0; + interpolate(x, y, t, actH1, actH2); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, tol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + //****************************************************************************** int main() { TestResult tr; From f0fc0d445718df993d904e4f1eaba5c8d567e91c Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 26 Oct 2021 03:18:04 -0400 Subject: [PATCH 145/169] much cleaner Adjoint jacobian --- doc/math.lyx | 253 ++++++++++++++++++++------------------- doc/math.pdf | Bin 264527 -> 272118 bytes gtsam/geometry/Pose3.cpp | 83 +++---------- 3 files changed, 147 insertions(+), 189 deletions(-) diff --git a/doc/math.lyx b/doc/math.lyx index f14ebc66f..6d7a5e318 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5090,29 +5090,22 @@ Derivative of Adjoint \begin_layout Standard Consider -\begin_inset Formula $f:SE(3)\rightarrow\mathbb{R}^{6}$ +\begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$ \end_inset is defined as -\begin_inset Formula $f(T)=Ad_{T}y$ -\end_inset - - for some constant -\begin_inset Formula $y=\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}$ +\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$ \end_inset . - Defining -\begin_inset Formula $\xi=\begin{bmatrix}\omega\\ -v -\end{bmatrix}$ -\end_inset + The derivative is notated (see +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Derivatives-of-Actions" +plural "false" +caps "false" +noprefix "false" - for the derivative notation (w.r.t. - pose -\begin_inset Formula $T$ \end_inset ): @@ -5121,68 +5114,17 @@ v \begin_layout Standard \begin_inset Formula \[ -f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix} +Df_{(T,y)}(\xi,\delta y)=D_{1}f_{(T,y)}(\xi)+D_{2}f_{(T,y)}(\delta y) \] \end_inset -Then we can compute -\begin_inset Formula $f'(T)$ +First, computing +\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$ \end_inset - by considering the rotation and translation separately. - To reduce confusion with -\begin_inset Formula $\omega_{y},v_{y}$ -\end_inset - -, we will use -\begin_inset Formula $R,t$ -\end_inset - - to denote the rotation and translation of -\begin_inset Formula $T\exp\xi$ -\end_inset - -. -\end_layout - -\begin_layout Standard -\begin_inset Formula -\[ -\frac{\partial}{\partial\omega}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}=\frac{\partial}{\partial\omega}\begin{bmatrix}R\omega_{y}\\{} -[t]_{\times}R\omega_{y}+Rv_{y} -\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times}\\ --[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} -\end{bmatrix} -\] - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Formula -\[ -\frac{\partial}{\partial t}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega\\ -v -\end{bmatrix}=\frac{\partial}{\partial t}\begin{bmatrix}R\omega_{y}\\{} -[t]_{\times}R\omega_{y}+Rv_{y} -\end{bmatrix}=\begin{bmatrix}0\\ --[R\omega_{y}] -\end{bmatrix} -\] - -\end_inset - -Applying chain rule for the translation, -\begin_inset Formula $\frac{\partial}{\partial v}=\frac{\partial}{\partial t}\frac{\partial t}{\partial v}$ + is easy, as its matrix is simply +\begin_inset Formula $Ad_{T}$ \end_inset : @@ -5191,76 +5133,139 @@ Applying chain rule for the translation, \begin_layout Standard \begin_inset Formula \[ -\frac{\partial}{\partial v}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}=\begin{bmatrix}0\\ --[R\omega_{y}]_{\times} -\end{bmatrix}R=\begin{bmatrix}0\\ --[R\omega_{y}]_{\times}R -\end{bmatrix}=\begin{bmatrix}0\\ --R[\omega_{y}]_{\times} -\end{bmatrix} +f(T,\xi_{b}+\delta\xi_{b})=Ad_{T}(\widehat{\xi_{b}+\delta\xi_{b}})=Ad_{T}(\hat{\xi}_{b})+Ad_{T}(\delta\hat{\xi}_{b}) \] \end_inset -The simplification -\family roman -\series medium -\shape up -\size normal -\emph off -\bar no -\strikeout off -\xout off -\uuline off -\uwave off -\noun off -\color none -\begin_inset Formula $[R\omega_{y}]_{\times}R=R[\omega_{y}]_{\times}$ -\end_inset - - can be derived by considering the result for when -\begin_inset Formula $\omega_{y}$ -\end_inset - - is each of the 3 standard basis vectors -\begin_inset Formula $\hat{e}_{i}$ -\end_inset - -: -\begin_inset Formula $-[R\hat{e}_{i}]_{\times}R$ -\end_inset - -. -\end_layout - -\begin_layout Standard -Now putting together the rotation and translation: \end_layout \begin_layout Standard \begin_inset Formula \[ -f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times} & 0\\ --[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} & -R[\omega_{y}]_{\times} -\end{bmatrix} +D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T} \] \end_inset +To compute +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ +\end_inset -\end_layout - -\begin_layout Standard -We can apply a similar procedure to compute the derivative of -\begin_inset Formula $Ad_{T}^{T}y$ +, we'll first define +\begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$ \end_inset . + From Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Derivatives-of-Actions" +plural "false" +caps "false" +noprefix "false" + +\end_inset + +, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +D_{2}g_{(T,\xi)}(\xi) & =T\hat{\xi}\\ +D_{2}g_{(T,\xi)}^{-1}(\xi) & =-\hat{\xi}T^{-1} +\end{align*} + +\end_inset + +Now we can use the definition of the Adjoint representation +\begin_inset Formula $Ad_{g}\hat{\xi}=g\hat{\xi}g^{-1}$ +\end_inset + + (aka conjugation by +\begin_inset Formula $g$ +\end_inset + +) then apply product rule and simplify: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}g^{-1}\right)(\xi)\\ + & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}g^{-1}(T,0)+g(T,0)\hat{\xi}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ + & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\ + & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\ + & =-Ad_{T}(ad_{\xi_{b}}\hat{\xi})\\ +D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}}) +\end{align*} + +\end_inset + +An alternative, perhaps more intuitive way of deriving this would be to + use the fact that the derivative at the origin +\begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$ +\end_inset + + by definition of the adjoint +\begin_inset Formula $ad_{\xi}$ +\end_inset + +. + Then applying the property +\begin_inset Formula $Ad_{AB}=Ad_{A}Ad_{B}$ +\end_inset + +, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}_{b}}(\xi)\right) +\] + +\end_inset + +It's difficult to apply a similar procedure to compute the derivative of + +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + (note the +\begin_inset Formula $^{*}$ +\end_inset + + denoting that we are now in the dual space) because +\begin_inset Formula $Ad_{T}^{T}$ +\end_inset + + cannot be naturally defined as a conjugation so we resort to crunching + through the algebra. + The details are omitted but the result is a form vaguely resembling (but + not quite) the +\begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +\begin{bmatrix}\omega_{T}\\ +v_{T} +\end{bmatrix}^{*} & \triangleq Ad_{T}^{T}\hat{\xi}_{b}^{*}\\ +D_{1}Ad_{T}^{T}\hat{\xi}_{b}^{*}(\xi) & =\begin{bmatrix}\hat{\omega}_{T} & \hat{v}_{T}\\ +\hat{v}_{T} & 0 +\end{bmatrix} +\end{align*} + +\end_inset + + \end_layout \begin_layout Subsection diff --git a/doc/math.pdf b/doc/math.pdf index 06dec078d5fb590fe6dfe74dcdc13aace1a22328..71f9dadc65bea48ee1dbd925f2cee058bb2bd832 100644 GIT binary patch delta 26953 zcmV)OK(@cnk`VUI5U|n(0XLUHE&(Z%`~|*$Q}HnjFePC%xgxL&QgFN4hA4*0Qo>XU zNg@3lf4d`~HZ0=a{p7H{dfFQH#g{jHihG2}>pUy*l zs=t7}Ogou@pt2}voL?YCO8?qF?i5~Yug_!MG=8)VcXp=T>8{oLC@>uT=v23MmAZX@ z2(Iv#*0h_ZZgmyR)aa@Yo9%zOR9*C(+5TJaQjdn)2Y(%o;l{yqC%=i3rkfzLK!tOV zKpYdlm8T%dRtl9uI3pF{e!k+UWWh{7W6Xk_N@tWvFtyqwl3(YnXNO#X5uFHZlc+VA zfDmF$GO9W$Kp7Geq5y(VvZbEs#1s^NV5>yzb8h1WDq*&;{4uu?E1{yq&$z(3X*eua zMxm$tG>o%k8j@35AX=7_5YDbeFbt#6m)ta<%_#7Bw0i19yQRc6G|a*iA#812k`N0& zWt~?j#35=Z%*Q7K5k%q!Apm%RFidQdP}9z+T zsTx1;scP$kY4-k~HQwW~-W~nlow>v(+h8q)=eq!OpW^QCnRHPQn8nJ1_7x=|5)`qy z8l?b>xdqm+`*x)M&b50ejFO3o( z?eb!6Iy*0Q?y=MBR(H;T@Mkn&J`f;+FFhcHkZl$7=ASbEo(9Zs@b-Q~--qEdpkl3vx8op)=XSO(mLe>?w~x zwqhq4C72_68Pfwh6M`&%D#y!uIclwQlfZgd-Kst^_01crk1Kre6fTTKN}EgR4Ge+9 zYy|K`G2iu$;Qc54gh#yf;MP9ecb!IKUT-8sams*@y{5C{viTyo?fj0Fh(D=;EJj?&nErM%5C<@<0mQ8ib@S|J@6WpW3>}p1BsmR^hd{MGqNh?`tHTD7>0#T$YnT38GMcA?Vx--&Z?GQd`Ac(^w5% z{gh=RCA>ST@t16cWXw(uvB|C&-I5lvN~HSB?iicOYIube+Y^GB-tQFkRi0PbPIH`w z#A)!dY2w$j2N+zA2rd_+rI>_R7NA{ITf9$Ksg`@cI*mQFU~2vQe2cJ$o&b{zN!H)!CRR~vgU4Wtlj+<@xkIRbs%h@X`q|&u>*?UCG3RK+NJgln zNCNK85XD#TaA;_8wu=X22NMplbtJYct-7}$wc)&8s8Y9$0cR`Vm4 zr)*zg%hqpvB8$@#S?b#@g%rJp&p-=sxtgSZZugh(%o?svbF*@mZ)8*aM&+cdMCvJz z%~;a^qf=mx5r^T6Q(%@W_5dPd6K4_LcaV|M#_uG1(}vMD;8L%LVf;EWe$Uvy`blg4 z0VGu@X1Ke4(VYp1_+0B|KU-q_NR%E>gKg-T9<3{7{tdvu%9op5+LsoouN@%5926CQ zw&za2!Q_IibjRC?A>wx$@$x&7 z4;vt+u;hcM4wm}p;b``ApxBzrcxXCrO(=}z)tV3CFy{W_NYF+QTXLfjujqszR}m?J z_69O%%sw(R#4MX(0I`B6=Y}8tbpPQ${b;X&v&#-m)RPb>6tgVj(G3AMlMyHr12Hf# zm$795DSuo^cjLwpzR#~vP6nR`o!-0QygAqz&wI`xhgi2H2S|h@I25UH@r?extGb(5 znhg>ZNzIJkTtomWqL=z=ujWtJZ~pqhnky!WkeXjz-(4AgWvC@os;lei)xR?)2%qGP zQkuQ5mi2_u?05WAuG{Nsl5<6~#hw5Bar)b$o`0_=|GoYPyj@)}Mhd0e+v!!#1<{4{ zFaP^uzN)9y(vudvODYYqrhsoa%DdFt4AIJLDSk-3M3I7tEyb^s+-5%qGTSAC^>ei) z`wvw#HJT(^H1iRu*T87TbY8;(k?0a>!Ub!IC=X-N-h?QzmT#&4)lA2dLTiZ>7G@)1 zPJaUC7qYV;&oF+y_fdpvejDMJ|7+t4$0pAcB=BXGDLvuz`ONB)U25S+- zF#CcP$5K+j>^f9rk_&3H>xXKK1kfT}f#!fes72g0Q(5@GM~ukki*>o);srUqsse=| z61Ooa@Hx`}LJ;PhTbOWI`g_n|pqN@jb+%K2t~0nU-E34BRxKqAYZ9pH?vT_hMt@cJ z4V*~ZXrt%EWQ2put^(;{N;oZ(o-Z8~gHLHOkc=OagZ3TM)Q%e>TBY}#mRSHmZ&f+x zY)}ArBY5uz6p=%}wK`a+q$(lC04`gqyX1Cj&d$l!RyInj!ZPxYtJ~ayq z%3G*cXSZSO{OqPf>~1Zb2L1tG9D}5{tIWhEy^jf8R6}$K7pZj}R@1TN72|vjzT<18 z-DcisY-er(5>>f+o@kW?gp}cNy$%~UnY zLr8}zZTL9@1{>Qq{HOVl_Mi{RUc-rm4Uj!2^83_a5Hb|1RWCW?A~`5-yLq|iy4P^C z7czWjy?usvBC`IH8BwYLNmF`D$%IV$xFF>mUEGH91VKZ7@NfJ`NooNXojBrBA;%cS zH`brI)Ycp}{{77>h%QF&oqssz-m$dNhyyDFu_@E&T@WSNS-b-|#yb#Wy#sr6a7KrM zvmy6@eJ20FUN9GN5e*V~q}*u2I8Krh#o3nB(rnT7NULNHuK?bfL;ff`cTD)rAHAGv z*-flA01y7>$Lc%GEYD{C7L$h%zWAGME-@LrfNpOC+xwv`t^Hi?-+zH1+z|nr`s1{I z#M`8xy31&?v2l?_GnWdpZJ(Zy?h545aCMs9iKC0oqA>XfZ2YiXJbK>!THUVe#oXV3 z2)#|)8cayTD7nx?S#^x$^E5ZnL+DW5CnU*d??0R5_osq>Exn`xIg||=X%>=b1J@UK z=u}6rOJ^yeR5|;hm4Cb(AjA$JfK=F~T0;|Kg2vX3ePRiA=AI%4DYRb?valw^FN`cP zjI7>f_qCm-;+){>&G9+`1WTA!F9bsD2w8!pR+tnAhbX-6p9?I4e1xqL5-gwpye;Sc zVzY8jRJMKy{|W}M4w@96xY8?pHYD2`th-rOPs?gm&DW*-Mt@=pxP?f@9G$PZPa6OZ zwgwAN?8T9c?He!K8n1T)7O?dY@6**V4uW)Qcr^r?ecD6F6ES0mvA%{YTWKu+R|c3Y z=D%(3C$5yXMZ--pCs!CEINQ5R$pSVEttFwX0M>&`Duc!Zs0A}#K4BKnT|O-r)6H%0 z)0UfA=nN?5(|=CJu)cdvrtkn@QmS!Ae`E4(v@PN-h&#z7 z%s1~)ev3z6(cYvnhxq98j-|6742)%t8_HhU`+JFKrP645?^S1Pd)rCHH9(+al4s-V zEU%r5+kex%IdWx}sc=Ogw#0xz>Z`-iUe4=fK+Knv0Xg2yJ|P7nE@GsT%FM<%NKOsR z8hD;cxIoEBzCSKT^LEBu-G~Hmae791WL2K;tpe}Q?7>EmdtQ17z=YI<%X*Il4N>QlDl=_0@C0(8qd%IW$ z6Rxl+ntzq(jF{}RCQ9;t#d%xI(4UP=cYm@8cG=Ug~N8@^kIX+uBl2yGOL5^!8iUWNb zIX5)4*oro>@}ny{n5>+wtL40Rpa*+7B+Z_xeVwefSrlh>hK{C?ac!v%D*BN2{$(Ish(<&%g(|t)?3nwZQ-Uw^(yv!9R4713caP;|7KE0290yFmpWTasYA#?fnAB zwJ&y4_$oLF2Dr`kc7Q8z6#->0R|lfkBZFL&-Fko0P64u9Bex}V+tPKDo*pI)U{Kj{ z0OpJC2JcQnLQJ3sW)V9zQh)bZ!b~g6V?Wo9ed2cr?|4NkK`l!w?Ximcm^G@cmT=3NI3Eq;gx% zVH37f3o9$vn`Jq35&^f}-)+GSSosx}{&u(d5SF}X_RP(WgO@8XN`L8Y?Xp@emKZRD zXrM%a1Cre?H}l&EAB5oJhvi~(|FFHO{6#stuWptl1Tcz9|Dz)Kcv`K?dbaW}a$Lfv zi^qEHenJFqZYG>&n|1Gf*c!WX3IQM1t1yQDxSKGD;UE2t7wG#<<40`U)#J^~ zXD8fw6-Ao?@qyoGxPSQnre1Fm0kpT(Ot*TJ2l0k^`_N?8}tnu+A9e=vpz@r#_NNK-`Ysx7d zAaVqvEe_%k``FKG;=0A1Bas+|{S<#@47U|ae)jbOhOd2B)n56Q1-37+L}STWu1WcluY}x_k zBKM#21!kF`tTFhALD?Bb<&02dO-n`xG+zPwm}6y1$C@u%hXy|2Z-OaKc;KxM+TQ$p z{pJsjj%^~7vCR`0H8CJCAa7!73OqatFHB`_XLM*FGBPqVm$795Dt}v#+cp+{pI@Oz z8>m=bMY2F2Qnbakz@lwB*q3Y`6k9XVVr;pRok{+EKVD?(LVKLfG!MlBK}M!%@?5_2 zopVTkcYE{ok49%q5+Mto-QH!IXPO#9rOIyC*_#CuL@aYgDP8=u?e^ss|ElKDEpx8u z;y+8gSyT_rs@{KEzJI;_8Oy1RF=CbS%h4?7oG4a=)qY$uwP;#=xv-12zOUDK)N~=f z4@3L5+;(?OyD9f|v+cr)mJe5Ck`W~}R`x629_JWhi$c07hOnZ@oK9iR*3E$+L^74- zcx&Q|cPWt>B?U8?AV$V%cWtu?@|8i{cg^}!7`SW8O_dVU3V*`6>ZMF>WC`!z@0-#3 z+j7b2;zNDER10s3CpKeL5v5qa+BUr#v$mh{ScQ$=hmD+9s_23D@uBwK&V%WMR)kuX zco>bBb zeWjLG4!#a2P|h7tDK_)-zK_$)NE$q5c|mj$J@#$VWRes@WsqFks2!y&1v6+@Sg&J0 z^IY_)gmNzYWGei|LK~vFb0C+xmqyW)G)`=D4Gp9OQGZVhqKUA@M3CMrVz5n+0|7|Y z(R}o1=+nFcPe-2~aM*8gkj?LK^HD>tP= z$euFHF&3(JGm$KWaM-D7+I77x_f_}RScXyR_J1lcYR2HAh@S$WEC^$L;-Xw8$$=4W zKs*+;RV)z_ROf04F-XoyQ0E9+*BdaU+ObU&77RI!NEI)92-Z3fG*CzqM9LfQ6pZR3dVam+Pk__J%e z`b0#&_iz<_z1cl{8qAn5+PZ3Mx8?`;ABZeG$IcLE}pmnZ#_S-)_Z(`A@~@MGDE(KN<|nH7p6k%xLFy1Ib7`Qi5Fj~mx_K|Itg3V*7i z%s}BnK}D<0&6~G0TjS-=86^^)ee_c|8D=QzzW0#*e)HSScdic~tH@CTD&#q2LeP{R zXqYoDk44TnJkg|4rx>SfM1atC&VO{O?4*H3LOAw|uhxn5jZEuv#(+I;Q({@~qQR{5 zQmMIA)9?@7`R9^1dr9_IUyQxwwSVj_pTXXVs0DK(Wd^0Mh-j#x%O8{UR&o%scND?SukGv8{+! z&g7=I3`;L#4a)|z&Mn18Yw+u1_oa)WOfRbo4+E<75 z#MTIMq(W@Sd<>sQK66J%?vO-j{uxokllE}X$LfwZP91jw`%D7_RDYsmGsxlc684qL z@0Xy@jPWNdA#qnfMi{W^IpD(JBBNdccaqglqL<{IpS zM9R4jpjnwc-b(9ZGh@kJ_L!HK1@o70ynUSLJ>D=S_$ zt9XZ3IqnAr(zst5_}hd2Do4euoY&R9Emwmdg&Zvqg-DpJL$krrd=Gf1Zn3br(Ln z!<<97kSZVixi4OBnl>(5`HM5h@2k4<_gHSPCQMsbU40)f>!cw@^6|-71L@jjF1TH^ z1IMuiS>{R>i&x=Y>`U^dU5DvrQSNqa^ReEowLP1S{= zZL<$Ue;n$4{O;-y$7;}EE~NEW0AV0rJ{y16n>()Unc)=5e*!2^j(L-@%@Y?jH6Sn` zZ(?c+JUj|7Ol59obZ8(kIWUuv94dcWOLN;s48HqU@Fk^Q{flW|2 z<@4=;%1;$!?O`BL>10${`oMn~Q8|IGq9atS#seu5idADyNnR++=(;W^D#;g%K67F$ zJxCQRj#)sV0E@kKfUFK`&mOMA~B1i(r2 z!iA8Gs!KnYdhjAxtK%{h))SVL@ZJ`_#p=^)62Dn>%x*7wscCMH6-a-#;8ijT!su+} z8K-+B)t4%{=L7|zHXuJQu~|JKDiu%Y0ybkz!vIb=+IMB;9fzD~$pDT-l0|i{4>=qu zW86TpdO;%$;So@TLV=_hGgR@y28)p*=_tuGItYh#+%&bG0Vh>EwA2pU)qX-@FZjWl zJE;^=hs|?(9|nru2t|J+Lpx|4lMW-hw?w8=wJr6-DR$vh)p0sf=y`C(xNg->bm`Yc zSHudVbpA*&nn!8!yL0h(x9BLIN?EO$<gZ%I?&yDaa!22XJC=)bJtM5Sy)^%*xYk97ln$M6d|!Eh?~C1l20xo{sl<7Ff2-m@m4(h2L9zT+c4kbCC&+x4O;F{#D#SCd3X zHi^41{j&))vdMOqg8Q-*Y@c#BONj;-Qn{j{YJ<>Fy!Jm7$xZL0q6KSE?%=dWNEaAR z>tu%-iKl-!a)(&fR19kPv>%LnA&^upa#(c1gKeVw+9XwV#U`DZ-5#9TwH978bkggt z%uWGY)0ud~HZ!}kDZh$z(wlts>HKeP_H2vj*KCV~=V*(-bGAhSp&hqB zlaOZ8-*}~-`p{a1khNkqE}#10(Z})8KS%NSBr*Y_ys;3{R1jIBXcyN7(p;a%vExF^vBVMcJC7`k$);*H9|@qHVbTY(k`J(Cas_iuOg3-7s3Tpkd zW9FE76x75Vx`QBUf(Owb??`8QTM*QF#ZV7|mED)3BCFiO2#l~UmW=TT&z;>GcE&a- zYUo@hBPXDTOKXZ|RKS?JOE(b{Y-lsTJxvlo7=rI}n4}cd!z7`XNz`i*j#kT<52Alc zQV~F(HKPmhN#7A(;-D!|kwWnoyy+Q6HCZmsg(-|;|1jFS?^DLBlMd2g5lFk%hf-^7 zMPvvcHVQ0P{-gm=_YHvhFmD5L5$g=4R*Dn$5N9hTX$mOX{UC4S17I=t+7bbwsqj{# zHuXnHF;Oh#t9v69h8hSD802i;f!TjOU^`<+7u7`48MggqBPqr!EeF{+obkSC7HsU= z8|M91`tr-Xr-uj@#mRfwz#mL$y1$MrubI!+E3slE5v+-cMrnJft@guou zoYlMfYBo@wc4=QNdQrN1zPP=auiE37$_$H}QczLgN!c$jqpG1sAI5uCIhcBC);Bt z0U!;Nv62+GJ}v>y3X^eA6Sp5Z0YwUvv62+GctZh;2>~{@1Wf@Y2>~^e5hxV5K2`x- z4FNQh5hxRy2shy>E)v;T7t$5s!7Q?W7)|U=cB3m2z~_HXmMM)Hq2FA zAy4Vn2F~OSFzeQD353?1I~t|>4_tjEjeNvHPFSvDk(sLDbBr+Cf*B(Wc^OQzByRu& zqF@j8WpQQVmotX7g2o&)Fn2Z=6EbCM&KV4g+d;fh+d?Y=MC`ABz_0;M+ETCt9tz=% z9CXm3G2j3(17ycsjRQ32Bfy>&{|dwvh-IwJs`0|LZx#O(6GOX##o<-+c@l+^TvRB% zB&Zha4uvtC!M%vDJ6R~|5Kje-=8YOkmbP#Vm5fO-kE^i!>iT-USu9`Oz1?*E@2{@kFP2}g zZ+>0fyh-p*x>$a<{O|IcH-t{QSS){Bz1xI0q7n@r2Bkz7S}a9_bXHRYXhHW?c=jwT zUxnqj>(}eBd>;O@`rqB<=4yQ%bCfT_mtQXMcfKSn6k$hy*ixe-oVCUbe>JjL+n69a zqafB|ahyB~QZ2$Arpb}#lLH41P=i!aA2k9Tf@~5AF z32+V|G6MJE`tHMri$OYen2*xHD`N$edvq_>*PBkj7akjPzW?F{e47!(?mO^HMi6`G zRCL;7$7Ak)XEFo_M9KRO0VIgRkFVet#Xi3Rq0wUxIF|I-gX^-Tt^)y=FK^cGUag?! zuzdOaMOeOGecFVJzU5wC{=Ql)zri|I*PC09mHxTg=XQN}^KNz9WtMK}ht;oFmtU_x zh0dY~LvUh-iC$jb;9Fz`zNhQJaEnB?Czg`{P2{A=l$X@W&)FP6pu4Sq6c>* z<0u|fYmK$YaWKiOe%|0m**;4mfHl(7F+sljZN1)LwpHZ$VWQ{p|CA!fEI5lb@}4!aoNNsp zx5k!cddk>d*u<)@A3*!H-4>uvR!!SiO*vULbs|vf1fce5uqlnT=21F9iZoP>qjX4r z@q)2YI$~5K<7)u?qwc2M0SD<>z6X$gxEl0{VLuv1%c%gdec!Q@eaCbfKzeiz5RQI~ zF-9k-Vl%4hr$?!tLVJ4ed1=oG{XN(DQL60c44arUitg4gR%fkSQj6IS8&R*Q@5TiN z$$lJZkrEx>IKhMVisUjerIiUOZHLBxw^Gt<){>%EI1GB?EFjpA$EBPKg7Q$rKFB^3 z7*JJ5m(w?^ex|5apm0AbBJ57u)4*`%#tmhOb%(PSO??B0n$}Oqu>q>}xFqcWDktWz zG9iEM&?rl!3B0NGD>aMn64E4 zo<~yj%T6+l-L0G=)a$q;XC01zqPZ;zrRX=ObK|_#Z&D}hU_ta489-kZf-9B zZh8QGeqlQWe0@4#is6!6N9jc1s|U`Vcj=h(!E@|gI%+khGR}{XqFd3Yr-}Z4`$#hA zL2@90?(Xt?TZ64zxKqJOx(8O`Nnp)Xb`q`Mv`<1pkIutTi_K)54*A3n!k(U< zLSPq^5CVT`FV8Cg@Pwfq07g(06zKtl0DhJX&;z?bf0rgi%n2}dfT8|$fDm?QPcRY+ zxHdS$Y@u+}HN_ncfg%Cdw*x?$IskoFDEv>b&Yu7Q!0+Y&M1(~C4)>e=GY|~^iww54 zMYyIQ&%**XaSyujGo z75ab6`OAD=gTJpU!WCe5T?W)2W(U3gBlbmsJ)i(I(jDsW`=5?~pNK_701%ih8ejvp zhrx;eiGFQ{+WmRHo<9=i1+V~K%TELV{Q3L)&FWfV5Cq)W`ycqP5eqA8s4AKn@&7LQ zZ>N$H!VBOl4G!fPcr*LtG0N3gG!i=axVTpzZZTW98L7EDJOA?Y{0aDf z{9qTDv-h6}Rd;77^1lJ5&_DN87Yc#7yZpCT6Aivz0Y$jI^WW`*q10erP>2BxZR>yV zr%Ha=q1WR7H&+#Ii-7#x5>W|B02qk`dlLh%r64LH0q_;MUMC3D>z4`v!a{Ha`kDf8 zbw~RH><~!epCb_$2MAvqVb|Z7pAO=G1-EraBCl8E*Zi(;{_Ff}AE8h$s4elrEW-9~ zh+}O?`)Rczlc(Uu=)Eb<4b!K*g1&zX$PV{&a)K1zs;npPktd2N1FtCGuBq}I-(O_? z?z`JkPY~XkWY~7$cWIqqG_lb}JU>p?H}-s2vF`P4B4$D3`-TWQ|IpMZocWH6v zy17e}8|2fRdiJV&)x9oV8V;M?Fx;$?)F!zsofM2QiM9+Xp61N6c{anqj*ov9yiLeQ z{n?9R`uJpuI%n*PRV#s?*nclZtkBndMKtzo#%HY2Js^u-#o1~Y55^{wM~+zM;i z@M@d)c^OAF7~Ku@O4z}Qm_gaDERE@pBR-w*Gi+Igk=fgZ{!OG`VEh4X7Q-ss35HzU z#INxZUx6+O{X)7rD)ontZWXRNVtuNy7#!IBAT8=R1Tj}nUbDMTd{-EUmT@_N;TA{F z70`OQTalHr>1(^xAb5X~mG&MJd0>j<#>>nYq)^!hS1RQNrznn>QF1{`0!rn;IL1{> zp*{EBn~(hl1~3^%i=yH{>^zDVVZ$Lq;jsC5{vaP5jg3gNJ5)EO__BU1f-pv86uPpV zl)QM%&JNs^fPKV<al>n$%9P*OmRzv}Ni*5L+G5y+-|5mUDB*Ptw`QGM~owRnn?3bV)(5 znn;VD!c)_I1}c9K3fBa+MP^(nR>b>Cwz4=nqVMob;^m3qp|^L8)5f-;;y0wf-zZ8T z$I%gAZ4yr%UPBCKv*EK+@o%v}=t@$0T50Yipc}Ki;old;LyFZD($?&x3HQ)9@ISoV z>gF*9QA{NJ*s|uOpMmN*hV73Uiwm~3uPhQWbyv9b?{0r!*7&&t?zhh#(p^c!^2Mo$ z;t3;1iL`zAzsy*(-V6)=I^}xHfvDk8!DEMoksqJ1OwA?%Pk9@=G743kfW}_9sk8Kq0JZFNhS-RF`PwI(TH?ciHL_)TJ z)~17P@46)V1wv3DetIqz-Dr!)OKHXh{{xHav*a$y`C|&pc2mMv%&RIGEc0|}D&b{iV7@TDX7ulw4}WLx)sti)t-fl^P;TJdwtUrK(K#igdK4r{gWYjr|0&8boaQjy?@To*;g+r9ohQ zn1Fw_>-`h_5Ks3odwTb_JM}VaxIEfWvh_#;d|5m%HyH2q)Jj0!jM^aGBc&#Sgj0hf zFQ_0{EH2!)>LN-_dDC|8@hwwOh(z&^uUKJ*J*nRlM>KCst*F;UWzCH67J6gjCMvC` zHK`Mb>UqXl5C?LIPi5)APID|7gD#0!dsu(YRth3=VoMkNQRkX{{JG1aVHuCV(}X4` zT%ihITt-l6g@fmu1!M|5+lZuPt1@ZpbFO?1C>}5=H9s$D=$cKsDS%8m=ems@a3S=_ z#yUm5=dtMS@FJtI&9GedbYMF0QRZDk`Mw-d)lS><-sF4>nr^JVni(h}{ z%Lca04PQN5)hHNpIyMNGII7FN%jdeQKA(B|p)Kw(51yW0l_w@3pw6{)@OAR08dgah zbMNAE3EqB}H<7iz8I8DMWi8`b8`}FV*IK@yJnGOQ=*VJRam9zjHPhrEu4uE|Ot(dl zAD7|s5D;Q`@TE$iwJ=fKeKuMp z`gnk6d!_EuY(8Ju9Volsf2>)}5~ZHls3Bu0mZh zx&>K|^+h9df=J)g+R;^7U7OJV&~P@2UrJ9lLHGhf=oQmIW?wA6^wwV5(58PBI+(%A zT77Qt$M+lOYdkm}CFW_(YJCwGT{#RxT>PfSE}wkVRHpMhVz&eC3cip?(0&J(@>?V( ztn?quSB)!<=JjWd7|@@xd#j7f{Mm;rixGEsolG&yxOND=6&cjZy>7iV1%p)H?!|gb^mvB;Fh7l`#*p?2<1`ksQ zX77_%X;4=e5;(U010PV9yJu~K9g1@HH_*8Fp5grc6KQGz>vBkgz3U5OMVZDLX{xT= zwu30&_qurJ*4`fg_d=c0fHv{`;~9v8-X-VDVaEdqA#0#s*~$?&aDIP}W8i_Tll)_* zt&JE3?PLCXVl>d{LI72%EnX-wB5ZKx*juriF8-dpy--W#*YEP^sA4|B7qLbroeiVn z-J2Q?m|JIcPT6c&M71OOp*K93X*C4qSH@3S`)~?enfe1`$jusRitS8T=?NIqdUZ_Gln`xth$zx z>LCW#bHNX`H{1spSBn!<%`##BG%D$TzDw9cNgeLZt95Le3`l>|OgU#Y8V?9bNt83O zgjHR!@{4}~Z^?EZTzv%>#VV}|iRA)A&Oh$DJZkiN5LK6}OnQ8>0Ph{Q*e7`^0$;|G zZ?8N;Du?QIHgBlC0WTG=*WRy1DzR45l5gsw97&?CEGz!=bRxV%dr!<6oYn)_gz!T2vx-ufUAEOUcTq3enV_RSoaIP%Ok{5ke@#Bj2&gn^>-V}R zXZJbZ$@BEX^Ca0?iUD@QS2K4b)T+-v5KU(6L^cvr3}*^QV2C$g8^3ks!?)|gsoio7 z_tP_a$Pj<^j!;X6UV#)tWc67TWAwUKjRDtqc)Vis%T@&k|C1v&)(R)qeHzl3u*SNr zc%8)2d)}w?0)mdBIMWUtcm%F94HFni6WIbOwXmJ9+=}Ut-IU{|>9N(6a~PQ6pe@&Y zE*+~y?}L9DaMb2q+6P(-&+K=l4+{6&G|*oo>nwl6DLz?oYGmX_?DXvX;ogxI%x&!} zoB1lIIgu)GNBM5D{ghK^FB^bmaUoO6DFMb}v>UN;z2+vues zuc%L@D2+B5+4*xvBha&Tm%^362C@RBbvMDS(d2eA{2y6%rgV=r?tWh+n&mUsna)95 zKGJ`5vNv9#H`NVq@*o^!))g+;izW|>;5wSlA*Htw=-iL9_M5axV z7GxX4mu0I3+dGPfn1fKBTC#i7m(0{nV|8M=3EQxADGpoA>5TI`3+Gg5pxk)uUc~12 zMSDNzh9|tbAtY)u2KVQxnS&OjD;pwSFxP)CXc@LN2J{L?rD-t|u0be1lPgpQnXZ&^ zCRM$O<5Mz`m{#%jqJ3Xp={(wcJbmg#b?nYZ_qLx08kUA4qmbTLmY}LqvzH|qaINX3 z`=Ou_TvnnJf4n6op838`|L~!5_uG8*4SJP`v5{KtS^6L&_0`S#8;R7mOC3!!V$@PlEts3M<`Q;^6P(B<>u{k9Jbe%+{nq|R^LFA+Gt|uD4(yii`vn$ zjJ+~!oW4lK1?5ouFr7|oQ|;!Y|;W9p3zyI z2ChIJ)kRuxZcBX5#fjb#;?{XuG+gcDhq#6J@#Op*7^)5y$mKFM*mzY0I$-+hos1hs z7)gKZwj0SNWX@HVjaN*9vN3=3a(x@B_|XJ~!%Jc0gtJGTio{6bOg@*Iu73LIGR z5j2AcJUbQYJgF46invtOswAE0JaG#pP)}VvQ7!k~yh%nxdQLT~db>8LBygd#@ z(ShZhp7oUkM`G2Rt!BL^f>X(#tZ3%HRCboKyCmeJIHE=qcXXm$QIq5B9O(YCr)OR9 zF$NzT_=~jaI~$W;?p$;>MkV1+GdU7%SX4w=TTDjtNi7Wel5G1yqmPMON8>U>UvGBZ z)QU+6ftRN~;^a(`zZ-win2`9Yg2lR06w7cY#%C(p-6OOFuAua~kB=Aog{V%^3;c=i z<~?GaFm{?)Hv$W5Fy~CE$k&G4XV0t_@^(_#fVk4J`~}(BmQv3R=}SKI-6oVFitoW* zTb1x>X%HeYJ*Ls~LSC3^%1F#qbzS12&#v6%uxL`4g?v?19YB*b?(Xg${Nh~Pg1fsr zT->=3oZv3O-Q9z`1h?SsE`cE7=iB}F?Bkw!>8hDCkJD$mtBbDd-I31_d>!yX?SueU zYNkEd41gaCx|r5{+tKB>$8z$uE-nqnF1ZqY_QThlEbI_VC}SOkUjFl^pnNy(KC*lg0ts2w9i~Lnm=> ztmZ^}y1vD!F2Jrv1p z2&M{9IQ&*7IUI|)CX?i!#dFJ<^U#kv;7ir7uY?^)@pa5HT)$8%oHLQ!X? z^?`qC4T0_@8^Vau8EMea*4uT#8pKg!tuV+GFh1PcbC_ad)g=&=&I=Q=wX$+ZJ*P(lJZexfKA}1+$kqR?RCyBPz>uvceT;^ zn!)f^zfaOuNMHJhIPd6}Be@;Nu3#++_F(xzhd<|`voQlp1Z^QQlqSe$kSB#}^43;z zPF}wbTm>NdX3)~KFBI;{!T)FKbkFAdmo+%@;!u3~DFq(STJi$u3Og^yG}#BVy=e}* z@zL*ZggoYjv-wO`JMeks2Z8%-H-Ct5{NzOS*^S&JiWgD~$f+XM?2DdRF5 zYdnwrJ?YnBU&qhWIHzE88}^rQ>$v%>@CSN-gO^PAkbape2_-NJFWdp74y==G&AmdftT&0FRXss=qtGnVN4+oF2t4 zfR7X1tM|Ds38wk?BBCtm&!RD@*BZyKwB2UAP$y+_VNe?vN(#Bj*Sb=+oyKx8qMc0F zS`m57)t=n$Nh*6c`IGgZq5cJNcYl>5G0KQkk`sKptFsU|dedW{U{BWI#g^7VZdKxf9z1W!K7iRcRcTIyU{Kyv&zgo9Slm2(64f=xtwxm*cVt?b*_ z8?g;{@1Q<Gs;>PZ zz=&d-0elu4kA94ZgKQ#E=&mbbLhM(&C7ywn;62^l>TZlY)B-tEMm%)1O@8V1$71v7 zu3sLG$blgOX%L8+QTuee3F(3C#D`g<+RseQ^aUDz*m^=2sJksRWITWV1%fRv+L>Gr z4rZ>UQf98NQzY%Cc`X9fZ@~r6X|$EGT$}clhX!`UY+7P{FY-1GAe{B;@cW0FKH(S@ z!4=`7VV0cdI82N=P3F!=X?Yp>xhy}5#B&k_pmFC}a#%zoGMBnF-<{YU;Bp6v$xh3B z*32)8(u`-D9po^|EdNnd1r}?6DmPJP#(jEzOU))=h*CVifIQXtQH`-*vO)c;dftGg z|TD?ufWY>J| zDTAKpQ2!&t*}(Rl!4ueGSwA9kS^xL&@Nr|l_{o=-Z)=zKwJ-wt73_$=+_Sb&B`ihf zh;i>57kNpTRI<{*ol0c6qT`2kaVG?Y)J>2hU_uA1SSV>;dd-MI;2Ac=u;XKq=zYO+ zC|!gVuCxNzi6twjRXwZSFC!lh+deT^KfPN58n12W7)l!{9mD*SWxs=B#!i2#g2r!igN2qAhT;LF zWBzAjK>`{`4#mUW_KA+aei|437mHR6T`ma4!JnLHOP9WN42=fKmtOy$*&#uF^$qFe z>o+8!D{WSlZ8lAPeOR<=Qe)_$h8$N%R0`4T4om4FZ}7X9l5w$9EEjwAlW7~4OpQ~; z%=@xTP;n_mm_;)?d!x9HLDT>eoR_Xj3BeeJi(d^vvC&O% zLp(QWs!D(OP9iynD1!VZU@-IP-~eU(JF%RwK)_BEr=vSOQ&WG(!Hi^wj zu|a<@WnyV*hquB0ocUnfd~?&OUj7Rulg&x z4TTC5mL6)_5m7_45!T{#*t~0&Jm@rwFjY{xi}v&b5VnCCn15^F5rhX>L+ENU=%h1P zo+{qm=fsCaus~I}O zl&Jz^PQ+#vdAuQTuil=Fz*q~|0Yhl<%hgCCy+f!GZcayWSFNQ!o? z`iN~v3Y_*}mqljLV@CLh;t2--1$C9|GQ7<38LFM(iCro+>|&DGZ8)w+^Ff?*9jba) zn^Tjh{BY{Vb9L2s@`Q-qIUOuo_fPt(b_NX|&p2Be9DwLKt9VxRi@xd*yc$eIWP_BE z;>bvk!-k#{6+h~A-OUb z43ZI^T3G1Y!fdy17PiG-Dl{REB;`aZrOr5kJ@=YIH!Art-nBO3rF%fP_D4_h}f*Q#f3=>m9 zpk9|a?a!4$HJqcN^Q}HKU)PYIlj(~-(ey6S3%s^;b%Z7;PG(_OaI7Z*&qBWyWyj;W z*j9czkG1KPbCx$*sSm`@D<&$^AD0gHxm6jCT*{RAd+ZWNAXE2DrVyUq|J_H^{L97S zS8oad8mRi{KT!zHE}ihod(&R;CtOK1D8~6Z3dHd}^To-fxm<~V>1!1*nTsi_aYo(I zDIsp@=eAog3Bs_@=`w^GX4fHNb8X`B4AhYyz_1%~Dkoe)0SZ zJ`tGSXBt;0-xzRwDNCSz^~5XG4F_y-CpSlgv44M(CA@#Ip|@nOU#RlOeBq&*2ko9#(N}ZG~j9{gV$4P zzsKTdB_+4MItm`lLY~idj5rY;KcmI%TbpEaGIm7!k zKcgL41?y6Uu#d-*pbuNVT}fAhlZH<`>AzWAbtY`+;SC7fdogp{^qaMqMDsa#@tpUs zsjKuhPMsXD%#xv88Ad zJ>6&cZq4H5d;L>WH?gyd6X>E7&88l)Sw_`aZB@kT*OBpy?hwns-bJ&vEcwxqHD^<* zv&qZ)xJ)D%>|SO*fQS`F{bw7qHdflQP1^kCate3y?5S(K`M!9FeB$uIDTlGI4N(Aa}?9gtyHJa_%AXx z6|;T$-?gTzvSp!f5Bw_)n5+H{Z@TMo_phihiGnm)wI8daRds65b%k=OU9oR+6mIsO z7tr%^Nk9DWtQKM5a#XtN3ZBOqJjAmyA<}aYV@O@ZYp|egGKqFdVRk1yes9!P0$mU~ zzU#~wgGtB(1p@vYU4RsrG2|G^cYbu?rI)ESXfQD*i}UIojs%=lcGezivMG2@d=u&R z{kyjmPcb!D9^1$Hy4uZwb`;^sRPnnqo#7&Go*`GxZEd;8AU_9dyK8@iTfTT?HLC&PBr?YI6eW1bjH3Z3?*1JB&wDf(nZQF+Okk}FMbk^Q^7xcDq+ z&UjG|^qmIend9y2_5BsXgh%Vi^syllM3l4xjt2|El6c6Zk^gv&lLQcYNGtLCO{T(Hb zp8@ng&6Fnl+@ssn^tK_jUzH#W=7%UiC!gNokqn*gT78>t1g$|>+nkL*RQB|+rsos3 z+jLYHglpE{_e^s)suu!%@U zTHcy17Or9w8`Ko<)e>-92k#KXPa^Kxfs7wUJq*TaBI2UmjN56~6Hh7((-iH(&HZ6> ziJ=+abI$KN!#_TPXj%RmH#~Y0h4AT&nxb7;0RxPFTE?k;me=H!YjO1Y`~RD@-H$Fu9G8adCuiNYFh} zW?GaPY(qcZdk1#3S03|HfQ2+jY>8`vD@qfTuGq)=Sm_-uL$|cZ8CIoJS zly-h%lMZ*q+}j;*Pv`tF{|f<{CV6M^HY)Hw@4z^ekc&tIajA%OswfUL*8{&V4onOM zAXg<%d2QmQF492Tjc!vbQIUdWef3lR9t8EC5JUZ)EQ+BS^Mn?q0+VYCyX%%;i=Sa~ ztAky`hN53Q1XyAiW{~A4X5weDU~-fmLwn`(?9V2xP9#z`4Y{16&?$e$(VZi0I|MpT zbB4VpLp@yG6&uwMqsS@T)zO{J!-mjd4hM?GrU_>uff0zRs3RQ!>mwXu7}>4 zuh&_3ykEdxwX#7mxN%+5ecF?L>!W_TjJ5SkF74vqt3_VFJ+gofG2`+SNZ?v?Cwf*i z=Hb5$b1!=buDpP>MXeAOJM6TRCzFbg!y4xz)bJbs6m}Zy@Pn&^SL3ml170)r$WS0F zm4}^Y+15w#Qrn2M!)FIeW-ov0N-3ay@an$405bImegrEXyyNagM|S4vcTtX-s+02) zBI|Jn^WDLo`?cDEhAmCXIZOCJx*SMZ09U%gR>v&2MM z?44!+>@nvN_={OT*P|Ep`)22aF{tn@M*y-H7zX#0%KRpWup!lV+KYx~0IPSkBA)wo z5a!470!Cc{%Jxlk@}Wvvf!x92<_t=h0`W(;m_C z*i7q_07qIxN#08PkAj{r5-HlU|0@qW1C;q1ay+;W+!6J#X$Tn3gM?*c72n9m?&{@r z|Dv4KlEBR+2>Ts=c<@^oIjf9|Uf)yI8T!*`ZpFRdiT8lCO{UoBJ7lNV6!L6gNa^6L zbJgp}`|T+NmutKv`-y0-nYYEByj>fQorI$ir$DY(>mk!)YMle>#n#=P*3{8A{LPsB#gG z92T??1!?63rmk73CGIvd18gl8tS|F>IFc);87i%D5)tddhct5ti+{th008BbfU#*5 zHHYEUOw$Cm-K#N?&bei;NZ zEVx=atyWkl;M~+aHA+-^%nn%i(wIb&7$>Qwo#+RvTdHebe&Mmyt{vkx#swG7n!j{DDKS)wg55jOydP1@Gg&7Fdai~WCeszbsVw9CT2OY@rolT|a4p#=0iXo?fO!7L3M zFM-Cd5B)5`ep)h2K9)_5qQnu_;2Bh$;%Wh{5L`$Or!H!}!;W8u*pI`11T_s{0bklU4hs6_>yqJ)RD>5@4J2OKNCpuW&0#F@j*M+Clyy~!#NS^IsR&H}TQwB2P8&7uC7)i{ zRKSgiKRyMd@&SM5_DoHUBwEiP@s3#YjySZ>l@RlTpuZD$?1tkzVYRHqjyd(AZVqL3 z=0({Zf-^g_D!xBcAhQVzy?FygrjT2V(VutJZt%-w9&U!X3-Atu>%x;tZrCx5*apt^ zo`Hb&xe$%V!j?2*fPBrPsC~Ch%|<7&CX$F*5#Da<7D$~buYAb~PZgKE6A`c^`n3ar z@s}OHBq4d_gtcA4U1u$j>+@2}8h=l6d6BPRWmYrfHevM?v&rC9Gfe%Hf^0rFQBmfX z=tn7ixg3vpDoZP|;vwem#b;mBp)GOfpYcMlEfTdBiMP|vbwGxVuCNt9bFdkcyL*NG zv&dGrx4VIcB0d1}>3gzCf+3Ub_lI5gtZSwFhmh5M@nsgKr}hIWNpNnNvPG#PVuBPH zc#D<)$D0WTaf_;9zifX4X2!}{{~P2ApAa4cArvIz-XkV|w=Xac-MBl@OCUgUzCpqE z;@A7_{cce(ADyAAdw{?w%)IJJ-pYYyPlDn)Ta*bW{T?k1rV6dpEQ&`EL5aqloj+>yiVxm+7AEK7Z9Cr0xU6t_RCSx`$>pA!35!`F)(|Vf%?Rrzw=TJZ ziQEU!uft}d@GdE;%vBHmzFx$B6dfmN^|Khou0jec?!p1&*|h6H%_Mix`hBxpx4C8( zDr+8G=^ST2+HFYGpN902!bMkt(k5(}60qt~zdhm?esPj5Y&J9}=4l?2(ic_JZ^Kk2 zHr2z}M_?nuNKQ3G%BW5O`-g#mI!Nmt18Hw3BOTPU+!U-LnaW2fYF4O*Yx;_M1%C}o z?4C*vnbetlsG03%qb+Ssd3Q+@9WH>Tzc=^0Ki^kx6g|L>B&Sb>hzKF(1+n~9>dyY0{w`69ZJoLvy&2#dAp_2-VaJ6EjBa6f4 z5QZB9xx`(5tjRIgUsyTPm6neUpfZ(02cQ>W7GA&Ggz~_w6k+-nF~e(dW3~R=kW=uG zSB+n*o2S@tD^3dN11%&@xKrC|qTVE2LzFqZPK;K?tjaOe+aEg|Vqc4~M@j``k?$Ai*tyGp|y!f%M;LZ`ekk_>d~VD`AU7VlQ~G72X&kn-L=6 z4*q&X#%68^=?0Q6l-vsmtlJaICWo$GFJ&~dURFs;i8`5q-?|%C0 zio?(cx`;)m`8@Nz4|R9+kNX@0$5s*=Ri!qF6Ok=<`#^)4=5LBGOy+9VcJn2_K`ol2 zBgd1ckcueMbs`Ah?!48d?}PsBLYuS4^TY;y2_HZ1*Z2MXeo|Zf4J6XydzWGiFzU`U z%GpxZ1`8?tCMerf_L`CKTpg?+xe~e8;A{Estjw1_2L(d~<~KxQQ@8cCq~KxaM`F{a z(C454P;h*bYR=B?pCr$xO^X^5o2-+S^C#l{|45QTA1EO%$t}&xCB-8l#v#G|xg^B+ zrN!ALB*dk~#QCHpgem^-BZjE|o8`GU`IE(R8Njq@{0^j=IN{fsPZusE*Ijv}qw+t8 zTpZ{d_E-05TK64V_b;%mZ_^l}G1O=TXc!pOW>rK9Y2;|4kx3C%i7>FlW!yC)Sz%cU z#W4}Kp1S_}&+tCQT3_)WZy{2Kek5TenNTjKf53<&gwog#5q<>;wZjcSG3-f$9z${S zvA|vg+A_MW8*!9&>3oJwu;5SABZ3>;DyiUkvD0IbR*`cF-E*s2Okh_TNVcCP5^grO}T?ntpc2W4ksAWzK2XxsZx?4R04aZ6p|;=i*^SwHIu95Q|H7QVsI@8^He^vU?;^_f^lWkK z4U5)Z+pw`-i9TmJ#St0%x>r#MRn^A_!nm*-RIk$gru1|?qCl+=# zE!OJQ#A`04j!ChY7^uK6dwx#}JM9KbDhj|}XmqJxDn-3`Aj^Nx64&QHyj*NrXi*v# zcnvx-7&-;%O=@DsGdpxIhe4G?oN->7GUm76*&+9K zSNXIA6^^fq#)dr^or(+PySmEJI>WK#tN6B1-za&ef4HtJ1-!7^M@gfcApROwnbK%;ti9@>qH$iPt6;vPTP%P#L*_qh1y?I z5V_6%oMvm{TW%OR`IiltHK#Gl#U8P{!bVM{6E+bWFk_Zb#k>vqY+yaP4L(I|;*=y7 zZWZ%S+a-%j_}F}K6t@W`b!J>n{_sGaGk0B16%9qwnoi@*HMSu|LE^!%QPYHwj2$(9 zG_;imwX20d(nxt1NOXjwX+Enq>Au{E)-sWroeXdeF0EoZv&|L7=R?_NrqFj8ao8(4 za-ou*$Ht#J%AI6^y2Bf{`-#Gu#qv>7Rm@xqrBwP=yl5Jng4^_{%q=yVJWY*7Av+6xMesGk|+ z&2aKrxyDLqKGMzaWUY|*e7MDL0xHgb9?&;C`Y-M&V~Ktl#n>-e{5c22X8qa{UMU|= zn!tw+1FP8sGn&TyE(dT@PmnKc~=Pp+*!;NO+X3{MPobk{t_=u9;)LQ*3TLi83 zTwbgij~l4OHQ$jh{iDTfw5>hHbt+2?*#_1zNN$R63(&LMh;;%*!jsuk@re0L5s=1e z&fgT+x0~Q{VL>8F2=6U@ZT&K|%*%c3J|dfR>h{7QLjX@=P8gv*Vw*||@4suQuI2EN z|KsP@=8edTj0yO^28Z2UElnN&6aB}Z)Xm-0)!p0G(h7-_mz@g$K%%9UQkF*ge>%X* A?*IS* delta 19439 zcmZs?V{l+k^eq}&lVoDsw(U%8V`AGUHYc`i+nLy$*tVUy^ZVa>>(#6GzI3ndy{pfM zKGnU}-n*ZsP-cFi)XhVJawRgiQzw2v_5f(NbI4RBWK|Rz5N^V7b{gv_zZJD)(qxFK zi*onv-60l3#Wy+ue1jr9X-N2iC@gjQ1ZAV8_d&tA49J*!_#&xOgZGi_0qe_0&#KJfxR%}N=_cOIW-mg>&_crOv*jA)RsgyG z``3Ji+rnaXO<+2X-lI|DCu3?wR1WJpaISYY=kUH$JIK7jm3DQ&sWk6`CWcYwfP@-u zve$ZtPPQtOuMA^e1hz4wftO5|c4{826|Zs{jS5?$o+);Bp7nSU56@J97GR6o426aw zQY9^+MhQtJDk6d|bR*re$eNuN2!Ponhx<<3b_tPBVx$NjaTt@UCcR)|a&aPt{T`=! zNXiX$Opz0Hl7SbSa6%neZH5UoLVx>}3DtxRDG;ZwkZsYL)D{}5rADNfXDZg*#-C3P5?FNTWB7 zjGFr?XvWU?&4es+FHDQ){-TfJ`iqghQhc z-H^Gp565kL8r|LQCkPg~KfofnWB7L-ncW|OcJwWuHPxA8l?uIZP9K;qH-0Qu+AGx4 zuaY-EE}rIh^V&R{#zy&yhMI&16Jsigl1e)!zYzNN#=@M6A#?X`VrP%)sHQ(9Gb^4F zHLCZLDD;)6bJcV+p-IJAsv(ptbv#3LM1nVY$OosR4e)AB>8T7W4p%VP1@2 ztcvU^uBmTZ6YmJp#{RA*qMysvL(dkX?{XHLVM2v?ADy#sYzdDGg-%nYufG*dZ+j zB!|ju!(AxPi0AQ`rd>z$oi`5MRjvAD1s z69_dp%h3b-emi{zF?ZYLd|rHNY2@YyHDr^O_TQqEwMagwj#oaeZq4c+_rG*{Bc4jF z-^k=#fw~7w;*!23RqJC37iK7WyupQNsKiV-u}}%412hoX-n;Ks(9%|wZPqxI`IWg1 zHbueilI&lODa0;~8YDq4uOb;jNzObhSX1wG^UH0zeJ3-Mft z`W*5fUQZxF6W{s|jhoODQ6|8etgWJ+PidB%t~E;Q!!YEkf!yocZ~iOQX@>1-Y3uVV zxneXE0B^XVVI_JjNUCKGGsMcQ;)$Np7@aizKAOh6$W2{Z#jEv4hMd$#%_S?e#L^oP+UXTGN)nHp_+T))@1n zD%rl!dBUXMxcS)DD zHmh$j3Z{-;B(lDp#qk9zH`o@S3>t~R;^o&}w$|X1rJefpU5riSc3GB7vK0SeJ+|x{ z-~uAP=uZ!lZZv?axRh)sPn1UWuz}xYCvY` z+b-A3^Fkxc-`Qen-Sd9bQ;&sxs4j;NHc?+Y-4g9G8TdEF+z0pu=hMfF_p@2OHzV*N z^c0_)#eQW%e!M9^By=5TY-IW4YS@taF710_yx|Q14|Wa4Sk{hh?|U}!4GjtT?3U0` z^s1y;XxdL!1S0jDgN>WP+$+Hk=q;35|Hg47`9)C9L`OxmM%w*3L{PT>AUHE8YhnUB z4KU$E+WLL-oHi|6)sfUgkRtgvS4-tyMbWQ%8m;K=yrJcA>IhY-ahy+Iq*-JRA(uvK zWrEei7_kH*oM+do_P&sw9=`Vl_hq>>G7&3*_O5g+0p>+inxz9Lj#rOqh+Ry`!lWEP zrRE4TI_K{NAji4Bawy*ao!?+fwRd~w3+T*Z--VvdE9FDWHOx zrxs6qv*=H3My(M+>9qM_T+*F5cjZ@BO=G0WONU%jZlrAU`jWQs@0w)>=LOAyrpP9B zhw_=fzcOREjawJGbDK?CI&QzLUW*KexV{N;+#*cf4!bQn>qn@csKZaF6*oNI^;GHW z4F+&746p1{YD%)%l6y~2OuJSdXamL)Cff%PXK76GRTwe9+N#V8>!pm9xeBbj1%EaL zK`X0MOAen$rs!K3Ck=4@XaYu%Ke$=5Nx&-U-I~*20!DYd2EH>7H?x#w4FnNj>a4-4KVpsyD~ z)Q?SF^KYENi{W?P2A%ZGiW{;>KgVZ_I&AF3HE2iWb=jOKcivw$$E3X?p_#}8+hx-hg&2l zey9>bM`y$)3BjhvamSjLiXrldVjT~PL5|5^g11&@v}VkKe!!awLI8I)DTM**lI^tW zQ7EYWCixh$5Cc9!^!|wW2pF_x?{vie$sXmz5av}ukZ98|L7K5x6|p+CEJMWUy)NCG z=CCoYfoJ09QzpMSUxozd7YR6KAbIZc0?-Cb$U}~oQ`)=eK^X?p?n@iqeEAXWEeJCfXrgaAX{4Rb8$45iW{N-a6FhN*pxndUzB_D62ou5Yue)`m+3wSad= z+EQ+9Nz|j>aiZ%hlb$^1;ifalag+#eq?)$krBz==7Qa5U{2N{&xP-(Qly#e@ZmZB= zw(X6hn+YM6mhX9STsOKU(Bg3q)Cs%1gbCF32lmPO3MasV$ySuYepa1AS z4MXfM$#Mh?w6+zmMiPs7ox4Fij@U*=-zG&qR04@1v8g+9-pL{Q#&d=FOonzM7CRE2 zBP~VAr(ONI*c>94(1{?RE|u=5YPn|iH!@CX;v%?Lr05*1%%Wke{WJxkx{hC?TYaU( z(F6#u{|{u!pVHmE4+(=%}?F_K}J{XA$9O)v?E z;^mJ;ze#?yXqU<>c{>K_KP{TC)Ir&SFc{GFluMo&BmrL80fdTt@N?nm7D&r#vv)|| z^jL-657?_)`CNobbq9{6y@uavb?S;<`>D|uZt28LrGQ_CSve%*`CL1OLs{8WNNZ#I z)JPEI(PMIWWT(aO1>WPz%!&+{1N}jIHbiyxERmlF#!R9q6KWxjzb0*K3H>Iq$8fW=`&8V^#`*~; zj6Fn~DFLmE7kvId`uq`)S99ID_8LClbWkQK?mf?Fr(I{%L;FhYYVKS~%Xay;ULF<@Prt`$r zjxF|yp&ev=r~%(%-=dhr`F~O85Ou9)8b@9=c>~q_I@OE^Zd>0Z6nW2k2Y&3&PMMnE zujPyvBJcQ==J5vH-zFp}e85o?;)=f>Pz_UV`i)a=5gp3*&0Z_I;YXWsCkRr?@7!R? zkhx9k8kB~gPIO1~&=}x36RE<;N8HBJDxq-To?H_WT;&Wd7j83k^kh$;cNZ@0>vgaH z`@YTMs>K^kBj^ks8Alxm?V`eYFmCgHDHhoc?lx4MX5tk7qU z`ndz4m#U<;X)GImHoqI)+{Pkq|Ma9-00X23C2aX+uv@+tp^M$`b|FBx4vICV(`8vS zfd2r0AB_}$d(XD=^Q?F+>7?=tsET$uS9pCY`5A?>+ne3xBPU<=R+aqH{Dix!N9r?b zJz)>Cz)X2+askV0bfWL=^dorN|9xsL5&!p#d)1W&pzHYh+vEXqy~63bh30SaZwo-C z0bg7opg(>)oy9g4JiJly`26*AaC1ZOxPzx;3LEd%`r_rYTZ5iDU;6#6KcOJVk)pr0 z=`Xl$Om8;-N1-Y@FUq(x((`FF0wVL4XN?u*8pq|U1V8Y^=yF1FODM5P_7x+)?_KHi z>CsxQuR)Hl&-eP3sau+X6;kU1JOd%b8?7=nk|AHld>vW(+v@bx$}cpetBKYfL+b@9 zs=Yphmo^<0Is`Q(=S;Z;%{(=n6W_N`Fb-lGKae_yGFtR}52-4WFD>`{=cHQPU)Aks zTC#;pC&PK=N3$`C2zH4R=NVCbOI^>}f-05z;yaX~7o|3*53um)#+3eQz5y>D6(4)M zj*XU_TpSY$fMS|b4mOqw8`A6 z6@D~HS0ra}n+wU~l5WLk6|_qT?!{~WI{c->F>*=-KCyoSMS|d;xnG%zPeUq(F*2t{ z)pNtEndGJ$vo=+7-3J4>kQqMbucCE5HVbjikLzXMx~_&{6n!`dlqfEn9?d}&4R;7S zF?HS7;o9UrYWw0`Z^#>7$Yfk?KsopCAKr*o8o-S@ER^|0I&LsHsMkE;{xI(K0Qk^) zO*K_m4JXn8H>$l=>7O5dew@|itAkdadea0NQxPhgNSnHFTo(Y2N)Iy5s6w>a`%84; zCT~*e_sgPyUi2QVMC4gKbevHL#pZo*a^=G{$1oS4^_H8V5aR3?`sb85L&t(1=l9k# zzg-t42Euc%lWokGgRU+e{Ta>~-&7lJr78i-*J)b1#gMh^+N0c~90=S+y=_%{NS5e< zTY;Nh*PaFuS7JbZT+RgfX{DGby)4^Ai8=`_A_TqXv~>AFO~JD&Jw(FR`S!)ExvLg~ zt9seHIg4Mzexs`7c{S88%%#`1x%aGH0JC||{@6f{k*i|rwKQtSI^P1z3#$-LtiiQW zeC9XGN}yz0sz3CmIHXlfBura!h!wZU@8tu7J!QS7i2I{&OdS>YDH-Vh)0WmP{JFf z(`i%>8YpX6zQxvz7tSxz+tq)3vM&`2USEpZmj|LFIX8!N{-B=T?>JbCTsa#(@-R%I zVY=yI_FTwD=MeZep07Yf`sBpI!mkwYAftbcHIUOaP|*GL(cRwi$Z65#m)B@?{iqfG zLUQ9W@X?OTqrYCcwe3kUTy9-fKlIAxa=VX{tf=mtN2bQyMC{{0A7AoqXrKwSX6VqJ zIs&3jOLQqY;R+|SH|1y1+1k&tm=+QLf`e`!SbEm1=0&;Wh=eon9kc?K0MU-ZQ?#06 zGr3zub@q8<1Xir3^`|%yJ&>R>8QiRq_dFMRdJhbLTy zOF--}CjI@vp0Z%}e)AVL-DMZ8yi<*{Vy~yR<_h{#1PcL zrM9f|J_l;gyT%RGN)-x7jCqBVR0T`&)KA)2`$p$ksVwZJwO^!TCNsSXoFI`zkjhOX z(^u+;#ww<0Vw^o+Z=fG8zPFn?rL@FpE86NSgtWA@6Zrcw8Y`7&n-{5Gja7dgxl=P{ zFy3GO8u`BXynel{TyJ@;dSa#>D&D>TN7t#&obMXHoZGV$sY-GG$>*s;Rd zq~WH?1ki#+QItPd$6G^GZ1@5A`<}^+0#M7*^7oSfgnnQd%`>@Q7KJiVW`( zwWKZ@P$cGqkGU~V=^*aFdCQ;*nvz);;XA$4%)U0RSXZi;;xzxl+igr>2L?fGrNRP1 zOQJ<&$1G4|R1qX5gMUK0DF_Mz<9-^CzEoD97y2sQ(Sr97TNAf9bzO%nv|xdzDW54l zRdS1)%#R$>?Ok+GDRp2j98n1C6i^YCLQ7pYj5)^-B zdD4od-I|Vce9(ikLe2`Xpqj&$O);9qt;~NXD8)J;H=+%iC~3#pN57G?k(mYZCA?&ZyDZ8wtotR z9-7-)LP@7?;AcBXbd)2Xh3F7w}O;LOd&~>3^6{ ztl_RrMlq2vLGP6hnV~vU4_xFZn>odIdaVS?)8`ywU+0Eq4t#^tX7zyUNif7%=3l7X zn4CdU`M*KgEIJ6yz%0u)qpjnQ9raf4wUK)$YkZ;JYaX7>L zl$D;uZ%^YrL5`~98$b=*{%or%tP?*)<0MV?NNja^^tsfG{G(V=c#437CEV-R5| zNuHrCqTl#UN?#C3$hnruuI((p8j0vSyt`>BlNG+z^o| zXpMS~zb=1aW+Yu|PqjJ0I7BDP%Lgwkl_3lmx~UNYZc*gybkF3yLUH1xCls2!#VH4F zoJ>DK)=!VV_-3b)fuF1^$5Xn+bIH$PUcxn%{%aMxe?R@YU&T^snoV!FjNA=Ibl5Hb zF~7~cpju4M+uO@M2K``t2AS&TTTXVTD`a?<(>;m@pWEF_x75&TL1p{YQQ!9A+xgZO zB^x#1dL@^GIafKBo4ix6o_^ZSog4bpywhfPC|Ym2nWd3%zfm=Dvb9qU-M#Vm=BC|y zTaVt*7Mt{(o~$sqqLYTqzTB3aE`fzsxQ&*lgEG?7Rb)>sdz!Z}`dc(G z*)^UNZ<=zfOor=|5=7I9TdwVvqWLx=3&W3Xjl9vYs%d0f=%69Nc7_dzc@{zXDV4z5 zN3)gz-G~L}V2D}po}ZIc8^Nf%g|v6bv>~c=OoN;Y*|@2Z{6F%mN;&Au+SKV!Jt~C1 z%(zOH;ChnbI3N@ay{+mEq!#5WhEM^d%^R%HV3q#%}Xv+u3@_O{eENLm` zhg6TD0y|`pixUS?t^}j`2n2o|29Dc3x7%EY3*xWH5n8I1xMM~aIcC=swvlbZp5edp zUkbl)-_}+dJ}OGY*B2{a?oEJpbg+qZYw(4?7h@PfP}*A?wTjOV-t`L2&?9)WkqlEl zW%l#!n1z{z%#wpY&9#0^6vszzf1fiavo7^hBW#|qvZfgzKBsFYSc8+ULfu&uzTYRG z#zcZ@+Kl4@d!wY{*SEo%Jg|5Ve=W(s^W6uZ{i$-b*AE4yO}<;-_RyLxx#M?Y&m=eI#<@(8cA_9kH){0kA37Ez-w4)n*;Nku=LJU&>Ec+Z*g z$p)aE@gx@aIX9qdI}C@|(p0FRSRGh%f{vZx{d#mx*m&-#!%Y|*K6(nDi+m2Y#+X7~I(`Xh5)Cc7=n#fqwd)QEPG zny!iuZ58sdPNao~&6!WD&35YxmbE`g_J8xM{};VtXCY!D`j2Sw@xd|5{j@W8u^?h* zWlD_4qXD$!?boFcy5BV>=Ook@h`Dpa^x<|yEH;Kl+-hF@3Y5kIH zS!nx>aWWPu?#vYz7ne2G`Fb$*euAFxW4b;APzq3EQo=o7{E*3(7o@jOS;4G42h6wR z|E7Jvn0%Pntnh0q6YQa9xVEc3JMLUsD)96MdVvma{ViQSx@)`h+8m6>f75VIAWg;+B@(d9%A?pSzi& z4FO%6CyB(zap+QhKOaw4ZeGqaI7UI5Lfi1Oa+Yo-Ro00z>qpDGm4i(;$U02=X&O3Y zr}pUrQMOTwh+xH4#muS5?-k+dxP}G}^FYpu{WgId7K>x%;eqSJJl#;%J%+24^mn}l zWNbM|CQwf@>bJG=BkCji^4KM54!#|bEU4X|_W2_m85an&HEpp%+HGMIZ#QXk@(#(>hxvFi|Q z=_-7hM=Wehy~`OX9xBp@#v??Kt4%*da+6AEBN6tE)oWag5J%(#kr zEEU^|in0d%Ex%5Knv_;x<=F_?UM+j$ADdKI&eF#$@d6+g=ffZ*3^|dVbx+GhFym z92NYP^9`HiK!fuW5~&GYlVDtkS>M36?OUq-LK!%$QKi2w3|rv{G4=6s0vOJgX1}e! z045Q3QEU5?{omCaLSPzl=Kt?B<@hg=)+&Gmih=;j@}F>RHO2uYg#qK>`X56E2`CXF zD98U8rWrw};Xs-G2iNg{ioyM#c^GlfC|FR||CulS0g4LyZ=wGdozei+Lj-0150?u3 z1O-by$AX~!r-%FP@Hi2=uQg8o=r@s&|3XE9us|)Mp9K+46kYFwrrd0iP_{p-iaNM{ z=W_d%tv566!Oz`9Wq6^;iWfmbNJt(7Zz+|}1U>$J1@8~U3M$;U8g?nmfFj89HY=p@ zFWo?z8ZFV}XluA6W$h%nfQ}p%FH=!YZfKe@SNOMn03I?$_J(VK}a{uP({7EK5;`Rv;IxhS>AVDnYq4k zSEXPk%OImPSdBl*WRP|OKtenkg~5;r#e--J5gX=BxbIfAFcTI9`+*$8&X1I)9aSQ> zUhw{GKoz}~)(+Eyg;t+FQZm%jdEng4k1|Ae3=&}tJ+!?tQ9&|u1dbm~)Zh3vpuIiT zdwKKxmi9)B`Kjk4mUE}Y*ukRu!VbQ;o)@;36Kk5_cKIxj|6q(Xd`FrE6XoNIn+`=z zfGLo6byHc-pVEi#HjkMuUfGxKfi(6zn^+|s0LRCu&TJ??(~OH<%Xt|Mmw_`ZVwl8w z)n}dIOLNl<*Dvx=1Eylc7<}|ZqnNf{z!s$ViS$-)M*{&WfYB#neUGG52hnQe)DIKz zJ-DAQ8q5X991}gT-LN0!I)sQSh4&DmKlz&>S}x}sONeC1Z2mW6$dzS$?_Umi(J zfQ4&n-|q5gGX3Jl+TK?8@4MHq*2=n%qYs~MExKI2o(-2v!s%%P#uVgb%-$LXeKJ$Z zY`R_@zh0l0z*#BEsKdBQjm#S`|Hw$9a_xszPz|NA(oAEDpe@ebt?oJ>jFjz~+}goA zr5CHmmBZ5`8(#wQquRmGcleLZP1I?0fZY>D)@9PUT*;r4Jf=AYTiJ|FOs6SD5LcNU z>|@~~{`G<=dIF*jJvflR)tPKI8m8ta8(W9%Sm6y=dh8W|Fp=x`1sMub*qr15W5}L#<)YCqWS;T8lfX2etgKRk-PQ1!n{Dau*vG zE?t8_P!%snB&R#yPQRfi3$*y}X9={#pHI7P`3FptVxDWvs5>}o3iv*7(dE`|*$+qy zIw>ds2J|V&%yCPvNBC*=xQqMucD|msR*yB6beq+bl?F3E2&=Zk7z)lTH?fGlyu1S3 z`-@$QR|0nNC0t3|b-MoO>AeDP8MU<fU$0M7H1iCD z2N4K~luVPcNH&oW8ZoE!>oH5eg}c|7TBA{Ma4jq)<|}?^^G&OW9=OplCpSpSh1iSB z>EzIOYNLC5fh%pyjYSmNBM(26Sv-W?Wj&g92IEDlFtT;$ZFc)7br4eW^Zejo{;^!U zL=d#A%>V6V{ac!LMQbH+Ur&I(b)Q#c!cWC^)h}Ix?IJW|iH6A_*(vd?rxNQHBL}Xb zVDAs%=2Dj!**spR4|QnGu$==9)6` zPq@V`qI6V72dNOC8a0^GTQ?00a}xWbX?u|j@8wG!2%yYo0fF)HR@6g$yT9d z1zv|)kSFauT5Y1fzlvpH&+A3i0p*Zapp$n}XX{?B(4-yc(oa9lrS7nnZFKUIN%(Zu zE}#3zRFKp-GKevjA*J+PG&_6{`F74TP5m~)JC7RjX-cSSFihXRU_>}%D@!H*7gZ`} zamRp!t2966unZ}Sr-x^~YOk@?9LVia_iWei-Ku7eDVLb^ruIcaHo~rAJYzpomchv zt=K7Wuj3R8GLzR+Bfk9Jg_uu-o$~w%cy`vlDqJM#G$Wq+*C+zi$}Z5g|EBQ4!t(Q% z=3`_YIr1-}2^QlmsDPR*WW(&&@iLPzVSoMojQh$V8qh8Hn>+fq_wW36;%~vm?PU5LRPU{f>S{=ruoFbL$-?@(ahQ+dn5kXng&z@Y3?^@+j0xBeRg zI;n>=VCb#W5a3#HsbAdxATKgFJz6S7`9B9=5^!08)OHAP%+_r~a6ZCR3S@AMR%U&n`<|V1)#Sx|>+UUb-%}PbisGX&qH)?9 zM69^57^xEB>>xIsR~ zE(7{-m0(1LAfT@Xy211_224SXs{#`YL-v#hVyKc9Z#2t9EN!3t3lx_@+Q~xPBBj+W z%7qcB;cSI?VZCr=Zjz~^$@(M{1}s$d>j#`@;9!RXsdhyB8AM>WP5QuVvM}X>BE+Q3 z5jUhscc=TZOAON_;K0wEgY2t!At{^kl7PTf1TsbcAA|S-yh!%xU?}B9R&fDJNi;j- z{Z=xF;?pn7fpSF!(O{}_pn}76TLBDm{vCL?W@TKheb^;wA~0E{w?Fo?^Fai}Q?(vS zL>RSW+;5`Muvm+B&_AcxxAOfAS*5<`!F$9z{Sfkg;KW2hjwTMN+cAwqSB~OWLouv{elZaGG^o|^+wTg zpcKNah!>$s3*!qB;}O%K3Wujj5D6+#B?)QaTF19TQr3LHwjz#b#3>JN_6}!PzdhITK0y;0?XQqlj>kt}Av;-PNt8pQ;lXeCUf`_7!gg-jq0A~VekEjjp z+?>Fq!s^slAJ~WIz`8;E6G;(Q1Gb9vC((ev{Z3zfC+okMz8okQaR4)1zoP%2#l=7b zGI3~;Sc}E5;L5t?7(5)RbigSMF}#oe0C(D9hFhf!e-~QQCV@LBD`3yvaL@*e&=8Ay z2-3{DtbW)i_(AcfuzVJ*xMm%=7}hVCmta;`^sg%HX>DHFK_zJSAGtv@_)e%zAvDn} zpQ~0yPUcf{yk7{^M=mbM!RB7en(K++dFVUQ4s$7iIbT`I)OIL`%|Ih@+4@^LOQ}U0 zRyY*Sj3WIF$Ti`K00hF&Onj+l`kjKGuOzR)ud)PSx`H6tzGw?7mmK8z$@rq>Jxf7i;+0v z&5|BBtFcD#K!OxH310GjHI&-$yl&=EkR(XWyd?L8{r(4qk5$l20T_>WqMr=8-*iHf zRjrTZL(~>DtCAuOry9nW*o}Qhcdg-n!&6vZWtrFIXqU|@CIf8(eXgkJhIGF*y^NGX zNh~bH>*$0l>mZHbgkvJtiza{a#f^H_sA1P67>exd9Wso}s z<1TAbCU|lNxjHZ7!qxzsW#4#Snl9uSJlmbC^^%FBzFzC7Dm3JBif9cJeX!u7AF9y@g13pEN?M&04`qvg^cmOnix* z3YGR^_y$ER?jNwxOrzf@qr-}6hYLJhZhr-0tUau?8&o}Wrdb@P%O=1@!k@4yYv$ zg823VX6Q*bG0N8iLmV$OehBs!-)@t|E3k0t@2615Zt~&0>Gp+$(Bs>Uw%Yb9ukT^d z39JOT?PJj02;twJwYUryPzF)VC32Z;CbsdD;RBC}d=WTMGg8#|x||WXyw1Wyz?n>I zR19>-uSC1&^~?A{B+amF>kavR{+^&i`__f4BjT>M%pWg9ztuq;ytCK@?7OyQ z|8O1+et+pHgylxxE*1!T{zJRJzL=X7`>L{wfwRrOl>35RIw5CFFeR6CqwDTQh?R7L zv5lVK*;rZb87dm_A$#zF!^4JOA!C*7 z;@12*KZNEeu`H4uB^8du;6w{p9@eO*5Gmd55NWp;iSb?AK` znBn5f5=8I4W>vaiJr{V|Wh|b@pDtU=kvJF*f*y)oocR z?fmHE##?@S=&7S1IIw0{`+-gcQQEgtqn)#2dn@Tw@|^JuUN-lH}i(%jN3PB}67@TCFoDXzr<8 zv!8OsubpsGqCeMSE5Ox7f;ug`ybj3kyiU55OdV9>ut@}E(o$cUQeziJdTulm=(JOh zl78&|sPwZqUwwT1_;OMnnXBm6d;v0%YxkiBL^`}YTAt$oK5AX$ZGzlveWA9Tu2uA! zEaA4d8Ew1AEy>CT5=4!k9;kKb1|^`K3gQS_QyifeOFlf+CX$Z#Yt0At1OtLMF12-Fa*)+Rhh3W=y+&lb4rccVcJ_a~Zzl1^jg70nz49T{H^gjT=eX<69&9_s@)g zx1*nrV}NV{;&5M}d#=fzGri$hW*WHl?@4~P7@p9`I4>zPO9ka9Kbp!rIIDM40ovM$ zjxI;XKS@ARe9d7Mkj37JGvI`DEKc_&8$PdgzmqAnnsV97?IPxIcV^WSjHKyDOE?ZJ zO?bvG{&>9o*gQR*t0kar!KTJYKz+GS|FkE0tnks$Pci_hl?}sk6BV0GZPDqQBwY?* zUuMU1d2G7%hdvp%=T_uZtDiB5-R^q3+3Tt{TT#sTSMRY6!~}(S)0|MZIgR9g!phI< z{}Nt1N1$&6twyK&6{f~-S$Q{zF5~U#wUTr^Mc=nHX_uk3;K!)(>`$=#G3n5*lKuX0 zJ|jq|XBv8)t5Bi!-dl(JIfvrFPlf3Q5rhz6)zB6M;>fk3jz#Aj1_dFyu9|p$bX1}X z!s0ONG$*tQ)EXU$CXzO&^wph)h9$^v#vD}>=@@j;ArpuA(Zr4K-{l^j-^stSk}XQV_IxPjh!k>#9lZS^ z$261tWz67iH=YUg^%p*J@xu@MZ1Hc*a`_x&CZj$8qySqhw9(!1(ZCX`#+3H9r>M*u z$y4{oiWhMrZ}P_iW2qz~vSd~uj2(?NN(mlE;qo4%DamaVvuAvUt>0>C!#%pQR%sAAb8vTGdwgleb+fW%-f4 zx2yL(qyf~ybtkEQC^t@4qz_Y09BX-BeHT4}vDjui!5k`H_6Ek@ zE?%!sCo)wIN;tJTapSerJK?r6KHa3T7AYUVPXleBbI9i}ujW^theTh`mn&64b=Slo%M7wz zun)iJyxr}qYz3fcq#Krqlh(w@A4yel{Hkz_hn5Et&EUp{q+Q%^2jgT!Z>@oLOHG_S z%W%HIms|d;SxRe^GB#0cS)YN2D;d6)ZM=#`sEDOR_2$b%O=Ax7Zhn7S?(A8{IifP>j&c& zG*dsQT!&#p6Z1}YvDERfaMl9o_ZN8k#YMgFV=->7U>{~$y|%Q6CV#t!EH%t{`?vX4 zdd8SpR6ud;#ksSoLvJJuFao0S&ExG8=vRwB{7TtPXOY{n>_!l_XwwEBk!XU$&a_y9 z4LbTNm`862jg2_*Od-94w*~kVB>F^7`edO$gA9TnHD&I~5IUqoi+}@W?GJHUh;X>R zh8;55z}x;46s{NhtmSmSHhaoCGyM=M1`+BJBDcR1BQaf4%Ss$|n^c-MLyYhkeld|p zF2SV}Z;a2#Me{q)i;nqWi!6n)_u!8^tp`H%URFFmK70HuHUYoA27;--ivo2bpbjLj>IrP$x#95714?xQ;$(;VWMgj%x^c@k2BNf6ZVODh zyh>*`sPUUuqN26Ly-QKLlX;xs@Zpi?3+4Z)>ysU_S77ERU^bhiXS-QrTE>)H9OuZZ?1C{aMuqAnnC7!vHLFZn@!^Sv$zmmW zgJy7_Ox+5v(y{;*F|#flR*grycr)DQ$&I|>4I*z&srqD%qfn)8(`zW{VNXvh?jxwV zPGK8Y@R@3sbO6`%2r8L<#+Mm=wNn^j10@7vCN7=Ui_?Mi$E6y&N#8wz2SZExL_-&P zd;6_P1gA#0?PuKGmY~m0E)`igo~+;Udsun(B8ggBBeOpLO?1rt-Qs@6L!U|`B2Pv; z@q5W~xsJk9G(0h&5hYDL>Fs#VnAztw!VnCTV*t~2zJrLj;I?K zie9@y6)gc2bLno1)=U^A7AfIe=Q+>WJZ3!>YlZVnICpxD4c~gfHQ`rNH%8=zT?Qu!_Xoq~3R_$i6B?1|oWR;h7ER(L8E zm8SYMC?r@|=`WPSgiO!>H=o79#GLAf3E@x6%)!LU%ErV(&%{Q_#6(FA$0%=aDrV&J zlZZ-;hlPoSnVISTx};KLLBxV_uqUA4;;?bSF-llEIlBb0v6JC7%5 z;(k6-qKH(?u=^o{2BbxI19BpvL`~n}gsX!&l!((qa}hw&8ur7Vk*PpHVNTV z83jY5H>g7_!Y#v6m--_UB=h!#5dlTQD*ofe?BQJ0MUs^}=vK>bU|AS4(itowme)6+ z!C*mNcniueJMrl0PExiRQc%*eeb8}D-v*!~4(N$bLfG%Xx-iyc$&L_t4o*RX6X;Ar z=3v0rSlxcT`1gkoSa>L_3kem=>{9JC_5Ts-6NEVeV*)#YbwVl<*;B|15Ci)C9Wy9! zxQ!0Qy!ZsT=Fx)H)ED>!Je+SIPJb`uz8|__$j)Rn~ps?Ohb+?S%ND1 z9yeZJ4Td(JHy2v@kR~ZjSUQ$tv|2IDLCJMBs^;oHAVK$`txVMmKU;8YW!b?w)~c;0 zvDqtBSrHejgqIc+5o;HNgn$b6_;I@u(Ud1*M8AJmMoRKpZD$y}q6Q;UT-h!xTgmch zCC{CfuiR%Je>0TM+huYCLJJOrPECJ`Ss6tQ9!k8m&!O8~O8&HCobdQ0msErn(yto& z>QgMUAXmzkr3Ut<25pHEt$GI*a$19UxC)Y=jZZ6`3im-T@13b(=K!Vs|0>)}qoH8* zFm7#L+aQ``EThnCi(xSKU4xKiNVXw_?AwrKhOs1BlYQ(`)HhC-rt>^9_%8Am#(6 zN8Bu3M=yEJY5h%BJ(5F7^rsS_2kWZSs-yvzXR&^hqlO z>i;a0v~@iO8r+j$GhLm1$&>%S*6s`hrUv7_w4a}lp-f~QypP!4KGdMUu1aHClsFg z5HYMXlb(t5B#ayYmZpw zYBVJ6SqbV*V75p;1gI`#SX6BYdNtNC{ z*WX9O=ClYkOW?spy-s1pyLsig3Pg;*$T+6_*(JS3yB)4QzI$s;*YI9#b?H~OTPaF` zNo%r5w2NhkYD1xB@}NjZUxed)cATv=xX18@6Z7w#LRWdguIO`PA2*ez@Mt(1!y!Z1!M8s7Eyx53< z1)%OiJN>PH`nTA`QK;WS>*>(0QQFF2sKo1#)m+WgAa#N__}+dC!EYnGz>|BQH#9DK z6iRzkd!f^_f#pkNq0=j)UdErnLB(}{#P;YnC-I2lYw{v-XOaw z&>1+I!d_8UpifJP6rI-e#DD7{Ol^mxga*m>66OUt>viX^BqWOlfqIR41bubSzmiyr z)wqkMobzXn?TR8lxZeMW%DQpi;xlB};5jYq1lGvCH&_x%oC>(s+NFE-k<#Uf9H~Cc zG)^vl5}{NQ(*%u1nO9P#u||E^L*-AeA|9?wKNmgm{Vj2uLDX%{D0DlNm;6md7zUQU zc^6q#L@A#4x#*G1S)U|1`Sl87-$-(oj$vji+@5p3&7K)4uZSqVh|sh}c-p+#+Fp;{ zO|ku$Si2?UUbWH4E@E$u<^TN??-q>d@{!)}%=^gs$7Z>@!RT#-kdKIJVC+z!G^Sl4ct*olWJ`j$J7&UipWru*fzK~PNaw|JS zT%iy(2=o-%hlWO-LY32no;|m+MTlSMX@dSInL=E3;i~2)8mbz`rlxRpV<=oh!&uWy z(@fo5P0d^b3WdV;A^-oxo%g?DhN=Af$wC2Ram^hmLLEa(7V@M~bsf0Vx`c0TMBVgf z3~U+lbc`<-*;@i?Gwp6I26(%XRj#-(C=Fn}0Jo@`nY0Pod3O>N8opNhi6T;`VgBX& z*gA8R8IO+rp48tXmAdN*G6pH8u5Vu56?Yl(zBFo>4ju@;te7miDAJaETIc(>`!O>@ zv&mxJl2rMr^A*ceaPiu#2u*`5y1a|fpY+k?r~|+86rEi9d7sv5zHaQ4?HzzPNk;6o z{uYNjsT_XX8ZwS^rT#TW^cc?|J4YYLdKrkk30{&A++o3y0Hlmmoxbb)P>?zaklK$r z-zI^s3gdXNax)y9O74?zQ{IulEPJS=gWt3Y?Xc6~5rto5Dp-!f0pWL20dJ&t1a^SS z!ti@KDXucQ96-8XST|QLwvHr3N>NFUj&xo*GVTJ&Ayd;1UtLUW?= zyTb77OQ8qc%NN2x-S=E&bph*Pt4goUZ?Jw1?+$b3QpRSHkc(%RWmGV^4xd3>$~Xnm z5sA~-OOPpmPP=~ovB!eFl1&q1eV7zXiZedm-6Hu_ggh}w6q-vRBOU{smR^J%_S~Is zCWS1<8OsFn&Ecjzkp*+%1e})fxuG*598J{vmAhE*AYR7NOaJXQg(2uL?GgOTS-<+L zkaa~BW#U}rGH_`qA2Vg=fzx?VE>jOq1lyRIB;jp-i8a+~86k-A?58;xKC+Wda#@5) zjqLb}u|t~LD*TurH%|0}JU^y}ST^3_p$(B-h6kZL;n~}PK_3@pM#+lPNe#>9I^@b# zxX7i&cY$oYIH_jghV@D(PMsc7spJ$OdY{y7Fc0DmY3q9{O#Jf6E{NYa*o?E_ns~3qmN1Mg=);0_+ zbq1_t%=MnDX6+7MHeCDtlb9yc#KE_V6~LNk{EwK;g;HLdL2Whag$KQXUI%>Q*A}$Z z8G#DNm@h|B7m&^()wA5lfyT7#&^lJ(Q?H}%r8hL* z;`BdpXR}&LNuv_AZA^AWQ3>Z1urtYcpfCQVA(X{C%Sry00@_hc8nC4wftV0 zIEE@J-jc@8%8$RY*jUYDCNpXM{d~zzOa1^AM%~lH+k_erZ@vd z3yR#z&l|3Hw|DU1cOp|{Ytnt6O%g9BKT75;#CpVZSP{Io41s%;DV8prIdSf4bh0cl zSF=;TTrUpU>_@EXogoZ|wqg7i86aTBn$-U^f zckM;1W4v>-gaWcMaOO_KNV`*_)8RMU*2Cnpcc(3tKMxMdUOyAGneVRqg8-7F&I7d# zFQW`{gO>8eb$%Rzj#!*?bYDE43wUKf1enGY49T8Fv)9E4nOO wzCF|Ql?73UYN6uWfRXPD{G(m6|M#jOYNvQamP4FZLlv%}$}J;fW^2y H_pose, OptionalJacobian<6, 6> H_xib) const { - // Ad * xi = [ R 0 . [w - // [t]R R ] v] - // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; // = AdjointMap() * xi - auto Rw = result.head<3>(); - const auto &w = xi_b.head<3>(), &v = xi_b.tail<3>(); - Matrix3 Rw_H_R, Rv_H_R, pRw_H_Rw; - const Matrix3 R = R_.matrix(); - const Matrix3 &Rw_H_w = R; - const Matrix3 &Rv_H_v = R; - - // Calculations - Rw = R_.rotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); - const Vector3 Rv = R_.rotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 pRw = cross(t_, Rw, boost::none /* pRw_H_t */, pRw_H_Rw); - result.tail<3>() = pRw + Rv; + const Matrix6 Ad = AdjointMap(); // Jacobians - if (H_pose) { - // pRw_H_posev = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R - // where [ ]x denotes the skew-symmetric operator. - // See docs/math.pdf for more details. - const Matrix3 &pRw_H_posev = Rw_H_R; - *H_pose = (Matrix6() << Rw_H_R, /* */ Z_3x3, // - /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_posev) - .finished(); - } - if (H_xib) { - // This is just equal to AdjointMap() but we can reuse pRw_H_Rw = [t]x - *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, - /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) - .finished(); - } + // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b + // D2 Ad_T(xi_b) = Ad_T + // See docs/math.pdf for more details. + // In D1 calculation, we could be more efficient by writing it out, but do not + // for readability + if (H_pose) *H_pose = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; - // Return - we computed result manually but it should be = AdjointMap() * xi - return result; + return Ad * xi_b; } /* ************************************************************************* */ /// The dual version of Adjoint Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_x) const { - // Ad^T * xi = [ R^T R^T.[-t] . [w - // 0 R^T ] v] - // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; // = AdjointMap().transpose() * x - const Vector3 &w = x.head<3>(), &v = x.tail<3>(); - auto Rv = result.tail<3>(); - Matrix3 Rw_H_R, Rv_H_R, Rtv_H_R; - const Matrix3 Rtranspose = R_.matrix().transpose(); - const Matrix3 &Rw_H_w = Rtranspose; - const Matrix3 &Rv_H_v = Rtranspose; - const Matrix3 &Rtv_H_tv = Rtranspose; - const Matrix3 tv_H_v = skewSymmetric(t_); - - // Calculations - const Vector3 Rw = R_.unrotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); - Rv = R_.unrotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 tv = cross(t_, v, boost::none /* tv_H_t */, tv_H_v); - const Vector3 Rtv = - R_.unrotate(tv, H_pose ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); - result.head<3>() = Rw - Rtv; + const Matrix6 &AdT = AdjointMap().transpose(); + const Vector6 &AdTx = AdT * x; // Jacobians + // See docs/math.pdf for more details. if (H_pose) { - // Rtv_H_posev = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R - // where [ ]x denotes the skew-symmetric operator. - // See docs/math.pdf for more details. - const Matrix3 &Rtv_H_posev = Rv_H_R; - *H_pose = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_posev, - /* */ Rv_H_R, /* */ Z_3x3) + const auto &w_T_hat = skewSymmetric(AdTx.head<3>()), + &v_T_hat = skewSymmetric(AdTx.tail<3>()); + *H_pose = (Matrix6() << w_T_hat, v_T_hat, // + /* */ v_T_hat, Z_3x3) .finished(); } if (H_x) { - // This is just equal to AdjointMap().transpose() but we can reuse [t]x - *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, - /* */ Z_3x3, Rv_H_v) - .finished(); + *H_x = AdT; } - // Return - this should be equivalent to AdjointMap().transpose() * xi - return result; + return AdTx; } /* ************************************************************************* */ From 115852cef72bcf380167c9ee47720e5944c1cfd7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 26 Oct 2021 03:24:00 -0400 Subject: [PATCH 146/169] delete "FIXME" because `compose` unit tests check `AdjointMap` --- gtsam/geometry/Pose3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index cff5fd231..b6107120e 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -145,7 +145,7 @@ public: * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ - Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect + Matrix6 AdjointMap() const; /** * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a From e4a2af5f3fd8ff206b8c8963b67b61adc04e08fb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 09:58:07 -0400 Subject: [PATCH 147/169] address review comments --- python/gtsam/examples/ImuFactorExample.py | 55 +++++++++++-------- .../gtsam/examples/PreintegrationExample.py | 30 ++++++---- 2 files changed, 51 insertions(+), 34 deletions(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index d6e6793f2..dda0638a1 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -31,9 +31,28 @@ BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) +def parse_args(): + """Parse command line arguments.""" + parser = argparse.ArgumentParser("ImuFactorExample.py") + parser.add_argument("--twist_scenario", + default="sick_twist", + choices=("zero_twist", "forward_twist", "loop_twist", + "sick_twist")) + parser.add_argument("--time", + "-T", + default=12, + type=int, + help="Total time in seconds") + parser.add_argument("--compute_covariances", + default=False, + action='store_true') + parser.add_argument("--verbose", default=False, action='store_true') + args = parser.parse_args() + + class ImuFactorExample(PreintegrationExample): """Class to run example of the Imu Factor.""" - def __init__(self, twist_scenario="sick_twist"): + def __init__(self, twist_scenario: str = "sick_twist"): self.velocity = np.array([2, 0, 0]) self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) @@ -65,7 +84,7 @@ class ImuFactorExample(PreintegrationExample): super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario], bias, params, dt) - def addPrior(self, i, graph): + def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph): """Add a prior on the navigation state at time `i`.""" state = self.scenario.navState(i) graph.push_back( @@ -73,7 +92,8 @@ class ImuFactorExample(PreintegrationExample): graph.push_back( gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise)) - def optimize(self, graph, initial): + def optimize(self, graph: gtsam.NonlinearFactorGraph, + initial: gtsam.Values): """Optimize using Levenberg-Marquardt optimization.""" params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") @@ -82,10 +102,10 @@ class ImuFactorExample(PreintegrationExample): return result def plot(self, - values, - title="Estimated Trajectory", - fignum=POSES_FIG + 1, - show=False): + values: gtsam.Values, + title: str = "Estimated Trajectory", + fignum: int = POSES_FIG + 1, + show: bool = False): """Plot poses in values.""" i = 0 while values.exists(X(i)): @@ -103,7 +123,10 @@ class ImuFactorExample(PreintegrationExample): if show: plt.show() - def run(self, T=12, compute_covariances=False, verbose=True): + def run(self, + T: int = 12, + compute_covariances: bool = False, + verbose: bool = True): """Main runner.""" graph = gtsam.NonlinearFactorGraph() @@ -193,21 +216,7 @@ class ImuFactorExample(PreintegrationExample): if __name__ == '__main__': - parser = argparse.ArgumentParser("ImuFactorExample.py") - parser.add_argument("--twist_scenario", - default="sick_twist", - choices=("zero_twist", "forward_twist", "loop_twist", - "sick_twist")) - parser.add_argument("--time", - "-T", - default=12, - type=int, - help="Total time in seconds") - parser.add_argument("--compute_covariances", - default=False, - action='store_true') - parser.add_argument("--verbose", default=False, action='store_true') - args = parser.parse_args() + args = parse_args() ImuFactorExample(args.twist_scenario).run(args.time, args.compute_covariances, diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index f38ff7bc9..95a15c077 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -12,7 +12,7 @@ Authors: Frank Dellaert, Varun Agrawal. # pylint: disable=invalid-name,unused-import,wrong-import-order -import math +from typing import Sequence import gtsam import matplotlib.pyplot as plt @@ -24,13 +24,13 @@ IMU_FIG = 1 POSES_FIG = 2 -class PreintegrationExample(object): +class PreintegrationExample: """Base class for all preintegration examples.""" @staticmethod - def defaultParams(g): + def defaultParams(g: float): """Create default parameters with Z *up* and realistic noise parameters""" params = gtsam.PreintegrationParams.MakeSharedU(g) - kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW + kGyroSigma = np.radians(0.5) / 60 # 0.5 degree ARW kAccelSigma = 0.1 / 60 # 10 cm VRW params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float)) params.setAccelerometerCovariance(kAccelSigma**2 * @@ -38,7 +38,11 @@ class PreintegrationExample(object): params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float)) return params - def __init__(self, twist=None, bias=None, params=None, dt=1e-2): + def __init__(self, + twist: np.ndarray = None, + bias: gtsam.imuBias.ConstantBias = None, + params: gtsam.PreintegrationParams = None, + dt: float = 1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -50,7 +54,7 @@ class PreintegrationExample(object): else: # default = loop with forward velocity 2m/s, while pitching up # with angular velocity 30 degree/sec (negative in FLU) - W = np.array([0, -math.radians(30), 0]) + W = np.array([0, -np.radians(30), 0]) V = np.array([2, 0, 0]) self.scenario = gtsam.ConstantTwistScenario(W, V) @@ -80,7 +84,8 @@ class PreintegrationExample(object): fig, self.axes = plt.subplots(4, 3) fig.set_tight_layout(True) - def plotImu(self, t, measuredOmega, measuredAcc): + def plotImu(self, t: float, measuredOmega: Sequence, + measuredAcc: Sequence): """Plot IMU measurements.""" plt.figure(IMU_FIG) @@ -114,12 +119,15 @@ class PreintegrationExample(object): ax.scatter(t, measuredAcc[i], color=color, marker='.') ax.set_xlabel('specific force ' + label) - def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): + def plotGroundTruthPose(self, + t: float, + scale: float = 0.3, + time_interval: float = 0.01): """Plot ground truth pose, as well as prediction from integrated IMU measurements.""" actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) - t = actualPose.translation() - self.maxDim = max([max(np.abs(t)), self.maxDim]) + translation = actualPose.translation() + self.maxDim = max([max(np.abs(translation)), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) @@ -127,7 +135,7 @@ class PreintegrationExample(object): plt.pause(time_interval) - def run(self, T=12): + def run(self, T: int = 12): """Simulate the loop.""" for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) From 755484c5798e0d41bd225a2978e57b4dec171cab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 10:01:50 -0400 Subject: [PATCH 148/169] fix small bug --- python/gtsam/examples/ImuFactorExample.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index dda0638a1..5b4b06b55 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -31,7 +31,7 @@ BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) -def parse_args(): +def parse_args() -> argparse.Namespace: """Parse command line arguments.""" parser = argparse.ArgumentParser("ImuFactorExample.py") parser.add_argument("--twist_scenario", @@ -48,6 +48,7 @@ def parse_args(): action='store_true') parser.add_argument("--verbose", default=False, action='store_true') args = parser.parse_args() + return args class ImuFactorExample(PreintegrationExample): From 15e57c7ec8c5d8ada86346bd46e9a8ec82ff76b7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 10:03:31 -0400 Subject: [PATCH 149/169] specify optional args as Optional type --- python/gtsam/examples/PreintegrationExample.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index 95a15c077..d3c0f79b8 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -12,7 +12,7 @@ Authors: Frank Dellaert, Varun Agrawal. # pylint: disable=invalid-name,unused-import,wrong-import-order -from typing import Sequence +from typing import Optional, Sequence import gtsam import matplotlib.pyplot as plt @@ -39,9 +39,9 @@ class PreintegrationExample: return params def __init__(self, - twist: np.ndarray = None, - bias: gtsam.imuBias.ConstantBias = None, - params: gtsam.PreintegrationParams = None, + twist: Optional[np.ndarray] = None, + bias: Optional[gtsam.imuBias.ConstantBias] = None, + params: Optional[gtsam.PreintegrationParams] = None, dt: float = 1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" From e31beee22b2a38676e792ba86503b763562d8f35 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 27 Oct 2021 22:33:11 -0400 Subject: [PATCH 150/169] removed ground truth; angles set in deg and converted to rad --- python/gtsam/examples/Pose2ISAM2Example.py | 53 +++++++++++----------- 1 file changed, 26 insertions(+), 27 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index fac7ecc1a..be4118b1f 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -8,7 +8,7 @@ See LICENSE for the license information Pose SLAM example using iSAM2 in the 2D plane. Author: Jerred Chen, Yusuf Ali -Modelled after: +Modeled after: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ @@ -21,12 +21,8 @@ import gtsam.utils.plot as gtsam_plot def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): - """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2. + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" - Based on version by: - - Ellon Paiva (Python), - - Duy Nguyen Ta and Frank Dellaert (MATLAB) - """ # Print the current estimates computed using iSAM2. print("*"*50 + f"\nInference after State {key+1}:\n") print(current_estimate) @@ -49,14 +45,8 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa axes.set_ylim(-1, 3) plt.pause(1) - return marginals - -def vector3(x, y, z): - """Create a 3D double numpy array.""" - return np.array([x, y, z], dtype=float) - def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, - key: int, xy_tol=0.6, theta_tol=0.3) -> int: + key: int, xy_tol=0.6, theta_tol=17) -> int: """Simple brute force approach which iterates through previous states and checks for loop closure. @@ -64,8 +54,8 @@ def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. current_estimate: The current estimates computed by iSAM2. key: Key corresponding to the current state estimate of the robot. - xy_tol: Optional argument for the x-y measurement tolerance. - theta_tol: Optional argument for the theta measurement tolerance. + xy_tol: Optional argument for the x-y measurement tolerance, in meters. + theta_tol: Optional argument for the theta measurement tolerance, in degrees. Returns: k: The key of the state which is helping add the loop closure constraint. If loop closure is not found, then None is returned. @@ -81,7 +71,7 @@ def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, current_estimate.atPose2(k).y()]) pose_theta = current_estimate.atPose2(k).theta() if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ - (abs(pose_theta - curr_theta) <= theta_tol): + (abs(pose_theta - curr_theta) <= theta_tol*np.pi/180): return k def Pose2SLAM_ISAM2_example(): @@ -89,11 +79,23 @@ def Pose2SLAM_ISAM2_example(): simple loop closure detection.""" plt.ion() + # Declare the 2D translational standard deviations of the prior factor's Gaussian model, in meters. + prior_xy_sigma = 0.3 + + # Declare the 2D rotational standard deviation of the prior factor's Gaussian model, in degrees. + prior_theta_sigma = 5 + + # Declare the 2D translational standard deviations of the odometry factor's Gaussian model, in meters. + odometry_xy_sigma = 0.2 + + # Declare the 2D rotational standard deviation of the odometry factor's Gaussian model, in degrees. + odometry_theta_sigma = 5 + # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) - ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, prior_xy_sigma, prior_theta_sigma*np.pi/180])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, odometry_xy_sigma, odometry_theta_sigma*np.pi/180])) # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() @@ -127,25 +129,20 @@ def Pose2SLAM_ISAM2_example(): for i in range(len(true_odometry)): - # Obtain "ground truth" change between the current pose and the previous pose. - true_odom_x, true_odom_y, true_odom_theta = true_odometry[i] - # Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise. noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i] # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. - loop = determine_loop_closure(odometry_measurements[i], current_estimate, i) + loop = determine_loop_closure(odometry_measurements[i], current_estimate, i, xy_tol=0.8, theta_tol=25) # Add a binary factor in between two existing states if loop closure is detected. # Otherwise, add a binary factor between a newly observed state and the previous state. - # Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement. - # This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used. if loop: graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, - gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE)) + gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) else: graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, - gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE)) + gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta)) @@ -156,10 +153,12 @@ def Pose2SLAM_ISAM2_example(): current_estimate = isam.calculateEstimate() # Report all current state estimates from the iSAM2 optimzation. - marginals = report_on_progress(graph, current_estimate, i) + report_on_progress(graph, current_estimate, i) initial_estimate.clear() # Print the final covariance matrix for each pose after completing inference on the trajectory. + marginals = gtsam.Marginals(graph, current_estimate) + i = 1 for i in range(1, len(true_odometry)+1): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") From c51a1a23098c87a13849a4e77cb36ed165983282 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 27 Oct 2021 22:35:03 -0400 Subject: [PATCH 151/169] removed ground truth; set ang in deg and convert to rad for Pose3iSAM2 --- python/gtsam/examples/Pose3ISAM2Example.py | 79 ++++++++++++---------- 1 file changed, 43 insertions(+), 36 deletions(-) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 3e78339bd..82458822a 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -8,7 +8,7 @@ See LICENSE for the license information Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen -Modelled after version by: +Modeled after version by: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ @@ -20,12 +20,8 @@ import matplotlib.pyplot as plt def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): - """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2. + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" - Based on version by: - - Ellon Paiva (Python), - - Duy Nguyen Ta and Frank Dellaert (MATLAB) - """ # Print the current estimates computed using iSAM2. print("*"*50 + f"\nInference after State {key+1}:\n") print(current_estimate) @@ -49,8 +45,6 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa axes.set_zlim3d(-30, 45) plt.pause(1) - return marginals - def createPoses(): """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], @@ -78,34 +72,27 @@ def createPoses(): return [gtsam.Pose3(P0), gtsam.Pose3(P1), gtsam.Pose3(P2), gtsam.Pose3(P3), gtsam.Pose3(P4), gtsam.Pose3(P5)] -def vector6(x, y, z, a, b, c): - """Create a 6D double numpy array.""" - return np.array([x, y, z, a, b, c], dtype=float) - -def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, - key: int, xyz_tol=0.6, rot_tol=0.3) -> int: +def determine_loop_closure(odom_tf: gtsam.Pose3, current_estimate: gtsam.Values, + key: int, xyz_tol=0.6, rot_tol=17) -> int: """Simple brute force approach which iterates through previous states and checks for loop closure. Args: - odom: Vector representing noisy odometry transformation measurement in the body frame, - where the vector represents [roll, pitch, yaw, x, y, z]. + odom_tf: The noisy odometry transformation measurement in the body frame. current_estimate: The current estimates computed by iSAM2. key: Key corresponding to the current state estimate of the robot. - xyz_tol: Optional argument for the translational tolerance. - rot_tol: Optional argument for the rotational tolerance. + xyz_tol: Optional argument for the translational tolerance, in meters. + rot_tol: Optional argument for the rotational tolerance, in degrees. Returns: k: The key of the state which is helping add the loop closure constraint. If loop closure is not found, then None is returned. """ if current_estimate: - rot = gtsam.Rot3.RzRyRx(odom[:3]) - odom_tf = gtsam.Pose3(rot, odom[3:6].reshape(-1,1)) prev_est = current_estimate.atPose3(key+1) - curr_est = prev_est.transformPoseFrom(odom_tf) + curr_est = prev_est.compose(odom_tf) for k in range(1, key+1): pose = current_estimate.atPose3(k) - if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \ + if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol*np.pi/180).all() and \ (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): return k @@ -114,11 +101,33 @@ def Pose3_ISAM2_example(): loop closure detection.""" plt.ion() + # Declare the 3D translational standard deviations of the prior factor's Gaussian model, in meters. + prior_xyz_sigma = 0.3 + + # Declare the 3D rotational standard deviations of the prior factor's Gaussian model, in degrees. + prior_rpy_sigma = 5 + + # Declare the 3D translational standard deviations of the odometry factor's Gaussian model, in meters. + odometry_xyz_sigma = 0.2 + + # Declare the 3D rotational standard deviations of the odometry factor's Gaussian model, in degrees. + odometry_rpy_sigma = 5 + # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3)) - ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2)) + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, + prior_xyz_sigma, + prior_xyz_sigma, + prior_xyz_sigma])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_rpy_sigma*np.pi/180, + odometry_rpy_sigma*np.pi/180, + odometry_rpy_sigma*np.pi/180, + odometry_xyz_sigma, + odometry_xyz_sigma, + odometry_xyz_sigma])) # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() @@ -154,38 +163,36 @@ def Pose3_ISAM2_example(): current_estimate = initial_estimate for i in range(len(odometry_tf)): - # Obtain "ground truth" transformation between the current pose and the previous pose. - true_odometry = odometry_tf[i] - # Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise. noisy_odometry = noisy_measurements[i] + # Compute the noisy odometry transformation according to the xyz translation and roll-pitch-yaw rotation. + noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1)) + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. - loop = determine_loop_closure(noisy_odometry, current_estimate, i) + loop = determine_loop_closure(noisy_tf, current_estimate, i, xyz_tol=18, rot_tol=30) # Add a binary factor in between two existing states if loop closure is detected. # Otherwise, add a binary factor between a newly observed state and the previous state. - # Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement. - # This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used. if loop: - graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, true_odometry, ODOMETRY_NOISE)) + graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, noisy_tf, ODOMETRY_NOISE)) else: - graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, true_odometry, ODOMETRY_NOISE)) + graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE)) # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. - noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1)) - computed_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) - initial_estimate.insert(i + 2, computed_estimate) + noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) + initial_estimate.insert(i + 2, noisy_estimate) # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. isam.update(graph, initial_estimate) current_estimate = isam.calculateEstimate() # Report all current state estimates from the iSAM2 optimization. - marginals = report_on_progress(graph, current_estimate, i) + report_on_progress(graph, current_estimate, i) initial_estimate.clear() # Print the final covariance matrix for each pose after completing inference. + marginals = gtsam.Marginals(graph, current_estimate) i = 1 while current_estimate.exists(i): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") From d98e6e500a20d72d9920a2b016bfea108fc1d3ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Oct 2021 09:51:31 -0400 Subject: [PATCH 152/169] address review comments --- python/gtsam/examples/ImuFactorExample.py | 17 +++++++++++++---- .../gtsam/examples/PreintegrationExample.py | 19 ++++++++++++++++--- 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 5b4b06b55..13ed24c6c 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -27,6 +27,7 @@ from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample BIAS_KEY = B(0) +GRAVITY = 9.81 np.set_printoptions(precision=3, suppress=True) @@ -42,7 +43,7 @@ def parse_args() -> argparse.Namespace: "-T", default=12, type=int, - help="Total time in seconds") + help="Total navigation time in seconds") parser.add_argument("--compute_covariances", default=False, action='store_true') @@ -70,8 +71,7 @@ class ImuFactorExample(PreintegrationExample): gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) - g = 9.81 - params = gtsam.PreintegrationParams.MakeSharedU(g) + params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) # Some arbitrary noise sigmas gyro_sigma = 1e-3 @@ -107,7 +107,16 @@ class ImuFactorExample(PreintegrationExample): title: str = "Estimated Trajectory", fignum: int = POSES_FIG + 1, show: bool = False): - """Plot poses in values.""" + """ + Plot poses in values. + + Args: + values: The values object with the poses to plot. + title: The title of the plot. + fignum: The matplotlib figure number. + POSES_FIG is a value from the PreintegrationExample which we simply increment to generate a new figure. + show: Flag indicating whether to display the figure. + """ i = 0 while values.exists(X(i)): pose_i = values.atPose3(X(i)) diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index d3c0f79b8..611c536c7 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -22,6 +22,7 @@ from mpl_toolkits.mplot3d import Axes3D IMU_FIG = 1 POSES_FIG = 2 +GRAVITY = 10 class PreintegrationExample: @@ -68,7 +69,7 @@ class PreintegrationExample: self.params = params else: # Default params with simple gravity constant - self.params = self.defaultParams(g=10) + self.params = self.defaultParams(g=GRAVITY) if bias is not None: self.actualBias = bias @@ -86,7 +87,13 @@ class PreintegrationExample: def plotImu(self, t: float, measuredOmega: Sequence, measuredAcc: Sequence): - """Plot IMU measurements.""" + """ + Plot IMU measurements. + Args: + t: The time at which the measurement was recoreded. + measuredOmega: Measured angular velocity. + measuredAcc: Measured linear acceleration. + """ plt.figure(IMU_FIG) # plot angular velocity @@ -123,7 +130,13 @@ class PreintegrationExample: t: float, scale: float = 0.3, time_interval: float = 0.01): - """Plot ground truth pose, as well as prediction from integrated IMU measurements.""" + """ + Plot ground truth pose, as well as prediction from integrated IMU measurements. + Args: + t: Time at which the pose was obtained. + scale: The scaling factor for the pose axes. + time_interval: The time to wait before showing the plot. + """ actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) translation = actualPose.translation() From 99ce18c85735e7722b744446d13ecf65d99f88ae Mon Sep 17 00:00:00 2001 From: jerredchen Date: Thu, 28 Oct 2021 12:29:00 -0400 Subject: [PATCH 153/169] formatting by Google style --- python/gtsam/examples/Pose2ISAM2Example.py | 20 +++++++++++++------ python/gtsam/examples/Pose3ISAM2Example.py | 23 ++++++++++++---------- 2 files changed, 27 insertions(+), 16 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index be4118b1f..c70dbfa6f 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -14,12 +14,14 @@ Modeled after: """ import math -import numpy as np -import gtsam + import matplotlib.pyplot as plt +import numpy as np + +import gtsam import gtsam.utils.plot as gtsam_plot -def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, +def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" @@ -94,8 +96,12 @@ def Pose2SLAM_ISAM2_example(): # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, prior_xy_sigma, prior_theta_sigma*np.pi/180])) - ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, odometry_xy_sigma, odometry_theta_sigma*np.pi/180])) + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, + prior_xy_sigma, + prior_theta_sigma*np.pi/180])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, + odometry_xy_sigma, + odometry_theta_sigma*np.pi/180])) # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() @@ -145,7 +151,9 @@ def Pose2SLAM_ISAM2_example(): gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. - computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta)) + computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, + noisy_odom_y, + noisy_odom_theta)) initial_estimate.insert(i + 2, computed_estimate) # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 82458822a..59b9fb2ac 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -8,15 +8,18 @@ See LICENSE for the license information Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen -Modeled after version by: +Modeled after: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ +from typing import List + +import matplotlib.pyplot as plt +import numpy as np + import gtsam import gtsam.utils.plot as gtsam_plot -import numpy as np -import matplotlib.pyplot as plt def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): @@ -45,7 +48,7 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa axes.set_zlim3d(-30, 45) plt.pause(1) -def createPoses(): +def create_poses() -> List[gtsam.Pose3]: """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], @@ -118,7 +121,7 @@ def Pose3_ISAM2_example(): # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_rpy_sigma*np.pi/180, prior_rpy_sigma*np.pi/180, - prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, prior_xyz_sigma, prior_xyz_sigma, prior_xyz_sigma])) @@ -141,7 +144,7 @@ def Pose3_ISAM2_example(): isam = gtsam.ISAM2(parameters) # Create the ground truth poses of the robot trajectory. - true_poses = createPoses() + true_poses = create_poses() # Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations # between each robot pose in the trajectory. @@ -149,9 +152,9 @@ def Pose3_ISAM2_example(): odometry_xyz = [(odometry_tf[i].x(), odometry_tf[i].y(), odometry_tf[i].z()) for i in range(len(odometry_tf))] odometry_rpy = [odometry_tf[i].rotation().rpy() for i in range(len(odometry_tf))] - # Corrupt the xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements. - noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), ODOMETRY_NOISE.covariance()) - for i in range(len(odometry_tf))] + # Corrupt xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements. + noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), \ + ODOMETRY_NOISE.covariance()) for i in range(len(odometry_tf))] # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate # iSAM2 incremental optimization. @@ -179,7 +182,7 @@ def Pose3_ISAM2_example(): else: graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE)) - # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. + # Compute and insert the initialization estimate for the current pose using a noisy odometry measurement. noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) initial_estimate.insert(i + 2, noisy_estimate) From b15297ae4096c9f8f0e745a8dd3b4a4505ae63db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Oct 2021 15:19:36 -0400 Subject: [PATCH 154/169] address review comments --- python/gtsam/examples/ImuFactorExample.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 13ed24c6c..c86a4e216 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -137,7 +137,14 @@ class ImuFactorExample(PreintegrationExample): T: int = 12, compute_covariances: bool = False, verbose: bool = True): - """Main runner.""" + """ + Main runner. + + Args: + T: Total trajectory time. + compute_covariances: Flag indicating whether to compute marginal covariances. + verbose: Flag indicating if printing should be verbose. + """ graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements From 06bb9cedd156cb8aae602ecc40c001dbb2fcf1cc Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 31 Oct 2021 20:53:15 -0400 Subject: [PATCH 155/169] Address review comments: negative sign and AdjointTranspose section --- doc/math.lyx | 242 +++++++++++++++++++++++++++++++++++++++++++++++---- doc/math.pdf | Bin 272118 -> 273096 bytes 2 files changed, 223 insertions(+), 19 deletions(-) diff --git a/doc/math.lyx b/doc/math.lyx index 6d7a5e318..4ee89a9cc 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5086,6 +5086,13 @@ reference "ex:projection" \begin_layout Subsection Derivative of Adjoint +\begin_inset CommandInset label +LatexCommand label +name "subsec:pose3_adjoint_deriv" + +\end_inset + + \end_layout \begin_layout Standard @@ -5098,7 +5105,7 @@ Consider \end_inset . - The derivative is notated (see + The derivative is notated (see Section \begin_inset CommandInset ref LatexCommand ref reference "sec:Derivatives-of-Actions" @@ -5114,7 +5121,7 @@ noprefix "false" \begin_layout Standard \begin_inset Formula \[ -Df_{(T,y)}(\xi,\delta y)=D_{1}f_{(T,y)}(\xi)+D_{2}f_{(T,y)}(\delta y) +Df_{(T,\xi_{b})}(\xi,\delta\xi_{b})=D_{1}f_{(T,\xi_{b})}(\xi)+D_{2}f_{(T,\xi_{b})}(\delta\xi_{b}) \] \end_inset @@ -5149,11 +5156,12 @@ D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T} \end_inset -To compute -\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ +We will derive +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi)$ \end_inset -, we'll first define + using two approaches. + In the first, we'll define \begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$ \end_inset @@ -5194,18 +5202,30 @@ Now we can use the definition of the Adjoint representation \begin_layout Standard \begin_inset Formula \begin{align*} -D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}g^{-1}\right)(\xi)\\ - & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}g^{-1}(T,0)+g(T,0)\hat{\xi}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ +D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}_{b}g^{-1}\right)(\xi)\\ + & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}_{b}g^{-1}(T,0)+g(T,0)\hat{\xi}_{b}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\ & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\ - & =-Ad_{T}(ad_{\xi_{b}}\hat{\xi})\\ + & =Ad_{T}(ad_{\hat{\xi}}\hat{\xi}_{b})\\ + & =-Ad_{T}(ad_{\hat{\xi}_{b}}\hat{\xi})\\ D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}}) \end{align*} \end_inset -An alternative, perhaps more intuitive way of deriving this would be to - use the fact that the derivative at the origin +Where +\begin_inset Formula $ad_{\hat{\xi}}:\mathfrak{g}\rightarrow\mathfrak{g}$ +\end_inset + + is the adjoint map of the lie algebra. +\end_layout + +\begin_layout Standard +The second, perhaps more intuitive way of deriving +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ +\end_inset + +, would be to use the fact that the derivative at the origin \begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$ \end_inset @@ -5224,28 +5244,212 @@ An alternative, perhaps more intuitive way of deriving this would be to \begin_layout Standard \begin_inset Formula \[ -D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}_{b}}(\xi)\right) +D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}}(\hat{\xi}_{b})\right)=-Ad_{T}\left(ad_{\hat{\xi}_{b}}(\hat{\xi})\right) \] \end_inset -It's difficult to apply a similar procedure to compute the derivative of - -\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ + +\end_layout + +\begin_layout Subsection +Derivative of AdjointTranspose +\end_layout + +\begin_layout Standard +The transpose of the Adjoint, +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $Ad_{T}^{T}:\mathfrak{g^{*}\rightarrow g^{*}}$ \end_inset - (note the +, is useful as a way to change the reference frame of vectors in the dual + space +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +(note the \begin_inset Formula $^{*}$ \end_inset - denoting that we are now in the dual space) because + denoting that we are now in the dual space) +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none +. + To be more concrete, where +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +as +\begin_inset Formula $Ad_{T}\hat{\xi}_{b}$ +\end_inset + + converts the +\emph on +twist +\emph default + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\xi_{b}$ +\end_inset + + from the +\begin_inset Formula $T$ +\end_inset + + frame, +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + converts the +\family default +\series default +\shape default +\size default +\emph on +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +wrench +\emph default + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\xi_{b}^{*}$ +\end_inset + + from the +\begin_inset Formula $T$ +\end_inset + + frame +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +. + It's difficult to apply a similar derivation as in Section +\begin_inset CommandInset ref +LatexCommand ref +reference "subsec:pose3_adjoint_deriv" +plural "false" +caps "false" +noprefix "false" + +\end_inset + + for the derivative of +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + because \begin_inset Formula $Ad_{T}^{T}$ \end_inset - cannot be naturally defined as a conjugation so we resort to crunching + cannot be naturally defined as a conjugation, so we resort to crunching through the algebra. - The details are omitted but the result is a form vaguely resembling (but - not quite) the + The details are omitted but the result is a form that vaguely resembles + (but does not exactly match) \begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$ \end_inset diff --git a/doc/math.pdf b/doc/math.pdf index 71f9dadc65bea48ee1dbd925f2cee058bb2bd832..40980354ead4a2339ffb1ef0636825a77fad1cac 100644 GIT binary patch delta 26881 zcmZsiV|1oX)2=7BZ95a=jxn)q+qSMGlVoDsn%K5&XJSo~iEYe&p7-7BTYIhj{pz#3 zYIXlTy6UXQY1FZ0)Oc|)_M`+tT3|~{UXef=wfjx;p0!6|>Y{ zF*1w2)NWj~Cj^8h_V;>_kC$oQwd0ow0u&&a-m9TbQ=4#4bD+|k1AUY<`B7L@6fY5? zG;k7}bf6Io3_Js+4LM&#dXI{fTmpr50dfyaSsUG{THR>?qN3luSeyU@dMv!94h3VK zOuS9}jaP#(2Mz07{aUjhFbK_J_#Wb{fCis{0lT$iEsh;SE<_r!t0oLufG^@XTQCkz z2N9c6E|Rgrxx**m?Zcg~_oaa(hcJ$Y_MGcX^u!^+nWgB^Bm6S@nt}*0ej%e~MdpDE zX}Bczho*4uNNA}Wind@K6A)_0Y@$hlbNdLBj_DLf3n#fpqf%D^mV9CD!jG_u>w$9U zWeq_jFq#VBpzz_4uiwAI=s*QA!F)nP_GClPLT)OstFb`4_e0UT!b0%I=c74epn^ta z0^x8d6^(eTR^S;mV^iQK)kFCyxD<`Tg>y6$myvv$_t~+ivw7=|k;=&Dj=$Pszk4Hm zpwIY>+CYEL#VV%+Dj^fSo)qj*h95uAt*Bc(hqET4#7}H z7o-GrK>Sn{m8+Wv|b+_x^e=ff*X3d`( zG$|kNQqm0ZCoB%L_x@}W%HCFNcEPyYkuREw@ZDlk>mO!3GR9PW+pS7aQ5{lH#rk^4 zGrnQZ-?&@>#1*OikX7H`Tj||z4l9S2F&y)`qx$BU;SQopi&mEP&n<21q#5W*ciO0NeX*`@fFcldI^a;I&1A{?yM%t{=hkymr_I#j^eswq zEOE-yP&ys?kNJUB*h^rb%pY39^=(%Bn?8!@6C$jT8%`&2NKef5Q}{Lcq7p(@rJk1W zmhl5AfRJT>9s2W?0R<>kQ+&(Pv3N-(u#v!eE03ZcVZvE#LYUpJF8#=vIOJV6rvC{o z{4!lQ8nHfC(){UG0Iyr}J(VRYAv;;)#vd}{kTv?N3(dm3#w_huykmg?Hj}?|y%B4Z zArJoDDFW)Di5#+g^7gWxe)|dbeDn)%@o|H0z#021D_#O)WOIEr_J@LwzqL?LB@)5u zYgdhA_m;cWG29sf>S}A{Nq_smW9I<`?EHj_^&@?f#uJ|mj%%ZF!2cK3|pR6 zQm2}yR!L_>%`MrjVAbC*W;@?Y;LRP(T-{vEjqU$cjwUwnY@D1)^@$h|Y@DDZ)^@t2 zcc>nK{dOLOwuG{ZS`*Sk1l~b&19ezQTP9tGjIKEEz|j+ODNKB`6Cg4yCX|Lk7>L5v z_?n<%y!=+gwdLvng=(hJ&BEEfff{}Dy69b|_<&QFW=+G6s)h{}bB9nYbz=B7nloT~ z@!(yRH4@jdH#5`3x7F-J0u@$7l5@IJqtXhX^kaQ4WO^(vRo4V(;OjpaH-0duRYc{o zZvbcdr}Iv4yS0O?o1pZ|-`vUzZWv+CTb9^!(t`nT zyOePM8T&3_swwPLp(8F6Dz)TiJRA@=(vRT@+WX|ZFy~acZ!R(jVv{cD18dE&VaAxR ziCNH1cu<0I8j3kqt;ua+VcK5gA=NRm=q7umRd?{AATgxSmuEB5V5v<~$S=&OVG;J| zGH;nFKK$SfzaK|%6PvW~U0QfTmuLahd+PMBCXrF|=0X;n%xI>RkxTL7a`ahlk1ky9 ze9vUXUBJRo6OFvLbBfk7Aokf2;+0&lQ z2&#Tw`{4v$s&0M=Br(B*H)?6m^%PQy1(S1W7GWB(w)WwB?95<#I+%aKr3wVtBzKMe zE}(D*A~KA<7OmY2*s+(t`V5|w>9yNB5xk-X(0u;UEcR> zBl)jn!y6@3i1#tNio}-ix>4)4m5X7-3LK)1X`E9QLAr`DoB9}8ipB%thhEyulGW}G z=vpY1y33L!J>I4d&Y>u|1EKX&)~zD@CiOBAV4DsqwQjh5dttby7mz`mf!%|Ta}#`ywIqo-P& z>b5PcpLhpzLSlg^5}Uy4hY}c|@3NXxFTCFwS>u6@$AGqxn_MKSExKoqNg1yh8rVyR z=Bj@Ab&{)&KP`b*x4A774fW|+`OUVH=U5*_^z@B|)jTo!Nq{fh$fy!C_Dh;|4J+j8oYEhC z<#9&ouLl^KZ=%x^p(@3($Z{IP$c1eDm0)X#-p2kK*Up(O zhk(u#7EtG$CejvqBk&a<(&IcLjKLj;O~I7z+rle+J|#j%`a0urV{~pbV60s=u@`Mu zI2Tp6`zT!Rr^?5Q5=DqiWt?!OPg+is)NNjGcLvU=ydaJkmek{6485AR5p*{B78liM zp!w8CG#j;QV!0(rOih*OO5%5GhI(4}d8m>%3&a{3qm`4zV!}f&zieidzG9Alk)jGW zawi|qwDW3kg+iV!+v3O=%k|g~32g^l$#I+Ebw$MMNn3NEU+WOaAgf4!_)?ME$yNG) zVg^b(Hxu3A5@>sGpmeMQDZ14m`J*-gL$}En40da;F^Xo+KIF3goj>CXSFiD`z?d|X zfTfGi+Z^q|WorkGncEn<$eprmw-hOX!+Jc~SHKN^iglmG&l!HoWtdpl%x<4?;0>9L zZT&gN%yFG%9a``7fhF2da^wo{O;gBvS}3p>ei+enmAQi*3@y)g>)cW{nj~TY!ln+1 zXtBus=Tt8kuGWi7lI+_8`9m{=So-%uVBm4{ggafYF6GjT&w0~7kTa0R_~YkF_`O#&qbs^C@m!p4b5&^>L0Ax&ES3DH?<7&9j z`YxS2GQyEffDmpS)zbaNYHnNjD0)rOA}rmU9g$Sy`uFDbVBV$BJ4_%_ zt^fam5!_r!&Po`Kx5z(`z{{$##HITqrS^uMBIhzr|K>tY0 zMlDfXWH9bVXC+zWq!?=kWNvt7Idca~H!D(B9uVt)zJ9~nJFXbqr~yxg1(IF~%{tap z(=1b8f?C*F%%zCe!}viF1$6x-vEK%P9mm<5y}K8`k@Uy@g`9Wm=E{!({T$D2&_NC^ zw%a(WeCL($Aw2=%FZ{@~QXDZ*CdWux&hf~Pw?1kn*F(2nHvjHyxzBrJ(;=KaiAIg#^- z1`NSJ|3KhT*Z%b#Oc%P2K!Pcy7+9~AIz~nb=_}&h+~@dckLv7CT%!3Rc^!f z_YSiS14GVbzRCj6EafEp>zdR8a<6uAXzR&cUB1T#YY@_W>{-G@uAoEwl?B${mEnrN z%_rp~nnQ6-p?gbvFSh6EHh=Xxp9=tuS-zT{tv`)FxpG$Jeg9TYa&7c<;=mA)|4FLO zoeRgj(xJZgbNBUbPuci3<=5D%S3BM)PV)0xWG&@3w|-H#@5LAAO^(=DkW+o3{_r1mwGr`;`$M_F1apW+^5$?r}wc+OyneYmrK=WX- za(?k7hsPZkmGGeaxe-iiibeCT9O`o1j18)Mk=}gKoYPeI-fidYEw}R1ZqBGYQwW!M zCwI2;DNQp8<@?^^$+-)EWok*|-t3@Z*sMZIHlL~m^U+bVNmFJMb^{YxoZPm>Z^bc7 zMmG4q+;sPg=)XNgUzc1Rk70}fNS?fy_!OCjVFpQ)?-qW)QAgV1FaVGOxQ2zi;uTNC z(8|@!IN1{=&ZHC=op7JtVSH*cN4Q;4oPYK;*wYX~(|0o_Ey^acx?!daM(pu-?sALe z`hd3&6w2gGQXTnPn%31-%N!60IdMf0G0~Y|H`X6y#MtnD0a{Y!vLOOgBd3>OVrMk4 zBE=>J2E1xpE}J@QXxDDgD&&XJ$86Zo#zZrF)c%KOV z{`ozO|EkNqOdV?bcXKu{?5wTS3jeAXV3ps0Ku-Zno2v}MPR7?+KWj(&8u85>l?!Gk zs`BL*G0^eK|4Mq}$NS*~@(4HXSHdSlhlyd!wukqU#EhKUnTgHxkL-N$fmq!;v&E&m zHje0SgZ*qyzZ`^WRgxq@KCH!(?gr%*O&5?z(&-$#SYm3qzgqxOR?kkJQqQMbwqyl( zjQ8-v%l)EPI)6GvMV+hT3HZ$^FhbwcRF z#fmipuf-~2KSGf5cdMVgT|+tu{NLRK1kE1%x5;RVGZ6@@6_`mpcY{|4g_$}-gP?2R zAyR+hP>(5qk39l=!J`c|B%|Vuy8(~5M+KU_{vc6uH1Lbxdq_gt`|`oWlX74`6O;bFDcQf0oZz6~^iG0BtRrhZdi1%>4P_dAL)Bng z3twEM@KM#Hv>`IzKYZRQwm<&u(Rc@4GdQ!#GQhI8gb;;X56)VQG73h+>G;EY`YITE zlI+|#US1a1sJ^zvz3HF*GVv9I@`V~hZ>zkisAh3J!LTZT{%jCx2w{IN$~1(lwP3_> zG{J&W+Jk;AYr^LDdQL%cd6k7lU1ol*m6svs>yk=Q9A9}t#tL~v0D+pp{o*dzol72g zm59;+Ypl!4Ri6{LKp^gV>1EgZ@`WAOtI9yooeQo(j#&R*aAd=uRxbe>bYBNq zQ@$DRwdqHKwpdVWScH-CMH!A{e}_Dp9o8`#N2A7<;QU{Ti;~&%AGAvtIy1{{02ufC z8Or~U4eabmb^nmUf;bpO@bUDLOLLqyAGWn8EQ==QHp%u1B)!n%X7gq9n) z?v#eI4vPRVTVgdEr8TsiR8S6;-V(rjbwucI_N73`C5sjyn*vk*>uqplMTgc&ZP|5NC#GeEXM-r1HVwUtH6G`){G(-ECD-1 zv-OxjYcXNGz=lbxR7($(<`lOh*pm;2Ll0hzX0^NE04fzspaS3xWCP`f*`$opz$LJz za_c^=%+l>5El8o>IvC@+5;RBU06hlapI=P2`CfcdIcV{|ly~+v4 z3#Ux|qOBr#Lg1v;`Bnv%49_J(3ubMGWTcBB5BrvkagSvup0N`Vw>f7e_cCq|24%s!(fq4c$O(pu( z$6~b%wj20M9VUL5xQZ7nXY}uv&R@0WKY|PHjYCXa%Fp~h9*SS~96TN>^xIrl->=_8 zZ4FvUa4t=P6-#!`gdBDK+TNRUICex=Jv!1MR)Cz`q$6{_ysaJm=DV5FPR+m8JNkwJ z>~DW&APgvV8z*qmk8Y9wa>i**2xKW(;+}RL;4Th>EMD|ZTM9&Jgscs=diC8t07FF} zi(_Z%a=Y1cOU-TGZ`E(d&({GX1VQx@USs*(Ui)W7tpo~XE@DH$FAS5}jzi!u6PPxb z1@!JV{pUZgK3$vzvV0zHWOa*@Xb1EJ@=wL06!Vc&e=qN5d}pRZu@)^|4RN{0s(HzW zOZmMAn^!#U>gmv5_-!Rl=)+Dek8_aJWAQC3E2`V259-GtTh(udF6Q!AaAfmMwf&1K?^^=%MG9_bpxt0EQ{{qyF%5NzSQX`1>&f@W*gwZvJYO#-kZH4 zB7vue_shW&e9OEpS+&MJb{F8e<9%#EC?jwQ-`yrZ&(98r$skVkz2>D16C^W>1JErL za=o=wC+c9WKHbtzy_IKsA7QlT%;EUn&$LKnj3$~%P>~;G|X`W`?;L<(Vj^Pel%mtKc zu}D9uvu5i)d7Ggzi-lB+D1e1z2^3>P=O_A0Aa5p+s!4(MMRtx~bsAbULY3GmP7DY; zHS_ldawtiQ$@HjbK0i-=qtsI<4R^T)8~*d19KRmDOm>78{cS$KbgU+0UhLS|qA)yp zL z_HgdpG->be`?h_w=?pTJ&~lcRB7fkvLkaw#`2rPG_N{wT=?RkN+_G0oUKC zbyRDh992i!OZ7p6$Z|Wy0WfLZ|3;=nJ#!&4wUpf&myP^3a9&L*`E zoR-Thk*Wt)@|#P%$U*|lVPQ(6cL*VE-YwcoAyK^ZLJq9<&N+nlA@FK&^>sOR>$yWe zHYMRuD`lF$;J50?U#p+|o!lJdx zaS!tKqQ^JL4tF#h&ly;`l4hFWi3Rv-Y85MwZP&n+I2YS z7q7b@uZ`}|6O&bJnhrGQpSyLwA}FpI|Jg{ zUlDksCrak%+;d-1v}eC`yt#DZ`KnqNLf6xkO8e0}E5(YCzmyHGb@S|fvVYG#yH6S} zJq8zT2zB2tsyEQkGQ@Ypv6-aF!F&tv^_VT$tr%ESRw#R#i`!4xx9ks)_!*H&h~*sY z?7<h#A1bFH`ac6U3dAz{v<4jG*;g)#fWgPe$Ez zkw{+{bDveaAPa)3ZpmiA(8(fN#2TdN89v_brvm%F;Kq}}spv@JpowuFFB|sy-do>Z zjxy`Sz)D8T@@8&EV-nF-p&glUWW;{vpoaYcQQ;o^cJ0ZGfm8Uj{cDzW%0p7Bwrl2m zAS9&lS8M7h>jnU*|Ax#DhohQ>I29cz%itx3z4iaCPp`FH^gEPBLXvJ5$;NmNE$WJh zDM>)Jlf7yEMT%}+)q?pKzm)f-oOF>4?>sNkrDmtTul@v-DCZcVWk(nkz0l86OllIt z84(G+XhN^2iz))1hVa^~7b77dbNR%S5lF>q!JiO4{SysHGf9Rv+Yv9t2#4rLTU=Iz z)uNWO?^yH6W}`~9c4+0DEq!EAY;aH3#b+fWV06r7NiCT!gLV^@kcWpySD_Vi_wE9i zX`Ni7T?$KGQcuMo7rq=M4+Mvoc0_(nr+{FGQbWe3V6pP~ zcH@>QE;GU@pR{h$_s0F|$GM1E2X1_Fjy*)RXyqi0RxlycT(!eJ1G+Q59vQ%}4qb>y zrht>7(}6OQZGI^`bLEMQ_g~S^K^;$v5ZQD(i%S8J-dI#q736&gaq*L|m-OI~Z5H;w zq?H)ISbKJ4qmTL)X4WC#m@ul+7YgGo@4|)1#O(78=CD0rrqLk_Z&0ZE$$qh;N)~sv zxT%g!V_YZG9wODjOP(c?nHf)6fhv*OETbKk1Y4F*hDdDSo@!z(;$uB{kxW+z2>+)y z%f$`6JxkiW7wNxa_&-a&-OipL>O3@d_y>8Zd~@c z+h%w=8y>uMX3yFgX{$7br$)xO<4#9p{4L2V(}W_3N$7pDsB73SN@R zL7V}vub_o^bqEv@+$MID{ZUD|?!NU!4BplGbEUgQuWM%ctHv`N`-Y?`iEo4G=BPkO z01GRvXF2o@%Cq^$_g*Dj79)CdQly5-@KQBtu`+$i{4^YzyA7?*-!w^-FT9<-t%@8{ zj%~>jX2vY3WiI?U(PDAdu*Unb_dDJp!{(2kFgY^M-%hdfixo>MO?K)1;MrBODytlxm`XZMFY6mcF+ObK*`glJ05m31M*e~)n< zM(E+H0tf8!wxoG_hYi>^Sr+t7BzddoQXpKQr~ zlUAutsAWnb_9(7~fI}azsZAl6lk+36q*hcUeg-O2f4zwDgBS@v6!ZDT+MYJ3QK%7| zHtK~WuGLhNcXNM_K68W}p0(pHy%5URc9G|on`-MxVDmuq_c+hL=OjS?T9%-J<9pij zf&V;w!gQ%fth2`*q5LaeX-fCJ=8r#Kx2d(nB~`3-o=&JwunuM>fzhx^-9OO^K}-=L zLC)^h+-u)}{xEfhmg+Gh5dKYW7N=7rhw(ge1}ZvcHo$m9q{(1>aW-WcjFO z9WmLl-J17o-ip1$mRTp}4(9G14FYGh>YY8E(3XzcmkzxJ@sD0={a;jArdvZJ)vW1K z->XuLQB|Eg_)xi*jooQr9^$pFTzzzWQMRvz`*i&MT=RH}2K_o_OvyyowMkqQ(G}%HtZ2%CUwcPAbEOBsNqMYUlbH(j_ zw&9U6t4MhSm(C7cICX1iC6+oSS}+}-$LTIL!gghc-jY3%fME|DVKP&Z&Pn6K0zUB~ zoOGDgU^6*h%dkUr8a5R>TNf)B}4z`mDg3vVKcb5_=B+ z7eSoLO(1BT#PT64@^Q81hQasI&`(}R^8+Bl0n}(U%POvVw!Xcs5Ge6EL}pdgP6thL z!wGIH3h30po7rQ3`p!S(w|qsyZ?o<{w3)ngZ=qxA)%m-uAD})~-Ak(wr#kZca(+`13~-Q%r9Yg7QsY z82EzJ7TDnORGZrx4%cbRC{aaSX-tXmx9A%NCAjb$+e&kc*KH=cPoyh$RHkr|GktNx zx{m?lkcYwZ$cBl(0cEj>m@W~@Q6%=Rb`5s^9QXVTD)RbFU2abdS?KERjq0dSoYP}X zA)i25RY|nwR@FX2xeu>pUWbp7+Mn$1IN6g>^;>g7^V{uLf6h$a<3Au_$1=%X28!~*Q9@6|H3;Gi zPWHVJx#$HS+vVf8g>6j;aVa8HF4WxW zkFse?KIB^YXlRTlOMvoj`(bLgHMbp@7a^|%un-Ao2y8 zGvKG%{h&lJ;`{*9%3z<6`0*gM%?PvnSRsajCatu|i`M{&hONjNTkQul;pzG3^`F|s z^(%!_$2?s-KLi5p*V$>Nuf!eG7asK!8g|}!>e}^iikFpcLg(I{RGVar_V9u~5rx5y zme@8p%tPBl6DPZ_^Q!GU>jjGAfX|8m8Saa3q096-$?{y$0xhWQiB6e%#zi4cB_mwx z7O1&6C^^qfH=47?15NNa;_dtJVKylp#k|QDX3fwnU=j!Zu^r#KVVJq8u%7z}I9{namL>0xzDbv)Tey(7>NzRW3*XVb|ffb%DeO(N1|mNtYO66bW6U4 zujAKbFitgMeY#|7kuiqDF^SE*YVT_fiH$TVty5zUO&5S*6aLQS#&XN^y4a(O9u zxr6spd&6sa={^8e1v?_g$)~S4<8V&ipPbxe0m-j%ygSEn)>zx_fHt(j=l&n0hSasX;!@CD*#C4xXqw8xpJFCm{_^2=$Tg5jL|6p>y}a z*kqjyf7QKruXD!OnB!%GyieJI)47)qR-75x0?*WegnxzWsvqd(4TWO7KTA{2wS4(wNoUraAx-ZsH-4h0=EH9R*r zt5FwGVn|4S!cr9 zC)ZOQ@@3z3H#(!H+E-WI7C{A<{et#fd})(nTangc#kr)5$n+j;uqnYO{f9*ar^l=96za7Fqft|s zqMT){NM@vF z^JuD7JeHFrMY%&tS29|L7?m&C$gy(aNMFe2WFJTpxwsS2hi6q`qF!DZ@#^Z}auEk5 z7H0yb)$-E1!u2hrxg__krjx5@6ehNfG-hDkb=eV%Xv&zu$Xva?FT0**dA#clhB{S! z1AFuUtlq}w9^aIf-dVS&oYrXMes=S9%hD5{_s1y z8uIl!9pe7b3tq4=aOeLBL;IPdPRm}$Qauu?hW0Ae*Wt1~T-6o7%a*pgUh{pwlDgFB z^r4%fb>|&orzY}2CD3-8;+*M{g7Kug(LMBD2mt)?~hXR+AhQ%qY*y^y(x!PDf{J0-8;{&7|%8Nd{tNZdD+LFeP zfLDr3hfFeB(KYo(32UHCMtaB+UHdnKeJc#O6y${C(Xx5HY$b$0f@V@2_yP5Tzg5oW z$5fHaC?@$%0>`795z~H&3Qia?)4U$1?|RakD5;%hkTat1bQ3!Xug4W}Bpz6KE}u;9 z?}_P>>qnArZimXtMqMB_zpxU)DDWQp19D`jtUCxBu_MeiFWLuaNJrmj~)z^PL@ z$~o+@9bQi)~#ze#Vk@&jV1BxmUbw6Kbtt~vv zg78o3sw?VT+Fj@It)LxeB%u=Cv_`oZ$o&W=!d3Naqgy6DAOiMo?XtdZjSXGRcLj^Y zC(y~TkL0j3puHt(iH06eFL-GbfROaas0tknQ;EH}&9joJ+4x4n+dPp>7((8%?3gU( zADKH7Xc6|kES*t0{SSe09z{#2E(;e=sq5F-UuO9d7q%q<{LuDN%yd9sV&F9MYg*5%ptrrZb(A=l+kO#9>foN zB!0VrNfbF;bf|mb{%EU{1gMI_Q=)H8z%8laHm7$S96vn|PMNHJ3=Nay71RtupA1_7 zcPH%nb~lI22r`uSehxn_amMU7*9i6bnFP1r-;QXWD6-dh8l5h{VH-W1!aYK5N6fo& zzQ53673i}a^`68@gMZAwm{?LVf)8NzLWj4ySt8CPKt@aCtjG}A0-R3)hIzf5o>APt z9h?(7AE+*KbspF}qcoj7hb;TE0$E&Yo8{vBGcH5?fKyLBbS^WW-v_zwnSC`5VCrcL z#mkXvhl9p>NJ6`XcO*_PNw(V_oJTVP%s3eV^BHsYL(N#M6Rmm6>KB&o2AEl6`_8yi zX6|M}2bf4YCQN6`z}KMcJUP?*lSihp$!6y#) zSdz>CmpH zD5UouA?x0yMt2m#?4{MFqiC7`V%EG9$_+Rp0~=d{e(8>-1M-492F$B(c>@-?qgmF| zkPbo+lWiz`pHL$qKZ85Hs!cW0MWvuu)MRlWd9DgDZz!y2! zKc3c+95;_m^G>T#)Rp0Yj$)LXj4U#e&>d+fhz{IO`XN~&*rZCTrDjs>N;8(hJxyoS zfzlsUqL_CInBeMG`5IE*JB>R`GoOMOUKUFpWQ;5`>c&+@I=}XBU(Y{a3>dE`g%Dpv#wb36cygVPT8V%uS%lkvVvE%CE4rV|SWY|#)%U`nZ$Kw%F-V!D!g+t(f z4T3(3?zA~*ZC>7|*wr;hezdYntyn*h2W*Qz8p_h#W`AbPH;_<;6&P7z+(<>E(ti;N zqQ&54HT8jCYKw=33(#^;STi7VfHjiiW6c)@?_vJaL$7Rv07`)lHJ%y6=bP@1G%r3}CSm^&`!WERskoR2|d|MC+-#HG=Uq$6WOqy}q}u@U#S5HQzX zv)JAZCjt$0(SD9R(03uK0P|FE^GdGQr(0iGz&qDLTO_|zF z|NKz<@yDqn))O!*iYp)x#lx9Y$I1^sOC-q*w5!9T+GONPgMmYMt1vV#W8zC&r!(7nAmPV zS6Lm^g66-ml>Z6$p>FMVv=zWZbFA*t@PcXJ=vnz-=~GGl#ysg{-`lMpCM%ox`f1T9 zfnzvbJFvQXmvwE?j8du530%Lpxca+x<|WA-YigqGO`c7+tb4pz!*?KFr7>*QcC1n} z>N0v(&Yty^C7UXUj15*S{DoF*&6%Nm9_M1qPOm$WV_<)w^3zAJ8?%%$7c+x!c+k zr9-KuEDzrqwDE2(8#v+bGULI6Hak=k3na6&a-*%ak*%yFnds2p`tjsxsF7w^L2Izp zoLmTh#lw$bdS43JBm{5uMl1Z6LUspnU_{BRFWiB54I~{Wc2&_$znGQ^KEtT=rFDQP zIB&OoiK|GdHoSF77Px8@gHUaWaibi*$@ff9RT;A&(l33Z>_3RxROz9>r};A7V0Mmi~GEr0l6 ztv2Ai^IMknUS&%&qB)AIZ=acdc>CB*v8i2ST;GAB*Z^q^21w_22IrjWLL0)KC8~bU zJWRBbM&XLtAZ)B&c~o+4z9;8g43~_T@{k)+gX%W>4%NUib(0NSIa=XR_leYYGS7=> z`$1Tg>#6*Lp$Z{EZx?lveN}KE1#m)_#3s`sTnQyIFogY)IxwPtGon{XCkH&d?Uc2+ z_i5Xfq@2d*f!)M!yX0kNO|hH(k7fFJ)Y)eh0#Wo-8GZJ$YSMoZ>trT7Je;^yV&N_W zAX$mOLwR9h=c2)1h#9*jFfP6L;fV^b7g{C3Zz!d63-}`&S}xu3|3U2Nz$BWZGZsVn zQ)L=hOpDUNRXjf`a+Y|#BJct@gj4N&J-O1{ryeuK0+@NaxZH>gAAY;^AUoC^|IIML zEN4oK&Nf)3svYsXyIycQx_&wM<|Qx(=(Lw~3jAJWPBm+Ka6uCcn9vkma5>8CQezth z9#|wN4I8jsoWF+n=v6bP1R$z-USAqgndqy7@P2E`+m_$VS)dsukai-kGma)dVE(~-`gL+JYm0D?Kf3^|C4$*EqW){%bj@?=r$Oy>oK zr-e$UeI69~Rk0%4LRb^_PYlPbtf{`BYKzU+SnSZ2F3A`vHO5zn6i(%1h2y)P)yE8B zblu+>gOkcxW%XQpD^y(GBTEVp4nKLrph{m=_$2H?*$$C56fRbhA*0q$N7QCs9o)M+ zfK10);zEAW3=RjVVWh{32=#z0uKI})g%tOmsnR}%ig^1p(~QeH01KW9s7{{%_PhCq{TorfO?sDI3)13-2bJ=8a4L)7&ZrleuVw(6VA09E&V zouD4mJ(RCcZj7E|!OZLO7&KAClIzxOSdH_DjnLA_f z9R1Q>1XgH?(r}3_Dlyz%Qj!vvq~DIcOhfSnCFofGT+kLBtEofRMhi4WAsOxo^=_ z2C-|)?T*eZJ`5Mvo;JyiCrNy|g5cEV#)6Qdx(7K5_9wA($-ZqIhTCVi0M8L@$j;oQ za?THqHyS&E*)@rXVs5+6BT}l7qEraSaS0Y{ZSfX`3%8?}T(Y&EBP_A*{~F`HGdSIJw` zHXAvkV&AGZ)4?l)Z^u`!0HIQN;+If3N zkj0c}U?qbgL=*0O0&Q$)of5XGPlEswMme8ESx&1CB1om}u;Y)}Y0F|Zz(UjaKH|grcY6c}(sTe!zUEvz^Q@9!)E8Zr!WSuCXSwMCL0-*$m=JQmTj% zo4ip?x*UFb7MOc=i0F8pS|4WoeDXKr@LlfV`Kkz!K1ld!oc~kES4G9yG~F_|L(l*L zhCpz44?0+IAKcwt2O9`3L4vzWaF^i0-3Cp7;1D3V9p3MI|9_o}v(~xlwd<*_r>}Zd z*REY<2w@>A5L2MO#(w)(CnU}k(`FU>a*ZWK)v}QrEa;NC(sx370V<%ssv7PB8b*^k zUTS%p88U=wnkt#RB8tVZ3(UM(b;kR1p|aM0JE`v%!{agEcjns0uHKr4nK>-XIF{pO zI7pDy4YF2m=z46=og4g+VqkkPpLo^@mO$X4=jpd@Ff*jx2;$uV3#+VsJh;lN^>4Ya zM_a<^bT!$0I`@GR^Sv0bq;5+5Sy=xKs9Bom)PX#A`s#e#far+0=jZwT-9AiP&2(Bc z>Pz{^zF*zwA8X-w%E8;3BGmou>~!(#VFiXLYjmcaUvhMkG>2t(t zM||`y-_e7S*3Ej7d)H&&UHoo~cK4wBiA3P{QyqR%XD6)!y(|{#P@n3O9~$3pJgkyi zOzt4-_v?bpMiD*L&o|x;Z3dgtv5#Kbi_A&pCHqSDas?=1%sh+*sfmNIA;d%yq&lK# zzqK#Gh4%%++yd^R0;orp-ihd+5=97HX>LddH9?AI4@sTa+Bpk?WiOXTFQA~HFKIeI zF}&h)MGbOy5R?D|928;0I&ai{{nG^g+O$B{Z{~x)sk?##+7aRe4)R-ak2YQRH#sMI zr0w@NJ5vS~LQWPum$G}@vgfAcT+MG=h~jcER2!s*lE8LV&|DP!slz?2z@qC?8cZF( z?f_82*-L|=Q!R|~_uY<4h#r;p`_dtroOxR*tWa)E(~+w(V5SiyQ=M>4O=F(gUI5}- zJuo3jQBy2c>8Tb*5}LhTuoGj7--`@=&+zMGXOetuY`mSx)$Z?>sa&nZ@HZe@(A#ue z6J+5820RU(p)>=>`Piw8h48L?8AeC!)|q2Iuc3fa=wuoFX8zSk`wz$NUUVE?MJe%`unK2d2q2L)5aUs z<#`04gGvIoFB}B8Hw~&*hS{hHTI5nlm2h< zITW>)^xe$l@5{SUrr^@EZny&4)I>l3u?nN0vO*S}_U9HJn!Z@%Sb*H?iswg;@e-BQpl zyL4(KLw%!36tl2Zlm>xAlyAaYo_I=WcFd4GCDTMakxibyP;9|4P7d)@G&I-m+W|Cu znN2x|nu>HHF4(y9{oeYQZNTLbW})=o6=?%Z(Tg63fzQlZdCni{s2eRIClD}Aomt5%i>+^m8i6!A?VE5D0@4|zJtNU%>@U0h&;b#S8%1c=P${Y1Vx^fXA zKCMrVz+Q;+ZUov7-lmuuCE2S_+>@oKp?7T2wr-FzO+jnN$L+s96o*EGY!^R7Z(W%Z z^MC{{Gaqx^)CiQ&d;Bpf5+3x^e)kcI61E+{!D6%ySPjLLLuNzd9hn1 zgL59?KX3cV!e3N^Bg#?QFo*X3>6&XK4`l=&&3$?I1{^vTuVPRqS*wH=1k*g0pPbhIRY4&;kmj0L4+-$nScv20B_4H&?E=&|6?R5 z&723&!t=2I2ej1P1I#htdH%DTpoIw;o{J2g=Rdm%THLANUC;o$|2V#?SQz1vaQ+(t zKTW6{9=io72#@UaDzEtO_g7P(9z5^Afu;}jeHI2u4yQnmYOfA(R$ z96XJ4>KQYC9?b{u-lz&Zs5!K@zP3tbjUcN-0!$`)ln_i)^FN;wj z@A_-_CV9Ys?!#%Bgxw43FCFPn0~7vX_dyKmZ1FF~`g5!ciVz|bK-Sl}PEdU7AY7&q)z$n}S{u-91 zAgq>8XK2$XSz$3mNW_>08pyclj+F(j-i(YuT@IBuvtK0`+e=9j=15sWOC=ND~qKTK}9pJRX2#L@U`9i6Ha$r5c|uVx(o}-Xj1-%53O=a zEJ@1nUi--D7ScrQ+)5C5GYc_gUm^KTcqD+NOSzor*A#_*DvT^R7Ku=O5Eumwm1+5N zA(SKvU@4AW8q}AwMcBkV#4)Z?6bjVeQXUebj*u~qnVWwv@NOqm^&AoAwu~Ea%b^{9 zEUBeJB4Kneq70)ql%hn_nEg1PFOD5K?>cYdA@=WvmjNpe-7erZgtv@j zp4_?BQMrMPqY!GlPW*c`8FL;&D@uW%fSv(=K6$V@hpM_F&CkY4hj9WwFKztqnmp=hBG%) z*39=4@1Wxu*EafG5phvbCyV;2YUy(3vw9+adl z+wW5$`UaU5*u1p9U9Ms${hY@_{wC!)@B_Epa0SA$0 zk(~&Ln=)EtpeT`^rI;TWR$QNlKeaZ6YDH~557S+VGXJypSJeJAQ5{7ip5zzF9;~T4$U6H5<4;7t?d@xB(3w`oBqlEJIR7mX`4^y0A=I? z%qvKz^D`_)wM^*1XEok|bbF69O2<2?c-g)tPeGGwmmiiOw?}>b?b&8NJEWN>ZH-AuS0(DP97_8#nB^N^t$;zrm>RJ_;sAj{o{){E_b}4+6 z@d|I`&1a)hfnbRrRe=Kaf_lxE&8Xqmd5tlx>}l8E%>{kKkE0v&U>0K`HKnEY9XpRF zoJXwcvJ-+x+dtRCXv2|#jql53aPc*ru*pyP912>`t#kU#-E{<|g1{;zd%aljb5syQ zAmd6Ym2uTDMUIBgCYDvaY!Z7P{D+Lj^DSycL7PK4eBIXVGzJic4JMO;YP$?7@R{JQ zqb{jHMb$)N;lc2jl#o1k3qOT5b0JqHjNj*LIn!k^9mVxn?;UuQTRMVp5 zz6D|8q{qJ>QE?}1QK)cvQky=CO|457lDo# zWr{M(vewHV>`5LyfsTBaTV&0miGdB|RM->V?Yyf=5$~|b3ufNA zZ=x+aCc;>Bj`@yK(%9^6Is<8p1e@D<^-4<4hR<6krS+xOX783-^_q>#9VdvY+MIr^ z#p>0{hME^uZqDk$ta+2TQz1ofj#7(GsO+ph{z#%RX~gWJm^59dwX|PAite$M?}>K6 z=Cl}utvhu{#t9kYd(})hX21kH6CJ%^C;TC4o^xo~CCj#vma8^nZhAv3&Tc18rE_oV z>(+&x_6FYC<`nY|yMdW9OUp$BDCqTUh z=x$*ciL@EBKdMYERv-eiAk(v5RPyST?auiOwT{IM8>yPEcXAfWMsb*Jc6jLsi^rPj z!gW$y@AV|QB6a@0HKJPWv3B?_uK579Lqp^?#;Q{%sVk z{2)Q}&^Hr-q}D42^50;hWMeyAK{ssG7A0R7{WiSBR|!vUDuHv&BY8Gx%S~b=oMWJD z>+9WKQF^AbdXjRsjj8K`+e*T+550q=>_yK=ir?fv$kBI<n`PK;#p-XYSaLMeOo^J@Ms%yT(Re)9gec2D=67bVvADtgWrY)lJe}uS zoxy!Sh=^VJLHHm_GJ_H|RE7=sejf0{<|Wu}Q1g4&UAX`A!MZt5ceX#1 z)zy_X6ykj$syXR&v~ss@uJ*0qTcQ@wS$BhVXs$i7&s88Nj;>JJ5x(OKS`KBP_w~=2 zpWkG@@nw8Z!Y6&_L__y*~g4Dn@Q)&G&C(*5->ARDKtAQm{C; z7ZLw4OV~Hy-unw!MSgZ~{`m+#MzdV&{epbCc|K&db=2XyeC*yI7<&>FXADo~U*Aq@yt)EiK=Ei)+esN*i{oU0tkfA5_rGohKU3oU= z#6jiucE1sVnkkoBfw=svRq*L%T7OX2)g=A-q_IG+= z&IL|bLOXCIXjW*q28S}DJz%*Oni8p`;lgI(LPHdJptoy@&H{NyBRo{lW z^$M1Cv2op6_2G!EMt8PpHenXHUu(b^;U>dW*UQyjE)Hah zbMQu)u5jfLQrq-AgR4=J#<~WFOafmxa)R}4VB1;by;Zs&_ZDX5xwWf6cRQm~FebL) zMqFL^!rMEKEb4mo0%4zv^S|p6GEPHtmmRw!{`Z!rh9^BWY{O*J)5%um{#McT3pc;AKIQwnVTCdI4F`=R-hY`w3*ZGAIG9w^CTA#&@%YpDNUwJAxgvJOJz-c*yI&*S|j6( zUR{LW4%-YJ1gib475F(4vfWI#xJlfd*OO_eSSvlXe(TyzYLL)cExBaGMq7B0V6^Gq z(G{$hM)gDUN@32N##FE@9k8PD3HC)cG{?R&HxVhCIt7ENegL?e81=h1u^FNY|&IiPC+ zv{+Q>=B4k>g0C)}Jvn;bP1WR!XuY#Xw<5l(za7B$>LexBB+RZW(}x`5OU}ZjlqqXa z1@!#P{_aO7An#0awy@FDCFC=V4%o_WJ8#+df!2Vhq?2)i<5+$Ui3OZV@}SN2Y3ddc zDSbHA6fh_d>3;8d(@c*slD`ES6nlOIQp+{3!^!Ss_6{%R{0Y~# z4@!?1C5%k@bM=)-D-~h|HQOCV$qUQlVo>_K@ba#F>L@>+zC+09j&)tiTZDwg?nU~r zsbxa9gnmB)XJ>tP#=A~mRYffc!V~2%`DV5!mQ9;a;e{)vcc3BG?`78{x8u-Fo7Y6S zNomaBc(Uup_1o0i-}EsJue1tBPpb_`-QwP9GXfjlsi)WJksst21*B=n=uiWW(SZ~o zB~=kg|B{)!UR~FGcN|I3J<~G-%<7P$d7_p&?D07=Yox_pkckvyo=cy5ZyBj-%^SJEe%aB59JUp`%_j$q%0T0e6eD(TikeD^I9^me=xBUwTvP zWFL&igyk+i;iQ|flg;SLcw6fua>vHlRv@E}8*lb~!0&4W-CJ*W12)x*sWWSgx`^k# z9x_fUhh(_X}>#&Ba!x_rNBCW2OS-juZ;Vn`K7QkrANE?f1Ets(B2W@R*xxN1pttpaNoI zRcu$&7l^v-CI+*#kXy`z64fu1id{2qGjos!EOI#dUil?@e*SLTerD1!glE!mh_7Md z`?1c$-HNUEHQ9YrVS>MP^ReZnLlPeY|E&B{n^tX*W{%9oPHc{r905(sn;WMhTWcAKEp$%VeRYS}PO@Bc%@ zJA#)>&&I+UXLL}oF@0G+y0~!}7Z$_U*xJzF`0E(p-^R6W-!`Fj-ehCOl+)%wZRxy< zYP8*4F;#!4Q=etgPgC3IDQ0{GThVgLX2B0WSlUHs)R0lIFHXrY3DwUgiCG7Y?Z>3P zgXCEV7QSsuN&R76M+GK_St7tpnr9FiJR+JGSJxyh8%c*TPG_P>w1TXohD6+REvyLE zxuV9Cmn07lCoxAIGpjkd_NlmDSq*u|;*@j~71o7x@j4T*Ni!*D=0Tp>V$XS+cKOHM<+4GUX!9$ zGycO_)nE%a+REg8_+z-2<(54eODYl;B|7OM3Df~KgWvB5W)+vDQ=}H?L(&gS;Z{(U zPER=||AtJBUb`QY*O&JqD13SPE%rBB4rwOi;FuSj=$fOc|BLbam+<4|;7ki7Mhs%& z;^1WG=I7>QVdth}XQyL8XH#@Ce{FBD0MbhdaV2;Tpw{Ii2uk zY|_$`sJBO1^R@eP{gf7E`C!j=p;FtTMFhVo;XUFTaYQD9w{ME3Vd!UJplLgkEPPf} zyH01VAk02FfGE1y+AY&`pB|GvuJ}(%CU~WKH7Psi8WlX%!gD9Md`PGYx(j9=w=oSv zvUoiDNld&p`UySm`yk5!dSuMsGw*F5gAf36YThh_uE2J5{Ghxr-e6o7Zevbd_P85a zTuOFgQh1YO)!SaG;?IF0#6#c+$`JN<7zS@~tnrK4RcBG42gZg$m{nR~Kpy-p^!K!w z*pc}fi)8m(VX^kxND%t*v}{t0TL4Ky-It==QXG4fTL5J;oduSyZGXxaq6KX3uorp+ zI#tIAUBO7`A7KnSMCRl#s;$=NA3s3TkOP1SY`umef&Ly(dq4fT&~}&yDYciBk!Xi< z12=O^R=J|Ga+Kz*7#Ti3dh@U6N{=h|oSuu12%oH+1FS@5wIRwm8mFqw$D*x60WKrY zlIV+*VLl}rU(*YFvbwX%P0}1ST(Wr3+?z^w=A_*$2|C0H5@sc zhGH`{J;*8q^*Uw{_!1>HGwt* z)6k5Rh%3>hCdeOl+>Ob;RjpgjFYC+V6`qT*!`SquMbEsZ>?U55_1JPKWgSPupCQNA z@|2cPPu_+y5cc|?Ph7ZaQxdi{B^srE*5N(AeCRi|SL+o}2i%E5uIJ>jBN z1=wB$S8u|UNyz<+B{piAu2IuQW7E9!4NX!7-{E-GXNWmWHVw;yAR?f$7n@U|5(YiX z&!KmiVALoYb)78IKYw<8v1uoUKyDsyuV*SRpWQc1NOVEx>&j>G!o4tZ!!R5-yg?*s;qHnYX(1wwj~xqVKZq_mznY`9J?diGIJ)=aP~!1>r)9_{y@*W801y!&V?Em)Qy6G>r+ z!|wHYk!7+->!E`@4Mj`8{(IK{?9X2ygq`6DL#BmD52EuT4(FJfOoLfJ8-fI?G%x`ITmZfE6y%hi_Ov%n8mh^-}B z*COBF>*#{F#-8WaCz4Zjj@jiH?yzZ2vJb@6M9GOFI?ZfzqOX!XNT_EJ+)r5?9Gj7B zAXMRTDAj$-CK{{<;27Hv#ANal`J4v3^2H1FSD!o<+6T+ZniUw58q*KfpCuW^Y;qA3 z?v<(&2eum4Lrh4w0vaR5Ef|FyBxFy^?85#=)q#TU2bVqCUOdYFv=!`#xkqLBY)PYdk!{w;XIekPe+#ADckQ3RpfBqHP#>hL4vQ&%{OYLL2%|3ey-kYp8y5vkU9LAFH^S99fc{_aNsIJY79jdZ^Q=S1|jiTv= z%@(c#=)|*y@5HpEEaf^omK3ecWI=Yvl_5!C#&Q$vZj-YDk~>772Lss}9}ZX`C(4_b zN(UY&YC_4OiIw!5k4GztzymQ4|NpgSH>-p?{SvMCh8 zT~$uIkq0m|DCvXs48Z=k2E~UEuclo_bw;03Ok3QJ{T$K`49d^uR>WvbrxY9dwh9yZ zjk;zEOqDH0YL1w)g}RBBVc_p&yQq|uDUpS5PCqof``KR&&qJCoP^%M;ks7U=8EI33 z<3f-CTgH;Th}81oA*D*908tYR56_?uqFFL=Shjs_O8=VwzH&O>I_~t1-!tfy{|T_KHNVB% z1`#A&7*m}u*w)6}I`Zg1r2N}p`V!MlSlkvuglo~{}|&UpR^}ToU~jt$T_e^zP|WG$JJt-Ol=XJ z{(~*Tiy!@t0P}7dv5h|=5P!?4c-E-vs(9|!H&b$ljB-a0bH|Jqvcu*KBu10C9kOtD z`Naz0+(TU2ZMpcI_Dk`@70_JIi9?1JFGMJ5e)2h zbdB90(Dif?g#((^d>7tKAL2@A}w>`{L5<%>MBdboW4eH+{@ z`xJzQwL4t0NioSM8leXo<*QsfKfUPs6LBzjzV80C^rR8v0$X=~NN(tN*hq7J0lbsR zX8M0623%?8Ktw7C2e$z_o4Sp!1(28HmHE;J>Tv+MfE=$zH76(cS0e|I9jJ!RCg*7B z^oltDMN&XLVM!@IZhl^VPDv?tJ|139ULI*_Np3EFDQONqZt2%W6!`yl!G8VG{|ry% z=KiNyo*wccb<9aqlVIpZ@TR#~q}b`c)8SxyRziURDxuK!cbaJQs1XVeg-4Q?m=BH| z7(nBSbul%4M0hK>Z2OLXHq5#pGPGDdTRsLK1vDJC{}k{k(Cgco@Y(a?b{K9SL=IK1 z7B{2vV=u~=1oLcbWW2^F>jzYfyw9Gf)uFY>^92yaye9m`U=eF|YlpC_JirM7n8^g4 zbS8BgjCvD7+i1~fDG^*4ND*5Y>G(ZcL}t#ym)~jk$a+m+Ci*h|kv4;31*v_;bK1MI z2<30Vy3E|sbubEMjLNFh4@t}Q@p=-hyAOBkk-PiOqk4fZWuF+^`|O(XmikY z!k@SNah{|nfS=6#!CK-S%HDY6`9D?LCHZlb@(n;8GOch*g$Cjs3OWF*=|3!8lC7wm zjXaGGeGSHFSevt}VPnQf4j3myKLwmA9LOLJ(awbWV*M?DX@5#Pqn%K}u=>Wf&|%b# zM_~eG-QlmvzG9z8ZE!Qn3F%KwXRK;M#eAjixW({h@iLTksUzvR>c2@X(|UE#cYtT0>{p@hRRHkd^5l}`{B6Z-5KsSX`5}80{@N|;w_zddiA3Pc;VsfX(-HjtWlge(BW4(|D>C{}Z z_0slEFqHf~Vp9w!cap|D-W`bB_M4{U%b=345W`_6QR?IY!RVlz6J2h$EySI3ab#J) z)Lw7dyMWNa7sh9RFvSymViceTfncWQtv^Su@tkL+6fz!y5Y}*d&F2%A{SX?Lj8sxHvr$1@d9(Gs-w{H|c zrwk7WcoaN_-Nc^G-s_-L5_ONd=VWYYiam_-0Th{f5O|sW=e}J!!nJDzlC2@w3fmG0 z?$$slH)jt;mwEgyi@_L%ARe6Y=vKnY@aOxQxQfssu_w5W!ge@b2q-J^Eg>26W4>49 zA<9W!sEBdqdnK8esDQk=zMe>>)1L&_qOWzwk-o7O4`aA3i+3EA0nAvE)P_SG4U3{S zZwX!44_TM-ODFD$JqX6$%e}2*=Dhq>rF$nrKDfz0MYaz&$ENk2Q-?kCPJi#G(?C?K zsn(qiY4@OJkPin+8e~H2WnL9ob6~7YH7rzB(tP6h5Jgb>QWgRAvS4VEXxi8Z)iKjy ztn>4_>Ku|?Vb$PJSv~_wWq9|wuYyJsfLW*Rl&v~SOuY>G&&iiKctxcJTM=tGf287A zV>WB}i996ha9iuiCKnD`kv8StL!qJ9? z?y*U=5F?Z36rs%{6bx4(dxr5*^6Gyh5{6TcEUTip0!oNbeKYzRh49eU5RtPo1t9#Y zO%n@3Q~vCTYc6krZbrR@iK_?m1WC?`Yk&tPXIxSzUkcxn~v#Gnr!P3NnW?#zUbplh}iTo^qlY z4nT}k)Z??B!qTqBvS6fM;_{NR&u$?XKf(uU>O%xV^LkHbz&>?}ub>`1LOuz$UDH89 z1G{5(K*5E1j9&99^H?%S@g;{8>`&8#o^H+myiwLK1pJzoyM@(!6}^K55IEAG{tcKxE}fG+UYCAq1N48se&9z1(w zgL9*nKuicLzZUtPb%P@_%yr3hZ5qREh)9pQ8T%$?l? zHY_Epow43-)09pVeYWXU`-f>C7^9Z>xouP`?2J#XFzN2I z@7xLNY(3r&ft1sOCG_{NXN33qRjP1il&0NpsRgam+}X)GBjrgxox?)}g4PDO=SE%5 zdGGDl=|0MeNGKTt`1Wv~xd+3lqqZ>IE?F1~j4+Dt7@KX^39 z2GSBnFfEG47KSpQZ2gETeSk)tJm)p3cK||eAdq`*9o)g7{jl~OQ1;c#^D)^q+UuV` z25|*3f$Thca3$|>48Rg)h466gQr}51wj>OwifHx`eyl}T*x7@k)4`;PX2{B1Ey8sdS+-$V5d z;=0T6op1i;kC=DRp{HwWNXOdxuNyCbaIOrTse2WtOru@U< z!KYdY7>2=HcZ=)T-cr`ZuS@UE4xxQjr_`^$x*wRH)g24edUF+>ED0>{<9}cK+p<3v z8l4&Xv!4Wlt3#g__E4-L^1T;{(>;@ZHRM@-Ih{L+Y&07EPVfgLkNPZL(of-L?-aAt z4!7z=vt3I-k(NpzU|+(i8rIC-)Wy}=%*gJ4A_rq@SmvZYCk!xVHje*&WRt!jdVwnU zOz@24Mr`dQ^>E6Lof~14VH3&&s<2YTU--aI(tLK>gPpI-SCV5%YE5MQp?d4cWZ0@5 z!oTOaE;Io|O&6gRP`0#1 zvLxf*4YEXo(`t)w-2jvvXxpT6w)6mGV>#wZZYn_>RMbL{Ace+~-18xHbf96C=#`88 zAq2HiOZ_KPD+*0P;m~YbU%M42t-5$Y*(YmElPPYZiZ+pUY7+8Htr#PG(OlXVYyLFl zomaE>g=lBV5F6{%02;Baoo*sBoPb%52IUa3HRaSO1sICx0VFYqXCR}jZIa@~YW30~ zN<4Wh$4Gqf@IrjkG*z*9ATXb!-xfthH-`R$smd!7n^dVdjMi9ry|^i@sbaGqiTL;{ ziM@=DC^4WWRIBlfF}iNLhyT05_d z1Qj)x7TMmcn7B*m;*%6n^+Bc*> zRcgz$tK%`CdhU-`1ZvUeKyo0hf1%JLW6G3g*TG4JO3?mLU!nr${tXbjlu?PkT88YJ zU8@pRzW$8K|N09qo(?<7jhqSqv-@<))}GiQx*ROo=5pWrg*0k?SL9Om@??M3bfO9p ztx5-Ko&o2~x?I4rJ|IV)>3BsIC)i?4fkU27>W#Aq#)B3_3#1;M^yadzaY^d)*4(b- zXMUQ-tBRxXy+fyu5tGi6IY)pG9JB?$6Je#*opVLH8t*mw{p?1n7cTOAQ zKfy==6Ue&xth#G)g2ITKLgLkkvd=AFVlX~3u7(KWX&A(A$>+EA(J7!lT3Mmp^yhvS zJN%m%)R*y~2Y3?>`+Aw!c@qPHx~t~;H)%{Wa_zYoA7cU))}e?fu!|b}Q!X;S%Gp7y zCGfaCW@){0csVAZZ8UDfK<8SM!^3Q6n~~q@dJV zL@;(?&A+5;n(6q%QP#gISxQy*pHo%bGQqG3-dR6VIWSYMJ7d_!qGCg+mBsCB$2&Yh@--P3 zLTtF;U%tXJW&N7j=dfpPq>93g5FoM87X-ePvw1@ZK&y#JYvImE%tUIW}|fR`{WIF z)`YcZD8?pQP{z(J`jc9&#=5*qcG!^BU8B{(Y7-Q;ZY(rCR#8cMh$d#fo{CPFf<{HE zPcQc$zGg$~uMgk2IFK&;VerY}p{>-OZ!%@A9iR|6m`;f!c$=)tAL6#KFyY%MX>%!S zoV>OTtf25=)oFvnRa4;u54$u?rT0UY5#jxMFqbfSaw>S1P?9K%4ISbI_ z#*Xc`jm1HM+{fm63YX#K;=qxF`YbzF;~HN)KAT*SBsi&+-Lv27N;bixhS5pqTaxV} z<-kg-o3Z!x-L=c}y-KE2%OOee@?uHQn;GO`jax@cOQ)7-C=)0Sc{L<>{2P<>H| zR;Z}stc^%7%_B!jm}uWyP$|BQE4pYi9W2AhM0#V!?0(X)2=Vmkt+NrNBwEl9nHCXB>i1usNxo|ybVnYb7EgBRf6YcBAG7#CQ zs_XR8wXum!|GDMwsx51Ba6a`%($q3r1Ea|m2VautMKrC8O}bF@?yy-wILG$PVxav? z+U*o>t-MQ%BT2oah2!=Rot}Z!cUQkVJu4cezb~VD$O$pi3yqo%8o~T!u-fB zna^U_Jo}zt*8X5`@wST%qb=8{2e#>5ez)Tp?k z1>u8+cI03j^a|MB z<&b$klxC}A%{cPXz*(HB1bJEk2+d+;p=6E?1 zX`k`r`|v;DN7ivJLa93oVbMUL3VeoSwwU5)W*MT*G6HD`qBspB{C54>7x=QQ3;h3r z2%-q|rYFP*1W-=)qab2w-V)Qwk7L#*gg+21T) zEjd!98JyuQu}D!x#rF7ADaT81FJGyNmI=wCV(J^glE|O+YaH%W&ZgPKSO^kZiB#k9 z9wRJ=HwY?ye0y|?`Z0SF{CF?{AI7MRqNd9E0FC19iR+s0L0Zu5{76%F)bhzz=NDnyi}L~-3aeQ1~k&4 z2C!nZ|K5ropj>ZS&>{v!%-g#e3ye9FR>gi1&8B>sp1tbqwI zQG4h^2|=wrVIm$w>kv1q+B+yS*1HM{`J(VD=N3LX1gU4v2zid5Xe< zP*cLBG8sko5#zAS!d&rt8irAr178>aO)tUcW%_8W-qwzsK1QDpy&Jyuc)dD0-qMvt z7;7)*mo`&X+WYf& znxnJ92N`PVcyE42($X{v2}pqD*;1>oU*YT#mEjPZI+@DLbEVWG=_GSt!ou3IarrwU zsno@G9IQ-xmh*7~KF?rkHu(<}667GqZcohg-SXL?C!;TO&eUkq8wM;&-B|zK)2p>N z9}l5{+FvQQC+E*^*U>d#lyu+^7>;t*Aofm zDIYBIrSek8v^gI$fet!T03Wm6Wz0{{FS@Cwe~(3sK_ru~=;tzxp~*#-RfXd`#Y{Jc zA+e)UeKRqQuEUX@Ex=|0iqR!0`cxS;C!NYl4#!fq_Iiv&e)-o3?gHpb%RnGMw6reB)9U*AyD+^VMI=1UGqmy?rCHJspx)P4N$6GnK7$j|!$4S=UFd3@nn zSnLoO+h;#RC-#ao(S|Kj_=I&9tLzcKUAm)qH1$;~G7^ zJPCBMpNf%QMPIkH%X2ANBDUJZ&Tg+qp6ARKE>nP&nLUj#(~?ow0vslEE>cpcO6l(v zXRA@k*&I&DG;sXo)0n@7!hpUfmS4<^Bgdcn7f;=_`pom2Vsl%3^S+G&XSPnT0$K9 z;R^RJZ17qJ0e(~+oJt}zudnAY$$!fYd4^tuW2nc|I2Y#s-X@P<)0d%=MC8tF=pCCo zE~*R!u(ssEw4PbkWb$D9KxS-`Sk5*OXR6^CrL^8_!7rtg4VnB%c0W|)-w1*c z3MV6fLB$BS?s8;^bpkC_erFHoQsVY8z3|WQ69iTr9sKx}vmef0<{{1Ol7p<5xPx3! zyoi7)#%gQ&ydooBq<3Rv=Z0Ken<1D%z}KmR9y$<_Dn*ovL1U0=x{iCA7QCl zE&!w9#;|fl7-ADPw!(h({(00+C~ni3++kHOTG`2z&K$8;hrEP%&kRLkBXt^$IeK9x zRk1ch{mPQ}ng^c8YNn~c-i1YA#yTZm{?t-VX(ADYJ$*fQd>%j9vcIbjBwRXk>Lo`E zLJ*Ko{DSpH9H1hRHoke&rrze2XwJLkaFo({(1jy4RLUyV&qi9Yn=riO6keFx0Em(e z`)87I7JVq4Q8Y_@eusDpYX0pBVKctENf_D=8FRHHn`cg3qr3fPFZ_n^)q-71Do8{F zW##yf4A7faoSb1n+5QhD==l!9rQMr=p*68Y1i}J1lJ5RX7*8~X1LgdmWV9yX+-_(9 z>;HymWem_s#Q$N2dfhlonh*g1y@{or6Ar-izncPvBw)Z;nK_y~R!ZSPSy&U0D3Lh+ zAEsbtVr2V2D)O+lodX^R>UXU{f7~?|y@ljVJXDe329aGLCUhdcqemc0+e{17g=8&x z>v?alg?eJu@}|9+9%6*xjd!YVnD5CS*`te~XeQ36s8oC#@hrWl(+-s3d`l= z^f~o3j9V2);bAJSaWho34edJ%cij2kPHcw%&e#UKG# zIFhJZ;dA3|s4P4afF@9wV^cyQPQ2WhSWw-mi*gWN@qAEDCX*oCe5H}Bs0LX6T8tWQ z#Dw2Upxil@&a&X?bE!DaD9~IPpg{s=I!NQ1SjMhvPE7Ei?S1Lm|J9Z@EXEUcSrzia zvZ&0O0LcDN&?Gt)PT!`nVGgB%uvBqMftj0NP>_+C zt|4@yi>$Q~cXci@`HssIo?bQ)g?kh4Qx%>?u@Y81tVKH?mK^RG zXzu0HrL#9z$@BYM@OxSY|H(xRY4;u{0_xD~?+J58&aQnI_hZcylezkn-GTK$dW#B+ z@jib!5PRM1j9@c;(yV@7?*1_KmTDCscb*SkUmpu`nW%Bn4`%p6tdm(7>~)4gCI|4X z92#dVg~O_(TvVOyr88B;vC@vBHiWU#rUlJ*umWvMF$Cf2q9^aNh>m&V)Yn{LC?yjyM-^UR*NfcMYgyG6$ zm5(pnOYeL0;T?XH_7Dp#=3R9+kQZ&W#Pc^(-YK2FBOtH|0-#Aee%jn2n2zIaP)M<9 zK?Hp#3O}37i9oz>S0z%)IC0_aoWt;9MX1zz)L1^t%d0OxKA7<V?J$O+N$b>BATS6cHG9e=0mW>204Nj=&QxllKa-Rwx2SELp&xbPI zojN-(;$u+~l{Tq>R>q_$cvyhtA{AY6ek+$PlX<*4D9kGGa<@z3sLKNOyzlrM_-fML ze9yDqZf*{%`Ec~M=g6IYBTWH4c4tn%y`LNQk*!6Rz^!_CaTue3>_;VWo-E4cQPLM= zT`nN@W#4gG1`anW-7r8twjY}&_J{=q6oVSkyopMn4`g?Xp`|7uGXxR!j$+oF9T&o70KYVJUuvBoh7 zqL@e80Y8S3?sWy!ZC)KJ5+2wu?jWS~zBgiV{#Vo&cDOs!{Sj!&qD>q3`ypu{bS1~J zz}s59zzko?+Zl>EiVmk?2y5R$5|LSw+f|WXo2!cSXzxKvVGQe;e?kFY8 z$FE+M$L(;^o=aUk)r8IV6ox#PQ>9e9J zuFfYG_SQz8RIM80q)0D*@9z98P{o0mg1sqqDh?4xaYvo{P6oQ%ERsJ#fo7RvsLoB1 zz-(+8z!D!}3L=w0T4~8MG8Iu{1SDyHy_;WsG_yw+G|19!i^s? z^YZDICNTqMExH}OY5mbrQ-2m69o|dk)wwbP9U;~V)KAUAGB|U@af@H@6aIA{2es}` zoBeI3PO-oPu~XWblbR=atF8X8_OzITc$w~Rr|DgOi{4lOoF~0IrgQqvDQ_7f^rSmg zkUH)3$?<(uPC79fVWsQ=X6GXJC61wofqn1jPAV5LS;%(MOiEuGKiPy6h40+)*-+l| zbt>P2`^)tHyj`%ce)7qgWc3dA z!r4T0c!+?_%1*>c^gp8&Uf%y((_mp@{J%7fYh5Xi!x5C8**Zme$ULZ=ozDF(t~H!l zpBdtC%~mof$q+J5^a{uOnD6eYjx5bg1XOgX>6l$zQ4s3Fw0G;e6NOcW@Av*pn%|M} znFe~hdN}@O<>acBRhQ?>y)3_>^NwPaGrQ|jpF-xZJU&%wH!rOdUBsG(bACRZ-Zu+@ zxpfpj-QQ4MdU{Oauq9iY=V~($^P&G3em;zQD$AA^k}$hus1X`U0~|OfI+r%#=O=8b z{3W|Z6M`b?hU=2H%mtvBH_3yaT>g@OAgQskWs+&LKB82t_F0a&>JNe@oS|9`K-ff5 zBALwZgito5I#hfw%Z?@r+eD&cnv)s<L;zA>RK|3#S# z@UxjTe-s#&p!UHp8s%R6X48{`ovh_nMa1B;;Py$&y>!|c9C&UDzK46T*OsmLdBpVZBT|7KY}num6iLh{_7wU;jEi3!>yyi=SXw|PXYwW z#kSZ~v|f89V#ZFQ-P(5!Ah^0KK|_g9TbZctxE4W#kx#O9(j7J5edY}F)3rt2T2z8+ zS=#8gZB?<<>B!-2P|GiF;+JTF;%YAl*^--%V1GuEyJd0E$^D8c$yFVJ^dV%Z94N(+ znEeSKiVPZ;tve!}dc6F0D^H*NY;bTBI^`M`twvX$SEV9X_H@~h-v9&r;NsS*siEK9 zaWJ~7BCgO_6>bFTiw(yZhrS3shc?CA<~x?Q7WM#CwRZ1Z%M^%^@EH|A8A~FFfs%oo{!?; zW62Pi8NE`Cf$NZji_*T+tNpZN)v*bS+1H}mFU*6a@jDSmsS-p=#am`PME1Rn=onvb zE9?PWNW}ln;Zst|8i19Q9xVsrF#S14J6t^0c5=&Hv1247Uy?NS~fN~C5W-4!8Y0D<;LH@9?d(tWo}0n zMb8v6zCqc3*6Z?m?|f-**Io{9?FK-1%WdMyN)A%C&xvZe#T-4da?FdIDQ;7cja%Fb zWF5Vf1^Lr;7X!GG5pb*-UHC*ph(rOZYo*#EXeL1; zD}VfqVcbu0Nua2j4Uih^e-AOl&@#nT@0h>*%_%*|-n*E-=LUewq^mQ43tvHN1(v9% zqeF#LcHQK`(FXa3)S#f!=KXZhoqiiPIV-5P_(T7|Le?W@qUW|P(D<^GZQ@pUYN;zs zTB}X3mV5xkjp5otB}edc>e?0XKoDCX;S{ch6JoCM7&dY1JisvPpO9}Cml+X*T&+FX zz%%_eM8?pVOd=Y3!f14?Xn!;y(`iq7?QSS)lUu|F<(Mfnut_*KJ7vZ}+2L!*3#vg@ zf@P?KN5IP=SoHagFz596>97{3?d}#+v$W3ODNlim@+0*aWC@zKB}%%XX#k7&N&$V3ZfrIk0f6HC05qi#Dq;B$li9}7PxUl4?TP$KmL z#z6Ul0jt~7Q8oKH2n=mGE) zj?9X!OGR3#d@T7r=$KqD7tN6XA&CO>!{y>&ZC;CFZ+Ix8;ZCp4uBkfi^2mqs;Ub&PtvnWa zYkn||i#9v1yLj_fkvIkJ-Aj0M+^z4vcBEP%AsEee_%!;LYveG{vNu)qb^~E1nT;quE0^2U?T~(VOTz7Y)R4+Gs-dv#8?( zgW(n>(DsV9~eIt))$2OZ<4{dUUnl_(R{@Qk@zU&GCFr(;wJDeGi(L7h5hxV-%>ZH7&Pz8AIZ zaaLO-;NF>0&}qO(NCZUhvN8ygC;>KB5?*Sx|C{ZD>ZmEj!}{Cz^VzEDV%RU>ngGVT z{c#8x`Ury7g~jN?d+H403~~JhI{KHvUFDUU69%$v`q@2z#t{Qx#-IUJxJnwVo4oA} zT#yGe>XNpSyKXMHCFTD54R5Hp?gVlYeFJ$bFDxvZ4`q(R!v<7-0j8U3_iqluSiV9* z6;;V@?Vpf}5O_1YsnZV*cU`Y7A&Uxu61Cw|DLO5j5X0Qor3>-;h(g zET5;QV}sLZ7=hB~_ScQtbg9^k!It@=fnbT-^c;_yP=q0ShvCP@E449>a(WznPF*Z#ub4)(vZ&9mHY{nl_honZllD^-kPKW$lz z5&MK+f3VIz?^|dleFrMw3(nHAaVvTq(eMF{#cu}Ip`S9Ax*#z{Xz$t!?$9{_&+~^ zhSm9LEbG6eLbNc{i<*pU9ZqZFy#^#%8D|j^S~mC3V1j}P63&TusX2Nipw5ontm0qY z-2$0tW7=`2Qma{P!DS0GZ8reNBrkan5at>A%+;?ELhCr<$1#egax&sjtOkJhbQ3e_ z(M$&Ia7g|=haf6>&%GL?jh#>5uM?!$SUZxWyE{rSW@eWEqhDTY>%?ucWBTVA6iDEe zYwC+N3KA+e54ZFc+HtR?+5SS;VVO6!ZBNXR`t}phH@y`0$(~_F8U_^-FSUHo?Rm^` zkpz19JU+#VTgajk!{u)8m}OdI;tH2c*srcLJViy3BtqwSC*`qg#6FRy~(oiF=Sl zK3cZiJ>)v6Ufi0l2clAuG)xmw$l27NX?A{{VjD*bcTk1Z7mj7i8_ldt2NH=gryP6Q z97KCiCE$=luw)R4k63xwHNiZj2yJ;xzpG%k*-Xl%paK_R2ih-6WbI91Zg+39=HEA+ z$IlN${CZX493m;$O;k}zG;=>ylG* z4{yGab3AGIJJ?AIvF#PXcrYDcelb|EmJl-jP7mmgfDFl1U4Zo~ zNmWPeL8MTeAyA}Ze|+qTTbz=_D%T*WD~9D4~U5vDDD4GZA>y z{Z|p`Cl-7|ZKA+4gZ|j19Pz(fDQh+OrtREJQ-=Q_Gmx~7jNG{L& z{;hGg*f6G2Ai9B1xU;2alq_1Dxb_ZO_m&?@NT477rII=qWPss2fC!J~XjS`% zf4@jU55te5!LQHnuvKKhFrETD+jBpFg=P3VHDJ-NkNB&h3E3P%X)RqJP4>hKo8BDz z1PutU6Sr?nHp$B(ZDBL=G|4=iGVw}aFj zsG`-)z%DSYQ?I-GmOh0JWw`D@gOJo#!wlT{nE7;6PseQ=e7$bv$L8Xi-`A3cqeOTt zmwAc_v(lX~u{xqb*6UX$n++NgFRa*+D^ zFB_kYZx%cJuZA_pgFaePJxojPP1w#DPSI%r zM7t)emI-x;!q6Hw;lj>g2poSJaf$+jnJB11dl2CV^`619yV|fJ^+#Xr%Wcw$kbZ%L z#V6t>t-q8PxUr3Q^|OMjK&pjmLj-V%J4(|Ia{_{y3-l~TX?-Gy>wz6zRtZI5XtCdyf@OT!795vVPp9%+#IpsyqOZ=Dr@r)?fI0UWpljt-aiEwm=W}%3O60#kvX?F+0eto<##*<9{nd}k z7452vcSzfBr->XsQqSrJG2Ou9zgKj5{X3E65H2K#h3^w3VhC9Kt=8;BVWxA^Yw;Mt zlt^7oai16HmLHj=wrNweH%NHsqT_ew%5Qfd3ttT*tQ}=vBs3>eJhvFvA)w+7H4*AF zN2RUsG7KgNlaNg_bi?8{_pI>0ZJt~Oh*LnjhFpKgN5QMN2~rOfek=g7q{K`#`c%;I z-zKt&Qxe$Ai1ISb@zd6HUlGUCosu6Pw_Fm`$;MO_r10>o^;-AV(%)P_e`;`S@=mB& z(cFhNdW11*6=>d<`e&ODCkTm;6k}zaAUE~?TEAm{Nm;r~@S*=8*`3UvnPL$cA}H`8 zZ9tA|l*-w;#XTn}8MxQ5bXoNov ztw1{$3)_zQ!skiwyGTgh#EV*e<0uU!Jv+{gUc_?96L^4r$btdERSQj7#`YgjpHn7p z)ADCy?kVJTgI1e;rPL+FDr^%%zIU~isg)`3(O(9~fC+Da#&65;vf_YYy;RZkN-7fh z$w|kD2uZf9AQ*xhyOZaX)nC$6v(`h!v1mm_nN?R`++MWQXZ80|fIRLZt&!3b+^b;G ziFTxXtqi^$nNlD!M**{3v+Cec0>5b^#7Z<2WU7I>5@K94!B)E*o2uHbOfus|OA zPZBs$o2)rL6H|FM*<5qD9*1kk3r}m2C-)qwLymcljchv4Pl~XCy=;ubGbGw9F^OJA z#Q1uY7@kKcTkYtFMeshvJwMN0v!$s}bLx6oY?ERO-XzdZ<#&6wE)riIquEH9_Df#! z^Rk;&o?k=rEi8|P8MC!!ESup3oixnc%>B*17`1+XWJyETVj8u+q1ZhQjXryo zXSfE}4HV=;n*r|YX7D$)#Bf#i>T(aAMlfNh?h{K=^O*42eB=$1g}m!q8t>fSK`_0F zPXY(Ejx%5ii7x#9Z9!G*%Z<{AiB`<@rU*vZfthNPSWI}H@!nJERwxuOH(JXLi@$R) zTcjc!%l+5Q#ZQs8WMwH;SL^<87i%)@6@+}E zk-%ajT1tk-5$hGi7v$B*_uowtN2D?dHrV*9&H<6r0$hb0bM9}8;L*1$Yi}O>z=U8a z_xZuc`!tU{P<3Eg&YV64^T%?YvT#c!y`B^h#g$V1Tz2Qlv^!J4^yMY(F?#aKJy*c3 zLHCr9_5n&nuDEdkN@I+dAog?5q-Smw(;8sjBt+VdU~SZ}Md)lC13jZQRGwq(f84^? zsP73k4xL>;Voo!fT}{MV!JWp2N6q5d7I}#pKM*LV$a(yD9WbGQVR~&uq0k(bh^oILBGr=T9kispAQrsb2wpo!HIEC7k_b_=LRw4Noy zr?K4Nz`>!Zej*ctqZ4_){@B@2T4>0jTv_|dKa+|Ve@UF13kHY{;w(hAh!w#I;9^;hxkS(WCx1WMT6tEKsFf?fK z5mRF*oPn~N$PR+Yb8dK;9TS9STaJUoiYAJ=)gp1>Q<-cDHhK$@!tbLfh6o21W3#B< z4?spIkepMFvvNa+yKS-l%>_`X@iZ?XZZ7)^8!^20XYy2_D}_(HcoRZKZ9@aa$V7N7 z36c;=J6lCdmH*N`4N|%9t1dfnE1!8e4!7Aa%e$VhifIhN7P-xBxRvYa6m&UM<%Fh) z7f4>^)(?EwD?`+-o!O&&h`!84+$ao#UWJBYg$T2#C?;0XvHA{784G^TvCUF%g$?32 z2`}Vl*KzI(S&T0$9p|%hfj^=P{eCo^M-^qg1H4sXl)*l`P%AAYPoAGTZ>qJe(zV!+BDp zo%v?4urzkdT%SLG#_dL=o5>K*RQaZSbM(x_akP_Q;_-&Z``?{Yw~ut4+tcY*9>>*$ zMjSN%X(4CT36|rP`-ANYKQ_qJ+=hSWWNiP+UjK`!&*HjpSvw^U&{@pz9AszdY@+Ioa$U&s zeLDa2_Gi=5<5ORdlr3IXpS!*?ZcjIVnOt^N_w{jU(&nG4qYx8Mkjogs%1qJZC+Dm7Q=mza*msx0^t}Dvxs@AY>DudL(In&%p$U081DhK%W zmdjRXbb%Te9W7!4uFOMrZ-wU%mDj9KLGx`?u-0GACMW4$=u1QR9l=D_K~VFAw~`j# zI><|2!-_~jDcfmiT2gwT3q?T+@V$~lxl>A4r-k<`@Y8t)Aniu@8m1j&Z6@W1Kp>p1_v5t=RJQY zRoi!Vcx;0b!o8>5WoA?hPIZ)}>E5uTw0Q#Mp0wR6d%5%7nq4v1-O7N@TdBjlr>#4q zZsRb00zNuRSy5?EYMp_xsQzP`(mjUK5V(sHx(yTs@N~{T*K|n)DtN__+=g!sQlQtZ zAxNCI!wds?smVzdDw(dMG->R30?>5dJM=d?!oOKEES_M?P3nDqgVixpY?}N&{FpZV zR#*^_N&GD!xO&rpbIKh}98>*wFfbFL>eX2y+fT_)1Vd*2gw_J#2UOk&`Okkt&7{XW z6aZK2FBAX?JSaQc|2VH&)d&D&&`JGHXst2i03rlX4z47p5A@anX23KofaQNviueE` zumH~gv0}A;NB|=L^I~!Ruf?l1Qwe|q1IozOTB8kcL;$e1{;~w@0sveI$P`GNT>tlg z^gjS#{yzZlS4+nMuNBpQwQle59B#PXQ4&-rutjJCtXXjGsK9Oyib73?v?9f1QtbV9 zmcxc;Sz7yAdjm8z?PvxuoMl&6l3GhrC<9DtSOqjKrVNsNped}HPSKuNgI6x7p?9_) zUV*B>yq^U^nu<=9Qt>~#X#(6@1o*;HGc3QcC1(*(_?tnopY5_ncUEWzq1PiH9~Si_7PlY)^~4^bN;K75Mo4|uk8 z^}VYMj59|_q&}Iz9BQXNL7R#aM<7uQ)`mUcvknn|6h}<-OQ|VPaxDnc5LXOh!rhyI z$*ut!k);udePW1f2aUj2N{w@wtdC=z7FE1M`-5*H3lX@f{MD*Q_;8$f(?RiDBFf!XbtC!jWwva{vJ^G(>tbb22+fV+s2k zG=k5{=)sPK#%yLYg99JEDb%I3DNF+py{ZHD)dS==lBTm`0f2RuB!g?;@!Jji`HPfd)xKvwyt)UA=RMH!C!B?OX^O(L({+Y z^Y$T&Bjmr37O3038NyMBitr1vL=;P#RxH?6T?O1y=ta%*nU>Zd^tknM^|Idg*LTzV z?)LGCwI6{?GQ&XHf5-6XA)`e%k^rtL7m;TF9f1e3HEM3A6LA+2tr$CH^TI0A4G@%$ zC!8jM{lehWHVvf;@C2`w#95j3yePM_)k!?v|I-L{PB{2bmIs7!L?mJcec6A%84QmW z;&xAuTj--Pfk^goWms9wlJCR%h3C@qa{%F+g)(%*b0;H?>~p5-x?wVEas#wr28R+I z|3CzgMGVB%?O_e`_ktG8n;>zf{#y&aY%Hk-1EeuHtla9?m_UQX+65_Xw&xgbE~gr>BOacw{M=h`*}RtyKS1K+Mk6l*J-2~)Cq*n6hezQ8iz&d>+kI8 zm_qcFUG_OfYw>Ni)8<~?3p0FP!#B^B@^5a`uFTd-LDBVs@vzL2KaYh|4G-%^g{SvF$t0t+oY8C#xonNuw*S9EzA7jVrrQ>GcMI;p z2hZRE0>RxT3GOhsGZ5U}ZE)8hgS!L>9w10?hrxq|aPpo1JLf*!TXm~ndUf|+`>}WJ zT5IFJY8y$n@*Q9Pusl>%PbdA32v}2F(yBJ=!hKPP-*n^%J2&1}*GL@oIn5aZ(4A^K)2^e*!lJ<4S zBZv=- z&PPzuz6R3RCK0{b5bY276}>CKUz&!=;TSOjY15i>SWGU`7sc`2=AvIt7%k1k`h2A^ zBsnHYOrfExm@zL>9avVO2L(%UGM=if%=dx!M{q5pJR-i3sRkfNWG||ZH6m8s!kEtx zINA2=1sEV_Gwkn6FuOXsn0MTBHA>4}k;Ws6R|Qk2!J#^q_`7NOC|BIWI{8-3wCh2T z*;d;M&6^!-ZX~WF442u=-!(rcP^*i-xLIZz{%HxAo@(`j!6kb?*E|tcr>Ddjs`J*d z8&;0_+u2@NA&ZIJWi3AIg^~RqLka`X;)0GLj3Me}+ zX4a8B=PPR6OsEOOt=Hx`>V7K|{3uJG{efBeA#&tM zV1WL`Uegg=?N$`A-`v)I**P{3T;P?Nz8aIfho!yy!5gk!a@=-EIjnuA5`M}L!Reuc zJ2~;ZIQb&@2b#V?^$!sL6up&fqImRnvl@LW&+91jy5H>-ByJ~ECNPzdSY$j~ULN%) z@adhvp^PhiFdTm6XB<*%4Absc?m*YuR{Hc!$CIdbqgh>6HeqT|Bj%e$F5bfs{_f}@ z7n9k%A>s~9{009wRG$35HBA|<&xm+P0FeyM8YJR2R#!w$E+l|hh9(g*Nt;zZ;x#oQ z|9^l}2jX85HsBvX+BUp|_`-rDDD+LR=a(43Kkgxdb1Bx9t%j`aTyzeh*b|0@_5Yx|32i2o;hIScZ8 z$^T?0X+yU_mZ1Avt^6mzC5Z-TBpkC=S9V!k}~=*i)KE)CRqcMr~xcAFGG#0i(t-*5+v4+PYc_6+I;u;tgWAt zXJlQGh*f_vm!oQr4-o5tSy@!P`%4iZKogSBc6h{w7{i3hq{#-k2wL1hD5u&;9}&@& zm6mbT(@KL3D`WxHTTvX%CDNe3Vk#_k=GV=r!lfi(g?*{1ReYbfUTag}GAbnw8I;r+ zm}Uj(W1;9KpDC4M*<%L1*4AJoNwlW1LgVL0vO{3NE)$vz?ay+p(xm?qmW5^rp(H3; zcn1Q_*jvBQ=VTKEg?C4;M z8L>!3G2(kN@2$`8f)FN6INQ=tMF7UWZL82WbA(+21k>o6afT$%no-q~m1BGmmQlBcp0ERn7XaiJT zfnb1Y8Qou7DacI$8!ogI15zT2N zHpZbE>{7l_TKDOUp$rcAU1-)+0ay%NM$<0u!T8E`E+SZXAGoknjRBEFB-H-nWb7^b zL8S8Y&@&{C(h|x16|8CzaHWX@g^t}h@mAK&^ zAx?ybx^L!dR}43Jo&xkgzXXAv(s>#aM+n)HHOQ`#WOD8~s!1&Fo`j_Lamd63fl! zaBTxQPtgsa2{5#5K^EaUeP}Fz}hxhy(zSgI(>N zbnIJL^@H>V5-v(uMyP3fL@o1LSc}tM^Ojl4pwkS-WMSDB-d!#rY>hAwdTHVjM21*P z>H2BV$zZS|O}4wwNtB3U$364{>6b=Zis@HLX=I)1lLA-f zr?BEUP4SjhAGHllq0=_%qSOq2?1(5;0>$8;pw7}Q_QzRKkk&Do%$Zu_79ow@n&WD; zD8h;0o~Cz=ITMXWu2VOetE;h-Cqne*(O~hafAWLYF$DQ*=JEHz0fe68%6m<}Xn6Y@ z_+SzyFQTHdsD4L}+$T6ASjTE{eTNnk{gwYl-5|}Ks6iYdcD@Cl@Xo7??O+l&nl%@x z2fP;&8o0?|kfP+|>cW8~U$D45KQ^AT#GLY>evSCUeeFPQYeR;NP9 zRl-n1GU|@cfN;q+yU{{O5k`d1nkmsZvx*Rxm+w)!v~@X6gA59VZqO+ zXMp5Exo%v2+5Ma*X#wK;k&iyWWv0Z(62W`Hxs!b6oK6oTMfA3gpBra1l|SJ$`_ueN z$En>8Spr9uwxX3Xb4iz{ESFES!5BiS+93v9`Kxi@gZCnw3FvExpkcGQQ2*c@3fdpd z^3`&F31ag;5rp2yI`G5K>?FSN=13AaW{@Gmovp*@9(Gz_SBWyVrLXxZ-U%pnZ+3U9 zGn2nCv^hP*WX~vMhHR6DcjFy7 z&3%_Yoy+Gb1Wai++V~j4w>ua1FCet`mi{>_DRT^doDPMT>*i-s8=9+Q%=`UgZ?V*c zte=8qqJGIrau-w6y)_doP5L_cwduvBByQ;EC}Ee;7j)P{w(O6^Y}>a^{xnKH>QeDI z?e;`j$cv|W0=8yRFs9dqQ*W8QW8X03{I*z8=vLV#5HXT7T9Rc2f9WkZYuSdd+%#WF ztwip}PjCLQJgpt}X%^PUg^6-R@O=5RUntZ`_+Vx%q8Xr2ZpK0nqQy{yh`R zg_(G}A^E_$sJPJ@c`qEGc?yX6xEqFoZ2QZy9%%AW?;5QOT<{bnpC z@O*Y3J?H#u>#Mv!rcHdU%2s4p8J2fWs>^o5!&vXKY&8bm=>KR^A`6082(?%7vS|;2 zMe50~t9`q3)q2t(I(MoZld~x)?Gj`Ni74!JDXMSVdBQmjf(XA^_ zxpm~pTbJ+n;$?kME(Hm8FE<~+Bno5twLw@Hr(oHpV198nNjhWv;B;dHJqwYYL zSH*MuvsAb5!}j+C`pMafxITfW|~Ya$I@>*%>7t#ugRKS3IQL*Wz9ZXhGZ+nav3 z*IV_SacMyflC^(&`*b%d<)_-{qS{!&5iEt>{Gy~P!TT}eV9$DHDQuo1-E$#xdR_zy zsBhFWlw+BxtjqrXVN1qiZ^=aS&0d{)f%*qY$lYm8)?__&DmNgfn^uoZI8uYY5x4(i zAA=yKpOUX>J-{)G(6Mx!vE5jA-b$+{M<$PRV`Fg-ViDRcD!%g-h+H*ia3F~ z!7&~1`(czWVH(hXH(i$EbB%9P+uMc(gO_5zfrh9cryO08(F`4JS$&>r04#nlzfJv zY`NVK%U_Rb`tBv|gZ9POg8tVK4ymvqsg+-$*LS6FW;VXnmCqC%IQh5Oux5-ooy>fP zJny*+P+y&_I!_pb-@7}1jI>UfAl>xME_exSv*A3HUYmklOC-v3s*gZ zp{F`yk}o%p_rqI3)v@SUx-;keXWBl~ARRc*xWzLqT^xz~mqk;Qfb2nkl_^XgPPY+S zLH_VHIo4Z-EQ@lJ4dmNv@4$9g)qx0vUP6b)2J0Z>d&hkmE8H7Io{MhH1yc<$i=>`W zPGpNr4BP;#ZFUjA8t#m}wmaCE%F8u>KmbqCJac=0DD*yQC)kryjz|X!YDjfxs17tY z0Am&g#)ks1t5YVu*2&Tqn33&9H<;8IUqO_7jZ+_XgL)6CkscH54F@d$|_g&Y^G`)p*Os@x^A7GkWIc!?^exo2=G*syhI(8 z9SY3%Fgo`2I_^&J3)rqvH-VHit-f`i@?`t@(y&<0)A~A}W#Iw7AmX=88_+KEu_6@_ zxYFE#pB+uO_h-%A%ick-ARv7~FGRzRIQ{VMQ{~HEt#dJM_=SHeA2V_IE`0atpwFqaP<7 zhdjs@#WC&cdeEKm^9G)NXN^+dJssBF@x$oCsi(@cdeztS@h!IQQgZBY75@ysY9;V% z_|WbwH(nlhr5k{HAOP8aZPqV%>&5iE-ZB0WT=Y9n9I+P|hIW_6^;;QZO}_7_7mv&Y z)d;>MoBwVQC3kV2AT+%5)~lczduv)-y@=s=0=N*xyC+P2G3-M3)eF|ZMgWCmTsWSn zN1vquETZMMnZ+j&?Nu=YT`S8UdN$EWtZ2*rcfx$^Naicp2@ptqd(_Q32ryiLh|0?& zy9Ooh>=ki;t)ASHC?xm>H3ofn@N*a)kD`lF-(B@F{@rMP<+b0T_ke;;mdxlgVu#lx z_DoSo+2D+G_0!1n4@h2CM*k_mGK{A`Y1de3E_D}r zY*6mqp?j%g4TJ0=@T_D7k8{H=&h4~K+%fd6NwTR+!+!ZEF;v=ElN7bdS}PRF!DYl9 ziCam@m0WPII6GJ~78u!HGZs0uW`#IZbCs*D4!&); z$1zT?{g~q%m8jBEV&4Lln9wMMn?kIN3$+0n=HXxuE69u$HBYU8iJbq@6poR@@bKTQ z!fc+VAmzwjVGC7|US43@ij`Kb1WlfTVSsy;6n@3sl1C1L1 zs9*q$P2p%c45ww8Ci4Esl1Vl-p*6zGl5w{pF#gu(R;glzmX#%AIqdfFM#8zvxXz*M>d?e?Njc}(;79=1S4sOUA|5u*N=3(O}Ltp?6NL+$DBxm6}b2# zqOx{|(GqqSz`1!$GQG^lgD)|uRPJrpS{ZL;waKadY9ih+_*;eePbQ@|_OrE%MTVIT z484VUl!uE%vQ5xJIT<08<=8T*beIBJRgvIE1p@dP}#A1THo&r_IrMbSn&i#(l zOAmGy1O^qtzaQ18%ohr*>mGebQoGFwn19`rOp_G zEu7OuKhn=bR4zq#i@~XbYVCEuShmhO==m9IbZDAfN4B~YUzAo3>>P*Vv3uy_@0UESR21qJ#3w~xgyD)et3E6#8=o<@iuprHPO zDTHLP?MgnHQ9gn!GHEx>_bO0JJP zD-JPDH|FoJvw7yS5GbKSvM4UsfHKwZVAho%uMB7eg0{H%wwQm=6Y}UrvX_GI2Te(W z*SP-rYUS|6jFIo<_>M}4>Be%HaMbw28a;zbQe7>ORf3D?(6ptkH~GlRG5blx_K~Il z+(4r1TwBY-h)+~wBk#*xG|?b z-1VWXj)EvV$X-@Qc4f>xJvOhT#BXn))FgJxNBsLutqbyU#hZ&Ep+d5~;QH|7(hEL< z54-~>M)yDfY&Jyawx}hY8t}gMR@%PXrgp7^S{F;ote9*o?I&29v!LR;6PYF{T?Z!M zyY#zu432MhVs9zws>ZGD3a>iqfP#NXEo=Qf>E260-l_ALVYf+Yr9~S0_ zx=BlOJx1Tk8!P8|BrsZ9$&?In#grVs%RsgyVY?>_A+|`;TcF-ZKQRDzk0}9oa>0&O6wy{98zVxw=J} zDrO=?9(YLv{pHPxg87rNai@G|4Q1NO+4wW!lBfh3J0%h#$M!9uShp{*0RLlmpqF^S z+qp&++tY8)m)Bdx!J@1nSN8z%QIz@Xr=!7Fhyqq7MBkX3R7V9iGh1$oz~P3~nI1?p zVXjIUM;oJq{=mXc0zukvQk6fk$Dic+nn?hVkd>ytQyg(kTMm~)VMgvQmJJz2!=S!b zH=++NAS20Tza`cgK6@(%=50Cp^BieTl8kG-bd+kmGP5Y*HyCQn?tEfVqi|95k4v9f zYswKAU{@d#BiUV7r z?KQKRPr0yaH0JV6>O_+f9_O*u(P6hnxZwdm(R$QKciCXv620Tw)e9)-kScb3*fLJD z*4qo3N`p18Tc=}WYB#N;zhxI2z0!0S%2IY0kz)|l<*+lyTB$EXjx!S<8SN3J9})fOoCZ)C4^ctsdh-*r3AXLeW`b6v!kUI(8t9bonkh?o5LPfDRm~Wy zpDtbU2a|+$k)MXmq|secHMy#9{C&Ns{pi~d(;H?8N?avWmfR%+=yF(AgPLisqK*4z z1TXW=EHqX;1T*-Lb73|#+IK_7SmDx3LFwZ*oQXsYxSwyyi(We^6*Ys*sfC-z9`e`eS{k^Sz_33r6MQ^4yf&ncsLCqHQy~ zR@@AcMiCw6S@9UUuLRLz^0A_<+iU4q1sgo~XV1~5oR+LioSXgxq&WeIFmiHXF{xgx zLS#wUEXHtSprEWv?us&3!>N@cYgxtE04`@4asYlYVbS@sO{g%+QZb=#F&Da?5K(K_ znzD+=d(DKE`Z@YFx02+5KJa|fxI2@rF78F*IYPO^)A(q0?6NX@gZ+WS9`U&hU!;6M zHr?hzd@L-itYmQTn{5N6XYl)1hg$LPaMt?~D=d9dKv7UC`YMYF6fK8k4SE_WkRPLl z3kFi>t-nKEc?NVTknV)*P&*LHW$)q&yb!PAp72aJBY`eyoWA_NQmJiSE}~*&n~!Jc zUkFihyL`u;*_2^@+;^qV;9G`1OJNICGLK}amEHtW z>k(4n4*o_|AI;n_GECmOFbK^j@`NW!E(JX;rhPf7)<(z>E$3xU?1_{XXJpXDT~lxa zdiS%PSMG)0utqF6%@vq$zi7MTzg*{;IJVL-YpS&&97_Fkw+}R#ZvL$L$Z4)+Z8ul? z1KgrJI&v^^gs5tQQ!j-9=`L9Q9y90o7OMJ<_UfuEc>teEa2&B>zeU|44 z_|Tp8A@8TUK14$02Sxe1y4SRv=kj1B4P5G4N3`Yt^+$_lykhcoe-E{so!$Q){^R}E!s1nOvU2{5ME*r@>5YMMvTubHL0r8wk-UNL1aS0Nf5dW)l=c|j?R)tm$Pn?pfLhy} zobZ;O%kQ=l%I3|w%^QkZD6SXwXbS5Y6!{LJT>lV-WsNnyO{GIx#0k56NeaOi{6}MjG-U+$Q?&&9#$#+F)2l5hfBI1K@K26jd%ydqC;3TCcdQn70)wHxy{S;6lip*o%9F5043Tkg^N+d4Sdq3&7wTl+4% zb4MEWBY7iv3?-f2XkKWFuSCDWJzy+rK!$H{T-dd!-I%SX^8jf08{yn(omKfTh+ECP zlPbs4v&F49ELwkM&Bl5u`h@$4L~88adhs3*B_6?JnqAwMGLHY)+&y=eLHgv80NM}n z?~`3We^k7wKWXduj&R7f&|4zL6ac@0o&>t*pXjo2yZ6?2svxdRt0QGk9SJO**grQn zq$X%nOFEmD==JK7HJ35Prdo^-RFaq9KPQJBbwl2&ild(DbZVceMLl|8zmLh5H5S`D zTlg~HqBbo46ntVS%w^(N^)s3> z=N*D5mM^!z$((aU@UMeG>CP*{K9>wT0C`zIENr@Gahf%#)#R)ZApNGWbqtb1)6dv?#0_53#0ZZ@>@0-`s+fKc@Ne6TkxvddL3I6pm&*CPwp{^l`gy8uP( zucMr5J-hMq^owwD`f-W$d;&_v!R;_Y@o9t zr;kgTd`m=H&DCZ5-Qo;A4ho49`b5Z-6`uzk9w=}Ys?V!trcYimXu3Ej2GJKL?GAtV zGA^NL$0QbwY~{h^Y9XFHQqc*P9^wBomtB{9UGag%GKq07VN{qSCZF8TpWsHiBKvCBg~OB0{Zd+8!c~W)R`yP|cnXqA`sG$#NPaYB ziiwEI11qV&!Y%w}Xw}N=gN*=YoVdf7&jO)bVDZ%xhQnV6hjFrFUxcbS@@LYR4RnQ- zM;vBpKQqLO;gsVFoux7;*2Rw$y^v=p+QMgX4d-7s`0MTcr`HT|RNp?t+AmoAIswFG zfBPx9R56@9P8MgxeY6T$sQ8vVD?-&`Ebx?|@G9M3s6w-xG@4h4)37jb+QYE$B}!pk zZ}~x~7+LSWqC_)+G*C@&t{tk7+hR7_28(r_%$C8nL3IpLo)rBF=-Fx_Iz*xo&g!YW zCH$reNaryZ`x4l Date: Tue, 2 Nov 2021 08:44:41 -0400 Subject: [PATCH 156/169] enable expression double multiply --- gtsam/nonlinear/Expression-inl.h | 12 ++++++++++++ gtsam/nonlinear/tests/testExpression.cpp | 13 +++++++++++++ 2 files changed, 25 insertions(+) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index cf2462dfc..b2ef6f055 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -246,6 +246,18 @@ struct apply_compose { return x.compose(y, H1, H2); } }; + +template <> +struct apply_compose { + double operator()(const double& x, const double& y, + OptionalJacobian<1, 1> H1 = boost::none, + OptionalJacobian<1, 1> H2 = boost::none) const { + if (H1) H1->setConstant(y); + if (H2) H2->setConstant(x); + return x * y; + } +}; + } // Global methods: diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 80262ae3f..92f4998a2 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -293,6 +293,19 @@ TEST(Expression, compose3) { EXPECT(expected == R3.keys()); } +/* ************************************************************************* */ +// Test compose with double type (should be multiplication). +TEST(Expression, compose4) { + // Create expression + gtsam::Key key = 1; + Double_ R1(key), R2(key); + Double_ R3 = R1 * R2; + + // Check keys + set expected = list_of(1); + EXPECT(expected == R3.keys()); +} + /* ************************************************************************* */ // Test with ternary function. Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, From 2307fc7fa24f3bb946fe16908084f6f4db4e63d8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 4 Nov 2021 17:50:12 -0400 Subject: [PATCH 157/169] add printErrors method to GaussianFactorGraph --- gtsam/linear/GaussianFactorGraph.cpp | 29 ++++++++++++++++++++++++++++ gtsam/linear/GaussianFactorGraph.h | 8 ++++++++ gtsam/linear/linear.i | 1 + 3 files changed, 38 insertions(+) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 13eaee7e3..24c4b9a0d 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -510,4 +510,33 @@ namespace gtsam { return optimize(function); } + /* ************************************************************************* */ + void GaussianFactorGraph::printErrors( + const VectorValues& values, const std::string& str, + const KeyFormatter& keyFormatter, + const std::function& + printCondition) const { + cout << str << "size: " << size() << endl << endl; + for (size_t i = 0; i < (*this).size(); i++) { + const sharedFactor& factor = (*this)[i]; + const double errorValue = + (factor != nullptr ? (*this)[i]->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + stringstream ss; + ss << "Factor " << i << ": "; + if (factor == nullptr) { + cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + cout << "error = " << errorValue << "\n"; + } + cout << endl; // only one "endl" at end might be faster, \n for each + // factor + } + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index e3304d5e8..d41374854 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -375,6 +375,14 @@ namespace gtsam { /** In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; + void printErrors( + const VectorValues& x, + const std::string& str = "GaussianFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; /// @} private: diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index d93b14d3e..c74161f26 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -401,6 +401,7 @@ class GaussianFactorGraph { // error and probability double error(const gtsam::VectorValues& c) const; double probPrime(const gtsam::VectorValues& c) const; + void printErrors(const gtsam::VectorValues& c, string str = "GaussianFactorGraph: ", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; gtsam::GaussianFactorGraph clone() const; gtsam::GaussianFactorGraph negate() const; From 4bd80357f5a1d5e451dc54982bf8efb517ea7136 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 6 Nov 2021 13:46:19 -0400 Subject: [PATCH 158/169] Use Eigen expressions more effectively and kill & in code. --- gtsam/geometry/Pose3.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 4bfb574b1..8e43105cd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -85,20 +85,20 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose, /// The dual version of Adjoint Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_x) const { - const Matrix6 &AdT = AdjointMap().transpose(); - const Vector6 &AdTx = AdT * x; + const Matrix6 Ad = AdjointMap(); + const Vector6 AdTx = Ad.transpose() * x; // Jacobians // See docs/math.pdf for more details. if (H_pose) { - const auto &w_T_hat = skewSymmetric(AdTx.head<3>()), - &v_T_hat = skewSymmetric(AdTx.tail<3>()); + const auto w_T_hat = skewSymmetric(AdTx.head<3>()), + v_T_hat = skewSymmetric(AdTx.tail<3>()); *H_pose = (Matrix6() << w_T_hat, v_T_hat, // /* */ v_T_hat, Z_3x3) .finished(); } if (H_x) { - *H_x = AdT; + *H_x = Ad.transpose(); } return AdTx; From 238563f0e5c164014e1624982ba797eb41b14d7c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 6 Nov 2021 13:51:15 -0400 Subject: [PATCH 159/169] Cleaner Jacobian. --- gtsam/geometry/Pose3.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 8e43105cd..d0e60f3fd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -93,9 +93,8 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, if (H_pose) { const auto w_T_hat = skewSymmetric(AdTx.head<3>()), v_T_hat = skewSymmetric(AdTx.tail<3>()); - *H_pose = (Matrix6() << w_T_hat, v_T_hat, // - /* */ v_T_hat, Z_3x3) - .finished(); + *H_pose << w_T_hat, v_T_hat, // + /* */ v_T_hat, Z_3x3; } if (H_x) { *H_x = Ad.transpose(); From c4cd2b5080d2a20bb2c77037821def2a4d21b157 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 18:05:58 -0400 Subject: [PATCH 160/169] fixed formatting (plus small fix: std::vector -> fastVector) --- .../tests/testSmartProjectionRigFactor.cpp | 525 ++++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 121 ++-- 2 files changed, 350 insertions(+), 296 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index a2a30e89a..8e6735dbd 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -19,15 +19,17 @@ * @date August 2021 */ -#include "smartFactorScenarios.h" -#include -#include +#include #include #include -#include +#include +#include + #include #include +#include "smartFactorScenarios.h" + using namespace boost::assign; using namespace std::placeholders; @@ -37,15 +39,15 @@ static const double sigma = 0.1; static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); -Key cameraId1 = 0; // first camera +Key cameraId1 = 0; // first camera Key cameraId2 = 1; Key cameraId3 = 2; @@ -56,15 +58,15 @@ LevenbergMarquardtParams lmParams; // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor) { +TEST(SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor2) { +TEST(SmartProjectionRigFactor, Constructor2) { using namespace vanillaPose; Cameras cameraRig; SmartProjectionParams params; @@ -73,19 +75,19 @@ TEST( SmartProjectionRigFactor, Constructor2) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor3) { +TEST(SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor4) { +TEST(SmartProjectionRigFactor, Constructor4) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartProjectionParams params; params.setRankTolerance(rankTol); SmartRigFactor factor1(model, cameraRig, params); @@ -93,7 +95,7 @@ TEST( SmartProjectionRigFactor, Constructor4) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor5) { +TEST(SmartProjectionRigFactor, Constructor5) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -102,10 +104,10 @@ TEST( SmartProjectionRigFactor, Constructor5) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Equals ) { +TEST(SmartProjectionRigFactor, Equals) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); @@ -115,23 +117,23 @@ TEST( SmartProjectionRigFactor, Equals ) { CHECK(assert_equal(*factor1, *factor2)); - SmartRigFactor::shared_ptr factor3(new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); - factor3->add(measurement1, x1); // now use default + SmartRigFactor::shared_ptr factor3( + new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + factor3->add(measurement1, x1); // now use default CHECK(assert_equal(*factor1, *factor3)); } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, noiseless ) { - +TEST(SmartProjectionRigFactor, noiseless) { using namespace vanillaPose; // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK) ); - factor.add(level_uv, x1); // both taken from the same camera + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK)); + factor.add(level_uv, x1); // both taken from the same camera factor.add(level_uv_right, x2); Values values; // it's a pose factor, hence these are poses @@ -181,12 +183,11 @@ TEST( SmartProjectionRigFactor, noiseless ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, noisy ) { - +TEST(SmartProjectionRigFactor, noisy) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); // Project two landmarks into two cameras Point2 pixelError(0.2, 0.2); @@ -195,25 +196,25 @@ TEST( SmartProjectionRigFactor, noisy ) { Values values; values.insert(x1, cam1.pose()); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartRigFactor::shared_ptr factor(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); Point2Vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - KeyVector views { x1, x2 }; - FastVector cameraIds { 0, 0 }; + KeyVector views{x1, x2}; + FastVector cameraIds{0, 0}; factor2->add(measurements, views, cameraIds); double actualError2 = factor2->error(values); @@ -225,10 +226,10 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_T_sensor, sharedK) ); + Pose3 body_T_sensor = + Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_T_sensor, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body = body_T_sensor.inverse(); @@ -247,8 +248,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views { x1, x2, x3 }; - FastVector cameraIds { 0, 0, 0 }; + KeyVector views{x1, x2, x3}; + FastVector cameraIds{0, 0, 0}; SmartProjectionParams params; params.setRankTolerance(1.0); @@ -256,7 +257,9 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); - smartFactor1.add(measurements_cam1, views); // use default CameraIds since we have a single camera + smartFactor1.add( + measurements_cam1, + views); // use default CameraIds since we have a single camera SmartRigFactor smartFactor2(model, cameraRig, params); smartFactor2.add(measurements_cam2, views); @@ -283,8 +286,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); Values values; values.insert(x1, wTb1); values.insert(x2, wTb2); @@ -304,16 +307,16 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), - Point3(0, 0, 1)); + Pose3 body_T_sensor1 = + Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + Pose3 body_T_sensor2 = + Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3 body_T_sensor3 = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_T_sensor1, sharedK) ); - cameraRig.push_back( Camera(body_T_sensor2, sharedK) ); - cameraRig.push_back( Camera(body_T_sensor3, sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_T_sensor1, sharedK)); + cameraRig.push_back(Camera(body_T_sensor2, sharedK)); + cameraRig.push_back(Camera(body_T_sensor3, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body1 = body_T_sensor1.inverse(); @@ -334,8 +337,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views { x1, x2, x3 }; - FastVector cameraIds { 0, 1, 2 }; + KeyVector views{x1, x2, x3}; + FastVector cameraIds{0, 1, 2}; SmartProjectionParams params; params.setRankTolerance(1.0); @@ -370,8 +373,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); Values values; values.insert(x1, wTb1); values.insert(x2, wTb2); @@ -387,29 +390,29 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { - +TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views {x1,x2,x3}; - FastVector cameraIds{0,0,0};// 3 measurements from the same camera in the rig + KeyVector views{x1, x2, x3}; + FastVector cameraIds{ + 0, 0, 0}; // 3 measurements from the same camera in the rig - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -427,21 +430,21 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { groundTruth.insert(x3, cam3.pose()); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); + EXPECT(assert_equal( + Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -450,15 +453,14 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, Factors ) { - +TEST(SmartProjectionRigFactor, Factors) { using namespace vanillaPose; // Default cameras for simple derivatives Rot3 R; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); - Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( - Pose3(R, Point3(1, 0, 0)), sharedK); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), + cam2(Pose3(R, Point3(1, 0, 0)), sharedK); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); @@ -470,12 +472,13 @@ TEST( SmartProjectionRigFactor, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - KeyVector views { x1, x2 }; - FastVector cameraIds { 0, 0 }; + KeyVector views{x1, x2}; + FastVector cameraIds{0, 0}; - SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor - > (model, Camera(Pose3::identity(), sharedK)); - smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds + SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( + model, Camera(Pose3::identity(), sharedK)); + smartFactor1->add(measurements_cam1, + views); // we have a single camera so use default cameraIds SmartRigFactor::Cameras cameras; cameras.push_back(cam1); @@ -501,7 +504,8 @@ TEST( SmartProjectionRigFactor, Factors ) { perturbedDelta.insert(x2, delta); double expectedError = 2500; - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + // After eliminating the point, A1 and A2 contain 2-rank information on + // cameras: Matrix16 A1, A2; A1 << -10, 0, 0, 0, 1, 0; A2 << 10, 0, 1, 0, -1, 0; @@ -528,8 +532,8 @@ TEST( SmartProjectionRigFactor, Factors ) { values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values, 0.0); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values, 0.0); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); @@ -538,11 +542,10 @@ TEST( SmartProjectionRigFactor, Factors ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { - +TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { using namespace vanillaPose; - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -551,15 +554,15 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + std::vector> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); // create smart factor - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); @@ -578,22 +581,21 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x2, cam2.pose(), noisePrior); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, - -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); + EXPECT(assert_equal(Pose3(Rot3(1.11022302e-16, -0.0314107591, 0.99950656, + -0.99950656, -0.0313952598, -0.000986635786, + 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -602,13 +604,12 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, landmarkDistance ) { - +TEST(SmartProjectionRigFactor, landmarkDistance) { using namespace vanillaPose; double excludeLandmarksFutherThanDist = 2; - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -624,17 +625,20 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -646,7 +650,8 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x2, cam2.pose(), noisePrior); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; @@ -662,14 +667,14 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { - +TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { using namespace vanillaPose; double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = + 1; // max 1 pixel of average reprojection error - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -682,7 +687,8 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + measurements_cam4.at(0) = + measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartProjectionParams params; params.setLinearizationMode(gtsam::HESSIAN); @@ -690,20 +696,24 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor4( + new SmartRigFactor(model, cameraRig, params)); smartFactor4->add(measurements_cam4, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -729,15 +739,14 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, CheckHessian) { - - KeyVector views { x1, x2, x3 }; +TEST(SmartProjectionRigFactor, CheckHessian) { + KeyVector views{x1, x2, x3}; using namespace vanillaPose; // Two slightly different cameras - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose2 = + level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedK); Camera cam3(pose3, sharedK); @@ -753,17 +762,20 @@ TEST( SmartProjectionRigFactor, CheckHessian) { params.setRankTolerance(10); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, cameraIds); NonlinearFactorGraph graph; @@ -771,53 +783,53 @@ TEST( SmartProjectionRigFactor, CheckHessian) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); + EXPECT(assert_equal(Pose3(Rot3(0.00563056869, -0.130848107, 0.991386438, + -0.991390265, -0.130426831, -0.0115837907, + 0.130819108, -0.98278564, -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); boost::shared_ptr factor1 = smartFactor1->linearize(values); boost::shared_ptr factor2 = smartFactor2->linearize(values); boost::shared_ptr factor3 = smartFactor3->linearize(values); - Matrix CumulativeInformation = factor1->information() + factor2->information() - + factor3->information(); + Matrix CumulativeInformation = + factor1->information() + factor2->information() + factor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize( - values); + boost::shared_ptr GaussianGraph = + graph.linearize(values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); - Matrix AugInformationMatrix = factor1->augmentedInformation() - + factor2->augmentedInformation() + factor3->augmentedInformation(); + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + + factor3->augmentedInformation(); // Check Information vector - Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block( + 0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, Hessian ) { - +TEST(SmartProjectionRigFactor, Hessian) { using namespace vanillaPose2; - KeyVector views { x1, x2 }; + KeyVector views{x1, x2}; // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); @@ -826,15 +838,15 @@ TEST( SmartProjectionRigFactor, Hessian ) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); - FastVector cameraIds { 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); + FastVector cameraIds{0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -843,15 +855,16 @@ TEST( SmartProjectionRigFactor, Hessian ) { // compute triangulation from linearization point // compute reprojection errors (sum squared) - // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) - // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] + // compare with factor.info(): the bottom right element is the squared sum of + // the reprojection errors (normalized by the covariance) check that it is + // correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { +TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -860,8 +873,7 @@ TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, Cal3Bundler ) { - +TEST(SmartProjectionRigFactor, Cal3Bundler) { using namespace bundlerPose; // three landmarks ~5 meters in front of camera @@ -874,11 +886,11 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); - FastVector cameraIds { 0, 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); + FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); @@ -898,21 +910,21 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x2, cam2.pose(), noisePrior); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); + EXPECT(assert_equal( + Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -924,9 +936,11 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { typedef GenericProjectionFactor TestProjectionFactor; static Symbol l0('L', 0); /* *************************************************************************/ -TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { - // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark - // at a single pose, a setup that occurs in multi-camera systems +TEST(SmartProjectionRigFactor, + hessianComparedToProjFactors_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems using namespace vanillaPose; Point2Vector measurements_lmk1; @@ -936,14 +950,15 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam // create redundant measurements: Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement // create inputs - std::vector keys { x1, x2, x3, x1}; + KeyVector keys{x1, x2, x3, x1}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); @@ -953,10 +968,15 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -965,8 +985,8 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -984,30 +1004,31 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, sharedK); Matrix HPoseActual, HEActual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = + -factor11.evaluateError(pose1, *point, HPoseActual, HEActual); F.block<2, 6>(0, 0) = HPoseActual; E.block<2, 3>(0, 0) = HEActual; TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, - HEActual); + b.segment<2>(2) = + -factor12.evaluateError(pose2, *point, HPoseActual, HEActual); F.block<2, 6>(2, 6) = HPoseActual; E.block<2, 3>(2, 0) = HEActual; TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, - HEActual); + b.segment<2>(4) = + -factor13.evaluateError(pose3, *point, HPoseActual, HEActual); F.block<2, 6>(4, 12) = HPoseActual; E.block<2, 3>(4, 0) = HEActual; TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); + b.segment<2>(6) = + -factor11.evaluateError(pose1, *point, HPoseActual, HEActual); F.block<2, 6>(6, 0) = HPoseActual; E.block<2, 3>(6, 0) = HEActual; @@ -1017,20 +1038,22 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -1049,8 +1072,7 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { - +TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { using namespace vanillaPose; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -1060,21 +1082,24 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector keys { x1, x2, x3 }; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0 }; - FastVector cameraIdsRedundant { 0, 0, 0, 0 }; + KeyVector keys{x1, x2, x3}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; + FastVector cameraIdsRedundant{0, 0, 0, 0}; - // For first factor, we create redundant measurement (taken by the same keys as factor 1, to - // make sure the redundancy in the keys does not create problems) + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement - std::vector keys_redundant = keys; - keys_redundant.push_back(keys.at(0)); // we readd the first key + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + KeyVector keys_redundant = keys; + keys_redundant.push_back(keys.at(0)); // we readd the first key SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); - smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); + smartFactor1->add(measurements_lmk1_redundant, keys_redundant, + cameraIdsRedundant); SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_lmk2, keys, cameraIds); @@ -1097,20 +1122,22 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -1120,13 +1147,14 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { #ifndef DISABLE_TIMING #include -// this factor is slightly slower (but comparable) to original SmartProjectionPoseFactor +// this factor is slightly slower (but comparable) to original +// SmartProjectionPoseFactor //-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) -//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 children, min: 0 max: 0) -//| -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 times, 0.065103 wall, 0.06 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 +// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 +// times, 0.065103 wall, 0.06 children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionRigFactor, timing ) { - +TEST(SmartProjectionRigFactor, timing) { using namespace vanillaPose; // Default cameras for simple derivatives @@ -1138,8 +1166,8 @@ TEST( SmartProjectionRigFactor, timing ) { Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Pose3 body_P_sensorId = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_P_sensorId, sharedKSimple) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); @@ -1152,8 +1180,9 @@ TEST( SmartProjectionRigFactor, timing ) { size_t nrTests = 10000; - for(size_t i = 0; iadd(measurements_lmk1[0], x1, cameraId1); smartFactorP->add(measurements_lmk1[1], x1, cameraId1); @@ -1165,7 +1194,7 @@ TEST( SmartProjectionRigFactor, timing ) { gttoc_(SmartRigFactor_LINEARIZE); } - for(size_t i = 0; iadd(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1181,16 +1210,21 @@ TEST( SmartProjectionRigFactor, timing ) { } #endif -///* ************************************************************************* */ -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -//BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +///* ************************************************************************* +///*/ +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, +// "gtsam_noiseModel_Constrained"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, +// "gtsam_noiseModel_Diagonal"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, +// "gtsam_noiseModel_Gaussian"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, +// "gtsam_noiseModel_Isotropic"); +// BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +// BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // -//TEST(SmartProjectionRigFactor, serialize) { +// TEST(SmartProjectionRigFactor, serialize) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; @@ -1235,4 +1269,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 6a3e31dd7..09a16e6fb 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -84,7 +84,8 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); } /* ************************************************************************* */ @@ -98,7 +99,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); factor1->add(measurement1, x1, x2, interp_factor); } @@ -122,10 +124,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - std::vector cameraIds{0, 0, 0}; + FastVector cameraIds{0, 0, 0}; Cameras cameraRig; - cameraRig.push_back( Camera(body_P_sensor, sharedK) ); + cameraRig.push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); @@ -153,7 +155,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor3)); } - { // create equal factors and show equal returns true (use default cameraId) + { // create equal factors and show equal returns true (use default cameraId) SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurements, key_pairs, interp_factors); @@ -164,7 +166,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // returns false (use default cameraIds) SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! + factor1->add(measurement2, x2, x2, interp_factor2, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -173,10 +176,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { { // create slightly different factors (different extrinsics) and show equal // returns false Cameras cameraRig2; - cameraRig2.push_back( Camera(body_P_sensor * body_P_sensor, sharedK) ); + cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! + factor1->add(measurement2, x2, x3, interp_factor2, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -186,7 +190,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! + factor1->add(measurement2, x2, x3, interp_factor1, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -377,13 +382,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -450,13 +458,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1,1,1}); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1}); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1,1,1}); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1}); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1,1,1}); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -470,7 +478,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { Values groundTruth; groundTruth.insert(x1, level_pose); groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); // pose above is the pose of the camera + groundTruth.insert(x3, pose_above); // pose above is the pose of the camera DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), @@ -503,20 +511,21 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), - Point3(0, 0, 1)); - Pose3 body_T_sensor3 = Pose3::identity(); + Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-0.03, 0., 0.01), Point3(1, 1, 1)); + Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-0.1, 0., 0.05), Point3(0, 0, 1)); + Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(-0.3, 0., -0.05), Point3(0, 1, 1)); - Camera camera1(interp_pose1*body_T_sensor1, sharedK); - Camera camera2(interp_pose2*body_T_sensor2, sharedK); - Camera camera3(interp_pose3*body_T_sensor3, sharedK); + Camera camera1(interp_pose1 * body_T_sensor1, sharedK); + Camera camera2(interp_pose2 * body_T_sensor2, sharedK); + Camera camera3(interp_pose3 * body_T_sensor3, sharedK); // Project three landmarks into three cameras - projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1); - projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2); - projectToMultipleCameras(camera1, camera2, camera3, landmark3, measurements_lmk3); + projectToMultipleCameras(camera1, camera2, camera3, landmark1, + measurements_lmk1); + projectToMultipleCameras(camera1, camera2, camera3, landmark2, + measurements_lmk2); + projectToMultipleCameras(camera1, camera2, camera3, landmark3, + measurements_lmk3); // create inputs std::vector> key_pairs; @@ -535,13 +544,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { cameraRig.push_back(Camera(body_T_sensor3, sharedK)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0,1,2}); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2}); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0,1,2}); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2}); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0,1,2}); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -555,7 +564,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { Values groundTruth; groundTruth.insert(x1, level_pose); groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); // pose above is the pose of the camera + groundTruth.insert(x3, pose_above); // pose above is the pose of the camera DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), @@ -608,7 +617,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 @@ -702,13 +712,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -766,13 +776,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -841,16 +851,20 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor4( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor4->add(measurements_lmk4, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -901,7 +915,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1038,7 +1053,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor1); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1193,14 +1209,17 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors_redundant.push_back( interp_factors.at(0)); // we readd the first interp factor - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1244,8 +1263,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, #ifndef DISABLE_TIMING #include //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, min: 0 max: 0) -//| -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, +// min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 +// children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; @@ -1271,7 +1291,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + SmartFactorRS::shared_ptr smartFactorRS( + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 From dfd4a774549923eacd2a380ea0e7fd3b876a65b3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 18:11:46 -0400 Subject: [PATCH 161/169] formatting + const& --- gtsam/slam/SmartProjectionRigFactor.h | 240 +++++++++--------- .../SmartProjectionPoseFactorRollingShutter.h | 82 +++--- 2 files changed, 171 insertions(+), 151 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 3cae1ea64..e8b59cfe4 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -14,8 +14,10 @@ * @brief Smart factor on poses, assuming camera calibration is fixed. * Same as SmartProjectionPoseFactor, except: * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) - * - it admits a different calibration for each measurement (i.e., it can model a multi-camera rig system) - * - it allows multiple observations from the same pose/key (again, to model a multi-camera system) + * - it admits a different calibration for each measurement (i.e., it + * can model a multi-camera rig system) + * - it allows multiple observations from the same pose/key (again, to + * model a multi-camera system) * @author Luca Carlone * @author Frank Dellaert */ @@ -30,40 +32,42 @@ namespace gtsam { * @addtogroup SLAM * * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating + * conditionally independent sets in factor graphs: a unifying perspective based + * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. */ /** * This factor assumes that camera calibration is fixed (but each measurement * can be taken by a different camera in the rig, hence can have a different - * extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension - * is 6 for each pose). This factor requires that values contains the involved poses (Pose3). - * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! - * If the calibration should be optimized, as well, use SmartProjectionFactor instead! + * extrinsic and intrinsic calibration). The factor only constrains poses + * (variable dimension is 6 for each pose). This factor requires that values + * contains the involved poses (Pose3). If all measurements share the same + * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor + * instead! If the calibration should be optimized, as well, use + * SmartProjectionFactor instead! * @addtogroup SLAM */ -template +template class SmartProjectionRigFactor : public SmartProjectionFactor { - private: typedef SmartProjectionFactor Base; typedef SmartProjectionRigFactor This; typedef typename CAMERA::CalibrationType CALIBRATION; static const int DimPose = 6; ///< Pose3 dimension - static const int ZDim = 2; ///< Measurement dimension + static const int ZDim = 2; ///< Measurement dimension protected: - /// vector of keys (one for each observation) with potentially repeated keys KeyVector nonUniqueKeys_; - /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) + /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each + /// camera) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement + /// vector of camera Ids (one for each observation, in the same order), + /// identifying which camera took the measurement FastVector cameraIds_; public: @@ -74,21 +78,20 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typedef boost::shared_ptr shared_ptr; /// Default constructor, only for serialization - SmartProjectionRigFactor() { - } + SmartProjectionRigFactor() {} /** * Constructor - * @param sharedNoiseModel isotropic noise model for the 2D feature measurements - * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in the camera rig + * @param sharedNoiseModel isotropic noise model for the 2D feature + * measurements + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in + * the camera rig * @param params parameters for the smart projection factors */ - SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, - const Cameras& cameraRig, - const SmartProjectionParams& params = - SmartProjectionParams()) - : Base(sharedNoiseModel, params), - cameraRig_(cameraRig) { + SmartProjectionRigFactor( + const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.linearizationMode = gtsam::HESSIAN; @@ -96,14 +99,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /** * Constructor - * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param sharedNoiseModel isotropic noise model for the 2D feature + * measurements * @param camera single camera (fixed poses wrt body and intrinsics) * @param params parameters for the smart projection factors */ - SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, - const Camera& camera, - const SmartProjectionParams& params = - SmartProjectionParams()) + SmartProjectionRigFactor( + const SharedNoiseModel& sharedNoiseModel, const Camera& camera, + const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; @@ -112,24 +115,28 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** Virtual destructor */ - ~SmartProjectionRigFactor() override { - } + ~SmartProjectionRigFactor() override {} /** * add a new measurement, corresponding to an observation from pose "poseKey" * and taken from the camera in the rig identified by "cameraId" * @param measured 2-dimensional location of the projection of a * single landmark in a single view (the measurement) - * @param poseKey key corresponding to the body pose of the camera taking the measurement - * @param cameraId ID of the camera in the rig taking the measurement (default 0) + * @param poseKey key corresponding to the body pose of the camera taking the + * measurement + * @param cameraId ID of the camera in the rig taking the measurement (default + * 0) */ - void add(const Point2& measured, const Key& poseKey, const size_t cameraId = 0) { + void add(const Point2& measured, const Key& poseKey, + const size_t& cameraId = 0) { // store measurement and key this->measured_.push_back(measured); this->nonUniqueKeys_.push_back(poseKey); - // also store keys in the keys_ vector: these keys are assumed to be unique, so we avoid duplicates here - if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) == this->keys_.end()) + // also store keys in the keys_ vector: these keys are assumed to be + // unique, so we avoid duplicates here + if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) == + this->keys_.end()) this->keys_.push_back(poseKey); // add only unique keys // store id of the camera taking the measurement @@ -137,68 +144,70 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** - * Variant of the previous "add" function in which we include multiple measurements + * Variant of the previous "add" function in which we include multiple + * measurements * @param measurements vector of the 2m dimensional location of the projection * of a single landmark in the m views (the measurements) - * @param poseKeys keys corresponding to the body poses of the cameras taking the measurements - * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements) + * @param poseKeys keys corresponding to the body poses of the cameras taking + * the measurements + * @param cameraIds IDs of the cameras in the rig taking each measurement + * (same order as the measurements) */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, const FastVector& cameraIds = FastVector()) { - if (poseKeys.size() != measurements.size() - || (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { - throw std::runtime_error("SmartProjectionRigFactor: " - "trying to add inconsistent inputs"); + if (poseKeys.size() != measurements.size() || + (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { + throw std::runtime_error( + "SmartProjectionRigFactor: " + "trying to add inconsistent inputs"); } if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionRigFactor: " - "camera rig includes multiple camera but add did not input cameraIds"); + "camera rig includes multiple camera but add did not input " + "cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], poseKeys[i], cameraIds.size() == 0 ? 0 : cameraIds[i]); + add(measurements[i], poseKeys[i], + cameraIds.size() == 0 ? 0 : cameraIds[i]); } } - /// return (for each observation) the (possibly non unique) keys involved in the measurements - const KeyVector nonUniqueKeys() const { - return nonUniqueKeys_; - } + /// return (for each observation) the (possibly non unique) keys involved in + /// the measurements + const KeyVector nonUniqueKeys() const { return nonUniqueKeys_; } /// return the calibration object - inline Cameras cameraRig() const { - return cameraRig_; - } + inline Cameras cameraRig() const { return cameraRig_; } /// return the calibration object - inline FastVector cameraIds() const { - return cameraIds_; - } + inline FastVector cameraIds() const { return cameraIds_; } /** * print * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override { + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionRigFactor: \n "; for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl; - cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); + cameraRig_[cameraIds_[i]].print("camera in rig:\n"); } Base::print("", keyFormatter); } /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { - const This *e = dynamic_cast(&p); - return e && Base::equals(p, tol) - && nonUniqueKeys_ == e->nonUniqueKeys() - && cameraRig_.equals(e->cameraRig()) - && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() && + cameraRig_.equals(e->cameraRig()) && + std::equal(cameraIds_.begin(), cameraIds_.end(), + e->cameraIds().begin()); } /** @@ -211,12 +220,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameras; cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) // = world_P_body - * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i - cameras.emplace_back( - world_P_sensor_i, - make_shared( - cameraRig_[cameraIds_[i]].calibration())); + const Pose3 world_P_sensor_i = + values.at(nonUniqueKeys_[i]) // = world_P_body + * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i + cameras.emplace_back(world_P_sensor_i, + make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } @@ -236,9 +245,10 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * Compute jacobian F, E and error vector at a given linearization point * @param values Values structure which must contain camera poses * corresponding to keys involved in this factor - * @return Return arguments are the camera jacobians Fs (including the jacobian with - * respect to both body poses we interpolate from), the point Jacobian E, - * and the error vector b. Note that the jacobians are computed for a given point. + * @return Return arguments are the camera jacobians Fs (including the + * jacobian with respect to both body poses we interpolate from), the point + * Jacobian E, and the error vector b. Note that the jacobians are computed + * for a given point. */ void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs, Matrix& E, Vector& b, @@ -248,7 +258,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Pose3& body_P_sensor = cameraRig_[ cameraIds_[i] ].pose(); + const Pose3& body_P_sensor = cameraRig_[cameraIds_[i]].pose(); const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); Eigen::Matrix H; world_P_body.compose(body_P_sensor, H); @@ -259,35 +269,36 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// linearize and return a Hessianfactor that is an approximation of error(p) boost::shared_ptr > createHessianFactor( - const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const { - - // we may have multiple observation sharing the same keys (e.g., 2 cameras measuring from the same body pose), - // hence the number of unique keys may be smaller than nrMeasurements - size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys + const Values& values, const double& lambda = 0.0, + bool diagonalDamping = false) const { + // we may have multiple observation sharing the same keys (e.g., 2 cameras + // measuring from the same body pose), hence the number of unique keys may + // be smaller than nrMeasurements + size_t nrUniqueKeys = + this->keys_ + .size(); // note: by construction, keys_ only contains unique keys Cameras cameras = this->cameras(values); // Create structures for Hessian Factors std::vector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector < Vector > gs(nrUniqueKeys); + std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector gs(nrUniqueKeys); if (this->measured_.size() != cameras.size()) // 1 observation per camera - throw std::runtime_error("SmartProjectionRigFactor: " - "measured_.size() inconsistent with input"); + throw std::runtime_error( + "SmartProjectionRigFactor: " + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(cameras); if (!this->result_) { // failed: return "empty/zero" Hessian if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, 0.0); + for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) v = Vector::Zero(DimPose); + return boost::make_shared >(this->keys_, + Gs, gs, 0.0); } else { throw std::runtime_error( "SmartProjectionRigFactor: " @@ -303,30 +314,34 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { // Whiten using noise model this->noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++){ + for (size_t i = 0; i < Fs.size(); i++) { Fs[i] = this->noiseModel_->Whiten(Fs[i]); } const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); - // Build augmented Hessian (with last row/column being the information vector) - // Note: we need to get the augumented hessian wrt the unique keys in key_ + // Build augmented Hessian (with last row/column being the information + // vector) Note: we need to get the augumented hessian wrt the unique keys + // in key_ SymmetricBlockMatrix augmentedHessianUniqueKeys = Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>( Fs, E, P, b, nonUniqueKeys_, this->keys_); - return boost::make_shared < RegularHessianFactor - > (this->keys_, augmentedHessianUniqueKeys); + return boost::make_shared >( + this->keys_, augmentedHessianUniqueKeys); } /** - * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) - * @param values Values structure which must contain camera poses and extrinsic pose for this factor + * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for + * LM) + * @param values Values structure which must contain camera poses and + * extrinsic pose for this factor * @return a Gaussian factor */ boost::shared_ptr linearizeDamped( - const Values& values, const double lambda = 0.0) const { - // depending on flag set on construction we may linearize to different linear factors + const Values& values, const double& lambda = 0.0) const { + // depending on flag set on construction we may linearize to different + // linear factors switch (this->params_.linearizationMode) { case HESSIAN: return this->createHessianFactor(values, lambda); @@ -337,30 +352,27 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// linearize - boost::shared_ptr linearize(const Values& values) const - override { + boost::shared_ptr linearize( + const Values& values) const override { return this->linearizeDamped(values); } private: - /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(nonUniqueKeys_); - ar & BOOST_SERIALIZATION_NVP(cameraRig_); - ar & BOOST_SERIALIZATION_NVP(cameraIds_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); + ar& BOOST_SERIALIZATION_NVP(cameraRig_); + ar& BOOST_SERIALIZATION_NVP(cameraIds_); } - }; // end of class declaration /// traits -template -struct traits > : public Testable< - SmartProjectionRigFactor > { -}; +template +struct traits > + : public Testable > {}; -} // \ namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 84e3437a7..b8e048a34 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -11,7 +11,8 @@ /** * @file SmartProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time + * @brief Smart projection factor on poses modeling rolling shutter effect with + * given readout time * @author Luca Carlone */ @@ -42,7 +43,6 @@ namespace gtsam { template class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { - private: typedef SmartProjectionFactor Base; typedef SmartProjectionPoseFactorRollingShutter This; @@ -57,10 +57,12 @@ class SmartProjectionPoseFactorRollingShutter /// pair of consecutive poses std::vector alphas_; - /// one or more cameras taking observations (fixed poses wrt body + fixed intrinsics) + /// one or more cameras taking observations (fixed poses wrt body + fixed + /// intrinsics) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement + /// vector of camera Ids (one for each observation, in the same order), + /// identifying which camera took the measurement FastVector cameraIds_; public: @@ -72,8 +74,9 @@ class SmartProjectionPoseFactorRollingShutter /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation - ///< pose is interpolated + static const int DimBlock = + 12; ///< size of the variable stacking 2 poses from which the observation + ///< pose is interpolated static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 2; ///< Measurement dimension (Point2) typedef Eigen::Matrix @@ -84,14 +87,14 @@ class SmartProjectionPoseFactorRollingShutter /** * Constructor * @param Isotropic measurement noise - * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) taking the measurements + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) + * taking the measurements * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params), - cameraRig_(cameraRig) { + : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.linearizationMode = gtsam::HESSIAN; @@ -130,7 +133,7 @@ class SmartProjectionPoseFactorRollingShutter */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, - const size_t cameraId = 0) { + const size_t& cameraId = 0) { // store measurements in base class this->measured_.push_back(measured); @@ -164,29 +167,33 @@ class SmartProjectionPoseFactorRollingShutter * for the i0-th measurement can be interpolated * @param alphas vector of interpolation params (in [0,1]), one for each * measurement (in the same order) - * @param cameraIds IDs of the cameras taking each measurement (same order as the measurements) + * @param cameraIds IDs of the cameras taking each measurement (same order as + * the measurements) */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& alphas, const FastVector& cameraIds = FastVector()) { - - if (world_P_body_key_pairs.size() != measurements.size() - || world_P_body_key_pairs.size() != alphas.size() - || (world_P_body_key_pairs.size() != cameraIds.size() - && cameraIds.size() != 0)) { // cameraIds.size()=0 is default - throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "trying to add inconsistent inputs"); + if (world_P_body_key_pairs.size() != measurements.size() || + world_P_body_key_pairs.size() != alphas.size() || + (world_P_body_key_pairs.size() != cameraIds.size() && + cameraIds.size() != 0)) { // cameraIds.size()=0 is default + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "trying to add inconsistent inputs"); } if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " - "camera rig includes multiple camera but add did not input cameraIds"); + "camera rig includes multiple camera but add did not input " + "cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, world_P_body_key_pairs[i].second, alphas[i], - cameraIds.size() == 0 ? 0 : cameraIds[i]); // use 0 as default if cameraIds was not specified + cameraIds.size() == 0 ? 0 + : cameraIds[i]); // use 0 as default if + // cameraIds was not specified } } @@ -200,14 +207,10 @@ class SmartProjectionPoseFactorRollingShutter const std::vector alphas() const { return alphas_; } /// return the calibration object - inline Cameras cameraRig() const { - return cameraRig_; - } + inline Cameras cameraRig() const { return cameraRig_; } /// return the calibration object - inline FastVector cameraIds() const { - return cameraIds_; - } + inline FastVector cameraIds() const { return cameraIds_; } /** * print @@ -226,7 +229,7 @@ class SmartProjectionPoseFactorRollingShutter << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl; - cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); + cameraRig_[cameraIds_[i]].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -234,7 +237,8 @@ class SmartProjectionPoseFactorRollingShutter /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const SmartProjectionPoseFactorRollingShutter* e = - dynamic_cast*>(&p); + dynamic_cast*>( + &p); double keyPairsEqual = true; if (this->world_P_body_key_pairs_.size() == @@ -253,9 +257,10 @@ class SmartProjectionPoseFactorRollingShutter keyPairsEqual = false; } - return e && Base::equals(p, tol) && alphas_ == e->alphas() && keyPairsEqual - && cameraRig_.equals(e->cameraRig()) - && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); + return e && Base::equals(p, tol) && alphas_ == e->alphas() && + keyPairsEqual && cameraRig_.equals(e->cameraRig()) && + std::equal(cameraIds_.begin(), cameraIds_.end(), + e->cameraIds().begin()); } /** @@ -292,7 +297,8 @@ class SmartProjectionPoseFactorRollingShutter dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); auto body_P_cam = cameraRig_[cameraIds_[i]].pose(); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); - PinholeCamera camera(w_P_cam, cameraRig_[cameraIds_[i]].calibration()); + PinholeCamera camera( + w_P_cam, cameraRig_[cameraIds_[i]].calibration()); // get jacobians and error vector for current measurement Point2 reprojectionError_i = @@ -317,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter /// linearize and return a Hessianfactor that is an approximation of error(p) boost::shared_ptr> createHessianFactor( - const Values& values, const double lambda = 0.0, + const Values& values, const double& lambda = 0.0, bool diagonalDamping = false) const { // we may have multiple observation sharing the same keys (due to the // rolling shutter interpolation), hence the number of unique keys may be @@ -405,7 +411,8 @@ class SmartProjectionPoseFactorRollingShutter */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; - for (size_t i = 0; i < this->measured_.size(); i++) { // for each measurement + for (size_t i = 0; i < this->measured_.size(); + i++) { // for each measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = @@ -415,8 +422,9 @@ class SmartProjectionPoseFactorRollingShutter interpolate(w_P_body1, w_P_body2, interpolationFactor); const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, make_shared( - cameraRig_[cameraIds_[i]].calibration())); + cameras.emplace_back(w_P_cam, + make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } @@ -429,7 +437,7 @@ class SmartProjectionPoseFactorRollingShutter * @return a Gaussian factor */ boost::shared_ptr linearizeDamped( - const Values& values, const double lambda = 0.0) const { + const Values& values, const double& lambda = 0.0) const { // depending on flag set on construction we may linearize to different // linear factors switch (this->params_.linearizationMode) { From 1e384686a16d43fce3684223a1e02126cd053471 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 18:34:34 -0400 Subject: [PATCH 162/169] more const& --- gtsam/slam/SmartProjectionRigFactor.h | 6 +++--- .../slam/SmartProjectionPoseFactorRollingShutter.h | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index e8b59cfe4..d7e802658 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -175,13 +175,13 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// return (for each observation) the (possibly non unique) keys involved in /// the measurements - const KeyVector nonUniqueKeys() const { return nonUniqueKeys_; } + const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; } /// return the calibration object - inline Cameras cameraRig() const { return cameraRig_; } + const Cameras& cameraRig() const { return cameraRig_; } /// return the calibration object - inline FastVector cameraIds() const { return cameraIds_; } + const FastVector& cameraIds() const { return cameraIds_; } /** * print diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index b8e048a34..d16cfa2da 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -199,18 +199,18 @@ class SmartProjectionPoseFactorRollingShutter /// return (for each observation) the keys of the pair of poses from which we /// interpolate - const std::vector> world_P_body_key_pairs() const { + const std::vector>& world_P_body_key_pairs() const { return world_P_body_key_pairs_; } /// return the interpolation factors alphas - const std::vector alphas() const { return alphas_; } + const std::vector& alphas() const { return alphas_; } /// return the calibration object - inline Cameras cameraRig() const { return cameraRig_; } + const Cameras& cameraRig() const { return cameraRig_; } /// return the calibration object - inline FastVector cameraIds() const { return cameraIds_; } + const FastVector& cameraIds() const { return cameraIds_; } /** * print From 710a64fed42d15df30876341d4ebb7b08e38030f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 19:22:28 -0400 Subject: [PATCH 163/169] now throwing exception is params are incorrect --- gtsam/slam/SmartProjectionRigFactor.h | 24 ++- .../tests/testSmartProjectionRigFactor.cpp | 170 +++++++++++------- .../SmartProjectionPoseFactorRollingShutter.h | 24 ++- ...martProjectionPoseFactorRollingShutter.cpp | 99 ++++++---- 4 files changed, 206 insertions(+), 111 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index d7e802658..4bfd56695 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -92,9 +92,15 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); } /** @@ -108,9 +114,15 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const SharedNoiseModel& sharedNoiseModel, const Camera& camera, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); cameraRig_.push_back(camera); } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 8e6735dbd..ec3d5ddf0 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -57,68 +57,87 @@ LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +/* ************************************************************************* */ +// default Cal3_S2 poses with rolling shutter effect +namespace vanillaRig { +using namespace vanillaPose; +SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors +} // namespace vanillaRig + /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor2) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, cameraRig, params); + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, cameraRig, params2); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor3) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor4) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, cameraRig, params); + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, cameraRig, params2); factor1.add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor5) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params); + using namespace vanillaRig; + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params2); factor1.add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Equals) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor2( + new SmartRigFactor(model, cameraRig, params)); factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); SmartRigFactor::shared_ptr factor3( - new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + new SmartRigFactor(model, Camera(Pose3::identity(), sharedK), params)); factor3->add(measurement1, x1); // now use default CHECK(assert_equal(*factor1, *factor3)); @@ -126,13 +145,13 @@ TEST(SmartProjectionRigFactor, Equals) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, noiseless) { - using namespace vanillaPose; + using namespace vanillaRig; // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK)); + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK), params); factor.add(level_uv, x1); // both taken from the same camera factor.add(level_uv_right, x2); @@ -184,7 +203,7 @@ TEST(SmartProjectionRigFactor, noiseless) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, noisy) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); @@ -200,14 +219,16 @@ TEST(SmartProjectionRigFactor, noisy) { Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor( + new SmartRigFactor(model, cameraRig, params)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor2( + new SmartRigFactor(model, cameraRig, params)); Point2Vector measurements; measurements.push_back(level_uv); @@ -223,7 +244,7 @@ TEST(SmartProjectionRigFactor, noisy) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { - using namespace vanillaPose; + using namespace vanillaRig; // create arbitrary body_T_sensor (transforms from sensor to body) Pose3 body_T_sensor = @@ -253,7 +274,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setDegeneracyMode(ZERO_ON_DEGENERACY); params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); @@ -304,7 +325,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { - using namespace vanillaPose; + using namespace vanillaRig; // create arbitrary body_T_sensor (transforms from sensor to body) Pose3 body_T_sensor1 = @@ -342,7 +363,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setDegeneracyMode(ZERO_ON_DEGENERACY); params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); @@ -406,13 +427,20 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { FastVector cameraIds{ 0, 0, 0}; // 3 measurements from the same camera in the rig - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -454,7 +482,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, Factors) { - using namespace vanillaPose; + using namespace vanillaRig; // Default cameras for simple derivatives Rot3 R; @@ -476,7 +504,7 @@ TEST(SmartProjectionRigFactor, Factors) { FastVector cameraIds{0, 0}; SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( - model, Camera(Pose3::identity(), sharedK)); + model, Camera(Pose3::identity(), sharedK), params); smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds @@ -543,7 +571,7 @@ TEST(SmartProjectionRigFactor, Factors) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { - using namespace vanillaPose; + using namespace vanillaRig; KeyVector views{x1, x2, x3}; @@ -563,13 +591,16 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -605,7 +636,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, landmarkDistance) { - using namespace vanillaPose; + using namespace vanillaRig; double excludeLandmarksFutherThanDist = 2; @@ -620,8 +651,8 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); @@ -668,7 +699,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { - using namespace vanillaPose; + using namespace vanillaRig; double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = @@ -742,7 +773,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { TEST(SmartProjectionRigFactor, CheckHessian) { KeyVector views{x1, x2, x3}; - using namespace vanillaPose; + using namespace vanillaRig; // Two slightly different cameras Pose3 pose2 = @@ -842,7 +873,12 @@ TEST(SmartProjectionRigFactor, Hessian) { cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); FastVector cameraIds{0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); Pose3 noise_pose = @@ -875,6 +911,9 @@ TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, Cal3Bundler) { using namespace bundlerPose; + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors // three landmarks ~5 meters in front of camera Point3 landmark3(3, 0, 3.0); @@ -892,13 +931,16 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) { cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -942,7 +984,7 @@ TEST(SmartProjectionRigFactor, // measurements of the same landmark at a single pose, a setup that occurs in // multi-camera systems - using namespace vanillaPose; + using namespace vanillaRig; Point2Vector measurements_lmk1; // Project three landmarks into three cameras @@ -960,7 +1002,8 @@ TEST(SmartProjectionRigFactor, cameraRig.push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1073,7 +1116,7 @@ TEST(SmartProjectionRigFactor, /* *************************************************************************/ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { - using namespace vanillaPose; + using namespace vanillaRig; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // Project three landmarks into three cameras @@ -1097,14 +1140,17 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { KeyVector keys_redundant = keys; keys_redundant.push_back(keys.at(0)); // we readd the first key - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, keys, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, keys, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1150,12 +1196,13 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { // this factor is slightly slower (but comparable) to original // SmartProjectionPoseFactor //-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) -//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 -// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 -// times, 0.065103 wall, 0.06 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.06 CPU +//(10000 times, 0.061226 wall, 0.06 children, min: 0 max: 0) +//| -SmartPoseFactor LINEARIZE: 0.06 CPU +//(10000 times, 0.073037 wall, 0.06 children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionRigFactor, timing) { - using namespace vanillaPose; + using namespace vanillaRig; // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); @@ -1182,7 +1229,7 @@ TEST(SmartProjectionRigFactor, timing) { for (size_t i = 0; i < nrTests; i++) { SmartRigFactor::shared_ptr smartFactorP( - new SmartRigFactor(model, cameraRig)); + new SmartRigFactor(model, cameraRig, params)); smartFactorP->add(measurements_lmk1[0], x1, cameraId1); smartFactorP->add(measurements_lmk1[1], x1, cameraId1); @@ -1195,7 +1242,8 @@ TEST(SmartProjectionRigFactor, timing) { } for (size_t i = 0; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + SmartFactor::shared_ptr smartFactor( + new SmartFactor(model, sharedKSimple, params)); smartFactor->add(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1225,7 +1273,7 @@ TEST(SmartProjectionRigFactor, timing) { // BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // // TEST(SmartProjectionRigFactor, serialize) { -// using namespace vanillaPose; +// using namespace vanillaRig; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); @@ -1242,7 +1290,7 @@ TEST(SmartProjectionRigFactor, timing) { // //// SERIALIZATION TEST FAILS: to be fixed ////TEST(SmartProjectionRigFactor, serialize2) { -//// using namespace vanillaPose; +//// using namespace vanillaRig; //// using namespace gtsam::serializationTestHelpers; //// SmartProjectionParams params; //// params.setRankTolerance(rankTol); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index d16cfa2da..4a9481d6b 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -95,9 +95,15 @@ class SmartProjectionPoseFactorRollingShutter const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); } /** @@ -110,9 +116,15 @@ class SmartProjectionPoseFactorRollingShutter const SharedNoiseModel& sharedNoiseModel, const Camera& camera, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); cameraRig_.push_back(camera); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 09a16e6fb..1d32eccca 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -75,6 +75,9 @@ Pose3 interp_pose3 = interpolate(pose_above, level_pose, interp_factor3); Camera cam1(interp_pose1, sharedK); Camera cam2(interp_pose2, sharedK); Camera cam3(interp_pose3, sharedK); +SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors } // namespace vanillaPoseRS LevenbergMarquardtParams lmParams; @@ -85,13 +88,12 @@ typedef SmartProjectionPoseFactorRollingShutter> TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; SmartFactorRS::shared_ptr factor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPoseRS; - SmartProjectionParams params; params.setRankTolerance(rankTol); SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); } @@ -100,13 +102,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; SmartFactorRS::shared_ptr factor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); factor1->add(measurement1, x1, x2, interp_factor); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { - using namespace vanillaPose; + using namespace vanillaPoseRS; // create fake measurements Point2Vector measurements; @@ -130,15 +132,18 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { cameraRig.push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations - SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor2( + new SmartFactorRS(model, cameraRig, params)); factor2->add(measurements, key_pairs, interp_factors, cameraIds); // create by adding a batch of measurements with a single calibrations - SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor3( + new SmartFactorRS(model, cameraRig, params)); factor3->add(measurements, key_pairs, interp_factors, cameraIds); { // create equal factors and show equal returns true - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); @@ -147,7 +152,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor3)); } { // create equal factors and show equal returns true (use default cameraId) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1); factor1->add(measurement2, x2, x3, interp_factor2); factor1->add(measurement3, x3, x4, interp_factor3); @@ -156,7 +162,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor3)); } { // create equal factors and show equal returns true (use default cameraId) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurements, key_pairs, interp_factors); EXPECT(factor1->equals(*factor2)); @@ -164,7 +171,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different keys) and show equal // returns false (use default cameraIds) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! @@ -177,7 +185,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // returns false Cameras cameraRig2; cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig2, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! @@ -188,7 +197,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different interp factors) and show // equal returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! @@ -216,7 +226,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorId = Pose3::identity(); - SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK)); + SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params); factor.add(level_uv, x1, x2, interp_factor1); factor.add(level_uv_right, x2, x3, interp_factor2); @@ -291,7 +301,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorNonId = body_P_sensor; - SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK)); + SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params); factor.add(level_uv, x1, x2, interp_factor1); factor.add(level_uv_right, x2, x3, interp_factor2); @@ -383,15 +393,15 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor3); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -457,13 +467,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { cameraRig.push_back(Camera(body_P_sensor, sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1}); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1}); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -543,13 +556,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { cameraRig.push_back(Camera(body_T_sensor2, sharedK)); cameraRig.push_back(Camera(body_T_sensor3, sharedK)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2}); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2}); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -597,7 +613,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // falls back to standard pixel measurements) Note: this is a quite extreme // test since in typical camera you would not have more than 1 measurement per // landmark at each interpolated pose - using namespace vanillaPose; + using namespace vanillaPoseRS; // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); @@ -618,13 +634,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam2.project(landmark1)); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple), params)); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor); - SmartFactor::Cameras cameras; + SmartFactorRS::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); @@ -772,7 +788,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // this would give an + // exception as expected + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); @@ -916,7 +934,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1054,7 +1072,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor1); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1210,16 +1228,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.at(0)); // we readd the first interp factor SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1263,15 +1281,19 @@ TEST(SmartProjectionPoseFactorRollingShutter, #ifndef DISABLE_TIMING #include //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, -// min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 -// children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.09 CPU +// (10000 times, 0.124106 wall, 0.09 children, min: 0 max: 0) +//| -RS LINEARIZE: 0.09 CPU +// (10000 times, 0.068719 wall, 0.09 children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors Rot3 R = Rot3::identity(); Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); @@ -1291,8 +1313,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS( - new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); + SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS( + model, Camera(body_P_sensorId, sharedKSimple), params)); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 @@ -1307,7 +1329,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { } for (size_t i = 0; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + SmartFactor::shared_ptr smartFactor( + new SmartFactor(model, sharedKSimple, params)); smartFactor->add(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); From 7fa3b5cc96a1e7d91a6bd3e5338dcbb3acf8c8c6 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 19:43:45 -0400 Subject: [PATCH 164/169] added variable in loop --- gtsam/slam/SmartProjectionRigFactor.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 4bfd56695..f4a72694c 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -232,12 +232,13 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameras; cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { + const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) // = world_P_body - * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i + * camera_i.pose(); // = body_P_cam_i cameras.emplace_back(world_P_sensor_i, make_shared( - cameraRig_[cameraIds_[i]].calibration())); + camera_i.calibration())); } return cameras; } From 29f3af560d882d81d0349cfcbae74a44de687a33 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 19:58:33 -0400 Subject: [PATCH 165/169] point2 -> measurement --- gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 4a9481d6b..75bd95226 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -143,7 +143,7 @@ class SmartProjectionPoseFactorRollingShutter * interpolated pose is the same as world_P_body_key1 * @param cameraId ID of the camera taking the measurement (default 0) */ - void add(const Point2& measured, const Key& world_P_body_key1, + void add(const MEASUREMENT& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, const size_t& cameraId = 0) { // store measurements in base class @@ -182,7 +182,7 @@ class SmartProjectionPoseFactorRollingShutter * @param cameraIds IDs of the cameras taking each measurement (same order as * the measurements) */ - void add(const Point2Vector& measurements, + void add(const MEASUREMENTS& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& alphas, const FastVector& cameraIds = FastVector()) { From dfd86e8c57f50236eb7e5a9bff8f0c03befd180f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 20:00:23 -0400 Subject: [PATCH 166/169] this will need to be applied in #861 --- gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 75bd95226..4a9481d6b 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -143,7 +143,7 @@ class SmartProjectionPoseFactorRollingShutter * interpolated pose is the same as world_P_body_key1 * @param cameraId ID of the camera taking the measurement (default 0) */ - void add(const MEASUREMENT& measured, const Key& world_P_body_key1, + void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, const size_t& cameraId = 0) { // store measurements in base class @@ -182,7 +182,7 @@ class SmartProjectionPoseFactorRollingShutter * @param cameraIds IDs of the cameras taking each measurement (same order as * the measurements) */ - void add(const MEASUREMENTS& measurements, + void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& alphas, const FastVector& cameraIds = FastVector()) { From e0af235e532823fbefb87cb9c71dabcad6ef6637 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 20:06:41 -0400 Subject: [PATCH 167/169] disabled timing for test --- gtsam/slam/tests/testSmartProjectionRigFactor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index ec3d5ddf0..1ea982391 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -29,6 +29,7 @@ #include #include "smartFactorScenarios.h" +#define DISABLE_TIMING using namespace boost::assign; using namespace std::placeholders; From 3a4cedac1f32775dcb033398bf726e2ee699c068 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 12:26:50 -0500 Subject: [PATCH 168/169] fixed readme --- gtsam/slam/ReadMe.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md index d216b9181..ae5edfdac 100644 --- a/gtsam/slam/ReadMe.md +++ b/gtsam/slam/ReadMe.md @@ -42,11 +42,9 @@ If the calibration should be optimized, as well, use `SmartProjectionFactor` ins Same as `SmartProjectionPoseFactor`, except: - it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole; -- it admits a different calibration for each measurement, i.e., it can model a multi-camera system; +- it allows measurements from multiple cameras, each camera with fixed but potentially different intrinsics and extrinsics; - it allows multiple observations from the same pose/key, again, to model a multi-camera system. -TODO: DimPose and ZDim are hardcoded. Copy/paste from `SmartProjectionPoseFactor`. Unclear what the use case is. - ## Linearized Smart Factors The factors below are less likely to be relevant to the user, but result from using the non-linear smart factors above. From 2e8d373ff520126494361b2664f1e15a1ca88fe8 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 12:32:43 -0500 Subject: [PATCH 169/169] serialization is still off --- gtsam/slam/tests/testSmartProjectionRigFactor.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 1ea982391..c652f9b41 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -1259,8 +1259,7 @@ TEST(SmartProjectionRigFactor, timing) { } #endif -///* ************************************************************************* -///*/ +///* **************************************************************************/ // BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, // "gtsam_noiseModel_Constrained"); // BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, @@ -1276,7 +1275,9 @@ TEST(SmartProjectionRigFactor, timing) { // TEST(SmartProjectionRigFactor, serialize) { // using namespace vanillaRig; // using namespace gtsam::serializationTestHelpers; -// SmartProjectionParams params; +// SmartProjectionParams params( +// gtsam::HESSIAN, +// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors // params.setRankTolerance(rankTol); // // Cameras cameraRig; // single camera in the rig