├── 0.prerequisites ├── 1.Eigen사용법 │ └── README.md ├── 2.PCL사용법 │ ├── PCL 자주 쓰이는 라이브러리 정리 │ │ └── README.md │ ├── PCL 정리 및 기본 사용법 │ │ └── README.md │ ├── README.md │ └── img │ │ ├── pcl_centroid.PNG │ │ ├── pcl_robot_sensor.PNG │ │ └── pcl_sor.PNG └── README.md ├── 1.ouster_setting ├── README.md └── README2.md ├── 2.graph_slam └── README.md ├── README.md ├── readme_materials ├── ouster-data.gif ├── ouster_after.png ├── ouster_error.png ├── ouster_packet.png └── ouster_type.png └── tf.png /0.prerequisites/1.Eigen사용법/README.md: -------------------------------------------------------------------------------- 1 | # Eigen on Robotics 2 | 3 | Original author: Hyungtae Lim (shapelim@kaist.ac.kr) 4 | 5 | # WBU!!!!!!!! 6 | ---------------- 7 | 8 | ## Eigen library를 사용하는 이유 9 | 10 | ROS 상에서 로봇의 pose 값은 [nav_msgs/Odometry](http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html)나 [geometry_msgs/PoseStamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseStamped.html) 타입으로 데이터를 제공하는데, 이 메세지를 C++ 상에서 활용할 때 Eigen의 Matrix로 변환해서 사용하면 편하다. 왜냐하면 4x4 변환행렬(transformation matrix)로 pose를 표현하게 되면 상대적인 pose를 구할 때나(e.g. t-1의 pose와 t의 pose를 pre_pose, curr_pose라는 이름의 Matrix4f로 변환했을 때, prev_pose.inverse() * curr_pose를 하면 t-1 pose의 관점에서 t의 pose를 바라봤을 때의 상대적인 포즈를 손쉽게 구할 수 있음 ) pose의 좌표계의 변환이 굉장히 용이해지기 때문. 11 | 12 | * 내용 참조: [Introduction to Robotics chapter 2 읽기](http://www.mech.sharif.ir/c/document_library/get_file?uuid=5a4bb247-1430-4e46-942c-d692dead831f&groupId=14040) 13 | 14 | 15 | ### 선언 16 | ```cpp 17 | Eigen::Matrix4f m4x4; 18 | Eigen::Matrix4f m4x4; 19 | Eigen::Matrix3d& eigen3x3 20 | ``` 21 | 22 | 23 | ```cpp 24 | MatrixXd m(2,2); 25 | double c= 3; 26 | 27 | m << 2, c, 28 | 4, 5; 29 | VectorXd v(2); 30 | v <<1, 3; 31 | 32 | VectorXd vv= m.transpose() * v; 33 | cout< cloud; 98 | pcl::PointXYZ point_xyz; // pcl::PointXYZ이라는 type에 data를 담는다. 99 | 100 | point_xyz.x = 1; 101 | point_xyz.y = 2; 102 | point_xyz.z = 3; 103 | cloud.push_back(point_xyz); 104 | 105 | point_xyz.x = 4; 106 | point_xyz.y = 5; 107 | point_xyz.z = 6; 108 | cloud.push_back(point_xyz); 109 | 110 | point_xyz.x = 7; 111 | point_xyz.y = 8; 112 | point_xyz.z = 9; 113 | cloud.push_back(point_xyz); 114 | ``` 115 | or 116 | ```cpp 117 | pcl::PointCloud cloud; 118 | cloud.push_back(pcl::PointXYZ(1, 2, 3)); 119 | cloud.push_back(pcl::PointXYZ(4, 5, 6)); 120 | cloud.push_back(pcl::PointXYZ(7, 8, 9)); 121 | ``` 122 | 123 | 출력을 하면 아래와 같은 결과를 볼 수 있다. 124 | ```cpp 125 | for (int i = 0 ; i < cloud.size(); ++i){ 126 | cout << i << ": "; 127 | cout << cloud.points[i].x << ", "; 128 | cout << cloud.points[i].y << ", "; 129 | cout << cloud.points[i].z << endl; 130 | 131 | } 132 | ``` 133 | ##### Result:
134 | 0: 1, 2, 3
135 | 1: 4, 5, 6
136 | 2: 7, 8, 9 137 | 138 | ## Functions 139 | 140 | http://docs.pointclouds.org/1.8.1/classpcl_1_1_point_cloud.html 141 | 142 | ### begin(): PointCloud의 첫 부분을 가르키는 *iterator*를 반환함 143 | 144 | ```cpp 145 | cout << "begin(): "; 146 | cout << cloud.begin()->x << ", "; 147 | cout << cloud.begin()->y << ", "; 148 | cout << cloud.begin()->z << endl; 149 | ``` 150 | 151 | ->의 의미는 옆 사이트에 설명되어 있다. https://ianuarias.tistory.com/16 152 | 153 | ->의 의미 한 줄 요약: return 값이 주소값이기 때문에, 주소값이 가르키는 객체의 멤버변수/멤버함수는 ->로 지칭함. 154 | 155 | ##### Result::
156 | begin(): 1, 2, 3 157 | 158 | ### end(): PointCloud의 끝 부분을 가르키는 *iterator*를 반환함 159 | 160 | ```cpp 161 | cout << "end(): "; 162 | cout << cloud.end()->x << ", "; 163 | cout << cloud.end()->y << ", "; 164 | cout << cloud.end()->z << endl; 165 | ``` 166 | ##### Result:
167 | end(): 0, 0, 0 168 | 169 | ????? 왜 0, 0, 0?: vector의 end()처럼 마지막 요소의 다음 부분을 가르키는 iterator를 리턴하기 때문. 170 | 171 | 따라서 우리가 원하는 pcl::PointCloud의 제일 뒷쪽의 요소를 가르키는 iterator를 뜻하려면 다음과 같이 -1을 빼주면 된다. 172 | 173 | ```cpp 174 | cout << "end() -1: "; 175 | cout << (cloud.end()-1)->x << ", "; 176 | cout << (cloud.end()-1)->y << ", "; 177 | cout << (cloud.end()-1)->z << endl; 178 | ``` 179 | ##### Result:
180 | end() - 1: 7, 8, 9 181 | 182 | ### front(): 첫번째 *원소*를 반환함 183 | ```cpp 184 | cout << "front(): "; 185 | cout << cloud.front().x << ", "; 186 | cout << cloud.front().y << ", "; 187 | cout << cloud.front().z << endl; 188 | ``` 189 | ##### Result:
190 | front(): 1, 2, 3 191 | 192 | ### back(): 마지막 *요소*를 반환함 193 | 194 | ```cpp 195 | cout << "back(): "; 196 | cout << cloud.back().x << ", "; 197 | cout << cloud.back().y << ", "; 198 | cout << cloud.back().z << endl; 199 | ``` 200 | 201 | ##### Result:
202 | back(): 7, 8, 9 203 | 204 | ### at(int index) 205 | pcl::PointCloud 상의 index에 해당하는 요소를 반환함. 206 | 207 | ```cpp 208 | cout << "at(1): "; 209 | cout << cloud.at(1).x << ", "; 210 | cout << cloud.at(1).y << ", "; 211 | cout << cloud.at(1).z << endl; 212 | ``` 213 | ##### Result:
214 | at(1): 4, 5, 6 215 | 216 | ### empty() 217 | ```cpp 218 | if (cloud.empty()) cout << "True"; else cout << "False"; 219 | cout << " | size of pc: " << cloud.size() << endl; 220 | ``` 221 | ##### Result:
222 | False | size of pc: 3 223 | 224 | ### clear() 225 | ```cpp 226 | cloud.empty(); 227 | if (cloud.empty()) cout << "True"; else cout << "False"; 228 | cout << " | size of pc: " << cloud.size() << endl; 229 | ``` 230 | ##### Result:
231 | True | size of pc: 0 232 | 233 | 234 | ## ★PointCloud 합치기 235 | ~~이런 pythonic한 문법이 되다니...갓PCL...~~ 236 | ```cpp 237 | pcl::PointCloud cloud2; 238 | cloud2.push_back(pcl::PointXYZ(1, 2, 3)); 239 | cloud2.push_back(pcl::PointXYZ(4, 5, 6)); 240 | 241 | 242 | pcl::PointCloud cloud3; 243 | cloud3.push_back(pcl::PointXYZ(7, 8, 9)); 244 | cloud3.push_back(pcl::PointXYZ(10, 11, 12)); 245 | 246 | cloud2 += cloud3; 247 | 248 | cout <<"size: " << cloud2.size() << endl; 249 | for (int i = 0 ; i < cloud2.size(); ++i){ 250 | cout << i << ": "; 251 | cout << cloud2.points[i].x << ", "; 252 | cout << cloud2.points[i].y << ", "; 253 | cout << cloud2.points[i].z << endl; 254 | } 255 | ``` 256 | ##### Result:
257 | size: 4
258 | 0: 1, 2, 3
259 | 1: 4, 5, 6
260 | 2: 7, 8, 9
261 | 3: 10, 11, 12 262 | 263 | ## PCL pointer Ptr 선언 264 | 265 | ### 사용하는 이유 266 | 267 | pcl::PointCloud의 pointer는 아래와 같이 선언할 수 있다. 268 | 269 | ```cpp 270 | pcl::PointCloud::Ptr ptr_cloud(new pcl::PointCloud); 271 | ``` 272 | 273 | 기존에 우리가 알던 *로 pointer를 선언하는 것과 다르다. 274 | 275 | 하지만 pcl에 구현되어있는 함수들이 대부분 Ptr을 매개체로 하여 출력값을 저장하기 때문에, 사용해야 한다. 276 | 277 | https://github.com/LimHyungTae/LimHyungTae.github.io/blob/master/_posts/2019-11-29-%5BROS%5D%20PCL%20%EC%9E%90%EC%A3%BC%20%EC%93%B0%EC%9D%B4%EB%8A%94%20%EA%B2%83%20%EC%A0%95%EB%A6%AC.md 278 | 279 | 그럼 pcl::PointCloud의 주소를 Ptr로 할당시키려면 아래와 같이 해야할까? 280 | ```cpp 281 | ptr_cloud = &cloud2; 282 | ``` 283 | 284 | 아님!! ~~boost::shared_ptr랑 관련 있는 거 같은데 c++ 고수님들 왜 그런건지 아시는 분 가르쳐주세요...~~ 285 | 286 | 아래와 같이 하면 pcl::PointCloud를 Ptr에 할당할 수 있다. 287 | 288 | ```cpp 289 | *ptr_cloud = cloud2; 290 | ``` 291 | 292 | ### 사용법 293 | 294 | ```cpp 295 | pcl::PointCloud::Ptr ptr_cloud(new pcl::PointCloud); 296 | *ptr_cloud = cloud2; 297 | 298 | cout<<"Original: "< 6 | Seungwon Song (sswan55@kaist.ac.kr)
7 | Hyungjin Kim (hjkim86@kaist.ac.kr) 8 | 9 | ----------- 10 | 11 | ## 형 변환(Type Conversion) 12 | 13 | ROS상에서 LiDAR 센서들은 sensor_msgs::PointCloud2나 Sensor_msgs::LaserScan(2D LiDAR의 경우) 타입을 통해 데이터가 들어오기 때문에, pcl을 통해 pointcloud를 다루기 위해서는 pcl::PointCloud로 형변환을 해줘야 한다. 14 | 15 | ### sensor_msgs::PointCloud2 :arrow_right: pcl::PointCloud 16 | ```cpp 17 | pcl::PointCloud cloudmsg2cloud(sensor_msgs::PointCloud2 cloudmsg) 18 | { 19 | pcl::PointCloud cloud_dst; 20 | pcl::fromROSMsg(cloudmsg, cloud_dst); 21 | return cloud_dst; 22 | } 23 | ``` 24 | ### pcl::PointCloud :arrow_right: sensor_msgs::PointCloud2 25 | ```cpp 26 | sensor_msgs::PointCloud2 cloud2cloudmsg(pcl::PointCloud cloud_src) 27 | { 28 | sensor_msgs::PointCloud2 cloudmsg; 29 | pcl::toROSMsg(cloudsrc, cloudmsg); 30 | cloudmsg.header.frame_id = "map"; 31 | return cloudmsg; 32 | } 33 | ``` 34 | 35 | ### sensor_msgs::LaserScan :arrow_right: sensor_msgs::PointCloud2 36 | ```cpp 37 | 38 | #include "laser_geometry/laser_geometry.h" 39 | 40 | sensor_msgs::PointCloud2 laser2cloudmsg(sensor_msgs::LaserScan laser) 41 | { 42 | static laser_geometry::LaserProjection projector; 43 | sensor_msgs::PointCloud2 pc2_dst; 44 | projector.projectLaser(laser, pc2_dst,-1,laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Distance); 45 | pc2_dst.header.frame_id = "map"; 46 | 47 | return pc2_dst; 48 | } 49 | ``` 50 | 51 | ## 변환 행렬 곱하기(Transformation) 52 | ![tf](../img/pcl_robot_sensor.PNG) 53 | 54 | 로보틱스에는 다양한 좌표계가 존재한다. 위의 사진의 경우만 봐도 센서 데이터는 센서의 원점을 기준으로 데이터를 얻는다. 하지만 우리가 실제로 요하는 것 로봇의 body 기준의 정보들이기 때문에, 센서 데이터를 취득한 후 원하는 좌표계에 변환(transformation)을 해주어야 한다. 변환을 할 아래와 같은 코드를 사용하면 Eigen::Matrix4f trans만큼 모든 포인트클라우드가 변환된다. 55 | ```cpp 56 | //Input: pcl::PointCloud source, cloud_src 57 | //Output: Transformed pcl::PointCloud, pc_transformed via 4x4 transformation matrix 58 | #include 59 | 60 | pcl::PointCloud pc_transformed; 61 | pcl::PointCloud::Ptr ptr_transformed(new pcl::PointCloud); 62 | 63 | Eigen::Matrix4f trans; 64 | trans<< 1, 0, 0, 0.165, 65 | 0, 1, 0, 0.000, 66 | 0, 0, 1, 0.320, 67 | 0, 0, 0, 1; 68 | pcl::transformPointCloud(cloud_src, *ptr_transformed, trans); 69 | 70 | pc_transformed = *ptr_transformed 71 | ``` 72 | 73 | ## 지정 축에 대해 포인트 클라우드 필터링하기(Filtering using a PassThrough Filter) 74 | 75 | ```cpp 76 | #include 77 | 78 | //Input: pcl::PointCloud source, cloud_src 79 | //Output: Filtered pcl::PointCloud, pc_filtered along z axis, from 0.5m to 100.0m 80 | 81 | pcl::PointCloud pc_filtered; 82 | pcl::PointCloud::Ptr ptr_filtered(new pcl::PointCloud); 83 | pcl::PassThrough filter; 84 | 85 | double min_range = 0.5; 86 | double max_range = 100.0; 87 | *ptr_filtered = cloud_src; 88 | 89 | filter.setInputCloud(ptr_filtered); 90 | filter.setFilterFieldName("z"); 91 | filter.setFilterLimits(min_range, max_range); 92 | // filter.setFilterLimitsNegative(true); 93 | filter.filter(*ptr_filtered); 94 | 95 | pc_filtered = *ptr_filtered; 96 | ``` 97 | 98 | ## Downsampling to a Voxel Grid 99 | 100 | ![centroid](../img/pcl_centroid.PNG) 101 | ```cpp 102 | #include 103 | 104 | //Input: pcl::PointCloud source, cloud_src 105 | //Output: voxelized pcl::PointCloud, pc_voxelized 106 | 107 | pcl::PointCloud pc_voxelized; 108 | pcl::PointCloud::Ptr ptr_filtered(new pcl::PointCloud); 109 | pcl::VoxelGrid voxel_filter; 110 | 111 | double var_voxelsize = 0.05; 112 | 113 | *ptr_filtered = cloud_src; 114 | voxel_filter.setInputCloud(ptr_filtered); 115 | voxel_filter.setLeafSize(var_voxelsize, var_voxelsize, var_voxelsize); 116 | voxel_filter.filter(*ptr_filtered); 117 | 118 | pc_voxelized = *ptr_filtered; 119 | ``` 120 | 그런데 굳이 filter()함수에 ptr을 넣지 않고 직접적으로 pcl::PointCloud로 받아도 된다. 121 | ```cpp 122 | void mapgen::voxelize(pcl::PointCloud::Ptr pc_src, pcl::PointCloud& pc_dst, double var_voxel_size){ 123 | 124 | static pcl::VoxelGrid voxel_filter; 125 | voxel_filter.setInputCloud(pc_src); 126 | voxel_filter.setLeafSize(var_voxel_size, var_voxel_size, var_voxel_size); 127 | voxel_filter.filter(pc_dst); 128 | 129 | } 130 | ``` 131 | 132 | ## Statistical Outlier Removal 133 | ![sor](../img/pcl_sor.PNG) 134 | 135 | The number of neighbors to analyze for each point is set to 10, and the standard deviation multiplier to 1.0 136 | ```cpp 137 | #include 138 | 139 | //Input: pcl::PointCloud source, cloud_src 140 | //Output: voxelized pcl::PointCloud, pc_sor_filtered 141 | 142 | pcl::PointCloud pc_sor_filtered; 143 | pcl::PointCloud::Ptr ptr_sor_filtered(new pcl::PointCloud); 144 | *ptr_sor_filtered = cloud_src; 145 | 146 | int num_neigbor_points = 10; 147 | double std_multiplier = 1.0; 148 | 149 | pcl::StatisticalOutlierRemoval sor; 150 | sor.setInputCloud (ptr_sor_filtered); 151 | sor.setMeanK (num_neigbor_points); 152 | sor.setStddevMulThresh (std_multiplier); 153 | sor.filter(*ptr_sor_filtered); 154 | 155 | pc_sor_filtered = *ptr_sor_filtered; 156 | ``` 157 | 158 | ## Reference 159 | [1] Using a matrix to transform a point cloud, http://pointclouds.org/documentation/tutorials/matrix_transform.php
160 | [2] Filtering a PointCloud using a PassThrough filter, http://pointclouds.org/documentation/tutorials/passthrough.php
161 | [3] Downsampling a PointCloud using a VoxelGrid filter, http://pointclouds.org/documentation/tutorials/voxel_grid.php
162 | [4] Removing outliers using a StatisticalOutlierRemoval filter, http://pointclouds.org/documentation/tutorials/statistical_outlier.php
163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /0.prerequisites/2.PCL사용법/PCL 정리 및 기본 사용법/README.md: -------------------------------------------------------------------------------- 1 | # PCL cheat sheet(1/2) 2 | 3 | Original author: Hyungtae Lim (shapelim@kaist.ac.kr) 4 | 5 | 6 | ---------------- 7 | 8 | ## PCL 선언하는 법 & T Type 9 | 10 | ### T Type 11 | 12 | pcl상의 PointCloud pcl::PointCloud에는 다양한 type을 담을 수 있는데, 13 | 14 | 주로 **LiDAR**를 사용할 때는 *pcl::PointXYZ, pcl::PointXYZI*를 많이 사용한다. 15 | 16 | **RGB-D나 스테레오 카메라**는 depth를 image에 align시키면 point의 색깔도 알 수 있기 때문에 *pcl::PointXYZRGB(A)*를 사용하는 경우도 있다. 17 | 18 | 더 다양한 type은 원래 pcl tutorial 페이지에서 확인할 수 있다.
19 | http://www.pointclouds.org/documentation/tutorials/adding_custom_ptype.php#adding-custom-ptype 20 | 21 | ```cpp 22 | pcl::PointCloud cloud; 23 | pcl::PointCloud cloud; 24 | pcl::PointCloud cloud; 25 | ``` 26 | 27 | {% highlight ruby %} 28 | pcl::PointCloud cloud; 29 | pcl::PointCloud cloud; 30 | pcl::PointCloud cloud; 31 | {% endhighlight %} 32 | 33 | ```cpp 34 | pcl::PointXYZ point_xyz; 35 | point_xyz.x = 1; 36 | point_xyz.y = 2; 37 | point_xyz.z = 3; 38 | ``` 39 | 혹은 아래와 같이 한 줄로 선언할 수도 있다. 40 | ```cpp 41 | 42 | pcl::PointXYZ point_xyz = {1, 2, 3}; // 1, 2, 3이 각각 x, y, z로 지정된다. 43 | ``` 44 | 45 | 아래의 예시들은 다음과 같이 header file과 namespace가 선언되어 있다고 가정한다. 46 | ```cpp 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | using namespace std; 53 | ``` 54 | 55 | ### pcl::PointCloud 선언해서 Points에 Point 넣는 법 56 | 57 | 기본적으로 pcl은 std::vector의 사용법과 유사하다. 58 | 59 | 왜냐하면 pcl의 내부를 살펴보면 std::vector로 구성되어 있기 때문이다 (http://docs.pointclouds.org/trunk/classpcl_1_1_point_cloud.html 참조) 60 | 61 | ```cpp 62 | pcl::PointCloud cloud; 63 | cloud.resize(3); //cloud의 size를 3으로 설정 64 | 65 | cloud.points[0].x = 1; 66 | cloud.points[0].y = 2; 67 | cloud.points[0].z = 3; 68 | 69 | cloud.points[1].x = 4; 70 | cloud.points[1].y = 5; 71 | cloud.points[1].z = 6; 72 | 73 | cloud.points[2].x = 7; 74 | cloud.points[2].y = 8; 75 | cloud.points[2].z = 9; 76 | 77 | ``` 78 | 79 | or 80 | ```cpp 81 | pcl::PointCloud cloud; 82 | pcl::PointXYZ point_xyz; // pcl::PointXYZ이라는 type에 data를 담는다. 83 | 84 | point_xyz.x = 1; 85 | point_xyz.y = 2; 86 | point_xyz.z = 3; 87 | cloud.push_back(point_xyz); 88 | 89 | point_xyz.x = 4; 90 | point_xyz.y = 5; 91 | point_xyz.z = 6; 92 | cloud.push_back(point_xyz); 93 | 94 | point_xyz.x = 7; 95 | point_xyz.y = 8; 96 | point_xyz.z = 9; 97 | cloud.push_back(point_xyz); 98 | ``` 99 | or 100 | ```cpp 101 | pcl::PointCloud cloud; 102 | cloud.push_back(pcl::PointXYZ(1, 2, 3)); 103 | cloud.push_back(pcl::PointXYZ(4, 5, 6)); 104 | cloud.push_back(pcl::PointXYZ(7, 8, 9)); 105 | ``` 106 | 107 | 출력을 하면 아래와 같은 결과를 볼 수 있다. 108 | ```cpp 109 | for (int i = 0 ; i < cloud.size(); ++i){ 110 | cout << i << ": "; 111 | cout << cloud.points[i].x << ", "; 112 | cout << cloud.points[i].y << ", "; 113 | cout << cloud.points[i].z << endl; 114 | 115 | } 116 | ``` 117 | ##### Result:
118 | 0: 1, 2, 3
119 | 1: 4, 5, 6
120 | 2: 7, 8, 9 121 | 122 | ## Functions 123 | 124 | http://docs.pointclouds.org/1.8.1/classpcl_1_1_point_cloud.html 125 | 126 | ### begin(): PointCloud의 첫 부분을 가르키는 *iterator*를 반환함 127 | 128 | ```cpp 129 | cout << "begin(): "; 130 | cout << cloud.begin()->x << ", "; 131 | cout << cloud.begin()->y << ", "; 132 | cout << cloud.begin()->z << endl; 133 | ``` 134 | 135 | ->의 의미는 옆 사이트에 설명되어 있다. https://ianuarias.tistory.com/16 136 | 137 | ->의 의미 한 줄 요약: return 값이 주소값이기 때문에, 주소값이 가르키는 객체의 멤버변수/멤버함수는 ->로 지칭함. 138 | 139 | ##### Result::
140 | begin(): 1, 2, 3 141 | 142 | ### end(): PointCloud의 끝 부분을 가르키는 *iterator*를 반환함 143 | 144 | ```cpp 145 | cout << "end(): "; 146 | cout << cloud.end()->x << ", "; 147 | cout << cloud.end()->y << ", "; 148 | cout << cloud.end()->z << endl; 149 | ``` 150 | ##### Result:
151 | end(): 0, 0, 0 152 | 153 | ????? 왜 0, 0, 0?: vector의 end()처럼 마지막 요소의 다음 부분을 가르키는 iterator를 리턴하기 때문. 154 | 155 | 따라서 우리가 원하는 pcl::PointCloud의 제일 뒷쪽의 요소를 가르키는 iterator를 뜻하려면 다음과 같이 -1을 빼주면 된다. 156 | 157 | ```cpp 158 | cout << "end() -1: "; 159 | cout << (cloud.end()-1)->x << ", "; 160 | cout << (cloud.end()-1)->y << ", "; 161 | cout << (cloud.end()-1)->z << endl; 162 | ``` 163 | ##### Result:
164 | end() - 1: 7, 8, 9 165 | 166 | ### front(): 첫번째 *원소*를 반환함 167 | ```cpp 168 | cout << "front(): "; 169 | cout << cloud.front().x << ", "; 170 | cout << cloud.front().y << ", "; 171 | cout << cloud.front().z << endl; 172 | ``` 173 | ##### Result:
174 | front(): 1, 2, 3 175 | 176 | ### back(): 마지막 *요소*를 반환함 177 | 178 | ```cpp 179 | cout << "back(): "; 180 | cout << cloud.back().x << ", "; 181 | cout << cloud.back().y << ", "; 182 | cout << cloud.back().z << endl; 183 | ``` 184 | 185 | ##### Result:
186 | back(): 7, 8, 9 187 | 188 | ### at(int index) 189 | pcl::PointCloud 상의 index에 해당하는 요소를 반환함. 190 | 191 | ```cpp 192 | cout << "at(1): "; 193 | cout << cloud.at(1).x << ", "; 194 | cout << cloud.at(1).y << ", "; 195 | cout << cloud.at(1).z << endl; 196 | ``` 197 | ##### Result:
198 | at(1): 4, 5, 6 199 | 200 | ### empty() 201 | ```cpp 202 | if (cloud.empty()) cout << "True"; else cout << "False"; 203 | cout << " | size of pc: " << cloud.size() << endl; 204 | ``` 205 | ##### Result:
206 | False | size of pc: 3 207 | 208 | ### clear() 209 | ```cpp 210 | cloud.empty(); 211 | if (cloud.empty()) cout << "True"; else cout << "False"; 212 | cout << " | size of pc: " << cloud.size() << endl; 213 | ``` 214 | ##### Result:
215 | True | size of pc: 0 216 | 217 | 218 | ## ★PointCloud 합치기 219 | ~~이런 pythonic한 문법이 되다니...갓PCL...~~ 220 | ```cpp 221 | pcl::PointCloud cloud2; 222 | cloud2.push_back(pcl::PointXYZ(1, 2, 3)); 223 | cloud2.push_back(pcl::PointXYZ(4, 5, 6)); 224 | 225 | 226 | pcl::PointCloud cloud3; 227 | cloud3.push_back(pcl::PointXYZ(7, 8, 9)); 228 | cloud3.push_back(pcl::PointXYZ(10, 11, 12)); 229 | 230 | cloud2 += cloud3; 231 | 232 | cout <<"size: " << cloud2.size() << endl; 233 | for (int i = 0 ; i < cloud2.size(); ++i){ 234 | cout << i << ": "; 235 | cout << cloud2.points[i].x << ", "; 236 | cout << cloud2.points[i].y << ", "; 237 | cout << cloud2.points[i].z << endl; 238 | } 239 | ``` 240 | ##### Result:
241 | size: 4
242 | 0: 1, 2, 3
243 | 1: 4, 5, 6
244 | 2: 7, 8, 9
245 | 3: 10, 11, 12 246 | 247 | ## PCL pointer Ptr 선언 248 | 249 | ### 사용하는 이유 250 | 251 | pcl::PointCloud의 pointer는 아래와 같이 선언할 수 있다. 252 | 253 | ```cpp 254 | pcl::PointCloud::Ptr ptr_cloud(new pcl::PointCloud); 255 | ``` 256 | 257 | 기존에 우리가 알던 *로 pointer를 선언하는 것과 다르다. 258 | 259 | 하지만 pcl에 구현되어있는 함수들이 대부분 Ptr을 매개체로 하여 출력값을 저장하기 때문에, 사용해야 한다. 260 | 261 | https://github.com/LimHyungTae/LimHyungTae.github.io/blob/master/_posts/2019-11-29-%5BROS%5D%20PCL%20%EC%9E%90%EC%A3%BC%20%EC%93%B0%EC%9D%B4%EB%8A%94%20%EA%B2%83%20%EC%A0%95%EB%A6%AC.md 262 | 263 | 그럼 pcl::PointCloud의 주소를 Ptr로 할당시키려면 아래와 같이 해야할까? 264 | ```cpp 265 | ptr_cloud = &cloud2; 266 | ``` 267 | 268 | 아님!! ~~boost::shared_ptr랑 관련 있는 거 같은데 c++ 고수님들 왜 그런건지 아시는 분 가르쳐주세요...~~ 269 | 270 | 아래와 같이 하면 pcl::PointCloud를 Ptr에 할당할 수 있다. 271 | 272 | ```cpp 273 | *ptr_cloud = cloud2; 274 | ``` 275 | 276 | ### 사용법 277 | 278 | ```cpp 279 | pcl::PointCloud::Ptr ptr_cloud(new pcl::PointCloud); 280 | *ptr_cloud = cloud2; 281 | 282 | cout<<"Original: "< 302 | Original:
303 | 0: 7, 8, 9
304 | 1: 10, 11, 12
305 | After:
306 | 0: 1, 2, 3
307 | 1: 4, 5, 6
308 | 2: 7, 8, 9
309 | 3: 10, 11, 12 310 | 311 | ### +=는 값을 할당하는 걸까, 복사하는 걸까? 312 | 313 | ```cpp 314 | cloud3.push_back(pcl::PointXYZ(12, 13, 14)); 315 | cout<<"After: "< 325 | After:
326 | 0: 1, 2, 3
327 | 1: 4, 5, 6
328 | 2: 7, 8, 9
329 | 3: 10, 11, 12 330 | 331 | 그대로임을 알 수 있다. 즉, 주소를 할당받아 link되어 있지 않고, points들을 통째로 복사해오는 것임. 332 | 333 | 334 | 335 | -------------------------------------------------------------------------------- /0.prerequisites/2.PCL사용법/README.md: -------------------------------------------------------------------------------- 1 | # PCL cheat sheet 2 | 3 | Original author: Hyungtae Lim (shapelim@kaist.ac.kr) 4 | 5 | 6 | ---------------- 7 | 8 | 9 | [1. PCL 정리 및 기본 사용법](https://github.com/LimHyungTae/graph_slam_tutorial/tree/master/2.PCL%EC%82%AC%EC%9A%A9%EB%B2%95/PCL%20%EC%A0%95%EB%A6%AC%20%EB%B0%8F%20%EA%B8%B0%EB%B3%B8%20%EC%82%AC%EC%9A%A9%EB%B2%95) 10 | 11 | [2. PCL 자주 쓰이는 라이브러리 정리](https://github.com/LimHyungTae/graph_slam_tutorial/tree/master/2.PCL%EC%82%AC%EC%9A%A9%EB%B2%95/PCL%20%EC%9E%90%EC%A3%BC%20%EC%93%B0%EC%9D%B4%EB%8A%94%20%EB%9D%BC%EC%9D%B4%EB%B8%8C%EB%9F%AC%EB%A6%AC%20%EC%A0%95%EB%A6%AC) 12 | -------------------------------------------------------------------------------- /0.prerequisites/2.PCL사용법/img/pcl_centroid.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/0.prerequisites/2.PCL사용법/img/pcl_centroid.PNG -------------------------------------------------------------------------------- /0.prerequisites/2.PCL사용법/img/pcl_robot_sensor.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/0.prerequisites/2.PCL사용법/img/pcl_robot_sensor.PNG -------------------------------------------------------------------------------- /0.prerequisites/2.PCL사용법/img/pcl_sor.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/0.prerequisites/2.PCL사용법/img/pcl_sor.PNG -------------------------------------------------------------------------------- /0.prerequisites/README.md: -------------------------------------------------------------------------------- 1 | # PCL cheat sheet(1/2) 2 | 3 | Original author: Hyungtae Lim (shapelim@kaist.ac.kr) 4 | 5 | 6 | ---------------- 7 | 8 | ## PCL 선언하는 법 & T Type 9 | 10 | ### T Type 11 | 12 | pcl상의 PointCloud pcl::PointCloud에는 다양한 type을 담을 수 있는데, 13 | 14 | 주로 **LiDAR**를 사용할 때는 *pcl::PointXYZ, pcl::PointXYZI*를 많이 사용한다. 15 | 16 | **RGB-D나 스테레오 카메라**는 depth를 image에 align시키면 point의 색깔도 알 수 있기 때문에 *pcl::PointXYZRGB(A)*를 사용하는 경우도 있다. 17 | 18 | 더 다양한 type은 원래 pcl tutorial 페이지에서 확인할 수 있다.
19 | http://www.pointclouds.org/documentation/tutorials/adding_custom_ptype.php#adding-custom-ptype 20 | 21 | ```cpp 22 | pcl::PointCloud cloud; 23 | pcl::PointCloud cloud; 24 | pcl::PointCloud cloud; 25 | ``` 26 | 27 | {% highlight ruby %} 28 | pcl::PointCloud cloud; 29 | pcl::PointCloud cloud; 30 | pcl::PointCloud cloud; 31 | {% endhighlight %} 32 | 33 | ```cpp 34 | pcl::PointXYZ point_xyz; 35 | point_xyz.x = 1; 36 | point_xyz.y = 2; 37 | point_xyz.z = 3; 38 | ``` 39 | 혹은 아래와 같이 한 줄로 선언할 수도 있다. 40 | ```cpp 41 | 42 | pcl::PointXYZ point_xyz = {1, 2, 3}; // 1, 2, 3이 각각 x, y, z로 지정된다. 43 | ``` 44 | 45 | 아래의 예시들은 다음과 같이 header file과 namespace가 선언되어 있다고 가정한다. 46 | ```cpp 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | using namespace std; 53 | ``` 54 | 55 | ### pcl::PointCloud 선언해서 Points에 Point 넣는 법 56 | 57 | 기본적으로 pcl은 std::vector의 사용법과 유사하다. 58 | 59 | 왜냐하면 pcl의 내부를 살펴보면 std::vector로 구성되어 있기 때문이다 (http://docs.pointclouds.org/trunk/classpcl_1_1_point_cloud.html 참조) 60 | 61 | ```cpp 62 | pcl::PointCloud cloud; 63 | cloud.resize(3); //cloud의 size를 3으로 설정 64 | 65 | cloud.points[0].x = 1; 66 | cloud.points[0].y = 2; 67 | cloud.points[0].z = 3; 68 | 69 | cloud.points[1].x = 4; 70 | cloud.points[1].y = 5; 71 | cloud.points[1].z = 6; 72 | 73 | cloud.points[2].x = 7; 74 | cloud.points[2].y = 8; 75 | cloud.points[2].z = 9; 76 | 77 | ``` 78 | 79 | or 80 | ```cpp 81 | pcl::PointCloud cloud; 82 | pcl::PointXYZ point_xyz; // pcl::PointXYZ이라는 type에 data를 담는다. 83 | 84 | point_xyz.x = 1; 85 | point_xyz.y = 2; 86 | point_xyz.z = 3; 87 | cloud.push_back(point_xyz); 88 | 89 | point_xyz.x = 4; 90 | point_xyz.y = 5; 91 | point_xyz.z = 6; 92 | cloud.push_back(point_xyz); 93 | 94 | point_xyz.x = 7; 95 | point_xyz.y = 8; 96 | point_xyz.z = 9; 97 | cloud.push_back(point_xyz); 98 | ``` 99 | or 100 | ```cpp 101 | pcl::PointCloud cloud; 102 | cloud.push_back(pcl::PointXYZ(1, 2, 3)); 103 | cloud.push_back(pcl::PointXYZ(4, 5, 6)); 104 | cloud.push_back(pcl::PointXYZ(7, 8, 9)); 105 | ``` 106 | 107 | 출력을 하면 아래와 같은 결과를 볼 수 있다. 108 | ```cpp 109 | for (int i = 0 ; i < cloud.size(); ++i){ 110 | cout << i << ": "; 111 | cout << cloud.points[i].x << ", "; 112 | cout << cloud.points[i].y << ", "; 113 | cout << cloud.points[i].z << endl; 114 | 115 | } 116 | ``` 117 | ##### Result:
118 | 0: 1, 2, 3
119 | 1: 4, 5, 6
120 | 2: 7, 8, 9 121 | 122 | ## Functions 123 | 124 | http://docs.pointclouds.org/1.8.1/classpcl_1_1_point_cloud.html 125 | 126 | ### begin(): PointCloud의 첫 부분을 가르키는 *iterator*를 반환함 127 | 128 | ```cpp 129 | cout << "begin(): "; 130 | cout << cloud.begin()->x << ", "; 131 | cout << cloud.begin()->y << ", "; 132 | cout << cloud.begin()->z << endl; 133 | ``` 134 | 135 | ->의 의미는 옆 사이트에 설명되어 있다. https://ianuarias.tistory.com/16 136 | 137 | ->의 의미 한 줄 요약: return 값이 주소값이기 때문에, 주소값이 가르키는 객체의 멤버변수/멤버함수는 ->로 지칭함. 138 | 139 | ##### Result::
140 | begin(): 1, 2, 3 141 | 142 | ### end(): PointCloud의 끝 부분을 가르키는 *iterator*를 반환함 143 | 144 | ```cpp 145 | cout << "end(): "; 146 | cout << cloud.end()->x << ", "; 147 | cout << cloud.end()->y << ", "; 148 | cout << cloud.end()->z << endl; 149 | ``` 150 | ##### Result:
151 | end(): 0, 0, 0 152 | 153 | ????? 왜 0, 0, 0?: vector의 end()처럼 마지막 요소의 다음 부분을 가르키는 iterator를 리턴하기 때문. 154 | 155 | 따라서 우리가 원하는 pcl::PointCloud의 제일 뒷쪽의 요소를 가르키는 iterator를 뜻하려면 다음과 같이 -1을 빼주면 된다. 156 | 157 | ```cpp 158 | cout << "end() -1: "; 159 | cout << (cloud.end()-1)->x << ", "; 160 | cout << (cloud.end()-1)->y << ", "; 161 | cout << (cloud.end()-1)->z << endl; 162 | ``` 163 | ##### Result:
164 | end() - 1: 7, 8, 9 165 | 166 | ### front(): 첫번째 *원소*를 반환함 167 | ```cpp 168 | cout << "front(): "; 169 | cout << cloud.front().x << ", "; 170 | cout << cloud.front().y << ", "; 171 | cout << cloud.front().z << endl; 172 | ``` 173 | ##### Result:
174 | front(): 1, 2, 3 175 | 176 | ### back(): 마지막 *요소*를 반환함 177 | 178 | ```cpp 179 | cout << "back(): "; 180 | cout << cloud.back().x << ", "; 181 | cout << cloud.back().y << ", "; 182 | cout << cloud.back().z << endl; 183 | ``` 184 | 185 | ##### Result:
186 | back(): 7, 8, 9 187 | 188 | ### at(int index) 189 | pcl::PointCloud 상의 index에 해당하는 요소를 반환함. 190 | 191 | ```cpp 192 | cout << "at(1): "; 193 | cout << cloud.at(1).x << ", "; 194 | cout << cloud.at(1).y << ", "; 195 | cout << cloud.at(1).z << endl; 196 | ``` 197 | ##### Result:
198 | at(1): 4, 5, 6 199 | 200 | ### empty() 201 | ```cpp 202 | if (cloud.empty()) cout << "True"; else cout << "False"; 203 | cout << " | size of pc: " << cloud.size() << endl; 204 | ``` 205 | ##### Result:
206 | False | size of pc: 3 207 | 208 | ### clear() 209 | ```cpp 210 | cloud.empty(); 211 | if (cloud.empty()) cout << "True"; else cout << "False"; 212 | cout << " | size of pc: " << cloud.size() << endl; 213 | ``` 214 | ##### Result:
215 | True | size of pc: 0 216 | 217 | 218 | ## ★PointCloud 합치기 219 | ~~이런 pythonic한 문법이 되다니...갓PCL...~~ 220 | ```cpp 221 | pcl::PointCloud cloud2; 222 | cloud2.push_back(pcl::PointXYZ(1, 2, 3)); 223 | cloud2.push_back(pcl::PointXYZ(4, 5, 6)); 224 | 225 | 226 | pcl::PointCloud cloud3; 227 | cloud3.push_back(pcl::PointXYZ(7, 8, 9)); 228 | cloud3.push_back(pcl::PointXYZ(10, 11, 12)); 229 | 230 | cloud2 += cloud3; 231 | 232 | cout <<"size: " << cloud2.size() << endl; 233 | for (int i = 0 ; i < cloud2.size(); ++i){ 234 | cout << i << ": "; 235 | cout << cloud2.points[i].x << ", "; 236 | cout << cloud2.points[i].y << ", "; 237 | cout << cloud2.points[i].z << endl; 238 | } 239 | ``` 240 | ##### Result:
241 | size: 4
242 | 0: 1, 2, 3
243 | 1: 4, 5, 6
244 | 2: 7, 8, 9
245 | 3: 10, 11, 12 246 | 247 | ## PCL pointer Ptr 선언 248 | 249 | ### 사용하는 이유 250 | 251 | pcl::PointCloud의 pointer는 아래와 같이 선언할 수 있다. 252 | 253 | ```cpp 254 | pcl::PointCloud::Ptr ptr_cloud(new pcl::PointCloud); 255 | ``` 256 | 257 | 기존에 우리가 알던 *로 pointer를 선언하는 것과 다르다. 258 | 259 | 하지만 pcl에 구현되어있는 함수들이 대부분 Ptr을 매개체로 하여 출력값을 저장하기 때문에, 사용해야 한다. 260 | 261 | https://github.com/LimHyungTae/LimHyungTae.github.io/blob/master/_posts/2019-11-29-%5BROS%5D%20PCL%20%EC%9E%90%EC%A3%BC%20%EC%93%B0%EC%9D%B4%EB%8A%94%20%EA%B2%83%20%EC%A0%95%EB%A6%AC.md 262 | 263 | 그럼 pcl::PointCloud의 주소를 Ptr로 할당시키려면 아래와 같이 해야할까? 264 | ```cpp 265 | ptr_cloud = &cloud2; 266 | ``` 267 | 268 | 아님!! ~~boost::shared_ptr랑 관련 있는 거 같은데 c++ 고수님들 왜 그런건지 아시는 분 가르쳐주세요...~~ 269 | 270 | 아래와 같이 하면 pcl::PointCloud를 Ptr에 할당할 수 있다. 271 | 272 | ```cpp 273 | *ptr_cloud = cloud2; 274 | ``` 275 | 276 | ### 사용법 277 | 278 | ```cpp 279 | pcl::PointCloud::Ptr ptr_cloud(new pcl::PointCloud); 280 | *ptr_cloud = cloud2; 281 | 282 | cout<<"Original: "< 302 | Original:
303 | 0: 7, 8, 9
304 | 1: 10, 11, 12
305 | After:
306 | 0: 1, 2, 3
307 | 1: 4, 5, 6
308 | 2: 7, 8, 9
309 | 3: 10, 11, 12 310 | 311 | ### +=는 값을 할당하는 걸까, 복사하는 걸까? 312 | 313 | ```cpp 314 | cloud3.push_back(pcl::PointXYZ(12, 13, 14)); 315 | cout<<"After: "< 325 | After:
326 | 0: 1, 2, 3
327 | 1: 4, 5, 6
328 | 2: 7, 8, 9
329 | 3: 10, 11, 12 330 | 331 | 그대로임을 알 수 있다. 즉, 주소를 할당받아 link되어 있지 않고, points들을 통째로 복사해오는 것임. 332 | 333 | 334 | 335 | -------------------------------------------------------------------------------- /1.ouster_setting/README.md: -------------------------------------------------------------------------------- 1 | # Setting of Ouster OS1-64 2 | 3 | ## 현 Ouster사에서 제공하는 driver를 통해 데이터를 받아보자 4 | 5 | 작성자: 임형태(shapelim@kaist.ac.kr), 박주현(mcvjoohyun@kaist.ac.kr) 6 | 7 | ---- 8 | 9 | 예시 Ouster Rosbag 파일은 [여기](https://www.dropbox.com/s/9gofcgfzaa8oyft/ouster_example.bag?dl=0)를 통해 다운 가능하다(2020-01-20 기준) 10 | 11 | ## How to install(세팅 하는 법) 12 | 13 | 1. 먼저, Ouster 사에서 제공하는 driver를 catkin_ws/src 내에서 git clone한다. (2020-01-09 기준) 14 | 15 | 
$ cd /home/$usr_name/catkin_ws/src
 16 | $ git clone https://github.com/ouster-lidar/ouster_example
