Hello SLAM(在Linux中实现第一个C++程序)

首先需要安装vim编辑器,输入命令

sudo apt install vim

在Ubuntu上安装好vim编辑器后,创建路径(/home/slambook/ch2),在该路径下创建一个cpp文档(touch hello.c),通过vim编辑器进行编辑。

利用C++代码书写第一个Hello SLAM的代码如下,在书写之前要点击i后,在左下角显示“插入”后即可进行代码编辑。书写完代码后点击esc键后输入:wq保存并退出,:q!是不保存退出。

使用gcc编译器进行编译。

发现报错error:iostream没有那个文件或目录

解决方法参考于博主xiaoqixiaoguai的博文

参考博文icon-default.png?t=N7T8https://blog.csdn.net/xiaoqixiaoguai/article/details/128051365

1.echo|g++ -v -x c++ -E -后查看是否有如下部分(中后部,具体详见上述博文)

/usr/lib/gcc/x86_64-linux-gnu/5/include-fixed

2. 输入gedit /etc/profile。若etc/profile文件为只读,需要sudo提升权限。进入文本文件后输入

export PATH =" usr/lib/gcc/x86_64-linux-gnu/5/-include-fixed:$PATH"

注意:没有安装gedit的需要按要求进行安装,即输入

sudo apt install gedit

再修改后再次进入该文本文件时显示已经修改,而再次输入echo|g++ -v -x c++ -E -后仍无缺失的那一行,但是可以编译运行cpp文件,没有影响。

随后利用g++编译并输出即可

  • 2
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
在激光SLAM使用gtsam加入地平面约束作为新的因子时,我们需要定义一个地平面约束因子类,并实现其evaluateError()和linearize()方法。 假设我们有一个地平面约束因子 $f$,它的观测值为 $z$,地平面法向量的估计值为 $n$,噪声模型为高斯分布 $\mathcal{N}(0, \Sigma)$。那么,地平面约束的残差函数 $e(z, n)$ 可以定义为: $$ e(z, n) = z - n^T p $$ 其,$p$ 是激光雷达扫描点的坐标,$n$ 是地平面法向量的估计值。 在C++实现地平面约束因子类的代码如下: ```C++ #include <gtsam/geometry/Point3.h> #include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/base/Vector.h> #include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h> #include <boost/make_shared.hpp> class GroundPlaneFactor : public gtsam::NoiseModelFactor1<gtsam::Point3> { public: GroundPlaneFactor(const gtsam::Key& key, const gtsam::Point3& point, const gtsam::SharedNoiseModel& model) : NoiseModelFactor1<gtsam::Point3>(model, key), point_(point) {} virtual ~GroundPlaneFactor() {} virtual gtsam::Vector evaluateError(const gtsam::Point3& n, boost::optional<gtsam::Matrix&> H = boost::none) const { gtsam::Vector3 error; double d = point_.dot(n); error[0] = d - point_.x() * n.x() - point_.y() * n.y() - point_.z() * n.z(); if (H) { *H = (gtsam::Matrix3() << -point_.x(), -point_.y(), -point_.z()).finished(); } return error; } static boost::shared_ptr<GroundPlaneFactor> Create(const gtsam::Key& key, const gtsam::Point3& point, double sigma) { gtsam::Matrix3 covariance = gtsam::Matrix3::Identity() * pow(sigma, 2); gtsam::noiseModel::Gaussian::shared_ptr noise_model = gtsam::noiseModel::Gaussian::Covariance(covariance); return boost::make_shared<GroundPlaneFactor>(key, point, noise_model); } private: gtsam::Point3 point_; }; ``` 上面的代码,我们继承了gtsam::NoiseModelFactor1类,并重载了evaluateError()方法。在evaluateError()方法,我们计算出当前地平面法向量的估计值 $n$ 对应的残差,并计算残差函数的一阶导数,返回一个Jacobian矩阵。在Create()方法,我们定义了一个静态的工厂函数,用于创建GroundPlaneFactor类的对象,并设置噪声模型。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值