有关C++问题

Freedy_S

这是头文件

1. #ifndef _UTILITY_LIDAR_ODOMETRY_H_
   
2. #define _UTILITY_LIDAR_ODOMETRY_H_
   
3. 
   
4. 
   
5. #include <ros/ros.h>
   
6. 
   
7. #include <sensor_msgs/Imu.h>
   
8. #include <sensor_msgs/PointCloud2.h>
   
9. #include <nav_msgs/Odometry.h>
   
10. #include<nav_msgs/Path.h>
    
11. #include<geometry_msgs/PoseStamped.h>
    
12. #include<geometry_msgs/Pose2D.h>
    
13. #include <geometry_msgs/Quaternion.h>
    
14. #include "cloud_msgs/cloud_info.h"
    
15. 
    
16. #include <opencv/cv.h>
    
17. 
    
18. #include <pcl/point_cloud.h>
    
19. #include <pcl/point_types.h>
    
20. #include <pcl_ros/point_cloud.h>
    
21. #include <pcl_conversions/pcl_conversions.h>
    
22. #include <pcl/range_image/range_image.h>
    
23. #include <pcl/filters/filter.h>
    
24. #include <pcl/filters/voxel_grid.h>
    
25. #include <pcl/kdtree/kdtree_flann.h>
    
26. #include <pcl/common/common.h>
    
27. #include <pcl/registration/icp.h>
    
28. 
    
29. #include <tf/transform_broadcaster.h>
    
30. #include <tf/transform_datatypes.h>
    
31. 
    
32. #include <vector>
    
33. #include <cmath>
    
34. #include <algorithm>
    
35. #include <queue>
    
36. #include <deque>
    
37. #include <iostream>
    
38. #include <fstream>
    
39. #include <ctime>
    
40. #include <cfloat>
    
41. #include <iterator>
    
42. #include <sstream>
    
43. #include <string>
    
44. #include <limits>
    
45. #include <iomanip>
    
46. #include <array>
    
47. #include <thread>
    
48. #include <mutex>
    
49. 
    
50. #define PI 3.14159265
    
51. 
    
52. using namespace std;
    
53. 
    
54. typedef pcl::PointXYZI  PointType;
    
55. 
    
56. extern const string pointCloudTopic = "/lidar_points";
    
57. extern const string imuTopic = "";
    
58. 
    
59. // Save pcd
    
60. extern const string fileDirectory = "/home/lizhiyuan/";
    
61. 
    
62. // Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
    
63. extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
    
64. 
    
65. // VLP-16
    
66. //extern const int N_SCAN = 16;
    
67. //extern const int Horizon_SCAN = 1800;
    
68. //extern const float ang_res_x = 0.2;
    
69. //extern const float ang_res_y = 2.0;
    
70. //extern const float ang_bottom = 15.0+0.1;
    
71. //extern const int groundScanInd = 7;
    
72. 
    
73. // HDL-32E
    
74. // extern const int N_SCAN = 32;
    
75. // extern const int Horizon_SCAN = 1800;
    
76. // extern const float ang_res_x = 360.0/float(Horizon_SCAN);
    
77. // extern const float ang_res_y = 41.33/float(N_SCAN-1);
    
78. // extern const float ang_bottom = 30.67;
    
79. // extern const int groundScanInd = 20;
    
80. 
    
81. //C-Fans-32
    
82. // extern const int N_SCAN = 32;
    
83. // extern const int Horizon_SCAN =600;
    
84. // extern const float ang_res_x = 0.2;
    
85. // extern const float ang_res_y =0.6;
    
86. // extern const float ang_bottom = 10.2;
    
87. // extern const int groundScanInd = 10;
    
88. 
    
89. //R-Fans-32
    
90. // extern const int N_SCAN = 32;
    
91. // extern const int Horizon_SCAN =2000;
    
92. // extern const float ang_res_x = 0.18;
    
93. // extern const float ang_res_y =1;
    
94. // extern const float ang_bottom = 20;
    
95. // extern const int groundScanInd = 10;
    
96. //R-fans-32M
    
97. extern const int N_SCAN = 32;
    
98. extern const int Horizon_SCAN =2000;
    
99. extern const float ang_res_x = 0.18;
    
100. //extern const float ang_res_y =0.87;
     
101. extern const float ang_bottom = 16.9;
     
102. extern const int groundScanInd = 10;
     
103. //HDL-64
     
104. // extern const int N_SCAN = 64;
     
105. // extern const int Horizon_SCAN =4000;
     
106. // extern const float ang_res_x = 0.09;
     
107. // extern const float ang_res_y =0.4;
     
108. // extern const float ang_bottom = 24.8;
     