17 | 18 | 2. 그 후 catkin_ws로 이동하여 complie해준다. 19 | 20 | 
$ cd /home/$usr_name/catkin_ws
 21 | $ catkin_make ouster_ros
22 | 23 | 혹은, catkin-tools를 이용하면 아래와 같이 컴파일 하면 된다. 24 | 
$ catkin build ouster_ros
25 | 26 | ## 드라이버를 통한 packet 파싱하기 27 | 28 | 1. [여기](https://www.dropbox.com/s/9gofcgfzaa8oyft/ouster_example.bag?dl=0)를 통해 다운받은 bag 파일을 rosbag play를 통해서 실행을 시킨다. 29 | 30 | 
rosbag play ouster_example.bag --clock
31 | 32 | 2. 그 후, driver상에서 제공해주는 launch file을 통해 data packet을 Ros 메세지로 변환할 수 있다. 33 | 34 | 
roslaunch /home/($user_name)/catkin_ws/src/ouster_example-master/ouster_ros/os1.launch replay:=true
35 | 36 | **그런데...막상 launch file을 실행하면 아래와 같이 md5sum 에러가 발생한다!!!!** 37 | 38 | ![error_occurs](/readme_materials/ouster_error.png) 39 | 40 | ## 문제가 발생하는 이유? 41 | 42 | Ouster 사에서 제공하는 driver 내의 패킷 메세지 내에 msg 상에 header가 포함되어 있지 않기 때문에 발생하는 것으로 보인다. 그로 인해서, packet data를 sensor_msgs로 변환할 때 ROS 상에서는 data 시간에 대한 정보가 없는 패킷을 받기 때문에 error가 발생시키는 것이다. 43 | 44 | 다시 말하자면, **Ouster사에서 제공하는 드라이버/(혹은 데이터를 패킷으로 생성할 때) 내에서 TimeStamp를 찍어주지 않기 때문에 발생하는 에러**로 보인다. 따라서 그 부분을 수정해줘야 한다. 45 | 46 | ## 해결책 47 | 48 | 1. Ouster_ros 폴더 내의 **msg/PacketMsg.msg**내에 std_msgs/Header header 추가해야 함. 49 | 50 | (수정후) 51 | 52 | 
std_msgs/Header header
 53 | uint8[] buf
