ROS中对point_cloud旋转平移

需要对扫描出来的点云数据标定，通过旋转平移使得其在一个正确的坐标系上
为了实现绕Z轴旋转并使xoy平面平移，矩阵是这样的
cosa -sina 0 x
sina cosa 0 y
0 0 1 0
0 0 0 1

用到了pcl库
订阅需要旋转和平移的值还有需要标定的point_cloud,发布标定后的point_cloud

#include "ros/ros.h"
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include "std_msgs/Float64.h"
#include <geometry_msgs/Point.h> //点 消息类型
#include <geometry_msgs/PointStamped.h> //可以在rviz中显示
#include <iostream>
#include <stdio.h>
#include<std_msgs/String.h>
#include<geometry_msgs/QuaternionStamped.h>
#include <pcl/registration/icp.h>   //ICP配准类相关头文件
using namespace std;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
pcl::PointCloud<pcl::PointXYZ> pc_v;
geometry_msgs::Quaternion cp_v;
using namespace Eigen;

ros::Publisher c_cloud;
void calibration(pcl::PointCloud<pcl::PointXYZ> in_pc,geometry_msgs::Quaternion in_cp){
    pcl::PointCloud<pcl::PointXYZ> out_pc;


 MatrixXf rotate(4,4);
 rotate << cos(atan(in_cp.w)),-sin(atan(in_cp.w)),0,in_cp.x,
           sin(atan(in_cp.w)),cos(atan(in_cp.w)),0,in_cp.y,
           0,0,1,0,
           0,0,0,1;
  pcl::transformPointCloud (in_pc, out_pc, rotate);

c_cloud.publish(out_pc);

}

void centercallback(const geometry_msgs::Quaternion& c_point){

    cp_v=c_point;

}

void pcCallback(const pcl::PointCloud<pcl::PointXYZ> pc){
    calibration(pc,cp_v);

}

int main(int argc,char * argv[])
{
    ros::init(argc, argv, "scan_pcl");
    ros::NodeHandle nh;
    ros::Rate r(1); //执行频率5Hz
    ros::Subscriber center = nh.subscribe("/movement/angle", 1000, centercallback);
    ros::Subscriber pc=nh.subscribe("/cloudc",1000,pcCallback);
    c_cloud=nh.advertise<PointCloud>("/point_cloud3",1000);

    while (ros::ok()) {
        ros::spinOnce();
     r.sleep();
}
    return 0;
}