HBA代码解读（1）tools.hpp

2024-02-20 00:36
文章标签 代码 解读 tools hpp hba

本文主要是介绍HBA代码解读（1）tools.hpp，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

其中，函数jr 和 jr_inv 暂时未找到出处。其他函数都已经注释。

#ifndef TOOLS_HPP
#define TOOLS_HPP#include <Eigen/Core>
#include <unordered_map>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <math.h>#define HASH_P 116101
#define MAX_N 10000000019
#define SMALL_EPS 1e-10
#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0//方阵
#define PLM(a) vector<Eigen::Matrix<double, a, a>, Eigen::aligned_allocator<Eigen::Matrix<double, a, a>>>
//矢量
#define PLV(a) vector<Eigen::Matrix<double, a, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, a, 1>>>
#define VEC(a) Eigen::Matrix<double, a, 1>#define G_m_s2 9.81
#define DIMU 18
#define DIM 15
#define DNOI 12
#define NMATCH 5
#define DVEL 6typedef pcl::PointXYZ PointType;
// typedef pcl::PointXYZI PointType;
// typedef pcl::PointXYZINormal PointType;
using namespace std;Eigen::Matrix3d I33(Eigen::Matrix3d::Identity());
Eigen::Matrix<double, DIMU, DIMU> I_imu(Eigen::Matrix<double, DIMU, DIMU>::Identity());// 根据XYZ值生成 hash位置
class VOXEL_LOC
{
public:int64_t x, y, z;VOXEL_LOC(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0): x(vx), y(vy), z(vz){}bool operator == (const VOXEL_LOC &other) const{return (x == other.x && y == other.y && z == other.z);}
};namespace std
{template<>struct hash<VOXEL_LOC>{size_t operator() (const VOXEL_LOC &s) const{using std::size_t; using std::hash;// return (((hash<int64_t>()(s.z)*HASH_P)%MAX_N + hash<int64_t>()(s.y))*HASH_P)%MAX_N + hash<int64_t>()(s.x);long long index_x, index_y, index_z;double cub_len = 0.125;index_x = int(round(floor((s.x)/cub_len + SMALL_EPS)));index_y = int(round(floor((s.y)/cub_len + SMALL_EPS)));index_z = int(round(floor((s.z)/cub_len + SMALL_EPS)));return (((((index_z * HASH_P) % MAX_N + index_y) * HASH_P) % MAX_N) + index_x) % MAX_N;}};
}//矩阵元素和
double matrixAbsSum(Eigen::MatrixXd mat)
{double sum = 0.0;for (int i = 0; i < mat.rows(); i++)for (int j = 0; j < mat.cols(); j++)sum += fabs(mat(i, j));return sum;
}// 误差柯西函数
double sigmoid_w(double r)
{return 1.0/(1+exp(-r));
}// 向量变成旋转矩阵
Eigen::Matrix3d Exp(const Eigen::Vector3d &ang)
{double ang_norm = ang.norm();Eigen::Matrix3d Eye3 = Eigen::Matrix3d::Identity();if (ang_norm > 0.0000001){Eigen::Vector3d r_axis = ang / ang_norm;Eigen::Matrix3d K;K << SKEW_SYM_MATRX(r_axis);/// Roderigous Tranformationreturn Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;}else{return Eye3;}
}
// 向量变成旋转矩阵
Eigen::Matrix3d Exp(const Eigen::Vector3d &ang_vel, const double &dt)
{double ang_vel_norm = ang_vel.norm();Eigen::Matrix3d Eye3 = Eigen::Matrix3d::Identity();if (ang_vel_norm > 0.0000001){Eigen::Vector3d r_axis = ang_vel / ang_vel_norm;Eigen::Matrix3d K;K << SKEW_SYM_MATRX(r_axis);double r_ang = ang_vel_norm * dt;/// Roderigous Tranformationreturn Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;}else{return Eye3;}
}// 旋转矩阵变成向量
Eigen::Vector3d Log(const Eigen::Matrix3d &R)
{double theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));Eigen::Vector3d K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
}// 构建v的反对称矩阵
Eigen::Matrix3d hat(const Eigen::Vector3d &v)
{Eigen::Matrix3d Omega;Omega <<  0, -v(2),  v(1),  v(2),     0, -v(0), -v(1),  v(0),     0;return Omega;
}Eigen::Matrix3d jr(Eigen::Vector3d vec)
{double ang = vec.norm();if(ang < 1e-9){return I33;}else{vec /= ang;double ra = sin(ang)/ang;return ra*I33 + (1-ra)*vec*vec.transpose() - (1-cos(ang))/ang * hat(vec);}
}Eigen::Matrix3d jr_inv(const Eigen::Matrix3d &rotR)
{Eigen::AngleAxisd rot_vec(rotR);Eigen::Vector3d axi = rot_vec.axis();double ang = rot_vec.angle();if(ang < 1e-9){return I33;}else{double ctt = ang / 2 / tan(ang/2);return ctt*I33 + (1-ctt)*axi*axi.transpose() + ang/2 * hat(axi);}
}// 主要用到了p和R
struct IMUST
{double t;Eigen::Matrix3d R;Eigen::Vector3d p;Eigen::Vector3d v;Eigen::Vector3d bg;Eigen::Vector3d ba;Eigen::Vector3d g;IMUST(){setZero();}IMUST(double _t, const Eigen::Matrix3d& _R, const Eigen::Vector3d& _p, const Eigen::Vector3d& _v,const Eigen::Vector3d& _bg, const Eigen::Vector3d& _ba,const Eigen::Vector3d& _g = Eigen::Vector3d(0, 0, -G_m_s2)):t(_t), R(_R), p(_p), v(_v), bg(_bg), ba(_ba), g(_g){}IMUST &operator+=(const Eigen::Matrix<double, DIMU, 1> &ist){this->R = this->R * Exp(ist.block<3, 1>(0, 0));this->p += ist.block<3, 1>(3, 0);this->v += ist.block<3, 1>(6, 0);this->bg += ist.block<3, 1>(9, 0);this->ba += ist.block<3, 1>(12, 0);this->g += ist.block<3, 1>(15, 0);return *this;}Eigen::Matrix<double, DIMU, 1> operator-(const IMUST &b) {Eigen::Matrix<double, DIMU, 1> a;a.block<3, 1>(0, 0) = Log(b.R.transpose() * this->R);a.block<3, 1>(3, 0) = this->p - b.p;a.block<3, 1>(6, 0) = this->v - b.v;a.block<3, 1>(9, 0) = this->bg - b.bg;a.block<3, 1>(12, 0) = this->ba - b.ba;a.block<3, 1>(15, 0) = this->g - b.g;return a;}IMUST &operator=(const IMUST &b){this->R = b.R;this->p = b.p;this->v = b.v;this->bg = b.bg;this->ba = b.ba;this->g = b.g;this->t = b.t;return *this;}void setZero(){t = 0; R.setIdentity();p.setZero(); v.setZero();bg.setZero(); ba.setZero();g << 0, 0, -G_m_s2;}
};// 复制四元数 q，t
void assign_qt(Eigen::Quaterniond& q, Eigen::Vector3d& t,const Eigen::Quaterniond& q_, const Eigen::Vector3d& t_)
{q.w() = q_.w(); q.x() = q_.x(); q.y() = q_.y(); q.z() = q_.z();t(0) = t_(0); t(1) = t_(1); t(2) = t_(2);
}//该结构体主要服务于 downsample_voxel
struct M_POINT
{float xyz[3];int count = 0;
};//计算每一个体素中的均值点
void downsample_voxel(pcl::PointCloud<PointType>& pc, double voxel_size)
{if (voxel_size < 0.01)return;std::unordered_map<VOXEL_LOC, M_POINT> feature_map;size_t pt_size = pc.size();  // pt_size 在此时是点个数for (size_t i = 0; i < pt_size; i++){PointType &pt_trans = pc[i];float loc_xyz[3];for (int j = 0; j < 3; j++){loc_xyz[j] = pt_trans.data[j] / voxel_size;  // 计算点所在体素if (loc_xyz[j] < 0)loc_xyz[j] -= 1.0;}VOXEL_LOC position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]);  //计算所在体素索引auto iter = feature_map.find(position);if (iter != feature_map.end()) {iter->second.xyz[0] += pt_trans.x;  //加入该点坐标iter->second.xyz[1] += pt_trans.y;iter->second.xyz[2] += pt_trans.z;iter->second.count++;}else{M_POINT anp;  // 第一个加入该体素的点anp.xyz[0] = pt_trans.x;anp.xyz[1] = pt_trans.y;anp.xyz[2] = pt_trans.z;anp.count = 1;feature_map[position] = anp;}}pt_size = feature_map.size();   // pt_size 在此时是体素个数pc.clear();pc.resize(pt_size);size_t i = 0;for (auto iter = feature_map.begin(); iter != feature_map.end(); ++iter) // 求得体素内的点的均值点{pc[i].x = iter->second.xyz[0] / iter->second.count;pc[i].y = iter->second.xyz[1] / iter->second.count;pc[i].z = iter->second.xyz[2] / iter->second.count;i++;}
}/*rr是旋转矩阵，tt是平移矩阵pl1 是vector<Eigen::Vector3d>对自身进行RT变换
*/
void pl_transform(pcl::PointCloud<PointType> &pl1, const Eigen::Matrix3d &rr, const Eigen::Vector3d &tt)
{for(PointType &ap : pl1.points){Eigen::Vector3d pvec(ap.x, ap.y, ap.z);pvec = rr * pvec + tt;ap.x = pvec[0];ap.y = pvec[1];ap.z = pvec[2];}
}/*该函数是通过 IMUST 中的R,P把 porig中的三维点 变换到 ptran中PLV 是vector<Eigen::Vector3d>IMUST 中的 R和 P表示旋转和平移
*/
void plvec_trans(PLV(3) &porig, PLV(3) &ptran, IMUST &stat)
{uint asize = porig.size();ptran.resize(asize);for(uint i=0; i<asize; i++)ptran[i] = stat.R * porig[i] + stat.p;
}/*VOX_FACTOR VOX_FACTOR 的数据结构包含了一个协方差P，一个中心点V和一个点数N。函数解释：1.1 void push(const Eigen::Vector3d &vec) 加入一个点并更新 P V N。1.2  cov() 就是计算协方差了1.3 VOX_FACTOR & operator+=(const VOX_FACTOR& sigv) 就是P V N 的加1.4  transform(const VOX_FACTOR &sigv, const IMUST &stat) 就是R和T对 P V N 的变化
*/
class VOX_FACTOR
{
public:Eigen::Matrix3d P;Eigen::Vector3d v;int N;VOX_FACTOR(){P.setZero();v.setZero();N = 0;}void clear(){P.setZero();v.setZero();N = 0;}void push(const Eigen::Vector3d &vec){N++;P += vec * vec.transpose();v += vec;}Eigen::Matrix3d cov(){Eigen::Vector3d center = v / N;return P/N - center*center.transpose();}VOX_FACTOR & operator+=(const VOX_FACTOR& sigv){this->P += sigv.P;this->v += sigv.v;this->N += sigv.N;return *this;}void transform(const VOX_FACTOR &sigv, const IMUST &stat){N = sigv.N;v = stat.R*sigv.v + N*stat.p;Eigen::Matrix3d rp = stat.R * sigv.v * stat.p.transpose();P = stat.R*sigv.P*stat.R.transpose() + rp + rp.transpose() + N*stat.p*stat.p.transpose();}
};//平面参数计算
const double threshold = 0.1;
bool esti_plane(Eigen::Vector4d& pca_result, const pcl::PointCloud<PointType>& point)
{Eigen::Matrix<double, NMATCH, 3> A;Eigen::Matrix<double, NMATCH, 1> b;b.setOnes();b *= -1.0f;   // AX + BY + CZ +D =0； 其中D等于 1for (int j = 0; j < NMATCH; j++){A(j, 0) = point[j].x;  // A的系数A(j, 1) = point[j].y;  // B的系数A(j, 2) = point[j].z;  // C的系数}Eigen::Vector3d normvec = A.colPivHouseholderQr().solve(b); // A B C 参数for (int j = 0; j < NMATCH; j++){if (fabs(normvec.dot(A.row(j)) + 1.0) > threshold)  // 点到面的距离 大于 阈值（0.1）return false;  // 表示不是一个平面}double n = normvec.norm();   // sqrt(A*A + B*B + C*C)pca_result(0) = normvec(0) / n;  // Apca_result(1) = normvec(1) / n; // Bpca_result(2) = normvec(2) / n; // Cpca_result(3) = 1.0 / n;  // D = 1.0return true;
}#endif


 

这篇关于HBA代码解读（1）tools.hpp的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/726502。

相关文章

活用c4d官方开发文档查询代码

活用c4d官方开发文档查询代码

当你问AI助手比如豆包，如何用python禁止掉xpresso标签时候，它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来
阅读更多...
poj 1258 Agri-Net（最小生成树模板代码）

poj 1258 Agri-Net（最小生成树模板代码）

感觉用这题来当模板更适合。 题意就是给你邻接矩阵求最小生成树啦。~ prim代码：效率很高。172k...0ms。 #include<stdio.h>#include<algorithm>using namespace std;const int MaxN = 101;const int INF = 0x3f3f3f3f;int g[MaxN][MaxN];int n
阅读更多...
MCU7.keil中build产生的hex文件解读

MCU7.keil中build产生的hex文件解读

1.hex文件大致解读 闲来无事,查看了MCU6.用keil新建项目的hex文件 用FlexHex打开 给我的第一印象是:经过软件的解释之后,发现这些数据排列地十分整齐 :02000F0080FE71:03000000020003F8:0C000300787FE4F6D8FD75810702000F3D:00000001FF 把解释后的数据当作十六进制来观察 1.每一行数据
阅读更多...
Java ArrayList扩容机制 （源码解读）

Java ArrayList扩容机制 （源码解读）

结论：初始长度为10，若所需长度小于1.5倍原长度，则按照1.5倍扩容。若不够用则按照所需长度扩容。 一. 明确类内部重要变量含义         1：数组默认长度         2:这是一个共享的空数组实例，用于明确创建长度为0时的ArrayList ，比如通过 new ArrayList<>(0)，ArrayList 内部的数组 elementData 会指向这个 EMPTY_EL
阅读更多...
计算机毕业设计 大学志愿填报系统 Java+SpringBoot+Vue 前后端分离 文档报告 代码讲解 安装调试

计算机毕业设计 大学志愿填报系统 Java+SpringBoot+Vue 前后端分离 文档报告 代码讲解 安装调试

🍊作者：计算机编程-吉哥 🍊简介：专业从事JavaWeb程序开发，微信小程序开发，定制化项目、 源码、代码讲解、文档撰写、ppt制作。做自己喜欢的事，生活就是快乐的。 🍊心愿：点赞 👍 收藏 ⭐评论 📝 🍅 文末获取源码联系 👇🏻 精彩专栏推荐订阅 👇🏻 不然下次找不到哟~Java毕业设计项目~热门选题推荐《1000套》 目录 1.技术选型 2.开发工具 3.功能
阅读更多...
代码随想录冲冲冲 Day39 动态规划Part7

代码随想录冲冲冲 Day39 动态规划Part7

198. 打家劫舍 dp数组的意义是在第i位的时候偷的最大钱数是多少 如果nums的size为0 总价值当然就是0 如果nums的size为1 总价值是nums[0] 遍历顺序就是从小到大遍历 之后是递推公式 对于dp[i]的最大价值来说有两种可能 1.偷第i个 那么最大价值就是dp[i-2]+nums[i] 2.不偷第i个 那么价值就是dp[i-1] 之后取这两个的最大值就是d
阅读更多...
pip-tools：打造可重复、可控的 Python 开发环境，解决依赖关系，让代码更稳定

pip-tools：打造可重复、可控的 Python 开发环境，解决依赖关系，让代码更稳定

在 Python 开发中，管理依赖关系是一项繁琐且容易出错的任务。手动更新依赖版本、处理冲突、确保一致性等等，都可能让开发者感到头疼。而 pip-tools 为开发者提供了一套稳定可靠的解决方案。 什么是 pip-tools？ pip-tools 是一组命令行工具，旨在简化 Python 依赖关系的管理，确保项目环境的稳定性和可重复性。它主要包含两个核心工具：pip-compile 和 pip
阅读更多...
【Tools】大模型中的自注意力机制

【Tools】大模型中的自注意力机制

摇来摇去摇碎点点的金黄 伸手牵来一片梦的霞光 南方的小巷推开多情的门窗 年轻和我们歌唱 摇来摇去摇着温柔的阳光 轻轻托起一件梦的衣裳 古老的都市每天都改变模样                      🎵 方芳《摇太阳》 自注意力机制（Self-Attention）是一种在Transformer等大模型中经常使用的注意力机制。该机制通过对输入序列中的每个元素计算与其他元素之间的相似性，
阅读更多...
D4代码AC集

D4代码AC集

贪心问题解决的步骤： （局部贪心能导致全局贪心）    1.确定贪心策略    2.验证贪心策略是否正确 排队接水 #include<bits/stdc++.h>using namespace std;int main(){int w,n,a[32000];cin>>w>>n;for(int i=1;i<=n;i++){cin>>a[i];}sort(a+1,a+n+1);int i=1
阅读更多...
Spring 源码解读：自定义实现Bean定义的注册与解析

Spring 源码解读：自定义实现Bean定义的注册与解析

引言 在Spring框架中，Bean的注册与解析是整个依赖注入流程的核心步骤。通过Bean定义，Spring容器知道如何创建、配置和管理每个Bean实例。本篇文章将通过实现一个简化版的Bean定义注册与解析机制，帮助你理解Spring框架背后的设计逻辑。我们还将对比Spring中的BeanDefinition和BeanDefinitionRegistry，以全面掌握Bean注册和解析的核心原理。
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2