Used GTSAM ExpmapDerivative to calculate Left Jacobian(-1)

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<Rot3> B;

examples/ABC_EQF/main.cpp

@@ -15,6 +15,8 @@
 #include <vector>
 #include <chrono>
 #include <cmath>
+#include <gtsam/slam/dataset.h>
+
 
 using namespace std;
 using namespace gtsam;
@@ -159,56 +161,26 @@ void runSimulation(EqF& filter, const std::vector<Data>& 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> 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
+    // Try to find the EQFdata file in the GTSAM examples directory
-    if (argc > 1) {
+    try {
-        csvFilePath = argv[1];
+        // Look specifically for EQFdata.csv in GTSAM examples
-    } else {
+        csvFilePath = findExampleDataFile("EqFdata.csv");
-        std::cout << "Please enter the path to your CSV data file: ";
+        std::cout << "Using GTSAM example data file: " << csvFilePath << std::endl;
-        std::cin >> csvFilePath;
+    } 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;
-
 }

examples/ABC_EQF/utilities.cpp

@@ -5,7 +5,7 @@
 #include <cmath>
 
 // 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) {
+// Matrix3 Rot3LeftJacobian(const Vector3& arr) {
-    double angle = arr.norm();
+//   return Rot3::ExpmapDerivative(-arr);
-
+//
-    // 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);
-}
 
 bool checkNorm(const Vector3& x, double tol) {
     return abs(x.norm() - 1) < tol || std::isnan(x.norm());

examples/ABC_EQF/utilities.h

@@ -9,6 +9,7 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Rot3.h>
 #include <Eigen/Dense>
 #include <functional>
 
@@ -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);