算法知识点

Bimme军

已于 2024-07-22 11:03:01 修改

阅读量710

点赞数 8

文章标签：算法

于 2024-01-26 21:40:21 首次发布

本文链接：https://blog.csdn.net/qq_44693581/article/details/125181771

版权

1 string 的压缩与解压（cartographer）

/**
 * @brief 将字符串进行压缩
 * 
 * @param[in] uncompressed 压缩前的string
 * @param[out] compressed 压缩后的string
 */
inline void FastGzipString(const std::string& uncompressed,
                           std::string* compressed) {
  boost::iostreams::filtering_ostream out;
  // 按gzip压缩
  out.push(
      boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));
  out.push(boost::iostreams::back_inserter(*compressed));
  boost::iostreams::write(out,
                          reinterpret_cast<const char*>(uncompressed.data()),
                          uncompressed.size());
}

/**
 * @brief 将字符串进行解压
 * 
 * @param[in] compressed 压缩的string
 * @param[out] decompressed 解压后的string
 */
inline void FastGunzipString(const std::string& compressed,
                             std::string* decompressed) {
  boost::iostreams::filtering_ostream out;
  // 按gzip解压
  out.push(boost::iostreams::gzip_decompressor());
  out.push(boost::iostreams::back_inserter(*decompressed));
  boost::iostreams::write(out, reinterpret_cast<const char*>(compressed.data()),
                          compressed.size());
}

2 遍历 senson_msgs::LaserScan 格式的激光数据方法（calib_odom）

//把激光雷达数据 转换为PI-ICP需要的数据
void Scan2::LaserScanToLDP(sensor_msgs::LaserScan *pScan,
                           LDP& ldp)
{
    int nPts = pScan->intensities.size();
    ldp = ld_alloc_new(nPts);  //使用 csm 内置函数初始化 ldp 变量

    for(int i = 0;i < nPts;i++)
    {
        double dist = pScan->ranges[i];
        if(dist > 0.1 && dist < 20)
        {
            ldp->valid[i] = 1;
            ldp->readings[i] = dist;
        }
        else
        {
            ldp->valid[i] = 0;
            ldp->readings[i] = -1;
        }
        ldp->theta[i] = pScan->angle_min+pScan->angle_increment*i;
    }
    ldp->min_theta = ldp->theta[0];
    ldp->max_theta = ldp->theta[nPts-1];
}

3.1 求解得到两帧数据之间的位姿差（calib_odom）

Eigen::Vector3d  cal_delta_distence(Eigen::Vector3d  now_pos, Eigen::Vector3d  last_pos) 
{
    Eigen::Vector3d d_pos;  //return value
    Eigen::Matrix3d Tnow,Tprev;

    double theta = last_pos(2);
    double x = last_pos(0);
    double y = last_pos(1);

    //前一次的位姿
    Tprev << cos(theta),-sin(theta),x,
             sin(theta), cos(theta),y,
                      0,          0,       1;

    //当前的位姿
    x = now_pos(0);
    y = now_pos(1);
    theta = now_pos(2);
    Tnow << cos(theta),-sin(theta),x,
             sin(theta), cos(theta),y,
                      0,          0,       1;

    //相对位姿
    Eigen::Matrix3d T = Tprev.inverse() * Tnow;

    d_pos(0) = T(0,2);
    d_pos(1) = T(1,2);
    d_pos(2) = atan2(T(1,0),T(0,0));

    return d_pos;
}

3.2 通过前两个时刻的pose预测下一时刻的pose

SAD/ch7/direct_ndt_lo.cc 中 AlignWithLocal

在这里插入图片描述

4 线性插值

// 线性插值  y=y1+(x-x1)/(x2-x1)*(y2-y1)
// tag: 画图说明一下
TimestampedTransform Interpolate(const TimestampedTransform& start,
				 const TimestampedTransform& end,
				 const common::Time time) {
	CHECK_LE(start.time, time);
	CHECK_GE(end.time, time);

	const double duration = common::ToSeconds(end.time - start.time);
	const double factor = common::ToSeconds(time - start.time) / duration;
	const Eigen::Vector3d origin =
			start.transform.translation() +
			(end.transform.translation() - start.transform.translation()) * factor;
	const Eigen::Quaterniond rotation =
			Eigen::Quaterniond(start.transform.rotation())
		.slerp(factor, Eigen::Quaterniond(end.transform.rotation()));
	return TimestampedTransform{time, transform::Rigid3d(origin, rotation)};
}

