31 lines
717 B
C++
31 lines
717 B
C++
//
|
|
// Created by darshan on 3/11/25.
|
|
//
|
|
|
|
#ifndef UTILITIES_H
|
|
#define UTILITIES_H
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <gtsam/base/Vector.h>
|
|
#include <Eigen/Dense>
|
|
#include <functional>
|
|
|
|
using namespace gtsam;
|
|
|
|
// Global configuration
|
|
extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL"
|
|
|
|
/**
|
|
* Utility functions
|
|
*/
|
|
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);
|
|
Matrix numericalDifferential(std::function<Vector(const Vector&)> f, const Vector& x);
|
|
#endif //UTILITIES_H
|