return pt.array().template round().template cast<int>();

// 引入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,发现是取整四舍五入。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值