2021SC@SDUSC
ImuTypes.cc
IMU参数,imu到相机、相机到imu、协方差、游走协方差
class Calib
{
template<class Archive>
void serializeMatrix(Archive &ar, cv::Mat& mat, const unsigned int version)
{
int cols, rows, type;
bool continuous;
if (Archive::is_saving::value) {
cols = mat.cols; rows = mat.rows; type = mat.type();
continuous = mat.isContinuous();
}
ar & cols & rows & type & continuous;
if (Archive::is_loading::value)
mat.create(rows, cols, type);
if (continuous) {
const unsigned int data_size = rows * cols * mat.elemSize();
ar & boost::serialization::make_array(mat.ptr(), data_size);
} else {
const unsigned int row_size = cols*mat.elemSize();
for (int i = 0; i < rows; i++) {
ar & boost::serialization::make_array(mat.ptr(i), row_size);
}
}
}
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
serializeMatrix(ar,Tcb,version);
serializeMatrix(ar,Tbc,version);
serializeMatrix(ar,Cov,version);
serializeMatrix(ar,CovWalk,version);
}
public:
Calib(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw)
{
Set(Tbc_,ng,na,ngw,naw);
}
Calib(const Calib &calib);
Calib(){}
void Set(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw);
public:
cv::Mat Tcb;
cv::Mat Tbc;
cv::Mat Cov, CovWalk;
};
角速度计算旋转量
float deltaT; 积分时间
cv::Mat deltaR; 指数映射
cv::Mat rightJ; 右乘 jacobian
class IntegratedRotation
{
public:
IntegratedRotation(){}
IntegratedRotation(const cv::Point3f &angVel, const Bias &imuBias, const float &time);
public:
float deltaT;
cv::Mat deltaR;
cv::Mat rightJ;
};
IMU测量的预积分
class Preintegrated
{
template<class Archive>
void serializeMatrix(Archive &ar, cv::Mat& mat, const unsigned int version)
{
int cols, rows, type;
bool continuous;
if (Archive::is_saving::value) {
cols = mat.cols; rows = mat.rows; type = mat.type();
continuous = mat.isContinuous();
}
ar & cols & rows & type & continuous;
if (Archive::is_loading::value)
mat.create(rows, cols, type);
if (continuous) {
const unsigned int data_size = rows * cols * mat.elemSize();
ar & boost::serialization::make_array(mat.ptr(), data_size);
} else {
const unsigned int row_size = cols*mat.elemSize();
for (int i = 0; i < rows; i++) {
ar & boost::serialization::make_array(mat.ptr(i), row_size);
}
}
}
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & dT;
serializeMatrix(ar,C,version);
serializeMatrix(ar,Info,version);
serializeMatrix(ar,Nga,version);
serializeMatrix(ar,NgaWalk,version);
ar & b;
serializeMatrix(ar,dR,version);
serializeMatrix(ar,dV,version);
serializeMatrix(ar,dP,version);
serializeMatrix(ar,JRg,version);
serializeMatrix(ar,JVg,version);
serializeMatrix(ar,JVa,version);
serializeMatrix(ar,JPg,version);
serializeMatrix(ar,JPa,version);
serializeMatrix(ar,avgA,version);
serializeMatrix(ar,avgW,version);
ar & bu;
serializeMatrix(ar,db,version);
ar & mvMeasurements;
}
public:
Preintegrated(const Bias &b_, const Calib &calib);// 赋值协方差+游走协方差,并初始化数据
Preintegrated(Preintegrated* pImuPre);//里面所有参数都一一赋值
Preintegrated() {}
~Preintegrated() {}
void CopyFrom(Preintegrated* pImuPre); // 初始化所有变量
void Initialize(const Bias &b_);
void IntegrateNewMeasurement(const cv::Point3f &acceleration, const cv::Point3f &angVel, const float &dt);
// 位置首先更新,因为它取决于先前计算的速度和旋转。
// 第二更新速度,因为它取决于先前计算的旋转。
// 旋转是最后更新的
void Reintegrate();
void MergePrevious(Preintegrated* pPrev);
void SetNewBias(const Bias &bu_);
IMU::Bias GetDeltaBias(const Bias &b_);
cv::Mat GetDeltaRotation(const Bias &b_);
cv::Mat GetDeltaVelocity(const Bias &b_);
cv::Mat GetDeltaPosition(const Bias &b_);
cv::Mat GetUpdatedDeltaRotation();
cv::Mat GetUpdatedDeltaVelocity();
cv::Mat GetUpdatedDeltaPosition();
cv::Mat GetOriginalDeltaRotation();
cv::Mat GetOriginalDeltaVelocity();
cv::Mat GetOriginalDeltaPosition();
Eigen::Matrix<double,15,15> GetInformationMatrix();
cv::Mat GetDeltaBias();
Bias GetOriginalBias();
Bias GetUpdatedBias();
public:
float dT;
cv::Mat C;
cv::Mat Info;
cv::Mat Nga, NgaWalk;
// Values for the original bias (when integration was computed)
Bias b;
cv::Mat dR, dV, dP;
cv::Mat JRg, JVg, JVa, JPg, JPa;
cv::Mat avgA;
cv::Mat avgW;
private:
// Updated bias
Bias bu;
// Dif between original and updated bias
// This is used to compute the updated values of the preintegration
cv::Mat db;
struct integrable
{
integrable(const cv::Point3f &a_, const cv::Point3f &w_ , const float &t_):a(a_),w(w_),t(t_){}
cv::Point3f a;
cv::Point3f w;
float t;
};
std::vector<integrable> mvMeasurements;
std::mutex mMutex;
};
// Lie Algebra Functions
cv::Mat ExpSO3(const float &x, const float &y, const float &z);
Eigen::Matrix<double,3,3> ExpSO3(const double &x, const double &y, const double &z);
cv::Mat ExpSO3(const cv::Mat &v);
cv::Mat LogSO3(const cv::Mat &R);
cv::Mat RightJacobianSO3(const float &x, const float &y, const float &z);
cv::Mat RightJacobianSO3(const cv::Mat &v);
cv::Mat InverseRightJacobianSO3(const float &x, const float &y, const float &z);
cv::Mat InverseRightJacobianSO3(const cv::Mat &v);
cv::Mat Skew(const cv::Mat &v);
cv::Mat NormalizeRotation(const cv::Mat &R);
}
Preintegrated,预积分类构造函数,根据输入的偏置初始化预积分参数。calib:imu标定参数的类
Preintegrated::Preintegrated(const Bias &b_, const Calib &calib)
{
Nga = calib.Cov.clone();
NgaWalk = calib.CovWalk.clone();
Initialize(b_);
}
// Copy constructor
Preintegrated::Preintegrated(Preintegrated* pImuPre): dT(pImuPre->dT), C(pImuPre->C.clone()), Info(pImuPre->Info.clone()),
Nga(pImuPre->Nga.clone()), NgaWalk(pImuPre->NgaWalk.clone()), b(pImuPre->b), dR(pImuPre->dR.clone()), dV(pImuPre->dV.clone()),
dP(pImuPre->dP.clone()), JRg(pImuPre->JRg.clone()), JVg(pImuPre->JVg.clone()), JVa(pImuPre->JVa.clone()), JPg(pImuPre->JPg.clone()),
JPa(pImuPre->JPa.clone()), avgA(pImuPre->avgA.clone()), avgW(pImuPre->avgW.clone()), bu(pImuPre->bu), db(pImuPre->db.clone()), mvMeasurements(pImuPre->mvMeasurements)
{
}