109. // extern const int groundScanInd = 20;
     
110. 
     
111. 
     
112. 
     
113. // VLS-128
     
114. // extern const int N_SCAN = 128;
     
115. // extern const int Horizon_SCAN = 1800;
     
116. // extern const float ang_res_x = 0.2;
     
117. // extern const float ang_res_y = 0.3;
     
118. // extern const float ang_bottom = 25.0;
     
119. // extern const int groundScanInd = 10;
     
120. 
     
121. // Ouster users may need to uncomment line 159 in imageProjection.cpp
     
122. // Usage of Ouster imu data is not fully supported yet (LeGO-LOAM needs 9-DOF IMU), please just publish point cloud data
     
123. // Ouster OS1-16
     
124. // extern const int N_SCAN = 16;
     
125. // extern const int Horizon_SCAN = 1024;
     
126. // extern const float ang_res_x = 360.0/float(Horizon_SCAN);
     
127. // extern const float ang_res_y = 33.2/float(N_SCAN-1);
     
128. // extern const float ang_bottom = 16.6+0.1;
     
129. // extern const int groundScanInd = 7;
     
130. 
     
131. // Ouster OS1-64
     
132. // extern const int N_SCAN = 64;
     
133. // extern const int Horizon_SCAN = 1024;
     
134. // extern const float ang_res_x = 360.0/float(Horizon_SCAN);
     
135. // extern const float ang_res_y = 33.2/float(N_SCAN-1);
     
136. // extern const float ang_bottom = 16.6+0.1;
     
137. // extern const int groundScanInd = 15;
     
138. 
     
139. extern const bool loopClosureEnableFlag = false;
     
140. extern const double mappingProcessInterval = 0.3;
     
141. 
     
142. extern const float scanPeriod = 0.1;
     
143. extern const int systemDelay = 0;
     
144. extern const int imuQueLength = 200;
     
145. 
     
146. extern const float sensorMinimumRange = 1.0;
     
147. extern const float sensorMountAngle = 0.0;
     
148. extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
     
149. extern const int segmentValidPointNum = 5;
     
150. extern const int segmentValidLineNum = 3;
     
151. extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
     
152. extern const float segmentAlphaY = 1/ 180.0 * M_PI;
     
153. 
     
154. 
     
155. extern const int edgeFeatureNum = 2;
     
156. extern const int surfFeatureNum = 4;
     
157. extern const int sectionsTotal = 6;
     
158. extern const float edgeThreshold = 0.1;
     
159. extern const float surfThreshold = 0.1;
     
160. extern const float nearestFeatureSearchSqDist = 25;
     
161. 
     
162. 
     
163. // Mapping Params
     
164. extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled)
     
165. extern const int   surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)
     
166. // history key frames (history submap for loop closure)
     
167. extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure
     
168. extern const int   historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure
     
169. extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment
     
170. 
     
171. extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized
     
172. 
     
173. 
     
174. struct smoothness_t{
     
175.     float value;
     
176.     size_t ind;
     
177. };
     
178. 
     
179. struct by_value{
     
180.     bool operator()(smoothness_t const &left, smoothness_t const &right) {
     
181.         return left.value < right.value;
     
182.     }
     
183. };
     
184. 
     
185. /*
     
186.     * A point cloud type that has "ring" channel
     
187.     */
     
188. struct PointXYZIR
     
189. {
     
190.     PCL_ADD_POINT4D
     
191.     PCL_ADD_INTENSITY;
     
192.     uint16_t ring;
     
193.     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     
194. } EIGEN_ALIGN16;
     
195. 
     
196. POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
     
197.                                    (float, x, x) (float, y, y)
     
198.                                    (float, z, z) (float, intensity, intensity)
     
199.                                    (uint16_t, ring, ring)
     
200. )
     
201. 
     
202. /*
     
203.     * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
     
204.     */
     
205. struct PointXYZIRPYT
     
206. {
     
207.     PCL_ADD_POINT4D
     
208.     PCL_ADD_INTENSITY;
     
209.     float roll;
     
210.     float pitch;
     
211.     float yaw;
     
212.     double time;
     
213.     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     
214. } EIGEN_ALIGN16;
     
215. 
     
216. POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
     
217.                                    (float, x, x) (float, y, y)
     
218.                                    (float, z, z) (float, intensity, intensity)
     
219.                                    (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
     
220.                                    (double, time, time)
     
221. )
     
222. 
     
223. typedef PointXYZIRPYT  PointTypePose;
     
224. 
     
225. #endif
     

复制代码

liuzhengyuan

你搞成付费了，应该是悬赏

高山

你弄个付费干啥