From f6a432961a845dd89fbff337a8da8ad0c441162e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Aug 2021 05:25:56 -0700 Subject: [PATCH 01/47] 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 a4a58cf8030a7a0a72c0ce1d83fdddc3e0392e47 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 13:33:57 -0400 Subject: [PATCH 02/47] 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 03/47] 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 cd682fecc3301a8601e8f8bd4ffe951a6bf4bf15 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 14:31:29 -0400 Subject: [PATCH 04/47] 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 7083de35a4aea261ab5a2e16585a94307b890c23 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 15:54:56 -0400 Subject: [PATCH 05/47] 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 06/47] 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 07/47] 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 08/47] 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 21279e6d5107f85021ba0788681b0733502a79bb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 10 Oct 2021 16:20:30 -0400 Subject: [PATCH 09/47] 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 10/47] 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 11/47] 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 12/47] 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 35061ca1356b081f94712ec5f343fb9592cf0d21 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 13:46:33 -0400 Subject: [PATCH 13/47] 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 14/47] 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 15/47] 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 16/47] 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 17/47] 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 108c77b57a48c322ad2b70201d4e1d2d652b923c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Oct 2021 08:52:00 -0400 Subject: [PATCH 18/47] 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 19/47] 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 20/47] 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 21/47] 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 22/47] 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 23/47] 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 24/47] 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 25/47] 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 26/47] 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 27/47] 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 28/47] 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 29/47] 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 30/47] 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 31/47] 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 32/47] 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 33/47] 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 34/47] 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 35/47] 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 bc68ecb5ab2a01566de479eea741e14edcfcc43e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 22 Oct 2021 01:18:51 -0400 Subject: [PATCH 36/47] 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 37/47] 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 38/47] 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 39/47] 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 40/47] 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 41/47] 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 42/47] 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 43/47] 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 44/47] 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 c195bb562c060214d5a09fb225f3a47bd5127758 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Oct 2021 12:46:06 -0400 Subject: [PATCH 45/47] 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 46/47] 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 47/47] 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;