5 按比例(ratio_)采样（src/cartographer/cartographer/common/fixed_ratio_sampler.cc）

ratio_ 应该为0到1之间的数，当ratio_ = 1 时，所有数据都返回true,表示所有数据都使用；
当ratio_ = 0.8 时，每10个数据中将会返回8个true和2个false；

bool FixedRatioSampler::Pulse() {
  ++num_pulses_;
  if (static_cast<double>(num_samples_) / num_pulses_ < ratio_) {
    ++num_samples_;
    return true;
  }
  // 返回false时代表数据可以不用,可以跳过计算
  return false;
}

其中p代表num_pulses_，s代表num_samples_，bool代表返回的bool值。上方为ratio_ = 0.8, 下方为ratio_ = 0.3 时的情况。
在这里插入图片描述

6 读取txt文件内容

无人驾驶技术入门（十九）| 手把手教你实现多传感器融合技术
https://zhuanlan.zhihu.com/p/67139241

7 统一角度至0-2π

        while (theta_pre > 2 * M_PI) theta_pre -= 2. * M_PI;
        while (theta_pre < 0.0) theta_pre += 2. * M_PI;

8 各种类型数据的最大最小值

    double a = std::numeric_limits<double>::max();
    double b = std::numeric_limits<double>::min();
    double c = std::numeric_limits<float>::max();
    double d = std::numeric_limits<float>::min();
    double e = std::numeric_limits<int>::max();
    double f = std::numeric_limits<int>::min();
    cout << "a : " << a << endl;
    cout << "b : " << b << endl;
    cout << "c : " << c << endl;
    cout << "d : " << d << endl;
    cout << "e : " << e << endl;
    cout << "f : " << f << endl;

a : 1.79769e+308
b : 2.22507e-308
c : 3.40282e+38
d : 1.17549e-38
e : 2.14748e+09
f : -2.14748e+09

9. atan() 和 atan2()

两个函数放回的都是（x，y）这个向量与x轴所形成的夹角，只是atan函数适用范围是[-π/2, π/2]，而atan2函数的适用范围更大[-π, π]。
在这里插入图片描述

在这里插入图片描述

10. 计算点云的边界

// 自动驾驶与机器人中的slam技术P144
using PointType = pcl::PointXYZI;
using PointCloudType = pcl::PointCloud<PointType>;

void GenerateBEVImage(PointCloudType::Ptr cloud) {
    // 计算点云边界
    auto minmax_x = std::minmax_element(cloud->points.begin(), cloud->points.end(),
                                        [](const PointType& p1, const PointType& p2) { return p1.x < p2.x; });
    auto minmax_y = std::minmax_element(cloud->points.begin(), cloud->points.end(),
                                        [](const PointType& p1, const PointType& p2) { return p1.y < p2.y; });
    double min_x = minmax_x.first->x;
    double max_x = minmax_x.second->x;
    double min_y = minmax_y.first->y;
    double max_y = minmax_y.second->y;
}

11. 地图分块

// 自动驾驶与机器人中的slam技术
// ch9/split_map.cc
for (const auto& pt : kf_cloud_voxeled->points) {
	// 计算每一个点所在的地图块的索引，gx,gy即为索引值
    int gx = floor((pt.x-50) / 100);  // floor 向下取整函数，即输入2.2返回2
    int gy = floor((pt.y-50) / 100);
}

举例说明，以上程序将地图分块的结果是x属于[50,150)，y属于[50,150)范围内的点的地图块的索引为0_0。
其中，黑色为坐标信息，蓝色为地图块的索引。
其中100 代表地图块大小为100mX100m；50代表向坐标原点的偏移，如果把50改成0，就变成了x属于[0,100)，y属于[0,100)范围内的点的地图块的索引为0_0。
请添加图片描述

12. 定时器

秒级别

#include <ctime>
	time_t time1 = time(nullptr);  // time now
	sleep(2);  // 休眠2秒
	time_t time2 = time(nullptr);  // time now
	time_t delta_time = time2 - time1;  // 时间差为2秒

毫秒，微秒，纳秒级别

#include <chrono>
		auto t0 = std::chrono::high_resolution_clock::now(); //time now
		usleep(50000);  // 50毫秒
		auto t1 = std::chrono::high_resolution_clock::now();
		std::chrono::duration<double, std::milli> dt = t1 - t0;  // 毫秒
		std::chrono::duration<double, std::micro> dt = t1 - t0;  // 微秒
		std::chrono::duration<double, std::nano> dt = t1 - t0;   // 纳秒
		std::cout << "dt: " << dt.count() << std::endl;