2021@SDUSC
2021年11月2日星期二——2021年11月4日星期四
2021年11月10日星期三——2021年11月11日星期四
附:發燒導致學習進度中斷,
一、學習背景:
經過前幾周對于雷達部分的學習,我對于ros以及LVI-SLAM包有了初步的認識,我們小組的下一個目標就是視覺部分,經過討論和對于資訊流的分析,我們決定按照visual_estimator,visual_feature和visual_loop的順序來學習,
因為LVI-SAM的視覺部分是建立在VINS-MONO的基礎之上的,而后者的包的分析資料網上有很多,所以我們可以以此作為參考來學習,但是同時也要重點注意LVI-SAM是融合了雷達和視覺系統的存在,在查閱資料時要注意區分不同點,
根據對于整個專案的規劃,我計劃利用十篇文章左右的篇幅來介紹這個visual_estimator,
在visual-estimator包下,我的第一個任務是對于initial部分的分析,
在學習的程序中,我發現在閱讀代碼時經常會陷入不知道在解釋什么的困境,于是決定回到VIN-Mono這篇論文中去,通過閱讀論文來為后續的學習鋪墊基礎,因此,在繼續這部分的學習之前,我先閱讀一下VIN-Mono的論文,
二、檔案概述:
Visual_estimator:
在visual_estimator下,主要有三個檔案夾:factor,initial和utility及3個頭檔案與4個cpp檔案,
- initial:初始化,
- factor:非線性優化,
- utility:輔助設備?可能是存放著輔助其他檔案完成功能的函式,
- estimator.cpp:主角,所有的函式都在其中,
- estimator_node.cpp:ros節點,也是上一個函式的入口,
- feature_manager.cpp:特征點管理
- parameters.cpp:讀取引數的函式,并不是引數
[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-ERHJtNew-1636788956482)(C:\Users\Fan luke\Desktop\image-20211102103333149.png)]
initial
[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-XvtC7tCi-1636788956486)(C:\Users\Fan luke\Desktop\image-20211110151329327.png)]
檔案夾內部一共有四個cpp檔案和四個與之對應的頭檔案,
三、initial內
1、初始化原理
這里參考了古月局上大佬的分析圖,出處附在文末,文章中給出了詳細的數學推導程序,
我正是因為卡在了這個地方,所以4號的時候不得不去看了看論文的講解,并把學習的內容記錄在了上一篇博客之中,這里就不再贅述了,

