diff --git a/examples/ABC_EQF/G.cpp b/examples/ABC_EQF/G.cpp index 1fb765c4f..6cbdd1e3e 100644 --- a/examples/ABC_EQF/G.cpp +++ b/examples/ABC_EQF/G.cpp @@ -49,7 +49,7 @@ G G::exp(const Vector& x) { int n = (x.size() - 6) / 3; Rot3 A = Rot3::Expmap(x.head<3>()); - Vector3 a_vee = Rot3LeftJacobian(x.head<3>()) * x.segment<3>(3); + Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3); Matrix3 a = wedge(a_vee); std::vector B; diff --git a/examples/ABC_EQF/main.cpp b/examples/ABC_EQF/main.cpp index c11d3a179..b21eae44a 100644 --- a/examples/ABC_EQF/main.cpp +++ b/examples/ABC_EQF/main.cpp @@ -15,6 +15,8 @@ #include #include #include +#include + using namespace std; using namespace gtsam; @@ -159,56 +161,26 @@ void runSimulation(EqF& filter, const std::vector& data) { std::cout << "Average calibration error (deg): " << (avg_cal_error * 180.0/M_PI) << std::endl; } -// int main(int argc, char** argv) { -// std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; -// std::cout << "========================================================" << std::endl; -// -// // Initialize filter -// int n_cal = 1; // Number of calibration states -// int n_sensors = 2; // Number of sensors -// -// // Initial covariance - larger values for higher uncertainty -// Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); -// initialSigma.diagonal().head<3>() = Vector3::Constant(0.5); // Attitude uncertainty -// initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.1); // Bias uncertainty -// initialSigma.diagonal().tail<3>() = Vector3::Constant(0.5); // Calibration uncertainty -// -// std::cout << "Creating filter with " << n_cal << " calibration states..." << std::endl; -// -// try { -// // Create filter -// EqF filter(initialSigma, n_cal, n_sensors); -// -// // Generate simulated data -// std::cout << "Generating simulated data..." << std::endl; -// std::vector data = loadSimulatedData(); -// -// // Run simulation -// runSimulation(filter, data); -// -// } catch (const std::exception& e) { -// std::cerr << "Error: " << e.what() << std::endl; -// return 1; -// } -// -// std::cout << "Done." << std::endl; -// return 0; + + + int main(int argc, char** argv) { std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; std::cout << "========================================================" << std::endl; std::string csvFilePath; - // Check if CSV file path is provided as command line argument - if (argc > 1) { - csvFilePath = argv[1]; - } else { - std::cout << "Please enter the path to your CSV data file: "; - std::cin >> csvFilePath; + // Try to find the EQFdata file in the GTSAM examples directory + try { + // Look specifically for EQFdata.csv in GTSAM examples + csvFilePath = findExampleDataFile("EqFdata.csv"); + std::cout << "Using GTSAM example data file: " << csvFilePath << std::endl; + } catch (const std::exception& e) { + std::cerr << "Error: Could not find EqFdata.csv in GTSAM examples directory" << std::endl; + std::cerr << e.what() << std::endl; + return 1; } - std::cout << "Using CSV data from: " << csvFilePath << std::endl; - try { // Run with CSV data runEqFWithCSVData(csvFilePath); @@ -219,5 +191,4 @@ int main(int argc, char** argv) { std::cout << "Done." << std::endl; return 0; - } \ No newline at end of file diff --git a/examples/ABC_EQF/utilities.cpp b/examples/ABC_EQF/utilities.cpp index 74f4a97c9..e35481ad3 100644 --- a/examples/ABC_EQF/utilities.cpp +++ b/examples/ABC_EQF/utilities.cpp @@ -5,7 +5,7 @@ #include // Global configuration -const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL" +const std::string COORDINATE = "EXPONENTIAL"; Matrix3 wedge(const Vector3& vec) { Matrix3 mat; @@ -21,22 +21,10 @@ Vector3 vee(const Matrix3& mat) { return vec; } -Matrix3 Rot3LeftJacobian(const Vector3& arr) { - double angle = arr.norm(); - - // Near |phi|==0, use first order Taylor expansion - if (angle < 1e-10) { - return Matrix3::Identity() + 0.5 * wedge(arr); - } - - Vector3 axis = arr / angle; - double s = sin(angle); - double c = cos(angle); - - return (s / angle) * Matrix3::Identity() + - (1 - (s / angle)) * (axis * axis.transpose()) + - ((1 - c) / angle) * wedge(axis); -} +// Matrix3 Rot3LeftJacobian(const Vector3& arr) { +// return Rot3::ExpmapDerivative(-arr); +// +// } bool checkNorm(const Vector3& x, double tol) { return abs(x.norm() - 1) < tol || std::isnan(x.norm()); diff --git a/examples/ABC_EQF/utilities.h b/examples/ABC_EQF/utilities.h index f19b06c07..b3837ad22 100644 --- a/examples/ABC_EQF/utilities.h +++ b/examples/ABC_EQF/utilities.h @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -22,7 +23,6 @@ extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL" */ Matrix3 wedge(const Vector3& vec); Vector3 vee(const Matrix3& mat); -Matrix3 Rot3LeftJacobian(const Vector3& arr); bool checkNorm(const Vector3& x, double tol = 1e-3); Matrix blockDiag(const Matrix& A, const Matrix& B); Matrix repBlock(const Matrix& A, int n);