|
|
@@ -1,4 +1,6 @@
|
|
|
#include <pcl/visualization/cloud_viewer.h>
|
|
|
+#include <pcl/common/transforms.h>
|
|
|
+#include <pcl/visualization/pcl_visualizer.h>
|
|
|
#include <iostream>
|
|
|
#include <pcl/io/io.h>
|
|
|
#include <pcl/io/pcd_io.h>
|
|
|
@@ -24,6 +26,9 @@ double gflag_z = -1;
|
|
|
double gflag_x_len = 1;
|
|
|
double gflag_y_len = 1;
|
|
|
double gflag_z_len = 1;
|
|
|
+double gflag_x_rotation = 0;
|
|
|
+double gflag_y_rotation = 0;
|
|
|
+double gflag_z_rotation = 0;
|
|
|
bool gbflag_show = true;
|
|
|
bool gbcar_show = true;
|
|
|
|
|
|
@@ -39,7 +44,7 @@ int GetOptLong(int argc, char *argv[]) {
|
|
|
int digit_optind = 0; // 设置短参数类型及是否需要参数
|
|
|
|
|
|
// 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
|
|
|
- // 第几个元素的描述，即是long_opts的下标值
|
|
|
+ // 第几个元素的描述，即是long_opts的下标值Cloud
|
|
|
int option_index = 0;
|
|
|
// 设置短参数类型及是否需要参数
|
|
|
const char *optstring = "m:h";
|
|
|
@@ -198,8 +203,31 @@ void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
|
|
|
// viewer.addText3D("car",pos,0.1,1.0,0.0,0.0);
|
|
|
|
|
|
if(gbflag_show)
|
|
|
+
|
|
|
viewer.addCube(gflag_x -0.5* gflag_x_len,gflag_x + 0.5* gflag_x_len, gflag_y - 0.5* gflag_y_len, gflag_y + 0.5* gflag_y_len,
|
|
|
gflag_z - 0.5* gflag_z_len,gflag_z + 0.5* gflag_z_len,0.0,1.0,0.0,"flag",0);
|
|
|
+ Eigen::Affine3f transformto_ori = Eigen::Affine3f::Identity();
|
|
|
+ transformto_ori.translation()<<-gflag_x,-gflag_y,-gflag_z;
|
|
|
+ viewer.updateShapePose("flag",transformto_ori);
|
|
|
+
|
|
|
+ Eigen::Affine3f rotation_x = Eigen::Affine3f::Identity();
|
|
|
+ rotation_x.rotate(Eigen::AngleAxisf(gflag_x_rotation*M_PI/180,Eigen::Vector3f::UnitX()));
|
|
|
+
|
|
|
+ Eigen::Affine3f rotation_y = Eigen::Affine3f::Identity();
|
|
|
+ rotation_y.rotate(Eigen::AngleAxisf(gflag_y_rotation*M_PI/180,Eigen::Vector3f::UnitY()));
|
|
|
+
|
|
|
+ Eigen::Affine3f rotation_z = Eigen::Affine3f::Identity();
|
|
|
+ rotation_z.rotate(Eigen::AngleAxisf(gflag_z_rotation*M_PI/180,Eigen::Vector3f::UnitZ()));
|
|
|
+
|
|
|
+ Eigen::Affine3f rotation=rotation_x*rotation_y*rotation_z;
|
|
|
+
|
|
|
+ Eigen::Affine3f tramsform_rotation = Eigen::Affine3f::Identity();
|
|
|
+ tramsform_rotation.rotate(rotation.rotation());
|
|
|
+ viewer.updateShapePose("flag",tramsform_rotation * transformto_ori);
|
|
|
+
|
|
|
+ Eigen::Affine3f transform_to_ori = Eigen::Affine3f::Identity();
|
|
|
+ transform_to_ori.translation()<<gflag_x,gflag_y,gflag_z;
|
|
|
+ viewer.updateShapePose("flag",transform_to_ori*(tramsform_rotation * transformto_ori));
|
|
|
//Draw Car
|
|
|
if(gbcar_show)
|
|
|
viewer.addCube(-0.9,0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
|
|
|
@@ -260,6 +288,9 @@ void LoadParam(std::string strxmlpath)
|
|
|
gflag_x_len = xp.GetParam("flag_x_len",1.8);
|
|
|
gflag_y_len = xp.GetParam("flag_y_len",1.0);
|
|
|
gflag_z_len = xp.GetParam("flag_z_len",1.5);
|
|
|
+ gflag_x_rotation = xp.GetParam("flag_x_rotation",0);
|
|
|
+ gflag_y_rotation = xp.GetParam("flag_y_rotation",0);
|
|
|
+ gflag_z_rotation = xp.GetParam("flag_z_rotation",0);
|
|
|
gbflag_show = xp.GetParam("flag_show",true);
|
|
|
gbcar_show = xp.GetParam("car_show",true);
|
|
|
}
|
dongjunhong