// 引入Eigen头文件与常用类型
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <pcl/point_types.h>//点类型定义
#include <pcl/point_cloud.h>//点云类
using PointType = pcl::PointXYZ;
using Vec3f = Eigen::Vector3f;
//非类型模板形参
template <int dim>
Eigen::Matrix<int, dim, 1> Pos2Grid(const Eigen::Matrix<float, dim, 1>& pt) {
return pt.array().template round().template cast<int>();
}
// round():四舍五入
int main()
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.width = 5;
cloud.height = 1;
cloud.points.resize(cloud.width * cloud.height);
for (size_t i = 0; i< cloud.points.size(); ++i)
{
cloud.points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud.points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud.points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
}
cloud.points[4].x = 3.12;
cloud.points[4].y = 4.12;
cloud.points[4].z = 5.82;
for(size_t i = 0; i< cloud.points.size(); ++i)
{
auto pt = cloud.points[i];
auto key = Pos2Grid(Eigen::Vector3f(pt.data));
std::cout << key << std::endl;
std::cout << "------------------------------" << std::endl;
}
}
// pointxyz转eigen
/*
pcl::PointXYZ pt;
// 方法1:通过坐标访问
Eigen::Vector3f eigen_pt(pt.x, pt.y, pt.z);
// 方法2:直接赋值转换
Eigen::Vector3f eigen_pt = pt.getVector3fMap();
// 方法3:通过Array转换
Eigen::Array3f array(pt.data);
Eigen::Vector3f eigen_pt = array;
*/
高翔《自动驾驶与机器人中的SLAM技术》第五章:代码源码:
template <int dim>
Eigen::Matrix<int, dim, 1> GridNN<dim>::Pos2Grid(const Eigen::Matrix<float, dim, 1>& pt) {
return pt.array().template round().template cast<int>();
// Eigen::Matrix<int, dim, 1> ret;
// for (int i = 0; i < dim; ++i) {
// ret(i, 0) = round(pt[i] * inv_resolution_);
// }
// return ret;
}
# 工程名
set (PROJECT_NAME mytest)
PROJECT(${PROJECT_NAME})
message("工程名: " ${PROJECT_NAME})
message("工程目录: " ${PROJECT_SOURCE_DIR})
cmake_minimum_required(VERSION 3.6)
set(CMAKE_CXX_STANDARD 17)
SET(CMAKE_BUILD_TYPE "DEBUG")
# eigen 3
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})
# pcl
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(${PROJECT_NAME} test1.cpp)
target_link_libraries(${PROJECT_NAME}
${PCL_LIBRARIES}
)
输出
0
0
0
------------------------------
0
0
0
------------------------------
-1
1
0
------------------------------
-1
1
0
------------------------------
3
4
6
------------------------------
观察最后一个3,4,6,发现是取整四舍五入。