Add flag cube rotation around the xyz axis

src/tool/pointcloudviewer/main.cpp

@@ -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);
 }

src/tool/pointcloudviewer/pointcloudviewer.xml

@@ -6,6 +6,9 @@
 		<param name="flag_x_len" value="1.8" />
 		<param name="flag_y_len" value="1.0" />
 		<param name="flag_z_len" value="1.5" />
+		<param name="flag_x_rotation" value="0" />
+		<param name="flag_y_rotation" value="0" />
+		<param name="flag_z_rotation" value="0" />
 		<param name="flag_show" value="true" />
 		<param name="car_show" value="true" />
 	</node>