slam学习笔记
发布时间
阅读量：
阅读量
ubuntu20.04 使用vs code编写
现放cmake文件（记得链接库文件和配置C++版本）
cmake_minimum_required( VERSION 2.8 )
project(learingMatrix)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++17")
include_directories("/usr/include/eigen3")
add_executable(learningMatrix eigenMatrix.cpp)
add_executable(learnningGeometry useGeometry.cpp)
add_executable(coordinatetrans coordinateTransform.cpp)
add_executable(plot plotTrajectory.cpp)
add_executable(cube visualizeGeometry.cpp)
include_directories(${Pangolin_INCLUDE_DIRS})
find_package( Pangolin )
target_link_libraries(plot ${Pangolin_LIBRARIES})
target_link_libraries(cube ${Pangolin_LIBRARIES})以下为eigen的简单运算部分
#include <iostream>
using namespace std;
#include <ctime>
#include <Eigen/Core> //eigen 核心
#include <Eigen/Dense> //稠密矩阵运算规则
using namespace Eigen;
#define MATRIX_SIZE 50
int main(int argc, char** argv)
{
Matrix<float,2,3> matrix_23; //Matrix模板类用三个参数生成一个矩阵 数据类型，行，列
Vector3d v_3d; //Vector3d生成一个三维行向量，d是double的意思,本质上还是Matrix一个特殊形式
Matrix<double,3,1> v1_3d; //同一个意思
Matrix3d matrix_33 = Matrix3d::Zero(); //Matrix3d生成一个3*3的方阵， Zero是0阵
Matrix<double , Dynamic,Dynamic> matrix_dynamic; //定义动态矩阵，行和列随数据变化
MatrixXd matrix_x; //同上
// ------------------------------------------------------------------------------------
matrix_23<<1,2,3,4,5,6; //输入数据（运算符重载）
cout<<"matrix 2*3 from 1 to 6 : "<<matrix_23<<endl; //输出（运算符重载）
cout<<"print matrix 2*3: "<<endl;
for(int i=0;i<2;i++)
{
for(int j=0;j<3;j++)
{
cout<<matrix_23(i,j)<<"\t";
}
cout<<endl;
}
//-----------------------------------------------------------------------------------------
v_3d << 3,2,1;
v1_3d <<4,5,6;
//Matrix<double ,2,1> result_wrong_type =matrix_23*v1_3d;(不能混用不同数据类型的矩阵，eigen不会自动升级数据类型)
Matrix<double , 2, 1> result = matrix_23.cast<double>()*v_3d; //cast转换数据类型
cout<<"[1,2,3;4,5,6]*[3,2,1]="<<result.transpose()<<endl;
Matrix<double , 2,1> result2 = matrix_23.cast<double>()*v1_3d;
cout<<"[1,2,3;4,5,6]*[4,5,6]="<<result2.transpose()<<endl; //transpose转置
//Eigen::Matrix<double , 2,3 > result_wrong_dimension = matrix_23.cast<double>()*v_3d; (不同维度的矩阵不能直接赋值，eigen不会自动填充空白)
//----------------------------------------------------------------------------------------
matrix_33 = Matrix3d::Random(); //随机填充数
cout<<"random matrix:\n"<<matrix_33<<endl;
cout<<"transpose :\n"<<matrix_33.transpose()<<endl;
cout<<"sum: \n"<<matrix_33.sum()<<endl;
cout<<"trace: \n"<<matrix_33.trace()<<endl;
cout<<"10 times: \n"<<10*matrix_33<<endl;
cout<<"inverse ; \n"<<matrix_33.inverse()<<endl; //不推荐直接求逆，维度一大起来，计算量太大
cout<<"det: \n"<<matrix_33.determinant()<<endl;
//----------------------------------------------------------------------------------------
SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose()*matrix_33); //矩阵对角化？
cout<<"Eigen value =\n"<< eigen_solver.eigenvalues()<<endl;
cout<<"Eigen vectors = \n"<<eigen_solver.eigenvectors()<<endl;
//----------------------------------------------------------------------------------------
Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN
= MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
matrix_NN = matrix_NN * matrix_NN.transpose();
Matrix<double,MATRIX_SIZE,1> v_Nd = MatrixXd::Random(MATRIX_SIZE,1);
clock_t time_stt = clock();
//----------------------------------------------------------------------------------------
//直接求逆
Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
cout<<"time of normal inverse is "
<<1000*(clock()-time_stt)/(double) CLOCKS_PER_SEC<<"ms"<<endl;
cout<<"x = "<<x.transpose()<<endl;
//----------------------------------------------------------------------------------------
//Qr分解
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout<<"time of Qr decomposition is "
<<1000*(clock()-time_stt)/(double) CLOCKS_PER_SEC<<"ms"<<endl;
cout<<"x = "<<x.transpose()<<endl;
//----------------------------------------------------------------------------------------
//正定阵可以用cholesky分解
time_stt = clock();
x = matrix_NN.ldlt().solve(v_Nd);
cout<<"time of ldlt decomposition is "
<<1000*(clock()-time_stt)/(double) CLOCKS_PER_SEC<<"ms"<<endl;
cout<<"x = "<<x.transpose()<<endl;
return 0;
}以下为eigen的几何运算部分
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
int main(int argc,char** argv)
{
Matrix3d rotation_matrix = Matrix3d::Identity(); //旋转矩阵
AngleAxisd rotation_vector(M_PI/4,Vector3d(0,0,1)); //旋转向量 绕Z轴旋转45度
cout.precision(3);
cout<<"rotation matrix= \n"<<rotation_vector.matrix()<<endl;//用.matrix（）把轴角式转换成矩阵输出
rotation_matrix=rotation_vector.toRotationMatrix(); //toRotationmatrix可以直接转换成矩阵并赋值
//用轴角 旋转向量--------------------------------------------------------------------------------------------
Vector3d v(1,0,0);
Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation (by angle axis) = "<<v_rotated.transpose()<<endl;
//用旋转矩阵 旋转向量----------------------------------------------------------------------------------------
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation (by matrix) = "<<v_rotated.transpose()<<endl;
//欧拉角转换-----------------------------------------------------------------------------------------------
Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
//欧式变换(长度、夹角、)
Isometry3d T = Isometry3d::Identity(); //4*4的矩阵！！！
T.rotate(rotation_vector); //旋转
T.pretranslate(Vector3d(1,3,4)); //平移
cout<<"Transform matrix = \n"<<T.matrix()<<endl;
// 用变换矩阵变换
Vector3d v_transformed = T*v;
cout<<"v transformed = "<<v_transformed.transpose();
//四元数（四元数和轴角之间可以相互转换）
Quaterniond q = Quaterniond(rotation_vector);
cout<<"quaternion from rotation vector = "<<q.coeffs().transpose()<<endl;
q = Quaterniond(rotation_matrix);
cout<<"quaternion from rotation matrix = "<<q.coeffs().transpose()<<endl;
//用四元数旋转一个向量
v_rotated = q * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
//从数学表达式应该是下面的qvq^-1
cout<<"should be equal to "<<(q*Quaterniond(0,1,0,0) * q.inverse()).coeffs().transpose()<<endl;
return 0;
}以下为两个小机器人的坐标变换问题
#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main(int argc,char** argv)
{
Quaterniond q1(0.35,0.2,0.3,0.1),q2(-0.5,0.4,-0.1,0.2); //一号位姿q1，t1，二号位姿q2,t2 四元数表示旋转，t为平移矩阵
q1.normalize(); //归一化数据
q2.normalize();
Vector3d t1(0.3,0.1,0.1),t2(-0.1,0.5,0.3);
Vector3d p1(0.5,0,0.2); //目标在一号机器人坐标系下的坐标
Isometry3d T1w(q1),T2w(q2); //隐藏了一部旋转向量设定
// T1w.rotate(q1);
//T2w.rotate(q2); //旋转向量设定
T1w.pretranslate(t1); //平移向量设定
T2w.pretranslate(t2);
Vector3d p2 =T2w * T1w.inverse()*p1; //目标由（一号转世界）（世界转二号）转换至二号机器人坐标系下，得到p2
cout<<endl<<p2.transpose();
return 0;
}以下为pangolin路径绘制部分
#include <iostream>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unistd.h>
using namespace std;
using namespace Eigen;
string trajectory_file = "/home/martin/桌面/code/eigen/trajectory.txt";
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>); //先声明函数
12. int main(int argc,char** argv)
{
vector<Isometry3d,Eigen::aligned_allocator<Isometry3d>> poses; //声明
ifstream fin(trajectory_file);
if(!fin)
{
cout<<"cannot find trajectory file at "<<trajectory_file<<endl;
return 1;
}
while (!fin.eof())
{
double time ,tx, ty, tz, qx, qy, qz, qw;
fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw; //读取时间、平移、旋转
Isometry3d Twr(Quaterniond(qw,qx,qy,qz)); //定义旋转向量
Twr.pretranslate(Vector3d(tx,ty,tz)); //定义平移向量
poses.push_back(Twr);
}
cout<<"real total "<<poses.size()<<" pose entries"<<endl;
DrawTrajectory(poses);
return 0;
}
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) //实例化
{
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024,768 ); //标题和窗口大小
glEnable(GL_DEPTH_TEST); //这三句直接写 这句是启用深度检测，下一句是启用混合
glEnable(GL_BLEND);
glBlendFunc(GL_SRC0_ALPHA,GL_ONE_MINUS_SRC_ALPHA); //表示颜色的混合方式
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000), //投影矩阵（前四个是尺寸，然后是near和far的边界值）
pangolin::ModelViewLookAt(0,-0.1,-1.8,0,0,0,0.0,-1.0,0.0) //初始化视角（三个一组分别是观测方向、目标位置、观测位置）
);
pangolin::View &d_cam =pangolin::CreateDisplay() //定义地图面板
.SetBounds(0.0,1.0,0.0,1.0,-1024.0f/768.0f) //末尾两个双精度数表示长宽比
.SetHandler(new pangolin::Handler3D(s_cam));
while (pangolin::ShouldQuit()==false)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //清除画面
d_cam.Activate(s_cam); //激活相机s
glClearColor(1.0f,1.0f,1.0f,1.0f); //opencv类似设定颜色和线宽
glLineWidth(2);
for (size_t i = 0; i < poses.size(); i++)
{
Vector3d Ow = poses[i].translation(); //平移向量
Vector3d Xw = poses[i]*(0.1*Vector3d(1,0,0)); //旋转向量
Vector3d Yw = poses[i]*(0.1*Vector3d(0,1,0));
Vector3d Zw = poses[i]*(0.1*Vector3d(0,0,1));
glBegin(GL_LINES); //给各个轴分配不同的颜色
glColor3f(1.0,0.0,0.0);
glVertex3d(Ow[0],Ow[1],Ow[2]);
glVertex3d(Xw[0],Xw[1],Xw[2]);
glColor3f(0.0,1.0,0.0);
glVertex3d(Ow[0],Ow[1],Ow[2]);
glVertex3d(Yw[0],Yw[1],Yw[2]);
glColor3f(0.0,0.0,1.0);
glVertex3d(Ow[0],Ow[1],Ow[2]);
glVertex3d(Zw[0],Zw[1],Zw[2]);
glEnd();
}
for (size_t i = 0; i < poses.size(); i++) //画线
{
glColor3f(0.0,0.0,0.0);
glBegin(GL_LINES);
auto p1 = poses[i],p2=poses[i+1];
glVertex3d(p1.translation()[0],p1.translation()[1],p1.translation()[2]);
glVertex3d(p2.translation()[0],p2.translation()[1],p2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000);
}
}以下为三维集合体可视化部分
#include <iostream>
#include <iomanip>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
#include <pangolin/pangolin.h>
struct RotationMatrix {
Matrix3d matrix = Matrix3d::Identity();
};
ostream &operator<<(ostream &out, const RotationMatrix &r) //重载运算符
{
out.setf(ios::fixed);
Matrix3d matrix = r.matrix;
out << '=';
out << "[" << setprecision(2) << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"
<< "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"
<< "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";
return out;
}
istream &operator>>(istream &in, RotationMatrix &r) //重载运算符
{
return in;
}
struct TranslationVector
{
Vector3d trans = Vector3d(0, 0, 0);
};
ostream &operator<<(ostream &out, const TranslationVector &t)
{
out << "=[" << t.trans(0) << ',' << t.trans(1) << ',' << t.trans(2) << "]";
return out;
}
istream &operator>>(istream &in, TranslationVector &t)
{
return in;
}
struct QuaternionDraw {
Quaterniond q;
};
ostream &operator<<(ostream &out, const QuaternionDraw quat)
{
auto c = quat.q.coeffs();
out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
return out;
}
istream &operator>>(istream &in, const QuaternionDraw quat)
{
return in;
}
int main(int argc, char **argv)
{
pangolin::CreateWindowAndBind("visualize geometry", 1000, 600);
glEnable(GL_DEPTH_TEST);
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),
pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY)
);
const int UI_WIDTH = 500;
pangolin::View &d_cam = pangolin::CreateDisplay().
SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f).
SetHandler(new pangolin::Handler3D(s_cam));
pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix()); //绘制左侧提示栏
pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());
pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
while (!pangolin::ShouldQuit())
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);
pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
Matrix<double, 4, 4> m = matrix;
RotationMatrix R;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
R.matrix(i, j) = m(j, i);
rotation_matrix = R;
TranslationVector t;
t.trans = Vector3d(m(0, 3), m(1, 3), m(2, 3));
t.trans = -R.matrix * t.trans;
translation_vector = t;
TranslationVector euler;
euler.trans = R.matrix.eulerAngles(2, 1, 0);
euler_angles = euler;
QuaternionDraw quat;
quat.q = Quaterniond(R.matrix);
quaternion = quat;
glColor3f(1.0, 1.0, 1.0);
pangolin::glDrawColouredCube(); //绘制直线
// draw the original axis
glLineWidth(3);
glColor3f(0.8f, 0.f, 0.f);
glBegin(GL_LINES);
glVertex3f(0, 0, 0);
glVertex3f(10, 0, 0);
glColor3f(0.f, 0.8f, 0.f);
glVertex3f(0, 0, 0);
glVertex3f(0, 10, 0);
glColor3f(0.2f, 0.2f, 1.f);
glVertex3f(0, 0, 0);
glVertex3f(0, 0, 10);
glEnd();
pangolin::FinishFrame();
}
}自学slam确实挺麻烦的，继续学。
全部评论 (0)
还没有任何评论哟~