54 | 55 | 2. Ouster_ros 안의**src/os1_node.cpp**내에서 118번째 줄과 123번 줄에 56 | 57 | 
lidar_packet.header.stamp.fromSec(ros::Time::now().toSec());
 58 | imu_packet.header.stamp.fromSec(ros::Time::now().toSec());
59 | 60 | 를 추가해줘야 한다. 61 | 62 | 원 파일의 일부분은 아래와 같다. 63 | 64 | 65 | ```cpp 66 | int connection_loop(ros::NodeHandle& nh, OS1::client& cli) { 67 | auto lidar_packet_pub = nh.advertise("lidar_packets", 1280); 68 | auto imu_packet_pub = nh.advertise("imu_packets", 100); 69 | 70 | PacketMsg lidar_packet, imu_packet; 71 | lidar_packet.buf.resize(OS1::lidar_packet_bytes + 1); 72 | imu_packet.buf.resize(OS1::imu_packet_bytes + 1); 73 | 74 | while (ros::ok()) { 75 | auto state = OS1::poll_client(cli); 76 | if (state == OS1::EXIT) { 77 | ROS_INFO("poll_client: caught signal, exiting"); 78 | return EXIT_SUCCESS; 79 | } 80 | if (state & OS1::ERROR) { 81 | ROS_ERROR("poll_client: returned error"); 82 | return EXIT_FAILURE; 83 | } 84 | if (state & OS1::LIDAR_DATA) { 85 | if (OS1::read_lidar_packet(cli, lidar_packet.buf.data())) 86 | { 87 | lidar_packet_pub.publish(lidar_packet); 88 | } 89 | } 90 | if (state & OS1::IMU_DATA) { 91 | if (OS1::read_imu_packet(cli, imu_packet.buf.data())) 92 | { 93 | imu_packet_pub.publish(imu_packet); 94 | } 95 | } 96 | ros::spinOnce(); 97 | } 98 | return EXIT_SUCCESS; 99 | } 100 | ``` 101 | 102 | 여기서 아래와 같이 119번째 줄과 125번 줄에 두 줄을 추가해주면 문제를 해결할 수 있다. 103 | 104 | ```cpp 105 | int connection_loop(ros::NodeHandle& nh, OS1::client& cli) { 106 | auto lidar_packet_pub = nh.advertise("lidar_packets", 1280); 107 | auto imu_packet_pub = nh.advertise("imu_packets", 100); 108 | 109 | PacketMsg lidar_packet, imu_packet; 110 | lidar_packet.buf.resize(OS1::lidar_packet_bytes + 1); 111 | imu_packet.buf.resize(OS1::imu_packet_bytes + 1); 112 | 113 | while (ros::ok()) { 114 | auto state = OS1::poll_client(cli); 115 | if (state == OS1::EXIT) { 116 | ROS_INFO("poll_client: caught signal, exiting"); 117 | return EXIT_SUCCESS; 118 | } 119 | if (state & OS1::ERROR) { 120 | ROS_ERROR("poll_client: returned error"); 121 | return EXIT_FAILURE; 122 | } 123 | if (state & OS1::LIDAR_DATA) { 124 | if (OS1::read_lidar_packet(cli, lidar_packet.buf.data())) 125 | { 126 | lidar_packet.header.stamp.fromSec(ros::Time::now().toSec()); 127 | lidar_packet_pub.publish(lidar_packet); 128 | } 129 | } 130 | if (state & OS1::IMU_DATA) { 131 | if (OS1::read_imu_packet(cli, imu_packet.buf.data())) 132 | { 133 | imu_packet.header.stamp.fromSec(ros::Time::now().toSec()); 134 | imu_packet_pub.publish(imu_packet); 135 | } 136 | } 137 | ros::spinOnce(); 138 | } 139 | return EXIT_SUCCESS; 140 | } 141 | ``` 142 | 143 | ## 결과 144 | 145 | 수정 결과, driver를 launch로 실행시켰을 때 아래와 같이 패킷이 **/os1_cloud_node/points**과 **/os1_cloud_node/imu**로 잘 파싱되는 것을 확인할 수 있다. 146 | 147 | ![ouster_after](/readme_materials/ouster_after.png) 148 | 149 | sensor_smgs/PointCloud2로 잘 파싱되어 있는 것을 확인할 수 있고, 바로 rviz를 통해서도 데이터를 육안으로 확인이 가능하다. 150 | 151 | ~~(새벽 한 시에 수정하고 데이터를 얻었어서...눈갱 죄송합니다)~~ 152 | 153 | ![ouster_type](/readme_materials/ouster_type.png) 154 | 155 | ![ouster_data](/readme_materials/ouster-data.gif) 156 | -------------------------------------------------------------------------------- /1.ouster_setting/README2.md: -------------------------------------------------------------------------------- 1 | # Setting of Ouster OS1-64 2 | 3 | ## 문제가 발생하는 이유? 4 | 5 | Ouster 사에서 제공하는 driver내에서 msg 상에 header가 포함되어 있지 않고, 그로 인해 ROS 상에서 data를 받게 되면 error가 발생한다. 6 | 7 | 다시 말하자면, 센서의 드라이버 내에서 TimeStamp를 찍어주지 않기 때문에 그 부분을 수정해줘야 함! 8 | 9 | 10 | 11 | 원 파일의 일부분은 아래와 같다. 12 | ```cpp 13 | int connection_loop(ros::NodeHandle& nh, OS1::client& cli) { 14 | auto lidar_packet_pub = nh.advertise("lidar_packets", 1280); 15 | auto imu_packet_pub = nh.advertise("imu_packets", 100); 16 | 17 | PacketMsg lidar_packet, imu_packet; 18 | lidar_packet.buf.resize(OS1::lidar_packet_bytes + 1); 19 | imu_packet.buf.resize(OS1::imu_packet_bytes + 1); 20 | 21 | while (ros::ok()) { 22 | auto state = OS1::poll_client(cli); 23 | if (state == OS1::EXIT) { 24 | ROS_INFO("poll_client: caught signal, exiting"); 25 | return EXIT_SUCCESS; 26 | } 27 | if (state & OS1::ERROR) { 28 | ROS_ERROR("poll_client: returned error"); 29 | return EXIT_FAILURE; 30 | } 31 | if (state & OS1::LIDAR_DATA) { 32 | if (OS1::read_lidar_packet(cli, lidar_packet.buf.data())) 33 | { 34 | lidar_packet_pub.publish(lidar_packet); 35 | } 36 | } 37 | if (state & OS1::IMU_DATA) { 38 | if (OS1::read_imu_packet(cli, imu_packet.buf.data())) 39 | { 40 | imu_packet_pub.publish(imu_packet); 41 | } 42 | } 43 | ros::spinOnce(); 44 | } 45 | return EXIT_SUCCESS; 46 | } 47 | ``` 48 | 아래와 같이 두 줄을 추가해주면 데이터를 받는데에 문제를 해결할 수 있다. 49 | ```cpp 50 | int connection_loop(ros::NodeHandle& nh, OS1::client& cli) { 51 | auto lidar_packet_pub = nh.advertise("lidar_packets", 1280); 52 | auto imu_packet_pub = nh.advertise("imu_packets", 100); 53 | 54 | PacketMsg lidar_packet, imu_packet; 55 | lidar_packet.buf.resize(OS1::lidar_packet_bytes + 1); 56 | imu_packet.buf.resize(OS1::imu_packet_bytes + 1); 57 | 58 | while (ros::ok()) { 59 | auto state = OS1::poll_client(cli); 60 | if (state == OS1::EXIT) { 61 | ROS_INFO("poll_client: caught signal, exiting"); 62 | return EXIT_SUCCESS; 63 | } 64 | if (state & OS1::ERROR) { 65 | ROS_ERROR("poll_client: returned error"); 66 | return EXIT_FAILURE; 67 | } 68 | if (state & OS1::LIDAR_DATA) { 69 | if (OS1::read_lidar_packet(cli, lidar_packet.buf.data())) 70 | { 71 | lidar_packet.header.stamp.fromSec(ros::Time::now().toSec()); 72 | lidar_packet_pub.publish(lidar_packet); 73 | } 74 | } 75 | if (state & OS1::IMU_DATA) { 76 | if (OS1::read_imu_packet(cli, imu_packet.buf.data())) 77 | { 78 | imu_packet.header.stamp.fromSec(ros::Time::now().toSec()); 79 | imu_packet_pub.publish(imu_packet); 80 | } 81 | } 82 | ros::spinOnce(); 83 | } 84 | return EXIT_SUCCESS; 85 | } 86 | ``` 87 | -------------------------------------------------------------------------------- /2.graph_slam/README.md: -------------------------------------------------------------------------------- 1 | # Conversion 2 | 3 | Original author: Hyungtae Lim (shapelim@kaist.ac.kr) 4 | 5 | 6 | ---------------- 7 | 8 | ## Conversion이 중요한 이유? 9 | 10 | ROS에서 [geometry_msgs/Pose](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Pose.html)로 데이터를 주기 때문에, 11 | 12 | xyz-roll-pitch-yaw(xyzrpy) 13 | xyz-quaternion(geoPose) 14 | 4x4 transformation matrix(eigen) 15 | 16 | ```cpp 17 | cv::Mat xyzrpy2mat(float x, float y, float z, float roll, float pitch, float yaw) 18 | { 19 | cv::Mat rot_vec = cv::Mat::zeros(3,1,CV_32FC1); 20 | rot_vec.at(0) = roll; 21 | rot_vec.at(1) = pitch; 22 | rot_vec.at(2) = yaw; 23 | 24 | cv::Mat rot_mat; 25 | cv::Rodrigues(rot_vec,rot_mat); 26 | 27 | cv::Mat result = cv::Mat::zeros(4,4,CV_32FC1); 28 | 29 | rot_mat.copyTo(result(cv::Rect(0,0,3,3))); 30 | 31 | result.at(0,3) = x; 32 | result.at(1,3) = y; 33 | result.at(2,3) = z; 34 | 35 | result.at(3,3) = 1; 36 | 37 | return result; 38 | } 39 | ``` 40 | xyz-roll-pitch-yaw(xyzrpy) 41 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 3D LiDAR Graph SLAM 튜토리얼 2 | 3 | 연구실 내에 튜토리얼 자료로 구축 중... 4 | 5 | 아직 작성하지 않았습니다 6 | 7 | ## WBU 8 | 9 | ## Description of rosbag file 10 | 11 | * Mode: 12 | 13 | 14 | 15 | 자칼의 xyz가 기존의 로봇들과 좀 다름!!! 거꾸로 가는듯한 느낌이 있음 16 | 17 | 18 | ## rosbag 19 | 20 | 
rosbag play A4.bag --clock
21 | 22 | 
rosparam set use_sim_time true
23 | 24 | 위의 두 명령어는 ros 상의 시간을 rosbag 기준으로 맞춰준다. 25 | 26 | ## launch ouster 27 | 28 | 
roslaunch /home/shapelim/catkin_ws/src/ouster_example-master/ouster_ros/os1.launch replay:=true
29 | 30 | **replay:=true** bag 파일의 패킷을 풀 때 필요함! 31 | 32 | ## When voxelizing 33 | leaf size is too small!! 34 | 35 | -> 0.2 이상으로 키우면 된다. 36 | 37 | 38 | icp: target의 관점에서 source를 바라볼 때의 tf가 계산됨! 39 | -------------------------------------------------------------------------------- /readme_materials/ouster-data.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/readme_materials/ouster-data.gif -------------------------------------------------------------------------------- /readme_materials/ouster_after.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/readme_materials/ouster_after.png -------------------------------------------------------------------------------- /readme_materials/ouster_error.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/readme_materials/ouster_error.png -------------------------------------------------------------------------------- /readme_materials/ouster_packet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/readme_materials/ouster_packet.png -------------------------------------------------------------------------------- /readme_materials/ouster_type.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/readme_materials/ouster_type.png -------------------------------------------------------------------------------- /tf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/graph_slam_tutorial/80fc71d40020386496dee9caffbb5b6209175689/tf.png --------------------------------------------------------------------------------