int main (int argc, char* argv[])
{
boost::program_options::options_description desc("Options");
desc.add_options()
("help", "Print help message")
("pcd_filename", boost::program_options::value<std::string>()->required(), "pcl pcd filename");
boost::program_options::variables_map vm;
try {
boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
if( vm.count("help") ){
std::cout << "Resize Point cloud using voxel grid filter" << std::endl;
std::cerr << desc << std::endl;
return 0;
}
boost::program_options::notify(vm);
} catch (boost::program_options::error& e) {
std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
std::cerr << desc << std::endl;
return -1;
}
std::string pcd_fname = vm["pcd_filename"].as<std::string>();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile (pcd_fname, *source_cloud);
// Rotate pointcloud 0.6[rad] around z axis
Eigen::Matrix4f rotate_around_zaxis = Eigen::Matrix4f::Identity();
float theta = 0.6;
rotate_around_zaxis(0,0) = cos(theta);
rotate_around_zaxis(0,1) = -sin(theta);
rotate_around_zaxis(1,0) = sin(theta);
rotate_around_zaxis(1,1) = cos(theta);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::transformPointCloud(*source_cloud, *rotated_cloud, rotate_around_zaxis);
// Transform point cloud using homogeneous transformation matrix
// which is got by icp mathcing between 3D map reference point and manual map reference point
Eigen::Matrix4f icp_transform_matrix = Eigen::Matrix4f::Identity();
icp_transform_matrix(0, 0) = 0.950235;
icp_transform_matrix(0, 1) = -0.307961;
icp_transform_matrix(0, 2) = 0.0470266;
icp_transform_matrix(0, 3) = 0.147511;
icp_transform_matrix(1, 0) = 0.30832;
icp_transform_matrix(1, 1) = 0.951281;
icp_transform_matrix(1, 2) = -0.000411891;
icp_transform_matrix(1, 3) = -0.0305933;
icp_transform_matrix(2, 0) = -0.0446088;
icp_transform_matrix(2, 1) = 0.0148907;
icp_transform_matrix(2, 2) = 0.998893;
icp_transform_matrix(2, 3) = -0.0312964;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::transformPointCloud(*rotated_cloud, *transformed_cloud, icp_transform_matrix);
pcl::io::savePCDFileASCII("transformed_cloud.pcd", *transformed_cloud);
std::cout << "Write to transformed_cloud.pcd" << std::endl;
return 0;
}
//--------------------------------------------------------------------------------
// apply yaw
//--------------------------------------------------------------------------------
void zz_camera_follow::apply_yaw (vec3& new_camera_dir_out, float follow_yaw_in, const vec3& target_dir_in)
{
mat3 rotate_around_zaxis(mat3_id);
rotate_around_zaxis.set_rot(follow_yaw_in, vec3(0, 0, 1));
// new_camera_dir = rotate_arount_zaxis * target_dir
mult(new_camera_dir_out, rotate_around_zaxis, target_dir_in);
}