2、第一個函式:initial_alignment
A、頭檔案:
using namespace Eigen;
using namespace std;
class ImageFrame
{
public:
ImageFrame(){};
ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 8, 1>>>>& _points,
const vector<float> &_lidar_initialization_info,
double _t):
t{_t}, is_key_frame{false}, reset_id{-1}, gravity{9.805}
{
points = _points;
// reset id in case lidar odometry relocate
reset_id = (int)round(_lidar_initialization_info[0]);
// Pose
T.x() = _lidar_initialization_info[1];
T.y() = _lidar_initialization_info[2];
T.z() = _lidar_initialization_info[3];
// Rotation
Eigen::Quaterniond Q = Eigen::Quaterniond(_lidar_initialization_info[7],
_lidar_initialization_info[4],
_lidar_initialization_info[5],
_lidar_initialization_info[6]);
R = Q.normalized().toRotationMatrix();
// Velocity
V.x() = _lidar_initialization_info[8];
V.y() = _lidar_initialization_info[9];
V.z() = _lidar_initialization_info[10];
// Acceleration bias
Ba.x() = _lidar_initialization_info[11];
Ba.y() = _lidar_initialization_info[12];
Ba.z() = _lidar_initialization_info[13];
// Gyroscope bias
Bg.x() = _lidar_initialization_info[14];
Bg.y() = _lidar_initialization_info[15];
Bg.z() = _lidar_initialization_info[16];
// Gravity
gravity = _lidar_initialization_info[17];
};
map<int, vector<pair<int, Eigen::Matrix<double, 8, 1>> > > points;
double t;
IntegrationBase *pre_integration;
bool is_key_frame;
// Lidar odometry info
int reset_id;
Vector3d T;
Matrix3d R;
Vector3d V;
Vector3d Ba;
Vector3d Bg;
double gravity;
};
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x);
class odometryRegister
{
public:
ros::NodeHandle n;
tf::Quaternion q_lidar_to_cam;
Eigen::Quaterniond q_lidar_to_cam_eigen;
ros::Publisher pub_latest_odometry;
odometryRegister(ros::NodeHandle n_in):
n(n_in)
{
q_lidar_to_cam = tf::Quaternion(0, 1, 0, 0); // rotate orientation // mark: camera - lidar
q_lidar_to_cam_eigen = Eigen::Quaterniond(0, 0, 0, 1); // rotate position by pi, (w, x, y, z) // mark: camera - lidar
// pub_latest_odometry = n.advertise<nav_msgs::Odometry>("odometry/test", 1000);
}
// convert odometry from ROS Lidar frame to VINS camera frame
vector<float> getOdometry(deque<nav_msgs::Odometry>& odomQueue, double img_time)
{
vector<float> odometry_channel;
odometry_channel.resize(18, -1); // reset id(1), P(3), Q(4), V(3), Ba(3), Bg(3), gravity(1)
nav_msgs::Odometry odomCur;
// pop old odometry msg
while (!odomQueue.empty())
{
if (odomQueue.front().header.stamp.toSec() < img_time - 0.05)
odomQueue.pop_front();
else
break;
}
if (odomQueue.empty())
{
return odometry_channel;
}
// find the odometry time that is the closest to image time
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
odomCur = odomQueue[i];
if (odomCur.header.stamp.toSec() < img_time - 0.002) // 500Hz imu
continue;
else
break;
}
// time stamp difference still too large
if (abs(odomCur.header.stamp.toSec() - img_time) > 0.05)
{
return odometry_channel;
}
// convert odometry rotation from lidar ROS frame to VINS camera frame (only rotation, assume lidar, camera, and IMU are close enough)
tf::Quaternion q_odom_lidar;
tf::quaternionMsgToTF(odomCur.pose.pose.orientation, q_odom_lidar);
tf::Quaternion q_odom_cam = tf::createQuaternionFromRPY(0, 0, M_PI) * (q_odom_lidar * q_lidar_to_cam); // global rotate by pi // mark: camera - lidar
tf::quaternionTFToMsg(q_odom_cam, odomCur.pose.pose.orientation);
// convert odometry position from lidar ROS frame to VINS camera frame
Eigen::Vector3d p_eigen(odomCur.pose.pose.position.x, odomCur.pose.pose.position.y, odomCur.pose.pose.position.z);
Eigen::Vector3d v_eigen(odomCur.twist.twist.linear.x, odomCur.twist.twist.linear.y, odomCur.twist.twist.linear.z);
Eigen::Vector3d p_eigen_new = q_lidar_to_cam_eigen * p_eigen;
Eigen::Vector3d v_eigen_new = q_lidar_to_cam_eigen * v_eigen;
odomCur.pose.pose.position.x = p_eigen_new.x();
odomCur.pose.pose.position.y = p_eigen_new.y();
odomCur.pose.pose.position.z = p_eigen_new.z();
odomCur.twist.twist.linear.x = v_eigen_new.x();
odomCur.twist.twist.linear.y = v_eigen_new.y();
odomCur.twist.twist.linear.z = v_eigen_new.z();
// odomCur.header.stamp = ros::Time().fromSec(img_time);
// odomCur.header.frame_id = "vins_world";
// odomCur.child_frame_id = "vins_body";
// pub_latest_odometry.publish(odomCur);
odometry_channel[0] = odomCur.pose.covariance[0];
odometry_channel[1] = odomCur.pose.pose.position.x;
odometry_channel[2] = odomCur.pose.pose.position.y;
odometry_channel[3] = odomCur.pose.pose.position.z;
odometry_channel[4] = odomCur.pose.pose.orientation.x;
odometry_channel[5] = odomCur.pose.pose.orientation.y;
odometry_channel[6] = odomCur.pose.pose.orientation.z;
odometry_channel[7] = odomCur.pose.pose.orientation.w;
odometry_channel[8] = odomCur.twist.twist.linear.x;
odometry_channel[9] = odomCur.twist.twist.linear.y;
odometry_channel[10] = odomCur.twist.twist.linear.z;
odometry_channel[11] = odomCur.pose.covariance[1];
odometry_channel[12] = odomCur.pose.covariance[2];
odometry_channel[13] = odomCur.pose.covariance[3];
odometry_channel[14] = odomCur.pose.covariance[4];
odometry_channel[15] = odomCur.pose.covariance[5];
odometry_channel[16] = odomCur.pose.covariance[6];
odometry_channel[17] = odomCur.pose.covariance[7];
return odometry_channel;
}
};
頭檔案定義了兩個類和一個函式,
B、函式主體
檔案開頭是利用相機的旋轉來標定imu的bias的函式,
#include ……………
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
//接受引數,包括幀、矩陣、滑窗內的幀、以及三維資訊方位bgs,
Matrix3d A;
Vector3d b;
Vector3d delta_bg;
A.setZero();
b.setZero();
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
//下邊是利用公式,根據滑窗和幀來計算,for回圈對應了公式當中的求和部分,根據介紹的公式是對應了Ax=B
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
//A是一個矩陣,可以LDLT分解,這里的ldlt函式正是這個功能,利用了IDLT分解求解線性方程,
delta_bg = A.ldlt().solve(b);
//傳遞ROS資訊流
ROS_INFO_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
//給滑窗內的imu預積分加上角速度bias
for (int i = 0; i <= WINDOW_SIZE; i++)
Bgs[i] += delta_bg;
//重新計算預積分,
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
{
frame_j = next(frame_i);
frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
}
}
利用gw的模長已知這個先驗條件進一步優化gc0
說明:這里的模長就是gw我們所測得真實的重力大小,
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
Vector3d g0 = g.normalized() * G.norm();
Vector3d lx, ly;
//VectorXd x;
int all_frame_count = all_image_frame.size();
int n_state = all_frame_count * 3 + 2 + 1;
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
//一開始也是設定引數,接受容器,這里需要注意下這個n維的Vector,根據論文中的公式可知,這里是用來建立切向空間的:> 因此,我們在其切線空間上用兩個變數重新引數化重力
for(int k = 0; k < 4; k++)
{
MatrixXd lxly(3, 2);
//建立切向空間的函式
lxly = TangentBasis(g0);
int i = 0;
//代入公式,求解答案
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(6, 9);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;
tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0;
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;
Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
//cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
//MatrixXd cov_inv = cov.inverse();
cov_inv.setIdentity();
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
b.segment<6>(i * 3) += r_b.head<6>();
A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();
b.tail<3>() += r_b.tail<3>();
A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
}
//ldlt法求解線性方程,并得到最終優化結果
A = A * 1000.0;
b = b * 1000.0;
x = A.ldlt().solve(b);
VectorXd dg = x.segment<2>(n_state - 3);
g0 = (g0 + lxly * dg).normalized() * G.norm();
//double s = x(n_state - 1);
}
g = g0;
}
這個函式利用imu的平移來估計重力、各個bk幀的速度以及尺度的函式,
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{ //開頭依舊是接受引數,設定容器
int all_frame_count = all_image_frame.size();
int n_state = all_frame_count * 3 + 3 + 1;
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
int i = 0;
//下邊的for回圈也是利用公式來計算求解的形式
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(6, 10);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
//cout << "delta_p " << frame_j->second.pre_integration->delta_p.transpose() << endl;
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
//cout << "delta_v " << frame_j->second.pre_integration->delta_v.transpose() << endl;
Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
//cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
//MatrixXd cov_inv = cov.inverse();
cov_inv.setIdentity();
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
b.segment<6>(i * 3) += r_b.head<6>();
A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
b.tail<4>() += r_b.tail<4>();
A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
}
//為什么要乘上一千呢?
A = A * 1000.0;
b = b * 1000.0;
//求解,依舊是LDLT分解
x = A.ldlt().solve(b);
//校正,校正這偏大的100?
double s = x(n_state - 1) / 100.0;
ROS_INFO("estimated scale: %f", s);
//評估預測的重力是否正確,偏差大于一或是負數都表示計算出現了錯誤,
g = x.segment<3>(n_state - 4);
ROS_INFO_STREAM(" result g " << g.norm() << " " << g.transpose());
if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
{
return false;
}
RefineGravity(all_image_frame, g, x);
s = (x.tail<1>())(0) / 100.0;
(x.tail<1>())(0) = s;
ROS_INFO_STREAM(" refine " << g.norm() << " " << g.transpose());
if(s < 0.0 )
return false;
else
return true;
}
這個是建立切向空間的函式,在重力優化時用到了,
MatrixXd TangentBasis(Vector3d &g0)
{
Vector3d b, c;
Vector3d a = g0.normalized();
Vector3d tmp(0, 0, 1);
if(a == tmp)
tmp << 1, 0, 0;
b = (tmp - a * (a.transpose() * tmp)).normalized();
c = a.cross(b);
MatrixXd bc(3, 2);
bc.block<3, 1>(0, 0) = b;
bc.block<3, 1>(0, 1) = c;
return bc;
}
呼叫上述的函式,得到最終結果,
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
solveGyroscopeBias(all_image_frame, Bgs);
if(LinearAlignment(all_image_frame, g, x))
return true;
else
return false;
}
四、問題及解答:
1、引數型別vector3,vector3d
ROS中的資料操作需要線性代數,Eigen庫是C++中的線性代數計算庫,
Eigen庫獨立于ROS,但是在ROS中可以使用,
Eigen庫可以參見http://eigen.tuxfamily.org
可以理解成是python中的numpy,
其中Eigen下不僅有矩陣,還有向量,
向量是矩陣的特殊情況,也是用矩陣定義的,定義如下:
typedef Matrix<float, 3, 1> Vector3f;
typedef Matrix<int, 1, 2> RowVector2i;12
這里的Vector是幾維的就是幾d,
我們見到的vector3d是3維的,
而vector3是另一個包下的,有geometry_msgs/Vector3 Message,
2、 gw?gc0?有什么樣的關系?
g表示的是gravity重力,w,c0都是我們之前提到的坐標系的名字,分別表示的是真實世界和參考c0.
我們在論文中介紹了重力精細化的步驟,表現在代碼中就是RefineGravity這個函式,
3、Eigen常見函式,閱讀代碼需要
代碼中涉及到Vector的地方有很多,不了解這些常用函式的話,看起來寸步難行,有必要學習一下,
讀了文章之后有了初步的印象,把代碼中出現的幾個摘錄下來:
C.setZero(rows,cols) // C = zeros(rows,cols) //全零矩陣
x.segment(i, n) // x(i+1 : i+n) //切片
x.segment(i) // x(i+1 : i+n) //切片R.transpose() // R.’ or conj(R’) // 可讀/寫 轉置
x.norm() // norm(x). //范數(注意:Eigen中沒有norm?)
在Eigen中最基本的快操作運算是用
.block()完成的,提取的子矩陣同樣分為動態大小和固定大小,x.cross(y) // cross(x, y) //交叉積,需要頭檔案 #include <Eigen/Geometry>
五、參考資料:
VINS-Mono
VINS-Mono翻譯_Pancheng1的博客-CSDN博客_vins-mono
Visual-Inertial Odometry - 知乎 (zhihu.com)
【SLAM】VINS-MONO決議——綜述_iwanderu的博客-CSDN博客
從零手寫VIO——(一)Introduction - 古月居 (guyuehome.com)
vi_sfm - ROS Wiki
詳解SLAM中的兩種常用的三角化求解地標點的方法_yg838457845的博客-CSDN博客_slam三角化
視覺SLAM中的對極約束、三角測量、PnP、ICP問題 - 古月居 (guyuehome.com)
例說姿態解算與導航18-IMU誤差源 - 知乎 (zhihu.com)
SFM原理簡介_lpj822的專欄-CSDN博客
[ROS中使用Eigen庫不定期更新]_陳顯森的博客-CSDN博客
EIgen:Matricx和vector類的定義和使用_飄零過客-CSDN博客
Eigen常用函式_yanzhiwen2的博客-CSDN博客
Eigen教程2----MatrixXd和VectorXd的用法_MaybeTnT的博客-CSDN博客
轉載請註明出處,本文鏈接:https://www.uj5u.com/qita/357261.html
標籤:其他
上一篇:Ubuntu中圖片與文字的疊加
