【ORB_SLAM3源码解读】原理解读代码实战
从零开始学习SLAM我学习过写过的代码都在这儿了
代码实战地址：https://github.com/xiaoqiuslamshizhanjiaocheng/xiaoqiuslamshizhanjiaocheng
图文原理地址：https://blog.csdn.net/qq_21950671/category_11508951.html?spm=1001.2014.3001.5482
视频原理地址：https://space.bilibili.com/222855903?spm_id_from=333.1007.0.0
ORB_SLAM3-0.2-beta
- fatal error: GeometricCamera.h
#include "CameraModels/GeometricCamera.h"
#include "CameraModels/Pinhole.h"
#include "CameraModels/KannalaBrandt8.h"
- error: ‘MapPoint’ was not declared in this scope
#include "MapPoint.h"
namespace ORB_SLAM3
{
class MapPoint;
ORB_SLAM2->ORB_SLAM3
- ORB_SLAM2 was not declared in this scope
ORB_SLAM2->ORB_SLAM3
- undefined reference to symbol ‘_ZN5boost6system15system_categoryEv’
set(LIBS
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM3.so
-lboost_system
)
ORB_SLAM3-RGBD-Inertial
ros::init(argc, argv, "RGBD_Inertial");
// 创建 ros 节点
ros::NodeHandle n("~");
// launch 文件中 ns=="node_namespace"
ros::init(argc, argv, "node_name"); // node name
ros::NodeHandle n; //n 命名空间为/node_namespace
ros::NodeHandle n1("sub"); // n1命名空间为/node_namespace/sub
ros::NodeHandle n2(n1,"sub2");// n2命名空间为/node_namespace/sub/sub2
ros::NodeHandle pn1("~"); //pn1 命名空间为/node_namespace/node_name
ros::NodeHandle pn2("~sub"); //pn2 命名空间为/node_namespace/node_name/sub
ros::NodeHandle pn3("~/sub"); //pn3 命名空间为/node_namespace/node_name/sub
ros::NodeHandle gn("/global"); // gn 命名空间为/global
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
// Se the logging level manually to DEBUG
// ros默认设置的消息日志级别，默认有Debug,Info,Warn,Error,Fatal(首字母大写)
//rosrun rqt_console rqt_console 可见
ros::shutdown();
// 调用ros::shutdown()函数来关闭节点，终结所有开放的订阅，发布，服务，调用。
// 代表函数非正常终止
return 1;
class ImuGrabber
class ImageGrabber
如何理解C++的class
std::mutex
如何理解C++线程和锁的用法？
#include const bool
const bool do_rectify;
C++函数的参数加了const和&有什么作用？
cv::createCLAHE
cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));
// 直方图https://blog.csdn.net/qq_21950671/article/details/107092044
// 生成自适应均衡化图像http://edu.pointborn.com/article/2021/5/18/1386.html
// clipLimit 颜色对比度的阈值
// titleGridSize 进行像素均衡化的网格大小,即在多少网格下进行直方图的均衡化操作
class Point
//IMU measurement (gyro, accelerometer and timestamp)
class Point
{
public:
// 两种IMU数据的构造函数
Point(const float &acc_x, const float &acc_y, const float &acc_z,
const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z,
const double ×tamp): a(acc_x,acc_y,acc_z), w(ang_vel_x,ang_vel_y,ang_vel_z), t(timestamp){}
Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double ×tamp):
a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp){}
public:
cv::Point3f a;
cv::Point3f w;
double t;
};
C++ std::size_t
// vector vImuMeas;
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
std::size_t 可以存放下理论上可能存在的对象的最大大小，该对象可以是任何类型，包括数组。
std::size_t 通常被用于数组索引和循环计数。
使用其它类型来进行数组索引操作的程序可能会在某些情况下出错，例如在 64 位系统中使用 unsigned int 进行索引时，如果索引号超过 UINT_MAX 或者依赖于 32 位取模运算的话，程序就会出错。
在对诸如 std::string、std::vector 等 C++ 容器进行索引操作时，正确的类型是该容器的成员 typedef size_type，而该类型通常被定义为与 std::size_t 相同。
#include std::list
std::list<IMU::Point> mlQueueImuData;
Lists将元素按顺序储存在链表中. 与向量(vectors)相比, 它允许快速的插入和删除，但是随机访问却比较慢.
Lst1.begin() 返回指向第一个元素的迭代器
Lst1.end() 返回末尾的迭代器
Lst1.erase() 删除一个元素
Lst1.pop_front() 删除第一个元素
https://blog.csdn.net/qq_21950671/article/details/98885199#t6
mpImuCalib = new IMU::Calib(Tbc,Ngsf,Nasf,Ngw/sf,Naw/sf);
class Bias
void KeyFrame::SetNewBias(const IMU::Bias &b)
{
unique_lock<mutex> lock(mMutexPose);
mImuBias = b;
if(mpImuPreintegrated)
mpImuPreintegrated->SetNewBias(b);
}
cv::Mat KeyFrame::GetGyroBias()
{
unique_lock<mutex> lock(mMutexPose);
return (cv::Mat_<float>(3,1) << mImuBias.bwx, mImuBias.bwy, mImuBias.bwz);
}
cv::Mat KeyFrame::GetAccBias()
{
unique_lock<mutex> lock(mMutexPose);
return (cv::Mat_<float>(3,1) << mImuBias.bax, mImuBias.bay, mImuBias.baz);
}
IMU::Bias KeyFrame::GetImuBias()
{
unique_lock<mutex> lock(mMutexPose);
return mImuBias;
}
ExpSO3 由旋转矢量计算旋转矩阵
// 李代数->李群 由旋转矢量计算旋转矩阵
const float eps = 1e-4;
cv::Mat ExpSO3(const float &x, const float &y, const float &z){
cv::Mat I = cv::Mat::eye(3,3,CV_32F);
// 反对对称矩阵
cv::Mat W = (cv::Mat_<float>(3,3) <<
0, -z, y,
z, 0, -x,
-y, x, 0);
const float d2 = x*x+y*y+z*z;
const float d = sqrt(d2);
if(d<eps)
return (I + W + 0.5f*W*W);
else
return (I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2);
}
// 李代数->李群
Eigen::Matrix<double,3,3> ExpSO3(const double &x, const double &y, const double &z){
Eigen::Matrix<double,3,3> I = Eigen::MatrixXd::Identity(3,3);
Eigen::Matrix<double,3,3> W;
W(0,0) = 0;
W(0,1) = -z;
W(0,2) = y;
W(1,0) = z;
W(1,1) = 0;
W(1,2) = -x;
W(2,0) = -y;
W(2,1) = x;
W(2,2) = 0;
const double d2 = x*x+y*y+z*z;
const double d = sqrt(d2);
if(d<eps)
return (I + W + 0.5*W*W);
else
return (I + W*sin(d)/d + W*W*(1.0-cos(d))/d2);
}
LogSO3 由旋转矩阵计算旋转矢量
cv::Mat LogSO3(const cv::Mat &R){
const float tr = R.at<float>(0,0) + R.at<float>(1,1) + R.at<float>(2,2);
cv::Mat w = (cv::Mat_<float>(3,1) <<(R.at<float>(2,1) - R.at<float>(1,2)) / 2,
(R.at<float>(0,2) - R.at<float>(2,0)) / 2,
(R.at<float>(1,0) - R.at<float>(0,1)) / 2);
const float costheta = (tr-1.0f)*0.5f;
if(costheta>1 || costheta<-1)
return w;
const float theta = acos(costheta);
const float s = sin(theta);
if(fabs(s)<eps)
return w;
else
return theta*w/s;
}
