Used GTSAM ExpmapDerivative to calculate Left Jacobian(-1)
darshan-17
jenniferoum
parent
9aefd92998
commit
d1673b7df9
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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;
|
||||
|
||||
}
|
||||
|
|
@ -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) {
|
||||
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());
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue