pcl最新版本有x86平台的优化,这里添加Arm平台,使用NEON指令优化:
#if __ARM_NEON
#include <arm_neon.h>
template<>
struct Transformer<float>{
float32x4_t tf_[4];
Transformer(const Eigen::Matrix4f& tf){
for (int i = 0; i < 4; ++i)
{
tf_[i] = vld1q_f32(tf.col(i).data());
}
}
void se3 (const float* src, float* tgt) const{
float32x4_t p0= vmulq_f32(vdupq_n_f32(src[0]),tf_[0]);
float32x4_t p1= vmulq_f32(vdupq_n_f32(src[1]),tf_[1]);
float32x4_t p2= vmulq_f32(vdupq_n_f32(src[2]),tf_[2]);
vst1q_f32(tgt,vaddq_f32(p0,vaddq_f32(p1,vaddq_f32(p2,tf_[3]))));
}
void so3 (const float* src, float* tgt) const{
float32x4_t p0= vmulq_f32(vdupq_n_f32(src[0]),tf_[0]);
float32x4_t p1= vmulq_f32(vdupq_n_f32(src[1]),tf_[1]);
float32x4_t p2= vmulq_f32(vdupq_n_f32(src[2]),tf_[2]);
vst1q_f32(tgt,vaddq_f32(p0,vaddq_f32(p1,p2));
}
};
以上针对单个点,可适用于稠密和稀疏点云,稠密点云可以四个点一起处理